Merge remote-tracking branch 'origin/dev' into device_visualization

# Conflicts: # README.md # README_zh.md # recipes/ros-humble-unilabos-msgs/recipe.yaml # recipes/unilabos/recipe.yaml # setup.py # unilabos/devices/liquid_handling/biomek.py # unilabos/devices/liquid_handling/biomek_test.py # unilabos/devices/liquid_handling/complete_biomek_protocol_0608.json # unilabos/registry/devices/liquid_handler.yaml # unilabos/registry/registry.py # unilabos/ros/msgs/message_converter.py # unilabos_msgs/action/LiquidHandlerMoveBiomek.action # unilabos_msgs/action/LiquidHandlerTransferBiomek.action
37-biomek-i5i7 (#40 )
2026-02-04 13:25:13 +00:00 · 2025-06-09 17:04:41 +08:00 · 2025-06-09 16:57:42 +08:00 · 2025-06-09 09:36:08 +08:00 · 2025-06-09 09:36:04 +08:00 · 2025-06-09 09:26:53 +08:00
179 changed files with 37627 additions and 3073 deletions
--- a/.gitignore
+++ b/.gitignore
@@ -6,6 +6,7 @@ __pycache__/
 .vscode
 *.py[cod]
 *$py.class
+service

 # C extensions
 *.so
@@ -231,4 +232,5 @@ CATKIN_IGNORE

 /**/local_config.py

-*.graphml
+*.graphml
+unilabos/device_mesh/view_robot.rviz
--- a/README.md
+++ b/README.md
@@ -4,83 +4,90 @@

 # Uni-Lab-OS

+<!-- Language switcher -->
+**English** | [中文](README_zh.md)
+
 [![GitHub Stars](https://img.shields.io/github/stars/dptech-corp/Uni-Lab-OS.svg)](https://github.com/dptech-corp/Uni-Lab-OS/stargazers)
 [![GitHub Forks](https://img.shields.io/github/forks/dptech-corp/Uni-Lab-OS.svg)](https://github.com/dptech-corp/Uni-Lab-OS/network/members)
 [![GitHub Issues](https://img.shields.io/github/issues/dptech-corp/Uni-Lab-OS.svg)](https://github.com/dptech-corp/Uni-Lab-OS/issues)
 [![GitHub License](https://img.shields.io/github/license/dptech-corp/Uni-Lab-OS.svg)](https://github.com/dptech-corp/Uni-Lab-OS/blob/main/LICENSE)

-Uni-Lab 操作系统是一个用于实验室自动化的综合平台，旨在连接和控制各种实验设备，实现实验流程的自动化和标准化。
+Uni-Lab-OS is a platform for laboratory automation, designed to connect and control various experimental equipment, enabling automation and standardization of experimental workflows.

-## 核心特点
+## 🏆 Competition

- 多设备集成管理
- 自动化实验流程
- 云端连接能力
- 灵活的配置系统
- 支持多种实验协议
+Join the [Intelligent Organic Chemistry Synthesis Competition](https://bohrium.dp.tech/competitions/1451645258) to explore automated synthesis with Uni-Lab-OS!

-## 文档
+## Key Features

-详细文档可在以下位置找到:
+- Multi-device integration management
+- Automated experimental workflows
+- Cloud connectivity capabilities
+- Flexible configuration system
+- Support for multiple experimental protocols

- [在线文档](https://readthedocs.dp.tech/Uni-Lab/v0.8.0/)
+## Documentation

-## 快速开始
+Detailed documentation can be found at:

-1. 配置Conda环境
+- [Online Documentation](https://readthedocs.dp.tech/Uni-Lab/v0.8.0/)

-Uni-Lab-OS 建议使用 `mamba` 管理环境。根据您的操作系统选择适当的环境文件:
+## Quick Start
+
+1. Configure Conda Environment
+
+Uni-Lab-OS recommends using `mamba` for environment management. Choose the appropriate environment file for your operating system:

 ```bash
-# 创建新环境
+# Create new environment
 mamba env create -f unilabos-[YOUR_OS].yaml
 mamba activate unilab

-# 或更新现有环境
-# 其中 `[YOUR_OS]` 可以是 `win64`, `linux-64`, `osx-64`, 或 `osx-arm64`。
-conda env update --file unilabos-[YOUR_OS].yml -n 环境名
+# Or update existing environment
+# Where `[YOUR_OS]` can be `win64`, `linux-64`, `osx-64`, or `osx-arm64`.
+conda env update --file unilabos-[YOUR_OS].yml -n environment_name

-# 现阶段，需要安装 `unilabos_msgs` 包
-# 可以前往 Release 页面下载系统对应的包进行安装
-conda install ros-humble-unilabos-msgs-0.9.0-xxxxx.tar.bz2
+# Currently, you need to install the `unilabos_msgs` package
+# You can download the system-specific package from the Release page
+conda install ros-humble-unilabos-msgs-0.9.3-xxxxx.tar.bz2

-# 安装PyLabRobot等前置
+# Install PyLabRobot and other prerequisites
 git clone https://github.com/PyLabRobot/pylabrobot plr_repo
 cd plr_repo
 pip install .[opentrons]
 ```

-2. 安装 Uni-Lab-OS:
+2. Install Uni-Lab-OS:

 ```bash
-# 克隆仓库
+# Clone the repository
 git clone https://github.com/dptech-corp/Uni-Lab-OS.git
 cd Uni-Lab-OS

-# 安装 Uni-Lab-OS
+# Install Uni-Lab-OS
 pip install .
 ```

-3. 启动 Uni-Lab 系统:
+3. Start Uni-Lab System:

-请见[文档-启动样例](https://readthedocs.dp.tech/Uni-Lab/v0.8.0/boot_examples/index.html)
+Please refer to [Documentation - Boot Examples](https://readthedocs.dp.tech/Uni-Lab/v0.8.0/boot_examples/index.html)

-## 消息格式
+## Message Format

-Uni-Lab-OS 使用预构建的 `unilabos_msgs` 进行系统通信。您可以在 [GitHub Releases](https://github.com/dptech-corp/Uni-Lab-OS/releases) 页面找到已构建的版本。
+Uni-Lab-OS uses pre-built `unilabos_msgs` for system communication. You can find the built versions on the [GitHub Releases](https://github.com/dptech-corp/Uni-Lab-OS/releases) page.

-## 许可证
+## License

-此项目采用 GPL-3.0 许可 - 详情请参阅 [LICENSE](LICENSE) 文件。
+This project is licensed under GPL-3.0 - see the [LICENSE](LICENSE) file for details.

-## 项目统计
+## Project Statistics

-### Stars 趋势
+### Stars Trend

 <a href="https://star-history.com/#dptech-corp/Uni-Lab-OS&Date">
  <img src="https://api.star-history.com/svg?repos=dptech-corp/Uni-Lab-OS&type=Date" alt="Star History Chart" width="600">
 </a>

-## 联系我们
+## Contact Us

 - GitHub Issues: [https://github.com/dptech-corp/Uni-Lab-OS/issues](https://github.com/dptech-corp/Uni-Lab-OS/issues)
--- a/README_zh.md
+++ b/README_zh.md
@@ -0,0 +1,93 @@
+<div align="center">
+  <img src="docs/logo.png" alt="Uni-Lab Logo" width="200"/>
+</div>
+
+# Uni-Lab-OS
+
+<!-- Language switcher -->
+[English](README.md) | **中文**
+
+[![GitHub Stars](https://img.shields.io/github/stars/dptech-corp/Uni-Lab-OS.svg)](https://github.com/dptech-corp/Uni-Lab-OS/stargazers)
+[![GitHub Forks](https://img.shields.io/github/forks/dptech-corp/Uni-Lab-OS.svg)](https://github.com/dptech-corp/Uni-Lab-OS/network/members)
+[![GitHub Issues](https://img.shields.io/github/issues/dptech-corp/Uni-Lab-OS.svg)](https://github.com/dptech-corp/Uni-Lab-OS/issues)
+[![GitHub License](https://img.shields.io/github/license/dptech-corp/Uni-Lab-OS.svg)](https://github.com/dptech-corp/Uni-Lab-OS/blob/main/LICENSE)
+
+Uni-Lab-OS是一个用于实验室自动化的综合平台，旨在连接和控制各种实验设备，实现实验流程的自动化和标准化。
+
+## 🏆 比赛
+
+欢迎参加[有机化学合成智能实验大赛](https://bohrium.dp.tech/competitions/1451645258)，使用 Uni-Lab-OS 探索自动化合成！
+
+## 核心特点
+
+- 多设备集成管理
+- 自动化实验流程
+- 云端连接能力
+- 灵活的配置系统
+- 支持多种实验协议
+
+## 文档
+
+详细文档可在以下位置找到:
+
+- [在线文档](https://readthedocs.dp.tech/Uni-Lab/v0.8.0/)
+
+## 快速开始
+
+1. 配置Conda环境
+
+Uni-Lab-OS 建议使用 `mamba` 管理环境。根据您的操作系统选择适当的环境文件:
+
+```bash
+# 创建新环境
+mamba env create -f unilabos-[YOUR_OS].yaml
+mamba activate unilab
+
+# 或更新现有环境
+# 其中 `[YOUR_OS]` 可以是 `win64`, `linux-64`, `osx-64`, 或 `osx-arm64`。
+conda env update --file unilabos-[YOUR_OS].yml -n 环境名
+
+# 现阶段，需要安装 `unilabos_msgs` 包
+# 可以前往 Release 页面下载系统对应的包进行安装
+conda install ros-humble-unilabos-msgs-0.9.3-xxxxx.tar.bz2
+
+# 安装PyLabRobot等前置
+git clone https://github.com/PyLabRobot/pylabrobot plr_repo
+cd plr_repo
+pip install .[opentrons]
+```
+
+2. 安装 Uni-Lab-OS:
+
+```bash
+# 克隆仓库
+git clone https://github.com/dptech-corp/Uni-Lab-OS.git
+cd Uni-Lab-OS
+
+# 安装 Uni-Lab-OS
+pip install .
+```
+
+3. 启动 Uni-Lab 系统:
+
+请见[文档-启动样例](https://readthedocs.dp.tech/Uni-Lab/v0.8.0/boot_examples/index.html)
+
+## 消息格式
+
+Uni-Lab-OS 使用预构建的 `unilabos_msgs` 进行系统通信。您可以在 [GitHub Releases](https://github.com/dptech-corp/Uni-Lab-OS/releases) 页面找到已构建的版本。
+
+## 许可证
+
+此项目采用 GPL-3.0 许可 - 详情请参阅 [LICENSE](LICENSE) 文件。
+
+## 项目统计
+
+### Stars 趋势
+
+<a href="https://star-history.com/#dptech-corp/Uni-Lab-OS&Date">
+  <img src="https://api.star-history.com/svg?repos=dptech-corp/Uni-Lab-OS&type=Date" alt="Star History Chart" width="600">
+</a>
+
+## 联系我们
+
+- GitHub Issues: [https://github.com/dptech-corp/Uni-Lab-OS/issues](https://github.com/dptech-corp/Uni-Lab-OS/issues) 
--- a/recipes/ros-humble-unilabos-msgs/recipe.yaml
+++ b/recipes/ros-humble-unilabos-msgs/recipe.yaml
@@ -1,6 +1,6 @@
 package:
  name: ros-humble-unilabos-msgs
-  version: 0.9.0
+  version: 0.9.3
 source:
  path: ../../unilabos_msgs
  folder: ros-humble-unilabos-msgs/src/work
--- a/recipes/unilabos/recipe.yaml
+++ b/recipes/unilabos/recipe.yaml
@@ -1,6 +1,6 @@
 package:
  name: unilabos
-  version: "0.9.0"
+  version: "0.9.3"

 source:
  path: ../..
--- a/setup.cfg
+++ b/setup.cfg
@@ -1,4 +1,2 @@
-[develop]
-script_dir=$base/lib/unilabos
 [install]
 install_scripts=$base/lib/unilabos
--- a/setup.py
+++ b/setup.py
@@ -4,7 +4,7 @@ package_name = 'unilabos'

 setup(
    name=package_name,
-    version='0.9.0',
+    version='0.9.3',
    packages=find_packages(),
    include_package_data=True,
    install_requires=['setuptools'],
--- a/test/commands/resource_add.md
+++ b/test/commands/resource_add.md
@@ -1,5 +1,5 @@
 使用plr_test.json启动，将Well加入Plate中

 ```bash
-ros2 action send_goal /devices/host_node/add_resource_from_outer unilabos_msgs/action/_resource_create_from_outer/ResourceCreateFromOuter "{ resources: [ { 'category': '', 'children': [], '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': 'plate_well_11_7', 'name': 'plate_well_11_7', 'pose': { 'orientation': { 'w': 1.0, 'x': 0.0, 'y': 0.0, 'z': 0.0 }, 'position': { 'x': 0.0, 'y': 0.0, 'z': 0.0 } }, 'sample_id': '', 'parent': 'plate', 'type': 'device' } ], device_ids: [ 'PLR_STATION' ], bind_parent_ids: [ 'plate' ], bind_locations: [ { 'x': 0.0, 'y': 0.0, 'z': 0.0 } ], other_calling_params: [ '{}' ] }"
+ros2 action send_goal /devices/host_node/create_resource_detailed unilabos_msgs/action/_resource_create_from_outer/ResourceCreateFromOuter "{ resources: [ { 'category': '', 'children': [], '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': 'plate_well_11_7', 'name': 'plate_well_11_7', 'pose': { 'orientation': { 'w': 1.0, 'x': 0.0, 'y': 0.0, 'z': 0.0 }, 'position': { 'x': 0.0, 'y': 0.0, 'z': 0.0 } }, 'sample_id': '', 'parent': 'plate', 'type': 'device' } ], device_ids: [ 'PLR_STATION' ], bind_parent_ids: [ 'plate' ], bind_locations: [ { 'x': 0.0, 'y': 0.0, 'z': 0.0 } ], other_calling_params: [ '{}' ] }"
 ```
--- a/test/experiments/biomek.json
+++ b/test/experiments/biomek.json
@@ -0,0 +1,22 @@
+{
+    "nodes": [
+        {
+            "id": "BIOMEK",
+            "name": "BIOMEK",
+            "parent": null,
+            "type": "device",
+            "class": "liquid_handler.biomek",
+            "position": {
+                "x": 620.6111111111111,
+                "y": 171,
+                "z": 0
+            },
+            "config": {
+            },
+            "data": {},
+            "children": [
+            ]
+        }
+    ],
+    "links": []
+}
--- a/test/experiments/plr_test_converted.json
+++ b/test/experiments/plr_test_converted.json
--- a/test/experiments/plr_test_converted_slim.json
+++ b/test/experiments/plr_test_converted_slim.json
--- a/test/experiments/test_moveit.json
+++ b/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": [
+        
+    ]
+}
--- a/unilabos-linux-64.yaml
+++ b/unilabos-linux-64.yaml
@@ -56,6 +56,8 @@ dependencies:
  - ros-humble-moveit-servo
  # simulation
  - ros-humble-simulation
+  - ros-humble-tf-transformations
+  - transforms3d
  # ros-humble-gazebo-ros // ignored because of the conflict with ign-gazebo
  # ilab equipments
 #  - ros-humble-unilabos-msgs
--- a/unilabos-osx-64.yaml
+++ b/unilabos-osx-64.yaml
@@ -56,6 +56,8 @@ dependencies:
  # - ros-humble-moveit-servo
  # simulation
  - ros-humble-simulation
+  - ros-humble-tf-transformations
+  - transforms3d
  # ros-humble-gazebo-ros // ignored because of the conflict with ign-gazebo
  # ilab equipments
 #  - ros-humble-unilabos-msgs
--- a/unilabos-osx-arm64.yaml
+++ b/unilabos-osx-arm64.yaml
@@ -58,6 +58,8 @@ dependencies:
  - ros-humble-moveit-servo
  # simulation
  - ros-humble-simulation
+  - ros-humble-tf-transformations
+  - transforms3d
  # ros-humble-gazebo-ros // ignored because of the conflict with ign-gazebo
  # ilab equipments
 #  - ros-humble-unilabos-msgs
--- a/unilabos-win64.yaml
+++ b/unilabos-win64.yaml
@@ -56,6 +56,8 @@ dependencies:
  - ros-humble-moveit-servo
  # simulation
  - ros-humble-simulation # ignored because of NO python3.11 package in WIN64
+  - ros-humble-tf-transformations
+  - transforms3d
  # ros-humble-gazebo-ros // ignored because of the conflict with ign-gazebo
  # ilab equipments
  # ros-humble-unilabos-msgs
--- a/unilabos/app/controler.py
+++ b/unilabos/app/controler.py
@@ -31,6 +31,6 @@ def job_add(req: JobAddReq) -> JobData:
        action_kwargs = {"command": json.dumps(action_kwargs)}
    elif "command" in action_kwargs:
        action_kwargs = action_kwargs["command"]
-    print(f"job_add:{req.device_id} {action_name} {action_kwargs}")
-    HostNode.get_instance().send_goal(req.device_id, action_name=action_name, action_kwargs=action_kwargs, goal_uuid=req.job_id)
+    # print(f"job_add:{req.device_id} {action_name} {action_kwargs}")
+    HostNode.get_instance().send_goal(req.device_id, action_name=action_name, action_kwargs=action_kwargs, goal_uuid=req.job_id, server_info=req.server_info)
    return JobData(jobId=req.job_id)
--- a/unilabos/app/main.py
+++ b/unilabos/app/main.py
@@ -10,6 +10,8 @@ from copy import deepcopy

 import yaml

+from unilabos.resources.graphio import tree_to_list
+
 # 首先添加项目根目录到路径
 current_dir = os.path.dirname(os.path.abspath(__file__))
 unilabos_dir = os.path.dirname(os.path.dirname(current_dir))
@@ -18,7 +20,6 @@ if unilabos_dir not in sys.path:

 from unilabos.config.config import load_config, BasicConfig, _update_config_from_env
 from unilabos.utils.banner_print import print_status, print_unilab_banner
-from unilabos.device_mesh.resource_visalization import ResourceVisualization


 def parse_args():
@@ -145,19 +146,19 @@ def main():
            else read_graphml(args_dict["graph"])
        )
        devices_and_resources = dict_from_graph(graph_res.physical_setup_graph)
-        args_dict["resources_config"] = initialize_resources(list(deepcopy(devices_and_resources).values()))
+        # args_dict["resources_config"] = initialize_resources(list(deepcopy(devices_and_resources).values()))
+        args_dict["resources_config"] = list(devices_and_resources.values())
        args_dict["devices_config"] = dict_to_nested_dict(deepcopy(devices_and_resources), devices_only=False)
-        # args_dict["resources_config"] = dict_to_tree(devices_and_resources, devices_only=False)
-
        args_dict["graph"] = graph_res.physical_setup_graph
    else:
        if args_dict["devices"] is None or args_dict["resources"] is None:
            print_status("Either graph or devices and resources must be provided.", "error")
            sys.exit(1)
        args_dict["devices_config"] = json.load(open(args_dict["devices"], encoding="utf-8"))
-        args_dict["resources_config"] = initialize_resources(
-            list(json.load(open(args_dict["resources"], encoding="utf-8")).values())
-        )
+        # args_dict["resources_config"] = initialize_resources(
+        #     list(json.load(open(args_dict["resources"], encoding="utf-8")).values())
+        # )
+        args_dict["resources_config"] = list(json.load(open(args_dict["resources"], encoding="utf-8")).values())

    print_status(f"{len(args_dict['resources_config'])} Resources loaded:", "info")
    for i in args_dict["resources_config"]:
@@ -188,11 +189,12 @@ def main():
    if args_dict["visual"] != "disable":
        enable_rviz = args_dict["visual"] == "rviz"
        if devices_and_resources is not None:
+            from unilabos.device_mesh.resource_visalization import ResourceVisualization  # 此处开启后，logger会变更为INFO，有需要请调整
            resource_visualization = ResourceVisualization(devices_and_resources, args_dict["resources_config"] ,enable_rviz=enable_rviz)
            args_dict["resources_mesh_config"] = resource_visualization.resource_model
            start_backend(**args_dict)
            server_thread = threading.Thread(target=start_server, kwargs=dict(
-                open_browser=not args_dict["disable_browser"]
+                open_browser=not args_dict["disable_browser"], port=args_dict["port"],
            ))
            server_thread.start()
            asyncio.set_event_loop(asyncio.new_event_loop())
@@ -201,10 +203,10 @@ def main():
                time.sleep(1)
        else:
            start_backend(**args_dict)
-            start_server(open_browser=not args_dict["disable_browser"])
+            start_server(open_browser=not args_dict["disable_browser"], port=args_dict["port"],)
    else:
        start_backend(**args_dict)
-        start_server(open_browser=not args_dict["disable_browser"])
+        start_server(open_browser=not args_dict["disable_browser"], port=args_dict["port"],)


 if __name__ == "__main__":
--- a/unilabos/app/model.py
+++ b/unilabos/app/model.py
@@ -51,8 +51,9 @@ class Resp(BaseModel):
 class JobAddReq(BaseModel):
    device_id: str = Field(examples=["Gripper"], description="device id")
    data: dict = Field(examples=[{"position": 30, "torque": 5, "action": "push_to"}])
-    job_id: str = Field(examples=["sfsfsfeq"], description="goal uuid")
-    node_id: str = Field(examples=["sfsfsfeq"], description="node uuid")
+    job_id: str = Field(examples=["job_id"], description="goal uuid")
+    node_id: str = Field(examples=["node_id"], description="node uuid")
+    server_info: dict = Field(examples=[{"send_timestamp": 1717000000.0}], description="server info")


 class JobStepFinishReq(BaseModel):
--- a/unilabos/app/mq.py
+++ b/unilabos/app/mq.py
@@ -1,6 +1,7 @@
 import json
 import time
 import traceback
+from typing import Optional
 import uuid

 import paho.mqtt.client as mqtt
@@ -12,7 +13,7 @@ import tempfile
 import os

 from unilabos.config.config import MQConfig
-from unilabos.app.controler import devices, job_add
+from unilabos.app.controler import job_add
 from unilabos.app.model import JobAddReq
 from unilabos.utils import logger
 from unilabos.utils.type_check import TypeEncoder
@@ -26,6 +27,7 @@ class MQTTClient:
    def __init__(self):
        self.mqtt_disable = not MQConfig.lab_id
        self.client_id = f"{MQConfig.group_id}@@@{MQConfig.lab_id}{uuid.uuid4()}"
+        logger.info("[MQTT] Client_id: " + self.client_id)
        self.client = mqtt.Client(CallbackAPIVersion.VERSION2, client_id=self.client_id, protocol=mqtt.MQTTv5)
        self._setup_callbacks()

@@ -42,20 +44,14 @@ class MQTTClient:
    def _on_connect(self, client, userdata, flags, rc, properties=None):
        logger.info("[MQTT] Connected with result code " + str(rc))
        client.subscribe(f"labs/{MQConfig.lab_id}/job/start/", 0)
-        isok, data = devices()
-        if not isok:
-            logger.error("[MQTT] on_connect ErrorHostNotInit")
-            return
+        client.subscribe(f"labs/{MQConfig.lab_id}/pong/", 0)

    def _on_message(self, client, userdata, msg) -> None:
-        logger.info("[MQTT] on_message<<<< " + msg.topic + " " + str(msg.payload))
+        # logger.info("[MQTT] on_message<<<< " + msg.topic + " " + str(msg.payload))
        try:
            payload_str = msg.payload.decode("utf-8")
            payload_json = json.loads(payload_str)
-            logger.debug(f"Topic: {msg.topic}")
-            logger.debug("Payload:", json.dumps(payload_json, indent=2, ensure_ascii=False))
            if msg.topic == f"labs/{MQConfig.lab_id}/job/start/":
-                logger.debug("job_add", type(payload_json), payload_json)
                if "data" not in payload_json:
                    payload_json["data"] = {}
                if "action" in payload_json:
@@ -65,6 +61,14 @@ class MQTTClient:
                job_req = JobAddReq.model_validate(payload_json)
                data = job_add(job_req)
                return
+            elif msg.topic == f"labs/{MQConfig.lab_id}/pong/":
+                # 处理pong响应，通知HostNode
+                from unilabos.ros.nodes.presets.host_node import HostNode
+
+                host_instance = HostNode.get_instance(0)
+                if host_instance:
+                    host_instance.handle_pong_response(payload_json)
+                return

        except json.JSONDecodeError as e:
            logger.error(f"[MQTT] JSON 解析错误: {e}")
@@ -158,12 +162,14 @@ class MQTTClient:
        status = {"data": device_status.get(device_id, {}), "device_id": device_id}
        address = f"labs/{MQConfig.lab_id}/devices/"
        self.client.publish(address, json.dumps(status), qos=2)
-        logger.critical(f"Device status published: address: {address}, {status}")
+        logger.debug(f"Device status published: address: {address}, {status}")

-    def publish_job_status(self, feedback_data: dict, job_id: str, status: str):
+    def publish_job_status(self, feedback_data: dict, job_id: str, status: str, return_info: Optional[str] = None):
        if self.mqtt_disable:
            return
-        jobdata = {"job_id": job_id, "data": feedback_data, "status": status}
+        if return_info is None:
+            return_info = "{}"
+        jobdata = {"job_id": job_id, "data": feedback_data, "status": status, "return_info": return_info}
        self.client.publish(f"labs/{MQConfig.lab_id}/job/list/", json.dumps(jobdata), qos=2)

    def publish_registry(self, device_id: str, device_info: dict):
@@ -181,6 +187,28 @@ class MQTTClient:
        self.client.publish(address, json.dumps(action_info), qos=2)
        logger.debug(f"Action data published: address: {address}, {action_id}, {action_info}")

+    def send_ping(self, ping_id: str, timestamp: float):
+        """发送ping消息到服务端"""
+        if self.mqtt_disable:
+            return
+        address = f"labs/{MQConfig.lab_id}/ping/"
+        ping_data = {"ping_id": ping_id, "client_timestamp": timestamp, "type": "ping"}
+        self.client.publish(address, json.dumps(ping_data), qos=2)
+
+    def setup_pong_subscription(self):
+        """设置pong消息订阅"""
+        if self.mqtt_disable:
+            return
+        pong_topic = f"labs/{MQConfig.lab_id}/pong/"
+        self.client.subscribe(pong_topic, 0)
+        logger.debug(f"Subscribed to pong topic: {pong_topic}")
+
+    def handle_pong(self, pong_data: dict):
+        """处理pong响应（这个方法会在收到pong消息时被调用）"""
+        logger.debug(f"Pong received: {pong_data}")
+        # 这里会被HostNode的ping-pong处理逻辑调用
+        pass
+

 mqtt_client = MQTTClient()

--- a/unilabos/app/web/client.py
+++ b/unilabos/app/web/client.py
@@ -30,18 +30,18 @@ class HTTPClient:
            self.auth = MQConfig.lab_id
        info(f"HTTPClient 初始化完成: remote_addr={self.remote_addr}")

-    def resource_add(self, resources: List[Dict[str, Any]]) -> requests.Response:
+    def resource_add(self, resources: List[Dict[str, Any]], database_process_later:bool) -> requests.Response:
        """
        添加资源

        Args:
            resources: 要添加的资源列表
-
+            database_process_later: 后台处理资源
        Returns:
            Response: API响应对象
        """
        response = requests.post(
-            f"{self.remote_addr}/lab/resource/",
+            f"{self.remote_addr}/lab/resource/?database_process_later={1 if database_process_later else 0}",
            json=resources,
            headers={"Authorization": f"lab {self.auth}"},
            timeout=5,
@@ -60,7 +60,7 @@ class HTTPClient:
            Dict: 返回的资源数据
        """
        response = requests.get(
-            f"{self.remote_addr}/lab/resource/",
+            f"{self.remote_addr}/lab/resource/?edge_format=1",
            params={"id": id, "with_children": with_children},
            headers={"Authorization": f"lab {self.auth}"},
            timeout=5,
@@ -96,7 +96,7 @@ class HTTPClient:
            Response: API响应对象
        """
        response = requests.patch(
-            f"{self.remote_addr}/lab/resource/batch_update/",
+            f"{self.remote_addr}/lab/resource/batch_update/?edge_format=1",
            json=resources,
            headers={"Authorization": f"lab {self.auth}"},
            timeout=5,
--- a/unilabos/app/web/utils/host_utils.py
+++ b/unilabos/app/web/utils/host_utils.py
@@ -42,7 +42,7 @@ def get_host_node_info() -> Dict[str, Any]:
    host_info["subscribed_topics"] = sorted(list(host_node._subscribed_topics))
    # 获取动作客户端信息
    for action_id, client in host_node._action_clients.items():
-        host_info["action_clients"] = {action_id: get_action_info(client, full_name=action_id)}
+        host_info["action_clients"][action_id] = get_action_info(client, full_name=action_id)

    # 获取设备状态
    host_info["device_status"] = host_node.device_status
--- a/unilabos/device_mesh/devices/benyao_arm/config/initial_positions.yaml
+++ b/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
--- a/unilabos/device_mesh/devices/benyao_arm/config/joint_limits.yaml
+++ b/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
--- a/unilabos/device_mesh/devices/benyao_arm/config/kinematics.yaml
+++ b/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
--- a/unilabos/device_mesh/devices/benyao_arm/config/macro.ros2_control.xacro
+++ b/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>
--- a/unilabos/device_mesh/devices/benyao_arm/config/macro.srdf.xacro
+++ b/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>
--- a/unilabos/device_mesh/devices/benyao_arm/config/move_group.json
+++ b/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"
+    }
+}
--- a/unilabos/device_mesh/devices/benyao_arm/config/moveit_controllers.yaml
+++ b/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
--- a/unilabos/device_mesh/devices/benyao_arm/config/moveit_planners.yaml
+++ b/unilabos/device_mesh/devices/benyao_arm/config/moveit_planners.yaml
@@ -0,0 +1,2 @@
+planner_configs:
+  - ompl_interface/OMPLPlanner
--- a/unilabos/device_mesh/devices/benyao_arm/config/pilz_cartesian_limits.yaml
+++ b/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
--- a/unilabos/device_mesh/devices/benyao_arm/config/ros2_controllers.yaml
+++ b/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
--- a/unilabos/device_mesh/devices/benyao_arm/joint_limit.yaml
+++ b/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
--- a/unilabos/device_mesh/devices/benyao_arm/macro_device.xacro
+++ b/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>
--- a/unilabos/device_mesh/devices/benyao_arm/meshes/arm_base.STL
+++ b/unilabos/device_mesh/devices/benyao_arm/meshes/arm_base.STL
--- a/unilabos/device_mesh/devices/benyao_arm/meshes/arm_link_1.STL
+++ b/unilabos/device_mesh/devices/benyao_arm/meshes/arm_link_1.STL
--- a/unilabos/device_mesh/devices/benyao_arm/meshes/arm_link_2.STL
+++ b/unilabos/device_mesh/devices/benyao_arm/meshes/arm_link_2.STL
--- a/unilabos/device_mesh/devices/benyao_arm/meshes/arm_link_3.STL
+++ b/unilabos/device_mesh/devices/benyao_arm/meshes/arm_link_3.STL
--- a/unilabos/device_mesh/devices/benyao_arm/meshes/arm_slideway.STL
+++ b/unilabos/device_mesh/devices/benyao_arm/meshes/arm_slideway.STL
--- a/unilabos/device_mesh/devices/benyao_arm/meshes/gripper_base.STL
+++ b/unilabos/device_mesh/devices/benyao_arm/meshes/gripper_base.STL
--- a/unilabos/device_mesh/devices/benyao_arm/meshes/gripper_left.STL
+++ b/unilabos/device_mesh/devices/benyao_arm/meshes/gripper_left.STL
--- a/unilabos/device_mesh/devices/benyao_arm/meshes/gripper_right.STL
+++ b/unilabos/device_mesh/devices/benyao_arm/meshes/gripper_right.STL
--- a/unilabos/device_mesh/devices/opentrons_liquid_handler/macro_device.xacro
+++ b/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"/>
--- a/unilabos/device_mesh/devices/slide_w140/macro_device.xacro
+++ b/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"/>
--- a/unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/hotel.png
+++ b/unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/hotel.png
--- a/unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/macro_device.xacro
+++ b/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>
--- a/unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/meshes/hotel.glb
+++ b/unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/meshes/hotel.glb
--- a/unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/meshes/hotel.stl
+++ b/unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/meshes/hotel.stl
--- a/unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/meta.json
+++ b/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"
+    ]
+}
--- a/unilabos/device_mesh/devices/toyo_xyz/config/full_dev.urdf.xacro
+++ b/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>
--- a/unilabos/device_mesh/devices/toyo_xyz/config/initial_positions.yaml
+++ b/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
--- a/unilabos/device_mesh/devices/toyo_xyz/config/joint_limits.yaml
+++ b/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
--- a/unilabos/device_mesh/devices/toyo_xyz/config/kinematics.yaml
+++ b/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
--- a/unilabos/device_mesh/devices/toyo_xyz/config/macro.ros2_control.xacro
+++ b/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>
--- a/unilabos/device_mesh/devices/toyo_xyz/config/macro.srdf.xacro
+++ b/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>
--- a/unilabos/device_mesh/devices/toyo_xyz/config/move_group.json
+++ b/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"
+    }
+}
--- a/unilabos/device_mesh/devices/toyo_xyz/config/moveit_controllers.yaml
+++ b/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
--- a/unilabos/device_mesh/devices/toyo_xyz/config/pilz_cartesian_limits.yaml
+++ b/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
--- a/unilabos/device_mesh/devices/toyo_xyz/config/ros2_controllers.yaml
+++ b/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
--- a/unilabos/device_mesh/devices/toyo_xyz/joint_config.json
+++ b/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"
+    }
+}
--- a/unilabos/device_mesh/devices/toyo_xyz/macro_device.xacro
+++ b/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>
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/base2_link.STL
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/base2_link.STL
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/base2_link.fbx
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/base2_link.fbx
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/base3_link.STL
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/base3_link.STL
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/base3_link.fbx
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/base3_link.fbx
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/base_link.STL
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/base_link.STL
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/base_link.fbx
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/base_link.fbx
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/chain_link.STL
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/chain_link.STL
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/chain_link.fbx
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/chain_link.fbx
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/end2_link.STL
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/end2_link.STL
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/end2_link.fbx
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/end2_link.fbx
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/end3_link.STL
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/end3_link.STL
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/end3_link.fbx
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/end3_link.fbx
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/end_link.STL
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/end_link.STL
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/end_link.fbx
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/end_link.fbx
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/fixed_link.STL
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/fixed_link.STL
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/fixed_link.fbx
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/fixed_link.fbx
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/length1_link.STL
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/length1_link.STL
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/length1_link.fbx
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/length1_link.fbx
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/length2_link.STL
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/length2_link.STL
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/length2_link.fbx
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/length2_link.fbx
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/length3_link.STL
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/length3_link.STL
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/length3_link.fbx
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/length3_link.fbx
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/slider1_link.STL
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/slider1_link.STL
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/slider1_link.fbx
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/slider1_link.fbx
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/slider2_link.STL
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/slider2_link.STL
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/slider2_link.fbx
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/slider2_link.fbx
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/slider3_link.STL
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/slider3_link.STL
--- a/unilabos/device_mesh/devices/toyo_xyz/meshes/slider3_link.fbx
+++ b/unilabos/device_mesh/devices/toyo_xyz/meshes/slider3_link.fbx
--- a/unilabos/device_mesh/devices/toyo_xyz/param_config.json
+++ b/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
+    }
+}
--- a/unilabos/device_mesh/resource_visalization.py
+++ b/unilabos/device_mesh/resource_visalization.py
@@ -1,13 +1,38 @@
+import json
 import os
 from pathlib import Path
+import re
+
+import yaml
 from launch import LaunchService
 from launch import LaunchDescription
 from launch_ros.actions import Node as nd
 import xacro
 from lxml import etree
-
+from launch_param_builder import load_yaml
+from launch_ros.parameter_descriptions import ParameterFile
 from unilabos.registry.registry import lab_registry
+from ament_index_python.packages import get_package_share_directory

+def get_pattern_matches(folder, pattern):
+    """Given all the files in the folder, find those that match the pattern.
+
+    If there are groups defined, the groups are returned. Otherwise the path to the matches are returned.
+    """
+    matches = []
+    if not folder.exists():
+        return matches
+    for child in folder.iterdir():
+        if not child.is_file():
+            continue
+        m = pattern.search(child.name)
+        if m:
+            groups = m.groups()
+            if groups:
+                matches.append(groups[0])
+            else:
+                matches.append(child)
+    return matches

 class ResourceVisualization:
    def __init__(self, device: dict, resource: dict, enable_rviz: bool = True):
@@ -31,9 +56,8 @@ class ResourceVisualization:
        self.enable_rviz = enable_rviz
        registry = lab_registry

-        self.srdf_str = '''
-        <?xml version="1.0" encoding="UTF-8"?>
-        <robot name="minimal">
+        self.srdf_str = '''<?xml version="1.0" ?>
+        <robot xmlns:xacro="http://ros.org/wiki/xacro" name="full_dev">

        </robot>
        '''
@@ -43,23 +67,46 @@ class ResourceVisualization:
        </robot>
        '''
        self.root = etree.fromstring(self.robot_state_str)
+        self.root_srdf = etree.fromstring(self.srdf_str)
                
        xacro_uri = self.root.nsmap["xacro"]

+        self.moveit_nodes = {}
+        self.moveit_nodes_kinematics = {}
+        self.moveit_controllers_yaml = {
+            "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
+            "moveit_simple_controller_manager": {
+                "controller_names": []
+            }
+        }
+        self.ros2_controllers_yaml = {
+            "controller_manager": {
+                "ros__parameters": {
+                    "update_rate": 100,
+                    "joint_state_broadcaster": {
+                        "type": "joint_state_broadcaster/JointStateBroadcaster",
+                    }
+                }
+            }
+        }
+
        # 遍历设备节点
        for node in device.values():
-            if node['type'] == 'device' and node['class'] != '':
-                device_class = node['class']
-                # 检查设备类型是否在注册表中
-                if device_class not in registry.device_type_registry.keys():
-                    raise ValueError(f"设备类型 {device_class} 未在注册表中注册")
-            elif node['type'] in self.resource_type:
-                # print(registry.resource_type_registry)
-                resource_class = node['class']
-                if resource_class not in registry.resource_type_registry.keys():
-                    raise ValueError(f"资源类型 {resource_class} 未在注册表中注册")
-                elif "model" in registry.resource_type_registry[resource_class].keys():
-                    model_config = registry.resource_type_registry[resource_class]['model']
+            if node['type'] in self.resource_type or (node['type'] == 'device' and node['class'] != ''):
+                model_config = {}
+                if node['type'] in self.resource_type:
+                    resource_class = node['class']
+                    if resource_class not in registry.resource_type_registry.keys():
+                        raise ValueError(f"资源类型 {resource_class} 未在注册表中注册")
+                    elif "model" in registry.resource_type_registry[resource_class].keys():
+                        model_config = registry.resource_type_registry[resource_class]['model']
+                elif node['type'] == 'device' and node['class'] != '':
+                    device_class = node['class']
+                    if device_class not in registry.device_type_registry.keys():
+                        raise ValueError(f"设备类型 {device_class} 未在注册表中注册")
+                    elif "model" in registry.device_type_registry[device_class].keys():
+                        model_config = registry.device_type_registry[device_class]['model']
+                if model_config:
                    if model_config['type'] == 'resource':
                        self.resource_model[node['id']] = {
                            'mesh': f"{str(self.mesh_path)}/resources/{model_config['mesh']}",
@@ -71,18 +118,45 @@ 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"]+'_')
+
+                        print('o'*20)
+                        node["parent"]
+                        node["id"]
+                        print('o'*20)
                        new_dev.set("x",str(float(node["position"]["x"])/1000))
                        new_dev.set("y",str(float(node["position"]["y"])/1000))
                        new_dev.set("z",str(float(node["position"]["z"])/1000))
                        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 +164,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 +204,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 +301,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 +325,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 +346,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 +366,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()
--- a/unilabos/device_mesh/ros2_controllers.yaml
+++ b/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
--- a/unilabos/device_mesh/view_robot.rviz
+++ b/unilabos/device_mesh/view_robot.rviz
@@ -1,23 +1,15 @@
 Panels:
  - Class: rviz_common/Displays
-    Help Height: 138
+    Help Height: 0
    Name: Displays
    Property Tree Widget:
      Expanded:
-        - /Global Options1
-        - /TF1
        - /TF1/Tree1
-        - /RobotModel1
-        - /PlanningScene1
-        - /PlanningScene1/Scene Geometry1
-        - /RobotState1
-        - /RobotState1/Links1
-        - /MotionPlanning1
        - /MotionPlanning1/Scene Geometry1
        - /MotionPlanning1/Scene Robot1
        - /MotionPlanning1/Planning Request1
-      Splitter Ratio: 0.5
-    Tree Height: 345
+      Splitter Ratio: 0.5016146302223206
+    Tree Height: 1112
  - Class: rviz_common/Selection
    Name: Selection
  - Class: rviz_common/Tool Properties
@@ -93,7 +85,7 @@ Visualization Manager:
      Value: false
      Visual Enabled: true
    - Class: moveit_rviz_plugin/PlanningScene
-      Enabled: false
+      Enabled: true
      Move Group Namespace: ""
      Name: PlanningScene
      Planning Scene Topic: /monitored_planning_scene
@@ -104,7 +96,7 @@ Visualization Manager:
        Scene Display Time: 0.009999999776482582
        Show Scene Geometry: true
        Voxel Coloring: Z-Axis
-        Voxel Rendering: Occupied Voxels
+        Voxel Rendering: Disabled
      Scene Robot:
        Attached Body Color: 150; 50; 150
        Links:
@@ -113,10 +105,108 @@ Visualization Manager:
          Expand Link Details: false
          Expand Tree: false
          Link Tree Style: Links in Alphabetic Order
+          PLR_STATION_deck_device_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          PLR_STATION_deck_first_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          PLR_STATION_deck_fourth_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          PLR_STATION_deck_main_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          PLR_STATION_deck_second_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          PLR_STATION_deck_socketTypeGenericSbsFootprint:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          PLR_STATION_deck_socketTypeHEPAModule:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          PLR_STATION_deck_third_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_link_1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_link_2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_link_3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_slideway:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_device_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          benyao_gripper_base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_gripper_left:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_gripper_right:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          hotel_base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          hotel_device_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          hotel_socketTypeGenericSbsFootprint:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          world:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
        Robot Alpha: 1
        Show Robot Collision: false
        Show Robot Visual: false
-      Value: false
+      Value: true
    - Attached Body Color: 150; 50; 150
      Class: moveit_rviz_plugin/RobotState
      Collision Enabled: false
@@ -203,6 +293,63 @@ Visualization Manager:
            Show Axes: false
            Show Trail: false
            Value: true
+          benyao_arm_base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_link_1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_link_2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_link_3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_slideway:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_device_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          benyao_gripper_base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_gripper_left:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_gripper_right:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          hotel_base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          hotel_device_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          hotel_socketTypeGenericSbsFootprint:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
          world:
            Alpha: 1
            Show Axes: false
@@ -230,7 +377,7 @@ Visualization Manager:
        Goal State Color: 250; 128; 0
        Interactive Marker Size: 0
        Joint Violation Color: 255; 0; 255
-        Planning Group: ""
+        Planning Group: benyao_arm
        Query Goal State: false
        Query Start State: false
        Show Workspace: false
@@ -242,9 +389,9 @@ Visualization Manager:
        Scene Alpha: 0.8999999761581421
        Scene Color: 50; 230; 50
        Scene Display Time: 0.009999999776482582
-        Show Scene Geometry: true
-        Voxel Coloring: Z-Axis
-        Voxel Rendering: Occupied Voxels
+        Show Scene Geometry: false
+        Voxel Coloring: Cell Probability
+        Voxel Rendering: All Voxels
      Scene Robot:
        Attached Body Color: 150; 50; 150
        Links:
@@ -290,6 +437,63 @@ Visualization Manager:
            Show Axes: false
            Show Trail: false
            Value: true
+          benyao_arm_base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_link_1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_link_2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_link_3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_arm_slideway:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_device_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          benyao_gripper_base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_gripper_left:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          benyao_gripper_right:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          hotel_base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          hotel_device_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          hotel_socketTypeGenericSbsFootprint:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
          world:
            Alpha: 1
            Show Axes: false
@@ -345,43 +549,43 @@ Visualization Manager:
  Views:
    Current:
      Class: rviz_default_plugins/Orbit
-      Distance: 1.0284695625305176
+      Distance: 2.622864246368408
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.05999999865889549
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Focal Point:
-        X: 0.29730814695358276
-        Y: 0.21228469908237457
-        Z: 0.20008830726146698
+        X: -0.2880733013153076
+        Y: -0.16004444658756256
+        Z: -0.16730672121047974
      Focal Shape Fixed Size: true
      Focal Shape Size: 0.05000000074505806
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.38979560136795044
+      Pitch: 0.48479583859443665
      Target Frame: <Fixed Frame>
      Value: Orbit (rviz)
-      Yaw: 0.06074193865060806
+      Yaw: 0.042561568319797516
    Saved: ~
 Window Geometry:
  Displays:
    collapsed: false
-  Height: 1656
+  Height: 2032
  Hide Left Dock: false
  Hide Right Dock: true
  MotionPlanning:
    collapsed: false
  MotionPlanning - Trajectory Slider:
    collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000003a3000005dcfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000002510000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000007a00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000002cb0000037f000002b800ffffff000000010000010f00000387fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003870000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000627000005dc00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000003a30000079bfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000027000004c60000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000002600000026fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000004f9000002c9000002b800ffffff000000010000010f00000387fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003870000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000bc50000079b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
  Selection:
    collapsed: false
  Tool Properties:
    collapsed: false
  Views:
    collapsed: true
-  Width: 2518
-  X: 385
-  Y: 120
+  Width: 3956
+  X: 140
+  Y: 54
--- a/unilabos/devices/laiyu_add_solid/init.py
+++ b/unilabos/devices/laiyu_add_solid/init.py
--- a/unilabos/devices/liquid_handling/biomek.py
+++ b/unilabos/devices/liquid_handling/biomek.py
--- a/unilabos/devices/liquid_handling/biomek.txt
+++ b/unilabos/devices/liquid_handling/biomek.txt
--- a/unilabos/devices/liquid_handling/biomek_temporary_protocol.json
+++ b/unilabos/devices/liquid_handling/biomek_temporary_protocol.json
--- a/unilabos/devices/liquid_handling/biomek_test.py
+++ b/unilabos/devices/liquid_handling/biomek_test.py
--- a/unilabos/devices/liquid_handling/complete_biomek_protocol.json
+++ b/unilabos/devices/liquid_handling/complete_biomek_protocol.json
--- a/unilabos/devices/liquid_handling/complete_biomek_protocol_0608.json
+++ b/unilabos/devices/liquid_handling/complete_biomek_protocol_0608.json
--- a/Show More
+++ b/Show More