From 56d45b94f55d1339d54d9725ffe3c8cca7cbb73a Mon Sep 17 00:00:00 2001 From: zhangshixiang <@zhangshixiang> Date: Sat, 7 Jun 2025 22:51:03 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E7=89=A9=E4=BD=93attach?= =?UTF-8?q?=E6=97=B6=EF=BC=8C=E5=A4=9A=E6=AC=A1=E8=B5=8B=E5=80=BC=E5=BD=93?= =?UTF-8?q?=E5=89=8D=E6=97=B6=E9=97=B4=E5=AF=BC=E8=87=B4=E5=8D=A1=E9=A1=BF?= =?UTF-8?q?=E9=97=AE=E9=A2=98=EF=BC=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- unilabos/device_mesh/view_robot.rviz | 139 +++++++++++++++--- .../ros_dev/liquid_handler_joint_publisher.py | 5 +- .../nodes/presets/resource_mesh_manager.py | 35 +++-- 3 files changed, 143 insertions(+), 36 deletions(-) diff --git a/unilabos/device_mesh/view_robot.rviz b/unilabos/device_mesh/view_robot.rviz index f042cc27..368a2edb 100644 --- a/unilabos/device_mesh/view_robot.rviz +++ b/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 - Tree Height: 602 + Splitter Ratio: 0.5016146302223206 + 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 - Voxel Coloring: Z-Axis - Voxel Rendering: Occupied Voxels + Show Scene Geometry: false + Voxel Coloring: Cell Probability + 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: Value: Orbit (rviz) - Yaw: 5.590744495391846 + Yaw: 0.042561568319797516 Saved: ~ Window Geometry: Displays: - collapsed: true - Height: 1897 - Hide Left Dock: true + collapsed: false + Height: 2032 + 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 - X: -857 - Y: 149 + Width: 3956 + X: 140 + Y: 54 diff --git a/unilabos/devices/ros_dev/liquid_handler_joint_publisher.py b/unilabos/devices/ros_dev/liquid_handler_joint_publisher.py index f5fa62a7..c265675e 100644 --- a/unilabos/devices/ros_dev/liquid_handler_joint_publisher.py +++ b/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) diff --git a/unilabos/ros/nodes/presets/resource_mesh_manager.py b/unilabos/ros/nodes/presets/resource_mesh_manager.py index d49f733d..3b494007 100644 --- a/unilabos/ros/nodes/presets/resource_mesh_manager.py +++ b/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,15 +551,26 @@ 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('资源碰撞网格添加完成')