mirror of
https://github.com/dptech-corp/Uni-Lab-OS.git
synced 2026-02-07 23:45:10 +00:00
Ready for open source (#47)
* Create app/main API * create example device * create ROS backend and example device SDK Wrapper * Add ROS host and host starting from app.py * Add gripper device and mock implementation * add "status_types" & "action_types" to ROS device decorator * add ActionServer debug example * [bugfix] complete mock gripper example * ROS Backend Host for Device action calling and Resource management * add conda/mamba ENV file * add host_node communication with app/main.py * add action message value mappings and converters * Update ilabos.yaml * Update issue templates * example devices.json and resources.json * Fix Device wrapper to use async property and actions (#7) * Fix Device wrapper to use async property and actions * Resolve #1 : support async get methods and actions. Give new examples. * add both sync/async GRBL controller SDK * 2 call device actions from appmainpy api to ros hostpy (#8) * feature: add job * fix:node start * feature:add get job status * fix:get device * clean * Resolve #5 device connection diagram and workflow compilation support (#9) * add syringe pump device and its compilation using device connection diagram * add RunzeSyringePump real device with ROS2 example * Prototype machine with 1 pump and 1 CNC * add ROS2ProtocolNode and related functions * add ilabos_msgs (to use PumpTransfer action) * add example device connection graph * refactor protocol_node code into separate file * add ROS2SerialNode * add SerialCommand srv in ilabos_msgs * add pump_protocol example, and fix bugs * [fix] serial service: avoid async service deadlock by directly call serial `send_command` * use SendCmd instead of SingleJointPosition for valve control * initialize device connection graph when server starts * Fix #5: async workflow execution (#16) * add rclpyx and protocol example for async-native workflow * use async in ROS2ProtocolNode, and host initialization * add examples of "ros-async" protocol implementation, and `run_in_event_loop` for using native async functions * use "ros-async" in protocols and device nodes * fix pump_protocol: push to 0.0 μL * Envs, docs, and conda recipes (#19) * update ENV: use python 3.11 and deprecate ros-humble-gazebo-ros * add ilabos-msgs conda recipe * Update ilabos.yaml * fix recipe and env yaml * Add sphinx docs * add aichemeco * add bioyong * add bioyong * Support XDL devices & protocols (#20) * [Feature] support multiple protocols in a single ProtocolNode * add Junjie's code * Support "Clean" protocol * Update Grignard_flow_batchreact_single_pumpvalve.json * test_grignard_add * add stir device node and example * Update device_node.py add print_publish flag to control the node's log output * NH4Cl_add * add "HeaterStirrer" device and "HeatChill" action * add wait time after each pump action for equilibration * fix stir * add Separate protocol * Refactor Separator device and Stir action * add rotavap_node * fix stir * add chiller node * Move rinsings into PumpTransfer * Fix SeparateProtocol under refactored Separator device and Stir action * Supports automatically add new protocol action_types * fix PumpTransfer protocol because of rinsing * Add Rotavap and Chiller devices * fix SeparateProtocol * add EvaporateProtocol * add rotavap devices config * fix HeaterStirrer and SeparatorController IO * Fix automatically add new protocol action_types * Add HeaterStirrer and SeparatorController device config * fix pump protocols * Fix Evaporate action * Update evaporate_protocol.py * add temp_sensor node and add function remap * update docs --------- Co-authored-by: 王俊杰 <1800011822@pku.edu.cn> Co-authored-by: q434343 <554662886@qq.com> * fix aichemeco * [Bugfix] fix Windows conda packaging * add file upload api * update dependencies: force to use 3.11 and remove conflict in WIN64 and OSX64 * update dependencies: force to use 3.11 and remove conflict in WIN64 and OSX64 * Create aichemeco_simple.py * fix * update * add aichemeco file * MQTT [1/2]: action start (#25) * add mq * fix * clean * add class * fix excel * update bioyong * add api * fix --------- Co-authored-by: caok@dp.tech <xiaoyeqiannian@163.com> * motor & grasp * Add Grasp motor support and enhance EleGripper class - Introduced a new motor configuration for Grasp in sjtu.json. - Updated EleGripper class to inherit from UniversalDriver and added status property. - Implemented move_and_rotate method for coordinated movement. - Adjusted threading logic in EleGripper initialization. - Registered Grasp motor in ROS2 device node configuration. This commit enhances the motor control capabilities by integrating the Grasp motor and improving the existing EleGripper functionality. * fix read data lenth * Update Grasp.py * MQTT (2/2): publish Device Status, Action Feedback & Results (#27) * Add bridges in HostNode for device_status publishing * Add "bridges" selection (fastapi & mqtt) when app start * add MQ feedback & result publisher, and fix message converter * fix UUID converting between ROS and MQ * lint api model.py * Continuous controllers: PID, MPC, custom controllers etc. (#23) * add controller config & wrapper * add controller setup at app.main * control loop example * fix com port * add agv , ur_arm and raman * MQTT (3/4): Unified Resources and Sync when starting the server (#28) * update http upload api * generate uuid when init device * example resource json * fix * add new example full-content json (device, resource, graph) * fix full-content json and related reading code * fix * add json_schema when initialize resources * fix * update schema * refactor heaterstirrer.dalong * fix * fix refactor heaterstirrer.dalong * refactor syringepump.runze: use ml instead of μL * Update ilabos/ros/host.py Co-authored-by: sourcery-ai[bot] <58596630+sourcery-ai[bot]@users.noreply.github.com> --------- Co-authored-by: 王俊杰 <1800011822@pku.edu.cn> Co-authored-by: sourcery-ai[bot] <58596630+sourcery-ai[bot]@users.noreply.github.com> * Distributed initialization with self-organizing network (#29) * add distributed launching option "--without_host" * fix --------- Co-authored-by: 王俊杰 <1800011822@pku.edu.cn> * Refactor Workstation: Add resource service and tracking (#30) * move ilabos/ros/rpc to ilabos/device_comms/rpc, and merge bioyond/aichemeco files under /devices * add Resource srv and message_converter * move graphio to ilabos/resources * refactor resources type conversion * add resource clients in device_node * add mock resources service * pass Gripper1 resource test * update http resource services * add AGV compile function * add AGV transfer protocol * update full mock_gripper edit_id example * update full mock_gripper edit_id example * get and update resource also in protocol_node * mock resource update in AichemecoHiwo * Create HT_hiwo.json * add children in resources * bugfixes * fix rpc * add Revvity winprep --------- Co-authored-by: wjjxxx <43375851+wjjxxx@users.noreply.github.com> Co-authored-by: 3218923350 <105201755+3218923350@users.noreply.github.com> * Distributed launch (2/2): distributed resource create (#32) * add resource_add request to host for slave mode * add AGV * fix protocol resources * optimize host callbacks * bugfixes * add revvity registry --------- Co-authored-by: 王俊杰 <1800011822@pku.edu.cn> Co-authored-by: wjjxxx <43375851+wjjxxx@users.noreply.github.com> * Refactor Driver Files Structure (#33) * Integration with pywinauto & recorder Added execute run and initialize procdure * 酶标仪状态检测、使用示例，整体流程待测试 * nivo ready version * Add HPLC driver and example script - Introduced HPLCDriver class for managing HPLC device status and operations. - Implemented device status monitoring and command execution via ROS2 actions. - Added example script (hplc.py) demonstrating how to run commands on the HPLC device. - Created PlayerUtil and UniversalDriver classes for shared functionality across devices. - Refactored NivoDriver to utilize the new UniversalDriver structure. - Enhanced error handling and process management in the NivoDriver. * 修复start的错误定位 * hplc tested ok * relative path to build msgs * template_driver & jiageng devices * fetch correct status type and action type * fix mtype fetch * gpc bus integration * ilab build * remove chs * recipe rename * modbus update 1 * json available * hplc & modbus rewrite * Update AgilentHPLC.py hplc datafile reader * move ilabos/ros/rpc to ilabos/device_comms/rpc, and merge bioyond/aichemeco files under /devices * modbus分设备 * gpc * gpc 2 * fix address * default register node * fix MainScreenGPC * add Resource srv and message_converter * move graphio to ilabos/resources * refactor resources type conversion * add resource clients in device_node * add mock resources service * pass Gripper1 resource test * update http resource services * add AGV compile function * add AGV transfer protocol * update recipe.yaml * update full mock_gripper edit_id example * update full mock_gripper edit_id example * get and update resource also in protocol_node * mock resource update in AichemecoHiwo * feat: add other jiageng PLC device code * ilabos compile * correct format * correct recipe format * correct setup.py format * remove unnecessary files * remove unnecessary files * Create HT_hiwo.json * add children in resources * hplc support sample_id * correct hplc sample_id * correct hplc sample_id * hplc upload * fix type hint * oss upload tested ver * recipe yaml fix for linux * update installation yaml * refactor: moved all driver files according to its feat * merge main to dev --------- Co-authored-by: 王俊杰 <2201110460@stu.pku.edu.cn> Co-authored-by: Junhan Chang <changjh@pku.edu.cn> Co-authored-by: jiawei <miaojiawei@dp.tech> * add: NMR LH and RU device control (#34) * Add Registry for device drivers and Support GraphML (#35) * read chemputer graphml * read graphml in app/main * add devices in ros/devices * add schema for devices * read registry directory and initialize when entry from main * Delete devices.py * Update add_protocol.md * delete unecessary files * feat: 2278 devices registry yaml (#36) * read chemputer graphml * read graphml in app/main * add devices in ros/devices * add schema for devices * read registry directory and initialize when entry from main * Delete devices.py * add: NMR LH and RU device control * fix: modify jiageng devices registry --------- Co-authored-by: Junhan Chang <changjh@pku.edu.cn> * Device/Resource Registry and GraphML support (#37) * add resource type conversion to PLR * add resource registry and test * add docs * fix registry * add solenoid_valve_mock, its registry and test * fix registry for directly using examples * add EvacuateAndRefillProtocol and testcases * allow function sequence call in ACTION * add read & write & extra_info for hardware_interface * Update device_node.py * add solenoid valve * add doc developer guide yaml * fixes for starting IK station * add graphml grouping parser * fix graphml grouping parser * add communication edge parser * fix io solenoid valve * Update .gitignore * Update plates.yaml --------- Co-authored-by: ColumbiaCC <2100011801@stu.pku.edu.cn> * Uni-Lab Doc v0.2 (#39) * add Uni-Lab docs * change doc name * Dev (#41) * Integration with pywinauto & recorder Added execute run and initialize procdure * 酶标仪状态检测、使用示例，整体流程待测试 * nivo ready version * Add HPLC driver and example script - Introduced HPLCDriver class for managing HPLC device status and operations. - Implemented device status monitoring and command execution via ROS2 actions. - Added example script (hplc.py) demonstrating how to run commands on the HPLC device. - Created PlayerUtil and UniversalDriver classes for shared functionality across devices. - Refactored NivoDriver to utilize the new UniversalDriver structure. - Enhanced error handling and process management in the NivoDriver. * 修复start的错误定位 * hplc tested ok * relative path to build msgs * template_driver & jiageng devices * fetch correct status type and action type * fix mtype fetch * gpc bus integration * ilab build * remove chs * recipe rename * modbus update 1 * json available * hplc & modbus rewrite * Update AgilentHPLC.py hplc datafile reader * move ilabos/ros/rpc to ilabos/device_comms/rpc, and merge bioyond/aichemeco files under /devices * modbus分设备 * gpc * gpc 2 * fix address * default register node * fix MainScreenGPC * add Resource srv and message_converter * move graphio to ilabos/resources * refactor resources type conversion * add resource clients in device_node * add mock resources service * pass Gripper1 resource test * update http resource services * add AGV compile function * add AGV transfer protocol * update recipe.yaml * update full mock_gripper edit_id example * update full mock_gripper edit_id example * get and update resource also in protocol_node * mock resource update in AichemecoHiwo * feat: add other jiageng PLC device code * ilabos compile * correct format * correct recipe format * correct setup.py format * remove unnecessary files * remove unnecessary files * Create HT_hiwo.json * add children in resources * hplc support sample_id * correct hplc sample_id * correct hplc sample_id * hplc upload * fix type hint * oss upload tested ver * recipe yaml fix for linux * update installation yaml * refactor: moved all driver files according to its feat * merge main to dev * add HPLC registry and json * 升级 ros2-distro-mutex 依赖版本至 0.6 * 修改 ros2-distro-mutex 依赖版本为通配符匹配 * 更新 ros-humble-ilabos-msgs 依赖为 robostack-humble 命名空间 * add resource type conversion to PLR * add resource registry and test * feat: 更新oss上传 * fix device id * add docs * fix registry * add solenoid_valve_mock, its registry and test * fix registry for directly using examples * add EvacuateAndRefillProtocol and testcases * allow function sequence call in ACTION * add read & write & extra_info for hardware_interface * Update device_node.py * add solenoid valve * add doc developer guide yaml * use robostack-staging * rclpy version test * lower rclpy * ensure 0.6* env * fixes for starting IK station * add graphml grouping parser * fix graphml grouping parser * add communication edge parser * fix io solenoid valve * Update .gitignore * Update plates.yaml * Feature/device node later init (#40) * 修改config路径，方便后续打包 增加device_node打印 * 支持plr序列化/init创建 * 统一命名 * import mgr logger optimize banner print * 日志OK * fix unicorn frame * banner print * correct import format * file path changes * 取消后补全，在加载设备的时候直接替换 * converter update * web page update * 在线device更新，node继承替换 * 修复动作、状态的类型缺失 和 命令提示 * web功能实现结束 * host节点更改完成 新增status时间戳管理 新增每10s动态发现其他node * ros2类型的节点也应该被包一次 * 修复类型提示 * websocket 动态显示状态 * add workflow & book theme for docs * add workflow & book theme for docs * fix workflow build * fix workflow build * 理清启动关系 * stm32 example * mac . name * device_instance device_cls * 新增config添加方式 更新mqtt提示 * plr test * 移动is_host_mode 新增slave_no_host * 确保config优先修改生效 * fix graph io * 支持带参数传入 * 支持物料解析 * 支持物料解析 * device为空的时候不进行绑定或初始化 * protocol node new * protocol node runnable * protocol node runnable --------- Co-authored-by: 王俊杰 <2201110460@stu.pku.edu.cn> Co-authored-by: Junhan Chang <changjh@pku.edu.cn> Co-authored-by: jiawei <miaojiawei@dp.tech> Co-authored-by: ColumbiaCC <2100011801@stu.pku.edu.cn> * Dev (#45) * Integration with pywinauto & recorder Added execute run and initialize procdure * 酶标仪状态检测、使用示例，整体流程待测试 * nivo ready version * Add HPLC driver and example script - Introduced HPLCDriver class for managing HPLC device status and operations. - Implemented device status monitoring and command execution via ROS2 actions. - Added example script (hplc.py) demonstrating how to run commands on the HPLC device. - Created PlayerUtil and UniversalDriver classes for shared functionality across devices. - Refactored NivoDriver to utilize the new UniversalDriver structure. - Enhanced error handling and process management in the NivoDriver. * 修复start的错误定位 * hplc tested ok * relative path to build msgs * template_driver & jiageng devices * fetch correct status type and action type * fix mtype fetch * gpc bus integration * ilab build * remove chs * recipe rename * modbus update 1 * json available * hplc & modbus rewrite * Update AgilentHPLC.py hplc datafile reader * move ilabos/ros/rpc to ilabos/device_comms/rpc, and merge bioyond/aichemeco files under /devices * modbus分设备 * gpc * gpc 2 * fix address * default register node * fix MainScreenGPC * add Resource srv and message_converter * move graphio to ilabos/resources * refactor resources type conversion * add resource clients in device_node * add mock resources service * pass Gripper1 resource test * update http resource services * add AGV compile function * add AGV transfer protocol * update recipe.yaml * update full mock_gripper edit_id example * update full mock_gripper edit_id example * get and update resource also in protocol_node * mock resource update in AichemecoHiwo * feat: add other jiageng PLC device code * ilabos compile * correct format * correct recipe format * correct setup.py format * remove unnecessary files * remove unnecessary files * Create HT_hiwo.json * add children in resources * hplc support sample_id * correct hplc sample_id * correct hplc sample_id * hplc upload * fix type hint * oss upload tested ver * recipe yaml fix for linux * update installation yaml * refactor: moved all driver files according to its feat * merge main to dev * add HPLC registry and json * 升级 ros2-distro-mutex 依赖版本至 0.6 * 修改 ros2-distro-mutex 依赖版本为通配符匹配 * 更新 ros-humble-ilabos-msgs 依赖为 robostack-humble 命名空间 * add resource type conversion to PLR * add resource registry and test * feat: 更新oss上传 * fix device id * add docs * fix registry * add solenoid_valve_mock, its registry and test * fix registry for directly using examples * add EvacuateAndRefillProtocol and testcases * allow function sequence call in ACTION * add read & write & extra_info for hardware_interface * Update device_node.py * add solenoid valve * add doc developer guide yaml * use robostack-staging * rclpy version test * lower rclpy * ensure 0.6* env * fixes for starting IK station * add graphml grouping parser * fix graphml grouping parser * add communication edge parser * fix io solenoid valve * Update .gitignore * Update plates.yaml * Feature/device node later init (#40) * 修改config路径，方便后续打包 增加device_node打印 * 支持plr序列化/init创建 * 统一命名 * import mgr logger optimize banner print * 日志OK * fix unicorn frame * banner print * correct import format * file path changes * 取消后补全，在加载设备的时候直接替换 * converter update * web page update * 在线device更新，node继承替换 * 修复动作、状态的类型缺失 和 命令提示 * web功能实现结束 * host节点更改完成 新增status时间戳管理 新增每10s动态发现其他node * ros2类型的节点也应该被包一次 * 修复类型提示 * websocket 动态显示状态 * add workflow & book theme for docs * add workflow & book theme for docs * fix workflow build * fix workflow build * 理清启动关系 * stm32 example * mac . name * device_instance device_cls * 新增config添加方式 更新mqtt提示 * plr test * 移动is_host_mode 新增slave_no_host * 确保config优先修改生效 * fix graph io * 支持带参数传入 * 支持物料解析 * 支持物料解析 * device为空的时候不进行绑定或初始化 * protocol node new * protocol node runnable * protocol node runnable * Feature/device node later init (#42) * 修改config路径，方便后续打包 增加device_node打印 * 支持plr序列化/init创建 * 统一命名 * import mgr logger optimize banner print * 日志OK * fix unicorn frame * banner print * correct import format * file path changes * 取消后补全，在加载设备的时候直接替换 * converter update * web page update * 在线device更新，node继承替换 * 修复动作、状态的类型缺失 和 命令提示 * web功能实现结束 * host节点更改完成 新增status时间戳管理 新增每10s动态发现其他node * ros2类型的节点也应该被包一次 * 修复类型提示 * websocket 动态显示状态 * add workflow & book theme for docs * add workflow & book theme for docs * fix workflow build * fix workflow build * 理清启动关系 * stm32 example * mac . name * device_instance device_cls * 新增config添加方式 更新mqtt提示 * plr test * 移动is_host_mode 新增slave_no_host * 确保config优先修改生效 * fix graph io * 支持带参数传入 * 支持物料解析 * 支持物料解析 * device为空的时候不进行绑定或初始化 * protocol node new * protocol node runnable * protocol node runnable * action * plr suc * plr suc!! * plr suc!! * plr suc!! * plr msgs * Feature/device node later init (#43) * 修改config路径，方便后续打包 增加device_node打印 * 支持plr序列化/init创建 * 统一命名 * import mgr logger optimize banner print * 日志OK * fix unicorn frame * banner print * correct import format * file path changes * 取消后补全，在加载设备的时候直接替换 * converter update * web page update * 在线device更新，node继承替换 * 修复动作、状态的类型缺失 和 命令提示 * web功能实现结束 * host节点更改完成 新增status时间戳管理 新增每10s动态发现其他node * ros2类型的节点也应该被包一次 * 修复类型提示 * websocket 动态显示状态 * add workflow & book theme for docs * add workflow & book theme for docs * fix workflow build * fix workflow build * 理清启动关系 * stm32 example * mac . name * device_instance device_cls * 新增config添加方式 更新mqtt提示 * plr test * 移动is_host_mode 新增slave_no_host * 确保config优先修改生效 * fix graph io * 支持带参数传入 * 支持物料解析 * 支持物料解析 * device为空的时候不进行绑定或初始化 * protocol node new * protocol node runnable * protocol node runnable * action * plr suc * plr suc!! * plr suc!! * plr suc!! * plr msgs * plr * action * plr reg fix * Feature/device node later init (#44) * 修改config路径，方便后续打包 增加device_node打印 * 支持plr序列化/init创建 * 统一命名 * import mgr logger optimize banner print * 日志OK * fix unicorn frame * banner print * correct import format * file path changes * 取消后补全，在加载设备的时候直接替换 * converter update * web page update * 在线device更新，node继承替换 * 修复动作、状态的类型缺失 和 命令提示 * web功能实现结束 * host节点更改完成 新增status时间戳管理 新增每10s动态发现其他node * ros2类型的节点也应该被包一次 * 修复类型提示 * websocket 动态显示状态 * add workflow & book theme for docs * add workflow & book theme for docs * fix workflow build * fix workflow build * 理清启动关系 * stm32 example * mac . name * device_instance device_cls * 新增config添加方式 更新mqtt提示 * plr test * 移动is_host_mode 新增slave_no_host * 确保config优先修改生效 * fix graph io * 支持带参数传入 * 支持物料解析 * 支持物料解析 * device为空的时候不进行绑定或初始化 * protocol node new * protocol node runnable * protocol node runnable * action * plr suc * plr suc!! * plr suc!! * plr suc!! * plr msgs * plr * fix convert error fix async logic error added async error print * new device test * test resource add * test resource add * test resource add * test resource add * local env setup * node type fix temp fix root_node error fix convert res from type error * resource tracker * fix bug from qhh * fix bug from qhh * fix bug from qhh * fix bug from qhh * refactor MQTT client logging and connection handling; update group ID in config * driver_params allow empty * allow other init param * fix driver param and enhance type hint * refactor MQConfig to use double quotes for string literals * fix wrong function calling * fix wrong function calling * fix log for mac * fix networkx compatibility * add mqtt loggers * add action to jsonschema converter * random client id * type converter & registry * correct conversion * fix action publish only from discovered devices * add "Bio" tag for action doc generation * 改进module提示 * Fix doc * mqtt不连接也可用 性价样例提示 * add docs * 更新plr test案例 * Update intro.md * 更新有机案例 * skip --------- Co-authored-by: Harvey Que <Q-Query@outlook.com> Co-authored-by: Junhan Chang <1700011741@pku.edu.cn> --------- Co-authored-by: 王俊杰 <2201110460@stu.pku.edu.cn> Co-authored-by: Junhan Chang <changjh@pku.edu.cn> Co-authored-by: jiawei <miaojiawei@dp.tech> Co-authored-by: ColumbiaCC <2100011801@stu.pku.edu.cn> Co-authored-by: Harvey Que <Q-Query@outlook.com> Co-authored-by: Junhan Chang <1700011741@pku.edu.cn> * Canonicalize before Open Source (#46) * big big refactor try01 * refactor 02 --------- Co-authored-by: ck <xiaoyeqiannian@163.com> Co-authored-by: 王俊杰 <1800011822@pku.edu.cn> Co-authored-by: q434343 <554662886@qq.com> Co-authored-by: Xuwznln <xuwznln@gmail.com> Co-authored-by: sourcery-ai[bot] <58596630+sourcery-ai[bot]@users.noreply.github.com> Co-authored-by: wjjxxx <43375851+wjjxxx@users.noreply.github.com> Co-authored-by: 3218923350 <105201755+3218923350@users.noreply.github.com> Co-authored-by: Xuwznln <1023025701@qq.com> Co-authored-by: 王俊杰 <2201110460@stu.pku.edu.cn> Co-authored-by: jiawei <miaojiawei@dp.tech> Co-authored-by: Jiawei <91898272+jiawei723@users.noreply.github.com> Co-authored-by: ColumbiaCC <2100011801@stu.pku.edu.cn> Co-authored-by: Harvey Que <Q-Query@outlook.com>
This commit is contained in:
Junhan Chang
GitHub
267
unilabos/ros/nodes/presets/protocol_node.py
Normal file
267
unilabos/ros/nodes/presets/protocol_node.py
Normal file
@@ -0,0 +1,267 @@
|
||||
import time
|
||||
import asyncio
|
||||
import traceback
|
||||
from typing import Union
|
||||
|
||||
import rclpy
|
||||
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
|
||||
|
||||
|
||||
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,
|
||||
)
|
||||
|
||||
# 初始化子设备
|
||||
communication_node_id = None
|
||||
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
|
||||
d = self.initialize_device(device_id, device_config)
|
||||
if d is None:
|
||||
continue
|
||||
|
||||
if "serial_" in device_id or "io_" in device_id:
|
||||
communication_node_id = device_id
|
||||
continue
|
||||
|
||||
# 设置硬件接口代理
|
||||
if d and hasattr(d, "_hardware_interface"):
|
||||
if (
|
||||
hasattr(d, d._hardware_interface["name"])
|
||||
and hasattr(d, d._hardware_interface["write"])
|
||||
and (d._hardware_interface["read"] is None or hasattr(d, d._hardware_interface["read"]))
|
||||
):
|
||||
|
||||
name = getattr(d, d._hardware_interface["name"])
|
||||
read = d._hardware_interface.get("read", None)
|
||||
write = d._hardware_interface.get("write", None)
|
||||
|
||||
# 如果硬件接口是字符串，通过通信设备提供
|
||||
if isinstance(name, str) and communication_node_id in self.sub_devices:
|
||||
self._setup_hardware_proxy(d, self.sub_devices[communication_node_id], read, write)
|
||||
|
||||
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}"
|
||||
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
|
||||
for action_name, action_mapping in node._action_value_mappings.items():
|
||||
action_id = f"/devices/{device_id_abs}/{action_name}"
|
||||
if action_id not in self._action_clients:
|
||||
self._action_clients[action_id] = ActionClient(
|
||||
self, action_mapping["type"], action_id, callback_group=self.callback_group
|
||||
)
|
||||
self.lab_logger().debug(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().debug(f"发布动作: {action_name}, 类型: {str_action_type}")
|
||||
|
||||
def _create_protocol_execute_callback(self, protocol_name, protocol_steps_generator):
|
||||
async def execute_protocol(goal_handle: ServerGoalHandle):
|
||||
"""执行完整的工作流"""
|
||||
self.get_logger().info(f'Executing {protocol_name} action...')
|
||||
action_value_mapping = self._action_value_mappings[protocol_name]
|
||||
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"])
|
||||
|
||||
# 向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()
|
||||
r.id = protocol_kwargs[k]["id"] if v == "unilabos_msgs/Resource" else protocol_kwargs[k][0]["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])
|
||||
|
||||
from unilabos.resources.graphio import physical_setup_graph
|
||||
self.get_logger().info(f'Working on physical setup: {physical_setup_graph}')
|
||||
protocol_steps = protocol_steps_generator(G=physical_setup_graph, **protocol_kwargs)
|
||||
|
||||
self.get_logger().info(f'Goal received: {protocol_kwargs}, running steps: \n{protocol_steps}')
|
||||
|
||||
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 type(action) == dict:
|
||||
# 如果是单个动作，直接执行
|
||||
if action["action_name"] == "wait":
|
||||
time.sleep(action["action_kwargs"]["time"])
|
||||
else:
|
||||
result = await self.execute_single_action(**action)
|
||||
elif type(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]
|
||||
|
||||
# 向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)
|
||||
|
||||
goal_handle.succeed()
|
||||
result = action_value_mapping["type"].Result()
|
||||
result.success = True
|
||||
|
||||
self._busy = False
|
||||
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/{self.device_id}/{device_id}/{action_name}"
|
||||
|
||||
# 检查动作客户端是否存在
|
||||
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, communication_device, read_method, write_method):
|
||||
"""为设备设置硬件接口代理"""
|
||||
extra_info = [getattr(device, info) for info in communication_device._hardware_interface.get("extra_info", [])]
|
||||
write_func = getattr(communication_device, communication_device._hardware_interface["write"])
|
||||
read_func = getattr(communication_device, communication_device._hardware_interface["read"])
|
||||
|
||||
def _read():
|
||||
return read_func(*extra_info)
|
||||
|
||||
def _write(command):
|
||||
return write_func(*extra_info, command)
|
||||
|
||||
if read_method:
|
||||
setattr(device, read_method, _read)
|
||||
if write_method:
|
||||
setattr(device, 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}")
|
||||
Reference in New Issue
Block a user