add full device

This reverts commit 2487bb6ffc.

.gitignore

@@ -232,4 +232,5 @@ CATKIN_IGNORE
 
 /**/local_config.py
 
-*.graphml
+*.graphml
+unilabos/device_mesh/view_robot.rviz

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.1-xxxxx.tar.bz2
+conda install ros-humble-unilabos-msgs-0.9.2-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.1-xxxxx.tar.bz2
+conda install ros-humble-unilabos-msgs-0.9.2-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.1
+  version: 0.9.2
 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.1"
+  version: "0.9.2"
 
 source:
   path: ../..

setup.py

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

test/experiments/biomek.json

@@ -0,0 +1,22 @@
+{
+    "nodes": [
+        {
+            "id": "BIOMEK",
+            "name": "BIOMEK",
+            "parent": null,
+            "type": "device",
+            "class": "liquid_handler.biomek",
+            "position": {
+                "x": 620.6111111111111,
+                "y": 171,
+                "z": 0
+            },
+            "config": {
+            },
+            "data": {},
+            "children": [
+            ]
+        }
+    ],
+    "links": []
+}

test/experiments/test_moveit.json

@@ -0,0 +1,35 @@
+{
+    "nodes": [
+
+        {
+            "id": "benyao",
+            "name": "benyao",
+            "children": [
+            ],
+            "parent": null,
+            "type": "device",
+            "class": "moveit.benyao_arm",
+            "position": {
+                "x": 0,
+                "y": 0,
+                "z": 0
+            },
+            "config": {
+                "moveit_type": "benyao_arm",
+                "joint_poses": {
+                    "arm": {
+                        "home": [0.0, 0.2, 0.0, 0.0, 0.0],
+                        "pick": [1.2, 0.0, 0.0, 0.0, 0.0]
+                    }
+                },
+                "device_config": {
+                }
+            },
+            "data": {
+            }
+        }
+    ],
+    "links": [
+        
+    ]
+}

unilabos/app/main.py

@@ -10,6 +10,8 @@ from copy import deepcopy
 
 import yaml
 
+from unilabos.resources.graphio import tree_to_list
+
 # 首先添加项目根目录到路径
 current_dir = os.path.dirname(os.path.abspath(__file__))
 unilabos_dir = os.path.dirname(os.path.dirname(current_dir))
@@ -144,19 +146,19 @@ def main():
             else read_graphml(args_dict["graph"])
         )
         devices_and_resources = dict_from_graph(graph_res.physical_setup_graph)
-        args_dict["resources_config"] = initialize_resources(list(deepcopy(devices_and_resources).values()))
+        # args_dict["resources_config"] = initialize_resources(list(deepcopy(devices_and_resources).values()))
+        args_dict["resources_config"] = list(devices_and_resources.values())
         args_dict["devices_config"] = dict_to_nested_dict(deepcopy(devices_and_resources), devices_only=False)
-        # args_dict["resources_config"] = dict_to_tree(devices_and_resources, devices_only=False)
-
         args_dict["graph"] = graph_res.physical_setup_graph
     else:
         if args_dict["devices"] is None or args_dict["resources"] is None:
             print_status("Either graph or devices and resources must be provided.", "error")
             sys.exit(1)
         args_dict["devices_config"] = json.load(open(args_dict["devices"], encoding="utf-8"))
-        args_dict["resources_config"] = initialize_resources(
-            list(json.load(open(args_dict["resources"], encoding="utf-8")).values())
-        )
+        # args_dict["resources_config"] = initialize_resources(
+        #     list(json.load(open(args_dict["resources"], encoding="utf-8")).values())
+        # )
+        args_dict["resources_config"] = list(json.load(open(args_dict["resources"], encoding="utf-8")).values())
 
     print_status(f"{len(args_dict['resources_config'])} Resources loaded:", "info")
     for i in args_dict["resources_config"]:

unilabos/app/mq.py

@@ -1,6 +1,7 @@
 import json
 import time
 import traceback
+from typing import Optional
 import uuid
 
 import paho.mqtt.client as mqtt
@@ -161,12 +162,14 @@ class MQTTClient:
         status = {"data": device_status.get(device_id, {}), "device_id": device_id}
         address = f"labs/{MQConfig.lab_id}/devices/"
         self.client.publish(address, json.dumps(status), qos=2)
-        logger.critical(f"Device status published: address: {address}, {status}")
+        logger.debug(f"Device status published: address: {address}, {status}")
 
-    def publish_job_status(self, feedback_data: dict, job_id: str, status: str):
+    def publish_job_status(self, feedback_data: dict, job_id: str, status: str, return_info: Optional[str] = None):
         if self.mqtt_disable:
             return
-        jobdata = {"job_id": job_id, "data": feedback_data, "status": status}
+        if return_info is None:
+            return_info = "{}"
+        jobdata = {"job_id": job_id, "data": feedback_data, "status": status, "return_info": return_info}
         self.client.publish(f"labs/{MQConfig.lab_id}/job/list/", json.dumps(jobdata), qos=2)
 
     def publish_registry(self, device_id: str, device_info: dict):

unilabos/app/web/client.py

@@ -30,18 +30,18 @@ class HTTPClient:
             self.auth = MQConfig.lab_id
         info(f"HTTPClient 初始化完成: remote_addr={self.remote_addr}")
 
