new registry sys

exp. support with add device

unilabos/ros/msgs/message_converter.py

@@ -11,6 +11,7 @@ from io import StringIO
 from typing import Iterable, Any, Dict, Type, TypeVar, Union
 
 import yaml
+from msgcenterpy.instances.ros2_instance import ROS2MessageInstance
 from pydantic import BaseModel
 from dataclasses import asdict, is_dataclass
 
@@ -727,46 +728,9 @@ def ros_message_to_json_schema(msg_class: Any, field_name: str) -> Dict[str, Any
     Returns:
         对应的 JSON Schema 定义
     """
-    schema = {"type": "object", "properties": {}, "required": []}
-
-    # 优先使用字段名作为标题，否则使用类名
+    schema = ROS2MessageInstance(msg_class()).get_json_schema()
     schema["title"] = field_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_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()
-        #
-        #     for field_name, field_type in fields_and_types.items():
-        #         # 将 ROS 字段类型转换为 JSON Schema
-        #         field_schema = ros_field_type_to_json_schema(field_type)
-        #
-        #         schema['properties'][field_name] = field_schema
-        #         schema['required'].append(field_name)
-        # elif hasattr(msg_class, '__slots__') and hasattr(msg_class, '_fields_and_field_types'):
-        #     # 直接从实例属性获取
-        #     for field_name in msg_class.__slots__:
-        #         # 移除前导下划线（如果有）
-        #         clean_name = field_name[1:] if field_name.startswith('_') else field_name
-        #
-        #         # 从 _fields_and_field_types 获取类型
-        #         if clean_name in msg_class._fields_and_field_types:
-        #             field_type = msg_class._fields_and_field_types[clean_name]
-        #             field_schema = ros_field_type_to_json_schema(field_type)
-        #
-        #             schema['properties'][clean_name] = field_schema
-        #             schema['required'].append(clean_name)
-    except Exception as e:
-        # 如果获取字段类型失败，添加错误信息
-        schema["description"] = f"解析消息字段时出错: {str(e)}"
-        logger.error(f"解析 {msg_class.__name__} 消息字段失败: {str(e)}")
-
+    schema.pop("description")
     return schema
 
 

unilabos/ros/nodes/base_device_node.py

@@ -34,7 +34,8 @@ from unilabos_msgs.action import SendCmd
 from unilabos_msgs.srv._serial_command import SerialCommand_Request, SerialCommand_Response
 
 from unilabos.config.config import BasicConfig
-from unilabos.utils.decorator import get_topic_config, get_all_subscriptions
+from unilabos.registry.decorators import get_topic_config
+from unilabos.utils.decorator import get_all_subscriptions
 
 from unilabos.resources.container import RegularContainer
 from unilabos.resources.graphio import (
@@ -57,6 +58,7 @@ from unilabos_msgs.msg import Resource  # type: ignore
 
 from unilabos.resources.resource_tracker import (
     DeviceNodeResourceTracker,
+    ResourceDictType,
     ResourceTreeSet,
     ResourceTreeInstance,
     ResourceDictInstance,
@@ -194,9 +196,9 @@ class PropertyPublisher:
         self._value = None
         try:
             self.publisher_ = node.create_publisher(msg_type, f"{name}", qos)
-        except AttributeError as ex:
+        except Exception as e:
             self.node.lab_logger().error(
-                f"创建发布者 {name} 失败，可能由于注册表有误，类型: {msg_type}，错误: {ex}\n{traceback.format_exc()}"
+                f"StatusError, DeviceId: {self.node.device_id} 创建发布者 {name} 失败，可能由于注册表有误，类型: {msg_type}，错误: {e}"
             )
         self.timer = node.create_timer(self.timer_period, self.publish_property)
         self.__loop = ROS2DeviceNode.get_asyncio_loop()
@@ -596,6 +598,12 @@ class BaseROS2DeviceNode(Node, Generic[T]):
                 self.s2c_resource_tree,  # type: ignore
                 callback_group=self.callback_group,
             ),
+            "s2c_device_manage": self.create_service(
+                SerialCommand,
+                f"/srv{self.namespace}/s2c_device_manage",
+                self.s2c_device_manage,  # type: ignore
+                callback_group=self.callback_group,
+            ),
         }
 
         # 向全局在线设备注册表添加设备信息
@@ -1064,6 +1072,48 @@ class BaseROS2DeviceNode(Node, Generic[T]):
 
         return res
 
+    async def s2c_device_manage(self, req: SerialCommand_Request, res: SerialCommand_Response):
+        """Handle add/remove device requests from HostNode via SerialCommand."""
+        try:
+            cmd = json.loads(req.command)
+            action = cmd.get("action", "")
+            data = cmd.get("data", {})
+            device_id = data.get("device_id", "")
+
+            if not device_id:
+                res.response = json.dumps({"success": False, "error": "device_id required"})
+                return res
+
+            if action == "add":
+                result = self.create_device(device_id, data)
+            elif action == "remove":
+                result = self.destroy_device(device_id)
+            else:
+                result = {"success": False, "error": f"Unknown action: {action}"}
+
+            res.response = json.dumps(result, ensure_ascii=False)
+
+        except NotImplementedError as e:
+            self.lab_logger().warning(f"[DeviceManage] {e}")
+            res.response = json.dumps({"success": False, "error": str(e)})
+        except Exception as e:
+            self.lab_logger().error(f"[DeviceManage] Error: {e}")
+            res.response = json.dumps({"success": False, "error": str(e)})
+
+        return res
+
+    def create_device(self, device_id: str, config: "ResourceDictType") -> dict:
+        """Create a sub-device dynamically. Override in HostNode / WorkstationNode."""
+        raise NotImplementedError(
+            f"{self.__class__.__name__} does not support dynamic device creation"
+        )
+
+    def destroy_device(self, device_id: str) -> dict:
+        """Destroy a sub-device dynamically. Override in HostNode / WorkstationNode."""
+        raise NotImplementedError(
+            f"{self.__class__.__name__} does not support dynamic device removal"
+        )
+
     async def transfer_resource_to_another(
         self,
         plr_resources: List["ResourcePLR"],
@@ -1206,22 +1256,40 @@ class BaseROS2DeviceNode(Node, Generic[T]):
         return self._lab_logger
 
     def create_ros_publisher(self, attr_name, msg_type, initial_period=5.0):
-        """创建ROS发布者"""
-        # 检测装饰器配置（支持 get_{attr_name} 方法和 @property）
+        """创建ROS发布者，仅当方法/属性有 @topic_config 装饰器时才创建。"""
+        # 检测 @topic_config 装饰器配置
         topic_config = {}
+        driver_class = type(self.driver_instance)
 
-        # 优先检测 get_{attr_name} 方法
-        if hasattr(self.driver_instance, f"get_{attr_name}"):
-            getter_method = getattr(self.driver_instance, f"get_{attr_name}")
-            topic_config = get_topic_config(getter_method)
+        # 区分 @property 和普通方法两种情况
+        is_prop = hasattr(driver_class, attr_name) and isinstance(
+            getattr(driver_class, attr_name), property
+        )
 
-        # 如果没有配置，检测 @property 装饰的属性
+        if is_prop:
+            # @property: 检测 fget 上的 @topic_config
+            class_attr = getattr(driver_class, attr_name)
+            if class_attr.fget is not None:
+                topic_config = get_topic_config(class_attr.fget)
+        else:
+            # 普通方法: 直接检测 attr_name 方法上的 @topic_config
+            if hasattr(self.driver_instance, attr_name):
+                method = getattr(self.driver_instance, attr_name)
+                if callable(method):
+                    topic_config = get_topic_config(method)
+
+        # 没有 @topic_config 装饰器则跳过发布
         if not topic_config:
-            driver_class = type(self.driver_instance)
-            if hasattr(driver_class, attr_name):
-                class_attr = getattr(driver_class, attr_name)
-                if isinstance(class_attr, property) and class_attr.fget is not None:
-                    topic_config = get_topic_config(class_attr.fget)
+            return
+
+        # 发布名称优先级: @topic_config(name=...) > get_ 前缀去除 > attr_name
+        cfg_name = topic_config.get("name")
+        if cfg_name:
+            publish_name = cfg_name
+        elif attr_name.startswith("get_"):
+            publish_name = attr_name[4:]
+        else:
+            publish_name = attr_name
 
         # 使用装饰器配置或默认值
         cfg_period = topic_config.get("period")
@@ -1234,10 +1302,10 @@ class BaseROS2DeviceNode(Node, Generic[T]):
         # 获取属性值的方法
         def get_device_attr():
             try:
-                if hasattr(self.driver_instance, f"get_{attr_name}"):
-                    return getattr(self.driver_instance, f"get_{attr_name}")()
-                else:
+                if is_prop:
                     return getattr(self.driver_instance, attr_name)
+                else:
+                    return getattr(self.driver_instance, attr_name)()
             except AttributeError as ex:
                 if ex.args[0].startswith(f"AttributeError: '{self.driver_instance.__class__.__name__}' object"):
                     self.lab_logger().error(
@@ -1249,8 +1317,8 @@ class BaseROS2DeviceNode(Node, Generic[T]):
                     )
                     self.lab_logger().error(traceback.format_exc())
 
-        self._property_publishers[attr_name] = PropertyPublisher(
-            self, attr_name, get_device_attr, msg_type, period, print_publish, qos
+        self._property_publishers[publish_name] = PropertyPublisher(
+            self, publish_name, get_device_attr, msg_type, period, print_publish, qos
         )
 
     def create_ros_action_server(self, action_name, action_value_mapping):
@@ -1258,14 +1326,17 @@ class BaseROS2DeviceNode(Node, Generic[T]):
         action_type = action_value_mapping["type"]
         str_action_type = str(action_type)[8:-2]
 
-        self._action_servers[action_name] = ActionServer(
-            self,
-            action_type,
-            action_name,
-            execute_callback=self._create_execute_callback(action_name, action_value_mapping),
-            callback_group=self.callback_group,
-        )
-
+        try:
+            self._action_servers[action_name] = ActionServer(
+                self,
+                action_type,
+                action_name,
+                execute_callback=self._create_execute_callback(action_name, action_value_mapping),
+                callback_group=self.callback_group,
+            )
+        except Exception as e:
+            self.lab_logger().error(f"创建ActionServer失败，Device: {self.device_id}, Action Name: {action_name}, Action Type: {action_type}, Error: {e}")
+            return
         self.lab_logger().trace(f"发布动作: {action_name}, 类型: {str_action_type}")
 
     def _setup_decorated_subscribers(self):

unilabos/ros/nodes/presets/camera.py

@@ -4,7 +4,14 @@ import cv2
 from sensor_msgs.msg import Image
 from cv_bridge import CvBridge
 from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode, DeviceNodeResourceTracker
+from unilabos.registry.decorators import device
 
+
+@device(
+    id="camera",
+    category=["camera"],
+    description="""VideoPublisher摄像头设备节点，用于实时视频采集和流媒体发布。该设备通过OpenCV连接本地摄像头（如USB摄像头、内置摄像头等），定时采集视频帧并将其转换为ROS2的sensor_msgs/Image消息格式发布到视频话题。主要用于实验室自动化系统中的视觉监控、图像分析、实时观察等应用场景。支持可配置的摄像头索引、发布频率等参数。""",
+)
 class VideoPublisher(BaseROS2DeviceNode):
     def __init__(self, device_id='video_publisher', registry_name="", device_uuid='', camera_index=0, period: float = 0.1, resource_tracker: DeviceNodeResourceTracker = None):
         # 初始化BaseROS2DeviceNode，使用自身作为driver_instance

unilabos/ros/nodes/presets/host_node.py

@@ -12,6 +12,7 @@ from geometry_msgs.msg import Point
 from rclpy.action import ActionClient, get_action_server_names_and_types_by_node
 from rclpy.service import Service
 from typing_extensions import TypedDict
+from unilabos_msgs.action import EmptyIn, StrSingleInput, ResourceCreateFromOuterEasy, ResourceCreateFromOuter
 from unilabos_msgs.msg import Resource  # type: ignore
 from unilabos_msgs.srv import (
     ResourceAdd,
@@ -23,6 +24,7 @@ from unilabos_msgs.srv import (
 from unilabos_msgs.srv._serial_command import SerialCommand_Request, SerialCommand_Response
 from unique_identifier_msgs.msg import UUID
 
+from unilabos.registry.decorators import device
 from unilabos.registry.placeholder_type import ResourceSlot, DeviceSlot
 from unilabos.registry.registry import lab_registry
 from unilabos.resources.container import RegularContainer
@@ -30,6 +32,7 @@ from unilabos.resources.graphio import initialize_resource
 from unilabos.resources.registry import add_schema
 from unilabos.resources.resource_tracker import (
     ResourceDict,
+    ResourceDictType,
     ResourceDictInstance,
     ResourceTreeSet,
     ResourceTreeInstance,
@@ -86,6 +89,7 @@ class TestLatencyReturn(TypedDict):
     status: str
 
 
+@device(id="host_node", category=[], description="Host Node", icon="icon_device.webp")
 class HostNode(BaseROS2DeviceNode):
     """
     主机节点类，负责管理设备、资源和控制器
@@ -274,44 +278,42 @@ class HostNode(BaseROS2DeviceNode):
         self._action_clients: Dict[str, ActionClient] = {  # 为了方便了解实际的数据类型，host的默认写好
             "/devices/host_node/create_resource": ActionClient(
                 self,
-                lab_registry.ResourceCreateFromOuterEasy,
+                ResourceCreateFromOuterEasy,
                 "/devices/host_node/create_resource",
                 callback_group=self.callback_group,
             ),
             "/devices/host_node/create_resource_detailed": ActionClient(
                 self,
-                lab_registry.ResourceCreateFromOuter,
+                ResourceCreateFromOuter,
                 "/devices/host_node/create_resource_detailed",
                 callback_group=self.callback_group,
             ),
             "/devices/host_node/test_latency": ActionClient(
                 self,
-                lab_registry.EmptyIn,
+                EmptyIn,
                 "/devices/host_node/test_latency",
                 callback_group=self.callback_group,
             ),
             "/devices/host_node/test_resource": ActionClient(
                 self,
-                lab_registry.EmptyIn,
+                EmptyIn,
                 "/devices/host_node/test_resource",
                 callback_group=self.callback_group,
             ),
             "/devices/host_node/_execute_driver_command": ActionClient(
                 self,
-                lab_registry.StrSingleInput,
+                StrSingleInput,
                 "/devices/host_node/_execute_driver_command",
                 callback_group=self.callback_group,
             ),
             "/devices/host_node/_execute_driver_command_async": ActionClient(
                 self,
-                lab_registry.StrSingleInput,
+                StrSingleInput,
                 "/devices/host_node/_execute_driver_command_async",
                 callback_group=self.callback_group,
             ),
         }  # 用来存储多个ActionClient实例
-        self._action_value_mappings: Dict[str, Dict] = (
-            {}
-        )  # device_id -> action_value_mappings(本地+远程设备统一存储)
+        self._action_value_mappings: Dict[str, Dict] = {}  # device_id -> action_value_mappings(本地+远程设备统一存储)
         self._slave_registry_configs: Dict[str, Dict] = {}  # registry_name -> registry_config(含action_value_mappings)
         self._goals: Dict[str, Any] = {}  # 用来存储多个目标的状态
         self._online_devices: Set[str] = {f"{self.namespace}/{device_id}"}  # 用于跟踪在线设备
@@ -329,10 +331,18 @@ class HostNode(BaseROS2DeviceNode):
         self._discover_devices()
 
         # 初始化所有本机设备节点，多一次过滤，防止重复初始化
+        local_machine = BasicConfig.machine_name
         for device_config in devices_config.root_nodes:
             device_id = device_config.res_content.id
             if device_config.res_content.type != "device":
                 continue
+            dev_machine = device_config.res_content.machine_name
+            if dev_machine and local_machine and dev_machine != local_machine:
+                self.lab_logger().info(
+                    f"[Host Node] Device {device_id} belongs to machine '{dev_machine}', "
+                    f"local is '{local_machine}', skipping initialization."
+                )
+                continue
             if device_id not in self.devices_names:
                 self.initialize_device(device_id, device_config)
             else:
@@ -658,7 +668,12 @@ class HostNode(BaseROS2DeviceNode):
             action_id = f"/devices/{device_id}/{action_name}"
             if action_id not in self._action_clients:
                 action_type = action_value_mapping["type"]
-                self._action_clients[action_id] = ActionClient(self, action_type, action_id)
+                try:
+                    self._action_clients[action_id] = ActionClient(self, action_type, action_id)
+                except Exception as e:
+                    self.lab_logger().error(
+                        f"创建ActionClient失败，Device: {device_id}, Action Name: {action_name}, Action Type: {action_type}, Error: {e}")
+                    continue
                 self.lab_logger().trace(
                     f"[Host Node] Created ActionClient (Local): {action_id}"
                 )  # 子设备再创建用的是Discover发现的
@@ -1258,9 +1273,9 @@ class HostNode(BaseROS2DeviceNode):
 
                 # 用 registry_name 索引已存储的 registry_config,获取 action_value_mappings
                 if registry_name and registry_name in self._slave_registry_configs:
-                    action_mappings = self._slave_registry_configs[registry_name].get(
-                        "class", {}
-                    ).get("action_value_mappings", {})
+                    action_mappings = (
+                        self._slave_registry_configs[registry_name].get("class", {}).get("action_value_mappings", {})
+                    )
                     if action_mappings:
                         self._action_value_mappings[edge_device_id] = action_mappings
                         self.lab_logger().info(
@@ -1280,14 +1295,19 @@ class HostNode(BaseROS2DeviceNode):
 
                 # 解析 devices_config,建立 device_id -> action_value_mappings 映射
                 if devices_config:
+                    machine_name = info["machine_name"]
+                    # Stamp machine_name on each device dict before parsing
                     for device_tree in devices_config:
                         for device_dict in device_tree:
+                            device_dict["machine_name"] = machine_name
                             device_id = device_dict.get("id", "")
                             class_name = device_dict.get("class", "")
                             if device_id and class_name and class_name in self._slave_registry_configs:
-                                action_mappings = self._slave_registry_configs[class_name].get(
-                                    "class", {}
-                                ).get("action_value_mappings", {})
+                                action_mappings = (
+                                    self._slave_registry_configs[class_name]
+                                    .get("class", {})
+                                    .get("action_value_mappings", {})
+                                )
                                 if action_mappings:
                                     self._action_value_mappings[device_id] = action_mappings
                                     self.lab_logger().info(
@@ -1295,6 +1315,18 @@ class HostNode(BaseROS2DeviceNode):
                                         f"for remote device {device_id} (class: {class_name})"
                                     )
 
+                    # Merge slave devices_config into self.devices_config tree
+                    try:
+                        slave_tree_set = ResourceTreeSet.load(devices_config)  # slave一定是根节点的tree
+                        for tree in slave_tree_set.trees:
+                            self.devices_config.trees.append(tree)
+                        self.lab_logger().info(
+                            f"[Host Node] Merged {len(slave_tree_set.trees)} slave device trees "
+                            f"(machine: {machine_name}) into devices_config"
+                        )
+                    except Exception as e:
+                        self.lab_logger().error(f"[Host Node] Failed to merge slave devices_config: {e}")
+
             self.lab_logger().debug(f"[Host Node] Node info update: {info}")
             response.response = "OK"
         except Exception as e:
@@ -1703,3 +1735,177 @@ class HostNode(BaseROS2DeviceNode):
             self.lab_logger().error(f"[Host Node-Resource] Error notifying resource tree update: {str(e)}")
             self.lab_logger().error(traceback.format_exc())
             return False
+
+    # ------------------------------------------------------------------
+    # Device lifecycle (add / remove) — pure forwarder
+    # ------------------------------------------------------------------
+
+    def notify_device_manage(self, target_node_id: str, action: str, config: ResourceDictType) -> bool:
+        """Forward an add/remove device command to the target node via ROS2 SerialCommand.
+
+        The HostNode does NOT interpret the command; it simply resolves the
+        target namespace and forwards the request to ``s2c_device_manage``.
+
+        If *target_node_id* equals the HostNode's own device_id (i.e. the
+        command targets the host itself), we call our local ``create_device``
+        / ``destroy_device`` directly instead of going through ROS2.
+        """
+        try:
+            # If the target is the host itself, handle locally
+            device_id = config["id"]
+            if target_node_id == self.device_id:
+                if action == "add":
+                    return self.create_device(device_id, config).get("success", False)
+                elif action == "remove":
+                    return self.destroy_device(device_id).get("success", False)
+
+            if target_node_id not in self.devices_names:
+                self.lab_logger().error(
+                    f"[Host Node-DeviceMgr] Target {target_node_id} not found in devices_names"
+                )
+                return False
+
+            namespace = self.devices_names[target_node_id]
+            device_key = f"{namespace}/{target_node_id}"
+            if device_key not in self._online_devices:
+                self.lab_logger().error(f"[Host Node-DeviceMgr] Target {device_key} is offline")
+                return False
+
+            srv_address = f"/srv{namespace}/s2c_device_manage"
+            self.lab_logger().info(
+                f"[Host Node-DeviceMgr] Forwarding {action}_device to {target_node_id} ({srv_address})"
+            )
+
+            sclient = self.create_client(SerialCommand, srv_address)
+            if not sclient.wait_for_service(timeout_sec=5.0):
+                self.lab_logger().error(f"[Host Node-DeviceMgr] Service {srv_address} not available")
+                return False
+
+            request = SerialCommand.Request()
+            request.command = json.dumps({"action": action, "data": config}, ensure_ascii=False)
+
+            future = sclient.call_async(request)
+            timeout = 30.0
+            start_time = time.time()
+            while not future.done():
+                if time.time() - start_time > timeout:
+                    self.lab_logger().error(
+                        f"[Host Node-DeviceMgr] Timeout waiting for {action}_device on {target_node_id}"
+                    )
+                    return False
+                time.sleep(0.05)
+
+            response = future.result()
+            self.lab_logger().info(
+                f"[Host Node-DeviceMgr] {action}_device on {target_node_id} completed"
+            )
+            return True
+
+        except Exception as e:
+            self.lab_logger().error(f"[Host Node-DeviceMgr] Error: {e}")
+            self.lab_logger().error(traceback.format_exc())
+            return False
+
+    def create_device(self, device_id: str, config: ResourceDictType) -> dict:
+        """Dynamically create a root-level device on the host."""
+        if not device_id:
+            return {"success": False, "error": "device_id required"}
+
+        if device_id in self.devices_names:
+            return {"success": False, "error": f"Device {device_id} already exists"}
+
+        try:
+            config.setdefault("id", device_id)
+            config.setdefault("type", "device")
+            config.setdefault("machine_name", BasicConfig.machine_name or "本地")
+            res_dict = ResourceDictInstance.get_resource_instance_from_dict(config)
+
+            self.initialize_device(device_id, res_dict)
+
+            if device_id not in self.devices_names:
+                return {"success": False, "error": f"initialize_device failed for {device_id}"}
+
+            # Add to config tree (devices_config)
+            tree = ResourceTreeInstance(res_dict)
+            self.devices_config.trees.append(tree)
+
+            # Add to resource tracker so s2c_resource_tree can find it
+            try:
+                for plr_resource in ResourceTreeSet([tree]).to_plr_resources():
+                    self._resource_tracker.add_resource(plr_resource)
+            except Exception as ex:
+                self.lab_logger().warning(f"[Host Node-DeviceMgr] PLR resource registration skipped: {ex}")
+
+            self.lab_logger().info(f"[Host Node-DeviceMgr] Device {device_id} created successfully")
+            return {"success": True, "device_id": device_id}
+
+        except Exception as e:
+            self.lab_logger().error(f"[Host Node-DeviceMgr] Failed to create {device_id}: {e}")
+            self.lab_logger().error(traceback.format_exc())
+            return {"success": False, "error": str(e)}
+
+    def destroy_device(self, device_id: str) -> dict:
+        """Remove a root-level device from the host."""
+        if not device_id:
+            return {"success": False, "error": "device_id required"}
+
+        if device_id not in self.devices_names:
+            return {"success": False, "error": f"Device {device_id} not found"}
+
+        if device_id == self.device_id:
+            return {"success": False, "error": "Cannot destroy host_node itself"}
+
+        try:
+            namespace = self.devices_names[device_id]
+            device_key = f"{namespace}/{device_id}"
+
+            # Remove action clients
+            action_prefix = f"/devices/{device_id}/"
+            to_remove = [k for k in self._action_clients if k.startswith(action_prefix)]
+            for k in to_remove:
+                try:
+                    self._action_clients[k].destroy()
+                except Exception:
+                    pass
+                del self._action_clients[k]
+
+            # Remove from config tree (devices_config)
+            self.devices_config.trees = [
+                t for t in self.devices_config.trees
+                if t.root_node.res_content.id != device_id
+            ]
+
+            # Remove from resource tracker
+            try:
+                tracked = self._resource_tracker.uuid_to_resources.copy()
+                for uid, res in tracked.items():
+                    res_id = res.get("id") if isinstance(res, dict) else getattr(res, "name", None)
+                    if res_id == device_id:
+                        self._resource_tracker.remove_resource(res)
+            except Exception as ex:
+                self.lab_logger().warning(f"[Host Node-DeviceMgr] Resource tracker cleanup: {ex}")
+
+            # Clean internal state
+            self._online_devices.discard(device_key)
+            self.devices_names.pop(device_id, None)
+            self.device_machine_names.pop(device_id, None)
+            self._action_value_mappings.pop(device_id, None)
+
+            # Destroy the ROS2 node of the device
+            instance = self.devices_instances.pop(device_id, None)
+            if instance is not None:
+                try:
+                    # noinspection PyProtectedMember
+                    ros_node = getattr(instance, "_ros_node", None)
+                    if ros_node is not None:
+                        ros_node.destroy_node()
+                except Exception as e:
+                    self.lab_logger().warning(f"[Host Node-DeviceMgr] Error destroying ROS node for {device_id}: {e}")
+
+            self.lab_logger().info(f"[Host Node-DeviceMgr] Device {device_id} destroyed")
+            return {"success": True, "device_id": device_id}
+
+        except Exception as e:
+            self.lab_logger().error(f"[Host Node-DeviceMgr] Failed to destroy {device_id}: {e}")
+            self.lab_logger().error(traceback.format_exc())
+            return {"success": False, "error": str(e)}

unilabos/ros/nodes/presets/workstation.py

@@ -20,7 +20,7 @@ from unilabos.ros.msgs.message_converter import (
     convert_from_ros_msg_with_mapping,
 )
 from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode, DeviceNodeResourceTracker, ROS2DeviceNode
-from unilabos.resources.resource_tracker import ResourceTreeSet, ResourceDictInstance
+from unilabos.resources.resource_tracker import ResourceDictType, ResourceTreeSet, ResourceDictInstance
 from unilabos.utils.type_check import get_result_info_str
 
 if TYPE_CHECKING:
@@ -177,6 +177,103 @@ class ROS2WorkstationNode(BaseROS2DeviceNode):
                     self.lab_logger().trace(f"为子设备 {device_id} 创建动作客户端: {action_name}")
         return d
 
+    def create_device(self, device_id: str, config: ResourceDictType) -> dict:
+        """Dynamically add a sub-device to this workstation."""
+        if not device_id:
+            return {"success": False, "error": "device_id required"}
+
+        if device_id in self.sub_devices:
+            return {"success": False, "error": f"Sub-device {device_id} already exists"}
+
+        try:
+            from unilabos.config.config import BasicConfig
+            config.setdefault("id", device_id)
+            config.setdefault("type", "device")
+            config.setdefault("machine_name", BasicConfig.machine_name or "本地")
+            res_dict = ResourceDictInstance.get_resource_instance_from_dict(config)
+
+            d = self.initialize_device(device_id, res_dict)
+            if d is None:
+                return {"success": False, "error": f"initialize_device returned None for {device_id}"}
+
+            # Add to children config list
+            self.children.append(res_dict)
+
+            # Add to resource tracker
+            try:
+                from unilabos.resources.resource_tracker import ResourceTreeInstance
+                tree = ResourceTreeInstance(res_dict)
+                for plr_resource in ResourceTreeSet([tree]).to_plr_resources():
+                    self.resource_tracker.add_resource(plr_resource)
+            except Exception as ex:
+                self.lab_logger().warning(f"[Workstation-DeviceMgr] PLR resource registration skipped: {ex}")
+
+            self.lab_logger().info(f"[Workstation-DeviceMgr] Sub-device {device_id} created")
+            return {"success": True, "device_id": device_id}
+
+        except Exception as e:
+            self.lab_logger().error(f"[Workstation-DeviceMgr] Failed to create {device_id}: {e}")
+            self.lab_logger().error(traceback.format_exc())
+            return {"success": False, "error": str(e)}
+
+    def destroy_device(self, device_id: str) -> dict:
+        """Dynamically remove a sub-device from this workstation."""
+        if not device_id:
+            return {"success": False, "error": "device_id required"}
+
+        if device_id not in self.sub_devices:
+            return {"success": False, "error": f"Sub-device {device_id} not found"}
+
+        try:
+            # Remove from children config list
+            self.children = [
+                c for c in self.children
+                if c.res_content.id != device_id
+            ]
+
+            # Remove from resource tracker
+            try:
+                tracked = self.resource_tracker.uuid_to_resources.copy()
+                for uid, res in tracked.items():
+                    res_id = res.get("id") if isinstance(res, dict) else getattr(res, "name", None)
+                    if res_id == device_id:
+                        self.resource_tracker.remove_resource(res)
+            except Exception as ex:
+                self.lab_logger().warning(f"[Workstation-DeviceMgr] Resource tracker cleanup: {ex}")
+
+            # Remove action clients for this sub-device
+            action_prefix = f"/devices/{device_id}/"
+            to_remove = [k for k in self._action_clients if k.startswith(action_prefix)]
+            for k in to_remove:
+                try:
+                    self._action_clients[k].destroy()
+                except Exception:
+                    pass
+                del self._action_clients[k]
+
+            # Destroy the ROS2 node
+            instance = self.sub_devices.pop(device_id, None)
+            if instance is not None:
+                ros_node = getattr(instance, "ros_node_instance", None)
+                if ros_node is not None:
+                    try:
+                        ros_node.destroy_node()
+                    except Exception as e:
+                        self.lab_logger().warning(
+                            f"[Workstation-DeviceMgr] Error destroying ROS node for {device_id}: {e}"
+                        )
+
+            # Remove from communication map if present
+            self.communication_node_id_to_instance.pop(device_id, None)
+
+            self.lab_logger().info(f"[Workstation-DeviceMgr] Sub-device {device_id} destroyed")
+            return {"success": True, "device_id": device_id}
+
+        except Exception as e:
+            self.lab_logger().error(f"[Workstation-DeviceMgr] Failed to destroy {device_id}: {e}")
+            self.lab_logger().error(traceback.format_exc())
+            return {"success": False, "error": str(e)}
+
     def create_ros_action_server(self, action_name, action_value_mapping):
         """创建ROS动作服务器"""
         if action_name not in self.protocol_names: