From 07d9db20c3b43bc8af048b9e2c4af210ec16d446 Mon Sep 17 00:00:00 2001 From: zhangshixiang <@zhangshixiang> Date: Sat, 7 Jun 2025 22:52:03 +0800 Subject: [PATCH] =?UTF-8?q?Revert=20"=E4=BF=AE=E6=94=B9=E7=89=A9=E4=BD=93a?= =?UTF-8?q?ttach=E6=97=B6=EF=BC=8C=E5=A4=9A=E6=AC=A1=E8=B5=8B=E5=80=BC?= =?UTF-8?q?=E5=BD=93=E5=89=8D=E6=97=B6=E9=97=B4=E5=AF=BC=E8=87=B4=E5=8D=A1?= =?UTF-8?q?=E9=A1=BF=E9=97=AE=E9=A2=98=EF=BC=8C"?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This reverts commit 56d45b94f55d1339d54d9725ffe3c8cca7cbb73a. --- 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, 36 insertions(+), 143 deletions(-) diff --git a/unilabos/device_mesh/view_robot.rviz b/unilabos/device_mesh/view_robot.rviz index 368a2edb..f042cc27 100644 --- a/unilabos/device_mesh/view_robot.rviz +++ b/unilabos/device_mesh/view_robot.rviz @@ -4,12 +4,15 @@ 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.5016146302223206 - Tree Height: 1112 + Splitter Ratio: 0.4940796494483948 + Tree Height: 602 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -53,7 +56,7 @@ Visualization Manager: Name: TF Show Arrows: true Show Axes: true - Show Names: false + Show Names: true Tree: {} Update Interval: 0 @@ -85,7 +88,7 @@ Visualization Manager: Value: false Visual Enabled: true - Class: moveit_rviz_plugin/PlanningScene - Enabled: true + Enabled: false Move Group Namespace: "" Name: PlanningScene Planning Scene Topic: /monitored_planning_scene @@ -96,7 +99,7 @@ Visualization Manager: Scene Display Time: 0.009999999776482582 Show Scene Geometry: true Voxel Coloring: Z-Axis - Voxel Rendering: Disabled + Voxel Rendering: Occupied Voxels Scene Robot: Attached Body Color: 150; 50; 150 Links: @@ -105,108 +108,10 @@ 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: true + Value: false - Attached Body Color: 150; 50; 150 Class: moveit_rviz_plugin/RobotState Collision Enabled: false @@ -389,9 +294,9 @@ Visualization Manager: Scene Alpha: 0.8999999761581421 Scene Color: 50; 230; 50 Scene Display Time: 0.009999999776482582 - Show Scene Geometry: false - Voxel Coloring: Cell Probability - Voxel Rendering: All Voxels + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels Scene Robot: Attached Body Color: 150; 50; 150 Links: @@ -564,28 +469,28 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.48479583859443665 + Pitch: 0.634795606136322 Target Frame: Value: Orbit (rviz) - Yaw: 0.042561568319797516 + Yaw: 5.590744495391846 Saved: ~ Window Geometry: Displays: - collapsed: false - Height: 2032 - Hide Left Dock: false + collapsed: true + Height: 1897 + Hide Left Dock: true Hide Right Dock: true MotionPlanning: - collapsed: false + collapsed: true MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000003a30000079bfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000027000004c60000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000002600000026fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000004f9000002c9000002b800ffffff000000010000010f00000387fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003870000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000bc50000079b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000003a300000714fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000027000002c80000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000002600000026fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000002fb00000440000002b800ffffff000000010000010f00000387fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003870000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000ddb0000071400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 Selection: collapsed: false Tool Properties: collapsed: false Views: collapsed: true - Width: 3956 - X: 140 - Y: 54 + Width: 3547 + X: -857 + Y: 149 diff --git a/unilabos/devices/ros_dev/liquid_handler_joint_publisher.py b/unilabos/devices/ros_dev/liquid_handler_joint_publisher.py index c265675e..f5fa62a7 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(goal_msg) + self.resource_action_client.send_goal_async(goal_msg) def resource_move(self, resource_id:str, link_name:str, channels:list[int]): resource = resource_id.rsplit("_",1) @@ -247,10 +247,13 @@ 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 3b494007..d49f733d 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 'children_mesh' in model_config.keys(): + if model_config['children_mesh'] is not None: 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,7 +391,6 @@ 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': @@ -400,7 +399,7 @@ class ResourceMeshManager(BaseROS2DeviceNode): transform = self.tf_buffer.lookup_transform( parent_id, resource_id, - time_start, + self.get_clock().now(), timeout=rclpy.duration.Duration(seconds=10) ) @@ -464,12 +463,11 @@ class ResourceMeshManager(BaseROS2DeviceNode): # self.__planning_scene.robot_state.attached_collision_objects.append(collision_object) req = ApplyPlanningScene.Request() req.scene = planning_scene - self.publish_resource_tf() - self._apply_planning_scene_service.call(req) + self._apply_planning_scene_service.call_async(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}") @@ -485,7 +483,7 @@ class ResourceMeshManager(BaseROS2DeviceNode): 遍历资源配置字典，为每个在resource_model中有对应模型的资源添加碰撞网格 该方法检查每个资源ID是否在self.resource_model中有对应的3D模型文件路径， - + 如果有，则调用add_collision_mesh方法将其添加到碰撞环境中。 """ self.get_logger().info('开始添加资源碰撞网格') @@ -494,7 +492,6 @@ 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: @@ -522,7 +519,6 @@ 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 @@ -551,26 +547,15 @@ 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 + planning_scene.world.collision_objects.append(collision_object) req = ApplyPlanningScene.Request() req.scene = planning_scene - self.publish_resource_tf() - self._apply_planning_scene_service.call(req) + self._apply_planning_scene_service.call_async(req) self.__planning_scene_publisher.publish(planning_scene) - + self.publish_resource_tf() + self.get_logger().info('资源碰撞网格添加完成')