-    def resource_add(self, resources: List[Dict[str, Any]]) -> requests.Response:
+    def resource_add(self, resources: List[Dict[str, Any]], database_process_later:bool) -> requests.Response:
         """
         添加资源
 
         Args:
             resources: 要添加的资源列表
-
+            database_process_later: 后台处理资源
         Returns:
             Response: API响应对象
         """
         response = requests.post(
-            f"{self.remote_addr}/lab/resource/",
+            f"{self.remote_addr}/lab/resource/?database_process_later={1 if database_process_later else 0}",
             json=resources,
             headers={"Authorization": f"lab {self.auth}"},
             timeout=5,
@@ -60,7 +60,7 @@ class HTTPClient:
             Dict: 返回的资源数据
         """
         response = requests.get(
-            f"{self.remote_addr}/lab/resource/",
+            f"{self.remote_addr}/lab/resource/?edge_format=1",
             params={"id": id, "with_children": with_children},
             headers={"Authorization": f"lab {self.auth}"},
             timeout=5,
@@ -96,7 +96,7 @@ class HTTPClient:
             Response: API响应对象
         """
         response = requests.patch(
-            f"{self.remote_addr}/lab/resource/batch_update/",
+            f"{self.remote_addr}/lab/resource/batch_update/?edge_format=1",
             json=resources,
             headers={"Authorization": f"lab {self.auth}"},
             timeout=5,

unilabos/device_mesh/devices/benyao_arm/config/initial_positions.yaml

@@ -0,0 +1,9 @@
+# Default initial positions for full_dev's ros2_control fake system
+
+initial_positions:
+  arm_base_joint: 0
+  arm_link_1_joint: 0
+  arm_link_2_joint: 0
+  arm_link_3_joint: 0
+  gripper_base_joint: 0
+  gripper_right_joint: 0.03

unilabos/device_mesh/devices/benyao_arm/config/joint_limits.yaml

@@ -0,0 +1,40 @@
+# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
+
+# For beginners, we downscale velocity and acceleration limits.
+# You can always specify higher scaling factors (<= 1.0) in your motion requests.  # Increase the values below to 1.0 to always move at maximum speed.
+default_velocity_scaling_factor: 0.1
+default_acceleration_scaling_factor: 0.1
+
+# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
+# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
+joint_limits:
+  arm_base_joint:
+    has_velocity_limits: true
+    max_velocity: 0
+    has_acceleration_limits: false
+    max_acceleration: 0
+  arm_link_1_joint:
+    has_velocity_limits: true
+    max_velocity: 0
+    has_acceleration_limits: false
+    max_acceleration: 0
+  arm_link_2_joint:
+    has_velocity_limits: true
+    max_velocity: 0
+    has_acceleration_limits: false
+    max_acceleration: 0
+  arm_link_3_joint:
+    has_velocity_limits: true
+    max_velocity: 0
+    has_acceleration_limits: false
+    max_acceleration: 0
+  gripper_base_joint:
+    has_velocity_limits: true
+    max_velocity: 0
+    has_acceleration_limits: false
+    max_acceleration: 0
+  gripper_right_joint:
+    has_velocity_limits: true
+    max_velocity: 0
+    has_acceleration_limits: false
+    max_acceleration: 0

unilabos/device_mesh/devices/benyao_arm/config/kinematics.yaml

@@ -0,0 +1,4 @@
+arm:
+  kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin
+  kinematics_solver_search_resolution: 0.0050000000000000001
+  kinematics_solver_timeout: 0.0050000000000000001

unilabos/device_mesh/devices/benyao_arm/config/macro.ros2_control.xacro

@@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:macro name="benyao_arm_ros2_control" params="device_name mesh_path">
+        <xacro:property name="initial_positions" value="${load_yaml(mesh_path + '/devices/benyao_arm/config/initial_positions.yaml')['initial_positions']}"/>
+
+        <ros2_control name="${device_name}benyao_arm" type="system">
+            <hardware>
+                <!-- By default, set up controllers for simulation. This won't work on real hardware -->
+                <plugin>mock_components/GenericSystem</plugin>
+            </hardware>
+            <joint name="${device_name}arm_base_joint">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['arm_base_joint']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="${device_name}arm_link_1_joint">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['arm_link_1_joint']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="${device_name}arm_link_2_joint">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['arm_link_2_joint']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="${device_name}arm_link_3_joint">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['arm_link_3_joint']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="${device_name}gripper_base_joint">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['gripper_base_joint']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="${device_name}gripper_right_joint">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['gripper_right_joint']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+
+        </ros2_control>
+    </xacro:macro>
+</robot>

unilabos/device_mesh/devices/benyao_arm/config/macro.srdf.xacro

@@ -0,0 +1,46 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!--This does not replace URDF, and is not an extension of URDF.
+    This is a format for representing semantic information about the robot structure.
+    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
+-->
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+    <xacro:macro name="benyao_arm_srdf" params="device_name">
+        <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
+        <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
+        <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
+        <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
+        <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
+        <group name="${device_name}arm">
+            <chain base_link="${device_name}arm_slideway" tip_link="${device_name}gripper_base"/>
+        </group>
+        <group name="${device_name}arm_gripper">
+            <joint name="${device_name}gripper_right_joint"/>
+        </group>
+        <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+        <disable_collisions link1="${device_name}arm_base" link2="${device_name}arm_link_2" reason="Adjacent"/>
+        <disable_collisions link1="${device_name}arm_base" link2="${device_name}arm_link_1" reason="Adjacent"/>
+        <disable_collisions link1="${device_name}arm_base" link2="${device_name}arm_link_3" reason="Never"/>
+        <disable_collisions link1="${device_name}arm_base" link2="${device_name}arm_slideway" reason="Adjacent"/>
+        <disable_collisions link1="${device_name}arm_link_1" link2="${device_name}arm_link_2" reason="Adjacent"/>
+        <disable_collisions link1="${device_name}arm_link_1" link2="${device_name}arm_link_3" reason="Never"/>
+        <disable_collisions link1="${device_name}arm_link_1" link2="${device_name}arm_slideway" reason="Never"/>
+        <disable_collisions link1="${device_name}arm_link_1" link2="${device_name}gripper_base" reason="Never"/>
+        <disable_collisions link1="${device_name}arm_link_1" link2="${device_name}gripper_left" reason="Never"/>
+        <disable_collisions link1="${device_name}arm_link_1" link2="${device_name}gripper_right" reason="Never"/>
+        <disable_collisions link1="${device_name}arm_link_2" link2="${device_name}arm_link_3" reason="Adjacent"/>
+        <disable_collisions link1="${device_name}arm_link_2" link2="${device_name}arm_slideway" reason="Never"/>
+        <disable_collisions link1="${device_name}arm_link_2" link2="${device_name}gripper_base" reason="Never"/>
+        <disable_collisions link1="${device_name}arm_link_2" link2="${device_name}gripper_left" reason="Never"/>
+        <disable_collisions link1="${device_name}arm_link_2" link2="${device_name}gripper_right" reason="Never"/>
+        <disable_collisions link1="${device_name}arm_link_3" link2="${device_name}arm_slideway" reason="Never"/>
+        <disable_collisions link1="${device_name}arm_link_3" link2="${device_name}gripper_base" reason="Adjacent"/>
+        <disable_collisions link1="${device_name}arm_link_3" link2="${device_name}gripper_left" reason="Never"/>
+        <disable_collisions link1="${device_name}arm_link_3" link2="${device_name}gripper_right" reason="Never"/>
+        <disable_collisions link1="${device_name}arm_slideway" link2="${device_name}gripper_base" reason="Never"/>
+        <disable_collisions link1="${device_name}arm_slideway" link2="${device_name}gripper_left" reason="Never"/>
+        <disable_collisions link1="${device_name}arm_slideway" link2="${device_name}gripper_right" reason="Never"/>
+        <disable_collisions link1="${device_name}gripper_base" link2="${device_name}gripper_left" reason="Adjacent"/>
+        <disable_collisions link1="${device_name}gripper_base" link2="${device_name}gripper_right" reason="Adjacent"/>
+        <disable_collisions link1="${device_name}gripper_left" link2="${device_name}gripper_right" reason="Never"/>
+    </xacro:macro>
+</robot>

unilabos/device_mesh/devices/benyao_arm/config/move_group.json

@@ -0,0 +1,14 @@
+{
+    "arm":
+    {
+        "joint_names": [ 
+            "arm_base_joint",
+            "arm_link_1_joint",
+            "arm_link_2_joint",
+            "arm_link_3_joint",
+            "gripper_base_joint"
+            ],
+        "base_link_name": "device_link",
+        "end_effector_name": "gripper_base"
+    }
+}

unilabos/device_mesh/devices/benyao_arm/config/moveit_controllers.yaml

@@ -0,0 +1,29 @@
+# MoveIt uses this configuration for controller management
+
+moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
+
+moveit_simple_controller_manager:
+  controller_names:
+    - arm_controller
+    - gripper_controller
+
+  arm_controller:
+    type: FollowJointTrajectory
+    action_ns: follow_joint_trajectory
+    default: true
+    joints:
+      - arm_base_joint
+      - arm_link_1_joint
+      - arm_link_2_joint
+      - arm_link_3_joint
+      - gripper_base_joint
+    action_ns: follow_joint_trajectory
+    default: true
+  gripper_controller:
+    type: FollowJointTrajectory
+    action_ns: follow_joint_trajectory
+    default: true
+    joints:
+      - gripper_right_joint
+    action_ns: follow_joint_trajectory
+    default: true

unilabos/device_mesh/devices/benyao_arm/config/moveit_planners.yaml

@@ -0,0 +1,2 @@
+planner_configs:
+  - ompl_interface/OMPLPlanner

unilabos/device_mesh/devices/benyao_arm/config/pilz_cartesian_limits.yaml

@@ -0,0 +1,6 @@
+# Limits for the Pilz planner
+cartesian_limits:
+  max_trans_vel: 1.0
+  max_trans_acc: 2.25
+  max_trans_dec: -5.0
+  max_rot_vel: 1.57

unilabos/device_mesh/devices/benyao_arm/config/ros2_controllers.yaml

@@ -0,0 +1,39 @@
+# This config file is used by ros2_control
+controller_manager:
+  ros__parameters:
+    update_rate: 100  # Hz
+
+    arm_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+
+    gripper_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+arm_controller:
+  ros__parameters:
+    joints:
+      - arm_base_joint
+      - arm_link_1_joint
+      - arm_link_2_joint
+      - arm_link_3_joint
+      - gripper_base_joint
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+
+gripper_controller:
+  ros__parameters:
+    joints:
+      - gripper_right_joint
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity

unilabos/device_mesh/devices/benyao_arm/joint_limit.yaml

@@ -0,0 +1,44 @@
+joint_limits:
+
+  arm_base_joint:
+    effort: 50
+    velocity: 1.0
+    lower: 0
+    upper: 1.5
+
+  arm_link_1_joint:
+    effort: 50
+    velocity: 1.0
+    lower: 0
+    upper: 0.6
+
+  arm_link_2_joint:
+    effort: 50
+    velocity: 1.0
+    lower: !degrees  -95
+    upper: !degrees  95
+
+  arm_link_3_joint:
+    effort: 50
+    velocity: 1.0
+    lower: !degrees  -195
+    upper: !degrees  195
+
+  gripper_base_joint:
+    effort: 50
+    velocity: 1.0
+    lower: !degrees  -95
+    upper: !degrees  95
+
+
+  gripper_right_joint:
+    effort: 50
+    velocity: 1.0
+    lower: 0
+    upper: 0.03
+
+  gripper_left_joint:
+    effort: 50
+    velocity: 1.0
+    lower: 0
+    upper: 0.03

unilabos/device_mesh/devices/benyao_arm/macro_device.xacro

@@ -0,0 +1,293 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="benyao_arm">
+
+  <xacro:macro name="benyao_arm" params="mesh_path:='' parent_link:='' station_name:='' device_name:='' x:=0 y:=0 z:=0 rx:=0 ry:=0 r:=0">
+    <!-- Read .yaml files from disk, load content into properties -->
+    <xacro:property name= "joint_limit_parameters" value="${xacro.load_yaml(mesh_path + '/devices/benyao_arm/joint_limit.yaml')}"/>
+
+    <!-- Extract subsections from yaml dictionaries -->
+    <xacro:property name= "sec_limits" value="${joint_limit_parameters['joint_limits']}"/>
+
+    <joint name="${station_name}${device_name}base_link_joint" type="fixed">
+      <origin xyz="${x} ${y} ${z}" rpy="${rx} ${ry} ${r}" />
+      <parent link="${parent_link}"/>
+      <child link="${station_name}${device_name}device_link"/>
+      <axis xyz="0 0 0"/>
+    </joint>
+
+    <link name="${station_name}${device_name}device_link"/>
+    <joint name="${station_name}${device_name}device_link_joint" type="fixed">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <parent link="${station_name}${device_name}device_link"/>
+      <child link="${station_name}${device_name}arm_slideway"/>
+      <axis xyz="0 0 0"/>
+    </joint>
+
+    <!-- JOINTS LIMIT PARAMETERS -->
+    <xacro:property name="limit_arm_base_joint"        value="${sec_limits['arm_base_joint']}" />
+    <xacro:property name="limit_arm_link_1_joint"      value="${sec_limits['arm_link_1_joint']}" />
+    <xacro:property name="limit_arm_link_2_joint"      value="${sec_limits['arm_link_2_joint']}" />
+    <xacro:property name="limit_arm_link_3_joint"      value="${sec_limits['arm_link_3_joint']}" />
+    <xacro:property name="limit_gripper_base_joint"    value="${sec_limits['gripper_base_joint']}" />
+    <xacro:property name="limit_gripper_right_joint"   value="${sec_limits['gripper_right_joint']}"/>
+    <xacro:property name="limit_gripper_left_joint"    value="${sec_limits['gripper_left_joint']}" />
+    <link name="${station_name}${device_name}arm_slideway">
+      <inertial>
+        <origin rpy="0 0 0" xyz="-0.913122246354019 -0.00141851388483838 0.0416079172839272"/>
+        <mass value="13.6578107753627"/>
+        <inertia ixx="0.0507627640890578" ixy="0.0245166532634714" ixz="-0.0112656803168519" iyy="5.2550852314372" iyz="0.000302974193920367" izz="5.26892263696439"/>
+      </inertial>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/arm_slideway.STL"/>
+        </geometry>
+        <material name="">
+          <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/arm_slideway.STL"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${station_name}${device_name}arm_base_joint" type="prismatic">
+      <origin rpy="0 0 0" xyz="0.307 0 0.1225"/>
+      <parent link="${station_name}${device_name}arm_slideway"/>
+      <child link="${station_name}${device_name}arm_base"/>
+      <axis xyz="1 0 0"/>
+      <limit 
+        effort="${limit_arm_base_joint['effort']}" 
+        lower="${limit_arm_base_joint['lower']}" 
+        upper="${limit_arm_base_joint['upper']}" 
+        velocity="${limit_arm_base_joint['velocity']}"/>
+    </joint>
+
+    <link name="${station_name}${device_name}arm_base">
+      <inertial>
+        <origin rpy="0 0 0" xyz="1.48458338655733E-06 -0.00831873687136486 0.351728466012153"/>
+        <mass value="16.1341586205194"/>
+        <inertia ixx="0.54871651759045" ixy="7.65476367433116E-07" ixz="2.0515139488158E-07" iyy="0.55113098995396" iyz="-5.13261457726806E-07" izz="0.0619081867727048"/>
+      </inertial>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/arm_base.STL"/>
+        </geometry>
+        <material name="">
+          <color rgba="1 1 1 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/arm_base.STL"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <link name="${station_name}${device_name}arm_link_1">
+      <inertial>
+        <origin rpy="0 0 0" xyz="0 -0.0102223856758559 0.0348505130779933"/>
+        <mass value="0.828629227096429"/>
+        <inertia ixx="0.00119703598787112" ixy="-2.46083048832131E-19" ixz="1.43864352731199E-19" iyy="0.00108355785790042" iyz="1.88092240278693E-06" izz="0.00160914803816438"/>
+      </inertial>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/arm_link_1.STL"/>
+        </geometry>
+        <material name="">
+          <color rgba="1 1 1 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/arm_link_1.STL"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${station_name}${device_name}arm_link_1_joint" type="prismatic">
+      <origin rpy="0 0 0" xyz="0 0.1249 0.15"/>
+      <parent link="${station_name}${device_name}arm_base"/>
+      <child link="${station_name}${device_name}arm_link_1"/>
+      <axis xyz="0 0 1"/>
+      <limit 
+      effort="${limit_arm_link_1_joint['effort']}" 
+      lower="${limit_arm_link_1_joint['lower']}" 
+      upper="${limit_arm_link_1_joint['upper']}" 
+      velocity="${limit_arm_link_1_joint['velocity']}"/>
+    </joint>
+    <link name="${station_name}${device_name}arm_link_2">
+      <inertial>
+        <origin rpy="0 0 0" xyz="-3.33066907387547E-16 0.100000000000003 -0.0325000000000004"/>
+        <mass value="2.04764861029349"/>
+        <inertia ixx="0.0150150059448827" ixy="-1.28113733272213E-17" ixz="6.7561418872754E-19" iyy="0.00262980501315445" iyz="7.44451536320152E-18" izz="0.0162030186138787"/>
+      </inertial>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/arm_link_2.STL"/>
+        </geometry>
+        <material name="">
+          <color rgba="1 1 1 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/arm_link_2.STL"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${station_name}${device_name}arm_link_2_joint" type="revolute">
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <parent link="${station_name}${device_name}arm_link_1"/>
+      <child link="${station_name}${device_name}arm_link_2"/>
+      <axis xyz="0 0 1"/>
+      <limit 
+        effort="${limit_arm_link_2_joint['effort']}" 
+        lower="${limit_arm_link_2_joint['lower']}" 
+        upper="${limit_arm_link_2_joint['upper']}" 
+        velocity="${limit_arm_link_2_joint['velocity']}"/>
+    </joint>
+    <link name="${station_name}${device_name}arm_link_3">
+      <inertial>
+        <origin rpy="0 0 0" xyz="4.77395900588817E-15 0.0861257730831348 -0.0227999999999999"/>
+        <mass value="1.19870202871083"/>
+        <inertia ixx="0.00780783223764428" ixy="7.26567379579506E-18" ixz="1.02766851352053E-18" iyy="0.00109642607170081" iyz="-9.73775385060067E-18" izz="0.0084997384510058"/>
+      </inertial>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/arm_link_3.STL"/>
+        </geometry>
+        <material name="">
+          <color rgba="1 1 1 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/arm_link_3.STL"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${station_name}${device_name}arm_link_3_joint" type="revolute">
+      <origin rpy="0 0 0" xyz="0 0.2 -0.0647"/>
+      <parent link="${station_name}${device_name}arm_link_2"/>
+      <child link="${station_name}${device_name}arm_link_3"/>
+      <axis xyz="0 0 1"/>
+      <limit 
+        effort="${limit_arm_link_3_joint['effort']}" 
+        lower="${limit_arm_link_3_joint['lower']}" 
+        upper="${limit_arm_link_3_joint['upper']}" 
+        velocity="${limit_arm_link_3_joint['velocity']}"/>
+    </joint>
+    <link name="${station_name}${device_name}gripper_base">
+      <inertial>
+        <origin rpy="0 0 0" xyz="-6.05365748571618E-05 0.0373027483464434 -0.0264392017534612"/>
+        <mass value="0.511925198394943"/>
+        <inertia ixx="0.000640463815051467" ixy="1.08132229596356E-06" ixz="7.165124649009E-07" iyy="0.000552164156414554" iyz="9.80000237347941E-06" izz="0.00103553457812823"/>
+      </inertial>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/gripper_base.STL"/>
+        </geometry>
+        <material name="">
+          <color rgba="1 1 1 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/gripper_base.STL"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${station_name}${device_name}gripper_base_joint" type="revolute">
+      <origin rpy="0 0 0" xyz="0 0.2 -0.045"/>
+      <parent link="${station_name}${device_name}arm_link_3"/>
+      <child link="${station_name}${device_name}gripper_base"/>
+      <axis xyz="0 0 1"/>
+      <limit 
+        effort="${limit_gripper_base_joint['effort']}" 
+        lower="${limit_gripper_base_joint['lower']}" 
+        upper="${limit_gripper_base_joint['upper']}" 
+        velocity="${limit_gripper_base_joint['velocity']}"/>
+    </joint>
+    <link name="${station_name}${device_name}gripper_right">
+      <inertial>
+        <origin rpy="0 0 0" xyz="0.0340005471193899 0.0339655085140826 -0.0325252119823062"/>
+        <mass value="0.013337481136229"/>
+        <inertia ixx="2.02427962974094E-05" ixy="1.78442722292145E-06" ixz="-4.36485961300289E-07" iyy="1.4816483393622E-06" iyz="2.60539468115799E-06" izz="1.96629693098755E-05"/>
+      </inertial>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/gripper_right.STL"/>
+        </geometry>
+        <material name="">
+          <color rgba="1 1 1 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/gripper_right.STL"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${station_name}${device_name}gripper_right_joint" type="prismatic">
+      <origin rpy="0 0 0" xyz="0 0.0942 -0.022277"/>
+      <parent link="${station_name}${device_name}gripper_base"/>
+      <child link="${station_name}${device_name}gripper_right"/>
+      <axis xyz="1 0 0"/>
+      <limit 
+        effort="${limit_gripper_right_joint['effort']}" 
+        lower="${limit_gripper_right_joint['lower']}" 
+        upper="${limit_gripper_right_joint['upper']}" 
+        velocity="${limit_gripper_right_joint['velocity']}"/>
+    </joint>
+    <link name="${station_name}${device_name}gripper_left">
+      <inertial>
+        <origin rpy="0 3.1416 0" xyz="-0.0340005471193521 0.0339655081029604 -0.0325252119827364"/>
+        <mass value="0.0133374811362292"/>
+        <inertia ixx="2.02427962974094E-05" ixy="-1.78442720812615E-06" ixz="4.36485961300305E-07" iyy="1.48164833936224E-06" iyz="2.6053946859901E-06" izz="1.96629693098755E-05"/>
+      </inertial>
+      <visual>
+        <origin rpy="0 3.1416 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/gripper_left.STL"/>
+        </geometry>
+        <material name="">
+          <color rgba="1 1 1 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin rpy="0 3.1416 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/gripper_left.STL"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${station_name}${device_name}gripper_left_joint" type="prismatic">
+      <origin rpy="0 3.1416 0" xyz="0 0.0942 -0.022277"/>
+      <parent link="${station_name}${device_name}gripper_base"/>
+      <child link="${station_name}${device_name}gripper_left"/>
+      <axis xyz="1 0 0"/>
+      <limit 
+        effort="${limit_gripper_left_joint['effort']}" 
+        lower="${limit_gripper_left_joint['lower']}" 
+        upper="${limit_gripper_left_joint['upper']}" 
+        velocity="${limit_gripper_left_joint['velocity']}"/>
+      <mimic joint="${station_name}${device_name}gripper_right_joint" multiplier="1" />
+    </joint>
+
+  </xacro:macro>
+</robot>

unilabos/device_mesh/devices/opentrons_liquid_handler/macro_device.xacro

@@ -3,11 +3,10 @@
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
 
   <xacro:macro name="opentrons_liquid_handler" 
-  params="parent_link:='' station_name:='' device_name:='' x:=0 y:=0 z:=0 r:=0 mesh_path:=''">
-
+  params="parent_link:='' station_name:='' device_name:='' x:=0 y:=0 z:=0 rx:=0 ry:=0 r:=0 mesh_path:=''">
 
     <joint name="${station_name}${device_name}base_link_joint" type="fixed">
-      <origin xyz="${x} ${y} ${z}" rpy="0 0 ${r}" />
+      <origin xyz="${x} ${y} ${z}" rpy="${rx} ${ry} ${r}" />
       <parent link="${parent_link}"/>
       <child link="${station_name}${device_name}device_link"/>
       <axis xyz="0 0 0"/>

unilabos/device_mesh/devices/slide_w140/macro_device.xacro

@@ -8,11 +8,11 @@
      For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
 
-  <xacro:macro name="slide_w140" params="mesh_path:='' length:=0.1 min_d:=0.1 max_d:=0.1 slider_d:=0.14 parent_link:='' station_name:='' device_name:='' x:=0 y:=0 z:=0 r:=0" >
+  <xacro:macro name="slide_w140" params="mesh_path:='' length:=0.1 min_d:=0.1 max_d:=0.1 slider_d:=0.14 parent_link:='' station_name:='' device_name:='' x:=0 y:=0 z:=0 rx:=0 ry:=0 r:=0" >
 
 
     <joint name="${station_name}${device_name}base_link_joint" type="fixed">
-      <origin xyz="${x} ${y} ${z}" rpy="0 0 ${r}" />
+      <origin xyz="${x} ${y} ${z}" rpy="${rx} ${ry} ${r}" />
       <parent link="${parent_link}"/>
       <child link="${station_name}${device_name}device_link"/>
       <axis xyz="0 0 0"/>

unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/macro_device.xacro

@@ -0,0 +1,59 @@
+<?xml version='1.0'?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="thermo_orbitor_rs2_hotel" 
+  params="parent_link:='' station_name:='' device_name:='' x:=0 y:=0 z:=0 rx:=0 ry:=0 r:=0
+          mesh_path:=''">
+
+    <joint name="${station_name}${device_name}base_link_joint" type="fixed">
+      <origin xyz="${x} ${y} ${z}" rpy="${rx} ${ry} ${r}" />
+      <parent link="${parent_link}"/>
+      <child link="${station_name}${device_name}device_link"/>
+      <axis xyz="0 0 0"/>
+    </joint>
+
+    <link name="${station_name}${device_name}device_link"/>
+    <joint name="${station_name}${device_name}device_link_joint" type="fixed">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <parent link="${station_name}${device_name}device_link"/>
+      <child link="${station_name}${device_name}base_link"/>
+      <axis xyz="0 0 0"/>
+    </joint>
+    <link name='${station_name}${device_name}base_link'>
+      <visual name='visual'>
+        <origin rpy="-${pi/2} 0 ${pi}" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/thermo_orbitor_rs2_hotel/meshes/hotel.stl"/>
+        </geometry>
+        <material name="clay">
+          <color rgba="1 1 1 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin rpy="-${pi/2} 0 ${pi}" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/thermo_orbitor_rs2_hotel/meshes/hotel.stl"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <link name='${station_name}${device_name}socketTypeGenericSbsFootprint'/>
+
+    <xacro:property name="delta" value="0.073" />
+    <xacro:macro name='platetargets' params='num'>
+      <joint name='${station_name}${device_name}socketTypeGenericSbsFootprint_${num}_60_1' type='fixed'>
+        <parent link="${station_name}${device_name}base_link"/>
+        <child link="${station_name}${device_name}socketTypeGenericSbsFootprint"/>
+        <origin rpy="0 0 ${pi/2}" xyz="-0.002 0.011 ${(num - 1) * delta + 0.038}"/>
+      </joint>
+    </xacro:macro>
+
+    <xacro:platetargets num='1' />
+    <xacro:platetargets num='2' />
+    <xacro:platetargets num='3' />
+    <xacro:platetargets num='4' />
+    <xacro:platetargets num='5' />
+    <xacro:platetargets num='6' />
+    <xacro:platetargets num='7' />
+    <xacro:platetargets num='8' />
+  </xacro:macro>
+</robot>

unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/meta.json

@@ -0,0 +1,10 @@
+{
+    "fileName": "thermo_orbitor_rs2_hotel",
+    "related": [
+        "thermo_skyline_stacker",
+        "thermo_cytomat2c_stacker_15",
+        "thermo_fisher_cytomat_stacker_16",
+        "thermo_cytomat2c_stacker_21",
+        "thermo_orbitor_rs2_hotel"
+    ]
+}

unilabos/device_mesh/devices/toyo_xyz/config/full_dev.urdf.xacro

@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="full_dev">
+    <xacro:arg name="device_name" default=""/>
+    <xacro:arg name="mesh_path" default="/home/z43/git_pj/Uni-Lab-OS/unilabos/device_mesh" />
+
+    <xacro:include filename="macro.ros2_control.xacro" />
+
+    <xacro:toyo_xyz_ros2_control device_name="$(arg device_name)" mesh_path="$(arg mesh_path)" />
+
+</robot>

unilabos/device_mesh/devices/toyo_xyz/config/initial_positions.yaml

@@ -0,0 +1,6 @@
+# Default initial positions for full_dev's ros2_control fake system
+
+initial_positions:
+  slider1_joint: 0
+  slider2_joint: 0
+  slider3_joint: 0

unilabos/device_mesh/devices/toyo_xyz/config/joint_limits.yaml

@@ -0,0 +1,25 @@
+# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
+
+# For beginners, we downscale velocity and acceleration limits.
+# You can always specify higher scaling factors (<= 1.0) in your motion requests.  # Increase the values below to 1.0 to always move at maximum speed.
+default_velocity_scaling_factor: 0.1
+default_acceleration_scaling_factor: 0.1
+
+# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
+# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
+joint_limits:
+  slider1_joint:
+    has_velocity_limits: false
+    max_velocity: 0
+    has_acceleration_limits: false
+    max_acceleration: 0
+  slider2_joint:
+    has_velocity_limits: false
+    max_velocity: 0
+    has_acceleration_limits: false
+    max_acceleration: 0
+  slider3_joint:
+    has_velocity_limits: false
+    max_velocity: 0
+    has_acceleration_limits: false
+    max_acceleration: 0

unilabos/device_mesh/devices/toyo_xyz/config/kinematics.yaml

@@ -0,0 +1,4 @@
+toyo_xyz:
+  kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin
+  kinematics_solver_search_resolution: 0.0050000000000000001
+  kinematics_solver_timeout: 0.0050000000000000001

unilabos/device_mesh/devices/toyo_xyz/config/macro.ros2_control.xacro

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:macro name="toyo_xyz_ros2_control" params="device_name mesh_path">
+        <xacro:property name="initial_positions" value="${load_yaml(mesh_path + '/devices/toyo_xyz/config/initial_positions.yaml')['initial_positions']}"/>
+
+        <ros2_control name="${device_name}toyo_xyz" type="system">
+            <hardware>
+                <!-- By default, set up controllers for simulation. This won't work on real hardware -->
+                <plugin>mock_components/GenericSystem</plugin>
+            </hardware>
+            <joint name="${device_name}slider1_joint">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['slider1_joint']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="${device_name}slider2_joint">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['slider2_joint']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="${device_name}slider3_joint">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['slider3_joint']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+
+        </ros2_control>
+    </xacro:macro>
+</robot>

unilabos/device_mesh/devices/toyo_xyz/config/macro.srdf.xacro

@@ -0,0 +1,109 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!--This does not replace URDF, and is not an extension of URDF.
+    This is a format for representing semantic information about the robot structure.
+    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
+-->
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+    <xacro:macro name="toyo_xyz_srdf" params="device_name">
+        <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
+        <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
+        <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
+        <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
+        <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
+        <group name="${device_name}toyo_xyz">
+            <chain base_link="${device_name}base_link" tip_link="${device_name}slider3_link"/>
+        </group>
+        <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+        <disable_collisions link1="${device_name}base2_link" link2="${device_name}base3_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base2_link" link2="${device_name}base_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base2_link" link2="${device_name}chain_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base2_link" link2="${device_name}end2_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base2_link" link2="${device_name}end3_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base2_link" link2="${device_name}end_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base2_link" link2="${device_name}fixed_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base2_link" link2="${device_name}length1_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base2_link" link2="${device_name}length2_link" reason="Default"/>
+        <disable_collisions link1="${device_name}base2_link" link2="${device_name}length3_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base2_link" link2="${device_name}slider1_link" reason="Adjacent"/>
+        <disable_collisions link1="${device_name}base2_link" link2="${device_name}slider2_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base2_link" link2="${device_name}slider3_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base3_link" link2="${device_name}base_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base3_link" link2="${device_name}chain_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base3_link" link2="${device_name}end2_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base3_link" link2="${device_name}end3_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base3_link" link2="${device_name}end_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base3_link" link2="${device_name}fixed_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base3_link" link2="${device_name}length1_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base3_link" link2="${device_name}length2_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base3_link" link2="${device_name}length3_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base3_link" link2="${device_name}slider1_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base3_link" link2="${device_name}slider2_link" reason="Adjacent"/>
+        <disable_collisions link1="${device_name}base3_link" link2="${device_name}slider3_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base_link" link2="${device_name}chain_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base_link" link2="${device_name}end2_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base_link" link2="${device_name}end3_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base_link" link2="${device_name}end_link" reason="Adjacent"/>
+        <disable_collisions link1="${device_name}base_link" link2="${device_name}fixed_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base_link" link2="${device_name}length1_link" reason="Adjacent"/>
+        <disable_collisions link1="${device_name}base_link" link2="${device_name}length2_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base_link" link2="${device_name}length3_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base_link" link2="${device_name}slider1_link" reason="Adjacent"/>
+        <disable_collisions link1="${device_name}base_link" link2="${device_name}slider2_link" reason="Never"/>
+        <disable_collisions link1="${device_name}base_link" link2="${device_name}slider3_link" reason="Never"/>
+        <disable_collisions link1="${device_name}chain_link" link2="${device_name}end2_link" reason="Never"/>
+        <disable_collisions link1="${device_name}chain_link" link2="${device_name}end3_link" reason="Never"/>
+        <disable_collisions link1="${device_name}chain_link" link2="${device_name}end_link" reason="Never"/>
+        <disable_collisions link1="${device_name}chain_link" link2="${device_name}fixed_link" reason="Default"/>
+        <disable_collisions link1="${device_name}chain_link" link2="${device_name}length1_link" reason="Never"/>
+        <disable_collisions link1="${device_name}chain_link" link2="${device_name}length2_link" reason="Never"/>
+        <disable_collisions link1="${device_name}chain_link" link2="${device_name}length3_link" reason="Never"/>
+        <disable_collisions link1="${device_name}chain_link" link2="${device_name}slider1_link" reason="Adjacent"/>
+        <disable_collisions link1="${device_name}chain_link" link2="${device_name}slider2_link" reason="Never"/>
+        <disable_collisions link1="${device_name}chain_link" link2="${device_name}slider3_link" reason="Never"/>
+        <disable_collisions link1="${device_name}end2_link" link2="${device_name}end3_link" reason="Never"/>
+        <disable_collisions link1="${device_name}end2_link" link2="${device_name}end_link" reason="Never"/>
+        <disable_collisions link1="${device_name}end2_link" link2="${device_name}fixed_link" reason="Never"/>
+        <disable_collisions link1="${device_name}end2_link" link2="${device_name}length1_link" reason="Never"/>
+        <disable_collisions link1="${device_name}end2_link" link2="${device_name}length2_link" reason="Default"/>
+        <disable_collisions link1="${device_name}end2_link" link2="${device_name}length3_link" reason="Never"/>
+        <disable_collisions link1="${device_name}end2_link" link2="${device_name}slider1_link" reason="Adjacent"/>
+        <disable_collisions link1="${device_name}end2_link" link2="${device_name}slider2_link" reason="Never"/>
+        <disable_collisions link1="${device_name}end2_link" link2="${device_name}slider3_link" reason="Never"/>
+        <disable_collisions link1="${device_name}end3_link" link2="${device_name}end_link" reason="Never"/>
+        <disable_collisions link1="${device_name}end3_link" link2="${device_name}fixed_link" reason="Never"/>
+        <disable_collisions link1="${device_name}end3_link" link2="${device_name}length1_link" reason="Never"/>
+        <disable_collisions link1="${device_name}end3_link" link2="${device_name}length2_link" reason="Never"/>
+        <disable_collisions link1="${device_name}end3_link" link2="${device_name}length3_link" reason="Default"/>
+        <disable_collisions link1="${device_name}end3_link" link2="${device_name}slider1_link" reason="Never"/>
+        <disable_collisions link1="${device_name}end3_link" link2="${device_name}slider2_link" reason="Adjacent"/>
+        <disable_collisions link1="${device_name}end3_link" link2="${device_name}slider3_link" reason="Never"/>
+        <disable_collisions link1="${device_name}end_link" link2="${device_name}fixed_link" reason="Never"/>
+        <disable_collisions link1="${device_name}end_link" link2="${device_name}length1_link" reason="Default"/>
+        <disable_collisions link1="${device_name}end_link" link2="${device_name}length2_link" reason="Never"/>
+        <disable_collisions link1="${device_name}end_link" link2="${device_name}length3_link" reason="Never"/>
+        <disable_collisions link1="${device_name}end_link" link2="${device_name}slider1_link" reason="Never"/>
+        <disable_collisions link1="${device_name}end_link" link2="${device_name}slider2_link" reason="Never"/>
+        <disable_collisions link1="${device_name}end_link" link2="${device_name}slider3_link" reason="Never"/>
+        <disable_collisions link1="${device_name}fixed_link" link2="${device_name}length1_link" reason="Never"/>
+        <disable_collisions link1="${device_name}fixed_link" link2="${device_name}length2_link" reason="Never"/>
+        <disable_collisions link1="${device_name}fixed_link" link2="${device_name}length3_link" reason="Default"/>
+        <disable_collisions link1="${device_name}fixed_link" link2="${device_name}slider1_link" reason="Never"/>
+        <disable_collisions link1="${device_name}fixed_link" link2="${device_name}slider2_link" reason="Adjacent"/>
+        <disable_collisions link1="${device_name}fixed_link" link2="${device_name}slider3_link" reason="Never"/>
+        <disable_collisions link1="${device_name}length1_link" link2="${device_name}length2_link" reason="Never"/>
+        <disable_collisions link1="${device_name}length1_link" link2="${device_name}length3_link" reason="Never"/>
+        <disable_collisions link1="${device_name}length1_link" link2="${device_name}slider1_link" reason="Never"/>
+        <disable_collisions link1="${device_name}length1_link" link2="${device_name}slider2_link" reason="Never"/>
+        <disable_collisions link1="${device_name}length1_link" link2="${device_name}slider3_link" reason="Never"/>
+        <disable_collisions link1="${device_name}length2_link" link2="${device_name}length3_link" reason="Never"/>
+        <disable_collisions link1="${device_name}length2_link" link2="${device_name}slider1_link" reason="Adjacent"/>
+        <disable_collisions link1="${device_name}length2_link" link2="${device_name}slider2_link" reason="Never"/>
+        <disable_collisions link1="${device_name}length2_link" link2="${device_name}slider3_link" reason="Never"/>
+        <disable_collisions link1="${device_name}length3_link" link2="${device_name}slider1_link" reason="Never"/>
+        <disable_collisions link1="${device_name}length3_link" link2="${device_name}slider2_link" reason="Adjacent"/>
+        <disable_collisions link1="${device_name}length3_link" link2="${device_name}slider3_link" reason="Never"/>
+        <disable_collisions link1="${device_name}slider1_link" link2="${device_name}slider2_link" reason="Adjacent"/>
+        <disable_collisions link1="${device_name}slider1_link" link2="${device_name}slider3_link" reason="Never"/>
+        <disable_collisions link1="${device_name}slider2_link" link2="${device_name}slider3_link" reason="Adjacent"/>
+    </xacro:macro>
+</robot>

unilabos/device_mesh/devices/toyo_xyz/config/move_group.json

@@ -0,0 +1,12 @@
+{
+    "toyo_xyz":
+    {
+        "joint_names": [ 
+            "slider1_joint",
+            "slider2_joint",
+            "slider3_joint"
+            ],
+        "base_link_name": "device_link",
+        "end_effector_name": "slider3_link"
+    }
+}

unilabos/device_mesh/devices/toyo_xyz/config/moveit_controllers.yaml

@@ -0,0 +1,16 @@
+# MoveIt uses this configuration for controller management
+
+moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
+
+moveit_simple_controller_manager:
+  controller_names:
+    - toyo_xyz_controller
+
+  toyo_xyz_controller:
+    type: FollowJointTrajectory
+    action_ns: follow_joint_trajectory
+    default: true
+    joints:
+      - slider1_joint
+      - slider2_joint
+      - slider3_joint

unilabos/device_mesh/devices/toyo_xyz/config/pilz_cartesian_limits.yaml

@@ -0,0 +1,6 @@
+# Limits for the Pilz planner
+cartesian_limits:
+  max_trans_vel: 1.0
+  max_trans_acc: 2.25
+  max_trans_dec: -5.0
+  max_rot_vel: 1.57

unilabos/device_mesh/devices/toyo_xyz/config/ros2_controllers.yaml

@@ -0,0 +1,34 @@
+# This config file is used by ros2_control
+controller_manager:
+
+  ros__parameters:
+    update_rate: 100  # Hz
+  
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    # action_ns: $(var device_id)
+    toyo_xyz_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+# joint_state_broadcaster:
+#   ros__parameters: {}
+
+toyo_xyz_controller:
+  ros__parameters:
+    joints:
+      - slider1_joint
+      - slider2_joint
+      - slider3_joint
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    allow_partial_joints_goal: false
+    open_loop_control: true
+    allow_integration_in_goal_trajectories: true
+    action_monitor_rate: 20.0
+    # goal_time: 0.0
+    # constraints:
+    #   stopped_velocity_tolerance: 0.01
+    #   goal_time: 0.0

unilabos/device_mesh/devices/toyo_xyz/joint_config.json

@@ -0,0 +1,14 @@
+{
+    "slider1_joint": {
+        "child":"slider1_link",
+        "axis" : "x"
+    },
+    "slider2_joint": {
+        "child":"slider2_link",
+        "axis" : "x"
+    },
+    "slider3_joint": {
+        "child":"slider3_link",
+        "axis" : "x"
+    }
+}

unilabos/device_mesh/devices/toyo_xyz/macro_device.xacro

@@ -0,0 +1,465 @@
+<?xml version="1.0" ?>
+
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="toyo_xyz" 
+  params="length1:=0.1 min_d1:=0.01 max_d1:=0.01 slider_d1:=0.135
+          length2:=0.1 min_d2:=0.01 max_d2:=0.01 slider_d2:=0.116
+          length3:=0.1 min_d3:=0.01 max_d3:=0.01 slider_d3:=0.090
+          parent_link:='' station_name:='' device_name:='' x:=0 y:=0 z:=0 rx:=0 ry:=0 r:=0
+          mesh_path:=''">
+
+
+    <joint name="${station_name}${device_name}base_link_joint" type="fixed">
+      <origin xyz="${x} ${y} ${z}" rpy="${rx} ${ry} ${r}" />
+      <parent link="${parent_link}"/>
+      <child link="${station_name}${device_name}device_link"/>
+      <axis xyz="0 0 0"/>
+    </joint>
+
+    <link name="${station_name}${device_name}device_link"/>
+    <joint name="${station_name}${device_name}device_link_joint" type="fixed">
+      <origin xyz="${-min_d1}  0 0" rpy="0 0 0" />
+      <parent link="${station_name}${device_name}device_link"/>
+      <child link="${station_name}${device_name}base_link"/>
+      <axis xyz="0 0 0"/>
+    </joint>
+
+    <link name="${station_name}${device_name}base_link">
+      <inertial>
+        <origin rpy="0 0 0" xyz="-0.0120236017581094 -0.0517106842624209 0.0409192044637189"/>
+        <mass value="0.69481680376074"/>
+        <inertia ixx="0.000936712501344659" ixy="7.23677144617562E-06" ixz="-1.24166862138852E-06" iyy="0.000379264833590696" iyz="9.52650081379508E-07" izz="0.000898185165939333"/>
+      </inertial>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/base_link.STL"/>
+        </geometry>
+         <material name="">
+          <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/base_link.STL"/>
+        </geometry>
+      </collision>
+    </link>
+    <link name="${station_name}${device_name}length1_link">
+      <inertial>
+        <origin rpy="0 0 0" xyz="0.494579219297241 -0.0104662192149046 0.020043711867505"/>
+        <mass value="5.94417515905243"/>
+        <inertia ixx="0.00674507129097306" ixy="9.38238612394185E-09" ixz="4.35684774317116E-06" iyy="0.485770401104387" iyz="2.57353070463078E-06" izz="0.489757831272925"/>
+      </inertial>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/length1_link.STL" scale="${(length1 + min_d1 + max_d1 + slider_d1)} 1 1"/>
+        </geometry>
+         <material name="">
+          <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/length1_link.STL" scale="${(length1 + min_d1 + max_d1 + slider_d1)} 1 1"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${station_name}${device_name}length1_joint" type="fixed">
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <parent link="${station_name}${device_name}base_link"/>
+      <child link="${station_name}${device_name}length1_link"/>
+      <axis xyz="0 0 1"/>
+    </joint>
+
+
+
+
+    <link name="${station_name}${device_name}slider1_link">
+      <inertial>
+        <origin rpy="0 0 0" xyz="0 0 -0.00730412052530301"/>
+        <mass value="0.118155140657232"/>
+        <inertia ixx="6.43342434854928E-05" ixy="-2.31210361323828E-11" ixz="3.06922820322201E-10" iyy="0.000154199389116579" iyz="1.20524516927812E-08" izz="0.000215806482689218"/>
+      </inertial>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/slider1_link.STL"/>
+        </geometry>
+         <material name="">
+          <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/slider1_link.STL"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${station_name}${device_name}slider1_joint" type="prismatic">
+      <origin rpy="0 0 0" xyz="${min_d1 + slider_d1/2} 0 0.078"/>
+      <parent link="${station_name}${device_name}base_link"/>
+      <child link="${station_name}${device_name}slider1_link"/>
+      <axis xyz="1 0 0"/>
+      <limit effort="0" lower="0" upper="${length1}" velocity="0"/>
+    </joint>
+
+
+
+    <link name="${station_name}${device_name}chain_link">
+      <inertial>
+        <origin rpy="0 0 0" xyz="0.49922930196331 -0.00327071484975672 0.0207539916460562"/>
+        <mass value="2.00787563607981"/>
+        <inertia ixx="0.00240786362662634" ixy="1.44801915606072E-05" ixz="-3.1726019259437E-05" iyy="0.167850452350525" iyz="-0.000138217054257536" izz="0.168373005725347"/>
+      </inertial>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/chain_link.STL" scale="${(length2 + min_d2 + max_d2 + slider_d2)} 1 1"/>
+        </geometry>
+         <material name="">
+          <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/chain_link.STL" scale="${(length2 + min_d2 + max_d2 + slider_d2)} 1 1"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${station_name}${device_name}chain_joint" type="fixed">
+      <origin rpy="0 0 1.5708" xyz="-0.02 -0.041 -0.0005"/>
+      <parent link="${station_name}${device_name}slider1_link"/>
+      <child link="${station_name}${device_name}chain_link"/>
+      <axis xyz="0 0 0"/>
+    </joint>
+
+
+
+    <link name="${station_name}${device_name}base2_link">
+      <inertial>
+        <origin rpy="0 0 0" xyz="-0.00645898766177746 0.0354023221709439 0.0277640039757229"/>
+        <mass value="0.341816215620884"/>
+        <inertia ixx="0.000246380769664854" ixy="-3.5162220656232E-06" ixz="3.72156984182819E-08" iyy="0.00014938476423929" iyz="-2.01567401863811E-06" izz="0.000271539741067036"/>
+      </inertial>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/base2_link.STL"/>
+        </geometry>
+         <material name="">
+          <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/base2_link.STL"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${station_name}${device_name}base2_joint" type="fixed">
+      <origin rpy="1.5708 0 1.5708" xyz="-0.0145 0.0295 0.04915"/>
+      <parent link="${station_name}${device_name}slider1_link"/>
+      <child link="${station_name}${device_name}base2_link"/>
+      <axis xyz="0 0 0"/>
+    </joint>
+
+
+
+    <link name="${station_name}${device_name}slider2_link">
+      <inertial>
+        <origin rpy="0 0 0" xyz="-1.94863440558524E-06 -4.2412482447185E-05 -0.00600639156586869"/>
+        <mass value="0.0568102419437891"/>
+        <inertia ixx="1.29719509999494E-05" ixy="-2.84670156002291E-09" ixz="-1.99529353027075E-10" iyy="5.43277686573641E-05" iyz="-5.94709690026503E-09" izz="6.6299564390705E-05"/>
+      </inertial>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/slider2_link.STL"/>
+        </geometry>
+         <material name="">
+          <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/slider2_link.STL"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${station_name}${device_name}slider2_joint" type="prismatic">
+      <origin rpy="1.5708 0 1.5708" xyz="0.0455 ${min_d2 + slider_d2/2 + 0.0295} 0.049"/>
+      <parent link="${station_name}${device_name}slider1_link"/>
+      <child link="${station_name}${device_name}slider2_link"/>
+      <axis xyz="1 0 0"/>
+      <limit effort="0" lower="0" upper="${length2}" velocity="0"/>
+    </joint>
+
+
+    <link  name="${station_name}${device_name}length2_link">
+      <inertial>
+        <origin
+          xyz="0.495 -0.00012803097983341 0.0216248093421714"
+          rpy="0 0 0" />
+        <mass  value="2.0281266583716" />
+        <inertia
+          ixx="0.000735867784216875"
+          ixy="4.46823528502648E-18"
+          ixz="1.62564733709183E-17"
+          iyy="0.166018547616233"
+          iyz="8.35811182464874E-07"
+          izz="0.166011809812984" />
+      </inertial>
+      <visual>
+        <origin  xyz="0 0 0"  rpy="0 0 0" />
+        <geometry>
+          <mesh  filename="file://${mesh_path}/devices/toyo_xyz/meshes/length2_link.STL" scale="${(length2 + min_d2 + max_d2 + slider_d2)} 1 1"/>
+        </geometry>
+        <material  name="">
+          <color  rgba="0.756862745098039 0.768627450980392 0.752941176470588 1" />
+        </material>
+      </visual>
+      <collision>
+        <origin  xyz="0 0 0"  rpy="0 0 0" />
+        <geometry>
+          <mesh  filename="file://${mesh_path}/devices/toyo_xyz/meshes/length2_link.STL" scale="${(length2 + min_d2 + max_d2 + slider_d2)} 1 1"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${station_name}${device_name}length2_joint" type="fixed">
+      <origin  xyz="-0.0145 0.0295 0.04915"  rpy="1.5708 0 1.5708" />
+      <parent  link="${station_name}${device_name}slider1_link" />
+      <child   link="${station_name}${device_name}length2_link" />
+      <axis    xyz="0 0 0" />
+    </joint>
+
+
+
+    <link name="${station_name}${device_name}fixed_link">
+      <inertial>
+        <origin rpy="0 0 0" xyz="0.0841027924880233 -1.38777878078145E-17 -0.00753342821771308"/>
+        <mass value="0.114714657110459"/>
+        <inertia ixx="4.09226560926746E-05" ixy="-1.19246814602201E-20" ixz="1.10812310425511E-07" iyy="0.000219464025912457" iyz="-3.47441993328604E-22" izz="0.000257639354963189"/>
+      </inertial>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/fixed_link.STL"/>
+        </geometry>
+         <material name="">
+          <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/fixed_link.STL"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${station_name}${device_name}fixed_joint" type="fixed">
+      <origin rpy="0 0 -1.5708" xyz="0 0.113 0.013"/>
+      <parent link="${station_name}${device_name}slider2_link"/>
+      <child link="${station_name}${device_name}fixed_link"/>
+      <axis xyz="0 0 0"/>
+    </joint>
+
+
+
+    <link name="${station_name}${device_name}base3_link">
+      <inertial>
+        <origin rpy="0 0 0" xyz="-0.0898266891415951 -0.000686554465181816 0.0214841690633115"/>
+        <mass value="0.253768676399038"/>
+        <inertia ixx="7.3143781299483E-05" ixy="-9.27468544438197E-07" ixz="-6.25920202213005E-07" iyy="0.000222729105415939" iyz="1.73297633775367E-06" izz="0.000226940773440045"/>
+      </inertial>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/base3_link.STL"/>
+        </geometry>
+         <material name="">
+          <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/base3_link.STL"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${station_name}${device_name}base3_joint" type="fixed">
+      <origin rpy="0 0 -1.5708" xyz="0 0.113 0.013"/>
+      <parent link="${station_name}${device_name}slider2_link"/>
+      <child link="${station_name}${device_name}base3_link"/>
+      <axis xyz="0 0 0"/>
+    </joint>
+
+
+
+    <link name="${station_name}${device_name}length3_link">
+      <inertial>
+        <origin rpy="0 0 0" xyz="0.495 2.22485346140083E-05 0.017182121490278"/>
+        <mass value="1.46660245378617"/>
+        <inertia ixx="0.000378412905544255" ixy="6.68641853450019E-20" ixz="1.30946080093724E-18" iyy="0.119979432170624" iyz="5.03952092605041E-07" izz="0.11996849156089"/>
+      </inertial>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/length3_link.STL" scale="${(length3 + min_d3 + max_d3 + slider_d3)} 1 1"/>
+        </geometry>
+         <material name="">
+          <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/length3_link.STL" scale="${(length3 + min_d3 + max_d3 + slider_d3)} 1 1"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${station_name}${device_name}length3_joint" type="fixed">
+      <origin rpy="0 0 -1.5708" xyz="0 0.113 0.013"/>
+      <parent link="${station_name}${device_name}slider2_link"/>
+      <child link="${station_name}${device_name}length3_link"/>
+      <axis xyz="0 0 0"/>
+    </joint> 
+
+
+
+    <link name="${station_name}${device_name}slider3_link">
+      <inertial>
+        <origin rpy="0 0 0" xyz="6.92336766672241E-06 2.33762226242717E-09 -0.0056316911606703"/>
+        <mass value="0.0319537658681183"/>
+        <inertia ixx="4.74895305241407E-06" ixy="-2.68838717157416E-13" ixz="4.74105113238926E-10" iyy="1.78967089054364E-05" iyz="-4.41481885417567E-13" izz="2.21974556051535E-05"/>
+      </inertial>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/slider3_link.STL"/>
+        </geometry>
+         <material name="">
+          <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/slider3_link.STL"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${station_name}${device_name}slider3_joint" type="prismatic">
+      <origin rpy="0 0 -1.5708" xyz="0 ${0.113 - min_d3 - slider_d3/2} 0.0635"/>
+      <parent link="${station_name}${device_name}slider2_link"/>
+      <child link="${station_name}${device_name}slider3_link"/>
+      <axis xyz="1 0 0"/>
+      <limit effort="0" lower="0" upper="${length3}" velocity="0"/>
+    </joint>
+
+
+
+    <link name="${station_name}${device_name}end3_link">
+      <inertial>
+        <origin rpy="0 0 0" xyz="0.00497294496885192 -2.44043265976157E-06 0.0183349877681029"/>
+        <mass value="0.0140489551253671"/>
+        <inertia ixx="3.43932725883609E-06" ixy="4.00452842192135E-11" ixz="-1.53817578472123E-08" iyy="1.94727500633638E-06" iyz="-3.82376052540752E-10" izz="1.72263562631362E-06"/>
+      </inertial>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/end3_link.STL"/>
+        </geometry>
+         <material name="">
+          <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/end3_link.STL"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${station_name}${device_name}end3_joint" type="fixed">
+      <origin rpy="0 0 -1.5708" xyz="0 -${min_d3 + slider_d3 + length3 + max_d3 -0.113} 0.013"/>
+      <parent link="${station_name}${device_name}slider2_link"/>
+      <child link="${station_name}${device_name}end3_link"/>
+      <axis xyz="0 0 0"/>
+    </joint>
+
+
+
+    <link name="${station_name}${device_name}end2_link">
+      <inertial>
+        <origin rpy="0 0 0" xyz="0.00495670363919842 -8.74633492434218E-06 0.0214414396703796"/>
+        <mass value="0.0200788183514264"/>
+        <inertia ixx="7.16397592830092E-06" ixy="4.75643325623778E-11" ixz="-2.20469268570818E-08" iyy="3.90488015971723E-06" iyz="-3.17180365916489E-09" izz="3.58779761734039E-06"/>
+      </inertial>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/end2_link.STL"/>
+        </geometry>
+         <material name="">
+          <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/end2_link.STL"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${station_name}${device_name}end2_joint" type="fixed">
+      <origin rpy="1.5708 0 1.5708" xyz="-0.0135 ${min_d2 + slider_d2 + length2 + max_d2 + 0.0295} 0.0492"/>
+      <parent link="${station_name}${device_name}slider1_link"/>
+      <child link="${station_name}${device_name}end2_link"/>
+      <axis xyz="0 0 0"/>
+    </joint>
+
+
+
+    <link name="${station_name}${device_name}end_link">
+      <inertial>
+        <origin rpy="0 0 0" xyz="0.00744127217617607 -2.30544170541074E-06 0.0287548952899474"/>
+        <mass value="0.0648209796507272"/>
+        <inertia ixx="4.85939921252094E-05" ixy="1.05486327324319E-09" ixz="-4.17427158603031E-08" iyy="2.1549381051207E-05" iyz="2.1129085201095E-09" izz="2.9433994127647E-05"/>
+      </inertial>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/end_link.STL"/>
+        </geometry>
+         <material name="">
+          <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/end_link.STL"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${station_name}${device_name}end_joint" type="fixed">
+      <origin rpy="0 0 0" xyz="${min_d1 + length1 + max_d1 + slider_d1} 0 0.001"/>
+      <parent link="${station_name}${device_name}base_link"/>
+      <child link="${station_name}${device_name}end_link"/>
+      <axis xyz="0 0 0"/>
+    </joint>
+
+
+
+  </xacro:macro>
+</robot>

unilabos/device_mesh/devices/toyo_xyz/param_config.json

@@ -0,0 +1,20 @@
+{
+    "private_param":
+    {
+        "min_d1": 0.01 ,
+        "max_d1": 0.01 ,
+        "slider_d1": 0.135,
+        "min_d2": 0.01 ,
+        "max_d2": 0.01 ,
+        "slider_d2": 0.116,
+        "min_d3": 0.01 ,
+        "max_d3": 0.01 ,
+        "slider_d3": 0.09
+    },
+    "public_param": 
+    {
+        "length1" :0.5,
+        "length2" :0.2,
+        "length3" :0.2
+    }
+}

unilabos/device_mesh/resource_visalization.py

@@ -1,13 +1,38 @@
+import json
 import os
 from pathlib import Path
+import re
+
+import yaml
 from launch import LaunchService
 from launch import LaunchDescription
 from launch_ros.actions import Node as nd
 import xacro
 from lxml import etree
-
+from launch_param_builder import load_yaml
+from launch_ros.parameter_descriptions import ParameterFile
 from unilabos.registry.registry import lab_registry
+from ament_index_python.packages import get_package_share_directory
 
+def get_pattern_matches(folder, pattern):
+    """Given all the files in the folder, find those that match the pattern.
+
+    If there are groups defined, the groups are returned. Otherwise the path to the matches are returned.
+    """
+    matches = []
+    if not folder.exists():
+        return matches
+    for child in folder.iterdir():
+        if not child.is_file():
+            continue
+        m = pattern.search(child.name)
+        if m:
+            groups = m.groups()
+            if groups:
+                matches.append(groups[0])
+            else:
+                matches.append(child)
+    return matches
 
 class ResourceVisualization:
     def __init__(self, device: dict, resource: dict, enable_rviz: bool = True):
@@ -31,9 +56,8 @@ class ResourceVisualization:
         self.enable_rviz = enable_rviz
         registry = lab_registry
 
-        self.srdf_str = '''
-        <?xml version="1.0" encoding="UTF-8"?>
-        <robot name="minimal">
+        self.srdf_str = '''<?xml version="1.0" ?>
+        <robot xmlns:xacro="http://ros.org/wiki/xacro" name="full_dev">
 
         </robot>
         '''
@@ -43,23 +67,46 @@ class ResourceVisualization:
         </robot>
         '''
         self.root = etree.fromstring(self.robot_state_str)
+        self.root_srdf = etree.fromstring(self.srdf_str)
                 
         xacro_uri = self.root.nsmap["xacro"]
 
+        self.moveit_nodes = {}
+        self.moveit_nodes_kinematics = {}
+        self.moveit_controllers_yaml = {
+            "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
+            "moveit_simple_controller_manager": {
+                "controller_names": []
+            }
+        }
+        self.ros2_controllers_yaml = {
+            "controller_manager": {
+                "ros__parameters": {
+                    "update_rate": 100,
+                    "joint_state_broadcaster": {
+                        "type": "joint_state_broadcaster/JointStateBroadcaster",
+                    }
+                }
+            }
+        }
+
         # 遍历设备节点
         for node in device.values():
-            if node['type'] == 'device' and node['class'] != '':
-                device_class = node['class']
-                # 检查设备类型是否在注册表中
-                if device_class not in registry.device_type_registry.keys():
-                    raise ValueError(f"设备类型 {device_class} 未在注册表中注册")
-            elif node['type'] in self.resource_type:
-                # print(registry.resource_type_registry)
-                resource_class = node['class']
-                if resource_class not in registry.resource_type_registry.keys():
-                    raise ValueError(f"资源类型 {resource_class} 未在注册表中注册")
-                elif "model" in registry.resource_type_registry[resource_class].keys():
-                    model_config = registry.resource_type_registry[resource_class]['model']
+            if node['type'] in self.resource_type or (node['type'] == 'device' and node['class'] != ''):
+                model_config = {}
+                if node['type'] in self.resource_type:
+                    resource_class = node['class']
+                    if resource_class not in registry.resource_type_registry.keys():
+                        raise ValueError(f"资源类型 {resource_class} 未在注册表中注册")
+                    elif "model" in registry.resource_type_registry[resource_class].keys():
+                        model_config = registry.resource_type_registry[resource_class]['model']
+                elif node['type'] == 'device' and node['class'] != '':
+                    device_class = node['class']
+                    if device_class not in registry.device_type_registry.keys():
+                        raise ValueError(f"设备类型 {device_class} 未在注册表中注册")
+                    elif "model" in registry.device_type_registry[device_class].keys():
+                        model_config = registry.device_type_registry[device_class]['model']
+                if model_config:
                     if model_config['type'] == 'resource':
                         self.resource_model[node['id']] = {
                             'mesh': f"{str(self.mesh_path)}/resources/{model_config['mesh']}",
@@ -71,18 +118,40 @@ class ResourceVisualization:
                                     'mesh_tf': model_config['children_mesh_tf']
                                 }
                     elif model_config['type'] == 'device':
+
                         new_include = etree.SubElement(self.root, f"{{{xacro_uri}}}include")
                         new_include.set("filename", f"{str(self.mesh_path)}/devices/{model_config['mesh']}/macro_device.xacro")
                         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("station_name", node["parent"]+'_')
+                        if node["parent"] is not None:
+                            new_dev.set("station_name", node["parent"]+'_')
                         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))
