Add topic config

unilabos/ros/nodes/base_device_node.py

@@ -21,6 +21,7 @@ from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.service import Service
 from unilabos_msgs.action import SendCmd
 from unilabos_msgs.srv._serial_command import SerialCommand_Request, SerialCommand_Response
+from unilabos.utils.decorator import get_topic_config, get_all_subscriptions
 
 from unilabos.resources.container import RegularContainer
 from unilabos.resources.graphio import (
@@ -48,7 +49,8 @@ from unilabos_msgs.msg import Resource  # type: ignore
 from unilabos.ros.nodes.resource_tracker import (
     DeviceNodeResourceTracker,
     ResourceTreeSet,
-    ResourceTreeInstance, ResourceDictInstance,
+    ResourceTreeInstance,
+    ResourceDictInstance,
 )
 from unilabos.ros.x.rclpyx import get_event_loop
 from unilabos.ros.utils.driver_creator import WorkstationNodeCreator, PyLabRobotCreator, DeviceClassCreator
@@ -168,6 +170,7 @@ class PropertyPublisher:
         msg_type,
         initial_period: float = 5.0,
         print_publish=True,
+        qos: int = 10,
     ):
         self.node = node
         self.name = name
@@ -175,10 +178,11 @@ class PropertyPublisher:
         self.get_method = get_method
         self.timer_period = initial_period
         self.print_publish = print_publish
+        self.qos = qos
 
         self._value = None
         try:
-            self.publisher_ = node.create_publisher(msg_type, f"{name}", 10)
+            self.publisher_ = node.create_publisher(msg_type, f"{name}", qos)
         except AttributeError as ex:
             self.node.lab_logger().error(
                 f"创建发布者 {name} 失败，可能由于注册表有误，类型: {msg_type}，错误: {ex}\n{traceback.format_exc()}"
@@ -186,7 +190,7 @@ class PropertyPublisher:
         self.timer = node.create_timer(self.timer_period, self.publish_property)
         self.__loop = get_event_loop()
         str_msg_type = str(msg_type)[8:-2]
-        self.node.lab_logger().trace(f"发布属性: {name}, 类型: {str_msg_type}, 周期: {initial_period}秒")
+        self.node.lab_logger().trace(f"发布属性: {name}, 类型: {str_msg_type}, 周期: {initial_period}秒, QoS: {qos}")
 
     def get_property(self):
         if asyncio.iscoroutinefunction(self.get_method):
@@ -326,6 +330,10 @@ class BaseROS2DeviceNode(Node, Generic[T]):
                     continue
                 self.create_ros_action_server(action_name, action_value_mapping)
 
+        # 创建订阅者（通过 @subscribe 装饰器）
+        self._topic_subscribers: Dict[str, Any] = {}
+        self._setup_decorated_subscribers()
+
         # 创建线程池执行器
         self._executor = ThreadPoolExecutor(
             max_workers=max(len(action_value_mappings), 1), thread_name_prefix=f"ROSDevice{self.device_id}"
@@ -1043,6 +1051,29 @@ class BaseROS2DeviceNode(Node, Generic[T]):
 
     def create_ros_publisher(self, attr_name, msg_type, initial_period=5.0):
         """创建ROS发布者"""
+        # 检测装饰器配置（支持 get_{attr_name} 方法和 @property）
+        topic_config = {}
+
+        # 优先检测 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 装饰的属性
+        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)
+
+        # 使用装饰器配置或默认值
+        cfg_period = topic_config.get("period")
+        cfg_print = topic_config.get("print_publish")
+        cfg_qos = topic_config.get("qos")
+        period: float = cfg_period if cfg_period is not None else initial_period
+        print_publish: bool = cfg_print if cfg_print is not None else self._print_publish
+        qos: int = cfg_qos if cfg_qos is not None else 10
 
         # 获取属性值的方法
         def get_device_attr():
@@ -1063,7 +1094,7 @@ 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, initial_period, self._print_publish
+            self, attr_name, get_device_attr, msg_type, period, print_publish, qos
         )
 
     def create_ros_action_server(self, action_name, action_value_mapping):
