Reapply "修改物体attach时，多次赋值当前时间导致卡顿问题，"

This reverts commit 07d9db20c3.

unilabos/device_mesh/view_robot.rviz

@@ -4,15 +4,12 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
-        - /TF1/Frames1
         - /TF1/Tree1
-        - /PlanningScene1/Scene Geometry1
-        - /MotionPlanning1
         - /MotionPlanning1/Scene Geometry1
         - /MotionPlanning1/Scene Robot1
         - /MotionPlanning1/Planning Request1
-      Splitter Ratio: 0.4940796494483948
+      Splitter Ratio: 0.5016146302223206
-    Tree Height: 602
+    Tree Height: 1112
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -56,7 +53,7 @@ Visualization Manager:
       Name: TF
       Show Arrows: true
       Show Axes: true
-      Show Names: true
+      Show Names: false
       Tree:
         {}
       Update Interval: 0
@@ -88,7 +85,7 @@ Visualization Manager:
       Value: false
       Visual Enabled: true
     - Class: moveit_rviz_plugin/PlanningScene
-      Enabled: false
+      Enabled: true
       Move Group Namespace: ""
       Name: PlanningScene
       Planning Scene Topic: /monitored_planning_scene
@@ -99,7 +96,7 @@ Visualization Manager:
         Scene Display Time: 0.009999999776482582
         Show Scene Geometry: true
         Voxel Coloring: Z-Axis
-        Voxel Rendering: Occupied Voxels
+        Voxel Rendering: Disabled
       Scene Robot:
         Attached Body Color: 150; 50; 150
         Links:
@@ -108,10 +105,108 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
+          PLR_STATION_deck_device_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          PLR_STATION_deck_first_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          PLR_STATION_deck_fourth_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          PLR_STATION_deck_main_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          PLR_STATION_deck_second_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          PLR_STATION_deck_socketTypeGenericSbsFootprint:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          PLR_STATION_deck_socketTypeHEPAModule:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          PLR_STATION_deck_third_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_link_1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_link_2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_link_3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_slideway:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_device_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          benyao_gripper_base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_gripper_left:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_gripper_right:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          hotel_base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          hotel_device_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          hotel_socketTypeGenericSbsFootprint:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          world:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
         Robot Alpha: 1
         Show Robot Collision: false
         Show Robot Visual: false
-      Value: false
+      Value: true
     - Attached Body Color: 150; 50; 150
       Class: moveit_rviz_plugin/RobotState
       Collision Enabled: false
@@ -294,9 +389,9 @@ Visualization Manager:
         Scene Alpha: 0.8999999761581421
         Scene Color: 50; 230; 50
         Scene Display Time: 0.009999999776482582
-        Show Scene Geometry: true
+        Show Scene Geometry: false
-        Voxel Coloring: Z-Axis
+        Voxel Coloring: Cell Probability
-        Voxel Rendering: Occupied Voxels
+        Voxel Rendering: All Voxels
       Scene Robot:
         Attached Body Color: 150; 50; 150
         Links:
@@ -469,28 +564,28 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.634795606136322
+      Pitch: 0.48479583859443665
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 5.590744495391846
+      Yaw: 0.042561568319797516
     Saved: ~
 Window Geometry:
   Displays:
-    collapsed: true
+    collapsed: false
-  Height: 1897
+  Height: 2032
-  Hide Left Dock: true
+  Hide Left Dock: false
   Hide Right Dock: true
   MotionPlanning:
-    collapsed: true
+    collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000003a300000714fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000027000002c80000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000002600000026fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000002fb00000440000002b800ffffff000000010000010f00000387fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003870000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000ddb0000071400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000003a30000079bfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000027000004c60000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000002600000026fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000004f9000002c9000002b800ffffff000000010000010f00000387fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003870000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000bc50000079b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Tool Properties:
     collapsed: false
   Views:
     collapsed: true
-  Width: 3547
+  Width: 3956
-  X: -857
+  X: 140
-  Y: 149
+  Y: 54

unilabos/devices/ros_dev/liquid_handler_joint_publisher.py

@@ -118,7 +118,7 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
             str_dict[resource] = link_name
 
         goal_msg.command = json.dumps(str_dict)
-        self.resource_action_client.send_goal_async(goal_msg)
+        self.resource_action_client.send_goal(goal_msg)
     
     def resource_move(self, resource_id:str, link_name:str, channels:list[int]):
         resource = resource_id.rsplit("_",1)