+                            new_dev.set("rx",str(float(node["config"]["rotation"]["x"])))
+                            new_dev.set("ry",str(float(node["config"]["rotation"]["y"])))
+                            new_dev.set("r",str(float(node["config"]["rotation"]["z"])))
+                        if "device_config" in node["config"]:
+                            for key, value in node["config"]["device_config"].items():
+                                new_dev.set(key, str(float(value)))
+
+                        # 添加ros2_controller
+                        if node['class'].startswith('moveit.'):
+                            new_include_controller = etree.SubElement(self.root, f"{{{xacro_uri}}}include")
+                            new_include_controller.set("filename", f"{str(self.mesh_path)}/devices/{model_config['mesh']}/config/macro.ros2_control.xacro")
+                            new_controller = etree.SubElement(self.root, f"{{{xacro_uri}}}{model_config['mesh']}_ros2_control")
+                            new_controller.set("device_name", node["id"]+"_")
+                            new_controller.set("mesh_path", str(self.mesh_path))
+
+                            # 添加moveit的srdf
+                            new_include_srdf = etree.SubElement(self.root_srdf, f"{{{xacro_uri}}}include")
+                            new_include_srdf.set("filename", f"{str(self.mesh_path)}/devices/{model_config['mesh']}/config/macro.srdf.xacro")
+                            new_srdf = etree.SubElement(self.root_srdf, f"{{{xacro_uri}}}{model_config['mesh']}_srdf")
+                            new_srdf.set("device_name", node["id"]+"_")
+                            self.moveit_nodes[node["id"]] = model_config['mesh']
                     else:
                         print("错误的注册表类型！")
         re = etree.tostring(self.root, encoding="unicode")
