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

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

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

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['children_mesh'] is not None:
-                        self.resource_model[f"{node['id']}_"] = model_config['children_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']}_"] = 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()