添加缺少物料："plate_well_G12",

This reverts commit 07d9db20c3.

.gitignore

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

test/experiments/plr_test_converted.json

@@ -163,7 +163,7 @@
             "type": "plate",
             "class": "opentrons_96_filtertiprack_1000ul",
             "position": {
-                "x": 0,
+                "x": 265.0,
                 "y": 0,
                 "z": 69
             },
@@ -5756,14 +5756,15 @@
                 "plate_well_D12",
                 "plate_well_E12",
                 "plate_well_F12",
-                "plate_well_G12"
+                "plate_well_G12",
+                "plate_well_H12"
             ],
             "parent": "deck",
             "type": "plate",
             "class": "nest_96_wellplate_2ml_deep",
             "position": {
-                "x": 265.0,
+                "x": 0,
-                "y": 0,
+                "y": 90.5,
                 "z": 69
             },
             "config": {
@@ -9584,6 +9585,104 @@
                 "pending_liquids": [],
                 "liquid_history": []
             }
+        },
+        {
+            "id": "plate_well_H12",
+            "name": "plate_well_H12",
+            "sample_id": null,
+            "children": [],
+            "parent": "plate_well",
+            "type": "device",
+            "class": "",
+            "position": {
+                "x": 109.87,
+                "y": 7.77,
+                "z": 3.03
+            },
+            "config": {
+                "type": "Well",
+                "size_x": 6.86,
+                "size_y": 6.86,
+                "size_z": 10.67,
+                "rotation": {
+                    "x": 0,
+                    "y": 0,
+                    "z": 0,
+                    "type": "Rotation"
+                },
+                "category": "well",
+                "model": null,
+                "max_volume": 360,
+                "material_z_thickness": 0.5,
+                "compute_volume_from_height": null,
+                "compute_height_from_volume": null,
+                "bottom_type": "flat",
+                "cross_section_type": "circle"
+            },
+            "data": {
+                "liquids": [],
+                "pending_liquids": [],
+                "liquid_history": []
+            }
+        },
+        {
+            "id": "benyao",
+            "name": "benyao",
+            "children": [
+            ],
+            "parent": null,
+            "type": "device",
+            "class": "moveit.benyao_arm",
+            "position": {
+                "x": -500,
+                "y": 1000,
+                "z": -100
+            },
+            "config": {
+                "moveit_type": "benyao_arm",
+                "joint_poses": {
+                    "arm": {
+                        "hotel_1": [1.05, 0.568, -1.0821, 0.0, 1.0821],
+                        "home": [0.865, 0.09, 0.8727, 0.0, -0.8727]
+                    }
+                },
+                "rotation": {
+                    "x": 0,
+                    "y": 0,
+                    "z": -1.5708,
+                    "type": "Rotation"
+                },
+                "device_config": {
+                }
+            },
+            "data": {
+            }
+        },
+        {
+            "id": "hotel",
+            "name": "hotel",
+            "children": [
+            ],
+            "parent": null,
+            "type": "device",
+            "class": "hotel.thermo_orbitor_rs2_hotel",
+            "position": {
+                "x": 0,
+                "y": -700,
+                "z": -10
+            },
+            "config": {
+                "rotation": {
+                    "x": 0,
+                    "y": 0,
+                    "z": 0,
+                    "type": "Rotation"
+                },
+                "device_config": {
+                }
+            },
+            "data": {
+            }
         }
     ],
     "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/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 = '''
