Initial commit

unilabos/ros/nodes/presets/controller_node.py

@@ -0,0 +1,122 @@
+from typing import Callable, Dict
+from std_msgs.msg import Float64
+
+from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode, DeviceNodeResourceTracker
+
+
+class ControllerNode(BaseROS2DeviceNode):
+    namespace_prefix = "/controllers"
+
+    def __init__(
+        self,
+        device_id: str,
+        controller_func: Callable,
+        update_rate: float,
+        inputs: Dict[str, Dict[str, type | str]],
+        outputs: Dict[str, Dict[str, type]],
+        parameters: Dict,
+        resource_tracker: DeviceNodeResourceTracker,
+    ):
+        """
+        通用控制器节点
+
+        :param controller_id: 控制器的唯一标识符（作为命名空间的一部分）
+        :param update_rate: 控制器更新频率 (Hz)
+        :param controller_func: 控制器函数，接收 Python 格式的 inputs 和 parameters 返回 outputs
+        :param input_types: 输入话题及其消息类型的字典
+        :param output_types: 输出话题及其消息类型的字典
+        :param parameters: 控制器函数的额外参数
+        """
+        # 先准备所需的属性，以便在调用父类初始化前就可以使用
+        self.device_id = device_id
+        self.controller_func = controller_func
+        self.update_rate = update_rate
+        self.update_time = 1.0 / update_rate
+        self.parameters = parameters
+        self.inputs = {topic: None for topic in inputs.keys()}
+        self.control_input_subscribers = {}
+        self.control_output_publishers = {}
+        self.topic_mapping = {
+            **{input_info["topic"]: input for input, input_info in inputs.items()},
+            **{output_info["topic"]: output for output, output_info in outputs.items()},
+        }
+
+        # 调用BaseROS2DeviceNode初始化，使用自身作为driver_instance
+        status_types = {}
+        action_value_mappings = {}
+        hardware_interface = {}
+
+        # 使用短ID作为节点名，完整ID（带namespace_prefix）作为device_id
+        BaseROS2DeviceNode.__init__(
+            self,
+            driver_instance=self,
+            device_id=device_id,
+            status_types=status_types,
+            action_value_mappings=action_value_mappings,
+            hardware_interface=hardware_interface,
+            print_publish=False,
+            resource_tracker=resource_tracker
+        )
+
+        # 原始初始化逻辑
+        # 初始化订阅者
+        for input, input_info in inputs.items():
+            msg_type = input_info["type"]
+            topic = str(input_info["topic"])
+            self.control_input_subscribers[input] = self.create_subscription(
+                msg_type, topic, lambda msg, t=topic: self.input_callback(t, msg), 10
+            )
+
+        # 初始化发布者
+        for output, output_info in outputs.items():
+            self.lab_logger().info(f"Creating publisher for {output} {output_info}")
+            msg_type = output_info["type"]
+            topic = str(output_info["topic"])
+            self.control_output_publishers[output] = self.create_publisher(msg_type, topic, 10)
+
+        # 定时器，用于定期调用控制逻辑
+        self.timer = self.create_timer(self.update_time, self.control_loop)
+
+    def input_callback(self, topic: str, msg):
+        """
+        更新指定话题的输入数据，并将 ROS 消息转换为普通 Python 数据。
+        支持 `std_msgs` 类型消息。
+        """
+        self.inputs[self.topic_mapping[topic]] = msg.data
+        self.lab_logger().info(f"Received input on topic {topic}: {msg.data}")
+
+    def control_loop(self):
+        """主控制逻辑"""
+        # 检查所有输入是否已更新
+        if all(value is not None for value in self.inputs.values()):
+            self.lab_logger().info(
+                f"Calling controller function with inputs: {self.inputs}, parameters: {self.parameters}"
+            )
+            try:
+                # 调用控制器函数，传入 Python 格式的数据
+                outputs = self.controller_func(**self.inputs, **self.parameters)
+                self.lab_logger().info(f"Inputs: {self.inputs}, Outputs: {outputs}")
+                self.inputs = {topic: None for topic in self.inputs.keys()}
+            except Exception as e:
+                self.lab_logger().error(f"Controller function execution failed: {e}")
+                return
+
+            # 发布控制信号，将普通 Python 数据转换为 ROS 消息
+            if isinstance(outputs, dict):
+                for topic, value in outputs.items():
+                    if topic in self.control_output_publishers:
+                        # 支持 Float64 输出
+                        if isinstance(value, (float, int)):
+                            self.control_output_publishers[topic].publish(Float64(data=value))
+                        else:
+                            self.lab_logger().error(f"Unsupported output type for topic {topic}: {type(value)}")
+                    else:
+                        self.lab_logger().warning(f"Output topic {topic} is not defined in output_types.")
+            else:
+                publisher = list(self.control_output_publishers.values())[0]
+                if isinstance(outputs, (float, int)):
+                    publisher.publish(Float64(data=outputs))
+                else:
+                    self.lab_logger().error(f"Unsupported output type: {type(outputs)}")
+        else:
+            self.lab_logger().info("Waiting for all inputs to be received.")

