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

Merge branch 'dev' into prcix9320

Browse Source
This commit is contained in:
q434343
2026-02-06 15:14:59 +08:00
parent abf5555e37 4f7d431c0b
commit f6d46e669d
20 changed files with 1603 additions and 361 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,12 +1,23 @@
from ast import Try
import inspect
import io
import json
import threading
import time
import traceback
from typing import get_type_hints, TypeVar, Generic, Dict, Any, Type, TypedDict, Optional, List, TYPE_CHECKING, Union, \
Tuple
from typing import (
get_type_hints,
TypeVar,
Generic,
Dict,
Any,
Type,
TypedDict,
Optional,
List,
TYPE_CHECKING,
Union,
Tuple,
)
from concurrent.futures import ThreadPoolExecutor
import asyncio
@@ -49,8 +60,10 @@ from unilabos.resources.resource_tracker import (
ResourceTreeSet,
ResourceTreeInstance,
ResourceDictInstance,
EXTRA_SAMPLE_UUID,
PARAM_SAMPLE_UUIDS,
JSON_UNILABOS_PARAM,
)
from unilabos.ros.x.rclpyx import get_event_loop
from unilabos.ros.utils.driver_creator import WorkstationNodeCreator, PyLabRobotCreator, DeviceClassCreator
from rclpy.task import Task, Future
from unilabos.utils.import_manager import default_manager
@@ -186,7 +199,7 @@ class PropertyPublisher:
f"创建发布者 {name} 失败，可能由于注册表有误，类型: {msg_type}，错误: {ex}\n{traceback.format_exc()}"
)
self.timer = node.create_timer(self.timer_period, self.publish_property)
self.__loop = get_event_loop()
self.__loop = ROS2DeviceNode.get_asyncio_loop()
str_msg_type = str(msg_type)[8:-2]
self.node.lab_logger().trace(f"发布属性: {name}, 类型: {str_msg_type}, 周期: {initial_period}秒, QoS: {qos}")
@@ -218,14 +231,15 @@ class PropertyPublisher:
def publish_property(self):
try:
self.node.lab_logger().trace(f"【.publish_property】开始发布属性: {self.name}")
# self.node.lab_logger().trace(f"【.publish_property】开始发布属性: {self.name}")
value = self.get_property()
if self.print_publish:
self.node.lab_logger().trace(f"【.publish_property】发布 {self.msg_type}: {value}")
pass
# self.node.lab_logger().trace(f"【.publish_property】发布 {self.msg_type}: {value}")
if value is not None:
msg = convert_to_ros_msg(self.msg_type, value)
self.publisher_.publish(msg)
self.node.lab_logger().trace(f"【.publish_property】属性 {self.name} 发布成功")
# self.node.lab_logger().trace(f"【.publish_property】属性 {self.name} 发布成功")
except Exception as e:
self.node.lab_logger().error(
f"【.publish_property】发布属性 {self.publisher_.topic} 出错: {str(e)}\n{traceback.format_exc()}"
@@ -363,6 +377,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
from pylabrobot.resources.deck import Deck
from pylabrobot.resources import Coordinate
from pylabrobot.resources import Plate
# 物料传输到对应的node节点
client = self._resource_clients["c2s_update_resource_tree"]
request = SerialCommand.Request()
@@ -390,33 +405,27 @@ class BaseROS2DeviceNode(Node, Generic[T]):
rts: ResourceTreeSet = ResourceTreeSet.from_raw_dict_list(input_resources)
parent_resource = None
if bind_parent_id != self.node_name:
parent_resource = self.resource_tracker.figure_resource(
{"name": bind_parent_id}
)
parent_resource = self.resource_tracker.figure_resource({"name": bind_parent_id})
for r in rts.root_nodes:
# noinspection PyUnresolvedReferences
r.res_content.parent_uuid = parent_resource.unilabos_uuid
else:
for r in rts.root_nodes:
r.res_content.parent_uuid = self.uuid
if len(LIQUID_INPUT_SLOT) and LIQUID_INPUT_SLOT[0] == -1 and len(rts.root_nodes) == 1 and isinstance(rts.root_nodes[0], RegularContainer):
rts_plr_instances = rts.to_plr_resources()
if len(rts.root_nodes) == 1 and isinstance(rts_plr_instances[0], RegularContainer):
# noinspection PyTypeChecker
container_instance: RegularContainer = rts.root_nodes[0]
found_resources = self.resource_tracker.figure_resource(
{"id": container_instance.name}, try_mode=True
)
container_instance: RegularContainer = rts_plr_instances[0]
found_resources = self.resource_tracker.figure_resource({"name": container_instance.name}, try_mode=True)
if not len(found_resources):
self.resource_tracker.add_resource(container_instance)
logger.info(f"添加物料{container_instance.name}到资源跟踪器")
else:
assert (
len(found_resources) == 1
), f"找到多个同名物料: {container_instance.name}, 请检查物料系统"
assert len(found_resources) == 1, f"找到多个同名物料: {container_instance.name}, 请检查物料系统"
found_resource = found_resources[0]
if isinstance(found_resource, RegularContainer):
logger.info(f"更新物料{container_instance.name}的数据{found_resource.state}")
found_resource.state.update(json.loads(container_instance.state))
found_resource.state.update(container_instance.state)
elif isinstance(found_resource, dict):
raise ValueError("已不支持 字典 版本的RegularContainer")
else:
@@ -424,14 +433,16 @@ class BaseROS2DeviceNode(Node, Generic[T]):
f"更新物料{container_instance.name}出现不支持的数据类型{type(found_resource)} {found_resource}"
)
# noinspection PyUnresolvedReferences
request.command = json.dumps({
"action": "add",
"data": {
"data": rts.dump(),
"mount_uuid": parent_resource.unilabos_uuid if parent_resource is not None else "",
"first_add": False,
},
})
request.command = json.dumps(
{
"action": "add",
"data": {
"data": rts.dump(),
"mount_uuid": parent_resource.unilabos_uuid if parent_resource is not None else self.uuid,
"first_add": False,
},
}
)
tree_response: SerialCommand.Response = await client.call_async(request)
uuid_maps = json.loads(tree_response.response)
plr_instances = rts.to_plr_resources()
@@ -473,7 +484,9 @@ class BaseROS2DeviceNode(Node, Generic[T]):
if len(ADD_LIQUID_TYPE) == 1 and len(LIQUID_VOLUME) == 1 and len(LIQUID_INPUT_SLOT) > 1:
ADD_LIQUID_TYPE = ADD_LIQUID_TYPE * len(LIQUID_INPUT_SLOT)
LIQUID_VOLUME = LIQUID_VOLUME * len(LIQUID_INPUT_SLOT)
self.lab_logger().warning(f"增加液体资源时，数量为1，自动补全为 {len(LIQUID_INPUT_SLOT)} 个")
self.lab_logger().warning(
f"增加液体资源时，数量为1，自动补全为 {len(LIQUID_INPUT_SLOT)} 个"
)
for liquid_type, liquid_volume, liquid_input_slot in zip(
ADD_LIQUID_TYPE, LIQUID_VOLUME, LIQUID_INPUT_SLOT
):
@@ -492,9 +505,15 @@ class BaseROS2DeviceNode(Node, Generic[T]):
input_wells = []
for r in LIQUID_INPUT_SLOT:
input_wells.append(plr_instance.children[r])
final_response["liquid_input_resource_tree"] = ResourceTreeSet.from_plr_resources(input_wells).dump()
final_response["liquid_input_resource_tree"] = ResourceTreeSet.from_plr_resources(
input_wells
).dump()
res.response = json.dumps(final_response)
if issubclass(parent_resource.__class__, Deck) and hasattr(parent_resource, "assign_child_at_slot") and "slot" in other_calling_param:
if (
issubclass(parent_resource.__class__, Deck)
and hasattr(parent_resource, "assign_child_at_slot")
and "slot" in other_calling_param
):
other_calling_param["slot"] = int(other_calling_param["slot"])
parent_resource.assign_child_at_slot(plr_instance, **other_calling_param)
else:
@@ -509,14 +528,16 @@ class BaseROS2DeviceNode(Node, Generic[T]):
rts_with_parent = ResourceTreeSet.from_plr_resources([parent_resource])
if rts_with_parent.root_nodes[0].res_content.uuid_parent is None:
rts_with_parent.root_nodes[0].res_content.parent_uuid = self.uuid
request.command = json.dumps({
"action": "add",
"data": {
"data": rts_with_parent.dump(),
"mount_uuid": rts_with_parent.root_nodes[0].res_content.uuid_parent,
"first_add": False,
},
})
request.command = json.dumps(
{
"action": "add",
"data": {
"data": rts_with_parent.dump(),
"mount_uuid": rts_with_parent.root_nodes[0].res_content.uuid_parent,
"first_add": False,
},
}
)
tree_response: SerialCommand.Response = await client.call_async(request)
uuid_maps = json.loads(tree_response.response)
self.resource_tracker.loop_update_uuid(input_resources, uuid_maps)
@@ -626,7 +647,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
) # type: ignore
raw_nodes = json.loads(response.response)
tree_set = ResourceTreeSet.from_raw_dict_list(raw_nodes)
self.lab_logger().debug(f"获取资源结果: {len(tree_set.trees)} 个资源树")
self.lab_logger().trace(f"获取资源结果: {len(tree_set.trees)} 个资源树 {tree_set.root_nodes}")
return tree_set
async def get_resource_with_dir(self, resource_id: str, with_children: bool = True) -> "ResourcePLR":
@@ -813,7 +834,9 @@ class BaseROS2DeviceNode(Node, Generic[T]):
}
def _handle_update(
plr_resources: List[Union[ResourcePLR, ResourceDictInstance]], tree_set: ResourceTreeSet, additional_add_params: Dict[str, Any]
plr_resources: List[Union[ResourcePLR, ResourceDictInstance]],
tree_set: ResourceTreeSet,
additional_add_params: Dict[str, Any],
) -> Tuple[Dict[str, Any], List[ResourcePLR]]:
"""
处理资源更新操作的内部函数
@@ -838,7 +861,10 @@ class BaseROS2DeviceNode(Node, Generic[T]):
original_parent_resource = original_instance.parent
original_parent_resource_uuid = getattr(original_parent_resource, "unilabos_uuid", None)
target_parent_resource_uuid = tree.root_node.res_content.uuid_parent
not_same_parent = original_parent_resource_uuid != target_parent_resource_uuid and original_parent_resource is not None
not_same_parent = (
original_parent_resource_uuid != target_parent_resource_uuid
and original_parent_resource is not None
)
old_name = original_instance.name
new_name = plr_resource.name
parent_appended = False
@@ -874,8 +900,16 @@ class BaseROS2DeviceNode(Node, Generic[T]):
else:
# 判断是否变更了resource_site，重新登记
target_site = original_instance.unilabos_extra.get("update_resource_site")
sites = original_instance.parent.sites if original_instance.parent is not None and hasattr(original_instance.parent, "sites") else None
site_names = list(original_instance.parent._ordering.keys()) if original_instance.parent is not None and hasattr(original_instance.parent, "sites") else []
sites = (
original_instance.parent.sites
if original_instance.parent is not None and hasattr(original_instance.parent, "sites")
else None
)
site_names = (
list(original_instance.parent._ordering.keys())
if original_instance.parent is not None and hasattr(original_instance.parent, "sites")
else []
)
if target_site is not None and sites is not None and site_names is not None:
site_index = sites.index(original_instance)
site_name = site_names[site_index]
@@ -912,9 +946,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
action = i.get("action") # remove, add, update
resources_uuid: List[str] = i.get("data") # 资源数据
additional_add_params = i.get("additional_add_params", {}) # 额外参数
self.lab_logger().trace(
f"[资源同步] 处理 {action}, " f"resources count: {len(resources_uuid)}"
)
self.lab_logger().trace(f"[资源同步] 处理 {action}, " f"resources count: {len(resources_uuid)}")
tree_set = None
if action in ["add", "update"]:
tree_set = await self.get_resource(
@@ -941,9 +973,13 @@ class BaseROS2DeviceNode(Node, Generic[T]):
tree.root_node.res_content.parent_uuid = self.uuid
r = SerialCommand.Request()
r.command = json.dumps(
{"data": {"data": new_tree_set.dump()}, "action": "update"}) # 和Update Resource一致
{"data": {"data": new_tree_set.dump()}, "action": "update"}
) # 和Update Resource一致
response: SerialCommand_Response = await self._resource_clients[
"c2s_update_resource_tree"].call_async(r) # type: ignore
"c2s_update_resource_tree"
].call_async(
r
) # type: ignore
self.lab_logger().info(f"确认资源云端 Add 结果: {response.response}")
results.append(result)
elif action == "update":
@@ -963,9 +999,13 @@ class BaseROS2DeviceNode(Node, Generic[T]):
tree.root_node.res_content.parent_uuid = self.uuid
r = SerialCommand.Request()
r.command = json.dumps(
{"data": {"data": new_tree_set.dump()}, "action": "update"}) # 和Update Resource一致
{"data": {"data": new_tree_set.dump()}, "action": "update"}
) # 和Update Resource一致
response: SerialCommand_Response = await self._resource_clients[
"c2s_update_resource_tree"].call_async(r) # type: ignore
"c2s_update_resource_tree"
].call_async(
r
) # type: ignore
self.lab_logger().info(f"确认资源云端 Update 结果: {response.response}")
results.append(result)
elif action == "remove":
@@ -1335,7 +1375,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
resource_id=resource_data["id"], with_children=True
)
if "sample_id" in resource_data:
plr_resource.unilabos_extra["sample_uuid"] = resource_data["sample_id"]
plr_resource.unilabos_extra[EXTRA_SAMPLE_UUID] = resource_data["sample_id"]
queried_resources[idx] = plr_resource
else:
uuid_indices.append((idx, unilabos_uuid, resource_data))
@@ -1348,31 +1388,17 @@ class BaseROS2DeviceNode(Node, Generic[T]):
for i, (idx, _, resource_data) in enumerate(uuid_indices):
plr_resource = plr_resources[i]
if "sample_id" in resource_data:
plr_resource.unilabos_extra["sample_uuid"] = resource_data["sample_id"]
plr_resource.unilabos_extra[EXTRA_SAMPLE_UUID] = resource_data["sample_id"]
queried_resources[idx] = plr_resource
# 第二遍：批量查询有uuid的资源
if uuid_indices:
uuids = [item[1] for item in uuid_indices]
resource_tree = await self.get_resource(uuids)
plr_resources = resource_tree.to_plr_resources()
# 通过uuid查找对应的plr_resource
tracker = self.resource_tracker
for idx, uuid, resource_data in uuid_indices:
try:
plr_resource = tracker.loop_find_with_uuid(plr_resources, uuid)
if "sample_id" in resource_data:
plr_resource.unilabos_extra["sample_uuid"] = resource_data["sample_id"]
queried_resources[idx] = plr_resource
except Exception as e:
self.lab_logger().error(f"资源查询失败: {e}\n{traceback.format_exc()}")
continue
self.lab_logger().debug(f"资源查询结果: 共 {len(queried_resources)} 个资源")
# 通过资源跟踪器获取本地实例
final_resources = queried_resources if is_sequence else queried_resources[0]
if not is_sequence:
plr = self.resource_tracker.figure_resource({"name": final_resources.name}, try_mode=False)
plr = self.resource_tracker.figure_resource(
{"name": final_resources.name}, try_mode=False
)
# 保留unilabos_extra
if hasattr(final_resources, "unilabos_extra") and hasattr(plr, "unilabos_extra"):
plr.unilabos_extra = getattr(final_resources, "unilabos_extra", {}).copy()
@@ -1411,8 +1437,12 @@ class BaseROS2DeviceNode(Node, Generic[T]):
execution_success = True
except Exception as _:
execution_error = traceback.format_exc()
error(f"异步任务 {ACTION.__name__} 报错了\n{traceback.format_exc()}\n原始输入：{str(action_kwargs)[:1000]}")
trace(f"异步任务 {ACTION.__name__} 报错了\n{traceback.format_exc()}\n原始输入：{action_kwargs}")
error(
f"异步任务 {ACTION.__name__} 报错了\n{traceback.format_exc()}\n原始输入：{str(action_kwargs)[:1000]}"
)
trace(
f"异步任务 {ACTION.__name__} 报错了\n{traceback.format_exc()}\n原始输入：{action_kwargs}"
)
future = ROS2DeviceNode.run_async_func(ACTION, trace_error=False, **action_kwargs)
future.add_done_callback(_handle_future_exception)
@@ -1432,9 +1462,11 @@ class BaseROS2DeviceNode(Node, Generic[T]):
except Exception as _:
execution_error = traceback.format_exc()
error(
f"同步任务 {ACTION.__name__} 报错了\n{traceback.format_exc()}\n原始输入：{str(action_kwargs)[:1000]}")
f"同步任务 {ACTION.__name__} 报错了\n{traceback.format_exc()}\n原始输入：{str(action_kwargs)[:1000]}"
)
trace(
f"同步任务 {ACTION.__name__} 报错了\n{traceback.format_exc()}\n原始输入：{action_kwargs}")
f"同步任务 {ACTION.__name__} 报错了\n{traceback.format_exc()}\n原始输入：{action_kwargs}"
)
future.add_done_callback(_handle_future_exception)
@@ -1501,13 +1533,18 @@ class BaseROS2DeviceNode(Node, Generic[T]):
if isinstance(rs, list):
for r in rs:
res = self.resource_tracker.parent_resource(r) # 获取 resource 对象
elif type(rs).__name__ == "ResourceHolder":
pass
if res is None:
res = rs
if id(res) not in seen:
seen.add(id(res))
unique_resources.append(res)
else:
res = self.resource_tracker.parent_resource(rs)
if id(res) not in seen:
seen.add(id(res))
unique_resources.append(res)
if res is None:
res = rs
if id(res) not in seen:
seen.add(id(res))
unique_resources.append(res)
# 使用新的资源树接口
if unique_resources:
@@ -1559,20 +1596,39 @@ class BaseROS2DeviceNode(Node, Generic[T]):
try:
function_name = target["function_name"]
function_args = target["function_args"]
# 获取 unilabos 系统参数
unilabos_param: Dict[str, Any] = target[JSON_UNILABOS_PARAM]
assert isinstance(function_args, dict), "执行动作时JSON必须为dict类型\n原JSON: {string}"
function = getattr(self.driver_instance, function_name)
assert callable(
function
), f"执行动作时JSON中的function_name对应的函数不可调用: {function_name}\n原JSON: {string}"
# 处理 ResourceSlot 类型参数
args_list = default_manager._analyze_method_signature(function)["args"]
# 处理参数（包含 unilabos 系统参数如 sample_uuids）
args_list = default_manager._analyze_method_signature(function, skip_unilabos_params=False)["args"]
for arg in args_list:
arg_name = arg["name"]
arg_type = arg["type"]
# 跳过不在 function_args 中的参数
if arg_name not in function_args:
# 处理 sample_uuids 参数注入
if arg_name == PARAM_SAMPLE_UUIDS:
raw_sample_uuids = unilabos_param.get(PARAM_SAMPLE_UUIDS, {})
# 将 material uuid 转换为 resource 实例
# key: sample_uuid, value: material_uuid -> resource 实例
resolved_sample_uuids: Dict[str, Any] = {}
for sample_uuid, material_uuid in raw_sample_uuids.items():
if material_uuid and self.resource_tracker:
resource = self.resource_tracker.uuid_to_resources.get(material_uuid)
resolved_sample_uuids[sample_uuid] = resource if resource else material_uuid
else:
resolved_sample_uuids[sample_uuid] = material_uuid
function_args[PARAM_SAMPLE_UUIDS] = resolved_sample_uuids
self.lab_logger().debug(
f"[JsonCommand] 注入 {PARAM_SAMPLE_UUIDS}: {resolved_sample_uuids}"
)
continue
# 处理单个 ResourceSlot
@@ -1601,6 +1657,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
f"转换ResourceSlot列表参数 {arg_name} 失败: {e}\n{traceback.format_exc()}"
)
raise JsonCommandInitError(f"ResourceSlot列表参数转换失败: {arg_name}")
# todo: 默认反报送
return function(**function_args)
except KeyError as ex:
@@ -1621,14 +1678,16 @@ class BaseROS2DeviceNode(Node, Generic[T]):
raise ValueError("至少需要提供一个 UUID")
uuids_list = list(uuids)
future = self._resource_clients["c2s_update_resource_tree"].call_async(SerialCommand.Request(
command=json.dumps(
{
"data": {"data": uuids_list, "with_children": True},
"action": "get",
}
future = self._resource_clients["c2s_update_resource_tree"].call_async(
SerialCommand.Request(
command=json.dumps(
{
"data": {"data": uuids_list, "with_children": True},
"action": "get",
}
)
)
))
)
# 等待结果（使用while循环，每次sleep 0.05秒，最多等待30秒）
timeout = 30.0
@@ -1686,6 +1745,9 @@ class BaseROS2DeviceNode(Node, Generic[T]):
try:
function_name = target["function_name"]
function_args = target["function_args"]
# 获取 unilabos 系统参数
unilabos_param: Dict[str, Any] = target.get(JSON_UNILABOS_PARAM, {})
assert isinstance(function_args, dict), "执行动作时JSON必须为dict类型\n原JSON: {string}"
function = getattr(self.driver_instance, function_name)
assert callable(
@@ -1695,14 +1757,30 @@ class BaseROS2DeviceNode(Node, Generic[T]):
function
), f"执行动作时JSON中的function并非异步: {function_name}\n原JSON: {string}"
# 处理 ResourceSlot 类型参数
args_list = default_manager._analyze_method_signature(function)["args"]
# 处理参数（包含 unilabos 系统参数如 sample_uuids）
args_list = default_manager._analyze_method_signature(function, skip_unilabos_params=False)["args"]
for arg in args_list:
arg_name = arg["name"]
arg_type = arg["type"]
# 跳过不在 function_args 中的参数
if arg_name not in function_args:
# 处理 sample_uuids 参数注入
if arg_name == PARAM_SAMPLE_UUIDS:
raw_sample_uuids = unilabos_param.get(PARAM_SAMPLE_UUIDS, {})
# 将 material uuid 转换为 resource 实例
# key: sample_uuid, value: material_uuid -> resource 实例
resolved_sample_uuids: Dict[str, Any] = {}
for sample_uuid, material_uuid in raw_sample_uuids.items():
if material_uuid and self.resource_tracker:
resource = self.resource_tracker.uuid_to_resources.get(material_uuid)
resolved_sample_uuids[sample_uuid] = resource if resource else material_uuid
else:
resolved_sample_uuids[sample_uuid] = material_uuid
function_args[PARAM_SAMPLE_UUIDS] = resolved_sample_uuids
self.lab_logger().debug(
f"[JsonCommandAsync] 注入 {PARAM_SAMPLE_UUIDS}: {resolved_sample_uuids}"
)
continue
# 处理单个 ResourceSlot
@@ -1792,6 +1870,15 @@ class ROS2DeviceNode:
它不继承设备类，而是通过代理模式访问设备类的属性和方法。
"""
# 类变量，用于循环管理
_asyncio_loop = None
_asyncio_loop_running = False
_asyncio_loop_thread = None
@classmethod
def get_asyncio_loop(cls):
return cls._asyncio_loop
@staticmethod
async def safe_task_wrapper(trace_callback, func, **kwargs):
try:
@@ -1868,6 +1955,11 @@ class ROS2DeviceNode:
print_publish: 是否打印发布信息
driver_is_ros:
"""
# 在初始化时检查循环状态
if ROS2DeviceNode._asyncio_loop_running and ROS2DeviceNode._asyncio_loop_thread is not None:
pass
elif ROS2DeviceNode._asyncio_loop_thread is None:
self._start_loop()
# 保存设备类是否支持异步上下文
self._has_async_context = hasattr(driver_class, "__aenter__") and hasattr(driver_class, "__aexit__")
@@ -1959,6 +2051,19 @@ class ROS2DeviceNode:
except Exception as e:
self._ros_node.lab_logger().error(f"设备后初始化失败: {e}")
def _start_loop(self):
def run_event_loop():
loop = asyncio.new_event_loop()
ROS2DeviceNode._asyncio_loop = loop
asyncio.set_event_loop(loop)
loop.run_forever()
ROS2DeviceNode._asyncio_loop_thread = threading.Thread(
target=run_event_loop, daemon=True, name="ROS2DeviceNode"
)
ROS2DeviceNode._asyncio_loop_thread.start()
logger.info(f"循环线程已启动")
class DeviceInfoType(TypedDict):
id: str

120
unilabos/ros/nodes/presets/host_node.py
View File

@@ -1,16 +1,17 @@
import collections
from dataclasses import dataclass, field
import json
import threading
import time
import traceback
import uuid
from typing import TYPE_CHECKING, Optional, Dict, Any, List, ClassVar, Set, TypedDict, Union
from dataclasses import dataclass, field
from typing import TYPE_CHECKING, Optional, Dict, Any, List, ClassVar, Set, Union
from action_msgs.msg import GoalStatus
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.msg import Resource # type: ignore
from unilabos_msgs.srv import (
ResourceAdd,
@@ -22,10 +23,20 @@ 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.placeholder_type import ResourceSlot, DeviceSlot
from unilabos.registry.registry import lab_registry
from unilabos.resources.container import RegularContainer
from unilabos.resources.graphio import initialize_resource
from unilabos.resources.registry import add_schema
from unilabos.resources.resource_tracker import (
ResourceDict,
ResourceDictInstance,
ResourceTreeSet,
ResourceTreeInstance,
RETURN_UNILABOS_SAMPLES,
JSON_UNILABOS_PARAM,
PARAM_SAMPLE_UUIDS,
)
from unilabos.ros.initialize_device import initialize_device_from_dict
from unilabos.ros.msgs.message_converter import (
get_msg_type,
@@ -36,17 +47,10 @@ from unilabos.ros.msgs.message_converter import (
)
from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode, ROS2DeviceNode, DeviceNodeResourceTracker
from unilabos.ros.nodes.presets.controller_node import ControllerNode
from unilabos.resources.resource_tracker import (
ResourceDict,
ResourceDictInstance,
ResourceTreeSet,
ResourceTreeInstance,
)
from unilabos.utils import logger
from unilabos.utils.exception import DeviceClassInvalid
from unilabos.utils.log import warning
from unilabos.utils.type_check import serialize_result_info
from unilabos.registry.placeholder_type import ResourceSlot, DeviceSlot
if TYPE_CHECKING:
from unilabos.app.ws_client import QueueItem
@@ -62,6 +66,18 @@ class TestResourceReturn(TypedDict):
devices: List[DeviceSlot]
class TestLatencyReturn(TypedDict):
"""test_latency方法的返回值类型"""
avg_rtt_ms: float
avg_time_diff_ms: float
max_time_error_ms: float
task_delay_ms: float
raw_delay_ms: float
test_count: int
status: str
class HostNode(BaseROS2DeviceNode):
"""
主机节点类，负责管理设备、资源和控制器
@@ -735,13 +751,14 @@ class HostNode(BaseROS2DeviceNode):
if bCreate:
self.lab_logger().trace(f"Status created: {device_id}.{property_name} = {msg.data}")
else:
self.lab_logger().debug(f"Status updated: {device_id}.{property_name} = {msg.data}")
self.lab_logger().trace(f"Status updated: {device_id}.{property_name} = {msg.data}")
def send_goal(
self,
item: "QueueItem",
action_type: str,
action_kwargs: Dict[str, Any],
sample_material: Dict[str, str],
server_info: Optional[Dict[str, Any]] = None,
) -> None:
"""
@@ -759,14 +776,14 @@ class HostNode(BaseROS2DeviceNode):
if action_name.startswith("auto-"):
action_name = action_name[5:]
action_id = f"/devices/{device_id}/_execute_driver_command"
action_kwargs = {
"string": json.dumps(
{
"function_name": action_name,
"function_args": action_kwargs,
}
)
json_command: Dict[str, Any] = {
"function_name": action_name,
"function_args": action_kwargs,
JSON_UNILABOS_PARAM: {
PARAM_SAMPLE_UUIDS: sample_material,
},
}
action_kwargs = {"string": json.dumps(json_command)}
if action_type.startswith("UniLabJsonCommandAsync"):
action_id = f"/devices/{device_id}/_execute_driver_command_async"
else:
@@ -777,21 +794,6 @@ class HostNode(BaseROS2DeviceNode):
raise ValueError(f"ActionClient {action_id} not found.")
action_client: ActionClient = self._action_clients[action_id]
# 遍历action_kwargs下的所有子dict，将"sample_uuid"的值赋给"sample_id"
def assign_sample_id(obj):
if isinstance(obj, dict):
if "sample_uuid" in obj:
obj["sample_id"] = obj["sample_uuid"]
obj.pop("sample_uuid")
for k, v in obj.items():
if k != "unilabos_extra":
assign_sample_id(v)
elif isinstance(obj, list):
for item in obj:
assign_sample_id(item)
assign_sample_id(action_kwargs)
goal_msg = convert_to_ros_msg(action_client._action_type.Goal(), action_kwargs)
# self.lab_logger().trace(f"[Host Node] Sending goal for {action_id}: {str(goal_msg)[:1000]}")
@@ -854,9 +856,14 @@ class HostNode(BaseROS2DeviceNode):
# 适配后端的一些额外处理
return_value = return_info.get("return_value")
if isinstance(return_value, dict):
unilabos_samples = return_info.get("unilabos_samples")
if isinstance(unilabos_samples, list):
return_info["unilabos_samples"] = unilabos_samples
unilabos_samples = return_value.pop(RETURN_UNILABOS_SAMPLES, None)
if isinstance(unilabos_samples, list) and unilabos_samples:
self.lab_logger().info(
f"[Host Node] Job {job_id[:8]} returned {len(unilabos_samples)} sample(s): "
f"{[s.get('name', s.get('id', 'unknown')) if isinstance(s, dict) else str(s)[:20] for s in unilabos_samples[:5]]}"
f"{'...' if len(unilabos_samples) > 5 else ''}"
)
return_info["samples"] = unilabos_samples
suc = return_info.get("suc", False)
if not suc:
status = "failed"
@@ -882,7 +889,7 @@ class HostNode(BaseROS2DeviceNode):
# 清理 _goals 中的记录
if job_id in self._goals:
del self._goals[job_id]
self.lab_logger().debug(f"[Host Node] Removed goal {job_id[:8]} from _goals")
self.lab_logger().trace(f"[Host Node] Removed goal {job_id[:8]} from _goals")
# 存储结果供 HTTP API 查询
try:
@@ -1327,10 +1334,20 @@ class HostNode(BaseROS2DeviceNode):
self.lab_logger().debug(f"[Host Node-Resource] List parameters: {request}")
return response
def test_latency(self):
def test_latency(self) -> TestLatencyReturn:
"""
测试网络延迟的action实现
通过5次ping-pong机制校对时间误差并计算实际延迟
Returns:
TestLatencyReturn: 包含延迟测试结果的字典，包括：
- avg_rtt_ms: 平均往返时间（毫秒）
- avg_time_diff_ms: 平均时间差（毫秒）
- max_time_error_ms: 最大时间误差（毫秒）
- task_delay_ms: 实际任务延迟（毫秒），-1表示无法计算
- raw_delay_ms: 原始时间差（毫秒），-1表示无法计算
- test_count: 有效测试次数
- status: 测试状态，"success"表示成功，"all_timeout"表示全部超时
"""
import uuid as uuid_module
@@ -1393,7 +1410,15 @@ class HostNode(BaseROS2DeviceNode):
if not ping_results:
self.lab_logger().error("❌ 所有ping-pong测试都失败了")
return {"status": "all_timeout"}
return {
"avg_rtt_ms": -1.0,
"avg_time_diff_ms": -1.0,
"max_time_error_ms": -1.0,
"task_delay_ms": -1.0,
"raw_delay_ms": -1.0,
"test_count": 0,
"status": "all_timeout",
}
# 统计分析
rtts = [r["rtt_ms"] for r in ping_results]
@@ -1401,7 +1426,7 @@ class HostNode(BaseROS2DeviceNode):
avg_rtt_ms = sum(rtts) / len(rtts)
avg_time_diff_ms = sum(time_diffs) / len(time_diffs)
max_time_diff_error_ms = max(abs(min(time_diffs)), abs(max(time_diffs)))
max_time_diff_error_ms: float = max(abs(min(time_diffs)), abs(max(time_diffs)))
self.lab_logger().info("-" * 50)
self.lab_logger().info("[测试统计]")
@@ -1441,7 +1466,7 @@ class HostNode(BaseROS2DeviceNode):
self.lab_logger().info("=" * 60)
return {
res: TestLatencyReturn = {
"avg_rtt_ms": avg_rtt_ms,
"avg_time_diff_ms": avg_time_diff_ms,
"max_time_error_ms": max_time_diff_error_ms,
@@ -1452,9 +1477,14 @@ class HostNode(BaseROS2DeviceNode):
"test_count": len(ping_results),
"status": "success",
}
return res
def test_resource(
self, resource: ResourceSlot = None, resources: List[ResourceSlot] = None, device: DeviceSlot = None, devices: List[DeviceSlot] = None
self,
resource: ResourceSlot = None,
resources: List[ResourceSlot] = None,
device: DeviceSlot = None,
devices: List[DeviceSlot] = None,
) -> TestResourceReturn:
if resources is None:
resources = []
@@ -1515,7 +1545,9 @@ class HostNode(BaseROS2DeviceNode):
# 构建服务地址
srv_address = f"/srv{namespace}/s2c_resource_tree"
self.lab_logger().trace(f"[Host Node-Resource] Host -> {device_id} ResourceTree {action} operation started -------")
self.lab_logger().trace(
f"[Host Node-Resource] Host -> {device_id} ResourceTree {action} operation started -------"
)
# 创建服务客户端
sclient = self.create_client(SerialCommand, srv_address)
@@ -1550,7 +1582,9 @@ class HostNode(BaseROS2DeviceNode):
time.sleep(0.05)
response = future.result()
self.lab_logger().trace(f"[Host Node-Resource] Host -> {device_id} ResourceTree {action} operation completed -------")
self.lab_logger().trace(
f"[Host Node-Resource] Host -> {device_id} ResourceTree {action} operation completed -------"
)
return True
except Exception as e:

2
unilabos/ros/nodes/presets/workstation.py
View File

@@ -340,6 +340,8 @@ class ROS2WorkstationNode(BaseROS2DeviceNode):
plr = self.resource_tracker.figure_resource({"name": res_name}, try_mode=False)
# 获取父资源
res = self.resource_tracker.parent_resource(plr)
if res is None:
res = plr
if id(res) not in seen:
seen.add(id(res))
unique_resources.append(res)

26
unilabos/ros/utils/driver_creator.py
View File

@@ -52,7 +52,8 @@ class DeviceClassCreator(Generic[T]):
if self.device_instance is not None:
for c in self.children:
if c.res_content.type != "device":
self.resource_tracker.add_resource(c.get_plr_nested_dict())
res = ResourceTreeSet([ResourceTreeInstance(c)]).to_plr_resources()[0]
self.resource_tracker.add_resource(res)
def create_instance(self, data: Dict[str, Any]) -> T:
"""
@@ -119,7 +120,7 @@ class PyLabRobotCreator(DeviceClassCreator[T]):
# return resource, source_type
def _process_resource_references(
self, data: Any, to_dict=False, states=None, prefix_path="", name_to_uuid=None
self, data: Any, processed_child_names: Optional[Dict[str, Any]], to_dict=False, states=None, prefix_path="", name_to_uuid=None
) -> Any:
"""
递归处理资源引用，替换_resource_child_name对应的资源
@@ -164,6 +165,7 @@ class PyLabRobotCreator(DeviceClassCreator[T]):
states[prefix_path] = resource_instance.serialize_all_state()
return serialized
else:
processed_child_names[child_name] = resource_instance
self.resource_tracker.add_resource(resource_instance)
# 立即设置UUID，state已经在resource_ulab_to_plr中处理过了
if name_to_uuid:
@@ -182,12 +184,12 @@ class PyLabRobotCreator(DeviceClassCreator[T]):
result = {}
for key, value in data.items():
new_prefix = f"{prefix_path}.{key}" if prefix_path else key
result[key] = self._process_resource_references(value, to_dict, states, new_prefix, name_to_uuid)
result[key] = self._process_resource_references(value, processed_child_names, to_dict, states, new_prefix, name_to_uuid)
return result
elif isinstance(data, list):
return [
self._process_resource_references(item, to_dict, states, f"{prefix_path}[{i}]", name_to_uuid)
self._process_resource_references(item, processed_child_names, to_dict, states, f"{prefix_path}[{i}]", name_to_uuid)
for i, item in enumerate(data)
]
@@ -234,7 +236,7 @@ class PyLabRobotCreator(DeviceClassCreator[T]):
# 首先处理资源引用
states = {}
processed_data = self._process_resource_references(
data, to_dict=True, states=states, name_to_uuid=name_to_uuid
data, {}, to_dict=True, states=states, name_to_uuid=name_to_uuid
)
try:
@@ -270,7 +272,12 @@ class PyLabRobotCreator(DeviceClassCreator[T]):
arg_value = spec_args[param_name].annotation
data[param_name]["_resource_type"] = self.device_cls.__module__ + ":" + arg_value
logger.debug(f"自动补充 _resource_type: {data[param_name]['_resource_type']}")
processed_data = self._process_resource_references(data, to_dict=False, name_to_uuid=name_to_uuid)
processed_child_names = {}
processed_data = self._process_resource_references(data, processed_child_names, to_dict=False, name_to_uuid=name_to_uuid)
for child_name, resource_instance in processed_data.items():
for ind, name in enumerate([child.res_content.name for child in self.children]):
if name == child_name:
self.children.pop(ind)
self.device_instance = super(PyLabRobotCreator, self).create_instance(processed_data) # 补全变量后直接调用，调用的自身的attach_resource
except Exception as e:
logger.error(f"PyLabRobot创建实例失败: {e}")
@@ -342,9 +349,10 @@ class WorkstationNodeCreator(DeviceClassCreator[T]):
try:
# 创建实例，额外补充一个给protocol node的字段，后面考虑取消
data["children"] = self.children
for child in self.children:
if child.res_content.type != "device":
self.resource_tracker.add_resource(child.get_plr_nested_dict())
# super(WorkstationNodeCreator, self).create_instance(data)的时候会attach
# for child in self.children:
# if child.res_content.type != "device":
# self.resource_tracker.add_resource(child.get_plr_nested_dict())
deck_dict = data.get("deck")
if deck_dict:
from pylabrobot.resources import Deck, Resource