Device visualization (#14)

* add 3d visualization

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

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

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

* 完成TF发布

* 修改模型方向，在yaml中添加变换属性

* 添加物料tf变化时，发送topic到前端

另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题

* 添加关节发布节点与物料可视化节点进入unilab

* 使用json启动plr与3D模型仿真

* 完成启动OT并联动rviz

* add 3d visualization

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

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

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

* 完成TF发布

* 修改模型方向，在yaml中添加变换属性

* 添加物料tf变化时，发送topic到前端

另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题

* 添加关节发布节点与物料可视化节点进入unilab

* 使用json启动plr与3D模型仿真

* 完成启动OT并联动rviz

* 修复rviz位置问题,

修复rviz位置问题,
在无tf变动时减缓发送频率
在backend中添加物料跟随方法

* fix: running logic

* fix: running logic

* fix: missing ot

* 在main中直接初始化republisher和物料的mesh节点

* 将joint_republisher和resource_mesh_manager添加进 main_slave_run.py中

---------

Co-authored-by: zhangshixiang <@zhangshixiang>
Co-authored-by: wznln <18435084+Xuwznln@users.noreply.github.com>

unilabos/app/backend.py

@@ -12,6 +12,8 @@ def start_backend(
     controllers_config: dict = {},
     bridges=[],
     without_host: bool = False,
+    visual: str = "None",
+    resources_mesh_config: dict = {},
     **kwargs
 ):
     if backend == "ros":
@@ -29,7 +31,9 @@ def start_backend(
     
     backend_thread = threading.Thread(
         target=main if not without_host else slave,
-        args=(devices_config, resources_config, graph, controllers_config, bridges)
+        args=(devices_config, resources_config, graph, controllers_config, bridges, visual, resources_mesh_config),
+        name="backend_thread",
+        daemon=True,
     )
     backend_thread.start()
     logger.info(f"Backend {backend} started.")

unilabos/app/main.py

@@ -1,10 +1,17 @@
 import argparse
+import asyncio
 import os
 import signal
 import sys
 import json
+import time
+
 import yaml
 from copy import deepcopy
+import threading
+
+import rclpy
+from unilabos.ros.nodes.resource_tracker import DeviceNodeResourceTracker
 
 # 首先添加项目根目录到路径
 current_dir = os.path.dirname(os.path.abspath(__file__))
@@ -14,6 +21,10 @@ if ilabos_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
+from unilabos.ros.nodes.presets.joint_republisher import JointRepublisher
+from unilabos.ros.nodes.presets.resource_mesh_manager import ResourceMeshManager
+from rclpy.executors import MultiThreadedExecutor
 
 
 def parse_args():
@@ -70,7 +81,12 @@ def parse_args():
         default=True,
         help="是否在启动时打开信息页",
     )
-
+    parser.add_argument(
+        "--visual",
+        choices=["rviz", "web","None"],
+        default="rviz",
+        help="选择可视化工具: 'rviz' 或 'web' 或 'None'，默认'rviz'",
+    )
     return parser.parse_args()
 
 
@@ -121,6 +137,7 @@ def main():
     # 注册表
     build_registry(args_dict["registry_path"])
 
+
     if args_dict["graph"] is not None:
         import unilabos.resources.graphio as graph_res
         graph_res.physical_setup_graph = (
@@ -132,6 +149,7 @@ def main():
         args_dict["resources_config"] = initialize_resources(list(deepcopy(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:
@@ -166,9 +184,28 @@ def main():
         signal.signal(signal.SIGINT, _exit)
         signal.signal(signal.SIGTERM, _exit)
         mqtt_client.start()
+    args_dict["resources_mesh_config"] = {}
+    
+    if args_dict["visual"] != "None":
+        if args_dict["visual"] == "rviz":
+            enable_rviz=True
+        elif args_dict["visual"] == "web":
+            enable_rviz=False
+        resource_visualization = ResourceVisualization(devices_and_resources, args_dict["resources_config"] ,enable_rviz=enable_rviz)
 
-    start_backend(**args_dict)
-    start_server(port=args_dict.get("port", 8002), open_browser=args_dict.get("open_browser", False))
+        args_dict["resources_mesh_config"] = resource_visualization.resource_model
+        # 将joint_republisher和resource_mesh_manager添加进 main_slave_run.py中
+        
+        start_backend(**args_dict)
+        server_thread = threading.Thread(target=start_server)
+        server_thread.start()
+        asyncio.set_event_loop(asyncio.new_event_loop())
+        resource_visualization.start()
+        while True:
+            time.sleep(1)
+    else:
+        start_backend(**args_dict)
+        start_server()
 
 
 if __name__ == "__main__":

unilabos/app/mq.py

@@ -35,7 +35,8 @@ class MQTTClient:
         self.client.on_disconnect = self._on_disconnect
 
     def _on_log(self, client, userdata, level, buf):
-        logger.info(f"[MQTT] log: {buf}")
+        # logger.info(f"[MQTT] log: {buf}")
+        pass
 
     def _on_connect(self, client, userdata, flags, rc, properties=None):
         logger.info("[MQTT] Connected with result code " + str(rc))