open_pub/Uni-Lab-OS
1
0
Fork 0
mirror of https://github.com/dptech-corp/Uni-Lab-OS.git synced 2025-12-17 13:01:12 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

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

添加物料模型管理类，遍历物料与resource_model，完成TF数据收集
This commit is contained in:
zhangshixiang
2025-04-24 00:59:43 +08:00
committed by Junhan Chang
parent b7a16cdfc8
commit 2baa232b86
40 changed files with 4678 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
unilabos/device_mesh/resource_visalization.py
View File

@@ -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()