unilabos/ros/nodes/presets/host_node.py

@@ -0,0 +1,623 @@
+import copy
+import threading
+import time
+import uuid
+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 rclpy.action import ActionClient, get_action_server_names_and_types_by_node
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.service import Service
+from unique_identifier_msgs.msg import UUID
+
+from unilabos.resources.registry import add_schema
+from unilabos.ros.initialize_device import initialize_device_from_dict
+from unilabos.ros.msgs.message_converter import (
+    get_msg_type,
+    get_ros_type_by_msgname,
+    convert_from_ros_msg,
+    convert_to_ros_msg,
+    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
+
+
+class HostNode(BaseROS2DeviceNode):
+    """
+    主机节点类，负责管理设备、资源和控制器
+
+    作为单例模式实现，确保整个应用中只有一个主机节点实例
+    """
+
+    _instance: ClassVar[Optional["HostNode"]] = None
+    _ready_event: ClassVar[threading.Event] = threading.Event()
+
+    @classmethod
+    def get_instance(cls, timeout=None) -> Optional["HostNode"]:
+        if cls._ready_event.wait(timeout):
+            return cls._instance
+        return None
+
+    def __init__(
+        self,
+        device_id: str,
+        devices_config: Dict[str, Any],
+        resources_config: Any,
+        physical_setup_graph: Optional[Dict[str, Any]] = None,
+        controllers_config: Optional[Dict[str, Any]] = None,
+        bridges: Optional[List[Any]] = None,
+        discovery_interval: float = 180.0,  # 设备发现间隔，单位为秒
+    ):
+        """
+        初始化主机节点
+
+        Args:
+            device_id: 节点名称
+            devices_config: 设备配置
+            resources_config: 资源配置
+            physical_setup_graph: 物理设置图
+            controllers_config: 控制器配置
+            bridges: 桥接器列表
+            discovery_interval: 设备发现间隔（秒），默认5秒
+        """
+        if self._instance is not None:
+            self._instance.lab_logger().critical("[Host Node] HostNode instance already exists.")
+        # 初始化Node基类，传递空参数覆盖列表
+        BaseROS2DeviceNode.__init__(
+            self,
+            driver_instance=self,
+            device_id=device_id,
+            status_types={},
+            action_value_mappings={},
+            hardware_interface={},
+            print_publish=False,
+            resource_tracker=DeviceNodeResourceTracker(),  # host node并不是通过initialize 包一层传进来的
+        )
+
+        # 设置单例实例
+        self.__class__._instance = self
+
+        # 初始化配置
+        self.devices_config = devices_config
+        self.resources_config = resources_config
+        self.physical_setup_graph = physical_setup_graph
+        if controllers_config is None:
+            controllers_config = {}
+        self.controllers_config = controllers_config
+        if bridges is None:
+            bridges = []
+        self.bridges = bridges
+
+        # 创建设备、动作客户端和目标存储
+        self.devices_names: Dict[str, str] = {}  # 存储设备名称和命名空间的映射
+        self.devices_instances: Dict[str, ROS2DeviceNode] = {}  # 存储设备实例
+        self._action_clients: Dict[str, ActionClient] = {}  # 用来存储多个ActionClient实例
+        self._action_value_mappings: Dict[str, Dict] = (
+            {}
+        )  # 用来存储多个ActionClient的type, goal, feedback, result的变量名映射关系
+        self._goals: Dict[str, Any] = {}  # 用来存储多个目标的状态
+        self._online_devices: Set[str] = set()  # 用于跟踪在线设备
+        self._last_discovery_time = 0.0  # 上次设备发现的时间
+        self._discovery_lock = threading.Lock()  # 设备发现的互斥锁
+        self._subscribed_topics = set()  # 用于跟踪已订阅的话题
+
+        # 创建物料增删改查服务（非客户端）
+        self._init_resource_service()
+
+        self.device_status = {}  # 用来存储设备状态
+        self.device_status_timestamps = {}  # 用来存储设备状态最后更新时间
+
+        # 首次发现网络中的设备
+        self._discover_devices()
+
+        # 初始化所有本机设备节点，多一次过滤，防止重复初始化
+        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.")
+                continue
+            if device_id not in self.devices_names:
+                self.initialize_device(device_id, device_config)
+            else:
+                self.lab_logger().warning(f"[Host Node] Device {device_id} already existed, skipping.")
+        self.update_device_status_subscriptions()
+        # TODO: 需要验证 初始化所有控制器节点
+        if controllers_config:
+            update_rate = controllers_config["controller_manager"]["ros__parameters"]["update_rate"]
+            for controller_id, controller_config in controllers_config["controller_manager"]["ros__parameters"][
+                "controllers"
+            ].items():
+                controller_config["update_rate"] = update_rate
+                self.initialize_controller(controller_id, controller_config)
+
+        for bridge in self.bridges:
+            if hasattr(bridge, "resource_add"):
+                self.lab_logger().info("[Host Node-Resource] Adding resources to bridge.")
+                bridge.resource_add(add_schema(resources_config))
+
+        # 创建定时器，定期发现设备
+        self._discovery_timer = self.create_timer(
+            discovery_interval, self._discovery_devices_callback, callback_group=ReentrantCallbackGroup()
+        )
+
+        self.lab_logger().info("[Host Node] Host node initialized.")
+        HostNode._ready_event.set()
+
+    def _discover_devices(self) -> None:
+        """
+        发现网络中的设备
+
+        检测ROS2网络中的所有设备节点，并为它们创建ActionClient
+        同时检测设备离线情况
+        """
+        self.lab_logger().debug("[Host Node] Discovering devices in the network...")
+
+        # 获取当前所有设备
+        nodes_and_names = self.get_node_names_and_namespaces()
+
+        # 跟踪本次发现的设备，用于检测离线设备
+        current_devices = set()
+
+        for device_id, namespace in nodes_and_names:
+            if not namespace.startswith("/devices"):
+                continue
+
+            # 将设备添加到当前设备集合
+            device_key = f"{namespace}/{device_id}"
+            current_devices.add(device_key)
+
+            # 如果是新设备，记录并创建ActionClient
+            if device_id not in self.devices_names:
+                self.lab_logger().info(f"[Host Node] Discovered new device: {device_key}")
+                self.devices_names[device_id] = namespace
+                self._create_action_clients_for_device(device_id, namespace)
+                self._online_devices.add(device_key)
+            elif device_key not in self._online_devices:
+                # 设备重新上线
+                self.lab_logger().info(f"[Host Node] Device reconnected: {device_key}")
+                self._online_devices.add(device_key)
+
+        # 检测离线设备
+        offline_devices = self._online_devices - current_devices
+        for device_key in offline_devices:
+            self.lab_logger().warning(f"[Host Node] Device offline: {device_key}")
+            self._online_devices.discard(device_key)
+
+        # 更新在线设备列表
+        self._online_devices = current_devices
+        self.lab_logger().debug(f"[Host Node] Total online devices: {len(self._online_devices)}")
+
+    def _discovery_devices_callback(self) -> None:
+        """
+        设备发现定时器回调函数
+        """
+        # 使用互斥锁确保同时只有一个发现过程
+        if self._discovery_lock.acquire(blocking=False):
+            try:
+                self._discover_devices()
+                # 发现新设备后，更新设备状态订阅
+                self.update_device_status_subscriptions()
+            finally:
+                self._discovery_lock.release()
+        else:
+            self.lab_logger().debug("[Host Node] Device discovery already in progress, skipping.")
+
+    def _create_action_clients_for_device(self, device_id: str, namespace: str) -> None:
+        """
+        为设备创建所有必要的ActionClient
+
+        Args:
+            device_id: 设备ID
+            namespace: 设备命名空间
+        """
+        for action_id, action_types in get_action_server_names_and_types_by_node(self, device_id, namespace):
+            if action_id not in self._action_clients:
+                try:
+                    action_type = get_ros_type_by_msgname(action_types[0])
+                    self._action_clients[action_id] = ActionClient(
+                        self, action_type, action_id, callback_group=self.callback_group
+                    )
+                    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:
+                    self.lab_logger().error(f"[Host Node] Failed to create ActionClient for {action_id}: {str(e)}")
+
+    def initialize_device(self, device_id: str, device_config: Dict[str, Any]) -> None:
+        """
+        根据配置初始化设备
+
+        此函数根据提供的设备配置动态导入适当的设备类并创建其实例。
+        同时为设备的动作值映射设置动作客户端。
+
+        Args:
+            device_id: 设备唯一标识符
+            device_config: 设备配置字典，包含类型和其他参数
+        """
+        self.lab_logger().info(f"[Host Node] Initializing device: {device_id}")
+
+        device_config_copy = copy.deepcopy(device_config)
+        d = initialize_device_from_dict(device_id, device_config_copy)
+        if d is None:
+            return
+        # noinspection PyProtectedMember
+        self.devices_names[device_id] = d._ros_node.namespace
+        self.devices_instances[device_id] = d
+        # noinspection PyProtectedMember
+        for action_name, action_value_mapping in d._ros_node._action_value_mappings.items():
+            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)
+                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:
+                self.lab_logger().warning(f"[Host Node] ActionClient {action_id} already exists.")
+        device_key = f"{self.devices_names[device_id]}/{device_id}"
+        # 添加到在线设备列表
+        self._online_devices.add(device_key)
+
+    def update_device_status_subscriptions(self) -> None:
+        """
+        更新设备状态订阅
+
+        扫描所有设备话题，为新的话题创建订阅，确保不会重复订阅
+        """
+        topic_names_and_types = self.get_topic_names_and_types()
+        for topic, types in topic_names_and_types:
+            # 检查是否为设备状态话题且未订阅过
+            if (
+                topic.startswith("/devices/")
+                and not types[0].endswith("FeedbackMessage")
+                and "_action" not in topic
+                and topic not in self._subscribed_topics
+            ):
+
+                # 解析设备名和属性名
+                parts = topic.split("/")
+                if len(parts) >= 4:
+                    device_id = parts[-2]
+                    property_name = parts[-1]
+
+                    # 初始化设备状态字典
+                    if device_id not in self.device_status:
+                        self.device_status[device_id] = {}
+                        self.device_status_timestamps[device_id] = {}
+
+                    # 默认初始化属性值为 None
+                    self.device_status[device_id][property_name] = None
+                    self.device_status_timestamps[device_id][property_name] = 0  # 初始化时间戳
+
+                    # 动态创建订阅
+                    try:
+                        type_class = msg_converter_manager.search_class(types[0].replace("/", "."))
+                        if type_class is None:
+                            self.lab_logger().error(f"[Host Node] Invalid type {types[0]} for {topic}")
+                        else:
+                            self.create_subscription(
+                                type_class,
+                                topic,
+                                lambda msg, d=device_id, p=property_name: self.property_callback(msg, d, p),
+                                1,
+                                callback_group=ReentrantCallbackGroup(),
+                            )
+                            # 标记为已订阅
+                            self._subscribed_topics.add(topic)
+                            self.lab_logger().debug(f"[Host Node] Subscribed to new topic: {topic}")
+                    except (NameError, SyntaxError) as e:
+                        self.lab_logger().error(f"[Host Node] Failed to create subscription for topic {topic}: {e}")
+
+    """设备相关"""
+
+    def property_callback(self, msg, device_id: str, property_name: str) -> None:
+        """
+        更新设备状态字典中的属性值，并发送到桥接器。
+
+        Args:
+            msg: 接收到的消息
+            device_id: 设备ID
+            property_name: 属性名称
+        """
+        # 更新设备状态字典
+        if hasattr(msg, "data"):
+            bChange = False
+            if isinstance(msg.data, (float, int, str)):
+                if self.device_status[device_id][property_name] != msg.data:
+                    bChange = True
+                self.device_status[device_id][property_name] = msg.data
+                # 更新时间戳
+                self.device_status_timestamps[device_id][property_name] = time.time()
+            else:
+                self.lab_logger().debug(
+                    f"[Host Node] Unsupported data type for {device_id}/{property_name}: {type(msg.data)}"
+                )
+
+            # 所有 Bridge 对象都应具有 publish_device_status 方法；都会收到设备状态更新
+            if bChange:
+                for bridge in self.bridges:
+                    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}"
+                        )
+
+    def send_goal(
+        self, device_id: str, action_name: str, action_kwargs: Dict[str, Any], goal_uuid: Optional[str] = None
+    ) -> None:
+        """
+        向设备发送目标请求
+
+        Args:
+            device_id: 设备ID
+            action_name: 动作名称
+            action_kwargs: 动作参数
+            goal_uuid: 目标UUID，如果为None则自动生成
+        """
+        action_id = f"/devices/{device_id}/{action_name}"
+        if action_id not in self._action_clients:
+            self.lab_logger().error(f"[Host Node] ActionClient {action_id} not found.")
+            return
+
+        action_client: ActionClient = self._action_clients[action_id]
+
+        goal_msg = convert_to_ros_msg(action_client._action_type.Goal(), action_kwargs)
+
+        self.lab_logger().info(f"[Host Node] Sending goal for {action_id}: {goal_msg}")
+        action_client.wait_for_server()
+
+        uuid_str = goal_uuid
+        if goal_uuid is not None:
+            u = uuid.UUID(goal_uuid)
+            goal_uuid_obj = UUID(uuid=list(u.bytes))
+        else:
+            goal_uuid_obj = None
+
+        future = action_client.send_goal_async(
+            goal_msg,
+            feedback_callback=lambda feedback_msg: self.feedback_callback(action_id, uuid_str, feedback_msg),
+            goal_uuid=goal_uuid_obj,
+        )
+        future.add_done_callback(lambda future: self.goal_response_callback(action_id, uuid_str, future))
+
+    def goal_response_callback(self, action_id: str, uuid_str: Optional[str], future) -> None:
+        """目标响应回调"""
+        goal_handle = future.result()
+        if not goal_handle.accepted:
+            self.lab_logger().warning(f"[Host Node] Goal {action_id} ({uuid_str}) rejected")
+            return
+
+        self.lab_logger().info(f"[Host Node] Goal {action_id} ({uuid_str}) accepted")
+        if uuid_str:
+            self._goals[uuid_str] = goal_handle
+            goal_handle.get_result_async().add_done_callback(
+                lambda future: self.get_result_callback(action_id, uuid_str, future)
+            )
+
+    def feedback_callback(self, action_id: str, uuid_str: Optional[str], feedback_msg) -> None:
+        """反馈回调"""
+        feedback_data = convert_from_ros_msg(feedback_msg)
+        feedback_data.pop("goal_id")
+        self.lab_logger().debug(f"[Host Node] Feedback for {action_id} ({uuid_str}): {feedback_data}")
+
+        if uuid_str:
+            for bridge in self.bridges:
+                if hasattr(bridge, "publish_job_status"):
+                    bridge.publish_job_status(feedback_data, uuid_str, "running")
+
+    def get_result_callback(self, action_id: str, uuid_str: Optional[str], future) -> None:
+        """获取结果回调"""
+        result_msg = future.result().result
+        result_data = convert_from_ros_msg(result_msg)
+        self.lab_logger().info(f"[Host Node] Result for {action_id} ({uuid_str}): success")
+        self.lab_logger().debug(f"[Host Node] Result data: {result_data}")
+
+        if uuid_str:
+            for bridge in self.bridges:
+                if hasattr(bridge, "publish_job_status"):
+                    bridge.publish_job_status(result_data, uuid_str, "success")
+
+    def cancel_goal(self, goal_uuid: str) -> None:
+        """取消目标"""
+        if goal_uuid in self._goals:
+            self.lab_logger().info(f"[Host Node] Cancelling goal {goal_uuid}")
+            self._goals[goal_uuid].cancel_goal_async()
+        else:
+            self.lab_logger().warning(f"[Host Node] Goal {goal_uuid} not found, cannot cancel")
+
+    def get_goal_status(self, uuid_str: str) -> int:
+        """获取目标状态"""
+        if uuid_str in self._goals:
+            g = self._goals[uuid_str]
+            status = g.status
+            self.lab_logger().debug(f"[Host Node] Goal status for {uuid_str}: {status}")
+            return status
+        self.lab_logger().warning(f"[Host Node] Goal {uuid_str} not found, status unknown")
+        return GoalStatus.STATUS_UNKNOWN
+
+    """Controller Node"""
+
+    def initialize_controller(self, controller_id: str, controller_config: Dict[str, Any]) -> None:
+        """
+        初始化控制器
+
+        Args:
+            controller_id: 控制器ID
+            controller_config: 控制器配置
+        """
+        self.lab_logger().info(f"[Host Node] Initializing controller: {controller_id}")
+
+        class_name = controller_config.pop("type")
+        controller_func = globals()[class_name]
+
+        for input_name, input_info in controller_config["inputs"].items():
+            controller_config["inputs"][input_name]["type"] = get_msg_type(eval(input_info["type"]))
+        for output_name, output_info in controller_config["outputs"].items():
+            controller_config["outputs"][output_name]["type"] = get_msg_type(eval(output_info["type"]))
+
+        if controller_config["parameters"] is None:
+            controller_config["parameters"] = {}
+
+        controller = ControllerNode(controller_id, controller_func=controller_func, **controller_config)
+        self.lab_logger().info(f"[Host Node] Controller {controller_id} created.")
+        # rclpy.get_global_executor().add_node(controller)
+
+    """Resource"""
+
+    def _init_resource_service(self):
+        self._resource_services: Dict[str, Service] = {
+            "resource_add": self.create_service(
+                ResourceAdd, "/resources/add", self._resource_add_callback, callback_group=ReentrantCallbackGroup()
+            ),
+            "resource_get": self.create_service(
+                ResourceGet, "/resources/get", self._resource_get_callback, callback_group=ReentrantCallbackGroup()
+            ),
+            "resource_delete": self.create_service(
+                ResourceDelete,
+                "/resources/delete",
+                self._resource_delete_callback,
+                callback_group=ReentrantCallbackGroup(),
+            ),
+            "resource_update": self.create_service(
+                ResourceUpdate,
+                "/resources/update",
+                self._resource_update_callback,
+                callback_group=ReentrantCallbackGroup(),
+            ),
+            "resource_list": self.create_service(
+                ResourceList, "/resources/list", self._resource_list_callback, callback_group=ReentrantCallbackGroup()
+            ),
+        }
+
+    def _resource_add_callback(self, request, response):
+        """
+        添加资源回调
+
+        处理添加资源请求，将资源数据传递到桥接器
+
+        Args:
+            request: 包含资源数据的请求对象
+            response: 响应对象
+
+        Returns:
+            响应对象，包含操作结果
+        """
+        resources = [convert_from_ros_msg(resource) for resource in request.resources]
+        self.lab_logger().info(f"[Host Node-Resource] Add request received: {len(resources)} resources")
+
+        success = False
+        if len(self.bridges) > 0:
+            r = self.bridges[-1].resource_add(add_schema(resources))
+            success = bool(r)
+
+        response.success = success
+        self.lab_logger().info(f"[Host Node-Resource] Add request completed, success: {success}")
+        return response
+
+    def _resource_get_callback(self, request, response):
+        """
+        获取资源回调
+
+        处理获取资源请求，从桥接器或本地查询资源数据
+
+        Args:
+            request: 包含资源ID的请求对象
+            response: 响应对象
+
+        Returns:
+            响应对象，包含查询到的资源
+        """
+        self.lab_logger().info(f"[Host Node-Resource] Get request for ID: {request.id}")
+
+        if len(self.bridges) > 0:
+            # 云上物料服务，根据 id 查询物料
+            try:
+                r = self.bridges[-1].resource_get(request.id, request.with_children)["data"]
+                self.lab_logger().debug(f"[Host Node-Resource] Retrieved from bridge: {len(r)} resources")
+            except Exception as e:
+                self.lab_logger().error(f"[Host Node-Resource] Error retrieving from bridge: {str(e)}")
+                r = []
+        else:
+            # 本地物料服务，根据 id 查询物料
+            r = [resource for resource in self.resources_config if resource.get("id") == request.id]
+            self.lab_logger().debug(f"[Host Node-Resource] Retrieved from local: {len(r)} resources")
+
+        response.resources = [convert_to_ros_msg(Resource, resource) for resource in r]
+        return response
+
+    def _resource_delete_callback(self, request, response):
+        """
+        删除资源回调
+
+        处理删除资源请求，将删除指令传递到桥接器
+
+        Args:
+            request: 包含资源ID的请求对象
+            response: 响应对象
+
+        Returns:
+            响应对象，包含操作结果
+        """
+        self.lab_logger().info(f"[Host Node-Resource] Delete request for ID: {request.id}")
+
+        success = False
+        if len(self.bridges) > 0:
+            try:
+                r = self.bridges[-1].resource_delete(request.id)
+                success = bool(r)
+            except Exception as e:
+                self.lab_logger().error(f"[Host Node-Resource] Error deleting resource: {str(e)}")
+
+        response.success = success
+        self.lab_logger().info(f"[Host Node-Resource] Delete request completed, success: {success}")
+        return response
+
+    def _resource_update_callback(self, request, response):
+        """
+        更新资源回调
+
+        处理更新资源请求，将更新指令传递到桥接器
+
+        Args:
+            request: 包含资源数据的请求对象
+            response: 响应对象
+
+        Returns:
+            响应对象，包含操作结果
+        """
+        resources = [convert_from_ros_msg(resource) for resource in request.resources]
+        self.lab_logger().info(f"[Host Node-Resource] Update request received: {len(resources)} resources")
+
+        success = False
+        if len(self.bridges) > 0:
+            try:
+                r = self.bridges[-1].resource_update(add_schema(resources))
+                success = bool(r)
+            except Exception as e:
+                self.lab_logger().error(f"[Host Node-Resource] Error updating resources: {str(e)}")
+
+        response.success = success
+        self.lab_logger().info(f"[Host Node-Resource] Update request completed, success: {success}")
+        return response
+
+    def _resource_list_callback(self, request, response):
+        """
+        列出资源回调
+
+        处理列出资源请求，返回所有可用资源
+
+        Args:
+            request: 请求对象
+            response: 响应对象
+
+        Returns:
+            响应对象，包含资源列表
+        """
+        self.lab_logger().info(f"[Host Node-Resource] List request received")
+        # 这里可以实现返回资源列表的逻辑
+        self.lab_logger().debug(f"[Host Node-Resource] List parameters: {request}")
+        return response