@@ -90,8 +159,37 @@ class ResourceVisualization:
         xacro.process_doc(doc)
         self.urdf_str = doc.toxml()
 
+        re_srdf = etree.tostring(self.root_srdf, encoding="unicode")
+        doc_srdf = xacro.parse(re_srdf)
+        xacro.process_doc(doc_srdf)
+        self.urdf_str_srdf = doc_srdf.toxml()
+        if self.moveit_nodes:
+            self.moveit_init()
 
-    def create_launch_description(self, urdf_str: str) -> LaunchDescription:
+    def moveit_init(self):
+
+        for name, config in self.moveit_nodes.items():
+            controller_dict = yaml.safe_load(open(f"{str(self.mesh_path)}/devices/{config}/config/ros2_controllers.yaml", "r"))
+            moveit_dict = yaml.safe_load(open(f"{str(self.mesh_path)}/devices/{config}/config/moveit_controllers.yaml", "r"))
+            kinematics_dict = yaml.safe_load(open(f"{str(self.mesh_path)}/devices/{config}/config/kinematics.yaml", "r"))
+            
+            for key_kinematics, value_kinematics in kinematics_dict.items():
+                self.moveit_nodes_kinematics[f'{name}_{key_kinematics}'] = value_kinematics
+            
+            for key, value in controller_dict['controller_manager']['ros__parameters'].items():
+                if key == 'update_rate' or key == 'joint_state_broadcaster':
+                    continue
+                self.ros2_controllers_yaml['controller_manager']['ros__parameters'][f"{name}_{key}"] = value
+                controller_dict[key]['ros__parameters']['joints'] = [f"{name}_{joint}" for joint in controller_dict[key]['ros__parameters']['joints']]
+                self.ros2_controllers_yaml[f"{name}_{key}"] = controller_dict[key]
+
+            for controller_name in moveit_dict['moveit_simple_controller_manager']['controller_names']:
+                self.moveit_controllers_yaml['moveit_simple_controller_manager']['controller_names'].append(f"{name}_{controller_name}")
+                moveit_dict['moveit_simple_controller_manager'][controller_name]['joints'] = [f"{name}_{joint}" for joint in moveit_dict['moveit_simple_controller_manager'][controller_name]['joints']]
+                self.moveit_controllers_yaml['moveit_simple_controller_manager'][f"{name}_{controller_name}"] = moveit_dict['moveit_simple_controller_manager'][controller_name]
+
+
+    def create_launch_description(self) -> LaunchDescription:
         """
         创建launch描述，包含robot_state_publisher和move_group节点
 
@@ -101,10 +199,93 @@ class ResourceVisualization:
         Returns:
             LaunchDescription: launch描述对象
         """