@@ -247,13 +247,10 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
             link_name =  self.lh_devices[parent_id]['joint_config']['link_names'][z_index]
             link_name =  f'{parent_id}_{link_name}'
             self.resource_move(resource_name_, link_name, [0,1,2,3,4,5,6,7])
-            time.sleep(1)
         elif option == "drop_trash":
             self.resource_move(resource_name_, "__trash", [0,1,2,3,4,5,6,7])
-            time.sleep(1)
         elif option == "drop":
             self.resource_move(resource_name_, "world", [0,1,2,3,4,5,6,7])
-            time.sleep(1)
         self.move_to(joint_positions_target_zero, speed, parent_id)
 
 

unilabos/ros/nodes/presets/resource_mesh_manager.py

@@ -161,7 +161,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                 self.resource_model[resource_config['id']] = {
                     'mesh': f"{str(self.mesh_path)}/device_mesh/resources/{model_config['mesh']}",
                     'mesh_tf': model_config['mesh_tf']}
-                if model_config['children_mesh'] is not None:
+                if 'children_mesh' in model_config.keys():
                     self.resource_model[f"{resource_config['id']}_"] = {
                         'mesh': f"{str(self.mesh_path)}/device_mesh/resources/{model_config['children_mesh']}",
                         'mesh_tf': model_config['children_mesh_tf']
@@ -300,7 +300,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                     "world",
                     resource_id,
                     rclpy.time.Time(seconds=0),
-                    rclpy.duration.Duration(seconds=5)
+                    # rclpy.duration.Duration(seconds=5)
                 )
                 
                 # 提取当前位姿信息
@@ -391,6 +391,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
             planning_scene = PlanningScene()
             planning_scene.is_diff = True
             planning_scene.robot_state.is_diff = True
+            time_start = self.get_clock().now()
             for resource_id, target_parent in cmd_dict.items():
                 parent_id = target_parent
                 if target_parent == '__trash':
@@ -399,7 +400,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                 transform = self.tf_buffer.lookup_transform(
                     parent_id,
                     resource_id,
-                    self.get_clock().now(),
+                    time_start,
                     timeout=rclpy.duration.Duration(seconds=10)
                 )
                 
@@ -463,11 +464,12 @@ class ResourceMeshManager(BaseROS2DeviceNode):
             #     self.__planning_scene.robot_state.attached_collision_objects.append(collision_object)
             req = ApplyPlanningScene.Request()
             req.scene = planning_scene
-            self._apply_planning_scene_service.call_async(req)
+            self.publish_resource_tf()
+            self._apply_planning_scene_service.call(req)
             self.__planning_scene_publisher.publish(planning_scene)
 
             # self.__collision_object_publisher.publish(CollisionObject())
-            self.publish_resource_tf()
+
             
         except Exception as e:
             self.get_logger().error(f"更新资源TF字典失败: {e}")
@@ -483,7 +485,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
         遍历资源配置字典，为每个在resource_model中有对应模型的资源添加碰撞网格
         
         该方法检查每个资源ID是否在self.resource_model中有对应的3D模型文件路径，
-        如果有，则调用add_collision_mesh方法将其添加到碰撞环境中。
+        
         """
         self.get_logger().info('开始添加资源碰撞网格')
 
@@ -492,6 +494,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
         ).scene
         planning_scene = PlanningScene()
         planning_scene.is_diff = True
+        count = 0
         for resource_id, tf_info in resource_tf_dict.items():
             
             if resource_id in self.resource_model:
@@ -519,6 +522,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                     quat_xyzw=q,
                     frame_id=resource_id
                 )
+                count += 1
                 planning_scene.world.collision_objects.append(collision_object)
             elif f"{tf_info['parent']}_" in self.resource_model:
                 # 获取资源的父级框架ID
@@ -547,14 +551,25 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                     quat_xyzw=q,
                     frame_id=resource_id
                 )
-
+                count += 1
                 planning_scene.world.collision_objects.append(collision_object)
 
+            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
+
         req = ApplyPlanningScene.Request()
         req.scene = planning_scene
-        self._apply_planning_scene_service.call_async(req)
-        self.__planning_scene_publisher.publish(planning_scene)
         self.publish_resource_tf()
+        self._apply_planning_scene_service.call(req)
+        self.__planning_scene_publisher.publish(planning_scene)
 
         self.get_logger().info('资源碰撞网格添加完成')