+        self.srdf_str = '''<?xml version="1.0" ?>
-        <?xml version="1.0" encoding="UTF-8"?>
+        <robot xmlns:xacro="http://ros.org/wiki/xacro" name="full_dev">
-        <robot name="minimal">
 
         </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'] != '':
+            if node['type'] in self.resource_type or (node['type'] == 'device' and node['class'] != ''):
-                device_class = node['class']
+                model_config = {}
-                # 检查设备类型是否在注册表中
+                if node['type'] in self.resource_type:
-                if device_class not in registry.device_type_registry.keys():
+                    resource_class = node['class']
-                    raise ValueError(f"设备类型 {device_class} 未在注册表中注册")
+                    if resource_class not in registry.resource_type_registry.keys():
-            elif node['type'] in self.resource_type:
+                        raise ValueError(f"资源类型 {resource_class} 未在注册表中注册")
-                # print(registry.resource_type_registry)
+                    elif "model" in registry.resource_type_registry[resource_class].keys():
-                resource_class = node['class']
+                        model_config = registry.resource_type_registry[resource_class]['model']
-                if resource_class not in registry.resource_type_registry.keys():
+                elif node['type'] == 'device' and node['class'] != '':
-                    raise ValueError(f"资源类型 {resource_class} 未在注册表中注册")
+                    device_class = node['class']
-                elif "model" in registry.resource_type_registry[resource_class].keys():
+                    if device_class not in registry.device_type_registry.keys():
-                    model_config = registry.resource_type_registry[resource_class]['model']
+                        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
+      Splitter Ratio: 0.5016146302223206
-    Tree Height: 345
+    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
+        Show Scene Geometry: false
-        Voxel Coloring: Z-Axis
+        Voxel Coloring: Cell Probability
-        Voxel Rendering: Occupied Voxels
+        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
+        X: -0.2880733013153076
-        Y: 0.21228469908237457
+        Y: -0.16004444658756256
-        Z: 0.20008830726146698
+        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
+  Width: 3956
-  X: 385
+  X: 140
-  Y: 120
+  Y: 54

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
+        joint_config        = json.load(open(f"{Path(__file__).parent.absolute()}/lh_joint_config.json", encoding="utf-8"))
-        # self.j_msg.name     = joint_names
+        self.resources_config = {x['id']:x for x in resources_config}
-        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']]
         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
+            x_joint (dict): x轴关节配置，包含factor和offset
-            y_joint (dict): y轴关节配置，包含plus和offset
+            y_joint (dict): y轴关节配置，包含factor和offset
-            z_joint (dict): z轴关节配置，包含plus和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"]
-            
+            z_index = index
-        
+
-        return joint_positions
+        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(
+        parent_id = self.find_resource_parent(resource_name_)
-                    link_name,
+
-                    resource_name,
-                    rclpy.time.Time()
-                )
-        x,y,z = transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z
         if x_joint is None:
-            x_joint_config = next(iter(self.joint_config['x'].items()))
+            xa,xb = next(iter(self.lh_devices[parent_id]['joint_config']['x'].items()))
-        elif x_joint in self.joint_config['x']:
+            x_joint_config = {xa:xb}
-            x_joint_config = self.joint_config['x'][x_joint]
+        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()))
+            ya,yb = next(iter(self.lh_devices[parent_id]['joint_config']['y'].items()))
-        elif y_joint in self.joint_config['y']:
+            y_joint_config = {ya:yb}
-            y_joint_config = self.joint_config['y'][y_joint]
+        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()))
+            za, zb = next(iter(self.lh_devices[parent_id]['joint_config']['z'].items()))
-        elif z_joint in self.joint_config['z']:
+            z_joint_config = {za :zb}
-            z_joint_config = self.joint_config['z'][z_joint]
+        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(joint_positions):
-        while loop_flag < len(self.joint_config['joint_names']):
             loop_flag = 0
-            for i in range(len(self.joint_config['joint_names'])):
+            for i in range(len(joint_positions)):
-                distance = joint_positions_target[i] - self.j_msg.position[i]
+                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()
+        for id, config in self.lh_devices.items():
-        self.j_pub.publish(self.j_msg)
+            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():
 

unilabos/devices/ros_dev/moveit_interface.py

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

unilabos/registry/devices/hotel.yaml

@@ -0,0 +1,9 @@
+hotel.thermo_orbitor_rs2_hotel:
+  description: Thermo Orbitor RS2 Hotel
+  class: 
+    module: unilabos.devices.resource_container.container:HotelContainer
+    type: python
+  model:
+    type: device
+    mesh: thermo_orbitor_rs2_hotel
+

unilabos/registry/devices/moveit_config.yaml

@@ -0,0 +1,17 @@
+moveit.toyo_xyz:
+  description: Toyo XYZ
+  class:
+    module: unilabos.devices.ros_dev.moveit_interface:MoveitInterface
+    type: ros2
+  model:
+    type: device
+    mesh: toyo_xyz
+
+moveit.benyao_arm:
+  description: Benyao Arm
+  class:
+    module: unilabos.devices.ros_dev.moveit_interface:MoveitInterface
+    type: ros2
+  model:
+    type: device
+    mesh: benyao_arm

unilabos/resources/graphio.py

@@ -9,7 +9,7 @@ try:
     from pylabrobot.resources.resource import Resource as ResourcePLR
 except ImportError:
     pass
-
+from typing import Union, get_origin, get_args
 
 physical_setup_graph: nx.Graph = None
 
@@ -298,7 +298,6 @@ def nested_dict_to_list(nested_dict: dict) -> list[dict]:  # FIXME 是tree？
 
     return result
 
-
 def convert_resources_to_type(
     resources_list: list[dict], resource_type: type, *, plr_model: bool = False
 ) -> Union[list[dict], dict, None, "ResourcePLR"]:
@@ -320,9 +319,15 @@ def convert_resources_to_type(
             return resource_ulab_to_plr(resources_list, plr_model)
         resources_tree = dict_to_tree({r["id"]: r for r in resources_list})
         return resource_ulab_to_plr(resources_tree[0], plr_model)
-    elif isinstance(resource_type, list) and all(issubclass(t, ResourcePLR) for t in resource_type):
+    elif isinstance(resource_type, list) :
-        resources_tree = dict_to_tree({r["id"]: r for r in resources_list})
+        if all((get_origin(t) is Union) for t in resource_type):
-        return [resource_ulab_to_plr(r, plr_model) for r in resources_tree]
+            resources_tree = dict_to_tree({r["id"]: r for r in resources_list})
+            return [resource_ulab_to_plr(r, plr_model) for r in resources_tree]
+        elif all(issubclass(t, ResourcePLR) for t in resource_type):
+            resources_tree = dict_to_tree({r["id"]: r for r in resources_list})
+            return [resource_ulab_to_plr(r, plr_model) for r in resources_tree]
+
+
     else:
         return None
 
@@ -343,9 +348,13 @@ def convert_resources_from_type(resources_list, resource_type: type) -> Union[li
     elif isinstance(resource_type, type) and issubclass(resource_type, ResourcePLR):
         resources_tree = [resource_plr_to_ulab(resources_list)]
         return tree_to_list(resources_tree)
-    elif isinstance(resource_type, list) and all(issubclass(t, ResourcePLR) for t in resource_type):
+    elif isinstance(resource_type, list) :
-        resources_tree = [resource_plr_to_ulab(r) for r in resources_list]
+        if all((get_origin(t) is Union) for t in resource_type):
-        return tree_to_list(resources_tree)
+            resources_tree = [resource_plr_to_ulab(r) for r in resources_list]
+            return tree_to_list(resources_tree)
+        elif all(issubclass(t, ResourcePLR) for t in resource_type):
+            resources_tree = [resource_plr_to_ulab(r) for r in resources_list]
+            return tree_to_list(resources_tree)
     else:
         return None
 

unilabos/ros/main_slave_run.py

@@ -9,6 +9,7 @@ import rclpy
 from unilabos.ros.nodes.presets.joint_republisher import JointRepublisher
 from unilabos.ros.nodes.presets.resource_mesh_manager import ResourceMeshManager
 from unilabos.ros.nodes.resource_tracker import DeviceNodeResourceTracker
+from unilabos.devices.ros_dev.liquid_handler_joint_publisher import LiquidHandlerJointPublisher
 from unilabos_msgs.msg import Resource  # type: ignore
 from unilabos_msgs.srv import ResourceAdd, SerialCommand  # type: ignore
 from rclpy.executors import MultiThreadedExecutor
@@ -72,16 +73,18 @@ def main(
         resource_mesh_manager = ResourceMeshManager(
             resources_mesh_config,
             resources_config,
-            resource_tracker= DeviceNodeResourceTracker(),
+            resource_tracker = host_node.resource_tracker,
             device_id = 'resource_mesh_manager',
         )
         joint_republisher = JointRepublisher(
             'joint_republisher',
-            DeviceNodeResourceTracker()
+            host_node.resource_tracker
         )
 
         executor.add_node(resource_mesh_manager)
         executor.add_node(joint_republisher)
+    lh_joint_pub = LiquidHandlerJointPublisher(resources_config=resources_config, resource_tracker=host_node.resource_tracker)
+    executor.add_node(lh_joint_pub)
 
     thread = threading.Thread(target=executor.spin, daemon=True, name="host_executor_thread")
     thread.start()

unilabos/ros/nodes/presets/resource_mesh_manager.py

@@ -91,8 +91,11 @@ class ResourceMeshManager(BaseROS2DeviceNode):
         self.__collision_object_publisher = self.create_publisher(
             CollisionObject, "/collision_object", 10
         )
+        self.__planning_scene_publisher = self.create_publisher(
+            PlanningScene, "/planning_scene", 10
+        )
         self.__attached_collision_object_publisher = self.create_publisher(
-            AttachedCollisionObject, "/attached_collision_object", 10
+            AttachedCollisionObject, "/attached_collision_object", 0
         )
 
         # 创建一个Action Server用于修改resource_tf_dict
@@ -121,7 +124,8 @@ class ResourceMeshManager(BaseROS2DeviceNode):
         """检查move_group节点是否已初始化完成"""
 
         # 获取当前可用的节点列表
-
+        if len(self.resource_tf_dict) == 0:
+            return
         tf_ready = self.tf_buffer.can_transform("world", next(iter(self.resource_tf_dict.keys())), rclpy.time.Time(),rclpy.duration.Duration(seconds=2))
         
         # if tf_ready:
@@ -129,8 +133,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
             self.move_group_ready = True
             self.publish_resource_tf()
             self.add_resource_collision_meshes(self.resource_tf_dict)
-            
+
-        # time.sleep(1)
        
     def add_resource_mesh_callback(self, goal_handle : ServerGoalHandle):
         tf_update_msg = goal_handle.request
@@ -147,7 +150,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
         """刷新资源配置"""
 
         registry = lab_registry
-        resource_config = json.loads(resource_config_str)
+        resource_config = json.loads(resource_config_str.replace("'",'"'))
         
         if resource_config['id'] in self.resource_config_dict:
             self.get_logger().info(f'资源 {resource_config["id"]} 已存在')
@@ -158,7 +161,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                 self.resource_model[resource_config['id']] = {
                     'mesh': f"{str(self.mesh_path)}/device_mesh/resources/{model_config['mesh']}",
                     'mesh_tf': model_config['mesh_tf']}
-                if model_config['children_mesh'] is not None:
+                if 'children_mesh' in model_config.keys():
                     self.resource_model[f"{resource_config['id']}_"] = {
                         'mesh': f"{str(self.mesh_path)}/device_mesh/resources/{model_config['children_mesh']}",
                         'mesh_tf': model_config['children_mesh_tf']
@@ -187,7 +190,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
 
                 pass
             elif parent is not None and resource_id in self.resource_model:
-                parent_link = f"{self.resource_config_dict[parent]['parent']}_{parent}_device_link".replace("None","")
+                parent_link = f"{self.resource_config_dict[parent]['parent']}_{parent}_device_link".replace("None_","")
 
             else:
 
@@ -297,7 +300,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                     "world",
                     resource_id,
                     rclpy.time.Time(seconds=0),
-                    rclpy.duration.Duration(seconds=5)
+                    # rclpy.duration.Duration(seconds=5)
                 )
                 
                 # 提取当前位姿信息
@@ -344,9 +347,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                 self.resource_pose_publisher.publish(changed_poses_msg)
             self.zero_count += 1
             
-        
+
-        
-    
     def _is_pose_equal(self, pose1, pose2, tolerance=1e-7):
         """
         比较两个位姿是否相等（考虑浮点数精度）
@@ -386,14 +387,23 @@ class ResourceMeshManager(BaseROS2DeviceNode):
             self.__planning_scene = self._get_planning_scene_service.call(
                 GetPlanningScene.Request()
                 ).scene
-            
+            self.__planning_scene.is_diff = True
-            for resource_id, target_parent in cmd_dict.items():
+            planning_scene = PlanningScene()
+            planning_scene.is_diff = True
+            planning_scene.robot_state.is_diff = True
+            time_start = self.get_clock().now()
+            count = 0
 
+            for resource_id, target_parent in cmd_dict.items():
+                parent_id = target_parent
+                if target_parent == '__trash':
+                    parent_id = 'world'
                 # 获取从resource_id到target_parent的转换
                 transform = self.tf_buffer.lookup_transform(
-                    target_parent,
+                    parent_id,
                     resource_id,
-                    rclpy.time.Time(seconds=0)
+                    time_start,
+                    timeout=rclpy.duration.Duration(seconds=10)
                 )
                 
                 # 提取转换中的位置和旋转信息
@@ -411,26 +421,62 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                 }
                 
                 self.resource_tf_dict[resource_id] = {
-                    "parent": target_parent,
+                    "parent": parent_id,
                     "position": position,
                     "rotation": rotation
                 }
                 
+                
                 # self.attach_collision_object(id=resource_id,link_name=target_parent)
-                collision_object = AttachedCollisionObject(
+                # time.sleep(0.02)
+                operation_attach = CollisionObject.ADD
+                operation_world = CollisionObject.REMOVE
+                if target_parent == 'world':
+                    operation_attach = CollisionObject.REMOVE
+                    operation_world = CollisionObject.ADD
+                elif target_parent == '__trash':
+                    operation_attach = CollisionObject.REMOVE
+
+                world_object = CollisionObject(
                     id=resource_id,
-                    link_name=target_parent,
+                    operation=operation_world
+                )
+                if target_parent != '__trash':
+                    planning_scene.world.collision_objects.append(world_object)
+
+
+                collision_object = AttachedCollisionObject(
                     object=CollisionObject(
                         id=resource_id,
-                        operation=CollisionObject.ADD   
+                        operation=operation_attach   
                     )
                 )
-                
+                if target_parent != 'world' and target_parent != '__trash':
-                self.__planning_scene.robot_state.attached_collision_objects.append(collision_object)
+                    collision_object.link_name = target_parent
+                planning_scene.robot_state.attached_collision_objects.append(collision_object)
+
+                count += 1
+
+                if count > 30:
+                    req = ApplyPlanningScene.Request()
+                    req.scene = planning_scene
+                    self.publish_resource_tf()
+                    self._apply_planning_scene_service.call(req)
+                    self.__planning_scene_publisher.publish(planning_scene)
+                    count = 0
+
+                    planning_scene = PlanningScene()
+                    planning_scene.is_diff = True
+                    planning_scene.robot_state.is_diff = True
+
             req = ApplyPlanningScene.Request()
-            req.scene = self.__planning_scene
+            req.scene = planning_scene
-            self._apply_planning_scene_service.call_async(req)
             self.publish_resource_tf()
+            self._apply_planning_scene_service.call(req)
+            self.__planning_scene_publisher.publish(planning_scene)
+
+            # self.__collision_object_publisher.publish(CollisionObject())
+
             
         except Exception as e:
             self.get_logger().error(f"更新资源TF字典失败: {e}")
@@ -440,18 +486,22 @@ class ResourceMeshManager(BaseROS2DeviceNode):
         return SendCmd.Result(success=True)
 
 
+
     def add_resource_collision_meshes(self,resource_tf_dict:dict):
         """
         遍历资源配置字典，为每个在resource_model中有对应模型的资源添加碰撞网格
         
         该方法检查每个资源ID是否在self.resource_model中有对应的3D模型文件路径，
-        如果有，则调用add_collision_mesh方法将其添加到碰撞环境中。
+        
         """
         self.get_logger().info('开始添加资源碰撞网格')
 
         self.__planning_scene = self._get_planning_scene_service.call(
             GetPlanningScene.Request()
         ).scene
+        planning_scene = PlanningScene()
+        planning_scene.is_diff = True
+        count = 0
         for resource_id, tf_info in resource_tf_dict.items():
             
             if resource_id in self.resource_model:
@@ -479,7 +529,8 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                     quat_xyzw=q,
                     frame_id=resource_id
                 )
-                self.__planning_scene.world.collision_objects.append(collision_object)
+                count += 1
+                planning_scene.world.collision_objects.append(collision_object)
             elif f"{tf_info['parent']}_" in self.resource_model:
                 # 获取资源的父级框架ID
                 id_ = f"{tf_info['parent']}_"
@@ -507,14 +558,26 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                     quat_xyzw=q,
                     frame_id=resource_id
                 )
+                count += 1
+                planning_scene.world.collision_objects.append(collision_object)
 
-                self.__planning_scene.world.collision_objects.append(collision_object)
+            if count > 30:
+                req = ApplyPlanningScene.Request()
+                req.scene = planning_scene
+                self.publish_resource_tf()
+                self._apply_planning_scene_service.call(req)
+                self.__planning_scene_publisher.publish(planning_scene)
+                count = 0
+                
+                planning_scene = PlanningScene()
+                planning_scene.is_diff = True
 
         req = ApplyPlanningScene.Request()
-        req.scene = self.__planning_scene
+        req.scene = planning_scene
-        self._apply_planning_scene_service.call_async(req)
+        self.publish_resource_tf()
-            
+        self._apply_planning_scene_service.call(req)
-        
+        self.__planning_scene_publisher.publish(planning_scene)
+
         self.get_logger().info('资源碰撞网格添加完成')
 
 
@@ -959,9 +1022,6 @@ class ResourceMeshManager(BaseROS2DeviceNode):
         Attach collision object to the robot.
         """
 
-        if link_name is None:
-            link_name = self.__end_effector_name
-
         msg = AttachedCollisionObject(
             object=CollisionObject(id=id, operation=CollisionObject.ADD)
         )