feat: node_info_update srv

fix: OTDeck cant create

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):
         """
         添加资源回调