tijiao

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:

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: