From 9e1e6da505e658109eed382df4676c7546e48736 Mon Sep 17 00:00:00 2001 From: Xuwznln <18435084+Xuwznln@users.noreply.github.com> Date: Fri, 26 Dec 2025 02:26:04 +0800 Subject: [PATCH] Add topic config --- unilabos/ros/nodes/base_device_node.py | 115 ++++++++++++++- unilabos/test/registry/example_devices.py | 49 ++----- unilabos/utils/decorator.py | 170 ++++++++++++++++++++++ 3 files changed, 289 insertions(+), 45 deletions(-) diff --git a/unilabos/ros/nodes/base_device_node.py b/unilabos/ros/nodes/base_device_node.py index 6952320..88dd4be 100644 --- a/unilabos/ros/nodes/base_device_node.py +++ b/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(" str: + return "Idle" - 支持多种泵送模式，具有高精度流量控制和自动校准功能。 - 适用于实验室自动化系统中的液体处理任务。 - """ - - _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 + async def action(self, addr: str) -> bool: return True + + + + def disconnect_device(self) -> bool: """ 断开设备连接 diff --git a/unilabos/utils/decorator.py b/unilabos/utils/decorator.py index 77e473c..667f353 100644 --- a/unilabos/utils/decorator.py +++ b/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