+        moveit_configs_utils_path = Path(get_package_share_directory("moveit_configs_utils"))
+        default_folder = moveit_configs_utils_path / "default_configs"
+        planning_pattern = re.compile("^(.*)_planning.yaml$")
+        pipelines = []
 
-        
+        for pipeline in get_pattern_matches(default_folder, planning_pattern):
+            if pipeline not in pipelines:
+                pipelines.append(pipeline)
+
+        if "ompl" in pipelines:
+            default_planning_pipeline = "ompl"
+        else:
+            default_planning_pipeline = pipelines[0]
+
+        planning_pipelines = {
+            "planning_pipelines": pipelines,
+            "default_planning_pipeline": default_planning_pipeline,
+        }
+
+        for pipeline in pipelines:
+            planning_pipelines[pipeline] = load_yaml(
+                default_folder /  f"{pipeline}_planning.yaml"
+            )
+
+        if "ompl" in planning_pipelines:
+            ompl_config = planning_pipelines["ompl"]
+            if "planner_configs" not in ompl_config:
+                ompl_config.update(load_yaml(default_folder / "ompl_defaults.yaml"))
+
+        yaml.safe_dump(self.ros2_controllers_yaml, open(f"{str(self.mesh_path)}/ros2_controllers.yaml", "w"))
+
+        robot_description_planning = {
+            "default_velocity_scaling_factor": 0.1,
+            "default_acceleration_scaling_factor": 0.1,
+            "cartesian_limits": {
+            "max_trans_vel": 1.0,
+            "max_trans_acc": 2.25,
+            "max_trans_dec": -5.0,
+            "max_rot_vel": 1.57
+            }
+        }
         # 解析URDF文件
