Merge branch 'device_visualization' of https://github.com/q434343/Uni-Lab-OS into device_visualization

增加post_init阶段，给予ros_node反向

README.md

@@ -49,7 +49,7 @@ conda env update --file unilabos-[YOUR_OS].yml -n environment_name
 
 # Currently, you need to install the `unilabos_msgs` package
 # You can download the system-specific package from the Release page
-conda install ros-humble-unilabos-msgs-0.9.2-xxxxx.tar.bz2
+conda install ros-humble-unilabos-msgs-0.9.3-xxxxx.tar.bz2
 
 # Install PyLabRobot and other prerequisites
 git clone https://github.com/PyLabRobot/pylabrobot plr_repo

README_zh.md

@@ -49,7 +49,7 @@ conda env update --file unilabos-[YOUR_OS].yml -n 环境名
 
 # 现阶段，需要安装 `unilabos_msgs` 包
 # 可以前往 Release 页面下载系统对应的包进行安装
-conda install ros-humble-unilabos-msgs-0.9.2-xxxxx.tar.bz2
+conda install ros-humble-unilabos-msgs-0.9.3-xxxxx.tar.bz2
 
 # 安装PyLabRobot等前置
 git clone https://github.com/PyLabRobot/pylabrobot plr_repo

recipes/ros-humble-unilabos-msgs/recipe.yaml

@@ -1,6 +1,6 @@
 package:
   name: ros-humble-unilabos-msgs
-  version: 0.9.2
+  version: 0.9.3
 source:
   path: ../../unilabos_msgs
   folder: ros-humble-unilabos-msgs/src/work

recipes/unilabos/recipe.yaml

@@ -1,6 +1,6 @@
 package:
   name: unilabos
-  version: "0.9.2"
+  version: "0.9.3"
 
 source:
   path: ../..

setup.cfg

@@ -1,4 +1,2 @@
-[develop]
-script_dir=$base/lib/unilabos
 [install]
 install_scripts=$base/lib/unilabos

setup.py

@@ -4,7 +4,7 @@ package_name = 'unilabos'
 
 setup(
     name=package_name,
-    version='0.9.2',
+    version='0.9.3',
     packages=find_packages(),
     include_package_data=True,
     install_requires=['setuptools'],

test/experiments/plr_test_converted.json

@@ -5930,8 +5930,8 @@
                 "cross_section_type": "rectangle"
             },
             "data": {
-                "liquids": [],
-                "pending_liquids": [],
+                "liquids": [["water", 50.0]],
+                "pending_liquids": [["water", 50.0]],
                 "liquid_history": []
             }
         },
@@ -5969,8 +5969,8 @@
                 "cross_section_type": "rectangle"
             },
             "data": {
-                "liquids": [],
-                "pending_liquids": [],
+                "liquids": [["water", 50.0]],
+                "pending_liquids": [["water", 50.0]],
                 "liquid_history": []
             }
         },
@@ -6008,8 +6008,8 @@
                 "cross_section_type": "rectangle"
             },
             "data": {
-                "liquids": [],
-                "pending_liquids": [],
+                "liquids": [["water", 50.0]],
+                "pending_liquids": [["water", 50.0]],
                 "liquid_history": []
             }
         },
@@ -6047,8 +6047,8 @@
                 "cross_section_type": "rectangle"
             },
             "data": {
-                "liquids": [],
-                "pending_liquids": [],
+                "liquids": [["water", 50.0]],
+                "pending_liquids": [["water", 50.0]],
                 "liquid_history": []
             }
         },
@@ -6086,8 +6086,8 @@
                 "cross_section_type": "rectangle"
             },
             "data": {
-                "liquids": [],
-                "pending_liquids": [],
+                "liquids": [["water", 50.0]],
+                "pending_liquids": [["water", 50.0]],
                 "liquid_history": []
             }
         },
@@ -6125,8 +6125,8 @@
                 "cross_section_type": "rectangle"
             },
             "data": {
-                "liquids": [],
-                "pending_liquids": [],
+                "liquids": [["water", 50.0]],
+                "pending_liquids": [["water", 50.0]],
                 "liquid_history": []
             }
         },
@@ -6164,8 +6164,8 @@
                 "cross_section_type": "rectangle"
             },
             "data": {
-                "liquids": [],
-                "pending_liquids": [],
+                "liquids": [["water", 50.0]],
+                "pending_liquids": [["water", 50.0]],
                 "liquid_history": []
             }
         },
