完成启动OT并联动rviz

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,
+                "with_trash": false,
-                "size_y": 653.5,
-                "size_z": 900,
                 "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,
+                "x": 132.5,
-                "y": 0,
+                "y": 271.5,
                 "z": 69
             },
             "config": {
                 "type": "Resource",
-                "size_x": 30,
+                "size_x": 127,
-                "size_y": 445.2,
+                "size_y": 85,
-                "size_z": 100,
+                "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": [

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']
                 
                 # 检查设备类型是否在注册表中

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: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 6.070750713348389
+      Yaw: 6.020748138427734
     Saved: ~
 Window Geometry:
   Displays:

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()

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:
+            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","")
+                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 = {

unilabos/registry/devices/deck.yaml

@@ -1,8 +1,5 @@
 OTDeck:
-  description: Opentrons deck
+  description: Opentrons deck 3d model
-  class: 
-    module: pylabrobot.resources.opentrons.deck:OTDeck
-    type: pylabrobot
   model:
     type: device
     mesh: opentrons_liquid_handler

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: 

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))