-        robot_description = urdf_str
+        robot_description = self.urdf_str
+        urdf_str_srdf = self.urdf_str_srdf
+
+        kinematics_dict = self.moveit_nodes_kinematics
+
+        if self.moveit_nodes:
+            controllers = []
+            ros2_controllers = ParameterFile(f"{str(self.mesh_path)}/ros2_controllers.yaml", allow_substs=True)
+
+            controllers.append(
+                nd(
+                    package="controller_manager",
+                    executable="ros2_control_node",
+                    output='screen',
+                    parameters=[
+                        {"robot_description": robot_description},
+                        ros2_controllers,
+                    ]
+                )
+            )
+            for controller in self.moveit_controllers_yaml['moveit_simple_controller_manager']['controller_names']:
+                controllers.append(
+                    nd(
+                        package="controller_manager",
+                        executable="spawner",
+                        arguments=[f"{controller}", "--controller-manager", f"controller_manager"],
+                        output="screen",
+                    )
+                )
+            controllers.append(
+                nd(
+                        package="controller_manager",
+                        executable="spawner",
+                        arguments=["joint_state_broadcaster", "--controller-manager", f"controller_manager"],
+                        output="screen",
+                )
+            )
+            for i in controllers:
+                self.launch_description.add_action(i)
+        else:
+            ros2_controllers = None
+
+
+
+
 
         # 创建robot_state_publisher节点
         robot_state_publisher = nd(
@@ -115,23 +296,21 @@ class ResourceVisualization:
             parameters=[{
                 'robot_description': robot_description,
                 'use_sim_time': False
-            }]
+            },
+            # kinematics_dict
+            ]
         )
 
-        # 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=[{
+                'allow_trajectory_execution': True,
                 'robot_description': robot_description,
-                'robot_description_semantic': self.srdf_str,
+                'robot_description_semantic': urdf_str_srdf,
+                'robot_description_kinematics': kinematics_dict,
                 'capabilities': '',
                 'disable_capabilities': '',
                 'monitor_dynamics': False,
@@ -141,7 +320,13 @@ class ResourceVisualization:
                 'publish_geometry_updates': True,
                 'publish_state_updates': True,
                 'publish_transforms_updates': True,
-            }]
+                # 'robot_description_planning': robot_description_planning,
+            },
+            self.moveit_controllers_yaml,
+            # ompl_planning_pipeline_config,
+            robot_description_planning,
+            planning_pipelines,
+            ]
         )
 
         # 将节点添加到launch描述中
@@ -156,7 +341,14 @@ class ResourceVisualization:
                 executable='rviz2',
                 name='rviz2',
                 arguments=['-d', f"{str(self.mesh_path)}/view_robot.rviz"],
-                output='screen'
+                output='screen',
+                parameters=[
+                    {'robot_description_kinematics': kinematics_dict,
+                     },
+                    robot_description_planning,
+                    planning_pipelines,
+
+                ]
             )
             self.launch_description.add_action(rviz_node)
 
@@ -169,6 +361,11 @@ class ResourceVisualization:
         Args:
             urdf_str: URDF文件路径
         """
-        launch_description = self.create_launch_description(self.urdf_str)
+        launch_description = self.create_launch_description()
+        # print('--------------------------------')
+        # print(self.moveit_controllers_yaml)
+        # print('--------------------------------')
+        # print(self.urdf_str)
+        # print('--------------------------------')
         self.launch_service.include_launch_description(launch_description)
         self.launch_service.run()

unilabos/device_mesh/ros2_controllers.yaml

@@ -0,0 +1,31 @@
+benyao_arm_controller:
+  ros__parameters:
+    command_interfaces:
+    - position
+    joints:
+    - benyao_arm_base_joint
+    - benyao_arm_link_1_joint
+    - benyao_arm_link_2_joint
+    - benyao_arm_link_3_joint
+    - benyao_gripper_base_joint
+    state_interfaces:
+    - position
+    - velocity
+benyao_gripper_controller:
+  ros__parameters:
+    command_interfaces:
+    - position
+    joints:
+    - benyao_gripper_right_joint
+    state_interfaces:
+    - position
+    - velocity
+controller_manager:
+  ros__parameters:
+    benyao_arm_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+    benyao_gripper_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    update_rate: 100

unilabos/device_mesh/view_robot.rviz

@@ -1,23 +1,15 @@
 Panels:
   - Class: rviz_common/Displays
-    Help Height: 138
+    Help Height: 0
     Name: Displays
     Property Tree Widget:
       Expanded:
-        - /Global Options1
-        - /TF1
         - /TF1/Tree1
-        - /RobotModel1
-        - /PlanningScene1
-        - /PlanningScene1/Scene Geometry1
-        - /RobotState1
-        - /RobotState1/Links1
-        - /MotionPlanning1
         - /MotionPlanning1/Scene Geometry1
         - /MotionPlanning1/Scene Robot1
         - /MotionPlanning1/Planning Request1
-      Splitter Ratio: 0.5
-    Tree Height: 345
+      Splitter Ratio: 0.5016146302223206
+    Tree Height: 1112
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -93,7 +85,7 @@ Visualization Manager:
       Value: false
       Visual Enabled: true
     - Class: moveit_rviz_plugin/PlanningScene
-      Enabled: false
+      Enabled: true
       Move Group Namespace: ""
       Name: PlanningScene
       Planning Scene Topic: /monitored_planning_scene
@@ -104,7 +96,7 @@ Visualization Manager:
         Scene Display Time: 0.009999999776482582
         Show Scene Geometry: true
         Voxel Coloring: Z-Axis
-        Voxel Rendering: Occupied Voxels
+        Voxel Rendering: Disabled
       Scene Robot:
         Attached Body Color: 150; 50; 150
         Links:
@@ -113,10 +105,108 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
+          PLR_STATION_deck_device_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          PLR_STATION_deck_first_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          PLR_STATION_deck_fourth_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          PLR_STATION_deck_main_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          PLR_STATION_deck_second_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          PLR_STATION_deck_socketTypeGenericSbsFootprint:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          PLR_STATION_deck_socketTypeHEPAModule:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          PLR_STATION_deck_third_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_link_1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_link_2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_link_3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_slideway:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_device_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          benyao_gripper_base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_gripper_left:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_gripper_right:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          hotel_base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          hotel_device_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          hotel_socketTypeGenericSbsFootprint:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          world:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
         Robot Alpha: 1
         Show Robot Collision: false
         Show Robot Visual: false
-      Value: false
+      Value: true
     - Attached Body Color: 150; 50; 150
       Class: moveit_rviz_plugin/RobotState
       Collision Enabled: false
@@ -203,6 +293,63 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
+          benyao_arm_base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_link_1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_link_2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_link_3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_slideway:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_device_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          benyao_gripper_base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_gripper_left:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_gripper_right:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          hotel_base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          hotel_device_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          hotel_socketTypeGenericSbsFootprint:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           world:
             Alpha: 1
             Show Axes: false
@@ -230,7 +377,7 @@ Visualization Manager:
         Goal State Color: 250; 128; 0
         Interactive Marker Size: 0
         Joint Violation Color: 255; 0; 255
-        Planning Group: ""
+        Planning Group: benyao_arm
         Query Goal State: false
         Query Start State: false
         Show Workspace: false
@@ -242,9 +389,9 @@ Visualization Manager:
         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
+        Show Scene Geometry: false
+        Voxel Coloring: Cell Probability
+        Voxel Rendering: All Voxels
       Scene Robot:
         Attached Body Color: 150; 50; 150
         Links:
@@ -290,6 +437,63 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
+          benyao_arm_base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_link_1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_link_2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_link_3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_slideway:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_device_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          benyao_gripper_base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_gripper_left:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_gripper_right:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          hotel_base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          hotel_device_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          hotel_socketTypeGenericSbsFootprint:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           world:
             Alpha: 1
             Show Axes: false
@@ -345,43 +549,43 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 1.0284695625305176
+      Distance: 2.622864246368408
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.29730814695358276
-        Y: 0.21228469908237457
-        Z: 0.20008830726146698
+        X: -0.2880733013153076
+        Y: -0.16004444658756256
+        Z: -0.16730672121047974
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.38979560136795044
+      Pitch: 0.48479583859443665
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 0.06074193865060806
+      Yaw: 0.042561568319797516
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1656
+  Height: 2032
   Hide Left Dock: false
   Hide Right Dock: true
   MotionPlanning:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000003a3000005dcfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000002510000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000007a00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000002cb0000037f000002b800ffffff000000010000010f00000387fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003870000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000627000005dc00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000003a30000079bfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000027000004c60000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000002600000026fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000004f9000002c9000002b800ffffff000000010000010f00000387fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003870000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000bc50000079b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Tool Properties:
     collapsed: false
   Views:
     collapsed: true
-  Width: 2518
-  X: 385
-  Y: 120
+  Width: 3956
+  X: 140
+  Y: 54

unilabos/devices/liquid_handling/liquid_handler_abstract.py

@@ -6,13 +6,8 @@ import asyncio
 import time
 
 from pylabrobot.liquid_handling import LiquidHandler
-from pylabrobot.resources import (
-    Resource,
-    TipRack,
-    Container,
-    Coordinate,
-    Well
-)
+from pylabrobot.resources import Resource, TipRack, Container, Coordinate, Well
+
 
 class LiquidHandlerAbstract(LiquidHandler):
     """Extended LiquidHandler with additional operations."""
@@ -21,6 +16,19 @@ class LiquidHandlerAbstract(LiquidHandler):
     # REMOVE LIQUID --------------------------------------------------
     # ---------------------------------------------------------------
 
+    async def create_protocol(
+        self,
+        protocol_name: str,
+        protocol_description: str,
+        protocol_version: str,
+        protocol_author: str,
+        protocol_date: str,
+        protocol_type: str,
+        none_keys: List[str] = [],
+    ):
+        """Create a new protocol with the given metadata."""
+        pass
+
     async def remove_liquid(
         self,
         vols: List[float],
@@ -35,26 +43,26 @@ class LiquidHandlerAbstract(LiquidHandler):
         spread: Optional[Literal["wide", "tight", "custom"]] = "wide",
         delays: Optional[List[int]] = None,
         is_96_well: Optional[bool] = False,
-        top: Optional[List(float)] = None,
-        none_keys: List[str] = []
+        top: Optional[List[float]] = None,
+        none_keys: List[str] = [],
     ):
         """A complete *remove* (aspirate → waste) operation."""
         trash = self.deck.get_trash_area()
         try:
             if is_96_well:
-                pass # This mode is not verified
+                pass  # This mode is not verified
             else:
                 if len(vols) != len(sources):
                     raise ValueError("Length of `vols` must match `sources`.")
 
                 for src, vol in zip(sources, vols):
-                    self.move_to(src, dis_to_top=top[0] if top else 0)
+                    await self.move_to(src, dis_to_top=top[0] if top else 0)
                     tip = next(self.current_tip)
                     await self.pick_up_tips(tip)
                     await self.aspirate(
                         resources=[src],
                         vols=[vol],
-                        use_channels=use_channels, # only aspirate96 used, default to None
+                        use_channels=use_channels,  # only aspirate96 used, default to None
                         flow_rates=[flow_rates[0]] if flow_rates else None,
                         offsets=[offsets[0]] if offsets else None,
                         liquid_height=[liquid_height[0]] if liquid_height else None,
@@ -64,15 +72,15 @@ class LiquidHandlerAbstract(LiquidHandler):
                     await self.custom_delay(seconds=delays[0] if delays else 0)
                     await self.dispense(
                         resources=waste_liquid,
-                         vols=[vol],
-                         use_channels=use_channels,
-                         flow_rates=[flow_rates[1]] if flow_rates else None,
-                         offsets=[offsets[1]] if offsets else None,
-                         liquid_height=[liquid_height[1]] if liquid_height else None,
-                         blow_out_air_volume=blow_out_air_volume[1] if blow_out_air_volume else None,
-                         spread=spread,
-                     )
-                    await self.discard_tips() # For now, each of tips is discarded after use
+                        vols=[vol],
+                        use_channels=use_channels,
+                        flow_rates=[flow_rates[1]] if flow_rates else None,
+                        offsets=[offsets[1]] if offsets else None,
+                        liquid_height=[liquid_height[1]] if liquid_height else None,
+                        blow_out_air_volume=blow_out_air_volume[1] if blow_out_air_volume else None,
+                        spread=spread,
+                    )
+                    await self.discard_tips()  # For now, each of tips is discarded after use
 
         except Exception as e:
             raise RuntimeError(f"Liquid removal failed: {e}") from e
@@ -100,13 +108,13 @@ class LiquidHandlerAbstract(LiquidHandler):
         mix_vol: Optional[int] = None,
         mix_rate: Optional[int] = None,
         mix_liquid_height: Optional[float] = None,
-        none_keys: List[str] = []
+        none_keys: List[str] = [],
     ):
         """A complete *add* (aspirate reagent → dispense into targets) operation."""
 
         try:
             if is_96_well:
-                pass # This mode is not verified.
+                pass  # This mode is not verified.
             else:
                 if len(asp_vols) != len(targets):
                     raise ValueError("Length of `vols` must match `targets`.")
@@ -122,7 +130,7 @@ class LiquidHandlerAbstract(LiquidHandler):
                         offsets=[offsets[0]] if offsets else None,
                         liquid_height=[liquid_height[0]] if liquid_height else None,
                         blow_out_air_volume=[blow_out_air_volume[0]] if blow_out_air_volume else None,
-                        spread=spread
+                        spread=spread,
                     )
                     if delays is not None:
                         await self.custom_delay(seconds=delays[0])
@@ -144,7 +152,8 @@ class LiquidHandlerAbstract(LiquidHandler):
                         mix_vol=mix_vol,
                         offsets=offsets if offsets else None,
                         height_to_bottom=mix_liquid_height if mix_liquid_height else None,
-                        mix_rate=mix_rate if mix_rate else None)
+                        mix_rate=mix_rate if mix_rate else None,
+                    )
                     if delays is not None:
                         await self.custom_delay(seconds=delays[1])
                     await self.touch_tip(targets[_])
@@ -158,13 +167,13 @@ class LiquidHandlerAbstract(LiquidHandler):
     # ---------------------------------------------------------------
     async def transfer_liquid(
         self,
-        asp_vols: Union[List[float], float],
-        dis_vols: Union[List[float], float],
         sources: Sequence[Container],
         targets: Sequence[Container],
         tip_racks: Sequence[TipRack],
         *,
         use_channels: Optional[List[int]] = None,
+        asp_vols: Union[List[float], float],
+        dis_vols: Union[List[float], float],
         asp_flow_rates: Optional[List[Optional[float]]] = None,
         dis_flow_rates: Optional[List[Optional[float]]] = None,
         offsets: Optional[List[Coordinate]] = None,
@@ -179,7 +188,7 @@ class LiquidHandlerAbstract(LiquidHandler):
         mix_rate: Optional[int] = None,
         mix_liquid_height: Optional[float] = None,
         delays: Optional[List[int]] = None,
-        none_keys: List[str] = []
+        none_keys: List[str] = [],
     ):
         """Transfer liquid from each *source* well/plate to the corresponding *target*.
 