@@ -1081,6 +1112,76 @@ class BaseROS2DeviceNode(Node, Generic[T]):
 
         self.lab_logger().trace(f"发布动作: {action_name}, 类型: {str_action_type}")
 
+    def _setup_decorated_subscribers(self):
+        """扫描 driver_instance 中带有 @subscribe 装饰器的方法并创建订阅者"""
+        subscriptions = get_all_subscriptions(self.driver_instance)
+
+        for method_name, method, config in subscriptions:
+            topic_template = config.get("topic")
+            msg_type = config.get("msg_type")
+            qos = config.get("qos", 10)
+
+            if not topic_template:
+                self.lab_logger().warning(f"订阅方法 {method_name} 缺少 topic 配置，跳过")
+                continue
+
+            # 如果没有指定 msg_type，尝试从类型注解推断
+            if msg_type is None:
+                try:
+                    hints = get_type_hints(method)
+                    # 第一个参数是 self，第二个是 msg
+                    param_names = list(hints.keys())
+                    if param_names:
+                        msg_type = hints[param_names[0]]
+                except Exception:
+                    pass
+
+            if msg_type is None:
+                self.lab_logger().warning(f"订阅方法 {method_name} 缺少 msg_type 配置且无法从类型注解推断，跳过")
+                continue
+
+            # 替换 topic 模板中的占位符
+            topic = self._resolve_topic_template(topic_template)
+
+            self.create_ros_subscriber(topic, msg_type, method, qos)
+
+    def _resolve_topic_template(self, topic_template: str) -> str:
+        """
+        解析 topic 模板，替换占位符
+
+        支持的占位符:
+            - {device_id}: 设备ID
+            - {namespace}: 完整命名空间
+        """
+        return topic_template.format(
+            device_id=self.device_id,
+            namespace=self.namespace,
+        )
+
+    def create_ros_subscriber(self, topic: str, msg_type, callback, qos: int = 10):
+        """
+        创建ROS订阅者
+
+        Args:
+            topic: Topic 名称
+            msg_type: ROS 消息类型
+            callback: 回调方法（会自动绑定到 driver_instance）
+            qos: QoS 深度配置
+        """
+        try:
+            subscription = self.create_subscription(
+                msg_type,
+                topic,
+                callback,
+                qos,
+                callback_group=self.callback_group,
+            )
+            self._topic_subscribers[topic] = subscription
+            str_msg_type = str(msg_type)[8:-2] if str(msg_type).startswith("<class") else str(msg_type)
+            self.lab_logger().trace(f"订阅Topic: {topic}, 类型: {str_msg_type}, QoS: {qos}")
+        except Exception as ex:
+            self.lab_logger().error(f"创建订阅者 {topic} 失败，类型: {msg_type}，错误: {ex}\n{traceback.format_exc()}")
+
     def get_real_function(self, instance, attr_name):
         if hasattr(instance.__class__, attr_name):
             obj = getattr(instance.__class__, attr_name)
@@ -1172,6 +1273,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
                 if asyncio.iscoroutinefunction(ACTION):
                     try:
                         self.lab_logger().trace(f"异步执行动作 {ACTION}")
+
                         def _handle_future_exception(fut: Future):
                             nonlocal execution_error, execution_success, action_return_value
                             try:
