From dc197bffe8c23d62fc28069f8ed3364a2bf9c9d8 Mon Sep 17 00:00:00 2001 From: zhangshixiang <@zhangshixiang> Date: Tue, 29 Apr 2025 22:13:34 +0800 Subject: [PATCH] =?UTF-8?q?=E5=AE=8C=E6=88=90=E5=90=AF=E5=8A=A8OT=E5=B9=B6?= =?UTF-8?q?=E8=81=94=E5=8A=A8rviz?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- test/experiments/test_copy.json | 67 ++++++-- unilabos/device_mesh/resource_visalization.py | 2 +- unilabos/device_mesh/view_robot.rviz | 158 +++++++++--------- .../ros_dev/liquid_handler_joint_publisher.py | 113 +++++++++++++ .../devices/ros_dev/resource_mesh_manager.py | 6 +- unilabos/registry/devices/deck.yaml | 5 +- .../resources/opentrons/tip_racks.yaml | 8 +- unilabos/ros/nodes/resource_tracker.py | 1 + 8 files changed, 257 insertions(+), 103 deletions(-) create mode 100644 unilabos/devices/ros_dev/liquid_handler_joint_publisher.py diff --git a/test/experiments/test_copy.json b/test/experiments/test_copy.json index c2b31e40..d6b861b4 100644 --- a/test/experiments/test_copy.json +++ b/test/experiments/test_copy.json @@ -20,7 +20,7 @@ } ], "backend": { - "type": "LiquidHandlerChatterboxBackend" + "type": "LiquidHandlerRvizBackend" } } }, @@ -34,7 +34,6 @@ "name": "deck", "sample_id": null, "children": [ - "teaching_carrier" ], "parent": "PLR_STATION", @@ -47,20 +46,13 @@ }, "config": { "type": "OTDeck", - "size_x": 1360, - "size_y": 653.5, - "size_z": 900, + "with_trash": false, "rotation": { "x": 0, "y": 0, "z": 0, "type": "Rotation" - }, - "category": "deck", - "num_rails": 32, - "with_trash": false, - "with_trash96": false, - "with_teaching_rack": false + } }, "data": {} }, @@ -70,20 +62,21 @@ "name": "teaching_carrier", "sample_id": null, "children": [ + "teaching_carrier_A1" ], "parent": "deck", "type": "plate", - "class": "nest_96_wellplate_100ul_pcr_full_skirt", + "class": "opentrons_96_filtertiprack_1000ul", "position": { - "x": 0, - "y": 0, + "x": 132.5, + "y": 271.5, "z": 69 }, "config": { "type": "Resource", - "size_x": 30, - "size_y": 445.2, - "size_z": 100, + "size_x": 127, + "size_y": 85, + "size_z": 0, "rotation": { "x": 0, "y": 0, @@ -94,6 +87,46 @@ "model": null }, "data": {} + }, + { + "id": "teaching_carrier_A1", + "name": "teaching_carrier_A1", + "sample_id": null, + "children": [], + "parent": "teaching_carrier", + "type": "device", + "class": "", + "position": { + "x": 10.87, + "y": 70.77, + "z": 3.03 + }, + "config": { + "type": "TipSpot", + "size_x": 6.86, + "size_y": 6.86, + "size_z": 10.67, + "rotation": { + "x": 0, + "y": 0, + "z": 0, + "type": "Rotation" + }, + "category": "tip_spot", + "model": null, + "prototype_tip": { + "type": "Tip", + "total_tip_length": 39.2, + "has_filter": true, + "maximal_volume": 20.0, + "fitting_depth": 3.29 + } + }, + "data": { + "liquids": [], + "pending_liquids": [], + "liquid_history": [] + } } ], "links": [ diff --git a/unilabos/device_mesh/resource_visalization.py b/unilabos/device_mesh/resource_visalization.py index 1a9a50fd..e56e96fa 100644 --- a/unilabos/device_mesh/resource_visalization.py +++ b/unilabos/device_mesh/resource_visalization.py @@ -45,7 +45,7 @@ class ResourceVisualization: # 遍历设备节点 for node in device.values(): - if node['type'] == 'device': + if node['type'] == 'device' and node['class'] != '': device_class = node['class'] # 检查设备类型是否在注册表中 diff --git a/unilabos/device_mesh/view_robot.rviz b/unilabos/device_mesh/view_robot.rviz index 4b6c9f53..df19a69e 100644 --- a/unilabos/device_mesh/view_robot.rviz +++ b/unilabos/device_mesh/view_robot.rviz @@ -12,6 +12,10 @@ Panels: - /PlanningScene1/Scene Geometry1 - /RobotState1 - /RobotState1/Links1 + - /MotionPlanning1 + - /MotionPlanning1/Scene Geometry1 + - /MotionPlanning1/Scene Robot1 + - /MotionPlanning1/Planning Request1 Splitter Ratio: 0.5 Tree Height: 345 - Class: rviz_common/Selection @@ -161,44 +165,44 @@ Visualization Manager: 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 + deck_device_link: + Alpha: 1 + Show Axes: false + Show Trail: false + deck_first_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + deck_fourth_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + deck_main_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + deck_second_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + deck_socketTypeGenericSbsFootprint: + Alpha: 1 + Show Axes: false + Show Trail: false + deck_socketTypeHEPAModule: + Alpha: 1 + Show Axes: false + Show Trail: false + deck_third_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true world: Alpha: 1 Show Axes: false @@ -227,7 +231,7 @@ Visualization Manager: Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 Planning Group: "" - Query Goal State: true + Query Goal State: false Query Start State: false Show Workspace: false Start State Alpha: 1 @@ -248,44 +252,44 @@ Visualization Manager: 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 + deck_device_link: + Alpha: 1 + Show Axes: false + Show Trail: false + deck_first_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + deck_fourth_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + deck_main_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + deck_second_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + deck_socketTypeGenericSbsFootprint: + Alpha: 1 + Show Axes: false + Show Trail: false + deck_socketTypeHEPAModule: + Alpha: 1 + Show Axes: false + Show Trail: false + deck_third_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true world: Alpha: 1 Show Axes: false @@ -356,10 +360,10 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.40479576587677 + Pitch: 0.4347957372665405 Target Frame: Value: Orbit (rviz) - Yaw: 6.070750713348389 + Yaw: 6.020748138427734 Saved: ~ Window Geometry: Displays: diff --git a/unilabos/devices/ros_dev/liquid_handler_joint_publisher.py b/unilabos/devices/ros_dev/liquid_handler_joint_publisher.py new file mode 100644 index 00000000..15adde08 --- /dev/null +++ b/unilabos/devices/ros_dev/liquid_handler_joint_publisher.py @@ -0,0 +1,113 @@ +import rclpy +import json +import time +from rclpy.executors import MultiThreadedExecutor +from rclpy.action import ActionServer +from sensor_msgs.msg import JointState +from ilabos_msgs.action import SendCmd +from rclpy.action.server import ServerGoalHandle +from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode +from tf_transformations import quaternion_from_euler +from tf2_ros import TransformBroadcaster, Buffer, TransformListener + + +class LiquidHandlerJointPublisher(BaseROS2DeviceNode): + def __init__(self,device_id:str, joint_config:dict,resource_tracker): + super().__init__( + driver_instance=self, + device_id=device_id, + status_types={}, + action_value_mappings={}, + hardware_interface={}, + print_publish=False, + resource_tracker=resource_tracker, + ) + # self.station_name = station_name + # self.dev_name = dev_name + self.j_msg = JointState() + # self.j_msg.name = joint_names + self.j_msg.name = [device_id+x for x in joint_config.keys()] + self.rate = 50 + self.j_msg.position = [0.0 for i in range(len(joint_config.keys()))] + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + self.j_pub = self.create_publisher(JointState,'/joint_states',10) + self.create_timer(0.02,self.sim_joint_pub_callback) + self.j_action = ActionServer( + self, + SendCmd, + "joint", + self.sim_joint_action_callback, + result_timeout=5000 + ) + + def sim_joint_action_callback(self,goal_handle: ServerGoalHandle): + """Move a single joint + + Args: + command: A JSON-formatted string that includes joint_name, speed, position + + joint_name (str): The name of the joint to move + speed (float): The speed of the movement, speed > 0 + position (float): The position to move to + + Returns: + None + """ + result = SendCmd.Result() + cmd_str = str(goal_handle.request.command).replace('\'','\"') + # goal_handle.execute() + + try: + cmd_dict = json.loads(cmd_str) + self.move_joint(**cmd_dict) + result.success = True + goal_handle.succeed() + + except Exception as e: + print(e) + goal_handle.abort() + result.success = False + + return result + + + def move_joint(self, joint_name, speed, position): + + # joint_index = self.j_msg.name.index(self.station_name+self.dev_name+joint_name) + joint_index = self.j_msg.name.index(joint_name) + distance = position - self.j_msg.position[joint_index] + if distance == 0: + return + flag = abs(distance)/distance + + while abs(distance)>speed/self.rate: + + self.j_msg.position[joint_index] += flag*speed/self.rate + self.sim_joint_pub_callback() + time.sleep(0.02) + distance = position - self.j_msg.position[joint_index] + + self.j_msg.position[joint_index] = position + self.sim_joint_pub_callback() + + + def sim_joint_pub_callback(self): + self.j_msg.header.stamp = self.get_clock().now().to_msg() + self.j_pub.publish(self.j_msg) + +def main(): + + joint_json:dict = json.load(open("device_data.json", encoding='utf-8')) + joint_action_list = {} + rclpy.init() + executor = MultiThreadedExecutor() + for station_name,dev_dict in joint_json.items(): + for dev_name,joint_names in dev_dict.items(): + joint_action_list[station_name+'_'+dev_name] = SimJointPublisher(station_name,dev_name,joint_names) + executor.add_node(joint_action_list[station_name+'_'+dev_name]) + + executor.spin() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/unilabos/devices/ros_dev/resource_mesh_manager.py b/unilabos/devices/ros_dev/resource_mesh_manager.py index c738b9a4..cf43b804 100644 --- a/unilabos/devices/ros_dev/resource_mesh_manager.py +++ b/unilabos/devices/ros_dev/resource_mesh_manager.py @@ -135,8 +135,8 @@ class ResourceMeshManager(BaseROS2DeviceNode): 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","") + elif parent not in self.resource_model and parent is not None and resource_config['class'] != '': + parent_link = f"{self.resource_config_dict[parent]['parent']}_{parent}_device_link".replace("None","") else: continue # 提取位置信息并转换单位 @@ -164,7 +164,7 @@ class ResourceMeshManager(BaseROS2DeviceNode): # print("-"*20) # print(f"resource_id: {resource_id}") # print(f"parent: {parent}") - # print(f"resource_config: {self.resource_model}") + # print(f"resource_config: {resource_config}") # print(f"parent_link: {parent_link}") # print("-"*20) rotation = { diff --git a/unilabos/registry/devices/deck.yaml b/unilabos/registry/devices/deck.yaml index 652240a5..4ebd46c1 100644 --- a/unilabos/registry/devices/deck.yaml +++ b/unilabos/registry/devices/deck.yaml @@ -1,8 +1,5 @@ OTDeck: - description: Opentrons deck - class: - module: pylabrobot.resources.opentrons.deck:OTDeck - type: pylabrobot + description: Opentrons deck 3d model model: type: device mesh: opentrons_liquid_handler \ No newline at end of file diff --git a/unilabos/registry/resources/opentrons/tip_racks.yaml b/unilabos/registry/resources/opentrons/tip_racks.yaml index c5292e2c..a605b55f 100644 --- a/unilabos/registry/resources/opentrons/tip_racks.yaml +++ b/unilabos/registry/resources/opentrons/tip_racks.yaml @@ -63,7 +63,13 @@ opentrons_96_filtertiprack_1000ul: class: module: pylabrobot.resources.opentrons.tip_racks:opentrons_96_filtertiprack_1000ul type: pylabrobot - + 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] + opentrons_96_filtertiprack_20ul: description: Opentrons 96 filtertiprack 20ul class: diff --git a/unilabos/ros/nodes/resource_tracker.py b/unilabos/ros/nodes/resource_tracker.py index c0886cd4..dc9f9c4a 100644 --- a/unilabos/ros/nodes/resource_tracker.py +++ b/unilabos/ros/nodes/resource_tracker.py @@ -48,6 +48,7 @@ class DeviceNodeResourceTracker: def loop_find_resource(self, resource, resource_cls_type, identifier_key, compare_value): res_list = [] + print(resource, resource_cls_type, identifier_key, compare_value) children = getattr(resource, "children", []) for child in children: res_list.extend(self.loop_find_resource(child, resource_cls_type, identifier_key, compare_value))