unilabos/ros/nodes/presets/protocol_node.py

@@ -0,0 +1,267 @@
+import time
+import asyncio
+import traceback
+from typing import Union
+
+import rclpy
+from unilabos.messages import *  # type: ignore  # protocol names
+from rclpy.action import ActionServer, ActionClient
+from rclpy.action.server import ServerGoalHandle
+from rclpy.callback_groups import ReentrantCallbackGroup
+from unilabos_msgs.msg import Resource  # type: ignore
+from unilabos_msgs.srv import ResourceGet, ResourceUpdate  # type: ignore
+
+from unilabos.compile import action_protocol_generators
+from unilabos.resources.graphio import list_to_nested_dict, nested_dict_to_list
+from unilabos.ros.initialize_device import initialize_device_from_dict
+from unilabos.ros.msgs.message_converter import (
+    get_action_type,
+    convert_to_ros_msg,
+    convert_from_ros_msg,
+    convert_from_ros_msg_with_mapping,
+)
+from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode, DeviceNodeResourceTracker
+
+
+class ROS2ProtocolNode(BaseROS2DeviceNode):
+    """
+    ROS2ProtocolNode代表管理ROS2环境中设备通信和动作的协议节点。
+    它初始化设备节点，处理动作客户端，并基于指定的协议执行工作流。
+    它还物理上代表一组协同工作的设备，如带夹持器的机械臂，带传送带的CNC机器等。
+    """
+
+    # 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):
+        self._setup_protocol_names(protocol_type)
+
+        # 初始化其它属性
+        self.children = children
+        self._busy = False
+        self.sub_devices = {}
+        self._goals = {}
+        self._protocol_servers = {}
+        self._action_clients = {}
+
+        # 初始化基类，让基类处理常规动作
+        super().__init__(
+            driver_instance=self,
+            device_id=device_id,
+            status_types={},
+            action_value_mappings=self.protocol_action_mappings,
+            hardware_interface={},
+            print_publish=False,
+            resource_tracker=resource_tracker,
+        )
+
+        # 初始化子设备
+        communication_node_id = None
+        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.")
+                continue
+            d = self.initialize_device(device_id, device_config)
+            if d is None:
+                continue
+
+            if "serial_" in device_id or "io_" in device_id:
+                communication_node_id = device_id
+                continue
+
+            # 设置硬件接口代理
+            if d and hasattr(d, "_hardware_interface"):
+                if (
+                    hasattr(d, d._hardware_interface["name"])
+                    and hasattr(d, d._hardware_interface["write"])
+                    and (d._hardware_interface["read"] is None or hasattr(d, d._hardware_interface["read"]))
+                ):
+
+                    name = getattr(d, d._hardware_interface["name"])
+                    read = d._hardware_interface.get("read", None)
+                    write = d._hardware_interface.get("write", None)
+
+                    # 如果硬件接口是字符串，通过通信设备提供
+                    if isinstance(name, str) and communication_node_id in self.sub_devices:
+                        self._setup_hardware_proxy(d, self.sub_devices[communication_node_id], read, write)
+
+    def _setup_protocol_names(self, protocol_type):
+        # 处理协议类型
+        if isinstance(protocol_type, str):
+            if "," not in protocol_type:
+                self.protocol_names = [protocol_type]
+            else:
+                self.protocol_names = [protocol.strip() for protocol in protocol_type.split(",")]
+        else:
+            self.protocol_names = protocol_type
+        # 准备协议相关的动作值映射
+        self.protocol_action_mappings = {}
+        for protocol_name in self.protocol_names:
+            protocol_type = globals()[protocol_name]
+            self.protocol_action_mappings[protocol_name] = get_action_type(protocol_type)
+
+    def initialize_device(self, device_id, device_config):
+        """初始化设备并创建相应的动作客户端"""
+        device_id_abs = f"{self.device_id}/{device_id}"
+        self.lab_logger().info(f"初始化子设备: {device_id_abs}")
+        d = self.sub_devices[device_id] = initialize_device_from_dict(device_id_abs, device_config)
+
+        # 为子设备的每个动作创建动作客户端
+        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():
+                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
+                    )
+                    self.lab_logger().debug(f"为子设备 {device_id} 创建动作客户端: {action_name}")
+        return d
+
+    def create_ros_action_server(self, action_name, action_value_mapping):
+        """创建ROS动作服务器"""
+        # 和Base创建的路径是一致的
+        protocol_name = action_name
+        action_type = action_value_mapping["type"]
+        str_action_type = str(action_type)[8:-2]
+        protocol_type = globals()[protocol_name]
+        protocol_steps_generator = action_protocol_generators[protocol_type]
+
+        self._action_servers[action_name] = ActionServer(
+            self,
+            action_type,
+            action_name,
+            execute_callback=self._create_protocol_execute_callback(action_name, protocol_steps_generator),
+            callback_group=ReentrantCallbackGroup(),
+        )
+
+        self.lab_logger().debug(f"发布动作: {action_name}, 类型: {str_action_type}")
+
+    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...')
+            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"])
+
+            # 向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])
+
+            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.get_logger().info(f'Goal received: {protocol_kwargs}, running steps: \n{protocol_steps}')
+
+            time_start = time.time()
+            time_overall = 100
+            self._busy = True
+
+            # 逐步执行工作流
+            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]
+
+            # 向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)
+
+            goal_handle.succeed()
+            result = action_value_mapping["type"].Result()
+            result.success = True
+
+            self._busy = False
+            return result
+        return execute_protocol
+
+    async def execute_single_action(self, device_id, action_name, action_kwargs):
+        """执行单个动作"""
+        # 构建动作ID
+        if device_id in ["", None, "self"]:
+            action_id = f"/devices/{self.device_id}/{action_name}"
+        else:
+            action_id = f"/devices/{self.device_id}/{device_id}/{action_name}"
+
+        # 检查动作客户端是否存在
+        if action_id not in self._action_clients:
+            self.lab_logger().error(f"找不到动作客户端: {action_id}")
+            return None
+
+        # 发送动作请求
+        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}")
+        action_client.wait_for_server()
+
+        # 等待动作完成
+        request_future = action_client.send_goal_async(goal_msg)
+        handle = await request_future
+
+        if not handle.accepted:
+            self.lab_logger().error(f"动作请求被拒绝: {action_name}")
+            return None
+
+        result_future = await handle.get_result_async()
+        self.lab_logger().info(f"动作完成: {action_name}")
+
+        return result_future.result
+
+
+    """还没有改过的部分"""
+
+    def _setup_hardware_proxy(self, device, communication_device, read_method, write_method):
+        """为设备设置硬件接口代理"""
+        extra_info = [getattr(device, info) for info in communication_device._hardware_interface.get("extra_info", [])]
+        write_func = getattr(communication_device, communication_device._hardware_interface["write"])
+        read_func = getattr(communication_device, communication_device._hardware_interface["read"])
+
+        def _read():
+            return read_func(*extra_info)
+
+        def _write(command):
+            return write_func(*extra_info, command)
+
+        if read_method:
+            setattr(device, read_method, _read)
+        if write_method:
+            setattr(device, write_method, _write)
+
+
+    async def _update_resources(self, goal, protocol_kwargs):
+        """更新资源状态"""
+        for k, v in goal.get_fields_and_field_types().items():
+            if v in ["unilabos_msgs/Resource", "sequence<unilabos_msgs/Resource>"]:
+                if protocol_kwargs[k] is not None:
+                    try:
+                        r = ResourceUpdate.Request()
+                        r.resources = [
+                            convert_to_ros_msg(Resource, rs) for rs in nested_dict_to_list(protocol_kwargs[k])
+                        ]
+                        await self._resource_clients["resource_update"].call_async(r)
+                    except Exception as e:
+                        self.lab_logger().error(f"更新资源失败: {e}")