@@ -1540,6 +1642,7 @@ class ROS2DeviceNode:
     这个类封装了设备类实例和ROS2节点的功能，提供ROS2接口。
     它不继承设备类，而是通过代理模式访问设备类的属性和方法。
     """
+
     @staticmethod
     async def safe_task_wrapper(trace_callback, func, **kwargs):
         try:
@@ -1562,7 +1665,9 @@ class ROS2DeviceNode:
                 error(f"异步任务 {func.__name__} 获取结果失败")
                 error(traceback.format_exc())
 
-        future = rclpy.get_global_executor().create_task(ROS2DeviceNode.safe_task_wrapper(inner_trace_callback, func, **kwargs))
+        future = rclpy.get_global_executor().create_task(
+            ROS2DeviceNode.safe_task_wrapper(inner_trace_callback, func, **kwargs)
+        )
         if trace_error:
             future.add_done_callback(_handle_future_exception)
         return future

unilabos/test/registry/example_devices.py

@@ -7,49 +7,18 @@ from typing import Dict, Any, List
 from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode
 
 
-class SmartPumpController:
+class AnyDevice:
-    """
+    @property
-    智能泵控制器
+    def status(self) -> str:
+        return "Idle"
 
-    支持多种泵送模式，具有高精度流量控制和自动校准功能。
+    async def action(self, addr: str) -> bool:
-    适用于实验室自动化系统中的液体处理任务。
-    """
-
-    _ros_node: BaseROS2DeviceNode
-
-    def __init__(self, device_id: str = "smart_pump_01", port: str = "/dev/ttyUSB0"):
-        """
-        初始化智能泵控制器
-
-        Args:
-            device_id: 设备唯一标识符
-            port: 通信端口
-        """
-        self.device_id = device_id
-        self.port = port
-        self.is_connected = False
-        self.current_flow_rate = 0.0
-        self.total_volume_pumped = 0.0
-        self.calibration_factor = 1.0
-        self.pump_mode = "continuous"  # continuous, volume, rate
-
-    def post_init(self, ros_node: BaseROS2DeviceNode):
-        self._ros_node = ros_node
-
-    def connect_device(self, timeout: int = 10) -> bool:
-        """
-        连接到泵设备
-
-        Args:
-            timeout: 连接超时时间（秒）
-
-        Returns:
-            bool: 连接是否成功
-        """
-        # 模拟连接过程
-        self.is_connected = True
         return True
 
+
+
+
+
     def disconnect_device(self) -> bool:
         """
         断开设备连接

unilabos/utils/decorator.py

