open_pub/Uni-Lab-OS
1
0
Fork 0
mirror of https://github.com/dptech-corp/Uni-Lab-OS.git synced 2026-02-04 13:25:13 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add topic config

Browse Source
This commit is contained in:
Xuwznln
2025-12-26 02:26:04 +08:00
parent 8a0f000bab
commit 9e1e6da505
3 changed files with 289 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

115
unilabos/ros/nodes/base_device_node.py
View File

@@ -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