@@ -201,14 +210,15 @@ class LiquidHandlerAbstract(LiquidHandler):
             # 96‑channel head mode
             # ------------------------------------------------------------------
             if is_96_well:
-                pass # This mode is not verified
+                pass  # This mode is not verified
             else:
                 if not (len(asp_vols) == len(sources) and len(dis_vols) == len(targets)):
                     raise ValueError("`sources`, `targets`, and `vols` must have the same length.")
 
                 tip_iter = self.iter_tips(tip_racks)
-                for src, tgt, asp_vol, asp_flow_rate, dis_vol, dis_flow_rate in (
-                        zip(sources, targets, asp_vols, asp_flow_rates, dis_vols, dis_flow_rates)):
+                for src, tgt, asp_vol, asp_flow_rate, dis_vol, dis_flow_rate in zip(
+                    sources, targets, asp_vols, asp_flow_rates, dis_vols, dis_flow_rates
+                ):
                     tip = next(tip_iter)
                     await self.pick_up_tips(tip)
                     # Aspirate from source
@@ -247,9 +257,9 @@ class LiquidHandlerAbstract(LiquidHandler):
         except Exception as exc:
             raise RuntimeError(f"Liquid transfer failed: {exc}") from exc
 
-# ---------------------------------------------------------------
-# Helper utilities
-# ---------------------------------------------------------------
+    # ---------------------------------------------------------------
+    # Helper utilities
+    # ---------------------------------------------------------------
 
     async def custom_delay(self, seconds=0, msg=None):
         """
@@ -266,28 +276,26 @@ class LiquidHandlerAbstract(LiquidHandler):
                 print(f"Done: {msg}")
                 print(f"Current time: {time.strftime('%H:%M:%S')}")
 
-    async def touch_tip(self,
-                        targets: Sequence[Container],
-                        ):
+    async def touch_tip(self, targets: Sequence[Container]):
         """Touch the tip to the side of the well."""
         await self.aspirate(
             resources=[targets],
             vols=[0],
             use_channels=None,
             flow_rates=None,
-            offsets=[Coordinate(x=-targets.get_size_x()/2,y=0,z=0)],
+            offsets=[Coordinate(x=-targets.get_size_x() / 2, y=0, z=0)],
             liquid_height=None,
-            blow_out_air_volume=None
+            blow_out_air_volume=None,
         )
-        #await self.custom_delay(seconds=1) # In the simulation, we do not need to wait
+        # await self.custom_delay(seconds=1) # In the simulation, we do not need to wait
         await self.aspirate(
             resources=[targets],
             vols=[0],
             use_channels=None,
             flow_rates=None,
-            offsets=[Coordinate(x=targets.get_size_x()/2,y=0,z=0)],
+            offsets=[Coordinate(x=targets.get_size_x() / 2, y=0, z=0)],
             liquid_height=None,
-            blow_out_air_volume=None
+            blow_out_air_volume=None,
         )
 
     async def mix(
@@ -298,9 +306,9 @@ class LiquidHandlerAbstract(LiquidHandler):
         height_to_bottom: Optional[float] = None,
         offsets: Optional[Coordinate] = None,
         mix_rate: Optional[float] = None,
-        none_keys: List[str] = []
+        none_keys: List[str] = [],
     ):
-        if mix_time is None: # No mixing required
+        if mix_time is None:  # No mixing required
             return
         """Mix the liquid in the target wells."""
         for _ in range(mix_time):
@@ -333,7 +341,7 @@ class LiquidHandlerAbstract(LiquidHandler):
         tip_iter = self.iter_tips(tip_racks)
         self.current_tip = tip_iter
 
-    async def move_to(self, well: Well, dis_to_top: float = 0 , channel: int = 0):
+    async def move_to(self, well: Well, dis_to_top: float = 0, channel: int = 0):
         """
         Move a single channel to a specific well with a given z-height.
 
@@ -352,4 +360,3 @@ class LiquidHandlerAbstract(LiquidHandler):
         await self.move_channel_x(channel, abs_loc.x)
         await self.move_channel_y(channel, abs_loc.y)
         await self.move_channel_z(channel, abs_loc.z + well_height + dis_to_top)
-

unilabos/devices/liquid_handling/test liquid handler/convert_biomek.py

@@ -0,0 +1,154 @@
+
+import json
+from typing import Sequence, Optional, List, Union, Literal
+json_path = "/Users/guangxinzhang/Documents/Deep Potential/opentrons/convert/protocols/enriched_steps/sci-lucif-assay4.json"
+
+with open(json_path, "r") as f:
+    data = json.load(f)
+
+transfer_example = data[0]
+#print(transfer_example)
+
+
+temp_protocol = []
+TipLocation = "BC1025F"  # Assuming this is a fixed tip location for the transfer
+sources = transfer_example["sources"]  # Assuming sources is a list of Container objects
+targets = transfer_example["targets"]  # Assuming targets is a list of Container objects
+tip_racks = transfer_example["tip_racks"]  # Assuming tip_racks is a list of TipRack objects
+asp_vols = transfer_example["asp_vols"]  # Assuming asp_vols is a list of volumes
+solvent = "PBS"
+
+def transfer_liquid(
+        #self,
+        sources,#: Sequence[Container],
+        targets,#: Sequence[Container],
+        tip_racks,#: Sequence[TipRack],
+        TipLocation,
+        # *,
+        # use_channels: Optional[List[int]] = None,
+        asp_vols: Union[List[float], float],
+        solvent: Optional[str] = None,
+        # dis_vols: Union[List[float], float],
+        # asp_flow_rates: Optional[List[Optional[float]]] = None,
+        # dis_flow_rates: Optional[List[Optional[float]]] = None,
+        # offsets,#: Optional[List[]] = None,
+        # touch_tip: bool = False,
+        # liquid_height: Optional[List[Optional[float]]] = None,
+        # blow_out_air_volume: Optional[List[Optional[float]]] = None,
+        # spread: Literal["wide", "tight", "custom"] = "wide",
+        # is_96_well: bool = False,
+        # mix_stage: Optional[Literal["none", "before", "after", "both"]] = "none",
+        # mix_times,#: Optional[list() = None,
+        # mix_vol: Optional[int] = None,
+        # mix_rate: Optional[int] = None,
+        # mix_liquid_height: Optional[float] = None,
+        # delays: Optional[List[int]] = None,
+        # none_keys: List[str] = []
+    ):
+        # -------- Build Biomek transfer step --------
+        # 1) Construct default parameter scaffold (values mirror Biomek “Transfer” block).
+
+        transfer_params = {
+            "Span8": False,
+            "Pod": "Pod1",
+            "items": {},                       # to be filled below
+            "Wash": False,
+            "Dynamic?": True,
+            "AutoSelectActiveWashTechnique": False,
+            "ActiveWashTechnique": "",
+            "ChangeTipsBetweenDests": False,
+            "ChangeTipsBetweenSources": False,
+            "DefaultCaption": "",              # filled after we know first pair/vol
+            "UseExpression": False,
+            "LeaveTipsOn": False,
+            "MandrelExpression": "",
+            "Repeats": "1",
+            "RepeatsByVolume": False,
+            "Replicates": "1",
+            "ShowTipHandlingDetails": False,
+            "ShowTransferDetails": True,
+            "Solvent": "Water",
+            "Span8Wash": False,
+            "Span8WashVolume": "2",
+            "Span8WasteVolume": "1",
+            "SplitVolume": False,
+            "SplitVolumeCleaning": False,
+            "Stop": "Destinations",
+            "TipLocation": "BC1025F",
+            "UseCurrentTips": False,
+            "UseDisposableTips": True,
+            "UseFixedTips": False,
+            "UseJIT": True,
+            "UseMandrelSelection": True,
+            "UseProbes": [True, True, True, True, True, True, True, True],
+            "WashCycles": "1",
+            "WashVolume": "110%",
+            "Wizard": False
+        }
+
+        items: dict = {}
+        for idx, (src, dst) in enumerate(zip(sources, targets)):
+            items[str(idx)] = {
+                "Source": str(src),
+                "Destination": str(dst),
+                "Volume": asp_vols[idx] 
+            }
+        transfer_params["items"] = items
+        transfer_params["Solvent"] =  solvent if solvent else "Water"
+        transfer_params["TipLocation"] = TipLocation
+
+        if len(tip_racks) == 1:
+             transfer_params['UseCurrentTips'] = True
+        elif len(tip_racks) > 1:
+            transfer_params["ChangeTipsBetweenDests"] = True
+
+        return transfer_params
+
+action = transfer_liquid(sources=sources,targets=targets,tip_racks=tip_racks, asp_vols=asp_vols,solvent = solvent, TipLocation=TipLocation)
+print(json.dumps(action,indent=2))  
+# print(action)
+
+
+
+
+"""
+      "transfer": {
+
+        "items": {},
+        "Wash": false,
+        "Dynamic?": true,
+        "AutoSelectActiveWashTechnique": false,
+        "ActiveWashTechnique": "",
+        "ChangeTipsBetweenDests": true,
+        "ChangeTipsBetweenSources": false,
+        "DefaultCaption": "Transfer 100 µL from P13 to P3",
+        "UseExpression": false,
+        "LeaveTipsOn": false,
+        "MandrelExpression": "",
+        "Repeats": "1",
+        "RepeatsByVolume": false,
+        "Replicates": "1",
+        "ShowTipHandlingDetails": true,
+        "ShowTransferDetails": true,
+        
+        "Span8Wash": false,
+        "Span8WashVolume": "2",
+        "Span8WasteVolume": "1",
+        "SplitVolume": false,
+        "SplitVolumeCleaning": false,
+        "Stop": "Destinations",
+        "TipLocation": "BC1025F",
+        "UseCurrentTips": false,
+        "UseDisposableTips": false,
+        "UseFixedTips": false,
+        "UseJIT": true,
+        "UseMandrelSelection": true,
+        "UseProbes": [true, true, true, true, true, true, true, true],
+        "WashCycles": "3",
+        "WashVolume": "110%",
+        "Wizard": false
+"""
+
+
+
+

unilabos/devices/pump_and_valve/solenoid_valve_mock.py

@@ -5,22 +5,22 @@ class SolenoidValveMock:
     def __init__(self, port: str = "COM6"):
         self._status = "Idle"
         self._valve_position = "OPEN"
-    
+
     @property
     def status(self) -> str:
         return self._status
-    
+
     @property
     def valve_position(self) -> str:
         return self._valve_position
 
     def get_valve_position(self) -> str:
         return self._valve_position
-    
+
     def set_valve_position(self, position):
         self._status = "Busy"
         time.sleep(5)
-        
+
         self._valve_position = position
         time.sleep(5)
         self._status = "Idle"

unilabos/devices/pump_and_valve/vacuum_pump_mock.py

@@ -4,17 +4,17 @@ import time
 class VacuumPumpMock:
     def __init__(self, port: str = "COM6"):
         self._status = "OPEN"
-    
+
     @property
     def status(self) -> str:
         return self._status
 
     def get_status(self) -> str:
         return self._status
-    
+
     def set_status(self, position):
         time.sleep(5)
-        
+
         self._status = position
         time.sleep(5)
 

unilabos/devices/resource_container/container.py

@@ -0,0 +1,9 @@
+
+class HotelContainer:
+    def __init__(self, rotation: dict, device_config: dict):
+        self.rotation = rotation
+        self.device_config = device_config
+        self.status = 'idle'
+
+    def get_rotation(self):
+        return self.rotation

unilabos/devices/ros_dev/lh_joint_config.json

@@ -0,0 +1,38 @@
+{
+    "OTDeck":{
+        "joint_names":[
+            "first_joint",
+            "second_joint",
+            "third_joint",
+            "fourth_joint"
+        ],
+        "link_names":[
+            "first_link",
+            "second_link",
+            "third_link",
+            "fourth_link"
+        ],
+        "y":{
+            "first_joint":{
+            "factor":-0.001,
+            "offset":0.163
+            }
+        },
+        "x":{
+            "second_joint":{
+            "factor":-0.001,
+            "offset":0.1775
+            }
+        },
+        "z":{
+            "third_joint":{
+            "factor":0.001,
+            "offset":0.0
+            },
+            "fourth_joint":{
+            "factor":0.001,
+            "offset":0.0
+            }
+        }
+    }
+}

unilabos/devices/ros_dev/liquid_handler_joint_publisher.py

@@ -1,9 +1,12 @@
+import asyncio
 import copy
+from pathlib import Path
+import threading
 import rclpy
 import json
 import time
 from rclpy.executors import MultiThreadedExecutor
-from rclpy.action import ActionServer
+from rclpy.action import ActionServer,ActionClient
 from sensor_msgs.msg import JointState
 from unilabos_msgs.action import SendCmd
 from rclpy.action.server import ServerGoalHandle
@@ -11,9 +14,11 @@ from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode
 from tf_transformations import quaternion_from_euler
 from tf2_ros import TransformBroadcaster, Buffer, TransformListener 
 
+from rclpy.node import Node
+import re
 
 class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
-    def __init__(self,device_id:str, joint_config:dict, lh_id:str,resource_tracker, rate=50):
+    def __init__(self,resources_config:list, resource_tracker, rate=50, device_id:str = "lh_joint_publisher"):
         super().__init__(
             driver_instance=self,
             device_id=device_id,
@@ -23,60 +28,115 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
             print_publish=False,
             resource_tracker=resource_tracker,  
         )  
