open_pub/Uni-Lab-OS
1
0
Fork 0
mirror of https://github.com/dptech-corp/Uni-Lab-OS.git synced 2025-12-17 04:51:10 +00:00
Code Issues Packages Projects Releases Wiki Activity

0.10.7 Update (#101)

Browse Source 
* Cleanup registry to be easy-understanding (#76)

* delete deprecated mock devices

* rename categories

* combine chromatographic devices

* rename rviz simulation nodes

* organic virtual devices

* parse vessel_id

* run registry completion before merge

---------

Co-authored-by: Xuwznln <18435084+Xuwznln@users.noreply.github.com>

* fix: workstation handlers and vessel_id parsing

* fix: working dir error when input config path
feat: report publish topic when error

* modify default discovery_interval to 15s

* feat: add trace log level

* feat: 添加ChinWe设备控制类，支持串口通信和电机控制功能 (#79)

* fix: drop_tips not using auto resource select

* fix: discard_tips error

* fix: discard_tips

* fix: prcxi_res

* add: prcxi res
fix: startup slow

* feat: workstation example

* fix pumps and liquid_handler handle

* feat: 优化protocol node节点运行日志

* fix all protocol_compilers and remove deprecated devices

* feat: 新增use_remote_resource参数

* fix and remove redundant info

* bugfixes on organic protocols

* fix filter protocol

* fix protocol node

* 临时兼容错误的driver写法

* fix: prcxi import error

* use call_async in all service to avoid deadlock

* fix: figure_resource

* Update recipe.yaml

* add workstation template and battery example

* feat: add sk & ak

* update workstation base

* Create workstation_architecture.md

* refactor: workstation_base 重构为仅含业务逻辑，通信和子设备管理交给 ProtocolNode

* refactor: ProtocolNode→WorkstationNode

* Add:msgs.action (#83)

* update: Workstation dev 将版本号从 0.10.3 更新为 0.10.4 (#84)

* Add:msgs.action

* update: 将版本号从 0.10.3 更新为 0.10.4

* simplify resource system

* uncompleted refactor

* example for use WorkstationBase

* feat: websocket

* feat: websocket test

* feat: workstation example

* feat: action status

* fix: station自己的方法注册错误

* fix: 还原protocol node处理方法

* fix: build

* fix: missing job_id key

* ws test version 1

* ws test version 2

* ws protocol

* 增加物料关系上传日志

* 增加物料关系上传日志

* 修正物料关系上传

* 修复工站的tracker实例追踪失效问题

* 增加handle检测，增加material edge关系上传

* 修复event loop错误

* 修复edge上报错误

* 修复async错误

* 更新schema的title字段

* 主机节点信息等支持自动刷新

* 注册表编辑器

* 修复status密集发送时，消息出错

* 增加addr参数

* fix: addr param

* fix: addr param

* 取消labid 和 强制config输入

* Add action definitions for LiquidHandlerSetGroup and LiquidHandlerTransferGroup

- Created LiquidHandlerSetGroup.action with fields for group name, wells, and volumes.
- Created LiquidHandlerTransferGroup.action with fields for source and target group names and unit volume.
- Both actions include response fields for return information and success status.

* Add LiquidHandlerSetGroup and LiquidHandlerTransferGroup actions to CMakeLists

* Add set_group and transfer_group methods to PRCXI9300Handler and update liquid_handler.yaml

* result_info改为字典类型

* 新增uat的地址替换

* runze multiple pump support

(cherry picked from commit 49354fcf39)

* remove runze multiple software obtainer

(cherry picked from commit 8bcc92a394)

* support multiple backbone

(cherry picked from commit 4771ff2347)

* Update runze pump format

* Correct runze multiple backbone

* Update runze_multiple_backbone

* Correct runze pump multiple receive method.

* Correct runze pump multiple receive method.

* 对于PRCXI9320的transfer_group，一对多和多对多

* 移除MQTT，更新launch文档，提供注册表示例文件，更新到0.10.5

* fix import error

* fix dupe upload registry

* refactor ws client

* add server timeout

* Fix: run-column with correct vessel id (#86)

* fix run_column

* Update run_column_protocol.py

(cherry picked from commit e5aa4d940a)

* resource_update use resource_add

* 新增版位推荐功能

* 重新规定了版位推荐的入参

* update registry with nested obj

* fix protocol node log_message, added create_resource return value

* fix protocol node log_message, added create_resource return value

* try fix add protocol

* fix resource_add

* 修复移液站错误的aspirate注册表

* Feature/xprbalance-zhida (#80)

* feat(devices): add Zhida GC/MS pretreatment automation workstation

* feat(devices): add mettler_toledo xpr balance

* balance

* 重新补全zhida注册表

* PRCXI9320 json

* PRCXI9320 json

* PRCXI9320 json

* fix resource download

* remove class for resource

* bump version to 0.10.6

* 更新所有注册表

* 修复protocolnode的兼容性

* 修复protocolnode的兼容性

* Update install md

* Add Defaultlayout

* 更新物料接口

* fix dict to tree/nested-dict converter

* coin_cell_station draft

* refactor: rename "station_resource" to "deck"

* add standardized BIOYOND resources: bottle_carrier, bottle

* refactor and add BIOYOND resources tests

* add BIOYOND deck assignment and pass all tests

* fix: update resource with correct structure; remove deprecated liquid_handler set_group action

* feat: 将新威电池测试系统驱动与配置文件并入 workstation_dev_YB2 (#92)

* feat: 新威电池测试系统驱动与注册文件

* feat: bring neware driver & battery.json into workstation_dev_YB2

* add bioyond studio draft

* bioyond station with communication init and resource sync

* fix bioyond station and registry

* fix: update resource with correct structure; remove deprecated liquid_handler set_group action

* frontend_docs

* create/update resources with POST/PUT for big amount/ small amount data

* create/update resources with POST/PUT for big amount/ small amount data

* refactor: add itemized_carrier instead of carrier consists of ResourceHolder

* create warehouse by factory func

* update bioyond launch json

* add child_size for itemized_carrier

* fix bioyond resource io

* Workstation templates: Resources and its CRUD, and workstation tasks (#95)

* coin_cell_station draft

* refactor: rename "station_resource" to "deck"

* add standardized BIOYOND resources: bottle_carrier, bottle

* refactor and add BIOYOND resources tests

* add BIOYOND deck assignment and pass all tests

* fix: update resource with correct structure; remove deprecated liquid_handler set_group action

* feat: 将新威电池测试系统驱动与配置文件并入 workstation_dev_YB2 (#92)

* feat: 新威电池测试系统驱动与注册文件

* feat: bring neware driver & battery.json into workstation_dev_YB2

* add bioyond studio draft

* bioyond station with communication init and resource sync

* fix bioyond station and registry

* create/update resources with POST/PUT for big amount/ small amount data

* refactor: add itemized_carrier instead of carrier consists of ResourceHolder

* create warehouse by factory func

* update bioyond launch json

* add child_size for itemized_carrier

* fix bioyond resource io

---------

Co-authored-by: h840473807 <47357934+h840473807@users.noreply.github.com>
Co-authored-by: Xie Qiming <97236197+Andy6M@users.noreply.github.com>

* 更新物料接口

* Workstation dev yb2 (#100)

* Refactor and extend reaction station action messages

* Refactor dispensing station tasks to enhance parameter clarity and add batch processing capabilities

- Updated `create_90_10_vial_feeding_task` to include detailed parameters for 90%/10% vial feeding, improving clarity and usability.
- Introduced `create_batch_90_10_vial_feeding_task` for batch processing of 90%/10% vial feeding tasks with JSON formatted input.
- Added `create_batch_diamine_solution_task` for batch preparation of diamine solution, also utilizing JSON formatted input.
- Refined `create_diamine_solution_task` to include additional parameters for better task configuration.
- Enhanced schema descriptions and default values for improved user guidance.

* 修复to_plr_resources

* add update remove

* 支持选择器注册表自动生成
支持转运物料

* 修复资源添加

* 修复transfer_resource_to_another生成

* 更新transfer_resource_to_another参数，支持spot入参

* 新增test_resource动作

* fix host_node error

* fix host_node test_resource error

* fix host_node test_resource error

* 过滤本地动作

* 移动内部action以兼容host node

* 修复同步任务报错不显示的bug

* feat: 允许返回非本节点物料，后面可以通过decoration进行区分，就不进行warning了

* update todo

* modify bioyond/plr converter, bioyond resource registry, and tests

* pass the tests

* update todo

* add conda-pack-build.yml

* add auto install script for conda-pack-build.yml

(cherry picked from commit 172599adcf)

* update conda-pack-build.yml

* update conda-pack-build.yml

* update conda-pack-build.yml

* update conda-pack-build.yml

* update conda-pack-build.yml

* Add version in __init__.py
Update conda-pack-build.yml
Add create_zip_archive.py

* Update conda-pack-build.yml

* Update conda-pack-build.yml (with mamba)

* Update conda-pack-build.yml

* Fix FileNotFoundError

* Try fix 'charmap' codec can't encode characters in position 16-23: character maps to <undefined>

* Fix unilabos msgs search error

* Fix environment_check.py

* Update recipe.yaml

* Update registry. Update uuid loop figure method. Update install docs.

* Fix nested conda pack

* Fix one-key installation path error

* Bump version to 0.10.7

* Workshop bj (#99)

* Add LaiYu Liquid device integration and tests

Introduce LaiYu Liquid device implementation, including backend, controllers, drivers, configuration, and resource files. Add hardware connection, tip pickup, and simplified test scripts, as well as experiment and registry configuration for LaiYu Liquid. Documentation and .gitignore for the device are also included.

* feat(LaiYu_Liquid): 重构设备模块结构并添加硬件文档

refactor: 重新组织LaiYu_Liquid模块目录结构
docs: 添加SOPA移液器和步进电机控制指令文档
fix: 修正设备配置中的最大体积默认值
test: 新增工作台配置测试用例
chore: 删除过时的测试脚本和配置文件

* add

* 重构: 将 LaiYu_Liquid.py 重命名为 laiyu_liquid_main.py 并更新所有导入引用

- 使用 git mv 将 LaiYu_Liquid.py 重命名为 laiyu_liquid_main.py
- 更新所有相关文件中的导入引用
- 保持代码功能不变，仅改善命名一致性
- 测试确认所有导入正常工作

* 修复: 在 core/__init__.py 中添加 LaiYuLiquidBackend 导出

- 添加 LaiYuLiquidBackend 到导入列表
- 添加 LaiYuLiquidBackend 到 __all__ 导出列表
- 确保所有主要类都可以正确导入

* 修复大小写文件夹名字

* 电池装配工站二次开发教程（带目录）上传至dev (#94)

* 电池装配工站二次开发教程

* Update intro.md

* 物料教程

* 更新物料教程，json格式注释

* Update prcxi driver & fix transfer_liquid mix_times (#90)

* Update prcxi driver & fix transfer_liquid mix_times

* fix: correct mix_times type

* Update liquid_handler registry

* test: prcxi.py

* Update registry from pr

* fix ony-key script not exist

* clean files

---------

Co-authored-by: Junhan Chang <changjh@dp.tech>
Co-authored-by: ZiWei <131428629+ZiWei09@users.noreply.github.com>
Co-authored-by: Guangxin Zhang <guangxin.zhang.bio@gmail.com>
Co-authored-by: Xie Qiming <97236197+Andy6M@users.noreply.github.com>
Co-authored-by: h840473807 <47357934+h840473807@users.noreply.github.com>
Co-authored-by: LccLink <1951855008@qq.com>
Co-authored-by: lixinyu1011 <61094742+lixinyu1011@users.noreply.github.com>
Co-authored-by: shiyubo0410 <shiyubo@dp.tech>
This commit is contained in:
Xuwznln
2025-10-12 23:34:26 +08:00
committed by GitHub
parent 172599adcf
commit 9aeffebde1
229 changed files with 136969 additions and 17429 deletions
Download Patch File Download Diff File Expand all files Collapse all files

134
unilabos/ros/main_slave_run.py
View File

@@ -1,25 +1,21 @@
import copy
import json
import os
import threading
import time
from typing import Optional, Dict, Any, List
import rclpy
from unilabos_msgs.srv._serial_command import SerialCommand_Response
from unilabos.ros.nodes.presets.resource_mesh_manager import ResourceMeshManager
from unilabos.ros.nodes.resource_tracker import DeviceNodeResourceTracker
from unilabos.ros.nodes.resource_tracker import DeviceNodeResourceTracker, ResourceTreeSet
from unilabos.devices.ros_dev.liquid_handler_joint_publisher import LiquidHandlerJointPublisher
from unilabos_msgs.msg import Resource # type: ignore
from unilabos_msgs.srv import ResourceAdd, SerialCommand # type: ignore
from unilabos_msgs.srv import SerialCommand # type: ignore
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node
from rclpy.timer import Timer
from unilabos.registry.registry import lab_registry
from unilabos.ros.initialize_device import initialize_device_from_dict
from unilabos.ros.msgs.message_converter import (
convert_to_ros_msg,
)
from unilabos.ros.nodes.presets.host_node import HostNode
from unilabos.utils import logger
from unilabos.config.config import BasicConfig
@@ -43,9 +39,9 @@ def exit() -> None:
def main(
devices_config: Dict[str, Any] = {},
resources_config: list=[],
resources_edge_config: list=[],
devices_config: ResourceTreeSet,
resources_config: ResourceTreeSet,
resources_edge_config: list[dict] = [],
graph: Optional[Dict[str, Any]] = None,
controllers_config: Dict[str, Any] = {},
bridges: List[Any] = [],
@@ -73,18 +69,22 @@ def main(
if visual != "disable":
from unilabos.ros.nodes.presets.joint_republisher import JointRepublisher
# 将 ResourceTreeSet 转换为 list 用于 visual 组件
resources_list = (
[node.res_content.model_dump(by_alias=True) for node in resources_config.all_nodes]
if resources_config
else []
)
resource_mesh_manager = ResourceMeshManager(
resources_mesh_config,
resources_config,
resource_tracker = host_node.resource_tracker,
device_id = 'resource_mesh_manager',
resources_list,
resource_tracker=host_node.resource_tracker,
device_id="resource_mesh_manager",
)
joint_republisher = JointRepublisher(
'joint_republisher',
host_node.resource_tracker
joint_republisher = JointRepublisher("joint_republisher", host_node.resource_tracker)
lh_joint_pub = LiquidHandlerJointPublisher(
resources_config=resources_list, resource_tracker=host_node.resource_tracker
)
lh_joint_pub = LiquidHandlerJointPublisher(resources_config=resources_config,
resource_tracker=host_node.resource_tracker)
executor.add_node(resource_mesh_manager)
executor.add_node(joint_republisher)
executor.add_node(lh_joint_pub)
@@ -97,9 +97,9 @@ def main(
def slave(
devices_config: Dict[str, Any] = {},
resources_config=[],
resources_edge_config=[],
devices_config: ResourceTreeSet,
resources_config: ResourceTreeSet,
resources_edge_config: list = [],
graph: Optional[Dict[str, Any]] = None,
controllers_config: Dict[str, Any] = {},
bridges: List[Any] = [],
@@ -113,11 +113,12 @@ def slave(
executor = rclpy.__executor
if not executor:
executor = rclpy.__executor = MultiThreadedExecutor()
devices_config_copy = copy.deepcopy(devices_config)
for device_id, device_config in devices_config.items():
d = initialize_device_from_dict(device_id, device_config)
if d is None:
continue
devices_instances = {}
for device_config in devices_config.root_nodes:
device_id = device_config.res_content.id
if device_config.res_content.type != "device":
d = initialize_device_from_dict(device_id, device_config.get_nested_dict())
devices_instances[device_id] = d
# 默认初始化
# if d is not None and isinstance(d, Node):
# executor.add_node(d)
@@ -129,20 +130,17 @@ def slave(
if visual != "disable":
from unilabos.ros.nodes.presets.joint_republisher import JointRepublisher
resource_mesh_manager = ResourceMeshManager(
resources_mesh_config,
resources_config,
resource_tracker= DeviceNodeResourceTracker(),
device_id = 'resource_mesh_manager',
)
joint_republisher = JointRepublisher(
'joint_republisher',
DeviceNodeResourceTracker()
resources_config, # type: ignore FIXME
resource_tracker=DeviceNodeResourceTracker(),
device_id="resource_mesh_manager",
)
joint_republisher = JointRepublisher("joint_republisher", DeviceNodeResourceTracker())
executor.add_node(resource_mesh_manager)
executor.add_node(joint_republisher)
thread = threading.Thread(target=executor.spin, daemon=True, name="slave_executor_thread")
thread.start()
@@ -151,25 +149,69 @@ def slave(
sclient.wait_for_service()
request = SerialCommand.Request()
request.command = json.dumps({
"machine_name": BasicConfig.machine_name,
"type": "slave",
"devices_config": devices_config_copy,
"registry_config": lab_registry.obtain_registry_device_info()
}, ensure_ascii=False, cls=TypeEncoder)
request.command = json.dumps(
{
"machine_name": BasicConfig.machine_name,
"type": "slave",
"devices_config": devices_config.dump(),
"registry_config": lab_registry.obtain_registry_device_info(),
},
ensure_ascii=False,
cls=TypeEncoder,
)
response = sclient.call_async(request).result()
logger.info(f"Slave node info updated.")
rclient = n.create_client(ResourceAdd, "/resources/add")
# 使用新的 c2s_update_resource_tree 服务
rclient = n.create_client(SerialCommand, "/c2s_update_resource_tree")
rclient.wait_for_service()
request = ResourceAdd.Request()
request.resources = [convert_to_ros_msg(Resource, resource) for resource in resources_config]
response = rclient.call_async(request).result()
logger.info(f"Slave resource added.")
# 序列化 ResourceTreeSet 为 JSON
if resources_config:
request = SerialCommand.Request()
request.command = json.dumps(
{
"data": {
"data": resources_config.dump(),
"mount_uuid": "",
"first_add": True,
},
"action": "add",
},
ensure_ascii=False,
)
tree_response: SerialCommand_Response = rclient.call_async(request).result()
uuid_mapping = json.loads(tree_response.response)
# 创建反向映射：new_uuid -> old_uuid
reverse_uuid_mapping = {new_uuid: old_uuid for old_uuid, new_uuid in uuid_mapping.items()}
for node in resources_config.root_nodes:
if node.res_content.type == "device":
for sub_node in node.children:
# 只有二级子设备
if sub_node.res_content.type != "device":
device_tracker = devices_instances[node.res_content.id].resource_tracker
# sub_node.res_content.uuid 已经是新UUID，需要用旧UUID去查找
old_uuid = reverse_uuid_mapping.get(sub_node.res_content.uuid)
if old_uuid:
# 找到旧UUID，使用UUID查找
resource_instance = device_tracker.figure_resource({"uuid": old_uuid})
else:
# 未找到旧UUID，使用name查找
resource_instance = device_tracker.figure_resource({"name": sub_node.res_content.name})
device_tracker.loop_update_uuid(resource_instance, uuid_mapping)
else:
logger.error("Slave模式不允许新增非设备节点下的物料")
continue
if tree_response:
logger.info(f"Slave resource tree added. Response: {tree_response.response}")
else:
logger.warning("Slave resource tree add response is None")
else:
logger.info("No resources to add.")
while True:
time.sleep(1)
if __name__ == "__main__":
main()

4
unilabos/ros/msgs/message_converter.py
View File

@@ -51,6 +51,8 @@ SendCmd = msg_converter_manager.get_class("unilabos_msgs.action:SendCmd")
imsg = msg_converter_manager.get_module("unilabos.messages")
Point3D = msg_converter_manager.get_class("unilabos.messages:Point3D")
from control_msgs.action import *
# 基本消息类型映射
_msg_mapping: Dict[Type, Type] = {
float: Float64,
@@ -193,7 +195,7 @@ _msg_converter_back: Dict[Type, Any] = {
"children": list(x.children),
"parent": x.parent if x.parent else None,
"type": x.type,
"class": x.category,
"class": "",
"position": {"x": x.pose.position.x, "y": x.pose.position.y, "z": x.pose.position.z},
"config": json_or_yaml_loads(x.config or "{}"),
"data": json_or_yaml_loads(x.data or "{}"),

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

File diff suppressed because it is too large Load Diff

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

@@ -1,11 +1,11 @@
import collections
import copy
from dataclasses import dataclass, field
import json
import threading
import time
import traceback
import uuid
from typing import Optional, Dict, Any, List, ClassVar, Set, Union
from typing import TYPE_CHECKING, Optional, Dict, Any, List, ClassVar, Set, Union
from action_msgs.msg import GoalStatus
from geometry_msgs.msg import Point
@@ -15,16 +15,14 @@ from rclpy.service import Service
from unilabos_msgs.msg import Resource # type: ignore
from unilabos_msgs.srv import (
ResourceAdd,
ResourceGet,
ResourceDelete,
ResourceUpdate,
ResourceList,
SerialCommand,
) # type: ignore
from unilabos_msgs.srv._serial_command import SerialCommand_Request, SerialCommand_Response
from unique_identifier_msgs.msg import UUID
from unilabos.app.register import register_devices_and_resources
from unilabos.config.config import BasicConfig
from unilabos.registry.registry import lab_registry
from unilabos.resources.graphio import initialize_resource
from unilabos.resources.registry import add_schema
@@ -38,8 +36,22 @@ 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.ros.nodes.resource_tracker import (
ResourceDictInstance,
ResourceTreeSet,
ResourceTreeInstance,
)
from unilabos.utils.exception import DeviceClassInvalid
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, WSResourceChatData
@dataclass
class DeviceActionStatus:
job_ids: Dict[str, float] = field(default_factory=dict)
class HostNode(BaseROS2DeviceNode):
@@ -51,6 +63,10 @@ class HostNode(BaseROS2DeviceNode):
_instance: ClassVar[Optional["HostNode"]] = None
_ready_event: ClassVar[threading.Event] = threading.Event()
_device_action_status: ClassVar[collections.defaultdict[str, DeviceActionStatus]] = collections.defaultdict(
DeviceActionStatus
)
_resource_tracker: ClassVar[DeviceNodeResourceTracker] = DeviceNodeResourceTracker() # 资源管理器实例
@classmethod
def get_instance(cls, timeout=None) -> Optional["HostNode"]:
@@ -61,8 +77,8 @@ class HostNode(BaseROS2DeviceNode):
def __init__(
self,
device_id: str,
devices_config: Dict[str, Any],
resources_config: list,
devices_config: ResourceTreeSet,
resources_config: ResourceTreeSet,
resources_edge_config: list[dict],
physical_setup_graph: Optional[Dict[str, Any]] = None,
controllers_config: Optional[Dict[str, Any]] = None,
@@ -92,7 +108,7 @@ class HostNode(BaseROS2DeviceNode):
action_value_mappings=lab_registry.device_type_registry["host_node"]["class"]["action_value_mappings"],
hardware_interface={},
print_publish=False,
resource_tracker=DeviceNodeResourceTracker(), # host node并不是通过initialize 包一层传进来的
resource_tracker=self._resource_tracker, # host node并不是通过initialize 包一层传进来的
)
# 设置单例实例
@@ -101,7 +117,7 @@ class HostNode(BaseROS2DeviceNode):
# 初始化配置
self.server_latest_timestamp = 0.0 #
self.devices_config = devices_config
self.resources_config = resources_config
self.resources_config = resources_config # 直接保存 ResourceTreeSet
self.resources_edge_config = resources_edge_config
self.physical_setup_graph = physical_setup_graph
if controllers_config is None:
@@ -136,6 +152,24 @@ class HostNode(BaseROS2DeviceNode):
"/devices/host_node/test_latency",
callback_group=self.callback_group,
),
"/devices/host_node/test_resource": ActionClient(
self,
lab_registry.EmptyIn,
"/devices/host_node/test_resource",
callback_group=self.callback_group,
),
"/devices/host_node/_execute_driver_command": ActionClient(
self,
lab_registry.StrSingleInput,
"/devices/host_node/_execute_driver_command",
callback_group=self.callback_group,
),
"/devices/host_node/_execute_driver_command_async": ActionClient(
self,
lab_registry.StrSingleInput,
"/devices/host_node/_execute_driver_command_async",
callback_group=self.callback_group,
),
} # 用来存储多个ActionClient实例
self._action_value_mappings: Dict[str, Dict] = (
{}
@@ -151,21 +185,14 @@ class HostNode(BaseROS2DeviceNode):
self.device_status = {} # 用来存储设备状态
self.device_status_timestamps = {} # 用来存储设备状态最后更新时间
if BasicConfig.upload_registry:
from unilabos.app.mq import mqtt_client
register_devices_and_resources(mqtt_client, lab_registry)
else:
self.lab_logger().warning("本次启动注册表不报送云端，如果您需要联网调试，请使用unilab-register命令进行单独报送，或者在启动命令增加--upload_registry")
time.sleep(1) # 等待MQTT连接稳定
time.sleep(1) # 等待通信连接稳定
# 首次发现网络中的设备
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."
)
for device_config in devices_config.root_nodes:
device_id = device_config.res_content.id
if device_config.res_content.type != "device":
continue
if device_id not in self.devices_names:
self.initialize_device(device_id, device_config)
@@ -180,56 +207,81 @@ class HostNode(BaseROS2DeviceNode):
].items():
controller_config["update_rate"] = update_rate
self.initialize_controller(controller_id, controller_config)
resources_config.insert(
0,
{
"id": "host_node",
"name": "host_node",
"parent": None,
"type": "device",
"class": "host_node",
"position": {"x": 0, "y": 0, "z": 0},
"config": {},
"data": {},
"children": [],
},
)
resource_with_parent_name = []
resource_ids_to_instance = {i["id"]: i for i in resources_config}
resource_name_to_with_parent_name = {}
for res in resources_config:
# if res.get("parent") and res.get("type") == "device" and res.get("class"):
# parent_id = res.get("parent")
# parent_res = resource_ids_to_instance[parent_id]
# if parent_res.get("type") == "device" and parent_res.get("class"):
# resource_with_parent_name.append(copy.deepcopy(res))
# resource_name_to_with_parent_name[resource_with_parent_name[-1]["id"]] = f"{parent_res['id']}/{res['id']}"
# resource_with_parent_name[-1]["id"] = f"{parent_res['id']}/{res['id']}"
# continue
resource_with_parent_name.append(copy.deepcopy(res))
# for edge in self.resources_edge_config:
# edge["source"] = resource_name_to_with_parent_name.get(edge.get("source"), edge.get("source"))
# edge["target"] = resource_name_to_with_parent_name.get(edge.get("target"), edge.get("target"))
# 创建 host_node 作为一个单独的 ResourceTree
host_node_dict = {
"id": "host_node",
"uuid": str(uuid.uuid4()),
"parent_uuid": "",
"name": "host_node",
"type": "device",
"class": "host_node",
"config": {},
"data": {},
"children": [],
"description": "",
"schema": {},
"model": {},
"icon": "",
}
# 创建 host_node 的 ResourceTree
host_node_instance = ResourceDictInstance.get_resource_instance_from_dict(host_node_dict)
host_node_tree = ResourceTreeInstance(host_node_instance)
resources_config.trees.insert(0, host_node_tree)
try:
for bridge in self.bridges:
if hasattr(bridge, "resource_add"):
if hasattr(bridge, "resource_tree_add") and resources_config:
from unilabos.app.web.client import HTTPClient
client: HTTPClient = bridge
resource_start_time = time.time()
resource_add_res = client.resource_add(add_schema(resource_with_parent_name), False)
# 传递 ResourceTreeSet 对象，在 client 中转换为字典并获取 UUID 映射
uuid_mapping = client.resource_tree_add(resources_config, "", True)
resource_end_time = time.time()
self.lab_logger().info(
f"[Host Node-Resource] 物料上传 {round(resource_end_time - resource_start_time, 5) * 1000} ms"
)
resource_add_res = client.resource_edge_add(self.resources_edge_config, False)
for edge in self.resources_edge_config:
edge["source_uuid"] = uuid_mapping.get(edge["source_uuid"], edge["source_uuid"])
edge["target_uuid"] = uuid_mapping.get(edge["target_uuid"], edge["target_uuid"])
resource_add_res = client.resource_edge_add(self.resources_edge_config)
resource_edge_end_time = time.time()
self.lab_logger().info(
f"[Host Node-Resource] 物料关系上传 {round(resource_edge_end_time - resource_end_time, 5) * 1000} ms"
)
# resources_config 通过各个设备的 resource_tracker 进行uuid更新，利用uuid_mapping
# resources_config 的 root node 是
# 创建反向映射：new_uuid -> old_uuid
reverse_uuid_mapping = {new_uuid: old_uuid for old_uuid, new_uuid in uuid_mapping.items()}
for tree in resources_config.trees:
node = tree.root_node
if node.res_content.type == "device":
for sub_node in node.children:
# 只有二级子设备
if sub_node.res_content.type != "device":
# slave节点走c2s更新接口，拿到add自行update uuid
device_tracker = self.devices_instances[node.res_content.id].resource_tracker
# sub_node.res_content.uuid 已经是新UUID，需要用旧UUID去查找
old_uuid = reverse_uuid_mapping.get(sub_node.res_content.uuid)
if old_uuid:
# 找到旧UUID，使用UUID查找
resource_instance = device_tracker.figure_resource({"uuid": old_uuid})
else:
# 未找到旧UUID，使用name查找
resource_instance = device_tracker.figure_resource(
{"name": sub_node.res_content.name}
)
device_tracker.loop_update_uuid(resource_instance, uuid_mapping)
else:
try:
for plr_resource in ResourceTreeSet([tree]).to_plr_resources():
self.resource_tracker.add_resource(plr_resource)
except Exception as ex:
self.lab_logger().warning("[Host Node-Resource] 根节点物料序列化失败！")
except Exception as ex:
self.lab_logger().error("[Host Node-Resource] 添加物料出错！")
self.lab_logger().error(traceback.format_exc())
# 创建定时器，定期发现设备
self._discovery_timer = self.create_timer(
discovery_interval, self._discovery_devices_callback, callback_group=ReentrantCallbackGroup()
@@ -278,23 +330,23 @@ class HostNode(BaseROS2DeviceNode):
self.devices_names[edge_device_id] = namespace
self._create_action_clients_for_device(device_id, namespace)
self._online_devices.add(device_key)
sclient = self.create_client(SerialCommand, f"/srv{namespace}/query_host_name")
sclient = self.create_client(SerialCommand, f"/srv{namespace}/re_register_device")
threading.Thread(
target=self._send_re_register,
args=(sclient,),
daemon=True,
name=f"ROSDevice{self.device_id}_query_host_name_{namespace}",
name=f"ROSDevice{self.device_id}_re_register_device_{namespace}",
).start()
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)
sclient = self.create_client(SerialCommand, f"/srv{namespace}/query_host_name")
sclient = self.create_client(SerialCommand, f"/srv{namespace}/re_register_device")
threading.Thread(
target=self._send_re_register,
args=(sclient,),
daemon=True,
name=f"ROSDevice{self.device_id}_query_host_name_{namespace}",
name=f"ROSDevice{self.device_id}_re_register_device_{namespace}",
).start()
# 检测离线设备
@@ -340,9 +392,10 @@ class HostNode(BaseROS2DeviceNode):
self.lab_logger().trace(f"[Host Node] Created ActionClient (Discovery): {action_id}")
action_name = action_id[len(namespace) + 1 :]
edge_device_id = namespace[9:]
# from unilabos.app.mq import mqtt_client
# from unilabos.app.comm_factory import get_communication_client
# comm_client = get_communication_client()
# info_with_schema = ros_action_to_json_schema(action_type)
# mqtt_client.publish_actions(action_name, {
# comm_client.publish_actions(action_name, {
# "device_id": edge_device_id,
# "device_type": "",
# "action_name": action_name,
@@ -358,14 +411,16 @@ class HostNode(BaseROS2DeviceNode):
bind_parent_ids: list[str],
bind_locations: list[Point],
other_calling_params: list[str],
):
) -> List[str]:
responses = []
for resource, device_id, bind_parent_id, bind_location, other_calling_param in zip(
resources, device_ids, bind_parent_ids, bind_locations, other_calling_params
):
# 这里要求device_id传入必须是edge_device_id
if device_id not in self.devices_names:
self.lab_logger().error(f"[Host Node] Device {device_id} not found in devices_names. Create resource failed.")
self.lab_logger().error(
f"[Host Node] Device {device_id} not found in devices_names. Create resource failed."
)
raise ValueError(f"[Host Node] Device {device_id} not found in devices_names. Create resource failed.")
device_key = f"{self.devices_names[device_id]}/{device_id}"
@@ -393,8 +448,8 @@ class HostNode(BaseROS2DeviceNode):
},
ensure_ascii=False,
)
response = await sclient.call_async(request)
responses.append(response)
response: SerialCommand.Response = await sclient.call_async(request)
responses.append(response.response)
return responses
async def create_resource(
@@ -425,10 +480,12 @@ class HostNode(BaseROS2DeviceNode):
res_creation_input.update(
{
"data": {
"liquids": [{
"liquid_type": liquid_type[0] if liquid_type else None,
"liquid_volume": liquid_volume[0] if liquid_volume else None,
}]
"liquids": [
{
"liquid_type": liquid_type[0] if liquid_type else None,
"liquid_volume": liquid_volume[0] if liquid_volume else None,
}
]
}
}
)
@@ -451,11 +508,22 @@ class HostNode(BaseROS2DeviceNode):
)
]
response = await self.create_resource_detailed(resources, device_ids, bind_parent_id, bind_location, other_calling_param)
response: List[str] = await self.create_resource_detailed(
resources, device_ids, bind_parent_id, bind_location, other_calling_param
)
return response
try:
new_li = []
for i in response:
res = json.loads(i)
new_li.append(res)
return {"resources": new_li, "liquid_input_resources": new_li}
except Exception as ex:
pass
_n = "\n"
raise ValueError(f"创建资源时失败！\n{_n.join(response)}")
def initialize_device(self, device_id: str, device_config: Dict[str, Any]) -> None:
def initialize_device(self, device_id: str, device_config: ResourceDictInstance) -> None:
"""
根据配置初始化设备，
@@ -468,9 +536,8 @@ class HostNode(BaseROS2DeviceNode):
"""
self.lab_logger().info(f"[Host Node] Initializing device: {device_id}")
device_config_copy = copy.deepcopy(device_config)
try:
d = initialize_device_from_dict(device_id, device_config_copy)
d = initialize_device_from_dict(device_id, device_config.get_nested_dict())
except DeviceClassInvalid as e:
self.lab_logger().error(f"[Host Node] Device class invalid: {e}")
d = None
@@ -482,7 +549,9 @@ class HostNode(BaseROS2DeviceNode):
self.devices_instances[device_id] = d
# noinspection PyProtectedMember
for action_name, action_value_mapping in d._ros_node._action_value_mappings.items():
if action_name.startswith("auto-") or str(action_value_mapping.get("type", "")).startswith("UniLabJsonCommand"):
if action_name.startswith("auto-") or str(action_value_mapping.get("type", "")).startswith(
"UniLabJsonCommand"
):
continue
action_id = f"/devices/{device_id}/{action_name}"
if action_id not in self._action_clients:
@@ -491,9 +560,10 @@ class HostNode(BaseROS2DeviceNode):
self.lab_logger().trace(
f"[Host Node] Created ActionClient (Local): {action_id}"
) # 子设备再创建用的是Discover发现的
# from unilabos.app.mq import mqtt_client
# from unilabos.app.comm_factory import get_communication_client
# comm_client = get_communication_client()
# info_with_schema = ros_action_to_json_schema(action_type)
# mqtt_client.publish_actions(action_name, {
# comm_client.publish_actions(action_name, {
# "device_id": device_id,
# "device_type": device_config["class"],
# "action_name": action_name,
@@ -523,7 +593,7 @@ class HostNode(BaseROS2DeviceNode):
# 解析设备名和属性名
parts = topic.split("/")
if len(parts) >= 4: # 可能有ProtocolNode，创建更长的设备
if len(parts) >= 4: # 可能有WorkstationNode，创建更长的设备
device_id = "/".join(parts[2:-1])
property_name = parts[-1]
@@ -591,46 +661,39 @@ class HostNode(BaseROS2DeviceNode):
if hasattr(bridge, "publish_device_status"):
bridge.publish_device_status(self.device_status, device_id, property_name)
if bCreate:
self.lab_logger().trace(
f"Status created: {device_id}.{property_name} = {msg.data}"
)
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().debug(f"Status updated: {device_id}.{property_name} = {msg.data}")
def send_goal(
self,
device_id: str,
item: "QueueItem",
action_type: str,
action_name: str,
action_kwargs: Dict[str, Any],
goal_uuid: Optional[str] = None,
server_info: Optional[Dict[str, Any]] = None,
) -> None:
"""
向设备发送目标请求
Args:
device_id: 设备ID
action_type: 动作类型
action_name: 动作名称
action_kwargs: 动作参数
goal_uuid: 目标UUID，如果为None则自动生成
server_info: 服务器发送信息，包含发送时间戳等
"""
if BasicConfig.direct_end:
raise ValueError("direct_end")
u = uuid.UUID(item.job_id)
device_id = item.device_id
action_name = item.action_name
if action_type.startswith("UniLabJsonCommand"):
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,
})
"string": json.dumps(
{
"function_name": action_name,
"function_args": action_kwargs,
}
)
}
if action_type.startswith("UniLabJsonCommandAsync"):
action_id = f"/devices/{device_id}/_execute_driver_command_async"
@@ -647,79 +710,73 @@ class HostNode(BaseROS2DeviceNode):
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
goal_uuid_obj = UUID(uuid=list(u.bytes))
future = action_client.send_goal_async(
goal_msg,
feedback_callback=lambda feedback_msg: self.feedback_callback(action_id, uuid_str, feedback_msg),
feedback_callback=lambda feedback_msg: self.feedback_callback(item, action_id, feedback_msg),
goal_uuid=goal_uuid_obj,
)
future.add_done_callback(lambda future: self.goal_response_callback(action_id, uuid_str, future))
future.add_done_callback(lambda future: self.goal_response_callback(item, action_id, future))
def goal_response_callback(self, action_id: str, uuid_str: Optional[str], future) -> None:
def goal_response_callback(self, item: "QueueItem", action_id: 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")
self.lab_logger().warning(f"[Host Node] Goal {item.action_name} ({item.job_id}) 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)
)
self.lab_logger().info(f"[Host Node] Goal {action_id} ({item.job_id}) accepted")
self._goals[item.job_id] = goal_handle
goal_handle.get_result_async().add_done_callback(
lambda future: self.get_result_callback(item, action_id, future)
)
def feedback_callback(self, action_id: str, uuid_str: Optional[str], feedback_msg) -> None:
def feedback_callback(self, item: "QueueItem", action_id: 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}")
self.lab_logger().trace(f"[Host Node] Feedback for {action_id} ({item.job_id}): {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")
for bridge in self.bridges:
if hasattr(bridge, "publish_job_status"):
bridge.publish_job_status(feedback_data, item, "running")
def get_result_callback(self, action_id: str, uuid_str: Optional[str], future) -> None:
def get_result_callback(self, item: "QueueItem", action_id: str, future) -> None:
"""获取结果回调"""
job_id = item.job_id
result_msg = future.result().result
result_data = convert_from_ros_msg(result_msg)
status = "success"
return_info_str = result_data.get("return_info")
if return_info_str is not None:
try:
ret = json.loads(return_info_str)
suc = ret.get("suc", False)
return_info = json.loads(return_info_str)
suc = return_info.get("suc", False)
if not suc:
status = "failed"
except json.JSONDecodeError:
status = "failed"
return_info = serialize_result_info("", False, result_data)
self.lab_logger().critical("错误的return_info类型，请断点修复")
else:
# 无 return_info 字段时，回退到 success 字段（若存在）
suc_field = result_data.get("success")
if isinstance(suc_field, bool):
status = "success" if suc_field else "failed"
return_info_str = serialize_result_info("", suc_field, result_data)
return_info = serialize_result_info("", suc_field, result_data)
else:
# 最保守的回退：标记失败并返回空JSON
status = "failed"
return_info_str = serialize_result_info("缺少return_info", False, result_data)
return_info = serialize_result_info("缺少return_info", False, result_data)
self.lab_logger().info(f"[Host Node] Result for {action_id} ({uuid_str}): {status}")
self.lab_logger().info(f"[Host Node] Result for {action_id} ({job_id}): {status}")
self.lab_logger().debug(f"[Host Node] Result data: {result_data}")
if uuid_str:
if job_id:
for bridge in self.bridges:
if hasattr(bridge, "publish_job_status"):
bridge.publish_job_status(result_data, uuid_str, status, return_info_str)
bridge.publish_job_status(result_data, item, status, return_info)
def cancel_goal(self, goal_uuid: str) -> None:
"""取消目标"""
@@ -729,14 +786,14 @@ class HostNode(BaseROS2DeviceNode):
else:
self.lab_logger().warning(f"[Host Node] Goal {goal_uuid} not found, cannot cancel")
def get_goal_status(self, uuid_str: str) -> int:
def get_goal_status(self, job_id: str) -> int:
"""获取目标状态"""
if uuid_str in self._goals:
g = self._goals[uuid_str]
if job_id in self._goals:
g = self._goals[job_id]
status = g.status
self.lab_logger().debug(f"[Host Node] Goal status for {uuid_str}: {status}")
self.lab_logger().debug(f"[Host Node] Goal status for {job_id}: {status}")
return status
self.lab_logger().warning(f"[Host Node] Goal {uuid_str} not found, status unknown")
self.lab_logger().warning(f"[Host Node] Goal {job_id} not found, status unknown")
return GoalStatus.STATUS_UNKNOWN
"""Controller Node"""
@@ -774,7 +831,7 @@ class HostNode(BaseROS2DeviceNode):
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()
SerialCommand, "/resources/get", self._resource_get_callback, callback_group=ReentrantCallbackGroup()
),
"resource_delete": self.create_service(
ResourceDelete,
@@ -797,15 +854,134 @@ class HostNode(BaseROS2DeviceNode):
self._node_info_update_callback,
callback_group=ReentrantCallbackGroup(),
),
"c2s_update_resource_tree": self.create_service(
SerialCommand,
"/c2s_update_resource_tree",
self._resource_tree_update_callback,
callback_group=ReentrantCallbackGroup(),
),
}
def _resource_tree_action_add_callback(self, data: dict, response: SerialCommand_Response): # OK
resource_tree_set = ResourceTreeSet.load(data["data"])
mount_uuid = data["mount_uuid"]
first_add = data["first_add"]
self.lab_logger().info(
f"[Host Node-Resource] Loaded ResourceTreeSet with {len(resource_tree_set.trees)} trees, "
f"{len(resource_tree_set.all_nodes)} total nodes"
)
# 处理资源添加逻辑
success = False
uuid_mapping = {}
if len(self.bridges) > 0:
from unilabos.app.web.client import HTTPClient
client: HTTPClient = self.bridges[-1]
resource_start_time = time.time()
uuid_mapping = client.resource_tree_add(resource_tree_set, mount_uuid, first_add)
success = True
resource_end_time = time.time()
self.lab_logger().info(
f"[Host Node-Resource] 物料创建上传 {round(resource_end_time - resource_start_time, 5) * 1000} ms"
)
if uuid_mapping:
self.lab_logger().info(f"[Host Node-Resource] UUID映射: {len(uuid_mapping)} 个节点")
if success:
from unilabos.resources.graphio import physical_setup_graph
# 将资源添加到本地图中
for node in resource_tree_set.all_nodes:
resource_dict = node.res_content.model_dump(by_alias=True)
if resource_dict.get("id") not in physical_setup_graph.nodes:
physical_setup_graph.add_node(resource_dict["id"], **resource_dict)
else:
physical_setup_graph.nodes[resource_dict["id"]]["data"].update(resource_dict.get("data", {}))
response.response = json.dumps(uuid_mapping) if success else "FAILED"
self.lab_logger().info(f"[Host Node-Resource] Resource tree add completed, success: {success}")
def _resource_tree_action_get_callback(self, data: dict, response: SerialCommand_Response): # OK
uuid_list: List[str] = data["data"]
with_children: bool = data["with_children"]
from unilabos.app.web.client import http_client
resource_response = http_client.resource_tree_get(uuid_list, with_children)
response.response = json.dumps(resource_response)
def _resource_tree_action_remove_callback(self, data: dict, response: SerialCommand_Response):
"""
子节点通知Host物料树删除
"""
self.lab_logger().info(f"[Host Node-Resource] Resource tree remove request received")
response.response = "OK"
self.lab_logger().info(f"[Host Node-Resource] Resource tree remove completed")
def _resource_tree_action_update_callback(self, data: dict, response: SerialCommand_Response):
"""
子节点通知Host物料树更新
"""
resource_tree_set = ResourceTreeSet.load(data["data"])
self.lab_logger().info(
f"[Host Node-Resource] Loaded ResourceTreeSet with {len(resource_tree_set.trees)} trees, "
f"{len(resource_tree_set.all_nodes)} total nodes"
)
from unilabos.app.web.client import http_client
resource_start_time = time.time()
uuid_mapping = http_client.resource_tree_update(resource_tree_set, "", False)
success = bool(uuid_mapping)
resource_end_time = time.time()
self.lab_logger().info(
f"[Host Node-Resource] 物料更新上传 {round(resource_end_time - resource_start_time, 5) * 1000} ms"
)
if uuid_mapping:
self.lab_logger().info(f"[Host Node-Resource] UUID映射: {len(uuid_mapping)} 个节点")
# 还需要加入到资源图中，暂不实现，考虑资源图新的获取方式
response.response = json.dumps(uuid_mapping)
self.lab_logger().info(f"[Host Node-Resource] Resource tree add completed, success: {success}")
def _resource_tree_update_callback(self, request: SerialCommand_Request, response: SerialCommand_Response):
"""
子节点通知Host物料树更新
接收序列化的 ResourceTreeSet 数据并进行处理
"""
self.lab_logger().info(f"[Host Node-Resource] Resource tree add request received")
try:
# 解析请求数据
data = json.loads(request.command)
action = data["action"]
data = data["data"]
if action == "add":
self._resource_tree_action_add_callback(data, response)
elif action == "get":
self._resource_tree_action_get_callback(data, response)
elif action == "update":
self._resource_tree_action_update_callback(data, response)
elif action == "remove":
self._resource_tree_action_remove_callback(data, response)
else:
self.lab_logger().error(f"[Host Node-Resource] Invalid action: {action}")
response.response = "ERROR"
except Exception as e:
self.lab_logger().error(f"[Host Node-Resource] Error adding resource tree: {e}")
self.lab_logger().error(traceback.format_exc())
response.response = f"ERROR: {str(e)}"
return response
def _node_info_update_callback(self, request, response):
"""
更新节点信息回调
"""
self.lab_logger().info(f"[Host Node] Node info update request received: {request}")
try:
from unilabos.app.mq import mqtt_client
from unilabos.app.communication import get_communication_client
info = json.loads(request.command)
if "SYNC_SLAVE_NODE_INFO" in info:
@@ -814,9 +990,10 @@ class HostNode(BaseROS2DeviceNode):
edge_device_id = info["edge_device_id"]
self.device_machine_names[edge_device_id] = machine_name
else:
comm_client = get_communication_client()
registry_config = info["registry_config"]
for device_config in registry_config:
mqtt_client.publish_registry(device_config["id"], device_config)
comm_client.publish_registry(device_config["id"], device_config)
self.lab_logger().debug(f"[Host Node] Node info update: {info}")
response.response = "OK"
except Exception as e:
@@ -843,14 +1020,16 @@ class HostNode(BaseROS2DeviceNode):
success = False
if len(self.bridges) > 0: # 边的提交待定
from unilabos.app.web.client import HTTPClient
client: HTTPClient = self.bridges[-1]
r = client.resource_add(add_schema(resources), False)
r = client.resource_add(add_schema(resources))
success = bool(r)
response.success = success
if success:
from unilabos.resources.graphio import physical_setup_graph
for resource in resources:
if resource.get("id") not in physical_setup_graph.nodes:
physical_setup_graph.add_node(resource["id"], **resource)
@@ -860,7 +1039,13 @@ class HostNode(BaseROS2DeviceNode):
self.lab_logger().info(f"[Host Node-Resource] Add request completed, success: {success}")
return response
def _resource_get_callback(self, request: ResourceGet.Request, response: ResourceGet.Response):
def _resource_get_process(self, data: Dict[str, Any]):
r = data["data"]
self.lab_logger().debug(f"[Host Node-Resource] Retrieved from bridge: {len(r)} resources")
resources = [convert_to_ros_msg(Resource, resource) for resource in r]
return resources
def _resource_get_callback(self, request: SerialCommand.Request, response: SerialCommand.Response):
"""
获取资源回调
@@ -873,23 +1058,13 @@ class HostNode(BaseROS2DeviceNode):
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 = [resource for resource in self.resources_config if resource.get("id") == request.id]
self.lab_logger().warning(f"[Host Node-Resource] Retrieved from local: {len(r)} resources")
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]
try:
data = json.loads(request.command)
http_req = self.bridges[-1].resource_get(data["id"], data["with_children"])
response.response = json.dumps(http_req["data"])
return response
except Exception as e:
self.lab_logger().error(f"[Host Node-Resource] Error retrieving from bridge: {str(e)}")
return response
def _resource_delete_callback(self, request, response):
@@ -991,9 +1166,10 @@ class HostNode(BaseROS2DeviceNode):
send_timestamp = time.time()
# 发送ping
from unilabos.app.mq import mqtt_client
from unilabos.app.communication import get_communication_client
mqtt_client.send_ping(ping_id, send_timestamp)
comm_client = get_communication_client()
comm_client.send_ping(ping_id, send_timestamp)
# 等待pong响应
timeout = 10.0
@@ -1073,6 +1249,7 @@ class HostNode(BaseROS2DeviceNode):
else:
self.lab_logger().warning("⚠️ 无法获取服务端任务下发时间，跳过任务延迟分析")
raw_delay_ms = -1
corrected_delay_ms = -1
self.lab_logger().info("=" * 60)
@@ -1089,6 +1266,14 @@ class HostNode(BaseROS2DeviceNode):
"status": "success",
}
def test_resource(
self, resource: ResourceSlot, resources: List[ResourceSlot], device: DeviceSlot, devices: List[DeviceSlot]
):
return {
"resources": ResourceTreeSet.from_plr_resources([resource, *resources]).dump(),
"devices": [device, *devices],
}
def handle_pong_response(self, pong_data: dict):
"""
处理pong响应
@@ -1108,3 +1293,76 @@ class HostNode(BaseROS2DeviceNode):
)
else:
self.lab_logger().warning("⚠️ 收到无效的Pong响应（缺少ping_id）")
def notify_resource_tree_update(self, device_id: str, action: str, resource_uuid_list: List[str]) -> bool:
"""
通知设备节点更新资源树
Args:
device_id: 目标设备ID
action: 操作类型 "add", "update", "remove"
resource_uuid_list: 资源UUIDs
Returns:
bool: 操作是否成功
"""
try:
# 检查设备是否存在
if device_id not in self.devices_names:
self.lab_logger().error(f"[Host Node-Resource] Device {device_id} not found in devices_names")
return False
namespace = self.devices_names[device_id]
device_key = f"{namespace}/{device_id}"
# 检查设备是否在线
if device_key not in self._online_devices:
self.lab_logger().error(f"[Host Node-Resource] Device {device_key} is offline")
return False
# 构建服务地址
srv_address = f"/srv{namespace}/s2c_resource_tree"
self.lab_logger().info(f"[Host Node-Resource] Notifying {device_id} for resource tree {action} operation")
# 创建服务客户端
sclient = self.create_client(SerialCommand, srv_address)
# 等待服务可用（设置超时）
if not sclient.wait_for_service(timeout_sec=5.0):
self.lab_logger().error(f"[Host Node-Resource] Service {srv_address} not available")
return False
# 构建请求数据
request_data = [
{
"action": action,
"data": resource_uuid_list,
}
]
# 创建请求
request = SerialCommand.Request()
request.command = json.dumps(request_data, ensure_ascii=False)
# 发送异步请求
future = sclient.call_async(request)
# 等待响应
timeout = 30.0
start_time = time.time()
while not future.done():
if time.time() - start_time > timeout:
self.lab_logger().error(f"[Host Node-Resource] Timeout waiting for response from {device_id}")
return False
time.sleep(0.05)
response = future.result()
self.lab_logger().info(
f"[Host Node-Resource] Resource tree {action} notification completed for {device_id}"
)
return True
except Exception as e:
self.lab_logger().error(f"[Host Node-Resource] Error notifying resource tree update: {str(e)}")
self.lab_logger().error(traceback.format_exc())
return False

398
unilabos/ros/nodes/presets/protocol_node.py
View File

@@ -1,398 +0,0 @@
import json
import time
import traceback
from pprint import pprint, saferepr, pformat
from typing import Union
import rclpy
from rosidl_runtime_py import message_to_ordereddict
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, String,
)
from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode, DeviceNodeResourceTracker, ROS2DeviceNode
from unilabos.utils.log import error
from unilabos.utils.type_check import serialize_result_info
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,
)
# 初始化子设备
self.communication_node_id_to_instance = {}
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
try:
d = self.initialize_device(device_id, device_config)
except Exception as ex:
self.lab_logger().error(f"[Protocol Node] Failed to initialize device {device_id}: {ex}\n{traceback.format_exc()}")
d = None
if d is None:
continue
if "serial_" in device_id or "io_" in device_id:
self.communication_node_id_to_instance[device_id] = d
continue
for device_id, device_config in self.children.items():
if device_config.get("type", "device") != "device":
continue
# 设置硬件接口代理
if device_id not in self.sub_devices:
self.lab_logger().error(f"[Protocol Node] {device_id} 还没有正确初始化，跳过...")
continue
d = self.sub_devices[device_id]
if d:
hardware_interface = d.ros_node_instance._hardware_interface
if (
hasattr(d.driver_instance, hardware_interface["name"])
and hasattr(d.driver_instance, hardware_interface["write"])
and (hardware_interface["read"] is None or hasattr(d.driver_instance, hardware_interface["read"]))
):
name = getattr(d.driver_instance, hardware_interface["name"])
read = hardware_interface.get("read", None)
write = hardware_interface.get("write", None)
# 如果硬件接口是字符串，通过通信设备提供
if isinstance(name, str) and name in self.sub_devices:
communicate_device = self.sub_devices[name]
communicate_hardware_info = communicate_device.ros_node_instance._hardware_interface
self._setup_hardware_proxy(d, self.sub_devices[name], read, write)
self.lab_logger().info(
f"\n通信代理：为子设备{device_id}\n "
f"添加了{read}方法(来源：{name} {communicate_hardware_info['write']}) \n "
f"添加了{write}方法(来源：{name} {communicate_hardware_info['read']})"
)
self.lab_logger().info(f"ROS2ProtocolNode {device_id} initialized with protocols: {self.protocol_names}")
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}"
device_id_abs = f"{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
node.resource_tracker = self.resource_tracker # 站内应当共享资源跟踪器
for action_name, action_mapping in node._action_value_mappings.items():
if action_name.startswith("auto-") or str(action_mapping.get("type", "")).startswith("UniLabJsonCommand"):
continue
action_id = f"/devices/{device_id_abs}/{action_name}"
if action_id not in self._action_clients:
try:
self._action_clients[action_id] = ActionClient(
self, action_mapping["type"], action_id, callback_group=self.callback_group
)
except Exception as ex:
self.lab_logger().error(f"创建动作客户端失败: {action_id}, 错误: {ex}")
continue
self.lab_logger().trace(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().trace(f"发布动作: {action_name}, 类型: {str_action_type}")
def _create_protocol_execute_callback(self, protocol_name, protocol_steps_generator):
async def execute_protocol(goal_handle: ServerGoalHandle):
"""执行完整的工作流"""
# 初始化结果信息变量
execution_error = ""
execution_success = False
protocol_return_value = None
self.get_logger().info(f"Executing {protocol_name} action...")
action_value_mapping = self._action_value_mappings[protocol_name]
step_results = []
try:
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"])
# # 🔧 添加调试信息
# print(f"🔍 转换后的 protocol_kwargs: {protocol_kwargs}")
# print(f"🔍 vessel 在转换后: {protocol_kwargs.get('vessel', 'NOT_FOUND')}")
# # 🔧 完全禁用Host查询，直接使用转换后的数据
# print(f"🔧 跳过Host查询，直接使用转换后的数据")
# 向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()
resource_id = (
protocol_kwargs[k]["id"] if v == "unilabos_msgs/Resource" else protocol_kwargs[k][0]["id"]
)
r.id = resource_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]
)
self.lab_logger().info(f"🔍 最终的 vessel: {protocol_kwargs.get('vessel', 'NOT_FOUND')}")
from unilabos.resources.graphio import physical_setup_graph
self.lab_logger().info(f"Working on physical setup: {physical_setup_graph}")
protocol_steps = protocol_steps_generator(G=physical_setup_graph, **protocol_kwargs)
logs = []
for step in protocol_steps:
if isinstance(step, dict) and "log_message" in step.get("action_kwargs", {}):
logs.append(step)
elif isinstance(step, list):
logs.append(step)
self.lab_logger().info(f"Goal received: {protocol_kwargs}, running steps: "
f"{json.dumps(logs, indent=4, ensure_ascii=False)}")
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 isinstance(action, dict):
# 如果是单个动作，直接执行
if action["action_name"] == "wait":
time.sleep(action["action_kwargs"]["time"])
step_results.append({"step": i + 1, "action": "wait", "result": "completed"})
else:
result = await self.execute_single_action(**action)
step_results.append({"step": i + 1, "action": action["action_name"], "result": result})
ret_info = json.loads(getattr(result, "return_info", "{}"))
if not ret_info.get("suc", False):
raise RuntimeError(f"Step {i + 1} failed.")
elif isinstance(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]
step_results.append(
{
"step": i + 1,
"parallel_actions": [a["action_name"] for a in actions],
"results": results,
}
)
# 向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)
# 设置成功状态和返回值
execution_success = True
protocol_return_value = {
"protocol_name": protocol_name,
"steps_executed": len(protocol_steps),
"step_results": step_results,
"total_time": time.time() - time_start,
}
goal_handle.succeed()
except Exception as e:
# 捕获并记录错误信息
str_step_results = [{k: dict(message_to_ordereddict(v)) if k == "result" and hasattr(v, "SLOT_TYPES") else v for k, v in i.items()} for i in step_results]
execution_error = f"{traceback.format_exc()}\n\nStep Result: {pformat(str_step_results)}"
execution_success = False
self.lab_logger().error(f"协议 {protocol_name} 执行出错: {str(e)} \n{traceback.format_exc()}")
# 设置动作失败
goal_handle.abort()
finally:
self._busy = False
# 创建结果消息
result = action_value_mapping["type"].Result()
result.success = execution_success
# 获取结果消息类型信息，检查是否有return_info字段
result_msg_types = action_value_mapping["type"].Result.get_fields_and_field_types()
# 设置return_info字段（如果存在）
for attr_name in result_msg_types.keys():
if attr_name in ["success", "reached_goal"]:
setattr(result, attr_name, execution_success)
elif attr_name == "return_info":
setattr(
result,
attr_name,
serialize_result_info(execution_error, execution_success, protocol_return_value),
)
self.lab_logger().info(f"协议 {protocol_name} 完成并返回结果")
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/{device_id}/{action_name}" # 执行时取消了主节点信息 /{self.device_id}
# 检查动作客户端是否存在
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: ROS2DeviceNode, communication_device: ROS2DeviceNode, read_method, write_method
):
"""为设备设置硬件接口代理"""
# extra_info = [getattr(device.driver_instance, info) for info in communication_device.ros_node_instance._hardware_interface.get("extra_info", [])]
write_func = getattr(
communication_device.driver_instance, communication_device.ros_node_instance._hardware_interface["write"]
)
read_func = getattr(
communication_device.driver_instance, communication_device.ros_node_instance._hardware_interface["read"]
)
def _read(*args, **kwargs):
return read_func(*args, **kwargs)
def _write(*args, **kwargs):
return write_func(*args, **kwargs)
if read_method:
# bound_read = MethodType(_read, device.driver_instance)
setattr(device.driver_instance, read_method, _read)
if write_method:
# bound_write = MethodType(_write, device.driver_instance)
setattr(device.driver_instance, 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}")

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

@@ -1,86 +1,415 @@
import collections
from typing import Union, Dict, Any, Optional
import json
import time
import traceback
from pprint import pformat
from typing import List, Dict, Any, Optional, TYPE_CHECKING
from unilabos_msgs.msg import Resource
from pylabrobot.resources import Resource as PLRResource, Plate, TipRack, Coordinate
from unilabos.ros.nodes.presets.protocol_node import ROS2ProtocolNode
from unilabos.ros.nodes.resource_tracker import DeviceNodeResourceTracker
import rclpy
from rosidl_runtime_py import message_to_ordereddict
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, ROS2DeviceNode
from unilabos.utils.type_check import serialize_result_info, get_result_info_str
if TYPE_CHECKING:
from unilabos.devices.workstation.workstation_base import WorkstationBase
class WorkStationContainer(Plate, TipRack):
class ROS2WorkstationNodeTempError(Exception):
pass
class ROS2WorkstationNode(BaseROS2DeviceNode):
"""
WorkStation 专用 Container 类，继承自 Plate和TipRack
注意这个物料必须通过plr_additional_res_reg.py注册到edge，才能正常序列化
ROS2WorkstationNode代表管理ROS2环境中设备通信和动作的协议节点。
它初始化设备节点，处理动作客户端，并基于指定的协议执行工作流。
它还物理上代表一组协同工作的设备，如带夹持器的机械臂，带传送带的CNC机器等。
"""
def __init__(self, name: str, size_x: float, size_y: float, size_z: float, category: str, ordering: collections.OrderedDict, model: Optional[str] = None,):
"""
这里的初始化入参要和plr的保持一致
"""
super().__init__(name, size_x, size_y, size_z, category=category, ordering=ordering, model=model)
self._unilabos_state = {} # 必须有此行，自己的类描述的是物料的
driver_instance: "WorkstationBase"
def load_state(self, state: Dict[str, Any]) -> None:
"""从给定的状态加载工作台信息。"""
super().load_state(state)
self._unilabos_state = state
def serialize_state(self) -> Dict[str, Dict[str, Any]]:
data = super().serialize_state()
data.update(self._unilabos_state) # Container自身的信息，云端物料将保存这一data，本地也通过这里的data进行读写，当前类用来表示这个物料的长宽高大小的属性，而data（state用来表示物料的内容，细节等）
return data
def get_workstation_plate_resource(name: str) -> PLRResource: # 要给定一个返回plr的方法
"""
用于获取一些模板，例如返回一个带有特定信息/子物料的 Plate，这里需要到注册表注册，例如unilabos/registry/resources/organic/workstation.yaml
可以直接运行该函数或者利用注册表补全机制，来检查是否资源出错
:param name: 资源名称
:return: Resource对象
"""
plate = WorkStationContainer(name, size_x=50, size_y=50, size_z=10, category="plate", ordering=collections.OrderedDict())
tip_rack = WorkStationContainer("tip_rack_inside_plate", size_x=50, size_y=50, size_z=10, category="tip_rack", ordering=collections.OrderedDict())
plate.assign_child_resource(tip_rack, Coordinate.zero())
return plate
class WorkStationExample(ROS2ProtocolNode):
def __init__(self,
# 你可以在这里增加任意的参数，对应启动json填写相应的参数内容
device_id: str,
children: dict,
protocol_type: Union[str, list[str]],
resource_tracker: DeviceNodeResourceTracker
):
super().__init__(device_id, children, protocol_type, resource_tracker)
def create_resource(
def __init__(
self,
resource_tracker: DeviceNodeResourceTracker,
resources: list[Resource],
bind_parent_id: str,
bind_location: dict[str, float],
liquid_input_slot: list[int],
liquid_type: list[str],
liquid_volume: list[int],
slot_on_deck: int,
) -> Dict[str, Any]:
return { # edge侧返回给前端的创建物料的结果。云端调用初始化瓶子等。执行该函数时，物料已经上报给云端，一般不需要继承使用
protocol_type: List[str],
children: Dict[str, Any],
*,
driver_instance: "WorkstationBase",
device_id: str,
status_types: Dict[str, Any],
action_value_mappings: Dict[str, Any],
hardware_interface: Dict[str, Any],
print_publish=True,
resource_tracker: Optional["DeviceNodeResourceTracker"] = None,
):
self._setup_protocol_names(protocol_type)
}
# 初始化非BaseROSNode的属性
self.children = children
# 初始化基类，让基类处理常规动作
super().__init__(
driver_instance=driver_instance,
device_id=device_id,
status_types=status_types,
action_value_mappings={**action_value_mappings, **self.protocol_action_mappings},
hardware_interface=hardware_interface,
print_publish=print_publish,
resource_tracker=resource_tracker,
)
def transfer_bottle(self, tip_rack: PLRResource, base_plate: PLRResource): # 使用自定义物料的举例
"""
将tip_rack assign给base_plate，两个入参都得是PLRResource，unilabos会代替当前物料操作，自动刷新他们的父子关系等状态
"""
pass
self._busy = False
self.sub_devices = {}
self._action_clients = {}
def trigger_resource_update(self, from_plate: PLRResource, to_base_plate: PLRResource):
"""
有些时候物料发生了子设备的迁移，一般对该设备的最大一级的物料进行操作，例如要将A物料搬移到B物料上，他们不共同拥有一个物料
该步骤操作结束后，会主动刷新from_plate的父物料，与to_base_plate的父物料（如没有则刷新自身）
# 初始化子设备
self.communication_node_id_to_instance = {}
"""
to_base_plate.assign_child_resource(from_plate, Coordinate.zero())
pass
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
try:
d = self.initialize_device(device_id, device_config)
except Exception as ex:
self.lab_logger().error(
f"[Protocol Node] Failed to initialize device {device_id}: {ex}\n{traceback.format_exc()}"
)
d = None
if d is None:
continue
if "serial_" in device_id or "io_" in device_id:
self.communication_node_id_to_instance[device_id] = d
continue
for device_id, device_config in self.children.items():
if device_config.get("type", "device") != "device":
continue
# 设置硬件接口代理
if device_id not in self.sub_devices:
self.lab_logger().error(f"[Protocol Node] {device_id} 还没有正确初始化，跳过...")
continue
d = self.sub_devices[device_id]
if d:
hardware_interface = d.ros_node_instance._hardware_interface
if (
hasattr(d.driver_instance, hardware_interface["name"])
and hasattr(d.driver_instance, hardware_interface["write"])
and (hardware_interface["read"] is None or hasattr(d.driver_instance, hardware_interface["read"]))
):
name = getattr(d.driver_instance, hardware_interface["name"])
read = hardware_interface.get("read", None)
write = hardware_interface.get("write", None)
# 如果硬件接口是字符串，通过通信设备提供
if isinstance(name, str) and name in self.sub_devices:
communicate_device = self.sub_devices[name]
communicate_hardware_info = communicate_device.ros_node_instance._hardware_interface
self._setup_hardware_proxy(d, self.sub_devices[name], read, write)
self.lab_logger().info(
f"\n通信代理：为子设备{device_id}\n "
f"添加了{read}方法(来源：{name} {communicate_hardware_info['write']}) \n "
f"添加了{write}方法(来源：{name} {communicate_hardware_info['read']})"
)
self.lab_logger().info(f"ROS2WorkstationNode {device_id} initialized with protocols: {self.protocol_names}")
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}"
device_id_abs = f"{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
node.resource_tracker = self.resource_tracker # 站内应当共享资源跟踪器
for action_name, action_mapping in node._action_value_mappings.items():
if action_name.startswith("auto-") or str(action_mapping.get("type", "")).startswith(
"UniLabJsonCommand"
):
continue
action_id = f"/devices/{device_id_abs}/{action_name}"
if action_id not in self._action_clients:
try:
self._action_clients[action_id] = ActionClient(
self, action_mapping["type"], action_id, callback_group=self.callback_group
)
except Exception as ex:
self.lab_logger().error(f"创建动作客户端失败: {action_id}, 错误: {ex}")
continue
self.lab_logger().trace(f"为子设备 {device_id} 创建动作客户端: {action_name}")
return d
def create_ros_action_server(self, action_name, action_value_mapping):
"""创建ROS动作服务器"""
if action_name not in self.protocol_names:
# 非protocol方法调用父类注册
return super().create_ros_action_server(action_name, action_value_mapping)
# 和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().trace(f"发布动作: {action_name}, 类型: {str_action_type}")
return
def _create_protocol_execute_callback(self, protocol_name, protocol_steps_generator):
async def execute_protocol(goal_handle: ServerGoalHandle):
"""执行完整的工作流"""
# 初始化结果信息变量
execution_error = ""
execution_success = False
protocol_return_value = None
self.lab_logger().info(f"Executing {protocol_name} action...")
action_value_mapping = self._action_value_mappings[protocol_name]
step_results = []
try:
self.lab_logger().warning("+" * 30)
self.lab_logger().info(protocol_steps_generator)
# 从目标消息中提取参数, 并调用Protocol生成器(根据设备连接图)生成action步骤
goal = goal_handle.request
protocol_kwargs = convert_from_ros_msg_with_mapping(goal, action_value_mapping["goal"])
# # 🔧 添加调试信息
# print(f"🔍 转换后的 protocol_kwargs: {protocol_kwargs}")
# print(f"🔍 vessel 在转换后: {protocol_kwargs.get('vessel', 'NOT_FOUND')}")
# # 🔧 完全禁用Host查询，直接使用转换后的数据
# print(f"🔧 跳过Host查询，直接使用转换后的数据")
# 向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()
resource_id = (
protocol_kwargs[k]["id"] if v == "unilabos_msgs/Resource" else protocol_kwargs[k][0]["id"]
)
r.id = resource_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]
)
self.lab_logger().info(f"🔍 最终的 vessel: {protocol_kwargs.get('vessel', 'NOT_FOUND')}")
from unilabos.resources.graphio import physical_setup_graph
self.lab_logger().info(f"Working on physical setup: {physical_setup_graph}")
protocol_steps = protocol_steps_generator(G=physical_setup_graph, **protocol_kwargs)
logs = []
for step in protocol_steps:
if isinstance(step, dict) and "log_message" in step.get("action_kwargs", {}):
logs.append(step)
elif isinstance(step, list):
logs.append(step)
self.lab_logger().info(
f"Goal received: {protocol_kwargs}, running steps: "
f"{json.dumps(logs, indent=4, ensure_ascii=False)}"
)
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 isinstance(action, dict):
# 如果是单个动作，直接执行
if action["action_name"] == "wait":
time.sleep(action["action_kwargs"]["time"])
step_results.append({"step": i + 1, "action": "wait", "result": "completed"})
else:
try:
result = await self.execute_single_action(**action)
step_results.append({"step": i + 1, "action": action["action_name"], "result": result})
ret_info = json.loads(getattr(result, "return_info", "{}"))
if not ret_info.get("suc", False):
raise RuntimeError(f"Step {i + 1} failed.")
except ROS2WorkstationNodeTempError as ex:
step_results.append(
{"step": i + 1, "action": action["action_name"], "result": ex.args[0]}
)
elif isinstance(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]
step_results.append(
{
"step": i + 1,
"parallel_actions": [a["action_name"] for a in actions],
"results": results,
}
)
# 向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)
# 设置成功状态和返回值
execution_success = True
protocol_return_value = {
"protocol_name": protocol_name,
"steps_executed": len(protocol_steps),
"step_results": step_results,
"total_time": time.time() - time_start,
}
goal_handle.succeed()
except Exception as e:
# 捕获并记录错误信息
str_step_results = [
{
k: dict(message_to_ordereddict(v)) if k == "result" and hasattr(v, "SLOT_TYPES") else v
for k, v in i.items()
}
for i in step_results
]
execution_error = f"{traceback.format_exc()}\n\nStep Result: {pformat(str_step_results)}"
execution_success = False
self.lab_logger().error(f"协议 {protocol_name} 执行出错: {str(e)} \n{traceback.format_exc()}")
# 设置动作失败
goal_handle.abort()
finally:
self._busy = False
# 创建结果消息
result = action_value_mapping["type"].Result()
result.success = execution_success
# 获取结果消息类型信息，检查是否有return_info字段
result_msg_types = action_value_mapping["type"].Result.get_fields_and_field_types()
# 设置return_info字段（如果存在）
for attr_name in result_msg_types.keys():
if attr_name in ["success", "reached_goal"]:
setattr(result, attr_name, execution_success)
elif attr_name == "return_info":
setattr(
result,
attr_name,
get_result_info_str(execution_error, execution_success, protocol_return_value),
)
self.lab_logger().info(f"协议 {protocol_name} 完成并返回结果")
return result
return execute_protocol
async def execute_single_action(self, device_id, action_name, action_kwargs):
"""执行单个动作"""
# 构建动作ID
if action_name == "log_message":
self.lab_logger().info(f"[Protocol Log] {action_kwargs}")
raise ROS2WorkstationNodeTempError(f"[Protocol Log] {action_kwargs}")
if device_id in ["", None, "self"]:
action_id = f"/devices/{self.device_id}/{action_name}"
else:
action_id = f"/devices/{device_id}/{action_name}" # 执行时取消了主节点信息 /{self.device_id}
# 检查动作客户端是否存在
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: ROS2DeviceNode, communication_device: ROS2DeviceNode, read_method, write_method
):
"""为设备设置硬件接口代理"""
# extra_info = [getattr(device.driver_instance, info) for info in communication_device.ros_node_instance._hardware_interface.get("extra_info", [])]
write_func = getattr(
communication_device.driver_instance, communication_device.ros_node_instance._hardware_interface["write"]
)
read_func = getattr(
communication_device.driver_instance, communication_device.ros_node_instance._hardware_interface["read"]
)
def _read(*args, **kwargs):
return read_func(*args, **kwargs)
def _write(*args, **kwargs):
return write_func(*args, **kwargs)
if read_method:
# bound_read = MethodType(_read, device.driver_instance)
setattr(device.driver_instance, read_method, _read)
if write_method:
# bound_write = MethodType(_write, device.driver_instance)
setattr(device.driver_instance, write_method, _write)

1093
unilabos/ros/nodes/resource_tracker.py
View File

File diff suppressed because it is too large Load Diff

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

@@ -4,9 +4,9 @@
这个模块包含用于创建设备类实例的工厂类。
基础工厂类提供通用的实例创建方法，而特定工厂类提供针对特定设备类的创建方法。
"""
import asyncio
import inspect
import json
import traceback
from abc import abstractmethod
from typing import Type, Any, Dict, Optional, TypeVar, Generic
@@ -54,7 +54,6 @@ class DeviceClassCreator(Generic[T]):
if c["type"] != "device":
self.resource_tracker.add_resource(c)
def create_instance(self, data: Dict[str, Any]) -> T:
"""
创建设备类实例
@@ -119,7 +118,9 @@ class PyLabRobotCreator(DeviceClassCreator[T]):
return nested_dict_to_list(resource), Resource
return resource, source_type
def _process_resource_references(self, data: Any, to_dict=False, states=None, prefix_path="") -> Any:
def _process_resource_references(
self, data: Any, to_dict=False, states=None, prefix_path="", name_to_uuid=None
) -> Any:
"""
递归处理资源引用，替换_resource_child_name对应的资源
@@ -128,11 +129,13 @@ class PyLabRobotCreator(DeviceClassCreator[T]):
to_dict: 是否返回字典形式的资源
states: 用于保存所有资源状态
prefix_path: 当前递归路径
name_to_uuid: name到uuid的映射字典
Returns:
处理后的数据
"""
from pylabrobot.resources import Deck, Resource
if states is None:
states = {}
@@ -147,7 +150,7 @@ class PyLabRobotCreator(DeviceClassCreator[T]):
target_type = import_manager.get_class(type_path)
contain_model = not issubclass(target_type, Deck)
resource, target_type = self._process_resource_mapping(resource, target_type)
resource_instance: Resource = resource_ulab_to_plr(resource, contain_model)
resource_instance: Resource = resource_ulab_to_plr(resource, contain_model) # 带state
states[prefix_path] = resource_instance.serialize_all_state()
# 使用 prefix_path 作为 key 存储资源状态
if to_dict:
@@ -156,6 +159,9 @@ class PyLabRobotCreator(DeviceClassCreator[T]):
return serialized
else:
self.resource_tracker.add_resource(resource_instance)
# 立即设置UUID，state已经在resource_ulab_to_plr中处理过了
if name_to_uuid:
self.resource_tracker.loop_set_uuid(resource_instance, name_to_uuid)
return resource_instance
except Exception as e:
logger.warning(f"无法导入资源类型 {type_path}: {e}")
@@ -170,12 +176,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)
result[key] = self._process_resource_references(value, 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}]")
self._process_resource_references(item, to_dict, states, f"{prefix_path}[{i}]", name_to_uuid)
for i, item in enumerate(data)
]
@@ -194,31 +200,51 @@ class PyLabRobotCreator(DeviceClassCreator[T]):
"""
deserialize_error = None
stack = None
# 递归遍历 children 构建 name_to_uuid 映射
def collect_name_to_uuid(children_dict: Dict[str, Any], result: Dict[str, str]):
"""递归遍历嵌套的 children 字典，收集 name 到 uuid 的映射"""
for child in children_dict.values():
if isinstance(child, dict):
result[child["name"]] = child["uuid"]
collect_name_to_uuid(child["children"], result)
name_to_uuid = {}
collect_name_to_uuid(self.children, name_to_uuid)
if self.has_deserialize:
deserialize_method = getattr(self.device_cls, "deserialize")
spect = inspect.signature(deserialize_method)
spec_args = spect.parameters
for param_name, param_value in data.copy().items():
if isinstance(param_value, dict) and "_resource_child_name" in param_value and "_resource_type" not in param_value:
if (
isinstance(param_value, dict)
and "_resource_child_name" in param_value
and "_resource_type" not in param_value
):
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']}")
# 首先处理资源引用
states = {}
processed_data = self._process_resource_references(data, to_dict=True, states=states)
processed_data = self._process_resource_references(
data, to_dict=True, states=states, name_to_uuid=name_to_uuid
)
try:
self.device_instance = deserialize_method(**processed_data)
from pylabrobot.resources import Resource
self.device_instance: Resource = deserialize_method(**processed_data)
self.resource_tracker.loop_set_uuid(self.device_instance, name_to_uuid)
all_states = self.device_instance.serialize_all_state()
for k, v in states.items():
logger.debug(f"PyLabRobot反序列化设置状态：{k}")
for kk, vv in all_states.items():
if kk not in v:
v[kk] = vv
self.device_instance.deck.load_all_state(v)
self.device_instance.load_all_state(v)
self.resource_tracker.add_resource(self.device_instance)
self.post_create()
self.post_create() # 对应DeviceClassCreator进行调用
return self.device_instance # type: ignore
except Exception as e:
# 先静默继续，尝试另外一种创建方法
@@ -230,12 +256,16 @@ class PyLabRobotCreator(DeviceClassCreator[T]):
spect = inspect.signature(self.device_cls.__init__)
spec_args = spect.parameters
for param_name, param_value in data.copy().items():
if isinstance(param_value, dict) and "_resource_child_name" in param_value and "_resource_type" not in param_value:
if (
isinstance(param_value, dict)
and "_resource_child_name" in param_value
and "_resource_type" not in param_value
):
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)
self.device_instance = super(PyLabRobotCreator, self).create_instance(processed_data)
processed_data = self._process_resource_references(data, to_dict=False, name_to_uuid=name_to_uuid)
self.device_instance = super(PyLabRobotCreator, self).create_instance(processed_data) # 补全变量后直接调用，调用的自身的attach_resource
except Exception as e:
logger.error(f"PyLabRobot创建实例失败: {e}")
logger.error(f"PyLabRobot创建实例堆栈: {traceback.format_exc()}")
@@ -248,59 +278,77 @@ class PyLabRobotCreator(DeviceClassCreator[T]):
return self.device_instance
def post_create(self):
if hasattr(self.device_instance, "setup") and asyncio.iscoroutinefunction(getattr(self.device_instance, "setup")):
if hasattr(self.device_instance, "setup") and asyncio.iscoroutinefunction(
getattr(self.device_instance, "setup")
):
from unilabos.ros.nodes.base_device_node import ROS2DeviceNode
def done_cb(*args):
from pylabrobot.resources import set_volume_tracking
# from pylabrobot.resources import set_tip_tracking
set_volume_tracking(enabled=True)
# set_tip_tracking(enabled=True) # 序列化tip_spot has为False
logger.debug(f"PyLabRobot设备实例 {self.device_instance} 设置完成")
from unilabos.config.config import BasicConfig
if BasicConfig.vis_2d_enable:
from pylabrobot.visualizer.visualizer import Visualizer
vis = Visualizer(resource=self.device_instance, open_browser=True)
def vis_done_cb(*args):
logger.info(f"PyLabRobot设备实例开启了Visualizer {self.device_instance}")
ROS2DeviceNode.run_async_func(vis.setup).add_done_callback(vis_done_cb)
logger.debug(f"PyLabRobot设备实例提交开启Visualizer {self.device_instance}")
ROS2DeviceNode.run_async_func(getattr(self.device_instance, "setup")).add_done_callback(done_cb)
class ProtocolNodeCreator(DeviceClassCreator[T]):
class WorkstationNodeCreator(DeviceClassCreator[T]):
"""
ProtocolNode设备类创建器
WorkstationNode设备类创建器
这个类提供了针对ProtocolNode设备类的实例创建方法，处理children参数。
这个类提供了针对WorkstationNode设备类的实例创建方法，处理children参数。
"""
def __init__(self, cls: Type[T], children: Dict[str, Any], resource_tracker: DeviceNodeResourceTracker):
"""
初始化ProtocolNode设备类创建器
初始化WorkstationNode设备类创建器
Args:
cls: ProtocolNode设备类
cls: WorkstationNode设备类
children: 子资源字典，用于资源替换
"""
super().__init__(cls, children, resource_tracker)
def create_instance(self, data: Dict[str, Any]) -> T:
"""
从数据创建ProtocolNode设备实例
从数据创建WorkstationNode设备实例
Args:
data: 用于创建实例的数据
Returns:
ProtocolNode设备类实例
WorkstationNode设备类实例
"""
try:
# 创建实例，额外补充一个给protocol node的字段，后面考虑取消
data["children"] = self.children
self.device_instance = super(ProtocolNodeCreator, self).create_instance(data)
deck_dict = data.get("deck")
if deck_dict:
from pylabrobot.resources import Deck, Resource
plrc = PyLabRobotCreator(Deck, self.children, self.resource_tracker)
deck = plrc.create_instance(deck_dict)
data["deck"] = deck
else:
data["deck"] = None
self.device_instance = super(WorkstationNodeCreator, self).create_instance(data)
self.post_create()
return self.device_instance
except Exception as e:
logger.error(f"ProtocolNode创建实例失败: {e}")
logger.error(f"ProtocolNode创建实例堆栈: {traceback.format_exc()}")
logger.error(f"WorkstationNode创建实例失败: {e}")
logger.error(f"WorkstationNode创建实例堆栈: {traceback.format_exc()}")
raise