@@ -6203,8 +6203,8 @@
                 "cross_section_type": "rectangle"
             },
             "data": {
-                "liquids": [],
-                "pending_liquids": [],
+                "liquids": [["water", 50.0]],
+                "pending_liquids": [["water", 50.0]],
                 "liquid_history": []
             }
         },

unilabos/device_mesh/resource_visalization.py

@@ -125,8 +125,13 @@ class ResourceVisualization:
                         new_dev.set("parent_link", "world")
                         new_dev.set("mesh_path", str(self.mesh_path))
                         new_dev.set("device_name", node["id"]+"_")
-                        if node["parent"] is not None:
-                            new_dev.set("station_name", node["parent"]+'_')
+                        # if node["parent"] is not None:
+                        #     new_dev.set("station_name", node["parent"]+'_')
+
+                        print('o'*20)
+                        node["parent"]
+                        node["id"]
+                        print('o'*20)
                         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))

unilabos/devices/ros_dev/lh_joint_config.json

@@ -15,7 +15,7 @@
         "y":{
             "first_joint":{
             "factor":-0.001,
-            "offset":0.163
+            "offset":0.166
             }
         },
         "x":{

unilabos/devices/ros_dev/liquid_handler_joint_publisher.py

@@ -57,7 +57,8 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
             if resource['class'] == 'liquid_handler':
                 deck_id = resource['config']['data']['children'][0]['_resource_child_name']
                 deck_class = resource['config']['data']['children'][0]['_resource_type'].split(':')[-1]
-                key = f'{resource["id"]}_{deck_id}'
+                key = f'{deck_id}'
+                # key = f'{resource["id"]}_{deck_id}'
                 self.lh_devices[key] = {
                     'joint_msg':JointState(
                         name=[f'{key}_{x}' for x in joint_config[deck_class]['joint_names']],
@@ -67,7 +68,9 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
                 }
                 self.deck_list.append(deck_id)
 
-
+        print('='*20)
+        print(self.lh_devices)
+        print('='*20)
         self.j_action       = ActionServer(
             self,
             SendCmd,
@@ -103,7 +106,7 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
         try:
             if parent_id in self.deck_list:
                 p_ = self.resources_config[parent_id]['parent']
-                str_ = f'{p_}_{parent_id}'
+                str_ = f'{parent_id}'
                 return str(str_)
             else:
                 return self.find_resource_parent(parent_id)
@@ -215,6 +218,10 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
         
         parent_id = self.find_resource_parent(resource_name_)
 
+
+        print('!'*20)
+        print(parent_id)
+        print('!'*20)
         if x_joint is None:
             xa,xb = next(iter(self.lh_devices[parent_id]['joint_config']['x'].items()))
             x_joint_config = {xa:xb}
@@ -243,6 +250,7 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
 
         self.move_to(joint_positions_target_zero, speed, parent_id)
         self.move_to(joint_positions_target, speed, parent_id)
+        time.sleep(1)
         if option == "pick":
             link_name =  self.lh_devices[parent_id]['joint_config']['link_names'][z_index]
             link_name =  f'{parent_id}_{link_name}'

unilabos/devices/ros_dev/moveit_interface.py

@@ -1,109 +1,66 @@
-from pathlib import Path
-from threading import Thread
+import json
 import time
-
-import rclpy
-import rclpy.duration
-import rclpy.time
-from unilabos_msgs.action import SendCmd
-from rclpy.action import ActionServer, ActionClient
-from rclpy.action.server import ServerGoalHandle
-from rclpy.callback_groups import ReentrantCallbackGroup
-from moveit_msgs.msg import JointConstraint, Constraints
 from copy import deepcopy
+from pathlib import Path
+
+from moveit_msgs.msg import JointConstraint, Constraints
+from rclpy.action import ActionClient
+from tf2_ros import Buffer, TransformListener
+from unilabos_msgs.action import SendCmd
 
 from unilabos.devices.ros_dev.moveit2 import MoveIt2
-
-import numpy as np
 from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode
 
-# from tf_transformations import quaternion_from_euler
-from tf2_ros import Buffer, TransformListener
-import json
 
+class MoveitInterface:
+    _ros_node: BaseROS2DeviceNode
+    tf_buffer: Buffer
+    tf_listener: TransformListener
 
-class MoveitInterface(BaseROS2DeviceNode):
-    def __init__(self, device_id, moveit_type, joint_poses, resource_tracker, rotation = None, device_config = None):
-        super().__init__(
-            driver_instance=self,
-            device_id=device_id,
-            status_types={},
-            action_value_mappings={},
-            hardware_interface={},
-            print_publish=False,
-            resource_tracker=resource_tracker,
+    def __init__(self, moveit_type, joint_poses, rotation=None, device_config=None):
+        self.device_config = device_config
+        self.rotation = rotation
+        self.data_config = json.load(
+            open(
+                f"{Path(__file__).parent.parent.parent.absolute()}/device_mesh/devices/{moveit_type}/config/move_group.json",
+                encoding="utf-8",
+            )
         )
-        self.callback_group         = ReentrantCallbackGroup()
-        data_config                 = json.load(open(f"{Path(__file__).parent.parent.parent.absolute()}/device_mesh/devices/{moveit_type}/config/move_group.json", encoding="utf-8"))
-
-        self.tf_buffer              = Buffer()
-        self.tf_listener            = TransformListener(self.tf_buffer, self)
-
-        self.arm_move_flag          = False
-
-        self.move_option            = ['pick', 'place', 'side_pick', 'side_place']
-        self.joint_poses            = joint_poses
-        self.cartesian_flag         = False
-
-        self.mesh_group             = ['reactor','sample','beaker']
-
+        self.arm_move_flag = False
+        self.move_option = ["pick", "place", "side_pick", "side_place"]
+        self.joint_poses = joint_poses
+        self.cartesian_flag = False
+        self.mesh_group = ["reactor", "sample", "beaker"]
         self.moveit2 = {}
-
-
         self.resource_action = None
         self.resource_client = None
         self.resource_action_ok = False
 
 
-        
-        for move_group, config in data_config.items():
+    def post_init(self, ros_node: BaseROS2DeviceNode):
+        self._ros_node = ros_node
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self._ros_node)
 
-            base_link_name         = f"{device_id}_{config['base_link_name']}"
-            end_effector_name      = f"{device_id}_{config['end_effector_name']}"
-            joint_names            = [f"{device_id}_{name}" for name in config['joint_names']]
+        for move_group, config in self.data_config.items():
+
+            base_link_name = f"{self._ros_node.device_id}_{config['base_link_name']}"
+            end_effector_name = f"{self._ros_node.device_id}_{config['end_effector_name']}"
+            joint_names = [f"{self._ros_node.device_id}_{name}" for name in config["joint_names"]]
 
             self.moveit2[f"{move_group}"] = MoveIt2(
-                node=self,
+                node=self._ros_node,
                 joint_names=joint_names,
                 base_link_name=base_link_name,
                 end_effector_name=end_effector_name,
-                group_name=f"{device_id}_{move_group}",
-                callback_group=self.callback_group,
-                use_move_group_action= True,
-                ignore_new_calls_while_executing = True
+                group_name=f"{self._ros_node.device_id}_{move_group}",
+                callback_group=self._ros_node.callback_group,
+                use_move_group_action=True,
+                ignore_new_calls_while_executing=True,
             )
             self.moveit2[f"{move_group}"].allowed_planning_time = 3.0
 
-            
-        self.moveit_pose_server = ActionServer(
-            self,
-            SendCmd,
-            f'set_position',
-            execute_callback=self.callback,
-            callback_group=self.callback_group,
-            result_timeout=5000
-        )
-
-        self.moveit_pick_server = ActionServer(
-            self,
-            SendCmd,
-            f'pick_and_place',
-            execute_callback=self.pick_and_place_callback,
-            callback_group=self.callback_group,
-            result_timeout=5000
-        )
-
-
-        self.moveit_joint_server = ActionServer(
-            self,
-            SendCmd,
-            f'set_status',
-            execute_callback=self.set_status_callback,
-            callback_group=self.callback_group,
-            result_timeout=5000
-        )
-
-        self.create_timer(1, self.wait_for_resource_action,callback_group=ReentrantCallbackGroup())
+        self._ros_node.create_timer(1, self.wait_for_resource_action, callback_group=self._ros_node.callback_group)
 
 
     def wait_for_resource_action(self):
@@ -112,33 +69,27 @@ class MoveitInterface(BaseROS2DeviceNode):
             while self.resource_action is None:
                 self.resource_action = self.check_tf_update_actions()
                 time.sleep(1)
-            self.resource_client = ActionClient(self, SendCmd, self.resource_action)
+            self.resource_client = ActionClient(self._ros_node, SendCmd, self.resource_action)
             self.resource_action_ok = True
             while not self.resource_client.wait_for_server(timeout_sec=5.0):
-                self.get_logger().info('等待 TfUpdate 服务器...')
-        
+                self._ros_node.lab_logger().info("等待 TfUpdate 服务器...")
 
     def check_tf_update_actions(self):
-        topics = self.get_topic_names_and_types()
-
-        
+        topics = self._ros_node.get_topic_names_and_types()
         for topic_item in topics:
-
             topic_name, topic_types = topic_item
-
-            if 'action_msgs/msg/GoalStatusArray' in topic_types:
+            if "action_msgs/msg/GoalStatusArray" in topic_types:
                 # 删除 /_action/status 部分
 
-                base_name = topic_name.replace('/_action/status', '')
+                base_name = topic_name.replace("/_action/status", "")
                 # 检查最后一个部分是否为 tf_update
-                parts = base_name.split('/')
-                if parts and parts[-1] == 'tf_update':
+                parts = base_name.split("/")
+                if parts and parts[-1] == "tf_update":
                     return base_name
-                
-        return None
-    
-    def callback(self,goal_handle: ServerGoalHandle):
 
+        return None
+
+    def set_position(self, command):
         """使用moveit 移动到指定点
         Args:
             command: A JSON-formatted string that includes quaternion, speed, position
@@ -147,42 +98,33 @@ class MoveitInterface(BaseROS2DeviceNode):
                     quaternion (list)   : 点的姿态(四元数) [x,y,z,w]
                     move_group (string) : The move group moveit will plan
                     speed (float)       : The speed of the movement, speed > 0
-                    retry (int)         : Retry times when moveit plan fails 
+                    retry (int)         : Retry times when moveit plan fails
 
         Returns:
             None
         """
 
         result = SendCmd.Result()
-
-        try:
-            cmd_str = str(goal_handle.request.command).replace('\'','\"')
-            cmd_dict = json.loads(cmd_str)
-
-            self.moveit_task(**cmd_dict)
-            goal_handle.succeed()
-            result.success=True
-            
-        except Exception as e:
-            print(e)
-            goal_handle.abort()
-            result.success=False
-
+        cmd_str = command.replace("'", '"')
+        cmd_dict = json.loads(cmd_str)
+        self.moveit_task(**cmd_dict)
         return result
 
-    def moveit_task(self,move_group,position,quaternion,speed=1,retry=10,cartesian=False, target_link = None ,offsets =[0,0,0]):
+    def moveit_task(
+        self, move_group, position, quaternion, speed=1, retry=10, cartesian=False, target_link=None, offsets=[0, 0, 0]
+    ):
 
         speed_ = float(max(0.1, min(speed, 1)))
 
-        self.moveit2[move_group].max_velocity       = speed_
-        self.moveit2[move_group].max_acceleration   = speed_
+        self.moveit2[move_group].max_velocity = speed_
+        self.moveit2[move_group].max_acceleration = speed_
 
         re_ = False
 
-        pose_result =[x + y for x, y in zip(position, offsets)]
+        pose_result = [x + y for x, y in zip(position, offsets)]
         # print(pose_result)
 
-        while retry > -1 and not re_ :
+        while retry > -1 and not re_:
 
             self.moveit2[move_group].move_to_pose(
                 target_link=target_link,
@@ -191,14 +133,14 @@ class MoveitInterface(BaseROS2DeviceNode):
                 cartesian=cartesian,
                 # cartesian_fraction_threshold=0.0,
                 cartesian_max_step=0.01,
-                weight_position=1.0
+                weight_position=1.0,
             )
-            re_ =  self.moveit2[move_group].wait_until_executed()
+            re_ = self.moveit2[move_group].wait_until_executed()
             retry += -1
 
         return re_
 
-    def moveit_joint_task(self, move_group, joint_positions, joint_names = None,speed=1,retry=10):
+    def moveit_joint_task(self, move_group, joint_positions, joint_names=None, speed=1, retry=10):
 
         re_ = False
 
@@ -206,107 +148,130 @@ class MoveitInterface(BaseROS2DeviceNode):
 
         speed_ = float(max(0.1, min(speed, 1)))
 
-        self.moveit2[move_group].max_velocity       = speed_
-        self.moveit2[move_group].max_acceleration   = speed_
-        
-        while retry > -1 and not re_ :
+        self.moveit2[move_group].max_velocity = speed_
+        self.moveit2[move_group].max_acceleration = speed_
 
-            self.moveit2[move_group].move_to_configuration(joint_positions=joint_positions_,joint_names=joint_names)
-            re_ =  self.moveit2[move_group].wait_until_executed()
+        while retry > -1 and not re_:
+
+            self.moveit2[move_group].move_to_configuration(joint_positions=joint_positions_, joint_names=joint_names)
+            re_ = self.moveit2[move_group].wait_until_executed()
 
             retry += -1
             print(self.moveit2[move_group].compute_fk(joint_positions))
         return re_
-    
 
-    def resource_manager(self,resource, parent_link):
+    def resource_manager(self, resource, parent_link):
         goal_msg = SendCmd.Goal()
         str_dict = {}
         str_dict[resource] = parent_link
 
         goal_msg.command = json.dumps(str_dict)
+        assert self.resource_client is not None
         self.resource_client.send_goal(goal_msg)
 
         return True
 
-
-
-    def pick_and_place_callback(self,goal_handle: ServerGoalHandle):
-        
+    def pick_and_place(self, command: str):
         """
-                Using MoveIt to make the robotic arm pick or place materials to a target point.
+        Using MoveIt to make the robotic arm pick or place materials to a target point.
 
-                Args:
-                    command: A JSON-formatted string that includes option, target, speed, lift_height, mt_height
+        Args:
+            command: A JSON-formatted string that includes option, target, speed, lift_height, mt_height
 
-                            *option (string)    : Action type: pick/place/side_pick/side_place
-                            *move_group (string): The move group moveit will plan
-                            *status(string)     : Target pose
-                            resource(string)    : The target resource
-                            x_distance (float)  : The distance to the target in x direction(meters)
-                            y_distance (float)  : The distance to the target in y direction(meters)
-                            lift_height (float) : The height at which the material should be lifted(meters)
-                            retry (float)       : Retry times when moveit plan fails 
-                            speed (float)       : The speed of the movement, speed > 0
-                Returns:
-                    None
+                    *option (string)    : Action type: pick/place/side_pick/side_place
+                    *move_group (string): The move group moveit will plan
+                    *status(string)     : Target pose
+                    resource(string)    : The target resource
+                    x_distance (float)  : The distance to the target in x direction(meters)
+                    y_distance (float)  : The distance to the target in y direction(meters)
+                    lift_height (float) : The height at which the material should be lifted(meters)
+                    retry (float)       : Retry times when moveit plan fails
+                    speed (float)       : The speed of the movement, speed > 0
+        Returns:
+            None
         """
         result = SendCmd.Result()
 
         try:
-            cmd_str = str(goal_handle.request.command).replace('\'','\"')
+            cmd_str = str(command).replace("'", '"')
             cmd_dict = json.loads(cmd_str)
 
             if cmd_dict["option"] in self.move_option:
                 option_index = self.move_option.index(cmd_dict["option"])
                 place_flag = option_index % 2
-                
+
                 config = {}
                 function_list = []
 
                 status = cmd_dict["status"]
                 joint_positions_ = self.joint_poses[cmd_dict["move_group"]][status]
 
-                config.update({k: cmd_dict[k] for k in ["speed", "retry", 'move_group'] if k in cmd_dict})
-
+                config.update({k: cmd_dict[k] for k in ["speed", "retry", "move_group"] if k in cmd_dict})
 
                 # 夹取
                 if not place_flag:
-                    if 'target' in cmd_dict.keys():
+                    if "target" in cmd_dict.keys():
                         function_list.append(lambda: self.resource_manager(cmd_dict["resource"], cmd_dict["target"]))
                     else:
-                        function_list.append(lambda: self.resource_manager(cmd_dict["resource"], self.moveit2[cmd_dict["move_group"]].end_effector_name))
+                        function_list.append(
+                            lambda: self.resource_manager(
+                                cmd_dict["resource"], self.moveit2[cmd_dict["move_group"]].end_effector_name
+                            )
+                        )
                 else:
-                    function_list.append(lambda: self.resource_manager(cmd_dict["resource"], 'world'))
+                    function_list.append(lambda: self.resource_manager(cmd_dict["resource"], "world"))
 
                 constraints = []
                 if "constraints" in cmd_dict.keys():
-                    
+
                     for i in range(len(cmd_dict["constraints"])):
                         v = float(cmd_dict["constraints"][i])
                         if v > 0:
-                            constraints.append(JointConstraint(
-                                joint_name=self.moveit2[cmd_dict["move_group"]].joint_names[i],
-                                position=joint_positions_[i],
-                                tolerance_above=v,
-                                tolerance_below=v,
-                                weight=1.0
-                            ))
+                            constraints.append(
+                                JointConstraint(
+                                    joint_name=self.moveit2[cmd_dict["move_group"]].joint_names[i],
+                                    position=joint_positions_[i],
+                                    tolerance_above=v,
+                                    tolerance_below=v,
+                                    weight=1.0,
+                                )
+                            )
 
-                
                 if "lift_height" in cmd_dict.keys():
-                    
-                    retval = self.moveit2[cmd_dict["move_group"]].compute_fk(joint_positions_)
+                    retval = None
+                    retry = config.get("retry", 10)
+                    while retval is None and retry > 0:
+                        retval = self.moveit2[cmd_dict["move_group"]].compute_fk(joint_positions_)
+                        time.sleep(0.1)
+                        retry -= 1
+                    if retval is None:
+                        result.success = False
+                        return result
                     pose = [retval.pose.position.x, retval.pose.position.y, retval.pose.position.z]
-                    quaternion = [retval.pose.orientation.x, retval.pose.orientation.y, retval.pose.orientation.z, retval.pose.orientation.w]
-                    
-                    function_list = [lambda: self.moveit_task(position=[retval.pose.position.x, retval.pose.position.y, retval.pose.position.z], quaternion=quaternion,**config, cartesian=self.cartesian_flag)] + function_list
+                    quaternion = [
+                        retval.pose.orientation.x,
+                        retval.pose.orientation.y,
+                        retval.pose.orientation.z,
+                        retval.pose.orientation.w,
+                    ]
+
+                    function_list = [
+                        lambda: self.moveit_task(
+                            position=[retval.pose.position.x, retval.pose.position.y, retval.pose.position.z],
+                            quaternion=quaternion,
+                            **config,
+                            cartesian=self.cartesian_flag,
+                        )
+                    ] + function_list
 
                     pose[2] += float(cmd_dict["lift_height"])
-                    function_list.append(lambda: self.moveit_task(position=pose, quaternion=quaternion,**config, cartesian=self.cartesian_flag))
+                    function_list.append(
+                        lambda: self.moveit_task(
+                            position=pose, quaternion=quaternion, **config, cartesian=self.cartesian_flag
+                        )
+                    )
                     end_pose = pose
 
-
                     if "x_distance" in cmd_dict.keys() or "y_distance" in cmd_dict.keys():
                         if "x_distance" in cmd_dict.keys():
                             deep_pose = deepcopy(pose)
@@ -314,27 +279,37 @@ class MoveitInterface(BaseROS2DeviceNode):
                         elif "y_distance" in cmd_dict.keys():
                             deep_pose = deepcopy(pose)
                             deep_pose[1] += float(cmd_dict["y_distance"])
-                    
-                        function_list = [lambda: self.moveit_task(position=pose, quaternion=quaternion,**config, cartesian=self.cartesian_flag)] + function_list
-                        function_list.append(lambda: self.moveit_task(position=deep_pose, quaternion=quaternion,**config, cartesian=self.cartesian_flag))
+
+                        function_list = [
+                            lambda: self.moveit_task(
+                                position=pose, quaternion=quaternion, **config, cartesian=self.cartesian_flag
+                            )
+                        ] + function_list
+                        function_list.append(
+                            lambda: self.moveit_task(
+                                position=deep_pose, quaternion=quaternion, **config, cartesian=self.cartesian_flag
+                            )
+                        )
                         end_pose = deep_pose
 
                     retval_ik = self.moveit2[cmd_dict["move_group"]].compute_ik(
-                        position=end_pose,
-                        quat_xyzw=quaternion,
-                        constraints=Constraints(joint_constraints=constraints))
-
-
-                    position_ = [retval_ik.position[retval_ik.name.index(i)] for i in self.moveit2[cmd_dict["move_group"]].joint_names]
-
-                    function_list = [lambda: self.moveit_joint_task(
-                        joint_positions=position_,
-                        joint_names=self.moveit2[cmd_dict["move_group"]].joint_names,
-                        **config,
-                    )] + function_list
+                        position=end_pose, quat_xyzw=quaternion, constraints=Constraints(joint_constraints=constraints)
+                    )
+                    position_ = [
+                        retval_ik.position[retval_ik.name.index(i)]
+                        for i in self.moveit2[cmd_dict["move_group"]].joint_names
+                    ]
+                    function_list = [
+                        lambda: self.moveit_joint_task(
+                            joint_positions=position_,
+                            joint_names=self.moveit2[cmd_dict["move_group"]].joint_names,
+                            **config,
+                        )
+                    ] + function_list
                 else:
-
-                    function_list = [lambda: self.moveit_joint_task(**config,joint_positions=joint_positions_)] + function_list
+                    function_list = [
+                        lambda: self.moveit_joint_task(**config, joint_positions=joint_positions_)
+                    ] + function_list
 
                 for i in range(len(function_list)):
                     if i == 0:
@@ -343,93 +318,59 @@ class MoveitInterface(BaseROS2DeviceNode):
                         self.cartesian_flag = True
 
                     re = function_list[i]()
-
                     if not re:
-
-                        print(i,re)
-                        goal_handle.abort()
-                        result.success=False
-
+                        print(i, re)
+                        result.success = False
                         return result
-                    
-                goal_handle.succeed()
-                result.success=True
-            
+                result.success = True
+
         except Exception as e:
             print(e)
-            goal_handle.abort()
             self.cartesian_flag = False
-            result.success=False
+            result.success = False
 
         return result
-    
-    def set_status_callback(self,goal_handle: ServerGoalHandle):
 
+    def set_status(self, command: str):
         """
-            Goto home position
+        Goto home position
 
-                Args:
-                    command: A JSON-formatted string that includes speed
-                            *status (string)    : The joint status moveit will plan
-                            *move_group (string): The move group moveit will plan
-                            separate (list)     : The joint index to be separated
-                            lift_height (float) : The height at which the material should be lifted(meters)
-                            x_distance (float)  : The distance to the target in x direction(meters)
-                            y_distance (float)  : The distance to the target in y direction(meters)
-                            speed (float)       : The speed of the movement, speed > 0
-                            retry (float)       : Retry times when moveit plan fails 
+            Args:
+                command: A JSON-formatted string that includes speed
+                        *status (string)    : The joint status moveit will plan
+                        *move_group (string): The move group moveit will plan
+                        separate (list)     : The joint index to be separated
+                        lift_height (float) : The height at which the material should be lifted(meters)
+                        x_distance (float)  : The distance to the target in x direction(meters)
+                        y_distance (float)  : The distance to the target in y direction(meters)
+                        speed (float)       : The speed of the movement, speed > 0
+                        retry (float)       : Retry times when moveit plan fails
 
-                Returns:
-                    None
+            Returns:
+                None
         """
 
         result = SendCmd.Result()
 
         try:
-            cmd_str = str(goal_handle.request.command).replace('\'','\"')
+            cmd_str = command.replace("'", '"')
             cmd_dict = json.loads(cmd_str)
-
             config = {}
             config["move_group"] = cmd_dict["move_group"]
             if "speed" in cmd_dict.keys():
                 config["speed"] = cmd_dict["speed"]
             if "retry" in cmd_dict.keys():
                 config["retry"] = cmd_dict["retry"]
-                
-            
-
-            if "separate" in cmd_dict.keys():
-                separate = cmd_dict["separate"]
-            else:
-                separate = None
 
             status = cmd_dict["status"]
             joint_positions_ = self.joint_poses[cmd_dict["move_group"]][status]
-
-            if separate is not None :
-
-                separate_j = [joint_positions_[i] for i in separate]
-                separate_n = [self.joint_names[i] for i in separate]
-
-                re = self.moveit_joint_task(**config, joint_positions=separate_j,joint_names=separate_n)
-                if not re:
-                    goal_handle.abort()
-                    result.success=False
-                    return result
-
-            re = self.moveit_joint_task(**config,joint_positions=joint_positions_)
-
+            re = self.moveit_joint_task(**config, joint_positions=joint_positions_)
             if not re:
-                goal_handle.abort()
-                result.success=False
+                result.success = False
                 return result
-            
-            goal_handle.succeed()
-            result.success=True
-            
+            result.success = True
         except Exception as e:
             print(e)
-            goal_handle.abort()
-            result.success=False
+            result.success = False
 
-        return result
+        return result

unilabos/registry/devices/liquid_handler.yaml

@@ -358,8 +358,8 @@ liquid_handler.biomek:
       transfer_biomek:
         type: LiquidHandlerTransferBiomek
         goal:
-          source: source
-          target: target
+          sources: sources
+          targets: targets
           tip_rack: tip_rack
           volume: volume
           aspirate_techniques: aspirate_techniques
@@ -417,8 +417,8 @@ liquid_handler.biomek:
       move_biomek:
         type: LiquidHandlerMoveBiomek
         goal:
-          source: resource
-          target: target
+          source: sources
+          target: targets
         feedback: {}
         result:
           name: name

unilabos/registry/devices/moveit_config.yaml

@@ -2,16 +2,55 @@ moveit.toyo_xyz:
   description: Toyo XYZ
   class:
     module: unilabos.devices.ros_dev.moveit_interface:MoveitInterface
-    type: ros2
+    type: python
+    action_value_mappings:
+      set_position:
+        type: SendCmd
+        goal:
+          command: command
+        feedback: { }
+        result: { }
+      pick_and_place:
+        type: SendCmd
+        goal:
+          command: command
+        feedback: { }
+        result: { }
+      set_status:
+        type: SendCmd
+        goal:
+          command: command
+        feedback: { }
+        result: { }
+
   model:
     type: device
     mesh: toyo_xyz
 
 moveit.benyao_arm:
   description: Benyao Arm
-  class:
-    module: unilabos.devices.ros_dev.moveit_interface:MoveitInterface
-    type: ros2
   model:
     type: device
-    mesh: benyao_arm
+    mesh: benyao_arm
+  class:
+    module: unilabos.devices.ros_dev.moveit_interface:MoveitInterface
+    type: python
+    action_value_mappings:
+      set_position:
+        type: SendCmd
+        goal:
+          command: command
+        feedback: {}
+        result: {}
+      pick_and_place:
+        type: SendCmd
+        goal:
+          command: command
+        feedback: {}
+        result: {}
+      set_status:
+        type: SendCmd
+        goal:
+          command: command
+        feedback: {}
+        result: {}

unilabos/registry/registry.py

@@ -87,7 +87,7 @@ class Registry:
                                 ),
                                 "handles": {
                                     "output": [{
-                                        "handler_key": "Labware",
+                                        "handler_key": "labware",
                                         "label": "Labware",
                                         "data_type": "resource",
                                         "data_source": "handle",

unilabos/ros/nodes/base_device_node.py

@@ -924,6 +924,12 @@ class ROS2DeviceNode:
             )
         self._ros_node: BaseROS2DeviceNode
         self._ros_node.lab_logger().info(f"初始化完成 {self._ros_node.uuid} {self.driver_is_ros}")
+        self.driver_instance._ros_node = self._ros_node  # type: ignore
+        if hasattr(self.driver_instance, "post_init"):
+            try:
+                self.driver_instance.post_init(self._ros_node)  # type: ignore
+            except Exception as e:
+                self._ros_node.lab_logger().error(f"设备后初始化失败: {e}")
 
     def _start_loop(self):
         def run_event_loop():

unilabos/ros/nodes/presets/resource_mesh_manager.py

@@ -190,7 +190,8 @@ class ResourceMeshManager(BaseROS2DeviceNode):
 
                 pass
             elif parent is not None and resource_id in self.resource_model:
-                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_","")
+                parent_link = f"{parent}_device_link".replace("None_","")
 
             else:
 
@@ -391,7 +392,8 @@ class ResourceMeshManager(BaseROS2DeviceNode):
             planning_scene = PlanningScene()
             planning_scene.is_diff = True
             planning_scene.robot_state.is_diff = True
-            time_start = self.get_clock().now()
+            # time_start = self.get_clock().now()
+            time_start = rclpy.time.Time(seconds=0)
             count = 0
 
             for resource_id, target_parent in cmd_dict.items():

unilabos_msgs/action/LiquidHandlerMoveBiomek.action

@@ -1,5 +1,5 @@
-string source
-string target
+string sources
+string targets
 
 ---
 string return_info

unilabos_msgs/action/LiquidHandlerTransferBiomek.action

@@ -1,5 +1,5 @@
-string source
-string target
+string sources
+string targets
 string tip_rack
 float64 volume
 string aspirate_technique