unilabos/ros/nodes/presets/serial_node.py

@@ -0,0 +1,84 @@
+from threading import Lock
+
+from unilabos_msgs.srv import SerialCommand
+from serial import Serial, SerialException
+
+from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode, DeviceNodeResourceTracker
+
+
+class ROS2SerialNode(BaseROS2DeviceNode):
+    def __init__(self, device_id, port: str, baudrate: int = 9600, resource_tracker: DeviceNodeResourceTracker=None):
+        # 保存属性，以便在调用父类初始化前使用
+        self.port = port
+        self.baudrate = baudrate
+        self._hardware_interface = {"name": "hardware_interface", "write": "send_command", "read": "read_data"}
+        self._busy = False
+        self._closing = False
+        self._query_lock = Lock()
+
+        # 初始化硬件接口
+        try:
+            self.hardware_interface = Serial(baudrate=baudrate, port=port)
+        except (OSError, SerialException) as e:
+            # 因为还没调用父类初始化，无法使用日志，直接抛出异常
+            raise RuntimeError(f"Failed to connect to serial port {port} at {baudrate} baudrate.") from e
+
+        # 初始化BaseROS2DeviceNode，使用自身作为driver_instance
+        BaseROS2DeviceNode.__init__(
+            self,
+            driver_instance=self,
+            device_id=device_id,
+            status_types={},
+            action_value_mappings={},
+            hardware_interface=self._hardware_interface,
+            print_publish=False,
+            resource_tracker=resource_tracker,
+        )
+
+        # 现在可以使用日志
+        self.lab_logger().info(
+            f"【ROS2SerialNode.__init__】初始化串口节点: {device_id}, 端口: {port}, 波特率: {baudrate}"
+        )
+        self.lab_logger().info(f"【ROS2SerialNode.__init__】成功连接串口设备")
+
+        # 创建服务
+        self.create_service(SerialCommand, "serialwrite", self.handle_serial_request)
+        self.lab_logger().info(f"【ROS2SerialNode.__init__】创建串口写入服务: serialwrite")
+
+    def send_command(self, command: str):
+        self.lab_logger().info(f"【ROS2SerialNode.send_command】发送命令: {command}")
+        with self._query_lock:
+            if self._closing:
+                self.lab_logger().error(f"【ROS2SerialNode.send_command】设备正在关闭，无法发送命令")
+                raise RuntimeError
+
+            full_command = f"{command}\n"
+            full_command_data = bytearray(full_command, "ascii")
+
+            response = self.hardware_interface.write(full_command_data)
+            # time.sleep(0.05)
+            output = self._receive(self.hardware_interface.read_until(b"\n"))
+            self.lab_logger().info(f"【ROS2SerialNode.send_command】接收响应: {output}")
+        return output
+
+    def read_data(self):
+        self.lab_logger().debug(f"【ROS2SerialNode.read_data】读取数据")
+        with self._query_lock:
+            if self._closing:
+                self.lab_logger().error(f"【ROS2SerialNode.read_data】设备正在关闭，无法读取数据")
+                raise RuntimeError
+            data = self.hardware_interface.read_until(b"\n")
+            result = self._receive(data)
+            self.lab_logger().debug(f"【ROS2SerialNode.read_data】读取到数据: {result}")
+            return result
+
+    def _receive(self, data: bytes):
+        ascii_string = "".join(chr(byte) for byte in data)
+        self.lab_logger().debug(f"【ROS2SerialNode._receive】接收数据: {ascii_string}")
+        return ascii_string
+
+    def handle_serial_request(self, request, response):
+        self.lab_logger().info(f"【ROS2SerialNode.handle_serial_request】收到串口命令请求: {request.command}")
+        response.response = self.send_command(request.command)
+        self.lab_logger().info(f"【ROS2SerialNode.handle_serial_request】命令响应: {response.response}")
+        return response