@@ -1,3 +1,9 @@
+from functools import wraps
+from typing import Any, Callable, Optional, TypeVar
+
+F = TypeVar("F", bound=Callable[..., Any])
+
+
 def singleton(cls):
     """
     单例装饰器
@@ -12,3 +18,167 @@ def singleton(cls):
 
     return get_instance
 
+
+def topic_config(
+    period: Optional[float] = None,
+    print_publish: Optional[bool] = None,
+    qos: Optional[int] = None,
+) -> Callable[[F], F]:
+    """
+    Topic发布配置装饰器
+
+    用于装饰 get_{attr_name} 方法或 @property，控制对应属性的ROS topic发布行为。
+
+    Args:
+        period: 发布周期（秒）。None 表示使用默认值 5.0
+        print_publish: 是否打印发布日志。None 表示使用节点默认配置
+        qos: QoS深度配置。None 表示使用默认值 10
+
+    Example:
+        class MyDriver:
+            # 方式1: 装饰 get_{attr_name} 方法
+            @topic_config(period=1.0, print_publish=False, qos=5)
+            def get_temperature(self):
+                return self._temperature
+
+            # 方式2: 与 @property 连用（topic_config 放在下面）
+            @property
+            @topic_config(period=0.1)
+            def position(self):
+                return self._position
+
+    Note:
+        与 @property 连用时，@topic_config 必须放在 @property 下面，
+        这样装饰器执行顺序为：先 topic_config 添加配置，再 property 包装。
+    """
+
+    def decorator(func: F) -> F:
+        @wraps(func)
+        def wrapper(*args, **kwargs):
+            return func(*args, **kwargs)
+
+        # 在函数上附加配置属性 (type: ignore 用于动态属性)
+        wrapper._topic_period = period  # type: ignore[attr-defined]
+        wrapper._topic_print_publish = print_publish  # type: ignore[attr-defined]
+        wrapper._topic_qos = qos  # type: ignore[attr-defined]
+        wrapper._has_topic_config = True  # type: ignore[attr-defined]
+
+        return wrapper  # type: ignore[return-value]
+
+    return decorator
+
+
+def get_topic_config(func) -> dict:
+    """
+    获取函数上的topic配置
+
+    Args:
+        func: 被装饰的函数
+
+    Returns:
+        包含 period, print_publish, qos 的配置字典
+    """
+    if hasattr(func, "_has_topic_config") and getattr(func, "_has_topic_config", False):
+        return {
+            "period": getattr(func, "_topic_period", None),
+            "print_publish": getattr(func, "_topic_print_publish", None),
+            "qos": getattr(func, "_topic_qos", None),
+        }
+    return {}
+
+
+def subscribe(
+    topic: str,
+    msg_type: Optional[type] = None,
+    qos: int = 10,
+) -> Callable[[F], F]:
+    """
+    Topic订阅装饰器
+
+    用于装饰 driver 类中的方法，使其成为 ROS topic 的订阅回调。
+    当 ROS2DeviceNode 初始化时，会自动扫描并创建对应的订阅者。
+
+    Args:
+        topic: Topic 名称模板，支持以下占位符：
+            - {device_id}: 设备ID (如 "pump_1")
+            - {namespace}: 完整命名空间 (如 "/devices/pump_1")
+        msg_type: ROS 消息类型。如果为 None，需要在回调函数的类型注解中指定
+        qos: QoS 深度配置，默认为 10
+
+    Example:
+        from std_msgs.msg import String, Float64
+
+        class MyDriver:
+            @subscribe(topic="/devices/{device_id}/set_speed", msg_type=Float64)
+            def on_speed_update(self, msg: Float64):
+                self._speed = msg.data
+                print(f"Speed updated to: {self._speed}")
+
+            @subscribe(topic="{namespace}/command")
+            def on_command(self, msg: String):
+                # msg_type 可从类型注解推断
+                self.execute_command(msg.data)
+
+    Note:
+        - 回调方法的第一个参数是 self，第二个参数是收到的 ROS 消息
+        - topic 中的占位符会在创建订阅时被实际值替换
+    """
+
+    def decorator(func: F) -> F:
+        @wraps(func)
+        def wrapper(*args, **kwargs):
+            return func(*args, **kwargs)
+
+        # 在函数上附加订阅配置
+        wrapper._subscribe_topic = topic  # type: ignore[attr-defined]
+        wrapper._subscribe_msg_type = msg_type  # type: ignore[attr-defined]
+        wrapper._subscribe_qos = qos  # type: ignore[attr-defined]
+        wrapper._has_subscribe = True  # type: ignore[attr-defined]
+
+        return wrapper  # type: ignore[return-value]
+
+    return decorator
+
+
+def get_subscribe_config(func) -> dict:
+    """
+    获取函数上的订阅配置
+
+    Args:
+        func: 被装饰的函数
+
+    Returns:
+        包含 topic, msg_type, qos 的配置字典
+    """
+    if hasattr(func, "_has_subscribe") and getattr(func, "_has_subscribe", False):
+        return {
+            "topic": getattr(func, "_subscribe_topic", None),
+            "msg_type": getattr(func, "_subscribe_msg_type", None),
+            "qos": getattr(func, "_subscribe_qos", 10),
+        }
+    return {}
+
+
+def get_all_subscriptions(instance) -> list:
+    """
+    扫描实例的所有方法，获取带有 @subscribe 装饰器的方法及其配置
+
+    Args:
+        instance: 要扫描的实例
+
+    Returns:
+        包含 (method_name, method, config) 元组的列表
+    """
+    subscriptions = []
+    for attr_name in dir(instance):
+        if attr_name.startswith("_"):
+            continue
+        try:
+            attr = getattr(instance, attr_name)
+            if callable(attr):
+                config = get_subscribe_config(attr)
+                if config:
+                    subscriptions.append((attr_name, attr, config))
+        except Exception:
+            pass
+    return subscriptions