Merge remote-tracking branch 'upstream/dev' into device_visualization

unilabos/ros/device_node_wrapper.py

@@ -55,7 +55,7 @@ def ros2_device_node(
             "read": "read_data",
             "extra_info": [],
         }
-
+    # FIXME 后面要删除
     for k, v in cls.__dict__.items():
         if not k.startswith("_") and isinstance(v, property):
             # noinspection PyUnresolvedReferences

unilabos/ros/msgs/message_converter.py

@@ -132,7 +132,11 @@ _msg_converter: Dict[Type, Any] = {
     Bool: lambda x: Bool(data=bool(x)),
     str: str,
     String: lambda x: String(data=str(x)),
-    Point: lambda x: Point(x=x.x, y=x.y, z=x.z) if not isinstance(x, dict) else Point(x=x.get("x", 0.0), y=x.get("y", 0.0), z=x.get("z", 0.0)),
+    Point: lambda x: (
+        Point(x=x.x, y=x.y, z=x.z)
+        if not isinstance(x, dict)
+        else Point(x=float(x.get("x", 0.0)), y=float(x.get("y", 0.0)), z=float(x.get("z", 0.0)))
+    ),
     Resource: lambda x: Resource(
         id=x.get("id", ""),
         name=x.get("name", ""),
@@ -142,7 +146,13 @@ _msg_converter: Dict[Type, Any] = {
         type=x.get("type", ""),
         category=x.get("class", "") or x.get("type", ""),
         pose=(
-            Pose(position=Point(x=float(x.get("position", {}).get("x", 0.0)), y=float(x.get("position", {}).get("y", 0.0)), z=float(x.get("position", {}).get("z", 0.0))))
+            Pose(
+                position=Point(
+                    x=float(x.get("position", {}).get("x", 0.0)),
+                    y=float(x.get("position", {}).get("y", 0.0)),
+                    z=float(x.get("position", {}).get("z", 0.0)),
+                )
+            )
             if x.get("position", None) is not None
             else Pose()
         ),
@@ -151,6 +161,7 @@ _msg_converter: Dict[Type, Any] = {
     ),
 }
 
+
 def json_or_yaml_loads(data: str) -> Any:
     try:
         return json.loads(data)
@@ -161,6 +172,7 @@ def json_or_yaml_loads(data: str) -> Any:
             pass
         raise e
 
+
 # ROS消息到Python转换器
 _msg_converter_back: Dict[Type, Any] = {
     float: float,
@@ -343,7 +355,7 @@ def convert_to_ros_msg(ros_msg_type: Union[Type, Any], obj: Any) -> Any:
         if hasattr(ros_msg, key):
             attr = getattr(ros_msg, key)
             if isinstance(attr, (float, int, str, bool)):
-                setattr(ros_msg, key, value)
+                setattr(ros_msg, key, type(attr)(value))
             elif isinstance(attr, (list, tuple)) and isinstance(value, Iterable):
                 td = ros_msg.SLOT_TYPES[ind].value_type
                 if isinstance(td, NamespacedType):
@@ -355,7 +367,7 @@ def convert_to_ros_msg(ros_msg_type: Union[Type, Any], obj: Any) -> Any:
                     logger.warning(f"Not Supported type: {td}")
                     setattr(ros_msg, key, [])  # FIXME
             elif "array.array" in str(type(attr)):
-                if attr.typecode == "f":
+                if attr.typecode == "f" or attr.typecode == "d":
                     setattr(ros_msg, key, [float(i) for i in value])
                 else:
                     setattr(ros_msg, key, value)
@@ -496,47 +508,90 @@ def convert_from_ros_msg_with_mapping(ros_msg: Any, value_mapping: Dict[str, str
         Python字典
     """
     data: Dict[str, Any] = {}
+    
+    # # 🔧 添加调试信息
+    # print(f"🔍 convert_from_ros_msg_with_mapping 开始")
+    # print(f"🔍 ros_msg 类型: {type(ros_msg)}")
+    # print(f"🔍 ros_msg 内容: {ros_msg}")
+    # print(f"🔍 value_mapping: {value_mapping}")
+    # print("-" * 60)
 
     for msg_name, attr_name in value_mapping.items():
+    #    print(f"🔍 处理映射: {msg_name} -> {attr_name}")
+        
         msg_path = msg_name.split(".")
         current = ros_msg
-
+        
+        # print(f"🔍 msg_path: {msg_path}")
+        # print(f"🔍 current 初始值: {current} (类型: {type(current)})")
+        
         try:
             if not attr_name.endswith("[]"):
                 # 处理单值映射
-                for name in msg_path:
-                    current = getattr(current, name)
-                data[attr_name] = convert_from_ros_msg(current)
+                # print(f"🔍 处理单值映射")
+                for i, name in enumerate(msg_path):
+                    # print(f"🔍 步骤 {i}: 获取属性 '{name}' 从 {type(current)}")
+                    if hasattr(current, name):
+                        current = getattr(current, name)
+                        # print(f"🔍 获取到: {current} (类型: {type(current)})")
+                    else:
+                        # print(f"❌ 属性 '{name}' 不存在于 {type(current)}")
+                        break
+                
+                converted_value = convert_from_ros_msg(current)
+                # print(f"🔍 转换后的值: {converted_value} (类型: {type(converted_value)})")
+                data[attr_name] = converted_value
+                # print(f"✅ 设置 data['{attr_name}'] = {converted_value}")
             else:
                 # 处理列表值映射
-                for name in msg_path:
+                # print(f"🔍 处理列表值映射")
+                for i, name in enumerate(msg_path):
+                    # print(f"🔍 列表步骤 {i}: 处理 '{name}' 从 {type(current)}")
                     if name.endswith("[]"):
                         base_name = name[:-2]
+                        # print(f"🔍 数组字段 base_name: '{base_name}'")
                         if hasattr(current, base_name):
                             current = list(getattr(current, base_name))
+                            # print(f"🔍 获取数组: {current} (长度: {len(current)})")
                         else:
+                            # print(f"❌ 数组字段 '{base_name}' 不存在")
                             current = []
                             break
                     else:
                         if isinstance(current, list):
+                            # print(f"🔍 从列表中获取属性 '{name}'")
                             next_level = []
                             for item in current:
                                 if hasattr(item, name):
                                     next_level.append(getattr(item, name))
                             current = next_level
+                            # print(f"🔍 列表处理结果: {current} (长度: {len(current)})")
                         elif hasattr(current, name):
                             current = getattr(current, name)
+                            # print(f"🔍 获取到属性: {current} (类型: {type(current)})")
                         else:
+                            # print(f"❌ 属性 '{name}' 不存在")
                             current = []
                             break
 
                 attr_key = attr_name[:-2]
                 if current:
-                    data[attr_key] = [convert_from_ros_msg(item) for item in current]
-        except (AttributeError, TypeError):
+                    converted_list = [convert_from_ros_msg(item) for item in current]
+                    data[attr_key] = converted_list
+                    # print(f"✅ 设置 data['{attr_key}'] = {converted_list}")
+                else:
+                    print(f"⚠️ 列表为空，跳过 '{attr_key}'")
+        except (AttributeError, TypeError) as e:
+            # print(f"❌ 映射转换错误 {msg_name} -> {attr_name}: {e}")
             logger.debug(f"Mapping conversion error for {msg_name} -> {attr_name}")
             continue
+        
+    #    print(f"🔍 当前 data 状态: {data}")
+    #    print("-" * 40)
 
+    #print(f"🔍 convert_from_ros_msg_with_mapping 结束")
+    #print(f"🔍 最终 data: {data}")
+    #print("=" * 60)
     return data
 
 
@@ -571,30 +626,30 @@ from unilabos.utils.import_manager import ImportManager
 from unilabos.config.config import ROSConfig
 
 basic_type_map = {
-    'bool': {'type': 'boolean'},
-    'int8': {'type': 'integer', 'minimum': -128, 'maximum': 127},
-    'uint8': {'type': 'integer', 'minimum': 0, 'maximum': 255},
-    'int16': {'type': 'integer', 'minimum': -32768, 'maximum': 32767},
-    'uint16': {'type': 'integer', 'minimum': 0, 'maximum': 65535},
-    'int32': {'type': 'integer', 'minimum': -2147483648, 'maximum': 2147483647},
-    'uint32': {'type': 'integer', 'minimum': 0, 'maximum': 4294967295},
-    'int64': {'type': 'integer'},
-    'uint64': {'type': 'integer', 'minimum': 0},
-    'double': {'type': 'number'},
-    'float': {'type': 'number'},
-    'float32': {'type': 'number'},
-    'float64': {'type': 'number'},
-    'string': {'type': 'string'},
-    'boolean': {'type': 'boolean'},
-    'char': {'type': 'string', 'maxLength': 1},
-    'byte': {'type': 'integer', 'minimum': 0, 'maximum': 255},
+    "bool": {"type": "boolean"},
+    "int8": {"type": "integer", "minimum": -128, "maximum": 127},
+    "uint8": {"type": "integer", "minimum": 0, "maximum": 255},
+    "int16": {"type": "integer", "minimum": -32768, "maximum": 32767},
+    "uint16": {"type": "integer", "minimum": 0, "maximum": 65535},
+    "int32": {"type": "integer", "minimum": -2147483648, "maximum": 2147483647},
+    "uint32": {"type": "integer", "minimum": 0, "maximum": 4294967295},
+    "int64": {"type": "integer"},
+    "uint64": {"type": "integer", "minimum": 0},
+    "double": {"type": "number"},
+    "float": {"type": "number"},
+    "float32": {"type": "number"},
+    "float64": {"type": "number"},
+    "string": {"type": "string"},
+    "boolean": {"type": "boolean"},
+    "char": {"type": "string", "maxLength": 1},
+    "byte": {"type": "integer", "minimum": 0, "maximum": 255},
 }
 
 
-def ros_field_type_to_json_schema(type_info: Type | str, slot_type: str=None) -> Dict[str, Any]:
+def ros_field_type_to_json_schema(type_info: Type | str, slot_type: str = None) -> Dict[str, Any]:
     """
     将 ROS 字段类型转换为 JSON Schema 类型定义
-    
+
     Args:
         type_info: ROS 类型
         slot_type: ROS 类型
@@ -603,10 +658,7 @@ def ros_field_type_to_json_schema(type_info: Type | str, slot_type: str=None) ->
         对应的 JSON Schema 类型定义
     """
     if isinstance(type_info, UnboundedSequence):
-        return {
-            'type': 'array',
-            'items': ros_field_type_to_json_schema(type_info.value_type)
-        }
+        return {"type": "array", "items": ros_field_type_to_json_schema(type_info.value_type)}
     if isinstance(type_info, NamespacedType):
         cls_name = ".".join(type_info.namespaces) + ":" + type_info.name
         type_class = msg_converter_manager.get_class(cls_name)
@@ -614,20 +666,20 @@ def ros_field_type_to_json_schema(type_info: Type | str, slot_type: str=None) ->
     elif isinstance(type_info, BasicType):
         return ros_field_type_to_json_schema(type_info.typename)
     elif isinstance(type_info, UnboundedString):
-        return basic_type_map['string']
+        return basic_type_map["string"]
     elif isinstance(type_info, str):
         if type_info in basic_type_map:
             return basic_type_map[type_info]
 
         # 处理时间和持续时间类型
-        if type_info in ('time', 'duration', 'builtin_interfaces/Time', 'builtin_interfaces/Duration'):
+        if type_info in ("time", "duration", "builtin_interfaces/Time", "builtin_interfaces/Duration"):
             return {
-                'type': 'object',
-                'properties': {
-                    'sec': {'type': 'integer', 'description': '秒'},
-                    'nanosec': {'type': 'integer', 'description': '纳秒'}
+                "type": "object",
+                "properties": {
+                    "sec": {"type": "integer", "description": "秒"},
+                    "nanosec": {"type": "integer", "description": "纳秒"},
                 },
-                'required': ['sec', 'nanosec']
+                "required": ["sec", "nanosec"],
             }
     else:
         return ros_message_to_json_schema(type_info)
@@ -638,9 +690,7 @@ def ros_field_type_to_json_schema(type_info: Type | str, slot_type: str=None) ->
     #         'type': 'array',
     #         'items': ros_field_type_to_json_schema(item_type)
     #     }
-    
 
-    
     # # 处理复杂类型（尝试加载并处理）
     # try:
     #     # 如果它是一个完整的消息类型规范 (包名/msg/类型名)
@@ -655,34 +705,31 @@ def ros_field_type_to_json_schema(type_info: Type | str, slot_type: str=None) ->
     #     logger.debug(f"无法解析类型 {field_type}: {str(e)}")
     # return {'type': 'object', 'description': f'未知类型: {field_type}'}
 
+
 def ros_message_to_json_schema(msg_class: Any) -> Dict[str, Any]:
     """
     将 ROS 消息类转换为 JSON Schema
-    
+
     Args:
         msg_class: ROS 消息类
-        
+
     Returns:
         对应的 JSON Schema 定义
     """
-    schema = {
-        'type': 'object',
-        'properties': {},
-        'required': []
-    }
-    
+    schema = {"type": "object", "properties": {}, "required": []}
+
     # 获取类名作为标题
-    if hasattr(msg_class, '__name__'):
-        schema['title'] = msg_class.__name__
-    
+    if hasattr(msg_class, "__name__"):
+        schema["title"] = msg_class.__name__
+
     # 获取消息的字段和字段类型
     try:
         for ind, slot_info in enumerate(msg_class._fields_and_field_types.items()):
             slot_name, slot_type = slot_info
             type_info = msg_class.SLOT_TYPES[ind]
             field_schema = ros_field_type_to_json_schema(type_info, slot_type)
-            schema['properties'][slot_name] = field_schema
-            schema['required'].append(slot_name)
+            schema["properties"][slot_name] = field_schema
+            schema["required"].append(slot_name)
         # if hasattr(msg_class, 'get_fields_and_field_types'):
         #     fields_and_types = msg_class.get_fields_and_field_types()
         #
@@ -707,62 +754,66 @@ def ros_message_to_json_schema(msg_class: Any) -> Dict[str, Any]:
         #             schema['required'].append(clean_name)
     except Exception as e:
         # 如果获取字段类型失败，添加错误信息
-        schema['description'] = f"解析消息字段时出错: {str(e)}"
+        schema["description"] = f"解析消息字段时出错: {str(e)}"
         logger.error(f"解析 {msg_class.__name__} 消息字段失败: {str(e)}")
-    
+
     return schema
 
-def ros_action_to_json_schema(action_class: Any) -> Dict[str, Any]:
+
+def ros_action_to_json_schema(action_class: Any, description="") -> Dict[str, Any]:
     """
     将 ROS Action 类转换为 JSON Schema
-    
+
     Args:
         action_class: ROS Action 类
-        
+        description: 描述
+
     Returns:
         完整的 JSON Schema 定义
     """
-    if not hasattr(action_class, 'Goal') or not hasattr(action_class, 'Feedback') or not hasattr(action_class, 'Result'):
+    if (
+        not hasattr(action_class, "Goal")
+        or not hasattr(action_class, "Feedback")
+        or not hasattr(action_class, "Result")
+    ):
         raise ValueError(f"{action_class.__name__} 不是有效的 ROS Action 类")
-    
+
     # 创建基础 schema
     schema = {
-        '$schema': 'http://json-schema.org/draft-07/schema#',
-        'title': action_class.__name__,
-        'description': f"ROS Action {action_class.__name__} 的 JSON Schema",
-        'type': 'object',
-        'properties': {
-            'goal': {
-                'description': 'Action 目标 - 从客户端发送到服务器',
+        "title": action_class.__name__,
+        "description": description,
+        "type": "object",
+        "properties": {
+            "goal": {
+                # 'description': 'Action 目标 - 从客户端发送到服务器',
                 **ros_message_to_json_schema(action_class.Goal)
             },
-            'feedback': {
-                'description': 'Action 反馈 - 执行过程中从服务器发送到客户端',
+            "feedback": {
+                # 'description': 'Action 反馈 - 执行过程中从服务器发送到客户端',
                 **ros_message_to_json_schema(action_class.Feedback)
             },
-            'result': {
-                'description': 'Action 结果 - 完成后从服务器发送到客户端',
+            "result": {
+                # 'description': 'Action 结果 - 完成后从服务器发送到客户端',
                 **ros_message_to_json_schema(action_class.Result)
-            }
+            },
         },
-        'required': ['goal']
+        "required": ["goal"],
     }
-    
+
     return schema
 
+
 def convert_ros_action_to_jsonschema(
-    action_name_or_type: Union[str, Type],
-    output_file: Optional[str] = None,
-    format: str = 'json'
+    action_name_or_type: Union[str, Type], output_file: Optional[str] = None, format: str = "json"
 ) -> Dict[str, Any]:
     """
     将 ROS Action 类型转换为 JSON Schema，并可选地保存到文件
-    
+
     Args:
         action_name_or_type: ROS Action 类型名称或类
         output_file: 可选，输出 JSON Schema 的文件路径
         format: 输出格式，'json' 或 'yaml'
-        
+
     Returns:
         JSON Schema 定义（字典）
     """
@@ -772,21 +823,21 @@ def convert_ros_action_to_jsonschema(
         action_type = get_ros_type_by_msgname(action_name_or_type)
     else:
         action_type = action_name_or_type
-    
+
     # 生成 JSON Schema
     schema = ros_action_to_json_schema(action_type)
-    
+
     # 如果指定了输出文件，将 Schema 保存到文件
     if output_file:
-        if format.lower() == 'json':
-            with open(output_file, 'w', encoding='utf-8') as f:
+        if format.lower() == "json":
+            with open(output_file, "w", encoding="utf-8") as f:
                 json.dump(schema, f, indent=2, ensure_ascii=False)
-        elif format.lower() == 'yaml':
-            with open(output_file, 'w', encoding='utf-8') as f:
+        elif format.lower() == "yaml":
+            with open(output_file, "w", encoding="utf-8") as f:
                 yaml.safe_dump(schema, f, default_flow_style=False, allow_unicode=True)
         else:
             raise ValueError(f"不支持的格式: {format}，请使用 'json' 或 'yaml'")
-    
+
     return schema
 
 
@@ -795,14 +846,14 @@ if __name__ == "__main__":
     # 示例：转换 NavigateToPose action
     try:
         from nav2_msgs.action import NavigateToPose
-        
+
         # 转换为 JSON Schema 并打印
         schema = convert_ros_action_to_jsonschema(NavigateToPose)
         print(json.dumps(schema, indent=2, ensure_ascii=False))
-        
+
         # 保存到文件
         # convert_ros_action_to_jsonschema(NavigateToPose, "navigate_to_pose_schema.json")
-        
+
         # 或者使用字符串形式的 action 名称
         # schema = convert_ros_action_to_jsonschema("nav2_msgs/action/NavigateToPose")
     except ImportError:

unilabos/ros/nodes/base_device_node.py

@@ -1,4 +1,5 @@
 import copy
+import io
 import json
 import threading
 import time
@@ -10,6 +11,7 @@ from concurrent.futures import ThreadPoolExecutor
 import asyncio
 
 import rclpy
+import yaml
 from rclpy.node import Node
 from rclpy.action import ActionServer, ActionClient
 from rclpy.action.server import ServerGoalHandle
@@ -166,7 +168,10 @@ class PropertyPublisher:
         self.print_publish = print_publish
 
         self._value = None
-        self.publisher_ = node.create_publisher(msg_type, f"{name}", 10)
+        try:
+            self.publisher_ = node.create_publisher(msg_type, f"{name}", 10)
+        except AttributeError as ex:
+            logger.error(f"创建发布者失败，可能由于注册表有误，类型: {msg_type}，错误: {ex}\n{traceback.format_exc()}")
         self.timer = node.create_timer(self.timer_period, self.publish_property)
         self.__loop = get_event_loop()
         str_msg_type = str(msg_type)[8:-2]
@@ -302,6 +307,8 @@ class BaseROS2DeviceNode(Node, Generic[T]):
         # 创建动作服务
         if self.create_action_server:
             for action_name, action_value_mapping in self._action_value_mappings.items():
+                if action_name.startswith("auto-") or str(action_value_mapping.get("type", "")).startswith("UniLabJsonCommand"):
+                    continue
                 self.create_ros_action_server(action_name, action_value_mapping)
 
         # 创建线程池执行器
@@ -622,7 +629,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
             execution_success = False
             action_return_value = None
 
-            self.lab_logger().info(f"执行动作: {action_name}")
+        #####    self.lab_logger().info(f"执行动作: {action_name}")
             goal = goal_handle.request
 
             # 从目标消息中提取参数, 并调用对应的方法
@@ -649,15 +656,18 @@ class BaseROS2DeviceNode(Node, Generic[T]):
                     if v in ["unilabos_msgs/Resource", "sequence<unilabos_msgs/Resource>"]:
                         self.lab_logger().info(f"查询资源状态: Key: {k} Type: {v}")
                         current_resources = []
+                        # TODO: resource后面需要分组
+                        only_one_resource = False
                         try:
                             if len(action_kwargs[k]) > 1:
                                 for i in action_kwargs[k]:
                                     r = ResourceGet.Request()
-                                    r.id = i["id"]
+                                    r.id = i["id"]  # splash optional
                                     r.with_children = True
                                     response = await self._resource_clients["resource_get"].call_async(r)
                                     current_resources.extend(response.resources)
                             else:
+                                only_one_resource = True
                                 r = ResourceGet.Request()
                                 r.id = (
                                     action_kwargs[k]["id"]
@@ -675,17 +685,20 @@ class BaseROS2DeviceNode(Node, Generic[T]):
                         type_hint = action_paramtypes[k]
                         final_type = get_type_class(type_hint)
                         # 判断 ACTION 是否需要特殊的物料类型如 pylabrobot.resources.Resource，并做转换
-                        final_resource = [convert_resources_to_type([i], final_type)[0] for i in resources_list]
+                        if only_one_resource:
+                            final_resource = convert_resources_to_type(resources_list, final_type)
+                        else:
+                            final_resource = [convert_resources_to_type([i], final_type)[0] for i in resources_list]
                         action_kwargs[k] = self.resource_tracker.figure_resource(final_resource)
 
-            self.lab_logger().info(f"准备执行: {action_kwargs}, 函数: {ACTION.__name__}")
+            ##### self.lab_logger().info(f"准备执行: {action_kwargs}, 函数: {ACTION.__name__}")
             time_start = time.time()
             time_overall = 100
 
             # 将阻塞操作放入线程池执行
             if asyncio.iscoroutinefunction(ACTION):
                 try:
-                    self.lab_logger().info(f"异步执行动作 {ACTION}")
+                    ##### self.lab_logger().info(f"异步执行动作 {ACTION}")
                     future = ROS2DeviceNode.run_async_func(ACTION, trace_error=False, **action_kwargs)
 
                     def _handle_future_exception(fut):
@@ -695,7 +708,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
                             execution_success = True
                         except Exception as e:
                             execution_error = traceback.format_exc()
-                            error(f"异步任务 {ACTION.__name__} 报错了")
+                            ##### error(f"异步任务 {ACTION.__name__} 报错了")
                             error(traceback.format_exc())
 
                     future.add_done_callback(_handle_future_exception)
@@ -703,7 +716,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
                     self.lab_logger().error(f"创建异步任务失败: {traceback.format_exc()}")
                     raise e
             else:
-                self.lab_logger().info(f"同步执行动作 {ACTION}")
+            #####    self.lab_logger().info(f"同步执行动作 {ACTION}")
                 future = self._executor.submit(ACTION, **action_kwargs)
 
                 def _handle_future_exception(fut):
@@ -758,7 +771,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
                 self.lab_logger().info(f"动作 {action_name} 已取消")
                 return action_type.Result()
 
-            self.lab_logger().info(f"动作执行完成: {action_name}")
+            ##### self.lab_logger().info(f"动作执行完成: {action_name}")
             del future
 
             # 向Host更新物料当前状态
@@ -794,7 +807,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
 
             # 发布结果
             goal_handle.succeed()
-            self.lab_logger().info(f"设置动作成功: {action_name}")
+            ##### self.lab_logger().info(f"设置动作成功: {action_name}")
 
             result_values = {}
             for msg_name, attr_name in action_value_mapping["result"].items():
@@ -813,7 +826,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
                 elif attr_name == "return_info":
                     setattr(result_msg, attr_name, serialize_result_info(execution_error, execution_success, action_return_value))
 
-            self.lab_logger().info(f"动作 {action_name} 完成并返回结果")
+            ##### self.lab_logger().info(f"动作 {action_name} 完成并返回结果")
             return result_msg
 
         return execute_callback
@@ -838,6 +851,8 @@ class BaseROS2DeviceNode(Node, Generic[T]):
 class DeviceInitError(Exception):
     pass
 
+class JsonCommandInitError(Exception):
+    pass
 
 class ROS2DeviceNode:
     """
@@ -914,11 +929,18 @@ class ROS2DeviceNode:
             driver_class.__module__.startswith("pylabrobot")
             or driver_class.__name__ == "LiquidHandlerAbstract"
             or driver_class.__name__ == "LiquidHandlerBiomek"
+            or driver_class.__name__ == "PRCXI9300Handler"
         )
 
         # TODO: 要在创建之前预先请求服务器是否有当前id的物料，放到resource_tracker中，让pylabrobot进行创建
         # 创建设备类实例
         if use_pylabrobot_creator:
+            # 先对pylabrobot的子资源进行加载，不然subclass无法认出
+            # 在下方对于加载Deck等Resource要手动import
+            # noinspection PyUnresolvedReferences
+            from unilabos.devices.liquid_handling.prcxi.prcxi import PRCXI9300Deck
+            # noinspection PyUnresolvedReferences
+            from unilabos.devices.liquid_handling.prcxi.prcxi import PRCXI9300Container
             self._driver_creator = PyLabRobotCreator(
                 driver_class, children=children, resource_tracker=self.resource_tracker
             )
@@ -954,12 +976,51 @@ class ROS2DeviceNode:
         self._ros_node: BaseROS2DeviceNode
         self._ros_node.lab_logger().info(f"初始化完成 {self._ros_node.uuid} {self.driver_is_ros}")
         self.driver_instance._ros_node = self._ros_node  # type: ignore
+        self.driver_instance._execute_driver_command = self._execute_driver_command  # type: ignore
+        self.driver_instance._execute_driver_command_async = self._execute_driver_command_async  # type: ignore
         if hasattr(self.driver_instance, "post_init"):
             try:
                 self.driver_instance.post_init(self._ros_node)  # type: ignore
             except Exception as e:
                 self._ros_node.lab_logger().error(f"设备后初始化失败: {e}")
 
+    def _execute_driver_command(self, string: str):
+        try:
+            target = json.loads(string)
+        except Exception as ex:
+            try:
+                target = yaml.safe_load(io.StringIO(string))
+            except Exception as ex2:
+                raise JsonCommandInitError(f"执行动作时JSON/YAML解析失败: \n{ex}\n{ex2}\n原内容: {string}\n{traceback.format_exc()}")
+        try:
+            function_name = target["function_name"]
+            function_args = target["function_args"]
+            assert isinstance(function_args, dict), "执行动作时JSON必须为dict类型\n原JSON: {string}"
+            function = getattr(self.driver_instance, function_name)
+            assert callable(function), f"执行动作时JSON中的function_name对应的函数不可调用: {function_name}\n原JSON: {string}"
+            return function(**function_args)
+        except KeyError as ex:
+            raise JsonCommandInitError(f"执行动作时JSON缺少function_name或function_args: {ex}\n原JSON: {string}\n{traceback.format_exc()}")
+
+    async def _execute_driver_command_async(self, string: str):
+        try:
+            target = json.loads(string)
+        except Exception as ex:
+            try:
+                target = yaml.safe_load(io.StringIO(string))
+            except Exception as ex2:
+                raise JsonCommandInitError(f"执行动作时JSON/YAML解析失败: \n{ex}\n{ex2}\n原内容: {string}\n{traceback.format_exc()}")
+        try:
+            function_name = target["function_name"]
+            function_args = target["function_args"]
+            assert isinstance(function_args, dict), "执行动作时JSON必须为dict类型\n原JSON: {string}"
+            function = getattr(self.driver_instance, function_name)
+            assert callable(function), f"执行动作时JSON中的function_name对应的函数不可调用: {function_name}\n原JSON: {string}"
+            assert asyncio.iscoroutinefunction(function), f"执行动作时JSON中的function并非异步: {function_name}\n原JSON: {string}"
+            return await function(**function_args)
+        except KeyError as ex:
+            raise JsonCommandInitError(f"执行动作时JSON缺少function_name或function_args: {ex}\n原JSON: {string}\n{traceback.format_exc()}")
+
     def _start_loop(self):
         def run_event_loop():
             loop = asyncio.new_event_loop()

unilabos/ros/nodes/presets/camera.py

@@ -0,0 +1,61 @@
+import rclpy
+from rclpy.node import Node
+import cv2
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge
+from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode, DeviceNodeResourceTracker
+
+class VideoPublisher(BaseROS2DeviceNode):
+    def __init__(self, device_id='video_publisher', camera_index=0, period: float = 0.1, resource_tracker: DeviceNodeResourceTracker = None):
+        # 初始化BaseROS2DeviceNode，使用自身作为driver_instance
+        BaseROS2DeviceNode.__init__(
+            self,
+            driver_instance=self,
+            device_id=device_id,
+            status_types={},
+            action_value_mappings={},
+            hardware_interface="camera",
+            print_publish=False,
+            resource_tracker=resource_tracker,
+        )
+        # 创建一个发布者，发布到 /video 话题，消息类型为 sensor_msgs/Image，队列长度设为 10
+        self.publisher_ = self.create_publisher(Image, f'/{device_id}/video', 10)
+        # 初始化摄像头（默认设备索引为 0）
+        self.cap = cv2.VideoCapture(camera_index)
+        if not self.cap.isOpened():
+            self.get_logger().error("无法打开摄像头")
+        # 用于将 OpenCV 的图像转换为 ROS 图像消息
+        self.bridge = CvBridge()
+        # 设置定时器，10 Hz 发布一次
+        timer_period = period  # 单位：秒
+        self.timer = self.create_timer(timer_period, self.timer_callback)
+
+    def timer_callback(self):
+        ret, frame = self.cap.read()
+        if not ret:
+            self.get_logger().error("读取视频帧失败")
+            return
+        # 将 OpenCV 图像转换为 ROS Image 消息，注意图像编码需与摄像头数据匹配，这里使用 bgr8
+        img_msg = self.bridge.cv2_to_imgmsg(frame, encoding="bgr8")
+        self.publisher_.publish(img_msg)
+        # self.get_logger().info("已发布视频帧")
+
+    def destroy_node(self):
+        # 释放摄像头资源
+        if self.cap.isOpened():
+            self.cap.release()
+        super().destroy_node()
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = VideoPublisher()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

unilabos/ros/nodes/presets/host_node.py

@@ -459,6 +459,8 @@ class HostNode(BaseROS2DeviceNode):
         self.devices_instances[device_id] = d
         # noinspection PyProtectedMember
         for action_name, action_value_mapping in d._ros_node._action_value_mappings.items():
+            if action_name.startswith("auto-") or str(action_value_mapping.get("type", "")).startswith("UniLabJsonCommand"):
+                continue
             action_id = f"/devices/{device_id}/{action_name}"
             if action_id not in self._action_clients:
                 action_type = action_value_mapping["type"]
@@ -561,12 +563,13 @@ class HostNode(BaseROS2DeviceNode):
                     if hasattr(bridge, "publish_device_status"):
                         bridge.publish_device_status(self.device_status, device_id, property_name)
                         self.lab_logger().debug(
-                            f"[Host Node] Status updated: {device_id}.{property_name} = {msg.data}"
+                           f"[Host Node] Status updated: {device_id}.{property_name} = {msg.data}"
                         )
 
     def send_goal(
         self,
         device_id: str,
+        action_type: str,
         action_name: str,
         action_kwargs: Dict[str, Any],
         goal_uuid: Optional[str] = None,
@@ -577,16 +580,30 @@ class HostNode(BaseROS2DeviceNode):
 
         Args:
             device_id: 设备ID
+            action_type: 动作类型
             action_name: 动作名称
             action_kwargs: 动作参数
             goal_uuid: 目标UUID，如果为None则自动生成
+            server_info: 服务器发送信息，包含发送时间戳等
         """
-        action_id = f"/devices/{device_id}/{action_name}"
+        if action_type.startswith("UniLabJsonCommand"):
+            if action_name.startswith("auto-"):
+                action_name = action_name[5:]
+            action_id = f"/devices/{device_id}/_execute_driver_command"
+            action_kwargs = {
+                "string": json.dumps({
+                    "function_name": action_name,
+                    "function_args": action_kwargs,
+                })
+            }
+            if action_type.startswith("UniLabJsonCommandAsync"):
+                action_id = f"/devices/{device_id}/_execute_driver_command_async"
+        else:
+            action_id = f"/devices/{device_id}/{action_name}"
         if action_name == "test_latency" and server_info is not None:
             self.server_latest_timestamp = server_info.get("send_timestamp", 0.0)
         if action_id not in self._action_clients:
-            self.lab_logger().error(f"[Host Node] ActionClient {action_id} not found.")
-            return
+            raise ValueError(f"ActionClient {action_id} not found.")
 
         action_client: ActionClient = self._action_clients[action_id]
 

unilabos/ros/nodes/presets/protocol_node.py

@@ -1,7 +1,5 @@
 import time
-import asyncio
 import traceback
-from types import MethodType
 from typing import Union
 
 import rclpy
@@ -19,9 +17,11 @@ from unilabos.ros.msgs.message_converter import (
     get_action_type,
     convert_to_ros_msg,
     convert_from_ros_msg,
-    convert_from_ros_msg_with_mapping,
+    convert_from_ros_msg_with_mapping, String,
 )
 from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode, DeviceNodeResourceTracker, ROS2DeviceNode
+from unilabos.utils.log import error
+from unilabos.utils.type_check import serialize_result_info
 
 
 class ROS2ProtocolNode(BaseROS2DeviceNode):
@@ -33,7 +33,15 @@ class ROS2ProtocolNode(BaseROS2DeviceNode):
 
     # create_action_server = False  # Action Server要自己创建
 
-    def __init__(self, device_id: str, children: dict, protocol_type: Union[str, list[str]], resource_tracker: DeviceNodeResourceTracker, *args, **kwargs):
+    def __init__(
+        self,
+        device_id: str,
+        children: dict,
+        protocol_type: Union[str, list[str]],
+        resource_tracker: DeviceNodeResourceTracker,
+        *args,
+        **kwargs,
+    ):
         self._setup_protocol_names(protocol_type)
 
         # 初始化其它属性
@@ -60,12 +68,14 @@ class ROS2ProtocolNode(BaseROS2DeviceNode):
 
         for device_id, device_config in self.children.items():
             if device_config.get("type", "device") != "device":
-                self.lab_logger().debug(f"[Protocol Node] Skipping type {device_config['type']} {device_id} already existed, skipping.")
+                self.lab_logger().debug(
+                    f"[Protocol Node] Skipping type {device_config['type']} {device_id} already existed, skipping."
+                )
                 continue
             try:
                 d = self.initialize_device(device_id, device_config)
             except Exception as ex:
-                self.lab_logger().error(f"[Protocol Node] Failed to initialize device {device_id}: {ex}")
+                self.lab_logger().error(f"[Protocol Node] Failed to initialize device {device_id}: {ex}\n{traceback.format_exc()}")
                 d = None
             if d is None:
                 continue
@@ -76,22 +86,27 @@ class ROS2ProtocolNode(BaseROS2DeviceNode):
 
             # 设置硬件接口代理
             if d:
+                hardware_interface = d.ros_node_instance._hardware_interface
                 if (
-                    hasattr(d.driver_instance, d.ros_node_instance._hardware_interface["name"])
-                    and hasattr(d.driver_instance, d.ros_node_instance._hardware_interface["write"])
-                    and (d.ros_node_instance._hardware_interface["read"] is None or hasattr(d.driver_instance, d.ros_node_instance._hardware_interface["read"]))
+                    hasattr(d.driver_instance, hardware_interface["name"])
+                    and hasattr(d.driver_instance, hardware_interface["write"])
+                    and (hardware_interface["read"] is None or hasattr(d.driver_instance, hardware_interface["read"]))
                 ):
 
-                    name = getattr(d.driver_instance, d.ros_node_instance._hardware_interface["name"])
-                    read = d.ros_node_instance._hardware_interface.get("read", None)
-                    write = d.ros_node_instance._hardware_interface.get("write", None)
+                    name = getattr(d.driver_instance, hardware_interface["name"])
+                    read = hardware_interface.get("read", None)
+                    write = hardware_interface.get("write", None)
 
                     # 如果硬件接口是字符串，通过通信设备提供
                     if isinstance(name, str) and name in self.sub_devices:
                         communicate_device = self.sub_devices[name]
                         communicate_hardware_info = communicate_device.ros_node_instance._hardware_interface
                         self._setup_hardware_proxy(d, self.sub_devices[name], read, write)
-                        self.lab_logger().info(f"\n通信代理：为子设备{device_id}\n    添加了{read}方法(来源：{name} {communicate_hardware_info['write']}) \n    添加了{write}方法(来源：{name} {communicate_hardware_info['read']})")
+                        self.lab_logger().info(
+                            f"\n通信代理：为子设备{device_id}\n    "
+                            f"添加了{read}方法(来源：{name} {communicate_hardware_info['write']}) \n    "
+                            f"添加了{write}方法(来源：{name} {communicate_hardware_info['read']})"
+                        )
 
     def _setup_protocol_names(self, protocol_type):
         # 处理协议类型
@@ -119,11 +134,17 @@ class ROS2ProtocolNode(BaseROS2DeviceNode):
         if d is not None and hasattr(d, "ros_node_instance"):
             node = d.ros_node_instance
             for action_name, action_mapping in node._action_value_mappings.items():
+                if action_name.startswith("auto-") or str(action_mapping.get("type", "")).startswith("UniLabJsonCommand"):
+                    continue
                 action_id = f"/devices/{device_id_abs}/{action_name}"
                 if action_id not in self._action_clients:
-                    self._action_clients[action_id] = ActionClient(
-                        self, action_mapping["type"], action_id, callback_group=self.callback_group
-                    )
+                    try:
+                        self._action_clients[action_id] = ActionClient(
+                            self, action_mapping["type"], action_id, callback_group=self.callback_group
+                        )
+                    except Exception as ex:
+                        self.lab_logger().error(f"创建动作客户端失败: {action_id}, 错误: {ex}")
+                        continue
                     self.lab_logger().debug(f"为子设备 {device_id} 创建动作客户端: {action_name}")
         return d
 
@@ -149,63 +170,135 @@ class ROS2ProtocolNode(BaseROS2DeviceNode):
     def _create_protocol_execute_callback(self, protocol_name, protocol_steps_generator):
         async def execute_protocol(goal_handle: ServerGoalHandle):
             """执行完整的工作流"""
-            self.get_logger().info(f'Executing {protocol_name} action...')
+            # 初始化结果信息变量
+            execution_error = ""
+            execution_success = False
+            protocol_return_value = None
+            self.get_logger().info(f"Executing {protocol_name} action...")
             action_value_mapping = self._action_value_mappings[protocol_name]
-            print('+'*30)
-            print(protocol_steps_generator)
-            # 从目标消息中提取参数, 并调用Protocol生成器(根据设备连接图)生成action步骤
-            goal = goal_handle.request
-            protocol_kwargs = convert_from_ros_msg_with_mapping(goal, action_value_mapping["goal"])
+            try:
+                print("+" * 30)
+                print(protocol_steps_generator)
+                # 从目标消息中提取参数, 并调用Protocol生成器(根据设备连接图)生成action步骤
+                goal = goal_handle.request
+                protocol_kwargs = convert_from_ros_msg_with_mapping(goal, action_value_mapping["goal"])
+                
+                # # 🔧 添加调试信息
+                # print(f"🔍 转换后的 protocol_kwargs: {protocol_kwargs}")
+                # print(f"🔍 vessel 在转换后: {protocol_kwargs.get('vessel', 'NOT_FOUND')}")
 
-            # 向Host查询物料当前状态
-            for k, v in goal.get_fields_and_field_types().items():
-                if v in ["unilabos_msgs/Resource", "sequence<unilabos_msgs/Resource>"]:
-                    r = ResourceGet.Request()
-                    r.id = protocol_kwargs[k]["id"] if v == "unilabos_msgs/Resource" else protocol_kwargs[k][0]["id"]
-                    r.with_children = True
-                    response = await self._resource_clients["resource_get"].call_async(r)
-                    protocol_kwargs[k] = list_to_nested_dict([convert_from_ros_msg(rs) for rs in response.resources])
+                # # 🔧 完全禁用Host查询，直接使用转换后的数据
+                # print(f"🔧 跳过Host查询，直接使用转换后的数据")
+                # 向Host查询物料当前状态
+                for k, v in goal.get_fields_and_field_types().items():
+                    if v in ["unilabos_msgs/Resource", "sequence<unilabos_msgs/Resource>"]:
+                        r = ResourceGet.Request()
+                        resource_id = (
+                            protocol_kwargs[k]["id"] if v == "unilabos_msgs/Resource" else protocol_kwargs[k][0]["id"]
+                        )
+                        r.id = resource_id
+                        r.with_children = True
+                        response = await self._resource_clients["resource_get"].call_async(r)
+                        protocol_kwargs[k] = list_to_nested_dict(
+                            [convert_from_ros_msg(rs) for rs in response.resources]
+                        )
 
-            from unilabos.resources.graphio import physical_setup_graph
-            self.get_logger().info(f'Working on physical setup: {physical_setup_graph}')
-            protocol_steps = protocol_steps_generator(G=physical_setup_graph, **protocol_kwargs)
+                self.lab_logger().info(f"🔍 最终传递给协议的 protocol_kwargs: {protocol_kwargs}")
+                self.lab_logger().info(f"🔍 最终的 vessel: {protocol_kwargs.get('vessel', 'NOT_FOUND')}")
 
-            self.get_logger().info(f'Goal received: {protocol_kwargs}, running steps: \n{protocol_steps}')
+                from unilabos.resources.graphio import physical_setup_graph
 
-            time_start = time.time()
-            time_overall = 100
-            self._busy = True
+                self.lab_logger().info(f"Working on physical setup: {physical_setup_graph}")
+                protocol_steps = protocol_steps_generator(G=physical_setup_graph, **protocol_kwargs)
+                
+                self.lab_logger().info(f"Goal received: {protocol_kwargs}, running steps: \n{protocol_steps}")
 
-            # 逐步执行工作流
-            for i, action in enumerate(protocol_steps):
-                self.get_logger().info(f'Running step {i+1}: {action}')
-                if type(action) == dict:
-                    # 如果是单个动作，直接执行
-                    if action["action_name"] == "wait":
-                        time.sleep(action["action_kwargs"]["time"])
-                    else:
-                        result = await self.execute_single_action(**action)
-                elif type(action) == list:
-                    # 如果是并行动作，同时执行
-                    actions = action
-                    futures = [rclpy.get_global_executor().create_task(self.execute_single_action(**a)) for a in actions]
-                    results = [await f for f in futures]
+                time_start = time.time()
+                time_overall = 100
+                self._busy = True
 
-            # 向Host更新物料当前状态
-            for k, v in goal.get_fields_and_field_types().items():
-                if v in ["unilabos_msgs/Resource", "sequence<unilabos_msgs/Resource>"]:
-                    r = ResourceUpdate.Request()
-                    r.resources = [
-                        convert_to_ros_msg(Resource, rs) for rs in nested_dict_to_list(protocol_kwargs[k])
-                    ]
-                    response = await self._resource_clients["resource_update"].call_async(r)
+                # 逐步执行工作流
+                step_results = []
+                for i, action in enumerate(protocol_steps):
+                    # self.get_logger().info(f"Running step {i + 1}: {action}")
+                    if isinstance(action, dict):
+                        # 如果是单个动作，直接执行
+                        if action["action_name"] == "wait":
+                            time.sleep(action["action_kwargs"]["time"])
+                            step_results.append({"step": i + 1, "action": "wait", "result": "completed"})
+                        else:
+                            result = await self.execute_single_action(**action)
+                            step_results.append({"step": i + 1, "action": action["action_name"], "result": result})
+                    elif isinstance(action, list):
+                        # 如果是并行动作，同时执行
+                        actions = action
+                        futures = [
+                            rclpy.get_global_executor().create_task(self.execute_single_action(**a)) for a in actions
+                        ]
+                        results = [await f for f in futures]
+                        step_results.append(
+                            {
+                                "step": i + 1,
+                                "parallel_actions": [a["action_name"] for a in actions],
+                                "results": results,
+                            }
+                        )
 
-            goal_handle.succeed()
+                # 向Host更新物料当前状态
+                for k, v in goal.get_fields_and_field_types().items():
+                    if v in ["unilabos_msgs/Resource", "sequence<unilabos_msgs/Resource>"]:
+                        r = ResourceUpdate.Request()
+                        r.resources = [
+                            convert_to_ros_msg(Resource, rs) for rs in nested_dict_to_list(protocol_kwargs[k])
+                        ]
+                        response = await self._resource_clients["resource_update"].call_async(r)
+
+                # 设置成功状态和返回值
+                execution_success = True
+                protocol_return_value = {
+                    "protocol_name": protocol_name,
+                    "steps_executed": len(protocol_steps),
+                    "step_results": step_results,
+                    "total_time": time.time() - time_start,
+                }
+
+                goal_handle.succeed()
+
+            except Exception as e:
+                # 捕获并记录错误信息
+                execution_error = traceback.format_exc()
+                execution_success = False
+                error(f"协议 {protocol_name} 执行失败")
+                error(traceback.format_exc())
+                self.lab_logger().error(f"协议执行出错: {str(e)}")
+
+                # 设置动作失败
+                goal_handle.abort()
+
+            finally:
+                self._busy = False
+
+            # 创建结果消息
             result = action_value_mapping["type"].Result()
-            result.success = True
+            result.success = execution_success
 
-            self._busy = False
+            # 获取结果消息类型信息，检查是否有return_info字段
+            result_msg_types = action_value_mapping["type"].Result.get_fields_and_field_types()
+
+            # 设置return_info字段（如果存在）
+            for attr_name in result_msg_types.keys():
+                if attr_name in ["success", "reached_goal"]:
+                    setattr(result, attr_name, execution_success)
+                elif attr_name == "return_info":
+                    setattr(
+                        result,
+                        attr_name,
+                        serialize_result_info(execution_error, execution_success, protocol_return_value),
+                    )
+
+            self.lab_logger().info(f"🤩🤩🤩🤩🤩🤩协议 {protocol_name} 完成并返回结果😎😎😎😎😎😎")
             return result
+
         return execute_protocol
 
     async def execute_single_action(self, device_id, action_name, action_kwargs):
@@ -225,7 +318,7 @@ class ROS2ProtocolNode(BaseROS2DeviceNode):
         action_client = self._action_clients[action_id]
         goal_msg = convert_to_ros_msg(action_client._action_type.Goal(), action_kwargs)
 
-        self.lab_logger().info(f"发送动作请求到: {action_id}")
+        ##### self.lab_logger().info(f"发送动作请求到: {action_id}")
         action_client.wait_for_server()
 
         # 等待动作完成
@@ -237,18 +330,23 @@ class ROS2ProtocolNode(BaseROS2DeviceNode):
             return None
 
         result_future = await handle.get_result_async()
-        self.lab_logger().info(f"动作完成: {action_name}")
+        ##### self.lab_logger().info(f"动作完成: {action_name}")
 
         return result_future.result
 
-
     """还没有改过的部分"""
 
-    def _setup_hardware_proxy(self, device: ROS2DeviceNode, communication_device: ROS2DeviceNode, read_method, write_method):
+    def _setup_hardware_proxy(
+        self, device: ROS2DeviceNode, communication_device: ROS2DeviceNode, read_method, write_method
+    ):
         """为设备设置硬件接口代理"""
         # extra_info = [getattr(device.driver_instance, info) for info in communication_device.ros_node_instance._hardware_interface.get("extra_info", [])]
-        write_func = getattr(communication_device.driver_instance, communication_device.ros_node_instance._hardware_interface["write"])
-        read_func = getattr(communication_device.driver_instance, communication_device.ros_node_instance._hardware_interface["read"])
+        write_func = getattr(
+            communication_device.driver_instance, communication_device.ros_node_instance._hardware_interface["write"]
+        )
+        read_func = getattr(
+            communication_device.driver_instance, communication_device.ros_node_instance._hardware_interface["read"]
+        )
 
         def _read(*args, **kwargs):
             return read_func(*args, **kwargs)
@@ -264,7 +362,6 @@ class ROS2ProtocolNode(BaseROS2DeviceNode):
             # bound_write = MethodType(_write, device.driver_instance)
             setattr(device.driver_instance, write_method, _write)
 
-
     async def _update_resources(self, goal, protocol_kwargs):
         """更新资源状态"""
         for k, v in goal.get_fields_and_field_types().items():

unilabos/ros/nodes/resource_tracker.py

@@ -10,6 +10,15 @@ class DeviceNodeResourceTracker(object):
         self.resource2parent_resource = {}
         pass
 
+    def prefix_path(self, resource):
+        resource_prefix_path = "/"
+        resource_parent = getattr(resource, "parent", None)
+        while resource_parent is not None:
+            resource_prefix_path = f"/{resource_parent.name}" + resource_prefix_path
+            resource_parent = resource_parent.parent
+
+        return resource_prefix_path
+
     def parent_resource(self, resource):
         if id(resource) in self.resource2parent_resource:
             return self.resource2parent_resource[id(resource)]

unilabos/ros/utils/driver_creator.py

@@ -148,7 +148,7 @@ class PyLabRobotCreator(DeviceClassCreator[T]):
                             contain_model = not issubclass(target_type, Deck)
                             resource, target_type = self._process_resource_mapping(resource, target_type)
                             resource_instance: Resource = resource_ulab_to_plr(resource, contain_model)
-
+                            states[prefix_path] = resource_instance.serialize_all_state()
                             # 使用 prefix_path 作为 key 存储资源状态
                             if to_dict:
                                 serialized = resource_instance.serialize()
@@ -199,7 +199,7 @@ class PyLabRobotCreator(DeviceClassCreator[T]):
             spect = inspect.signature(deserialize_method)
             spec_args = spect.parameters
             for param_name, param_value in data.copy().items():
-                if "_resource_child_name" in param_value and "_resource_type" not in param_value:
+                if isinstance(param_value, dict) and "_resource_child_name" in param_value and "_resource_type" not in param_value:
                     arg_value = spec_args[param_name].annotation
                     data[param_name]["_resource_type"] = self.device_cls.__module__ + ":" + arg_value
                     logger.debug(f"自动补充 _resource_type: {data[param_name]['_resource_type']}")
@@ -230,7 +230,7 @@ class PyLabRobotCreator(DeviceClassCreator[T]):
                 spect = inspect.signature(self.device_cls.__init__)
                 spec_args = spect.parameters
                 for param_name, param_value in data.copy().items():
-                    if "_resource_child_name" in param_value and "_resource_type" not in param_value:
+                    if isinstance(param_value, dict) and "_resource_child_name" in param_value and "_resource_type" not in param_value:
                         arg_value = spec_args[param_name].annotation
                         data[param_name]["_resource_type"] = self.device_cls.__module__ + ":" + arg_value
                         logger.debug(f"自动补充 _resource_type: {data[param_name]['_resource_type']}")