From 2990e70c25a174bb7f6e736d96bc600b5b381c1c Mon Sep 17 00:00:00 2001 From: zhangshixiang <@zhangshixiang> Date: Fri, 25 Apr 2025 10:47:46 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E6=A8=A1=E5=9E=8B=E6=96=B9?= =?UTF-8?q?=E5=90=91=EF=BC=8C=E5=9C=A8yaml=E4=B8=AD=E6=B7=BB=E5=8A=A0?= =?UTF-8?q?=E5=8F=98=E6=8D=A2=E5=B1=9E=E6=80=A7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- test/experiments/test.json | 10 +- unilabos/app/main.py | 7 +- .../macro_device.xacro | 4 +- unilabos/device_mesh/resource_visalization.py | 40 +- unilabos/device_mesh/view_robot.rviz | 923 ++++++++++++++++++ .../devices/ros_dev/resource_mesh_manager.py | 252 ++++- .../registry/resources/opentrons/plates.yaml | 4 +- 7 files changed, 1173 insertions(+), 67 deletions(-) create mode 100644 unilabos/device_mesh/view_robot.rviz diff --git a/test/experiments/test.json b/test/experiments/test.json index a07592c2..97ad5ec7 100644 --- a/test/experiments/test.json +++ b/test/experiments/test.json @@ -9,8 +9,8 @@ "type": "device", "class": "gripper.mock", "position": { - "x": 620.6111111111111, - "y": 171, + "x": 1000, + "y": 1000, "z": 0 }, "config": { @@ -27,9 +27,9 @@ "type": "plate", "class": "nest_96_wellplate_100ul_pcr_full_skirt", "position": { - "x": 620.6111111111111, - "y": 171, - "z": 0 + "x": 0, + "y": 0, + "z": 69 }, "config": { }, diff --git a/unilabos/app/main.py b/unilabos/app/main.py index a31df68a..caa029c1 100644 --- a/unilabos/app/main.py +++ b/unilabos/app/main.py @@ -166,10 +166,9 @@ def main(): resource_visualization = ResourceVisualization(args_dict["devices_config"], args_dict["resources_config"],registry_dict) start_backend(**args_dict) - print('-'*100) - print(resource_visualization.resource_model) - print(json.dumps(args_dict["resources_config"], indent=4, ensure_ascii=False)) - print('-'*100) + # print('-'*100) + # print(resource_visualization.resource_model) + # print('-'*100) server_thread = threading.Thread(target=start_server) server_thread.start() diff --git a/unilabos/device_mesh/devices/opentrons_liquid_handler/macro_device.xacro b/unilabos/device_mesh/devices/opentrons_liquid_handler/macro_device.xacro index 5dc945ba..4e660557 100644 --- a/unilabos/device_mesh/devices/opentrons_liquid_handler/macro_device.xacro +++ b/unilabos/device_mesh/devices/opentrons_liquid_handler/macro_device.xacro @@ -8,14 +8,14 @@ - + - + diff --git a/unilabos/device_mesh/resource_visalization.py b/unilabos/device_mesh/resource_visalization.py index e73a6807..3a616dab 100644 --- a/unilabos/device_mesh/resource_visalization.py +++ b/unilabos/device_mesh/resource_visalization.py @@ -28,7 +28,12 @@ class ResourceVisualization: self.mesh_path = Path(__file__).parent.absolute() self.enable_rviz = enable_rviz + self.srdf_str = ''' + + + + ''' self.robot_state_str= ''' @@ -56,6 +61,13 @@ class ResourceVisualization: new_dev = etree.SubElement(self.root, f"{{{xacro_uri}}}{model_config['mesh']}") new_dev.set("parent_link", "world") new_dev.set("mesh_path", str(self.mesh_path)) + new_dev.set("device_name", node["id"]+"_") + new_dev.set("x",str(float(node["position"]["x"])/1000)) + new_dev.set("y",str(float(node["position"]["y"])/1000)) + new_dev.set("z",str(float(node["position"]["z"])/1000)) + if "rotation" in node["config"]: + new_dev.set("r",str(float(node["config"]["rotation"]["z"])/1000)) + elif node['type'] in self.resource_type: # print(registry.resource_type_registry) @@ -65,9 +77,14 @@ class ResourceVisualization: elif "model" in registry.resource_type_registry[resource_class].keys(): model_config = registry.resource_type_registry[resource_class]['model'] if model_config['type'] == 'resource': - self.resource_model[node['id']] = f"{str(self.mesh_path)}/resources/{model_config['mesh']}" + self.resource_model[node['id']] = { + 'mesh': f"{str(self.mesh_path)}/resources/{model_config['mesh']}", + 'mesh_tf': model_config['mesh_tf']} if model_config['children_mesh'] is not None: - self.resource_model[f"{node['id']}_"] = f"{str(self.mesh_path)}/resources/{model_config['children_mesh']}" + self.resource_model[f"{node['id']}_"] = { + 'mesh': f"{str(self.mesh_path)}/resources/{model_config['children_mesh']}", + 'mesh_tf': model_config['children_mesh_tf'] + } re = etree.tostring(self.root, encoding="unicode") doc = xacro.parse(re) @@ -102,23 +119,35 @@ class ResourceVisualization: }] ) + joint_state_publisher_node = nd( + package='joint_state_publisher_gui', # 或 joint_state_publisher + executable='joint_state_publisher_gui', + name='joint_state_publisher', + output='screen' + ) # 创建move_group节点 move_group = nd( package='moveit_ros_move_group', executable='move_group', output='screen', parameters=[{ - 'robot_description': robot_description, - 'allow_trajectory_execution': True, + 'robot_description': self.robot_state_str, + 'robot_description_semantic': self.srdf_str, 'capabilities': '', 'disable_capabilities': '', 'monitor_dynamics': False, - 'publish_monitored_planning_scene': True + 'publish_monitored_planning_scene': True, + 'publish_robot_description_semantic': True, + 'publish_planning_scene': True, + 'publish_geometry_updates': True, + 'publish_state_updates': True, + 'publish_transforms_updates': True, }] ) # 将节点添加到launch描述中 self.launch_description.add_action(robot_state_publisher) + self.launch_description.add_action(joint_state_publisher_node) self.launch_description.add_action(move_group) # 如果启用RViz,添加RViz节点 @@ -127,6 +156,7 @@ class ResourceVisualization: package='rviz2', executable='rviz2', name='rviz2', + arguments=['-d', f"{str(self.mesh_path)}/view_robot.rviz"], output='screen' ) self.launch_description.add_action(rviz_node) diff --git a/unilabos/device_mesh/view_robot.rviz b/unilabos/device_mesh/view_robot.rviz new file mode 100644 index 00000000..ef202c9d --- /dev/null +++ b/unilabos/device_mesh/view_robot.rviz @@ -0,0 +1,923 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1 + - /TF1/Tree1 + - /RobotModel1 + - /PlanningScene1 + - /PlanningScene1/Scene Geometry1 + - /RobotState1 + - /RobotState1/Links1 + Splitter Ratio: 0.5 + Tree Height: 275 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + Gripper1_device_link: + Value: false + Gripper1_first_link: + Value: true + Gripper1_fourth_link: + Value: false + Gripper1_main_link: + Value: false + Gripper1_second_link: + Value: false + Gripper1_socketTypeGenericSbsFootprint: + Value: false + Gripper1_socketTypeHEPAModule: + Value: false + Gripper1_third_link: + Value: false + Plate1: + Value: false + Plate1_A1: + Value: false + Plate1_A10: + Value: false + Plate1_A11: + Value: false + Plate1_A12: + Value: false + Plate1_A2: + Value: false + Plate1_A3: + Value: false + Plate1_A4: + Value: false + Plate1_A5: + Value: false + Plate1_A6: + Value: false + Plate1_A7: + Value: false + Plate1_A8: + Value: false + Plate1_A9: + Value: false + Plate1_B1: + Value: false + Plate1_B10: + Value: false + Plate1_B11: + Value: false + Plate1_B12: + Value: false + Plate1_B2: + Value: false + Plate1_B3: + Value: false + Plate1_B4: + Value: false + Plate1_B5: + Value: false + Plate1_B6: + Value: false + Plate1_B7: + Value: false + Plate1_B8: + Value: false + Plate1_B9: + Value: false + Plate1_C1: + Value: false + Plate1_C10: + Value: false + Plate1_C11: + Value: false + Plate1_C12: + Value: false + Plate1_C2: + Value: false + Plate1_C3: + Value: false + Plate1_C4: + Value: false + Plate1_C5: + Value: false + Plate1_C6: + Value: false + Plate1_C7: + Value: false + Plate1_C8: + Value: false + Plate1_C9: + Value: false + Plate1_D1: + Value: false + Plate1_D10: + Value: false + Plate1_D11: + Value: false + Plate1_D12: + Value: false + Plate1_D2: + Value: false + Plate1_D3: + Value: false + Plate1_D4: + Value: false + Plate1_D5: + Value: false + Plate1_D6: + Value: false + Plate1_D7: + Value: false + Plate1_D8: + Value: false + Plate1_D9: + Value: false + Plate1_E1: + Value: false + Plate1_E10: + Value: false + Plate1_E11: + Value: false + Plate1_E12: + Value: false + Plate1_E2: + Value: false + Plate1_E3: + Value: false + Plate1_E4: + Value: false + Plate1_E5: + Value: false + Plate1_E6: + Value: false + Plate1_E7: + Value: false + Plate1_E8: + Value: false + Plate1_E9: + Value: false + Plate1_F1: + Value: false + Plate1_F10: + Value: false + Plate1_F11: + Value: false + Plate1_F12: + Value: false + Plate1_F2: + Value: false + Plate1_F3: + Value: false + Plate1_F4: + Value: false + Plate1_F5: + Value: false + Plate1_F6: + Value: false + Plate1_F7: + Value: false + Plate1_F8: + Value: false + Plate1_F9: + Value: false + Plate1_G1: + Value: false + Plate1_G10: + Value: false + Plate1_G11: + Value: false + Plate1_G12: + Value: false + Plate1_G2: + Value: false + Plate1_G3: + Value: false + Plate1_G4: + Value: false + Plate1_G5: + Value: false + Plate1_G6: + Value: false + Plate1_G7: + Value: false + Plate1_G8: + Value: false + Plate1_G9: + Value: false + Plate1_H1: + Value: false + Plate1_H10: + Value: false + Plate1_H11: + Value: false + Plate1_H12: + Value: false + Plate1_H2: + Value: false + Plate1_H3: + Value: false + Plate1_H4: + Value: false + Plate1_H5: + Value: false + Plate1_H6: + Value: false + Plate1_H7: + Value: false + Plate1_H8: + Value: false + Plate1_H9: + Value: false + world: + Value: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + world: + Gripper1_device_link: + Gripper1_main_link: + Gripper1_first_link: + Gripper1_second_link: + Gripper1_fourth_link: + {} + Gripper1_third_link: + {} + Plate1_C6: + {} + Gripper1_socketTypeGenericSbsFootprint: + {} + Gripper1_socketTypeHEPAModule: + {} + Plate1: + Plate1_A1: + {} + Plate1_A10: + {} + Plate1_A11: + {} + Plate1_A12: + {} + Plate1_A2: + {} + Plate1_A3: + {} + Plate1_A4: + {} + Plate1_A5: + {} + Plate1_A6: + {} + Plate1_A7: + {} + Plate1_A8: + {} + Plate1_A9: + {} + Plate1_B1: + {} + Plate1_B10: + {} + Plate1_B11: + {} + Plate1_B12: + {} + Plate1_B2: + {} + Plate1_B3: + {} + Plate1_B4: + {} + Plate1_B5: + {} + Plate1_B6: + {} + Plate1_B7: + {} + Plate1_B8: + {} + Plate1_B9: + {} + Plate1_C1: + {} + Plate1_C10: + {} + Plate1_C11: + {} + Plate1_C12: + {} + Plate1_C2: + {} + Plate1_C3: + {} + Plate1_C4: + {} + Plate1_C7: + {} + Plate1_C8: + {} + Plate1_C9: + {} + Plate1_D1: + {} + Plate1_D10: + {} + Plate1_D11: + {} + Plate1_D12: + {} + Plate1_D2: + {} + Plate1_D3: + {} + Plate1_D4: + {} + Plate1_D5: + {} + Plate1_D6: + {} + Plate1_D7: + {} + Plate1_D8: + {} + Plate1_D9: + {} + Plate1_E1: + {} + Plate1_E10: + {} + Plate1_E11: + {} + Plate1_E12: + {} + Plate1_E2: + {} + Plate1_E3: + {} + Plate1_E4: + {} + Plate1_E5: + {} + Plate1_E6: + {} + Plate1_E7: + {} + Plate1_E8: + {} + Plate1_E9: + {} + Plate1_F1: + {} + Plate1_F10: + {} + Plate1_F11: + {} + Plate1_F12: + {} + Plate1_F2: + {} + Plate1_F3: + {} + Plate1_F4: + {} + Plate1_F5: + {} + Plate1_F6: + {} + Plate1_F7: + {} + Plate1_F8: + {} + Plate1_F9: + {} + Plate1_G1: + {} + Plate1_G10: + {} + Plate1_G11: + {} + Plate1_G12: + {} + Plate1_G2: + {} + Plate1_G3: + {} + Plate1_G4: + {} + Plate1_G5: + {} + Plate1_G6: + {} + Plate1_G7: + {} + Plate1_G8: + {} + Plate1_G9: + {} + Plate1_H1: + {} + Plate1_H10: + {} + Plate1_H11: + {} + Plate1_H12: + {} + Plate1_H2: + {} + Plate1_H3: + {} + Plate1_H4: + {} + Plate1_H5: + {} + Plate1_H6: + {} + Plate1_H7: + {} + Plate1_H8: + {} + Plate1_H9: + {} + Plate1_C5: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Gripper1_device_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Gripper1_first_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Gripper1_fourth_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Gripper1_main_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Gripper1_second_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Gripper1_socketTypeGenericSbsFootprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Gripper1_socketTypeHEPAModule: + Alpha: 1 + Show Axes: false + Show Trail: false + Gripper1_third_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: moveit_rviz_plugin/PlanningScene + Enabled: false + Move Group Namespace: "" + Name: PlanningScene + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + 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 + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Gripper1_device_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Gripper1_first_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Gripper1_fourth_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Gripper1_main_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Gripper1_second_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Gripper1_socketTypeGenericSbsFootprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Gripper1_socketTypeHEPAModule: + Alpha: 1 + Show Axes: false + Show Trail: false + Gripper1_third_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: false + Value: false + - Attached Body Color: 150; 50; 150 + Class: moveit_rviz_plugin/RobotState + Collision Enabled: false + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Gripper1_device_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Gripper1_first_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Gripper1_fourth_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Gripper1_main_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Gripper1_second_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Gripper1_socketTypeGenericSbsFootprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Gripper1_socketTypeHEPAModule: + Alpha: 1 + Show Axes: false + Show Trail: false + Gripper1_third_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotState + Robot Alpha: 1 + Robot Description: robot_description + Robot State Topic: display_robot_state + Show All Links: true + Show Highlights: true + Value: false + Visual Enabled: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Gripper1_device_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Gripper1_first_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Gripper1_fourth_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Gripper1_main_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Gripper1_second_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Gripper1_socketTypeGenericSbsFootprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Gripper1_socketTypeHEPAModule: + Alpha: 1 + Show Axes: false + Show Trail: false + Gripper1_third_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 3x + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: "" + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + 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 + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Gripper1_device_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Gripper1_first_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Gripper1_fourth_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Gripper1_main_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Gripper1_second_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Gripper1_socketTypeGenericSbsFootprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Gripper1_socketTypeHEPAModule: + Alpha: 1 + Show Axes: false + Show Trail: false + Gripper1_third_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 1.49553644657135 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 1.2573390007019043 + Y: 1.1951926946640015 + Z: 0.23975235223770142 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5797955989837646 + Target Frame: + Value: Orbit (rviz) + Yaw: 6.250748634338379 + Saved: ~ +Window Geometry: + "": + collapsed: false + " - Trajectory Slider": + collapsed: false + Displays: + collapsed: false + Height: 1656 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000003a3000005dcfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e0000020b0000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000007a00fffffffbffffffff0100000285000003c5000002b700ffffff000000010000010f00000387fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003870000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000627000005dc00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 2518 + X: 125 + Y: 145 diff --git a/unilabos/devices/ros_dev/resource_mesh_manager.py b/unilabos/devices/ros_dev/resource_mesh_manager.py index 7943189e..7b1b3610 100644 --- a/unilabos/devices/ros_dev/resource_mesh_manager.py +++ b/unilabos/devices/ros_dev/resource_mesh_manager.py @@ -15,8 +15,10 @@ from rclpy.task import Future import copy from typing import Tuple, Optional, Union, Any, List from tf_transformations import quaternion_from_euler -from tf2_ros import StaticTransformBroadcaster - +from tf2_ros import TransformBroadcaster, Buffer, TransformListener +from rclpy.action import ActionServer +from unilabos_msgs.action import SendCmd +from rclpy.action.server import ServerGoalHandle class ResourceMeshManager(Node): def __init__(self, resource_model: dict, resource_config: list, node_name: str): @@ -30,13 +32,13 @@ class ResourceMeshManager(Node): super().__init__(node_name) self.resource_model = resource_model - self.resource_config = resource_config - self.resource_config_dict = {item['id']: item for item in self.resource_config} + self.resource_config_dict = {item['id']: item for item in resource_config} self.move_group_ready = False self.resource_tf_dict = {} - self.tf_broadcaster = StaticTransformBroadcaster(self) - - self.create_timer(1, self.publish_resource_tf) + self.tf_broadcaster = TransformBroadcaster(self) + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + self.create_timer(0.02, self.publish_resource_tf) callback_group = ReentrantCallbackGroup() self._get_planning_scene_service = self.create_client( @@ -74,9 +76,65 @@ class ResourceMeshManager(Node): AttachedCollisionObject, "/attached_collision_object", 10 ) + # 创建一个Action Server用于修改resource_tf_dict + self._action_server = ActionServer( + self, + SendCmd, + f'{node_name}/tf_update', + self.tf_update, + callback_group=callback_group + ) + + def tf_update(self, goal_handle : ServerGoalHandle): + tf_update_msg = goal_handle.request + + try: + cmd_dict = json.loads(tf_update_msg.command.replace("'",'"')) + for resource_id, target_parent in cmd_dict.items(): + + # 获取从resource_id到target_parent的转换 + transform = self.tf_buffer.lookup_transform( + target_parent, + resource_id, + rclpy.time.Time() + ) + + # 提取转换中的位置和旋转信息 + position = { + "x": transform.transform.translation.x, + "y": transform.transform.translation.y, + "z": transform.transform.translation.z + } + + rotation = { + "x": transform.transform.rotation.x, + "y": transform.transform.rotation.y, + "z": transform.transform.rotation.z, + "w": transform.transform.rotation.w + } + + self.resource_tf_dict[resource_id] = { + "parent": target_parent, + "position": position, + "rotation": rotation + } + + print(self.resource_tf_dict) + self.attach_collision_object(id=resource_id,link_name=target_parent) + + self.publish_resource_tf() + + + + except Exception as e: + self.get_logger().error(f"更新资源TF字典失败: {e}") + goal_handle.abort() + return SendCmd.Result(success=False) + goal_handle.succeed() + return SendCmd.Result(success=True) - """检查move_group节点是否已初始化完成""" def check_move_group_ready(self): + """检查move_group节点是否已初始化完成""" while not self.move_group_ready: # 获取当前可用的节点列表 if self._get_planning_scene_service.service_is_ready() and self._apply_planning_scene_service.service_is_ready(): @@ -91,20 +149,54 @@ class ResourceMeshManager(Node): #遍历resource_config中的资源配置，判断panent是否在resource_model中， - for resource_config in self.resource_config: - if resource_config['parent'] in self.resource_model: - # self.resource_tf_dict[resource_config['id']] = resource_config['parent'] - self.resource_tf_dict.update({resource_config['id']:{"parent":resource_config['parent'], - "position":resource_config['position'], - "rotation":resource_config['config']['rotation']}}) - elif resource_config['parent'] is None and resource_config['id'] in self.resource_model: - self.resource_tf_dict.update({resource_config['id']:{'parent':'world', - "position":resource_config['position'], - "rotation":resource_config['config']['rotation']}}) - elif resource_config['parent'] not in self.resource_model and resource_config['parent'] is not None: - self.resource_tf_dict.update({resource_config['id']:{'parent':f"{self.resource_config_dict[resource_config['parent']]['parent']}{resource_config['parent']}_device_link".replace("None",""), - "position":resource_config['position'], - "rotation":resource_config['config']['rotation']}}) + for resource_id, resource_config in self.resource_config_dict.items(): + + parent = resource_config['parent'] + parent_link = 'world' + if parent in self.resource_model: + parent_link = parent + elif parent is None and resource_id in self.resource_model: + pass + elif parent not in self.resource_model and parent is not None: + parent_link = f"{self.resource_config_dict[parent]['parent']}{parent}_device_link".replace("None","") + else: + continue + # 提取位置信息并转换单位 + position = { + "x": float(resource_config['position']['x'])/1000, + "y": float(resource_config['position']['y'])/1000, + "z": float(resource_config['position']['z'])/1000 + } + + rotation_dict = { + "x": 0, + "y": 0, + "z": 0 + } + + if 'rotation' in resource_config['config']: + rotation_dict = resource_config['config']['rotation'] + + # 从欧拉角转换为四元数 + q = quaternion_from_euler( + float(rotation_dict['x']), + float(rotation_dict['y']), + float(rotation_dict['z']) + ) + + rotation = { + "x": q[0], + "y": q[1], + "z": q[2], + "w": q[3] + } + + # 更新资源TF字典 + self.resource_tf_dict[resource_id] = { + "parent": parent_link, + "position": position, + "rotation": rotation + } def publish_resource_tf(self): """ @@ -113,12 +205,7 @@ class ResourceMeshManager(Node): 遍历self.resource_tf_dict中的每个元素，根据key，parent，以及position和rotation， 发布key和parent之间的tf关系 """ - self.get_logger().info('开始发布资源TF关系') - - # 创建静态TF广播器 - - - # 存储所有需要发布的静态变换 + transforms = [] # 遍历资源TF字典 @@ -135,31 +222,92 @@ class ResourceMeshManager(Node): transform.child_frame_id = resource_id # 设置位置 - transform.transform.translation.x = float(position['x'])/1000 - transform.transform.translation.y = float(position['y'])/1000 - transform.transform.translation.z = float(position['z'])/1000 - - # 从欧拉角转换为四元数 - q = quaternion_from_euler( - float(rotation['x']), - float(rotation['y']), - float(rotation['z']) - ) + transform.transform.translation.x = float(position['x']) + transform.transform.translation.y = float(position['y']) + transform.transform.translation.z = float(position['z']) # 设置旋转 - transform.transform.rotation.x = q[0] - transform.transform.rotation.y = q[1] - transform.transform.rotation.z = q[2] - transform.transform.rotation.w = q[3] + transform.transform.rotation.x = rotation['x'] + transform.transform.rotation.y = rotation['y'] + transform.transform.rotation.z = rotation['z'] + transform.transform.rotation.w = rotation['w'] transforms.append(transform) # 一次性发布所有静态变换 if transforms: self.tf_broadcaster.sendTransform(transforms) - self.get_logger().info(f'已发布 {len(transforms)} 个资源TF关系') + # self.get_logger().info(f'已发布 {len(transforms)} 个资源TF关系') + def add_resource_collision_meshes(self): + """ + 遍历资源配置字典，为每个在resource_model中有对应模型的资源添加碰撞网格 + 该方法检查每个资源ID是否在self.resource_model中有对应的3D模型文件路径， + 如果有，则调用add_collision_mesh方法将其添加到碰撞环境中。 + """ + self.get_logger().info('开始添加资源碰撞网格') + + for resource_id, tf_info in self.resource_tf_dict.items(): + + if resource_id in self.resource_model: + # 获取位置信息 + + position = [ + float(self.resource_model[resource_id]['mesh_tf'][0]), + float(self.resource_model[resource_id]['mesh_tf'][1]), + float(self.resource_model[resource_id]['mesh_tf'][2]) + ] + + # 获取旋转信息并转换为四元数 + + q = quaternion_from_euler( + float(self.resource_model[resource_id]['mesh_tf'][3]), + float(self.resource_model[resource_id]['mesh_tf'][4]), + float(self.resource_model[resource_id]['mesh_tf'][5]) + ) + + # 添加碰撞网格 + self.add_collision_mesh( + filepath=self.resource_model[resource_id]['mesh'], + id=resource_id, + position=position, + quat_xyzw=q, + frame_id=resource_id + ) + + elif f"{tf_info['parent']}_" in self.resource_model: + # 获取资源的父级框架ID + id_ = f"{tf_info['parent']}_" + + # 获取位置信息 + position = [ + float(self.resource_model[id_]['mesh_tf'][0]), + float(self.resource_model[id_]['mesh_tf'][1]), + float(self.resource_model[id_]['mesh_tf'][2]) + ] + + # 获取旋转信息并转换为四元数 + + q = quaternion_from_euler( + float(self.resource_model[id_]['mesh_tf'][3]), + float(self.resource_model[id_]['mesh_tf'][4]), + float(self.resource_model[id_]['mesh_tf'][5]) + ) + + # 添加碰撞网格 + self.add_collision_mesh( + filepath=self.resource_model[id_]['mesh'], + id=resource_id, + position=position, + quat_xyzw=q, + frame_id=resource_id + ) + time.sleep(0.01) + + self.get_logger().info('资源碰撞网格添加完成') + + def add_collision_primitive( self, id: str, @@ -672,7 +820,8 @@ class ResourceMeshManager(Node): if __name__ == '__main__': model_s = ''' -{'Plate1': '/home/z43/git_pj/Uni-Lab-OS/unilabos/device_mesh/resources/tecan_nested_tip_rack/meshes/plate.stl', 'Plate1_': '/home/z43/git_pj/Uni-Lab-OS/unilabos/device_mesh/resources/generic_labware_tube_10_75/meshes/0_base.stl'} +{'Plate1': {'mesh': '/home/z43/git_pj/Uni-Lab-OS/unilabos/device_mesh/resources/tecan_nested_tip_rack/meshes/plate.stl', 'mesh_tf': [0.064, 0.043, 0, -1.5708, 0, 1.5708]}, +'Plate1_': {'mesh': '/home/z43/git_pj/Uni-Lab-OS/unilabos/device_mesh/resources/generic_labware_tube_10_75/meshes/0_base.stl', 'mesh_tf': [0.0018, 0.0018, 0, -1.5708,0, 0]}} ''' resource_model = json.loads(model_s.replace("'",'"')) @@ -686,8 +835,8 @@ if __name__ == '__main__': "type": "device", "class": "gripper.mock", "position": { - "x": 620.6111111111111, - "y": 171, + "x": 0, + "y": 0, "z": 0 }, "config": {}, @@ -799,9 +948,9 @@ if __name__ == '__main__': "type": "device", "class": "", "position": { - "x": 620.6111111111111, - "y": 171, - "z": 0 + "x": 0, + "y": 0, + "z": 69 }, "config": { "type": "Plate", @@ -4668,5 +4817,8 @@ if __name__ == '__main__': rclpy.init() resource_mesh_manager = ResourceMeshManager(resource_model, resource_config, 'resource_mesh_manager') resource_mesh_manager.resource_mesh_setup() - print(json.dumps(resource_mesh_manager.resource_tf_dict, indent=4, ensure_ascii=False)) + resource_mesh_manager.publish_resource_tf() + # resource_mesh_manager.clear_all_collision_objects() + # print(json.dumps(resource_mesh_manager.resource_tf_dict, indent=4, ensure_ascii=False)) + resource_mesh_manager.add_resource_collision_meshes() rclpy.spin(resource_mesh_manager) \ No newline at end of file diff --git a/unilabos/registry/resources/opentrons/plates.yaml b/unilabos/registry/resources/opentrons/plates.yaml index 0a7c4b84..c00b2744 100644 --- a/unilabos/registry/resources/opentrons/plates.yaml +++ b/unilabos/registry/resources/opentrons/plates.yaml @@ -54,8 +54,10 @@ nest_96_wellplate_100ul_pcr_full_skirt: model: type: resource mesh: tecan_nested_tip_rack/meshes/plate.stl + mesh_tf: [0.064, 0.043, 0, -1.5708, 0, 1.5708] children_mesh: generic_labware_tube_10_75/meshes/0_base.stl - + children_mesh_tf: [0.0018, 0.0018, 0, -1.5708,0, 0] + appliedbiosystemsmicroamp_384_wellplate_40ul: description: Applied Biosystems microamp 384 wellplate 40ul class: