From 42f0994147afdbead94fe7a27a973d77c68dde03 Mon Sep 17 00:00:00 2001
From: zhangshixiang <@zhangshixiang>
Date: Wed, 7 May 2025 04:04:42 +0800
Subject: [PATCH] tijiao

---
 unilabos/device_mesh/view_robot.rviz          | 10 +++++-----
 .../nodes/presets/resource_mesh_manager.py    | 20 +++++++++++++++++--
 2 files changed, 23 insertions(+), 7 deletions(-)

diff --git a/unilabos/device_mesh/view_robot.rviz b/unilabos/device_mesh/view_robot.rviz
index 84810fae..64f6b358 100644
--- a/unilabos/device_mesh/view_robot.rviz
+++ b/unilabos/device_mesh/view_robot.rviz
@@ -352,18 +352,18 @@ Visualization Manager:
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.3028717339038849
-        Y: 0.19851508736610413
-        Z: 0.25103560090065
+        X: 0.29730814695358276
+        Y: 0.21228469908237457
+        Z: 0.20008830726146698
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.2897956669330597
+      Pitch: 0.38979560136795044
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 0.4607405960559845
+      Yaw: 0.06074193865060806
     Saved: ~
 Window Geometry:
   Displays:
diff --git a/unilabos/ros/nodes/presets/resource_mesh_manager.py b/unilabos/ros/nodes/presets/resource_mesh_manager.py
index 2ca8d0fb..c1495818 100644
--- a/unilabos/ros/nodes/presets/resource_mesh_manager.py
+++ b/unilabos/ros/nodes/presets/resource_mesh_manager.py
@@ -383,6 +383,10 @@ class ResourceMeshManager(BaseROS2DeviceNode):
         
         try:
             cmd_dict = json.loads(tf_update_msg.command.replace("'",'"'))
+            self.__planning_scene = self._get_planning_scene_service.call(
+                GetPlanningScene.Request()
+                ).scene
+            
             for resource_id, target_parent in cmd_dict.items():
 
                 # 获取从resource_id到target_parent的转换
@@ -412,8 +416,20 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                     "rotation": rotation
                 }
                 
-                self.attach_collision_object(id=resource_id,link_name=target_parent)
-
+                # self.attach_collision_object(id=resource_id,link_name=target_parent)
+                collision_object = AttachedCollisionObject(
+                    id=resource_id,
+                    link_name=target_parent,
+                    object=CollisionObject(
+                        id=resource_id,
+                        operation=CollisionObject.ADD   
+                    )
+                )
+                
+                self.__planning_scene.robot_state.attached_collision_objects.append(collision_object)
+            req = ApplyPlanningScene.Request()
+            req.scene = self.__planning_scene
+            self._apply_planning_scene_service.call_async(req)
             self.publish_resource_tf()
             
         except Exception as e: