将joint_republisher和resource_mesh_manager添加进 main_slave_run.py中

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

unilabos/app/main.py

@@ -184,6 +184,7 @@ 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":
@@ -192,28 +193,9 @@ def main():
             enable_rviz=False
         resource_visualization = ResourceVisualization(devices_and_resources, args_dict["resources_config"] ,enable_rviz=enable_rviz)
 
-        # 如果没有初始化，则初始化ros，并创建一个多线程执行器
-        # 在main_slave_run.py中也会初始化ros，并创建一个多线程执行器
-        # 所以这里需要判断是否已经初始化，如果已经初始化，则不重复初始化    
-        if not rclpy.ok():
-            rclpy.init(args = ["--log-level", "debug"],)
-        executor = rclpy.__executor
-        if not executor:
-            executor = rclpy.__executor = MultiThreadedExecutor()
+        args_dict["resources_mesh_config"] = resource_visualization.resource_model
+        # 将joint_republisher和resource_mesh_manager添加进 main_slave_run.py中
         
-        resource_mesh_manager = ResourceMeshManager(
-            resource_visualization.resource_model,
-            args_dict["resources_config"],
-            resource_tracker= DeviceNodeResourceTracker(),
-            device_id = 'resource_mesh_manager',
-        )
-        joint_republisher = JointRepublisher(
-            'joint_republisher',
-            DeviceNodeResourceTracker()
-        )
-
-        rclpy.__executor.add_node(resource_mesh_manager)
-        rclpy.__executor.add_node(joint_republisher)
         start_backend(**args_dict)
         server_thread = threading.Thread(target=start_server)
         server_thread.start()

unilabos/ros/main_slave_run.py

@@ -6,6 +6,9 @@ import time
 from typing import Optional, Dict, Any, List
 
 import rclpy
+from unilabos.ros.nodes.presets.joint_republisher import JointRepublisher
+from unilabos.ros.nodes.presets.resource_mesh_manager import ResourceMeshManager
+from unilabos.ros.nodes.resource_tracker import DeviceNodeResourceTracker
 from unilabos_msgs.msg import Resource  # type: ignore
 from unilabos_msgs.srv import ResourceAdd, SerialCommand  # type: ignore
 from rclpy.executors import MultiThreadedExecutor
@@ -45,15 +48,15 @@ def main(
     graph: Optional[Dict[str, Any]] = None,
     controllers_config: Dict[str, Any] = {},
     bridges: List[Any] = [],
+    visual: str = "None",
+    resources_mesh_config: dict = {},
     args: List[str] = ["--log-level", "debug"],
     discovery_interval: float = 5.0,
 ) -> None:
     """主函数"""
-    if not rclpy.ok():
-        rclpy.init(args=args)
-    executor = rclpy.__executor
-    if not executor:
-        executor = rclpy.__executor = MultiThreadedExecutor()
+
+    rclpy.init(args=args)
+    executor = rclpy.__executor = MultiThreadedExecutor()
     # 创建主机节点
     host_node = HostNode(
         "host_node",
@@ -65,6 +68,21 @@ def main(
         discovery_interval,
     )
 
+    if visual != "None":
+        resource_mesh_manager = ResourceMeshManager(
+            resources_mesh_config,
+            resources_config,
+            resource_tracker= DeviceNodeResourceTracker(),
+            device_id = 'resource_mesh_manager',
+        )
+        joint_republisher = JointRepublisher(
+            'joint_republisher',
+            DeviceNodeResourceTracker()
+        )
+
+        executor.add_node(resource_mesh_manager)
+        executor.add_node(joint_republisher)
+        
     thread = threading.Thread(target=executor.spin, daemon=True, name="host_executor_thread")
     thread.start()