完成在main中启动设备可视化

完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model

添加物料模型管理类，遍历物料与resource_model，完成TF数据收集

test/experiments/test.json

@@ -25,7 +25,7 @@
             ],
             "parent": null,
             "type": "plate",
-            "class": "nest_96_wellplate_2ml_deep",
+            "class": "nest_96_wellplate_100ul_pcr_full_skirt",
             "position": {
                 "x": 620.6111111111111,
                 "y": 171,

unilabos/app/main.py

@@ -5,6 +5,7 @@ import sys
 import json
 import yaml
 from copy import deepcopy
+import threading
 
 # 首先添加项目根目录到路径
 current_dir = os.path.dirname(os.path.abspath(__file__))
@@ -14,7 +15,7 @@ if ilabos_dir not in sys.path:
 
 from unilabos.config.config import load_config, BasicConfig
 from unilabos.utils.banner_print import print_status, print_unilab_banner
-
+from unilabos.device_mesh.resource_visalization import ResourceVisualization
 
 def parse_args():
     """解析命令行参数"""
@@ -102,7 +103,8 @@ def main():
     print_unilab_banner(args_dict)
 
     # 注册表
-    build_registry(args_dict["registry_path"])
+    registry_dict = build_registry(args_dict["registry_path"])
+
 
     if args_dict["graph"] is not None:
         import unilabos.resources.graphio as graph_res
@@ -150,8 +152,16 @@ def main():
         signal.signal(signal.SIGTERM, _exit)
         mqtt_client.start()
 
+    resource_visualization = ResourceVisualization(args_dict["devices_config"], args_dict["resources_config"],registry_dict)
     start_backend(**args_dict)
-    start_server()
+    print('-'*100)
+    print(resource_visualization.resource_model)
+    print(json.dumps(args_dict["resources_config"], indent=4, ensure_ascii=False))
+    print('-'*100)
+    server_thread = threading.Thread(target=start_server)
+    server_thread.start()
+
+    resource_visualization.start()
 
 
 if __name__ == "__main__":

unilabos/device_mesh/devices/opentrons_liquid_handler/macro_device.xacro

@@ -24,7 +24,7 @@
     <link name='${station_name}${device_name}main_link'>
       <visual>
         <geometry>
-          <mesh filename="file://${mesh_path}/device/opentrons_liquid_handler/meshes/ot2-0.stl"/>
+          <mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-0.stl"/>
         </geometry>
         <material name="">
             <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
@@ -33,7 +33,7 @@
       <collision>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="file://${mesh_path}/device/opentrons_liquid_handler/meshes/ot2-0.stl"/>
+          <mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-0.stl"/>
         </geometry>
       </collision>
     </link>
@@ -41,7 +41,7 @@
     <link name='${station_name}${device_name}first_link'>
       <visual>
         <geometry>
-          <mesh filename="file://${mesh_path}/device/opentrons_liquid_handler/meshes/ot2-1.stl"/>
+          <mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-1.stl"/>
         </geometry>
         <material name="">
             <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
@@ -50,7 +50,7 @@
       <collision>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="file://${mesh_path}/device/opentrons_liquid_handler/meshes/ot2-1.stl"/>
+          <mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-1.stl"/>
         </geometry>
       </collision>
     </link>
@@ -58,7 +58,7 @@
     <link name='${station_name}${device_name}second_link'>
       <visual>
         <geometry>
-          <mesh filename="file://${mesh_path}/device/opentrons_liquid_handler/meshes/ot2-2.stl"/>
+          <mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-2.stl"/>
         </geometry>
         <material name="">
             <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
@@ -67,7 +67,7 @@
       <collision>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="file://${mesh_path}/device/opentrons_liquid_handler/meshes/ot2-2.stl"/>
+          <mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-2.stl"/>
         </geometry>
       </collision>
     </link>
@@ -75,7 +75,7 @@
     <link name='${station_name}${device_name}third_link'>
       <visual>
         <geometry>
-          <mesh filename="file://${mesh_path}/device/opentrons_liquid_handler/meshes/ot2-3a.stl"/>
+          <mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-3a.stl"/>
         </geometry>
         <material name="">
             <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
@@ -84,7 +84,7 @@
       <collision>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="file://${mesh_path}/device/opentrons_liquid_handler/meshes/ot2-3a.stl"/>
+          <mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-3a.stl"/>
         </geometry>
       </collision>
     </link>
@@ -92,7 +92,7 @@
     <link name='${station_name}${device_name}fourth_link'>
       <visual>
         <geometry>
-          <mesh filename="file://${mesh_path}/device/opentrons_liquid_handler/meshes/ot2-3b.stl"/>
+          <mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-3b.stl"/>
         </geometry>
         <material name="">
             <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
@@ -101,7 +101,7 @@
       <collision>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="file://${mesh_path}/device/opentrons_liquid_handler/meshes/ot2-3b.stl"/>
+          <mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-3b.stl"/>
         </geometry>
       </collision>
     </link>

unilabos/device_mesh/devices/slide_w140/macro_device.xacro

@@ -35,7 +35,7 @@
       <visual>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="file://${mesh_path}/device/slide_w140/meshes/base_link.STL"/>
+          <mesh filename="file://${mesh_path}/devices/slide_w140/meshes/base_link.STL"/>
         </geometry>
         <material name="">
           <color rgba="1 1 1 1"/>
@@ -44,7 +44,7 @@
       <collision>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="file://${mesh_path}/device/slide_w140/meshes/base_link.STL"/>
+          <mesh filename="file://${mesh_path}/devices/slide_w140/meshes/base_link.STL"/>
         </geometry>
       </collision>
     </link>
@@ -57,7 +57,7 @@
       <visual>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="file://${mesh_path}/device/slide_w140/meshes/slider.STL" />
+          <mesh filename="file://${mesh_path}/devices/slide_w140/meshes/slider.STL" />
         </geometry>
         <material name="">
           <color rgba="1 1 1 1"/>
@@ -66,7 +66,7 @@
       <collision>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="file://${mesh_path}/device/slide_w140/meshes/slider.STL" />
+          <mesh filename="file://${mesh_path}/devices/slide_w140/meshes/slider.STL" />
         </geometry>
       </collision>
     </link>
@@ -86,7 +86,7 @@
       <visual>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="file://${mesh_path}/device/slide_w140/meshes/length.STL" scale="${(length + min_d + max_d + slider_d)} 1 1"/>
+          <mesh filename="file://${mesh_path}/devices/slide_w140/meshes/length.STL" scale="${(length + min_d + max_d + slider_d)} 1 1"/>
         </geometry>
         <material name="">
           <color rgba="1 1 1 1"/>
@@ -95,7 +95,7 @@
       <collision>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="file://${mesh_path}/device/slide_w140/meshes/length.STL" scale="${(length + min_d + max_d + slider_d)} 1 1"/>
+          <mesh filename="file://${mesh_path}/devices/slide_w140/meshes/length.STL" scale="${(length + min_d + max_d + slider_d)} 1 1"/>
         </geometry>
       </collision>
     </link>
@@ -113,7 +113,7 @@
       <visual>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="file://${mesh_path}/device/slide_w140/meshes/slide_end.STL"/>
+          <mesh filename="file://${mesh_path}/devices/slide_w140/meshes/slide_end.STL"/>
         </geometry>
         <material name="">
           <color rgba="1 1 1 1"/>
@@ -122,7 +122,7 @@
       <collision>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="file://${mesh_path}/device/slide_w140/meshes/slide_end.STL"/>
+          <mesh filename="file://${mesh_path}/devices/slide_w140/meshes/slide_end.STL"/>
         </geometry>
       </collision>
     </link>

unilabos/device_mesh/resource_visalization.py

@@ -1,3 +1,5 @@
+import os
+from pathlib import Path
 from launch import LaunchService
 from launch import LaunchDescription
 from launch_ros.actions import Node as nd
@@ -6,19 +8,26 @@ from lxml import etree
 
 
 class ResourceVisualization:
-    def __init__(self, device: dict, registry: dict,  resource: dict, enable_rviz: bool = False):
-        
+    def __init__(self, device: dict, resource: dict, registry: dict, enable_rviz: bool = True):
         """初始化资源可视化类
         
+        该类用于将设备和资源的3D模型可视化展示。通过解析设备和资源的配置信息,
+        从注册表中获取对应的3D模型文件,并使用ROS2和RViz进行可视化。
+        
         Args:
-            device: 设备配置字典
-            registry: 注册表字典
+            device (dict): 设备配置字典,包含设备的类型、位置等信息
+            resource (dict): 资源配置字典,包含资源的类型、位置等信息 
+            registry (dict): 注册表字典,包含设备和资源类型的注册信息
+            enable_rviz (bool, optional): 是否启用RViz可视化. Defaults to True.
         """
         self.launch_service = LaunchService()
         self.launch_description = LaunchDescription()
         self.resource_dict = resource
         self.resource_model = {}
         self.resource_type = ['plate', 'container']
+        self.mesh_path = Path(__file__).parent.absolute()
+        self.enable_rviz = enable_rviz
+
 
         self.robot_state_str= '''<?xml version="1.0" ?>
         <robot xmlns:xacro="http://ros.org/wiki/xacro" name="full_dev">
@@ -28,8 +37,9 @@ class ResourceVisualization:
         self.root = etree.fromstring(self.robot_state_str)
                 
         xacro_uri = self.root.nsmap["xacro"]
+
         # 遍历设备节点
-        for node in device['nodes']:
+        for node in device.values():
             if node['type'] == 'device':
                 device_class = node['class']
                 
@@ -37,37 +47,40 @@ class ResourceVisualization:
                 if device_class not in registry.device_type_registry.keys():
                     raise ValueError(f"设备类型 {device_class} 未在注册表中注册")
                 
-                elif "model" in device_class.keys():
+                elif "model" in registry.device_type_registry[device_class].keys():
                     model_config = registry.device_type_registry[device_class]['model']
 
                     if model_config['type'] == 'device':
                         new_include = etree.SubElement(self.root, f"{{{xacro_uri}}}include")
-                        new_include.set("filename", f"{model_config['mesh']}/macro_device.xacro")
+                        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))
                         
             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} 未在注册表中注册")
-                if model_config['type'] == 'resource':
+                elif "model" in registry.resource_type_registry[resource_class].keys():
                     model_config = registry.resource_type_registry[resource_class]['model']
-                    self.resource_model[node['id']] = model_config['mesh']
+                    if model_config['type'] == 'resource':
+                        self.resource_model[node['id']] = f"{str(self.mesh_path)}/resources/{model_config['mesh']}"
                         if model_config['children_mesh'] is not None:
-                        self.resource_model[f"{node['id']}_"] = model_config['children_mesh']
+                            self.resource_model[f"{node['id']}_"] = f"{str(self.mesh_path)}/resources/{model_config['children_mesh']}"
             
         re = etree.tostring(self.root, encoding="unicode")
         doc = xacro.parse(re)
         xacro.process_doc(doc)
+        self.urdf_str = doc.toxml()
 
 
-    def create_launch_description(self, urdf_str: str, enable_rviz: bool = False) -> LaunchDescription:
+    def create_launch_description(self, urdf_str: str) -> LaunchDescription:
         """
         创建launch描述，包含robot_state_publisher和move_group节点
 
         Args:
             urdf_str: URDF文本
-            enable_rviz: 是否启用RViz可视化
 
         Returns:
             LaunchDescription: launch描述对象
@@ -109,7 +122,7 @@ class ResourceVisualization:
         self.launch_description.add_action(move_group)
 
         # 如果启用RViz,添加RViz节点
-        if enable_rviz:
+        if self.enable_rviz:
             rviz_node = nd(
                 package='rviz2',
                 executable='rviz2',
@@ -120,13 +133,13 @@ class ResourceVisualization:
 
         return self.launch_description
 
-    def start(self, urdf_str: str) -> None:
+    def start(self) -> None:
         """
         启动可视化服务
 
         Args:
             urdf_str: URDF文件路径
         """
-        launch_description = self.create_launch_description(urdf_str)
+        launch_description = self.create_launch_description(self.urdf_str)
         self.launch_service.include_launch_description(launch_description)
         self.launch_service.run()

unilabos/registry/devices/robot_gripper.yaml

@@ -20,8 +20,8 @@ gripper.mock:
           position: position
           effort: torque
   model:
-    tpye: device
-    mesh: slide_w140
+    type: device
+    mesh: opentrons_liquid_handler
 
 gripper.misumi_rz:
   description: Misumi RZ gripper

unilabos/registry/resources/opentrons/plates.yaml

@@ -53,8 +53,8 @@ nest_96_wellplate_100ul_pcr_full_skirt:
     type: pylabrobot
   model: 
     type: resource
-    mesh: /home/z43/git_pj/uni-lab-assets/device_models/tecan_nested_tip_rack/meshes/plate.stl
-    children_mesh: /home/z43/git_pj/uni-lab-assets/device_models/generic_labware_tube_10_75/meshes/0_base.stl
+    mesh: tecan_nested_tip_rack/meshes/plate.stl
+    children_mesh: generic_labware_tube_10_75/meshes/0_base.stl
     
 appliedbiosystemsmicroamp_384_wellplate_40ul:
   description: Applied Biosystems microamp 384 wellplate 40ul