mirror of
https://github.com/dptech-corp/Uni-Lab-OS.git
synced 2026-02-06 23:15: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
672
unilabos/ros/nodes/base_device_node.py
Normal file
672
unilabos/ros/nodes/base_device_node.py
Normal file
@@ -0,0 +1,672 @@
|
||||
import threading
|
||||
import time
|
||||
import traceback
|
||||
import uuid
|
||||
from typing import get_type_hints, TypeVar, Generic, Dict, Any, Type, TypedDict, Optional
|
||||
|
||||
from concurrent.futures import ThreadPoolExecutor
|
||||
import asyncio
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.action import ActionServer
|
||||
from rclpy.action.server import ServerGoalHandle
|
||||
from rclpy.client import Client
|
||||
from rclpy.callback_groups import ReentrantCallbackGroup
|
||||
|
||||
from unilabos.resources.graphio import convert_resources_to_type, convert_resources_from_type
|
||||
from unilabos.ros.msgs.message_converter import (
|
||||
convert_to_ros_msg,
|
||||
convert_from_ros_msg,
|
||||
convert_from_ros_msg_with_mapping,
|
||||
convert_to_ros_msg_with_mapping,
|
||||
)
|
||||
from unilabos_msgs.srv import ResourceAdd, ResourceGet, ResourceDelete, ResourceUpdate, ResourceList # type: ignore
|
||||
from unilabos_msgs.msg import Resource # type: ignore
|
||||
|
||||
from unilabos.ros.nodes.resource_tracker import DeviceNodeResourceTracker
|
||||
from unilabos.ros.x.rclpyx import get_event_loop
|
||||
from unilabos.ros.utils.driver_creator import ProtocolNodeCreator, PyLabRobotCreator, DeviceClassCreator
|
||||
from unilabos.utils.async_util import run_async_func
|
||||
from unilabos.utils.log import info, debug, warning, error, critical, logger
|
||||
from unilabos.utils.type_check import get_type_class
|
||||
|
||||
T = TypeVar("T")
|
||||
|
||||
|
||||
# 在线设备注册表
|
||||
registered_devices: Dict[str, "DeviceInfoType"] = {}
|
||||
|
||||
|
||||
# 实现同时记录自定义日志和ROS2日志的适配器
|
||||
class ROSLoggerAdapter:
|
||||
"""同时向自定义日志和ROS2日志发送消息的适配器"""
|
||||
|
||||
@property
|
||||
def identifier(self):
|
||||
return f"{self.namespace}/{self.node_name}"
|
||||
|
||||
def __init__(self, ros_logger, node_name, namespace):
|
||||
"""
|
||||
初始化日志适配器
|
||||
|
||||
Args:
|
||||
ros_logger: ROS2日志记录器
|
||||
node_name: 节点名称
|
||||
namespace: 命名空间
|
||||
"""
|
||||
self.ros_logger = ros_logger
|
||||
self.node_name = node_name
|
||||
self.namespace = namespace
|
||||
self.level_2_logger_func = {
|
||||
"info": info,
|
||||
"debug": debug,
|
||||
"warning": warning,
|
||||
"error": error,
|
||||
"critical": critical,
|
||||
}
|
||||
|
||||
def _log(self, level, msg, *args, **kwargs):
|
||||
"""实际执行日志记录的内部方法"""
|
||||
# 添加前缀，使日志更易识别
|
||||
msg = f"[{self.identifier}] {msg}"
|
||||
# 向ROS2日志发送消息（标准库logging不支持stack_level参数）
|
||||
ros_log_func = getattr(self.ros_logger, "debug") # 默认发送debug，这样不会显示在控制台
|
||||
ros_log_func(msg)
|
||||
self.level_2_logger_func[level](msg, *args, stack_level=1, **kwargs)
|
||||
|
||||
def debug(self, msg, *args, **kwargs):
|
||||
"""记录DEBUG级别日志"""
|
||||
self._log("debug", msg, *args, **kwargs)
|
||||
|
||||
def info(self, msg, *args, **kwargs):
|
||||
"""记录INFO级别日志"""
|
||||
self._log("info", msg, *args, **kwargs)
|
||||
|
||||
def warning(self, msg, *args, **kwargs):
|
||||
"""记录WARNING级别日志"""
|
||||
self._log("warning", msg, *args, **kwargs)
|
||||
|
||||
def error(self, msg, *args, **kwargs):
|
||||
"""记录ERROR级别日志"""
|
||||
self._log("error", msg, *args, **kwargs)
|
||||
|
||||
def critical(self, msg, *args, **kwargs):
|
||||
"""记录CRITICAL级别日志"""
|
||||
self._log("critical", msg, *args, **kwargs)
|
||||
|
||||
|
||||
def init_wrapper(
|
||||
self,
|
||||
device_id: str,
|
||||
driver_class: type[T],
|
||||
status_types: Dict[str, Any],
|
||||
action_value_mappings: Dict[str, Any],
|
||||
hardware_interface: Dict[str, Any],
|
||||
print_publish: bool,
|
||||
children: Optional[list] = None,
|
||||
driver_params: Optional[Dict[str, Any]] = None,
|
||||
driver_is_ros: bool = False,
|
||||
*args,
|
||||
**kwargs,
|
||||
):
|
||||
"""初始化设备节点的包装函数，和ROS2DeviceNode初始化保持一致"""
|
||||
if driver_params is None:
|
||||
driver_params = kwargs.copy()
|
||||
if children is None:
|
||||
children = []
|
||||
kwargs["device_id"] = device_id
|
||||
kwargs["driver_class"] = driver_class
|
||||
kwargs["driver_params"] = driver_params
|
||||
kwargs["status_types"] = status_types
|
||||
kwargs["action_value_mappings"] = action_value_mappings
|
||||
kwargs["hardware_interface"] = hardware_interface
|
||||
kwargs["children"] = children
|
||||
kwargs["print_publish"] = print_publish
|
||||
kwargs["driver_is_ros"] = driver_is_ros
|
||||
super(type(self), self).__init__(*args, **kwargs)
|
||||
|
||||
|
||||
class PropertyPublisher:
|
||||
def __init__(
|
||||
self,
|
||||
node: "BaseROS2DeviceNode",
|
||||
name: str,
|
||||
get_method,
|
||||
msg_type,
|
||||
initial_period: float = 5.0,
|
||||
print_publish=True,
|
||||
):
|
||||
self.node = node
|
||||
self.name = name
|
||||
self.msg_type = msg_type
|
||||
self.get_method = get_method
|
||||
self.timer_period = initial_period
|
||||
self.print_publish = print_publish
|
||||
|
||||
self._value = None
|
||||
self.publisher_ = node.create_publisher(msg_type, f"{name}", 10)
|
||||
self.timer = node.create_timer(self.timer_period, self.publish_property)
|
||||
self.__loop = get_event_loop()
|
||||
str_msg_type = str(msg_type)[8:-2]
|
||||
self.node.lab_logger().debug(f"发布属性: {name}, 类型: {str_msg_type}, 周期: {initial_period}秒")
|
||||
|
||||
def get_property(self):
|
||||
if asyncio.iscoroutinefunction(self.get_method):
|
||||
# 如果是异步函数，运行事件循环并等待结果
|
||||
self.node.get_logger().debug(f"【PropertyPublisher.get_property】获取异步属性: {self.name}")
|
||||
loop = self.__loop
|
||||
if loop:
|
||||
future = asyncio.run_coroutine_threadsafe(self.get_method(), loop)
|
||||
self._value = future.result()
|
||||
return self._value
|
||||
else:
|
||||
self.node.get_logger().error(f"【PropertyPublisher.get_property】事件循环未初始化")
|
||||
return None
|
||||
else:
|
||||
# 如果是同步函数，直接调用并返回结果
|
||||
self.node.get_logger().debug(f"【PropertyPublisher.get_property】获取同步属性: {self.name}")
|
||||
self._value = self.get_method()
|
||||
return self._value
|
||||
|
||||
async def get_property_async(self):
|
||||
try:
|
||||
# 获取异步属性值
|
||||
self.node.get_logger().debug(f"【PropertyPublisher.get_property_async】异步获取属性: {self.name}")
|
||||
self._value = await self.get_method()
|
||||
except Exception as e:
|
||||
self.node.get_logger().error(f"【PropertyPublisher.get_property_async】获取异步属性出错: {str(e)}")
|
||||
|
||||
def publish_property(self):
|
||||
try:
|
||||
self.node.get_logger().debug(f"【PropertyPublisher.publish_property】开始发布属性: {self.name}")
|
||||
value = self.get_property()
|
||||
if self.print_publish:
|
||||
self.node.get_logger().info(f"【PropertyPublisher.publish_property】发布 {self.msg_type}: {value}")
|
||||
if value is not None:
|
||||
msg = convert_to_ros_msg(self.msg_type, value)
|
||||
self.publisher_.publish(msg)
|
||||
self.node.get_logger().debug(f"【PropertyPublisher.publish_property】属性 {self.name} 发布成功")
|
||||
except Exception as e:
|
||||
traceback.print_exc()
|
||||
self.node.get_logger().error(f"【PropertyPublisher.publish_property】发布属性出错: {str(e)}")
|
||||
|
||||
def change_frequency(self, period):
|
||||
# 动态改变定时器频率
|
||||
self.timer_period = period
|
||||
self.node.get_logger().info(
|
||||
f"【PropertyPublisher.change_frequency】修改 {self.name} 定时器周期为: {self.timer_period} 秒"
|
||||
)
|
||||
|
||||
# 重置定时器
|
||||
self.timer.cancel()
|
||||
self.timer = self.node.create_timer(self.timer_period, self.publish_property)
|
||||
|
||||
|
||||
class BaseROS2DeviceNode(Node, Generic[T]):
|
||||
"""
|
||||
ROS2设备节点基类
|
||||
|
||||
这个类提供了ROS2设备节点的基本功能，包括属性发布、动作服务等。
|
||||
通过泛型参数T来指定具体的设备类型。
|
||||
"""
|
||||
|
||||
@property
|
||||
def identifier(self):
|
||||
return f"{self.namespace}/{self.device_id}"
|
||||
|
||||
node_name: str
|
||||
namespace: str
|
||||
# TODO 要删除，添加时间相关的属性，避免动态添加属性的警告
|
||||
time_spent = 0.0
|
||||
time_remaining = 0.0
|
||||
create_action_server = True
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
driver_instance: T,
|
||||
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,
|
||||
):
|
||||
"""
|
||||
初始化ROS2设备节点
|
||||
|
||||
Args:
|
||||
driver_instance: 设备实例
|
||||
device_id: 设备标识符
|
||||
status_types: 需要发布的状态和传感器信息
|
||||
action_value_mappings: 设备动作
|
||||
hardware_interface: 硬件接口配置
|
||||
print_publish: 是否打印发布信息
|
||||
"""
|
||||
self.driver_instance = driver_instance
|
||||
self.device_id = device_id
|
||||
self.uuid = str(uuid.uuid4())
|
||||
self.publish_high_frequency = False
|
||||
self.callback_group = ReentrantCallbackGroup()
|
||||
self.resource_tracker = resource_tracker
|
||||
|
||||
# 初始化ROS节点
|
||||
self.node_name = f'{device_id.split("/")[-1]}'
|
||||
self.namespace = f"/devices/{device_id}"
|
||||
Node.__init__(self, self.node_name, namespace=self.namespace) # type: ignore
|
||||
if self.resource_tracker is None:
|
||||
self.lab_logger().critical("资源跟踪器未初始化，请检查")
|
||||
|
||||
# 创建自定义日志记录器
|
||||
self._lab_logger = ROSLoggerAdapter(self.get_logger(), self.node_name, self.namespace)
|
||||
|
||||
self._action_servers = {}
|
||||
self._property_publishers = {}
|
||||
self._status_types = status_types
|
||||
self._action_value_mappings = action_value_mappings
|
||||
self._hardware_interface = hardware_interface
|
||||
self._print_publish = print_publish
|
||||
|
||||
# 创建属性发布者
|
||||
for attr_name, msg_type in self._status_types.items():
|
||||
if isinstance(attr_name, (int, float)):
|
||||
if "param" in msg_type.keys():
|
||||
pass
|
||||
else:
|
||||
for k, v in msg_type.items():
|
||||
self.create_ros_publisher(k, v, initial_period=5.0)
|
||||
else:
|
||||
self.create_ros_publisher(attr_name, msg_type)
|
||||
|
||||
# 创建动作服务
|
||||
if self.create_action_server:
|
||||
for action_name, action_value_mapping in self._action_value_mappings.items():
|
||||
self.create_ros_action_server(action_name, action_value_mapping)
|
||||
|
||||
# 创建线程池执行器
|
||||
self._executor = ThreadPoolExecutor(max_workers=max(len(action_value_mappings), 1))
|
||||
|
||||
# 创建资源管理客户端
|
||||
self._resource_clients: Dict[str, Client] = {
|
||||
"resource_add": self.create_client(ResourceAdd, "/resources/add"),
|
||||
"resource_get": self.create_client(ResourceGet, "/resources/get"),
|
||||
"resource_delete": self.create_client(ResourceDelete, "/resources/delete"),
|
||||
"resource_update": self.create_client(ResourceUpdate, "/resources/update"),
|
||||
"resource_list": self.create_client(ResourceList, "/resources/list"),
|
||||
}
|
||||
|
||||
# 向全局在线设备注册表添加设备信息
|
||||
self.register_device()
|
||||
rclpy.get_global_executor().add_node(self)
|
||||
self.lab_logger().debug(f"ROS节点初始化完成")
|
||||
|
||||
def register_device(self):
|
||||
"""向注册表中注册设备信息"""
|
||||
topics_info = self._property_publishers.copy()
|
||||
actions_info = self._action_servers.copy()
|
||||
# 创建设备信息
|
||||
device_info = DeviceInfoType(
|
||||
id=self.device_id,
|
||||
uuid=self.uuid,
|
||||
node_name=self.node_name,
|
||||
namespace=self.namespace,
|
||||
driver_instance=self.driver_instance,
|
||||
status_publishers=topics_info,
|
||||
actions=actions_info,
|
||||
hardware_interface=self._hardware_interface,
|
||||
base_node_instance=self,
|
||||
)
|
||||
# 加入全局注册表
|
||||
registered_devices[self.device_id] = device_info
|
||||
|
||||
def lab_logger(self):
|
||||
"""
|
||||
获取实验室自定义日志记录器
|
||||
|
||||
这个日志记录器会同时向ROS2日志和自定义日志发送消息，
|
||||
并使用node_name和namespace作为标识。
|
||||
|
||||
Returns:
|
||||
日志记录器实例
|
||||
"""
|
||||
return self._lab_logger
|
||||
|
||||
def create_ros_publisher(self, attr_name, msg_type, initial_period=5.0):
|
||||
"""创建ROS发布者"""
|
||||
|
||||
# 获取属性值的方法
|
||||
def get_device_attr():
|
||||
try:
|
||||
if hasattr(self.driver_instance, f"get_{attr_name}"):
|
||||
return getattr(self.driver_instance, f"get_{attr_name}")()
|
||||
else:
|
||||
return getattr(self.driver_instance, attr_name)
|
||||
except AttributeError as ex:
|
||||
self.lab_logger().error(
|
||||
f"publish error, {str(type(self.driver_instance))[8:-2]} has no attribute '{attr_name}'"
|
||||
)
|
||||
|
||||
self._property_publishers[attr_name] = PropertyPublisher(
|
||||
self, attr_name, get_device_attr, msg_type, initial_period, self._print_publish
|
||||
)
|
||||
|
||||
def create_ros_action_server(self, action_name, action_value_mapping):
|
||||
"""创建ROS动作服务器"""
|
||||
action_type = action_value_mapping["type"]
|
||||
str_action_type = str(action_type)[8:-2]
|
||||
|
||||
self._action_servers[action_name] = ActionServer(
|
||||
self,
|
||||
action_type,
|
||||
action_name,
|
||||
execute_callback=self._create_execute_callback(action_name, action_value_mapping),
|
||||
callback_group=ReentrantCallbackGroup(),
|
||||
)
|
||||
|
||||
self.lab_logger().debug(f"发布动作: {action_name}, 类型: {str_action_type}")
|
||||
|
||||
def _create_execute_callback(self, action_name, action_value_mapping):
|
||||
"""创建动作执行回调函数"""
|
||||
|
||||
async def execute_callback(goal_handle: ServerGoalHandle):
|
||||
self.lab_logger().info(f"执行动作: {action_name}")
|
||||
goal = goal_handle.request
|
||||
|
||||
# 从目标消息中提取参数, 并调用对应的方法
|
||||
if "sequence" in self._action_value_mappings:
|
||||
# 如果一个指令对应函数的连续调用，如启动和等待结果，默认参数应该属于第一个函数调用
|
||||
def ACTION(**kwargs):
|
||||
for i, action in enumerate(self._action_value_mappings["sequence"]):
|
||||
if i == 0:
|
||||
self.lab_logger().info(f"执行序列动作第一步: {action}")
|
||||
getattr(self.driver_instance, action)(**kwargs)
|
||||
else:
|
||||
self.lab_logger().info(f"执行序列动作后续步骤: {action}")
|
||||
getattr(self.driver_instance, action)()
|
||||
|
||||
action_paramtypes = get_type_hints(
|
||||
getattr(self.driver_instance, self._action_value_mappings["sequence"][0])
|
||||
)
|
||||
else:
|
||||
ACTION = getattr(self.driver_instance, action_name)
|
||||
action_paramtypes = get_type_hints(ACTION)
|
||||
|
||||
action_kwargs = convert_from_ros_msg_with_mapping(goal, action_value_mapping["goal"])
|
||||
self.lab_logger().debug(f"接收到原始目标: {action_kwargs}")
|
||||
|
||||
# 向Host查询物料当前状态
|
||||
for k, v in goal.get_fields_and_field_types().items():
|
||||
if v in ["unilabos_msgs/Resource", "sequence<unilabos_msgs/Resource>"]:
|
||||
self.lab_logger().info(f"查询资源状态: Key: {k} Type: {v}")
|
||||
try:
|
||||
r = ResourceGet.Request()
|
||||
r.id = action_kwargs[k]["id"] if v == "unilabos_msgs/Resource" else action_kwargs[k][0]["id"]
|
||||
r.with_children = True
|
||||
response = await self._resource_clients["resource_get"].call_async(r)
|
||||
except Exception:
|
||||
logger.error(f"资源查询失败，默认使用本地资源")
|
||||
# 删除对response.resources的检查，因为它总是存在
|
||||
resources_list = [convert_from_ros_msg(rs) for rs in response.resources] # type: ignore # FIXME
|
||||
self.lab_logger().debug(f"资源查询结果: {len(resources_list)} 个资源")
|
||||
type_hint = action_paramtypes[k]
|
||||
final_type = get_type_class(type_hint)
|
||||
# 判断 ACTION 是否需要特殊的物料类型如 pylabrobot.resources.Resource，并做转换
|
||||
final_resource = convert_resources_to_type(resources_list, final_type)
|
||||
action_kwargs[k] = self.resource_tracker.figure_resource(final_resource)
|
||||
|
||||
self.lab_logger().info(f"准备执行: {action_kwargs}, 函数: {ACTION.__name__}")
|
||||
time_start = time.time()
|
||||
time_overall = 100
|
||||
|
||||
# 将阻塞操作放入线程池执行
|
||||
if asyncio.iscoroutinefunction(ACTION):
|
||||
try:
|
||||
self.lab_logger().info(f"异步执行动作 {ACTION}")
|
||||
future = ROS2DeviceNode.run_async_func(ACTION, **action_kwargs)
|
||||
except Exception as e:
|
||||
self.lab_logger().error(f"创建异步任务失败: {traceback.format_exc()}")
|
||||
raise e
|
||||
else:
|
||||
self.lab_logger().info(f"同步执行动作 {ACTION}")
|
||||
future = self._executor.submit(ACTION, **action_kwargs)
|
||||
|
||||
action_type = action_value_mapping["type"]
|
||||
feedback_msg_types = action_type.Feedback.get_fields_and_field_types()
|
||||
result_msg_types = action_type.Result.get_fields_and_field_types()
|
||||
|
||||
while not future.done():
|
||||
if goal_handle.is_cancel_requested:
|
||||
self.lab_logger().info(f"取消动作: {action_name}")
|
||||
future.cancel() # 尝试取消线程池中的任务
|
||||
goal_handle.canceled()
|
||||
return action_type.Result()
|
||||
|
||||
self.time_spent = time.time() - time_start
|
||||
self.time_remaining = time_overall - self.time_spent
|
||||
|
||||
# 发布反馈
|
||||
feedback_values = {}
|
||||
for msg_name, attr_name in action_value_mapping["feedback"].items():
|
||||
if hasattr(self.driver_instance, f"get_{attr_name}"):
|
||||
method = getattr(self.driver_instance, f"get_{attr_name}")
|
||||
if not asyncio.iscoroutinefunction(method):
|
||||
feedback_values[msg_name] = method()
|
||||
elif hasattr(self.driver_instance, attr_name):
|
||||
feedback_values[msg_name] = getattr(self.driver_instance, attr_name)
|
||||
|
||||
if self._print_publish:
|
||||
self.lab_logger().info(f"反馈: {feedback_values}")
|
||||
|
||||
feedback_msg = convert_to_ros_msg_with_mapping(
|
||||
ros_msg_type=action_type.Feedback(),
|
||||
obj=feedback_values,
|
||||
value_mapping=action_value_mapping["feedback"],
|
||||
)
|
||||
|
||||
goal_handle.publish_feedback(feedback_msg)
|
||||
time.sleep(0.5)
|
||||
|
||||
if future.cancelled():
|
||||
self.lab_logger().info(f"动作 {action_name} 已取消")
|
||||
return action_type.Result()
|
||||
|
||||
self.lab_logger().info(f"动作执行完成: {action_name}")
|
||||
del future
|
||||
|
||||
# 向Host更新物料当前状态
|
||||
for k, v in goal.get_fields_and_field_types().items():
|
||||
if v not in ["unilabos_msgs/Resource", "sequence<unilabos_msgs/Resource>"]:
|
||||
continue
|
||||
self.lab_logger().info(f"更新资源状态: {k}")
|
||||
r = ResourceUpdate.Request()
|
||||
# 仅当action_kwargs[k]不为None时尝试转换
|
||||
akv = action_kwargs[k]
|
||||
apv = action_paramtypes[k]
|
||||
final_type = get_type_class(apv)
|
||||
if final_type is None:
|
||||
continue
|
||||
try:
|
||||
r.resources = [
|
||||
convert_to_ros_msg(Resource, self.resource_tracker.root_resource(rs))
|
||||
for rs in convert_resources_from_type(akv, final_type) # type: ignore # FIXME # 考虑反查到最大的
|
||||
]
|
||||
response = await self._resource_clients["resource_update"].call_async(r)
|
||||
self.lab_logger().debug(f"资源更新结果: {response}")
|
||||
except Exception as e:
|
||||
self.lab_logger().error(f"资源更新失败: {e}")
|
||||
self.lab_logger().error(traceback.format_exc())
|
||||
|
||||
# 发布结果
|
||||
goal_handle.succeed()
|
||||
self.lab_logger().info(f"设置动作成功: {action_name}")
|
||||
|
||||
result_values = {}
|
||||
for msg_name, attr_name in action_value_mapping["result"].items():
|
||||
if hasattr(self.driver_instance, f"get_{attr_name}"):
|
||||
result_values[msg_name] = getattr(self.driver_instance, f"get_{attr_name}")()
|
||||
elif hasattr(self.driver_instance, attr_name):
|
||||
result_values[msg_name] = getattr(self.driver_instance, attr_name)
|
||||
|
||||
result_msg = convert_to_ros_msg_with_mapping(
|
||||
ros_msg_type=action_type.Result(), obj=result_values, value_mapping=action_value_mapping["result"]
|
||||
)
|
||||
|
||||
for attr_name in result_msg_types.keys():
|
||||
if attr_name in ["success", "reached_goal"]:
|
||||
setattr(result_msg, attr_name, True)
|
||||
|
||||
self.lab_logger().info(f"动作 {action_name} 完成并返回结果")
|
||||
return result_msg
|
||||
|
||||
return execute_callback
|
||||
|
||||
# 异步上下文管理方法
|
||||
async def __aenter__(self):
|
||||
"""进入异步上下文"""
|
||||
self.lab_logger().info(f"进入异步上下文: {self.device_id}")
|
||||
if hasattr(self.driver_instance, "__aenter__"):
|
||||
await self.driver_instance.__aenter__() # type: ignore
|
||||
self.lab_logger().info(f"异步上下文初始化完成: {self.device_id}")
|
||||
return self
|
||||
|
||||
async def __aexit__(self, exc_type, exc_val, exc_tb):
|
||||
"""退出异步上下文"""
|
||||
self.lab_logger().info(f"退出异步上下文: {self.device_id}")
|
||||
if hasattr(self.driver_instance, "__aexit__"):
|
||||
await self.driver_instance.__aexit__(exc_type, exc_val, exc_tb) # type: ignore
|
||||
self.lab_logger().info(f"异步上下文清理完成: {self.device_id}")
|
||||
|
||||
|
||||
class DeviceInitError(Exception):
|
||||
pass
|
||||
|
||||
|
||||
class ROS2DeviceNode:
|
||||
"""
|
||||
ROS2设备节点类
|
||||
|
||||
这个类封装了设备类实例和ROS2节点的功能，提供ROS2接口。
|
||||
它不继承设备类，而是通过代理模式访问设备类的属性和方法。
|
||||
"""
|
||||
|
||||
# 类变量，用于循环管理
|
||||
_loop = None
|
||||
_loop_running = False
|
||||
_loop_thread = None
|
||||
|
||||
@classmethod
|
||||
def get_loop(cls):
|
||||
return cls._loop
|
||||
|
||||
@classmethod
|
||||
def run_async_func(cls, func, **kwargs):
|
||||
return run_async_func(func, loop=cls._loop, **kwargs)
|
||||
|
||||
@property
|
||||
def driver_instance(self):
|
||||
return self._driver_instance
|
||||
|
||||
@property
|
||||
def ros_node_instance(self):
|
||||
return self._ros_node
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
device_id: str,
|
||||
driver_class: Type[T],
|
||||
driver_params: Dict[str, Any],
|
||||
status_types: Dict[str, Any],
|
||||
action_value_mappings: Dict[str, Any],
|
||||
hardware_interface: Dict[str, Any],
|
||||
children: Dict[str, Any],
|
||||
print_publish: bool = True,
|
||||
driver_is_ros: bool = False,
|
||||
):
|
||||
"""
|
||||
初始化ROS2设备节点
|
||||
|
||||
Args:
|
||||
device_id: 设备标识符
|
||||
driver_class: 设备类
|
||||
status_types: 状态类型映射
|
||||
action_value_mappings: 动作值映射
|
||||
hardware_interface: 硬件接口配置
|
||||
children:
|
||||
print_publish: 是否打印发布信息
|
||||
driver_is_ros:
|
||||
"""
|
||||
# 在初始化时检查循环状态
|
||||
if ROS2DeviceNode._loop_running and ROS2DeviceNode._loop_thread is not None:
|
||||
pass
|
||||
elif ROS2DeviceNode._loop_thread is None:
|
||||
self._start_loop()
|
||||
|
||||
# 保存设备类是否支持异步上下文
|
||||
self._has_async_context = hasattr(driver_class, "__aenter__") and hasattr(driver_class, "__aexit__")
|
||||
self._driver_class = driver_class
|
||||
self.driver_is_ros = driver_is_ros
|
||||
self.resource_tracker = DeviceNodeResourceTracker()
|
||||
|
||||
# use_pylabrobot_creator 使用 cls的包路径检测
|
||||
use_pylabrobot_creator = driver_class.__module__.startswith("pylabrobot")
|
||||
|
||||
# TODO: 要在创建之前预先请求服务器是否有当前id的物料，放到resource_tracker中，让pylabrobot进行创建
|
||||
# 创建设备类实例
|
||||
if use_pylabrobot_creator:
|
||||
self._driver_creator = PyLabRobotCreator(
|
||||
driver_class, children=children, resource_tracker=self.resource_tracker
|
||||
)
|
||||
else:
|
||||
from unilabos.ros.nodes.presets.protocol_node import ROS2ProtocolNode
|
||||
|
||||
if self._driver_class is ROS2ProtocolNode:
|
||||
self._driver_creator = ProtocolNodeCreator(driver_class, children=children)
|
||||
else:
|
||||
self._driver_creator = DeviceClassCreator(driver_class)
|
||||
|
||||
if driver_is_ros:
|
||||
driver_params["device_id"] = device_id
|
||||
driver_params["resource_tracker"] = self.resource_tracker
|
||||
self._driver_instance = self._driver_creator.create_instance(driver_params)
|
||||
if self._driver_instance is None:
|
||||
logger.critical(f"设备实例创建失败 {driver_class}, params: {driver_params}")
|
||||
raise DeviceInitError("错误: 设备实例创建失败")
|
||||
|
||||
# 创建ROS2节点
|
||||
if driver_is_ros:
|
||||
self._ros_node = self._driver_instance # type: ignore
|
||||
else:
|
||||
self._ros_node = BaseROS2DeviceNode(
|
||||
driver_instance=self._driver_instance,
|
||||
device_id=device_id,
|
||||
status_types=status_types,
|
||||
action_value_mappings=action_value_mappings,
|
||||
hardware_interface=hardware_interface,
|
||||
print_publish=print_publish,
|
||||
resource_tracker=self.resource_tracker,
|
||||
)
|
||||
self._ros_node: BaseROS2DeviceNode
|
||||
self._ros_node.lab_logger().info(f"初始化完成 {self._ros_node.uuid} {self.driver_is_ros}")
|
||||
|
||||
def _start_loop(self):
|
||||
def run_event_loop():
|
||||
loop = asyncio.new_event_loop()
|
||||
ROS2DeviceNode._loop = loop
|
||||
asyncio.set_event_loop(loop)
|
||||
loop.run_forever()
|
||||
|
||||
ROS2DeviceNode._loop_thread = threading.Thread(target=run_event_loop, daemon=True, name="ROS2DeviceNode")
|
||||
ROS2DeviceNode._loop_thread.start()
|
||||
logger.info(f"循环线程已启动")
|
||||
|
||||
|
||||
class DeviceInfoType(TypedDict):
|
||||
id: str
|
||||
uuid: str
|
||||
node_name: str
|
||||
namespace: str
|
||||
driver_instance: Any
|
||||
status_publishers: Dict[str, PropertyPublisher]
|
||||
actions: Dict[str, ActionServer]
|
||||
hardware_interface: Dict[str, Any]
|
||||
base_node_instance: BaseROS2DeviceNode
|
||||
Reference in New Issue
Block a user