From 07b7835a8333dc12c659f02091eba729803c6c53 Mon Sep 17 00:00:00 2001
From: zhangshixiang <@zhangshixiang>
Date: Sun, 8 Jun 2025 00:45:25 +0800
Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E7=BC=BA=E5=B0=91=E7=89=A9?=
 =?UTF-8?q?=E6=96=99=EF=BC=9A"plate=5Fwell=5FG12",?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 test/experiments/plr_test_converted.json      |  3 ++-
 .../nodes/presets/resource_mesh_manager.py    | 25 ++++++++++++-------
 2 files changed, 18 insertions(+), 10 deletions(-)

diff --git a/test/experiments/plr_test_converted.json b/test/experiments/plr_test_converted.json
index d2b3397c..c1f86648 100644
--- a/test/experiments/plr_test_converted.json
+++ b/test/experiments/plr_test_converted.json
@@ -5756,7 +5756,8 @@
                 "plate_well_D12",
                 "plate_well_E12",
                 "plate_well_F12",
-                "plate_well_G12"
+                "plate_well_G12",
+                "plate_well_H12"
             ],
             "parent": "deck",
             "type": "plate",
diff --git a/unilabos/ros/nodes/presets/resource_mesh_manager.py b/unilabos/ros/nodes/presets/resource_mesh_manager.py
index 3b494007..c797534f 100644
--- a/unilabos/ros/nodes/presets/resource_mesh_manager.py
+++ b/unilabos/ros/nodes/presets/resource_mesh_manager.py
@@ -392,6 +392,8 @@ class ResourceMeshManager(BaseROS2DeviceNode):
             planning_scene.is_diff = True
             planning_scene.robot_state.is_diff = True
             time_start = self.get_clock().now()
+            count = 0
+
             for resource_id, target_parent in cmd_dict.items():
                 parent_id = target_parent
                 if target_parent == '__trash':
@@ -453,15 +455,20 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                     collision_object.link_name = target_parent
                 planning_scene.robot_state.attached_collision_objects.append(collision_object)
 
-            #     collision_object = AttachedCollisionObject(
-            #         link_name=target_parent,
-            #         object=CollisionObject(
-            #             id=resource_id,
-            #             operation=CollisionObject.ADD   
-            #         )
-            #     )
-                
-            #     self.__planning_scene.robot_state.attached_collision_objects.append(collision_object)
+                count += 1
+
+                if count > 30:
+                    req = ApplyPlanningScene.Request()
+                    req.scene = planning_scene
+                    self.publish_resource_tf()
+                    self._apply_planning_scene_service.call(req)
+                    self.__planning_scene_publisher.publish(planning_scene)
+                    count = 0
+
+                    planning_scene = PlanningScene()
+                    planning_scene.is_diff = True
+                    planning_scene.robot_state.is_diff = True
+
             req = ApplyPlanningScene.Request()
             req.scene = planning_scene
             self.publish_resource_tf()