From 2d034f728a91a1d118c0feedd7c92d35e74cc834 Mon Sep 17 00:00:00 2001
From: wznln <18435084+Xuwznln@users.noreply.github.com>
Date: Wed, 30 Apr 2025 15:21:29 +0800
Subject: [PATCH] fix: slave mode spin not working

---
 unilabos/ros/main_slave_run.py       | 28 +++++++---------------------
 unilabos/ros/utils/driver_creator.py |  2 --
 2 files changed, 7 insertions(+), 23 deletions(-)

diff --git a/unilabos/ros/main_slave_run.py b/unilabos/ros/main_slave_run.py
index 974359a3..60dc189e 100644
--- a/unilabos/ros/main_slave_run.py
+++ b/unilabos/ros/main_slave_run.py
@@ -1,10 +1,12 @@
 import copy
 import json
 import os
+import threading
 import traceback
 from typing import Optional, Dict, Any, List
 
 import rclpy
+from rclpy.callback_groups import ReentrantCallbackGroup
 from unilabos_msgs.msg import Resource  # type: ignore
 from unilabos_msgs.srv import ResourceAdd, SerialCommand  # type: ignore
 from rclpy.executors import MultiThreadedExecutor
@@ -17,7 +19,6 @@ from unilabos.ros.msgs.message_converter import (
     convert_to_ros_msg,
 )
 from unilabos.ros.nodes.presets.host_node import HostNode
-from unilabos.ros.x.rclpyx import run_event_loop_in_thread
 from unilabos.utils import logger
 from unilabos.config.config import BasicConfig
 from unilabos.utils.type_check import TypeEncoder
@@ -63,16 +64,8 @@ def main(
         discovery_interval,
     )
 
-    executor.add_node(host_node)
-    # run_event_loop_in_thread()
-
-    try:
-        executor.spin()
-    except Exception as e:
-        logger.error(traceback.format_exc())
-        print(f"Exception caught: {e}")
-    finally:
-        exit()
+    thread = threading.Thread(target=executor.spin, daemon=True, name="host_executor_thread")
+    thread.start()
 
 
 def slave(
@@ -102,6 +95,9 @@ def slave(
     n = Node(f"slaveMachine_{machine_name}", parameter_overrides=[])
     executor.add_node(n)
 
+    thread = threading.Thread(target=executor.spin, daemon=True, name="slave_executor_thread")
+    thread.start()
+
     if not BasicConfig.slave_no_host:
         sclient = n.create_client(SerialCommand, "/node_info_update")
         sclient.wait_for_service()
@@ -125,15 +121,5 @@ def slave(
         logger.info(f"Slave resource added.")
 
 
-    run_event_loop_in_thread()
-
-    try:
-        executor.spin()
-    except Exception as e:
-        print(f"Exception caught: {e}")
-    finally:
-        exit()
-
-
 if __name__ == "__main__":
     main()
diff --git a/unilabos/ros/utils/driver_creator.py b/unilabos/ros/utils/driver_creator.py
index b2402b5e..24c98b21 100644
--- a/unilabos/ros/utils/driver_creator.py
+++ b/unilabos/ros/utils/driver_creator.py
@@ -224,8 +224,6 @@ class PyLabRobotCreator(DeviceClassCreator[T]):
         if hasattr(self.device_instance, "setup") and asyncio.iscoroutinefunction(getattr(self.device_instance, "setup")):
             from unilabos.ros.nodes.base_device_node import ROS2DeviceNode
             ROS2DeviceNode.run_async_func(getattr(self.device_instance, "setup")).add_done_callback(lambda x: logger.debug(f"PyLabRobot设备实例 {self.device_instance} 设置完成"))
-# 2486229810384
-#2486232539792
 
 class ProtocolNodeCreator(DeviceClassCreator[T]):
     """