-
-        # joint_config_dict = {
-        #     "joint_names":[
-        #         "first_joint",
-        #         "second_joint",
-        #         "third_joint",
-        #         "fourth_joint"
-        #     ],
-        #     "y":{
-        #         "first_joint":{
-        #         "factor":-1,
-        #         "offset":0.0
-        #         }
-        #     },
-        #     "x":{
-        #         "second_joint":{
-        #         "factor":-1,
-        #         "offset":0.0
-        #         }
-        #     },
-        #     "z":{
-        #         "third_joint":{
-        #         "factor":1,
-        #         "offset":0.0
-        #         },
-        #         "fourth_joint":{
-        #         "factor":1,
-        #         "offset":0.0
-        #         }
-        #     }
-        # }
-
+        
+        # 初始化参数
         self.j_msg          = JointState()
-        self.lh_id          = lh_id
-        # self.j_msg.name     = joint_names
-        self.joint_config   = joint_config
-        self.j_msg.position = [0.0 for i in range(len(joint_config['joint_names']))]
-        self.j_msg.name     = [f"{self.lh_id}_{x}" for x in joint_config['joint_names']]
-        # self.joint_config   = joint_config_dict
-        # self.j_msg.position = [0.0 for i in range(len(joint_config_dict['joint_names']))]
-        # self.j_msg.name     = [f"{self.lh_id}_{x}" for x in joint_config_dict['joint_names']]
+        joint_config        = json.load(open(f"{Path(__file__).parent.absolute()}/lh_joint_config.json", encoding="utf-8"))
+        self.resources_config = {x['id']:x for x in resources_config}
         self.rate           = rate
         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.lh_joint_pub_callback)
+        self.create_timer(1,self.lh_joint_pub_callback)
+
+
+        self.resource_action = None
+        
+        while self.resource_action is None:
+            self.resource_action = self.check_tf_update_actions()
+            time.sleep(1)
+        
+        self.resource_action_client = ActionClient(self, SendCmd, self.resource_action)
+        while not self.resource_action_client.wait_for_server(timeout_sec=1.0):
+            self.get_logger().info('等待 TfUpdate 服务器...')
+
+        self.deck_list = []
+        self.lh_devices = {}
+        # 初始化设备ID与config信息
+        for resource in resources_config:
+            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}'
+                self.lh_devices[key] = {
+                    'joint_msg':JointState(
+                        name=[f'{key}_{x}' for x in joint_config[deck_class]['joint_names']],
+                        position=[0.0 for _ in joint_config[deck_class]['joint_names']]
+                        ),
+                    'joint_config':joint_config[deck_class]
+                }
+                self.deck_list.append(deck_id)
+
+
         self.j_action       = ActionServer(
             self,
             SendCmd,
-            "joint",
+            "hl_joint_action",
             self.lh_joint_action_callback,
             result_timeout=5000
         )
 
+                
+    def check_tf_update_actions(self):
+        topics = self.get_topic_names_and_types()
+
+        
+        for topic_item in topics:
+
+            topic_name, topic_types = topic_item
+
+            if 'action_msgs/msg/GoalStatusArray' in topic_types:
+                # 删除 /_action/status 部分
+
+                base_name = topic_name.replace('/_action/status', '')
+                # 检查最后一个部分是否为 tf_update
+                parts = base_name.split('/')
+                if parts and parts[-1] == 'tf_update':
+                    return base_name
+                
+        return None
+    
+
+    def find_resource_parent(self, resource_id:str):
+        # 遍历父辈，找到父辈的父辈，直到找到设备ID
+        parent_id = self.resources_config[resource_id]['parent']
+        try:
+            if parent_id in self.deck_list:
+                p_ = self.resources_config[parent_id]['parent']
+                str_ = f'{p_}_{parent_id}'
+                return str(str_)
+            else:
+                return self.find_resource_parent(parent_id)
+        except Exception as e:
+            return None
+
+
+    def send_resource_action(self, resource_id_list:list[str], link_name:str):
+        goal_msg = SendCmd.Goal()
+        str_dict = {}
+        for resource in resource_id_list:
+            str_dict[resource] = link_name
+
+        goal_msg.command = json.dumps(str_dict)
+        self.resource_action_client.send_goal(goal_msg)
+    
+    def resource_move(self, resource_id:str, link_name:str, channels:list[int]):
+        resource = resource_id.rsplit("_",1)
+        
+        channel_list = ['A','B','C','D','E','F','G','H']
+
+        resource_list = []
+        match = re.match(r'([a-zA-Z_]+)(\d+)', resource[1])
+        if match:
+            number = match.group(2)
+            for channel in channels:
+                resource_list.append(f"{resource[0]}_{channel_list[channel]}{number}")
+
+        if len(resource_list) > 0:
+            self.send_resource_action(resource_list, link_name)
+
+
+
     def lh_joint_action_callback(self,goal_handle: ServerGoalHandle):
         """Move a single joint
 
@@ -101,12 +161,13 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
             goal_handle.succeed()
             
         except Exception as e:
-            print(e)
+            print(f'Liquid handler action error: \n{e}')
             goal_handle.abort()
             result.success = False
-
+        
         return result
     def inverse_kinematics(self, x, y, z, 
+                           parent_id,
                            x_joint:dict, 
                            y_joint:dict, 
                            z_joint:dict   ):
@@ -117,77 +178,97 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
             x (float): x坐标
             y (float): y坐标
             z (float): z坐标
-            x_joint (dict): x轴关节配置，包含plus和offset
-            y_joint (dict): y轴关节配置，包含plus和offset
-            z_joint (dict): z轴关节配置，包含plus和offset
+            x_joint (dict): x轴关节配置，包含factor和offset
+            y_joint (dict): y轴关节配置，包含factor和offset
+            z_joint (dict): z轴关节配置，包含factor和offset
             
         Returns:
             dict: 关节名称和对应位置的字典
         """
-        joint_positions = copy.deepcopy(self.j_msg.position)
+        joint_positions = copy.deepcopy(self.lh_devices[parent_id]['joint_msg'].position)
         
+        z_index = 0
         # 处理x轴关节
         for joint_name, config in x_joint.items():
-            index = self.j_msg.name.index(f"{self.lh_id}_{joint_name}")
+            index = self.lh_devices[parent_id]['joint_msg'].name.index(f"{parent_id}_{joint_name}")
             joint_positions[index] = x * config["factor"] + config["offset"]
             
         # 处理y轴关节
         for joint_name, config in y_joint.items():
-            index = self.j_msg.name.index(f"{self.lh_id}_{joint_name}")
+            index = self.lh_devices[parent_id]['joint_msg'].name.index(f"{parent_id}_{joint_name}")
             joint_positions[index] = y * config["factor"] + config["offset"]
             
         # 处理z轴关节
         for joint_name, config in z_joint.items():
-            index = self.j_msg.name.index(f"{self.lh_id}_{joint_name}")
+            index = self.lh_devices[parent_id]['joint_msg'].name.index(f"{parent_id}_{joint_name}")
             joint_positions[index] = z * config["factor"] + config["offset"]
-            
-        
-        return joint_positions
+            z_index = index
+
+        return joint_positions ,z_index
 
 
-    def move_joints(self, resource_name, link_name, speed, x_joint=None, y_joint=None, z_joint=None):
+    def move_joints(self, resource_names, x, y, z, option, speed = 0.1 ,x_joint=None, y_joint=None, z_joint=None):
+        if isinstance(resource_names, list):
+            resource_name_ = resource_names[0]
+        else:
+            resource_name_ = resource_names
         
-        transform = self.tf_buffer.lookup_transform(
-                    link_name,
-                    resource_name,
-                    rclpy.time.Time()
-                )
-        x,y,z = transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z
+        parent_id = self.find_resource_parent(resource_name_)
+
         if x_joint is None:
-            x_joint_config = next(iter(self.joint_config['x'].items()))
-        elif x_joint in self.joint_config['x']:
-            x_joint_config = self.joint_config['x'][x_joint]
+            xa,xb = next(iter(self.lh_devices[parent_id]['joint_config']['x'].items()))
+            x_joint_config = {xa:xb}
+        elif x_joint in self.lh_devices[parent_id]['joint_config']['x']:
+            x_joint_config = self.lh_devices[parent_id]['joint_config']['x'][x_joint]
         else:
             raise ValueError(f"x_joint {x_joint} not in joint_config['x']")
         if y_joint is None:
-            y_joint_config = next(iter(self.joint_config['y'].items()))
-        elif y_joint in self.joint_config['y']:
-            y_joint_config = self.joint_config['y'][y_joint]
+            ya,yb = next(iter(self.lh_devices[parent_id]['joint_config']['y'].items()))
+            y_joint_config = {ya:yb}
+        elif y_joint in self.lh_devices[parent_id]['joint_config']['y']:
+            y_joint_config = self.lh_devices[parent_id]['joint_config']['y'][y_joint]
         else:
             raise ValueError(f"y_joint {y_joint} not in joint_config['y']")
         if z_joint is None:
-            z_joint_config = next(iter(self.joint_config['z'].items()))
-        elif z_joint in self.joint_config['z']:
-            z_joint_config = self.joint_config['z'][z_joint]
+            za, zb = next(iter(self.lh_devices[parent_id]['joint_config']['z'].items()))
+            z_joint_config = {za :zb}
+        elif z_joint in self.lh_devices[parent_id]['joint_config']['z']:
+            z_joint_config = self.lh_devices[parent_id]['joint_config']['z'][z_joint]
         else:
             raise ValueError(f"z_joint {z_joint} not in joint_config['z']")
-        joint_positions_target = self.inverse_kinematics(x,y,z,x_joint_config,y_joint_config,z_joint_config)
 
+        joint_positions_target, z_index = self.inverse_kinematics(x,y,z,parent_id,x_joint_config,y_joint_config,z_joint_config)
+        joint_positions_target_zero = copy.deepcopy(joint_positions_target)
+        joint_positions_target_zero[z_index] = 0
+
+        self.move_to(joint_positions_target_zero, speed, parent_id)
+        self.move_to(joint_positions_target, speed, parent_id)
+        if option == "pick":
+            link_name =  self.lh_devices[parent_id]['joint_config']['link_names'][z_index]
+            link_name =  f'{parent_id}_{link_name}'
+            self.resource_move(resource_name_, link_name, [0,1,2,3,4,5,6,7])
+        elif option == "drop_trash":
+            self.resource_move(resource_name_, "__trash", [0,1,2,3,4,5,6,7])
+        elif option == "drop":
+            self.resource_move(resource_name_, "world", [0,1,2,3,4,5,6,7])
+        self.move_to(joint_positions_target_zero, speed, parent_id)
+
+
+    def move_to(self, joint_positions ,speed, parent_id):
         loop_flag = 0
 
-
-        while loop_flag < len(self.joint_config['joint_names']):
+        while loop_flag < len(joint_positions):
             loop_flag = 0
-            for i in range(len(self.joint_config['joint_names'])):
-                distance = joint_positions_target[i] - self.j_msg.position[i]
+            for i in range(len(joint_positions)):
+                distance = joint_positions[i] - self.lh_devices[parent_id]['joint_msg'].position[i]
                 if distance == 0:
                     loop_flag += 1
                     continue
                 minus_flag = distance/abs(distance)
                 if abs(distance) > speed/self.rate:
-                    self.j_msg.position[i] += minus_flag * speed/self.rate
+                    self.lh_devices[parent_id]['joint_msg'].position[i] += minus_flag * speed/self.rate
                 else :
-                    self.j_msg.position[i] = joint_positions_target[i]
+                    self.lh_devices[parent_id]['joint_msg'].position[i] = joint_positions[i]
                     loop_flag += 1
                     
 
@@ -195,10 +276,103 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
             self.lh_joint_pub_callback()
             time.sleep(1/self.rate)
 
-
     def lh_joint_pub_callback(self):
-        self.j_msg.header.stamp = self.get_clock().now().to_msg()
-        self.j_pub.publish(self.j_msg)
+        for id, config in self.lh_devices.items():
+            config['joint_msg'].header.stamp = self.get_clock().now().to_msg()
+            self.j_pub.publish(config['joint_msg'])
+
+
+
+
+class JointStatePublisher(Node):
+    def __init__(self):
+        super().__init__('joint_state_publisher')
+
+        self.lh_action = None
+        
+        while self.lh_action is None:
+            self.lh_action = self.check_hl_joint_actions()
+            time.sleep(1)
+        
+        self.lh_action_client = ActionClient(self, SendCmd, self.lh_action)
+        while not self.lh_action_client.wait_for_server(timeout_sec=1.0):
+            self.get_logger().info('等待 TfUpdate 服务器...')
+
+        
+        
+    def check_hl_joint_actions(self):
+        topics = self.get_topic_names_and_types()
+
+        
+        for topic_item in topics:
+
+            topic_name, topic_types = topic_item
+
+            if 'action_msgs/msg/GoalStatusArray' in topic_types:
+                # 删除 /_action/status 部分
+
+                base_name = topic_name.replace('/_action/status', '')
+                # 检查最后一个部分是否为 tf_update
+                parts = base_name.split('/')
+                if parts and parts[-1] == 'hl_joint_action':
+                    return base_name
+                
+        return None
+    
+    def send_resource_action(self, resource_name, x,y,z,option, speed = 0.1,x_joint=None, y_joint=None, z_joint=None):
+        goal_msg = SendCmd.Goal()
+        str_dict = {
+            'resource_names':resource_name,
+            'x':x,
+            'y':y,
+            'z':z,
+            'option':option,
+            'speed':speed,
+            'x_joint':x_joint,
+            'y_joint':y_joint,
+            'z_joint':z_joint
+        }
+        
+
+        goal_msg.command = json.dumps(str_dict)
+
+        if not self.lh_action_client.wait_for_server(timeout_sec=5.0):
+            self.get_logger().error('Action server not available')
+            return None
+        
+        try:
+            # 创建新的executor
+            executor = rclpy.executors.MultiThreadedExecutor()
+            executor.add_node(self)
+            
+            # 发送目标
+            future = self.lh_action_client.send_goal_async(goal_msg)
+            
+            # 使用executor等待结果
+            while not future.done():
+                executor.spin_once(timeout_sec=0.1)
+            
+            handle = future.result()
+            
+            if not handle.accepted:
+                self.get_logger().error('Goal was rejected')
+                return None
+                
+            # 等待最终结果
+            result_future = handle.get_result_async()
+            while not result_future.done():
+                executor.spin_once(timeout_sec=0.1)
+                
+            result = result_future.result()
+            return result
+            
+        except Exception as e:
+            self.get_logger().error(f'Error during action execution: {str(e)}')
+            return None
+        finally:
+            # 清理executor
+            executor.remove_node(self)
+
 
 def main():