feat: node_info_update srv

fix: OTDeck cant create

unilabos/ros/main_slave_run.py

@@ -1,14 +1,16 @@
+import json
 import os
 import traceback
 from typing import Optional, Dict, Any, List
 
 import rclpy
 from unilabos_msgs.msg import Resource  # type: ignore
-from unilabos_msgs.srv import ResourceAdd  # type: ignore
+from unilabos_msgs.srv import ResourceAdd, SerialCommand  # type: ignore
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
 from rclpy.timer import Timer
 
+from unilabos.registry.registry import lab_registry
 from unilabos.ros.initialize_device import initialize_device_from_dict
 from unilabos.ros.msgs.message_converter import (
     convert_to_ros_msg,
@@ -17,6 +19,7 @@ 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
 
 
 def exit() -> None:
@@ -98,17 +101,29 @@ def slave(
     n = Node(f"slaveMachine_{machine_name}", parameter_overrides=[])
     executor.add_node(n)
 
-    if BasicConfig.slave_no_host:
-        # 确保ResourceAdd存在
-        if "ResourceAdd" in globals():
-            rclient = n.create_client(ResourceAdd, "/resources/add")
-            rclient.wait_for_service()  # FIXME 可能一直等待，加一个参数
+    if not BasicConfig.slave_no_host:
+        sclient = n.create_client(SerialCommand, "/node_info_update")
+        sclient.wait_for_service()
+
+        request = SerialCommand.Request()
+        request.command = json.dumps({
+            "machine_name": machine_name,
+            "type": "slave",
+            "devices_config": devices_config,
+            "registry_config": lab_registry.obtain_registry_device_info()
+        }, ensure_ascii=False, cls=TypeEncoder)
+        response = sclient.call_async(request)
+        logger.info(f"Slave node info update response: {response}")
+
+        rclient = n.create_client(ResourceAdd, "/resources/add")
+        rclient.wait_for_service()  # FIXME 可能一直等待，加一个参数
+
+        request = ResourceAdd.Request()
+        request.resources = [convert_to_ros_msg(Resource, resource) for resource in resources_config]
+        response = rclient.call_async(request)
+        logger.info(f"Slave resource add response: {response}")
+
 
-            request = ResourceAdd.Request()
-            request.resources = [convert_to_ros_msg(Resource, resource) for resource in resources_config]
-            response = rclient.call_async(request)
-        else:
-            print("Warning: ResourceAdd service not available")
 
     run_event_loop_in_thread()
 

unilabos/ros/nodes/presets/host_node.py

@@ -1,4 +1,5 @@
 import copy
+import json
 import threading
 import time
 import traceback
@@ -7,7 +8,7 @@ from typing import Optional, Dict, Any, List, ClassVar, Set
 
 from action_msgs.msg import GoalStatus
 from unilabos_msgs.msg import Resource  # type: ignore
-from unilabos_msgs.srv import ResourceAdd, ResourceGet, ResourceDelete, ResourceUpdate, ResourceList  # type: ignore
+from unilabos_msgs.srv import ResourceAdd, ResourceGet, ResourceDelete, ResourceUpdate, ResourceList, SerialCommand  # type: ignore
 from rclpy.action import ActionClient, get_action_server_names_and_types_by_node
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.service import Service
@@ -20,7 +21,8 @@ from unilabos.ros.msgs.message_converter import (
     get_ros_type_by_msgname,
     convert_from_ros_msg,
     convert_to_ros_msg,
-    msg_converter_manager, ros_action_to_json_schema,
+    msg_converter_manager,
+    ros_action_to_json_schema,
 )
 from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode, ROS2DeviceNode, DeviceNodeResourceTracker
 from unilabos.ros.nodes.presets.controller_node import ControllerNode
@@ -106,7 +108,7 @@ class HostNode(BaseROS2DeviceNode):
         self._subscribed_topics = set()  # 用于跟踪已订阅的话题
 
         # 创建物料增删改查服务（非客户端）
-        self._init_resource_service()
+        self._init_host_service()
 
         self.device_status = {}  # 用来存储设备状态
         self.device_status_timestamps = {}  # 用来存储设备状态最后更新时间
@@ -117,7 +119,9 @@ class HostNode(BaseROS2DeviceNode):
         # 初始化所有本机设备节点，多一次过滤，防止重复初始化
         for device_id, device_config in devices_config.items():
             if device_config.get("type", "device") != "device":
-                self.lab_logger().debug(f"[Host Node] Skipping type {device_config['type']} {device_id} already existed, skipping.")
+                self.lab_logger().debug(
+                    f"[Host Node] Skipping type {device_config['type']} {device_id} already existed, skipping."
+                )
                 continue
             if device_id not in self.devices_names:
                 self.initialize_device(device_id, device_config)
@@ -226,6 +230,7 @@ class HostNode(BaseROS2DeviceNode):
                     )
                     self.lab_logger().debug(f"[Host Node] Created ActionClient: {action_id}")
                     from unilabos.app.mq import mqtt_client
+
                     info_with_schema = ros_action_to_json_schema(action_type)
                     mqtt_client.publish_actions(action_id, info_with_schema)
                 except Exception as e:
@@ -259,6 +264,7 @@ class HostNode(BaseROS2DeviceNode):
                 self._action_clients[action_id] = ActionClient(self, action_type, action_id)
                 self.lab_logger().debug(f"[Host Node] Created ActionClient: {action_id}")
                 from unilabos.app.mq import mqtt_client
+
                 info_with_schema = ros_action_to_json_schema(action_type)
                 mqtt_client.publish_actions(action_id, info_with_schema)
             else:
@@ -473,7 +479,7 @@ class HostNode(BaseROS2DeviceNode):
 
     """Resource"""
 
-    def _init_resource_service(self):
+    def _init_host_service(self):
         self._resource_services: Dict[str, Service] = {
             "resource_add": self.create_service(
                 ResourceAdd, "/resources/add", self._resource_add_callback, callback_group=ReentrantCallbackGroup()
@@ -496,8 +502,28 @@ class HostNode(BaseROS2DeviceNode):
             "resource_list": self.create_service(
                 ResourceList, "/resources/list", self._resource_list_callback, callback_group=ReentrantCallbackGroup()
             ),
+            "node_info_update": self.create_service(
+                SerialCommand,
+                "/node_info_update",
+                self._node_info_update_callback,
+                callback_group=ReentrantCallbackGroup(),
+            ),
         }
 
+    def _node_info_update_callback(self, request, response):
+        """
+        更新节点信息回调
+        """
+        self.lab_logger().info(f"[Host Node] Node info update request received: {request}")
+        try:
+            info = json.loads(request.command)
+            self.lab_logger().info(f"[Host Node] Node info update: {info}")
+            response.response = "OK"
+        except Exception as e:
+            self.lab_logger().error(f"[Host Node] Error updating node info: {str(e)}")
+            response.response = "ERROR"
+        return response
+
     def _resource_add_callback(self, request, response):
         """
         添加资源回调