mirror of
https://github.com/dptech-corp/Uni-Lab-OS.git
synced 2025-12-15 13:44:39 +00:00
1706 lines
76 KiB
Python
1706 lines
76 KiB
Python
import copy
|
||
import inspect
|
||
import io
|
||
import json
|
||
import threading
|
||
import time
|
||
import traceback
|
||
from typing import get_type_hints, TypeVar, Generic, Dict, Any, Type, TypedDict, Optional, List, TYPE_CHECKING, Union
|
||
|
||
from concurrent.futures import ThreadPoolExecutor
|
||
import asyncio
|
||
|
||
import rclpy
|
||
import yaml
|
||
from msgcenterpy import ROS2MessageInstance
|
||
from rclpy.node import Node
|
||
from rclpy.action import ActionServer, ActionClient
|
||
from rclpy.action.server import ServerGoalHandle
|
||
from rclpy.client import Client
|
||
from rclpy.callback_groups import ReentrantCallbackGroup
|
||
from rclpy.service import Service
|
||
from unilabos_msgs.action import SendCmd
|
||
from unilabos_msgs.srv._serial_command import SerialCommand_Request, SerialCommand_Response
|
||
|
||
from unilabos.resources.container import RegularContainer
|
||
from unilabos.resources.graphio import (
|
||
resource_ulab_to_plr,
|
||
initialize_resources,
|
||
dict_to_tree,
|
||
resource_plr_to_ulab,
|
||
tree_to_list,
|
||
)
|
||
from unilabos.resources.plr_additional_res_reg import register
|
||
from unilabos.ros.msgs.message_converter import (
|
||
convert_to_ros_msg,
|
||
convert_from_ros_msg_with_mapping,
|
||
convert_to_ros_msg_with_mapping,
|
||
)
|
||
from unilabos_msgs.srv import (
|
||
ResourceAdd,
|
||
ResourceDelete,
|
||
ResourceUpdate,
|
||
ResourceList,
|
||
SerialCommand,
|
||
) # type: ignore
|
||
from unilabos_msgs.msg import Resource # type: ignore
|
||
|
||
from unilabos.ros.nodes.resource_tracker import (
|
||
DeviceNodeResourceTracker,
|
||
ResourceTreeSet,
|
||
ResourceTreeInstance, ResourceDictInstance,
|
||
)
|
||
from unilabos.ros.x.rclpyx import get_event_loop
|
||
from unilabos.ros.utils.driver_creator import WorkstationNodeCreator, PyLabRobotCreator, DeviceClassCreator
|
||
from rclpy.task import Task, Future
|
||
from unilabos.utils.import_manager import default_manager
|
||
from unilabos.utils.log import info, debug, warning, error, critical, logger, trace
|
||
from unilabos.utils.type_check import get_type_class, TypeEncoder, get_result_info_str
|
||
|
||
if TYPE_CHECKING:
|
||
from pylabrobot.resources import Resource as ResourcePLR
|
||
|
||
T = TypeVar("T")
|
||
|
||
|
||
# 在线设备注册表
|
||
registered_devices: Dict[str, "DeviceInfoType"] = {}
|
||
|
||
|
||
# 实现同时记录自定义日志和ROS2日志的适配器
|
||
class ROSLoggerAdapter:
|
||
"""同时向自定义日志和ROS2日志发送消息的适配器"""
|
||
|
||
@property
|
||
def identifier(self):
|
||
return f"{self.namespace}"
|
||
|
||
def __init__(self, ros_logger, namespace):
|
||
"""
|
||
初始化日志适配器
|
||
|
||
Args:
|
||
ros_logger: ROS2日志记录器
|
||
namespace: 命名空间
|
||
"""
|
||
self.ros_logger = ros_logger
|
||
self.namespace = namespace
|
||
self.level_2_logger_func = {
|
||
"info": info,
|
||
"debug": debug,
|
||
"trace": trace,
|
||
"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 trace(self, msg, *args, **kwargs):
|
||
"""记录TRACE级别日志"""
|
||
self._log("trace", msg, *args, **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,
|
||
device_uuid: str,
|
||
driver_class: type[T],
|
||
device_config: ResourceTreeInstance,
|
||
status_types: Dict[str, Any],
|
||
action_value_mappings: Dict[str, Any],
|
||
hardware_interface: Dict[str, Any],
|
||
print_publish: bool,
|
||
driver_params: Optional[Dict[str, Any]] = None,
|
||
driver_is_ros: bool = False,
|
||
*args,
|
||
**kwargs,
|
||
):
|
||
"""初始化设备节点的包装函数，和ROS2DeviceNode初始化保持一致"""
|
||
if driver_params is None:
|
||
driver_params = kwargs.copy()
|
||
kwargs["device_id"] = device_id
|
||
kwargs["device_uuid"] = device_uuid
|
||
kwargs["driver_class"] = driver_class
|
||
kwargs["device_config"] = device_config
|
||
kwargs["driver_params"] = driver_params
|
||
kwargs["status_types"] = status_types
|
||
kwargs["action_value_mappings"] = action_value_mappings
|
||
kwargs["hardware_interface"] = hardware_interface
|
||
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
|
||
try:
|
||
self.publisher_ = node.create_publisher(msg_type, f"{name}", 10)
|
||
except AttributeError as ex:
|
||
self.node.lab_logger().error(
|
||
f"创建发布者 {name} 失败，可能由于注册表有误，类型: {msg_type}，错误: {ex}\n{traceback.format_exc()}"
|
||
)
|
||
self.timer = node.create_timer(self.timer_period, self.publish_property)
|
||
self.__loop = get_event_loop()
|
||
str_msg_type = str(msg_type)[8:-2]
|
||
self.node.lab_logger().trace(f"发布属性: {name}, 类型: {str_msg_type}, 周期: {initial_period}秒")
|
||
|
||
def get_property(self):
|
||
if asyncio.iscoroutinefunction(self.get_method):
|
||
# 如果是异步函数，运行事件循环并等待结果
|
||
self.node.lab_logger().trace(f"【.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.lab_logger().error(f"【.get_property】事件循环未初始化")
|
||
return None
|
||
else:
|
||
# 如果是同步函数，直接调用并返回结果
|
||
self.node.lab_logger().trace(f"【.get_property】获取同步属性: {self.name}")
|
||
self._value = self.get_method()
|
||
return self._value
|
||
|
||
async def get_property_async(self):
|
||
try:
|
||
# 获取异步属性值
|
||
self.node.lab_logger().trace(f"【.get_property_async】异步获取属性: {self.name}")
|
||
self._value = await self.get_method()
|
||
except Exception as e:
|
||
self.node.lab_logger().error(f"【.get_property_async】获取异步属性出错: {str(e)}")
|
||
|
||
def publish_property(self):
|
||
try:
|
||
self.node.lab_logger().trace(f"【.publish_property】开始发布属性: {self.name}")
|
||
value = self.get_property()
|
||
if self.print_publish:
|
||
self.node.lab_logger().trace(f"【.publish_property】发布 {self.msg_type}: {value}")
|
||
if value is not None:
|
||
msg = convert_to_ros_msg(self.msg_type, value)
|
||
self.publisher_.publish(msg)
|
||
self.node.lab_logger().trace(f"【.publish_property】属性 {self.name} 发布成功")
|
||
except Exception as e:
|
||
self.node.lab_logger().error(
|
||
f"【.publish_property】发布属性 {self.publisher_.topic} 出错: {str(e)}\n{traceback.format_exc()}"
|
||
)
|
||
|
||
def change_frequency(self, period):
|
||
# 动态改变定时器频率
|
||
self.timer_period = period
|
||
self.node.get_logger().info(f"【.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
|
||
# 内部共享变量
|
||
_time_spent = 0.0
|
||
_time_remaining = 0.0
|
||
# 是否创建Action
|
||
create_action_server = True
|
||
|
||
def __init__(
|
||
self,
|
||
driver_instance: T,
|
||
device_id: str,
|
||
device_uuid: str,
|
||
status_types: Dict[str, Any],
|
||
action_value_mappings: Dict[str, Any],
|
||
hardware_interface: Dict[str, Any],
|
||
print_publish=True,
|
||
resource_tracker: "DeviceNodeResourceTracker" = None, # type: ignore
|
||
):
|
||
"""
|
||
初始化ROS2设备节点
|
||
|
||
Args:
|
||
driver_instance: 设备实例
|
||
device_id: 设备标识符
|
||
device_uuid: 设备标识符
|
||
status_types: 需要发布的状态和传感器信息
|
||
action_value_mappings: 设备动作
|
||
hardware_interface: 硬件接口配置
|
||
print_publish: 是否打印发布信息
|
||
"""
|
||
self.driver_instance = driver_instance
|
||
self.device_id = device_id
|
||
self.uuid = device_uuid
|
||
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.namespace)
|
||
|
||
self._action_servers: Dict[str, ActionServer] = {}
|
||
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():
|
||
if action_name.startswith("auto-") or str(action_value_mapping.get("type", "")).startswith(
|
||
"UniLabJsonCommand"
|
||
):
|
||
continue
|
||
self.create_ros_action_server(action_name, action_value_mapping)
|
||
|
||
# 创建线程池执行器
|
||
self._executor = ThreadPoolExecutor(
|
||
max_workers=max(len(action_value_mappings), 1), thread_name_prefix=f"ROSDevice{self.device_id}"
|
||
)
|
||
|
||
# 创建资源管理客户端
|
||
self._resource_clients: Dict[str, Client] = {
|
||
"resource_add": self.create_client(ResourceAdd, "/resources/add", callback_group=self.callback_group),
|
||
"resource_get": self.create_client(SerialCommand, "/resources/get", callback_group=self.callback_group),
|
||
"resource_delete": self.create_client(
|
||
ResourceDelete, "/resources/delete", callback_group=self.callback_group
|
||
),
|
||
"resource_update": self.create_client(
|
||
ResourceUpdate, "/resources/update", callback_group=self.callback_group
|
||
),
|
||
"resource_list": self.create_client(ResourceList, "/resources/list", callback_group=self.callback_group),
|
||
"c2s_update_resource_tree": self.create_client(
|
||
SerialCommand, "/c2s_update_resource_tree", callback_group=self.callback_group
|
||
),
|
||
}
|
||
|
||
def re_register_device(req, res):
|
||
self.register_device()
|
||
self.lab_logger().info("Host要求重新注册当前节点")
|
||
res.response = ""
|
||
return res
|
||
|
||
async def append_resource(req: SerialCommand_Request, res: SerialCommand_Response):
|
||
# 物料传输到对应的node节点
|
||
rclient = self.create_client(ResourceAdd, "/resources/add")
|
||
rclient.wait_for_service()
|
||
rclient2 = self.create_client(ResourceAdd, "/resources/add")
|
||
rclient2.wait_for_service()
|
||
request = ResourceAdd.Request()
|
||
request2 = ResourceAdd.Request()
|
||
command_json = json.loads(req.command)
|
||
namespace = command_json["namespace"]
|
||
bind_parent_id = command_json["bind_parent_id"]
|
||
edge_device_id = command_json["edge_device_id"]
|
||
location = command_json["bind_location"]
|
||
other_calling_param = command_json["other_calling_param"]
|
||
resources = command_json["resource"]
|
||
initialize_full = other_calling_param.pop("initialize_full", False)
|
||
# 用来增加液体
|
||
ADD_LIQUID_TYPE = other_calling_param.pop("ADD_LIQUID_TYPE", [])
|
||
LIQUID_VOLUME = other_calling_param.pop("LIQUID_VOLUME", [])
|
||
LIQUID_INPUT_SLOT = other_calling_param.pop("LIQUID_INPUT_SLOT", [])
|
||
slot = other_calling_param.pop("slot", "-1")
|
||
resource = None
|
||
if slot != "-1": # slot为负数的时候采用assign方法
|
||
other_calling_param["slot"] = slot
|
||
# 本地拿到这个物料，可能需要先做初始化?
|
||
if isinstance(resources, list):
|
||
if (
|
||
len(resources) == 1 and isinstance(resources[0], list) and not initialize_full
|
||
): # 取消，不存在的情况
|
||
# 预先initialize过，以整组的形式传入
|
||
request.resources = [convert_to_ros_msg(Resource, resource_) for resource_ in resources[0]]
|
||
elif initialize_full:
|
||
resources = initialize_resources(resources)
|
||
request.resources = [convert_to_ros_msg(Resource, resource) for resource in resources]
|
||
else:
|
||
request.resources = [convert_to_ros_msg(Resource, resource) for resource in resources]
|
||
else:
|
||
if initialize_full:
|
||
resources = initialize_resources([resources])
|
||
request.resources = [convert_to_ros_msg(Resource, resources)]
|
||
if len(LIQUID_INPUT_SLOT) and LIQUID_INPUT_SLOT[0] == -1:
|
||
container_instance = request.resources[0]
|
||
container_query_dict: dict = resources
|
||
found_resources = self.resource_tracker.figure_resource(
|
||
{"id": container_query_dict["name"]}, try_mode=True
|
||
)
|
||
if not len(found_resources):
|
||
self.resource_tracker.add_resource(container_instance)
|
||
logger.info(f"添加物料{container_query_dict['name']}到资源跟踪器")
|
||
else:
|
||
assert (
|
||
len(found_resources) == 1
|
||
), f"找到多个同名物料: {container_query_dict['name']}, 请检查物料系统"
|
||
resource = found_resources[0]
|
||
if isinstance(resource, Resource):
|
||
regular_container = RegularContainer(resource.id)
|
||
regular_container.ulr_resource = resource
|
||
regular_container.ulr_resource_data.update(json.loads(container_instance.data))
|
||
logger.info(f"更新物料{container_query_dict['name']}的数据{resource.data} ULR")
|
||
elif isinstance(resource, dict):
|
||
if "data" not in resource:
|
||
resource["data"] = {}
|
||
resource["data"].update(json.loads(container_instance.data))
|
||
request.resources[0].name = resource["name"]
|
||
logger.info(f"更新物料{container_query_dict['name']}的数据{resource['data']} dict")
|
||
else:
|
||
logger.info(
|
||
f"更新物料{container_query_dict['name']}出现不支持的数据类型{type(resource)} {resource}"
|
||
)
|
||
response: ResourceAdd.Response = await rclient.call_async(request)
|
||
# 应该先add_resource了
|
||
final_response = {
|
||
"created_resources": [ROS2MessageInstance(i).get_python_dict() for i in request.resources],
|
||
"liquid_input_resources": [],
|
||
}
|
||
res.response = json.dumps(final_response)
|
||
# 如果driver自己就有assign的方法，那就使用driver自己的assign方法
|
||
if hasattr(self.driver_instance, "create_resource"):
|
||
create_resource_func = getattr(self.driver_instance, "create_resource")
|
||
try:
|
||
ret = create_resource_func(
|
||
resource_tracker=self.resource_tracker,
|
||
resources=request.resources,
|
||
bind_parent_id=bind_parent_id,
|
||
bind_location=location,
|
||
liquid_input_slot=LIQUID_INPUT_SLOT,
|
||
liquid_type=ADD_LIQUID_TYPE,
|
||
liquid_volume=LIQUID_VOLUME,
|
||
slot_on_deck=slot,
|
||
)
|
||
res.response = get_result_info_str("", True, ret)
|
||
except Exception as e:
|
||
self.lab_logger().error(
|
||
f"运行设备的create_resource出错：{create_resource_func}\n{traceback.format_exc()}"
|
||
)
|
||
res.response = get_result_info_str(traceback.format_exc(), False, {})
|
||
return res
|
||
# 接下来该根据bind_parent_id进行assign了，目前只有plr可以进行assign，不然没有办法输入到物料系统中
|
||
if bind_parent_id != self.node_name:
|
||
resource = self.resource_tracker.figure_resource(
|
||
{"name": bind_parent_id}
|
||
) # 拿到父节点，进行具体assign等操作
|
||
# request.resources = [convert_to_ros_msg(Resource, resources)]
|
||
|
||
try:
|
||
from pylabrobot.resources.resource import Resource as ResourcePLR
|
||
from pylabrobot.resources.deck import Deck
|
||
from pylabrobot.resources import Coordinate
|
||
from pylabrobot.resources import OTDeck
|
||
from pylabrobot.resources import Plate
|
||
|
||
contain_model = not isinstance(resource, Deck)
|
||
if isinstance(resource, ResourcePLR):
|
||
# resources.list()
|
||
plr_instance = ResourceTreeSet.from_raw_list(resources).to_plr_resources()[0]
|
||
# resources_tree = dict_to_tree(copy.deepcopy({r["id"]: r for r in resources}))
|
||
# plr_instance = resource_ulab_to_plr(resources_tree[0], contain_model)
|
||
|
||
if isinstance(plr_instance, Plate):
|
||
empty_liquid_info_in = [(None, 0)] * plr_instance.num_items
|
||
for liquid_type, liquid_volume, liquid_input_slot in zip(
|
||
ADD_LIQUID_TYPE, LIQUID_VOLUME, LIQUID_INPUT_SLOT
|
||
):
|
||
empty_liquid_info_in[liquid_input_slot] = (liquid_type, liquid_volume)
|
||
plr_instance.set_well_liquids(empty_liquid_info_in)
|
||
input_wells_ulr = [
|
||
convert_to_ros_msg(
|
||
Resource,
|
||
resource_plr_to_ulab(plr_instance.get_well(LIQUID_INPUT_SLOT), with_children=False),
|
||
)
|
||
for r in LIQUID_INPUT_SLOT
|
||
]
|
||
final_response["liquid_input_resources"] = [
|
||
ROS2MessageInstance(i).get_python_dict() for i in input_wells_ulr
|
||
]
|
||
res.response = json.dumps(final_response)
|
||
if isinstance(resource, OTDeck) and "slot" in other_calling_param:
|
||
other_calling_param["slot"] = int(other_calling_param["slot"])
|
||
resource.assign_child_at_slot(plr_instance, **other_calling_param)
|
||
else:
|
||
_discard_slot = other_calling_param.pop("slot", "-1")
|
||
resource.assign_child_resource(
|
||
plr_instance,
|
||
Coordinate(location["x"], location["y"], location["z"]),
|
||
**other_calling_param,
|
||
)
|
||
request2.resources = [
|
||
convert_to_ros_msg(Resource, r) for r in tree_to_list([resource_plr_to_ulab(resource)])
|
||
]
|
||
rclient2.call(request2)
|
||
# 发送给ResourceMeshManager
|
||
action_client = ActionClient(
|
||
self,
|
||
SendCmd,
|
||
"/devices/resource_mesh_manager/add_resource_mesh",
|
||
callback_group=self.callback_group,
|
||
)
|
||
goal = SendCmd.Goal()
|
||
goal.command = json.dumps(
|
||
{
|
||
"resources": resources,
|
||
"bind_parent_id": bind_parent_id,
|
||
}
|
||
)
|
||
future = action_client.send_goal_async(goal)
|
||
|
||
def done_cb(*args):
|
||
self.lab_logger().info(f"向meshmanager发送新增resource完成")
|
||
|
||
future.add_done_callback(done_cb)
|
||
except ImportError:
|
||
self.lab_logger().error("Host请求添加物料时，本环境并不存在pylabrobot")
|
||
except Exception as e:
|
||
self.lab_logger().error("Host请求添加物料时出错")
|
||
self.lab_logger().error(traceback.format_exc())
|
||
return res
|
||
|
||
# noinspection PyTypeChecker
|
||
self._service_server: Dict[str, Service] = {
|
||
"re_register_device": self.create_service(
|
||
SerialCommand,
|
||
f"/srv{self.namespace}/re_register_device",
|
||
re_register_device,
|
||
callback_group=self.callback_group,
|
||
),
|
||
"append_resource": self.create_service(
|
||
SerialCommand,
|
||
f"/srv{self.namespace}/append_resource",
|
||
append_resource, # type: ignore
|
||
callback_group=self.callback_group,
|
||
),
|
||
"s2c_resource_tree": self.create_service(
|
||
SerialCommand,
|
||
f"/srv{self.namespace}/s2c_resource_tree",
|
||
self.s2c_resource_tree, # type: ignore
|
||
callback_group=self.callback_group,
|
||
),
|
||
}
|
||
|
||
# 向全局在线设备注册表添加设备信息
|
||
self.register_device()
|
||
rclpy.get_global_executor().add_node(self)
|
||
self.lab_logger().debug(f"ROS节点初始化完成")
|
||
|
||
async def sleep(self, rel_time: float, callback_group=None):
|
||
if callback_group is None:
|
||
callback_group = self.callback_group
|
||
await ROS2DeviceNode.async_wait_for(self, rel_time, callback_group)
|
||
|
||
@classmethod
|
||
async def create_task(cls, func, trace_error=True, **kwargs) -> Task:
|
||
return ROS2DeviceNode.run_async_func(func, trace_error, **kwargs)
|
||
|
||
async def update_resource(self, resources: List["ResourcePLR"]):
|
||
r = SerialCommand.Request()
|
||
tree_set = ResourceTreeSet.from_plr_resources(resources)
|
||
for tree in tree_set.trees:
|
||
root_node = tree.root_node
|
||
if not root_node.res_content.uuid_parent:
|
||
logger.warning(f"更新无父节点物料{root_node}，自动以当前设备作为根节点")
|
||
root_node.res_content.parent_uuid = self.uuid
|
||
r.command = json.dumps({"data": {"data": tree_set.dump()}, "action": "update"})
|
||
response: SerialCommand_Response = await self._resource_clients["c2s_update_resource_tree"].call_async(r) # type: ignore
|
||
try:
|
||
uuid_maps = json.loads(response.response)
|
||
self.resource_tracker.loop_update_uuid(resources, uuid_maps)
|
||
except Exception as e:
|
||
self.lab_logger().error(f"更新资源uuid失败: {e}")
|
||
self.lab_logger().error(traceback.format_exc())
|
||
self.lab_logger().debug(f"资源更新结果: {response}")
|
||
|
||
async def get_resource(self, resources_uuid: List[str], with_children: bool = True) -> ResourceTreeSet:
|
||
"""
|
||
根据资源UUID列表获取资源树
|
||
|
||
Args:
|
||
resources_uuid: 资源UUID列表
|
||
with_children: 是否包含子节点，默认为True
|
||
|
||
Returns:
|
||
ResourceTreeSet: 资源树集合
|
||
"""
|
||
response: SerialCommand.Response = await self._resource_clients["c2s_update_resource_tree"].call_async(
|
||
SerialCommand.Request(
|
||
command=json.dumps(
|
||
{
|
||
"data": {"data": resources_uuid, "with_children": with_children},
|
||
"action": "get",
|
||
}
|
||
)
|
||
)
|
||
) # type: ignore
|
||
raw_nodes = json.loads(response.response)
|
||
tree_set = ResourceTreeSet.from_raw_list(raw_nodes)
|
||
self.lab_logger().debug(f"获取资源结果: {len(tree_set.trees)} 个资源树")
|
||
return tree_set
|
||
|
||
async def get_resource_with_dir(self, resource_id: str, with_children: bool = True) -> "ResourcePLR":
|
||
"""
|
||
根据资源ID获取单个资源实例
|
||
|
||
Args:
|
||
resource_ids: 资源ID字符串
|
||
with_children: 是否包含子节点，默认为True
|
||
|
||
Returns:
|
||
ResourcePLR: PLR资源实例
|
||
"""
|
||
r = SerialCommand.Request()
|
||
r.command = json.dumps(
|
||
{
|
||
"id": resource_id,
|
||
"uuid": None,
|
||
"with_children": with_children,
|
||
}
|
||
)
|
||
# 发送请求并等待响应
|
||
response: SerialCommand_Response = await self._resource_clients["resource_get"].call_async(r)
|
||
raw_data = json.loads(response.response)
|
||
|
||
# 转换为 PLR 资源
|
||
tree_set = ResourceTreeSet.from_raw_list(raw_data)
|
||
plr_resource = tree_set.to_plr_resources()[0]
|
||
self.lab_logger().debug(f"获取资源 {resource_id} 成功")
|
||
return plr_resource
|
||
|
||
def transfer_to_new_resource(
|
||
self, plr_resource: "ResourcePLR", tree: ResourceTreeInstance, additional_add_params: Dict[str, Any]
|
||
):
|
||
parent_uuid = tree.root_node.res_content.parent_uuid
|
||
if parent_uuid:
|
||
parent_resource: ResourcePLR = self.resource_tracker.uuid_to_resources.get(parent_uuid)
|
||
if parent_resource is None:
|
||
self.lab_logger().warning(
|
||
f"物料{plr_resource}请求挂载{tree.root_node.res_content.name}的父节点{parent_uuid}不存在"
|
||
)
|
||
else:
|
||
try:
|
||
# 特殊兼容所有plr的物料的assign方法，和create_resource append_resource后期同步
|
||
additional_params = {}
|
||
extra = getattr(plr_resource, "unilabos_extra", {})
|
||
if len(extra):
|
||
self.lab_logger().info(f"发现物料{plr_resource}额外参数: " + str(extra))
|
||
if "update_resource_site" in extra:
|
||
additional_add_params["site"] = extra["update_resource_site"]
|
||
site = additional_add_params.get("site", None)
|
||
spec = inspect.signature(parent_resource.assign_child_resource)
|
||
if "spot" in spec.parameters:
|
||
ordering_dict: Dict[str, Any] = getattr(parent_resource, "_ordering")
|
||
if ordering_dict:
|
||
site = list(ordering_dict.keys()).index(site)
|
||
additional_params["spot"] = site
|
||
old_parent = plr_resource.parent
|
||
if old_parent is not None:
|
||
# plr并不支持同一个deck的加载和卸载
|
||
self.lab_logger().warning(f"物料{plr_resource}请求从{old_parent}卸载")
|
||
old_parent.unassign_child_resource(plr_resource)
|
||
self.lab_logger().warning(
|
||
f"物料{plr_resource}请求挂载到{parent_resource}，额外参数：{additional_params}"
|
||
)
|
||
|
||
# ⭐ assign 之前，需要从 resources 列表中移除
|
||
# 因为资源将不再是顶级资源，而是成为 parent_resource 的子资源
|
||
# 如果不移除，figure_resource 会找到两次：一次在 resources，一次在 parent 的 children
|
||
resource_id = id(plr_resource)
|
||
for i, r in enumerate(self.resource_tracker.resources):
|
||
if id(r) == resource_id:
|
||
self.resource_tracker.resources.pop(i)
|
||
self.lab_logger().debug(
|
||
f"从顶级资源列表中移除 {plr_resource.name}（即将成为 {parent_resource.name} 的子资源）"
|
||
)
|
||
break
|
||
|
||
parent_resource.assign_child_resource(plr_resource, location=None, **additional_params)
|
||
|
||
func = getattr(self.driver_instance, "resource_tree_transfer", None)
|
||
if callable(func):
|
||
# 分别是 物料的原来父节点，当前物料的状态，物料的新父节点（此时物料已经重新assign了）
|
||
func(old_parent, plr_resource, parent_resource)
|
||
except Exception as e:
|
||
self.lab_logger().warning(
|
||
f"物料{plr_resource}请求挂载{tree.root_node.res_content.name}的父节点{parent_resource}[{parent_uuid}]失败！\n{traceback.format_exc()}"
|
||
)
|
||
|
||
async def s2c_resource_tree(self, req: SerialCommand_Request, res: SerialCommand_Response):
|
||
"""
|
||
处理资源树更新请求
|
||
|
||
支持三种操作：
|
||
- add: 添加新资源到资源树
|
||
- update: 更新现有资源
|
||
- remove: 从资源树中移除资源
|
||
"""
|
||
from pylabrobot.resources.resource import Resource as ResourcePLR
|
||
|
||
def _handle_add(
|
||
plr_resources: List[ResourcePLR], tree_set: ResourceTreeSet, additional_add_params: Dict[str, Any]
|
||
) -> Dict[str, Any]:
|
||
"""
|
||
处理资源添加操作的内部函数
|
||
|
||
Args:
|
||
plr_resources: PLR资源列表
|
||
tree_set: 资源树集合
|
||
additional_add_params: 额外的添加参数
|
||
|
||
Returns:
|
||
操作结果字典
|
||
"""
|
||
for plr_resource, tree in zip(plr_resources, tree_set.trees):
|
||
self.resource_tracker.add_resource(plr_resource)
|
||
self.transfer_to_new_resource(plr_resource, tree, additional_add_params)
|
||
|
||
func = getattr(self.driver_instance, "resource_tree_add", None)
|
||
if callable(func):
|
||
func(plr_resources)
|
||
|
||
return {"success": True, "action": "add"}
|
||
|
||
def _handle_remove(resources_uuid: List[str]) -> Dict[str, Any]:
|
||
"""
|
||
处理资源移除操作的内部函数
|
||
|
||
Args:
|
||
resources_uuid: 要移除的资源UUID列表
|
||
|
||
Returns:
|
||
操作结果字典，包含移除的资源列表
|
||
"""
|
||
found_resources: List[List[Union[ResourcePLR, dict]]] = self.resource_tracker.figure_resource(
|
||
[{"uuid": uid} for uid in resources_uuid], try_mode=True
|
||
)
|
||
found_plr_resources = []
|
||
other_plr_resources = []
|
||
|
||
for found_resource in found_resources:
|
||
for resource in found_resource:
|
||
if issubclass(resource.__class__, ResourcePLR):
|
||
found_plr_resources.append(resource)
|
||
else:
|
||
other_plr_resources.append(resource)
|
||
|
||
# 调用driver的remove回调
|
||
func = getattr(self.driver_instance, "resource_tree_remove", None)
|
||
if callable(func):
|
||
func(found_plr_resources)
|
||
|
||
# 从parent卸载并从tracker移除
|
||
for plr_resource in found_plr_resources:
|
||
if plr_resource.parent is not None:
|
||
plr_resource.parent.unassign_child_resource(plr_resource)
|
||
self.resource_tracker.remove_resource(plr_resource)
|
||
self.lab_logger().info(f"移除物料 {plr_resource} 及其子节点")
|
||
|
||
for other_plr_resource in other_plr_resources:
|
||
self.resource_tracker.remove_resource(other_plr_resource)
|
||
self.lab_logger().info(f"移除物料 {other_plr_resource} 及其子节点")
|
||
|
||
return {
|
||
"success": True,
|
||
"action": "remove",
|
||
# "removed_plr": found_plr_resources,
|
||
# "removed_other": other_plr_resources,
|
||
}
|
||
|
||
def _handle_update(
|
||
plr_resources: List[ResourcePLR], tree_set: ResourceTreeSet, additional_add_params: Dict[str, Any]
|
||
) -> Dict[str, Any]:
|
||
"""
|
||
处理资源更新操作的内部函数
|
||
|
||
Args:
|
||
plr_resources: PLR资源列表（包含新状态）
|
||
tree_set: 资源树集合
|
||
additional_add_params: 额外的参数
|
||
|
||
Returns:
|
||
操作结果字典
|
||
"""
|
||
for plr_resource, tree in zip(plr_resources, tree_set.trees):
|
||
states = plr_resource.serialize_all_state()
|
||
original_instance: ResourcePLR = self.resource_tracker.figure_resource(
|
||
{"uuid": tree.root_node.res_content.uuid}, try_mode=False
|
||
)
|
||
|
||
# Update操作中包含改名：需要先remove再add
|
||
if original_instance.name != plr_resource.name:
|
||
old_name = original_instance.name
|
||
new_name = plr_resource.name
|
||
self.lab_logger().info(f"物料改名操作：{old_name} -> {new_name}")
|
||
|
||
# 收集所有相关的uuid（包括子节点）
|
||
_handle_remove([original_instance.unilabos_uuid])
|
||
original_instance.name = new_name
|
||
_handle_add([original_instance], tree_set, additional_add_params)
|
||
|
||
self.lab_logger().info(f"物料改名完成：{old_name} -> {new_name}")
|
||
|
||
# 常规更新：不涉及改名
|
||
original_parent_resource = original_instance.parent
|
||
original_parent_resource_uuid = getattr(original_parent_resource, "unilabos_uuid", None)
|
||
target_parent_resource_uuid = tree.root_node.res_content.uuid_parent
|
||
|
||
self.lab_logger().info(
|
||
f"物料{original_instance} 原始父节点{original_parent_resource_uuid} "
|
||
f"目标父节点{target_parent_resource_uuid} 更新"
|
||
)
|
||
|
||
# 更新extra
|
||
if getattr(plr_resource, "unilabos_extra", None) is not None:
|
||
original_instance.unilabos_extra = getattr(plr_resource, "unilabos_extra") # type: ignore # noqa: E501
|
||
|
||
# 如果父节点变化，需要重新挂载
|
||
if (
|
||
original_parent_resource_uuid != target_parent_resource_uuid
|
||
and original_parent_resource is not None
|
||
):
|
||
self.transfer_to_new_resource(original_instance, tree, additional_add_params)
|
||
|
||
# 加载状态
|
||
original_instance.load_all_state(states)
|
||
child_count = len(original_instance.get_all_children())
|
||
self.lab_logger().info(
|
||
f"更新了资源属性 {plr_resource}[{tree.root_node.res_content.uuid}] " f"及其子节点 {child_count} 个"
|
||
)
|
||
|
||
# 调用driver的update回调
|
||
func = getattr(self.driver_instance, "resource_tree_update", None)
|
||
if callable(func):
|
||
func(plr_resources)
|
||
|
||
return {"success": True, "action": "update"}
|
||
|
||
try:
|
||
data = json.loads(req.command)
|
||
results = []
|
||
|
||
for i in data:
|
||
action = i.get("action") # remove, add, update
|
||
resources_uuid: List[str] = i.get("data") # 资源数据
|
||
additional_add_params = i.get("additional_add_params", {}) # 额外参数
|
||
self.lab_logger().info(
|
||
f"[Resource Tree Update] Processing {action} operation, " f"resources count: {len(resources_uuid)}"
|
||
)
|
||
tree_set = None
|
||
if action in ["add", "update"]:
|
||
tree_set = await self.get_resource(
|
||
resources_uuid=resources_uuid, with_children=True if action == "add" else False
|
||
)
|
||
try:
|
||
if action == "add":
|
||
if tree_set is None:
|
||
raise ValueError("tree_set不能为None")
|
||
plr_resources = tree_set.to_plr_resources()
|
||
result = _handle_add(plr_resources, tree_set, additional_add_params)
|
||
results.append(result)
|
||
elif action == "update":
|
||
if tree_set is None:
|
||
raise ValueError("tree_set不能为None")
|
||
plr_resources = tree_set.to_plr_resources()
|
||
result = _handle_update(plr_resources, tree_set, additional_add_params)
|
||
results.append(result)
|
||
elif action == "remove":
|
||
result = _handle_remove(resources_uuid)
|
||
results.append(result)
|
||
except Exception as e:
|
||
error_msg = f"Error processing {action} operation: {str(e)}"
|
||
self.lab_logger().error(f"[Resource Tree Update] {error_msg}")
|
||
self.lab_logger().error(traceback.format_exc())
|
||
results.append({"success": False, "action": action, "error": error_msg})
|
||
|
||
# 返回处理结果
|
||
result_json = {"results": results, "total": len(data)}
|
||
res.response = json.dumps(result_json, ensure_ascii=False, cls=TypeEncoder)
|
||
self.lab_logger().info(f"[Resource Tree Update] Completed processing {len(data)} operations")
|
||
|
||
except json.JSONDecodeError as e:
|
||
error_msg = f"Invalid JSON format: {str(e)}"
|
||
self.lab_logger().error(f"[Resource Tree Update] {error_msg}")
|
||
res.response = json.dumps({"success": False, "error": error_msg}, ensure_ascii=False)
|
||
except Exception as e:
|
||
error_msg = f"Unexpected error: {str(e)}"
|
||
self.lab_logger().error(f"[Resource Tree Update] {error_msg}")
|
||
self.lab_logger().error(traceback.format_exc())
|
||
res.response = json.dumps({"success": False, "error": error_msg}, ensure_ascii=False)
|
||
|
||
return res
|
||
|
||
async def transfer_resource_to_another(
|
||
self,
|
||
plr_resources: List["ResourcePLR"],
|
||
target_device_id: str,
|
||
target_resources: List["ResourcePLR"],
|
||
sites: List[str],
|
||
):
|
||
# 准备工作
|
||
uids = []
|
||
target_uids = []
|
||
for plr_resource in plr_resources:
|
||
uid = getattr(plr_resource, "unilabos_uuid", None)
|
||
if uid is None:
|
||
raise ValueError(f"来源物料{plr_resource}没有unilabos_uuid属性，无法转运")
|
||
uids.append(uid)
|
||
for target_resource in target_resources:
|
||
uid = getattr(target_resource, "unilabos_uuid", None)
|
||
if uid is None:
|
||
raise ValueError(f"目标物料{target_resource}没有unilabos_uuid属性，无法转运")
|
||
target_uids.append(uid)
|
||
srv_address = f"/srv{target_device_id}/s2c_resource_tree"
|
||
sclient = self.create_client(SerialCommand, srv_address)
|
||
# 等待服务可用（设置超时）
|
||
if not sclient.wait_for_service(timeout_sec=5.0):
|
||
self.lab_logger().error(f"[{self.device_id} Node-Resource] Service {srv_address} not available")
|
||
raise ValueError(f"[{self.device_id} Node-Resource] Service {srv_address} not available")
|
||
|
||
# 先从当前节点移除资源
|
||
await self.s2c_resource_tree(
|
||
SerialCommand_Request(
|
||
command=json.dumps([{"action": "remove", "data": uids}], ensure_ascii=False) # 只移除父节点
|
||
),
|
||
SerialCommand_Response(),
|
||
)
|
||
|
||
# 通知云端转运资源
|
||
for plr_resource, target_uid, site in zip(plr_resources, target_uids, sites):
|
||
tree_set = ResourceTreeSet.from_plr_resources([plr_resource])
|
||
for root_node in tree_set.root_nodes:
|
||
root_node.res_content.parent = None
|
||
root_node.res_content.parent_uuid = target_uid
|
||
r = SerialCommand.Request()
|
||
r.command = json.dumps({"data": {"data": tree_set.dump()}, "action": "update"}) # 和Update Resource一致
|
||
response: SerialCommand_Response = await self._resource_clients["c2s_update_resource_tree"].call_async(r) # type: ignore
|
||
self.lab_logger().info(f"资源云端转运到{target_device_id}结果: {response.response}")
|
||
|
||
# 创建请求
|
||
request = SerialCommand.Request()
|
||
request.command = json.dumps(
|
||
[
|
||
{
|
||
"action": "add",
|
||
"data": tree_set.all_nodes_uuid, # 只添加父节点，子节点会自动添加
|
||
"additional_add_params": {"site": site},
|
||
}
|
||
],
|
||
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"[{self.device_id} Node-Resource] Timeout waiting for response from {target_device_id}"
|
||
)
|
||
return False
|
||
time.sleep(0.05)
|
||
self.lab_logger().info(f"资源本地增加到{target_device_id}结果: {response.response}")
|
||
return None
|
||
|
||
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
|
||
from unilabos.config.config import BasicConfig
|
||
from unilabos.ros.nodes.presets.host_node import HostNode
|
||
|
||
if not BasicConfig.is_host_mode:
|
||
sclient = self.create_client(SerialCommand, "/node_info_update")
|
||
# 启动线程执行发送任务
|
||
threading.Thread(
|
||
target=self.send_slave_node_info,
|
||
args=(sclient,),
|
||
daemon=True,
|
||
name=f"ROSDevice{self.device_id}_send_slave_node_info",
|
||
).start()
|
||
else:
|
||
host_node = HostNode.get_instance(0)
|
||
if host_node is not None:
|
||
host_node.device_machine_names[self.device_id] = "本地"
|
||
|
||
def send_slave_node_info(self, sclient):
|
||
sclient.wait_for_service()
|
||
request = SerialCommand.Request()
|
||
from unilabos.config.config import BasicConfig
|
||
|
||
request.command = json.dumps(
|
||
{
|
||
"SYNC_SLAVE_NODE_INFO": {
|
||
"machine_name": BasicConfig.machine_name,
|
||
"type": "slave",
|
||
"edge_device_id": self.device_id,
|
||
}
|
||
},
|
||
ensure_ascii=False,
|
||
cls=TypeEncoder,
|
||
)
|
||
|
||
# 发送异步请求并等待结果
|
||
future = sclient.call_async(request)
|
||
response = future.result()
|
||
|
||
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:
|
||
if ex.args[0].startswith(f"AttributeError: '{self.driver_instance.__class__.__name__}' object"):
|
||
self.lab_logger().error(
|
||
f"publish error, {str(type(self.driver_instance))[8:-2]} has no attribute '{attr_name}'"
|
||
)
|
||
else:
|
||
self.lab_logger().error(
|
||
f"publish error, when {str(type(self.driver_instance))[8:-2]} getting attribute '{attr_name}'"
|
||
)
|
||
self.lab_logger().error(traceback.format_exc())
|
||
|
||
self._property_publishers[attr_name] = PropertyPublisher(
|
||
self, attr_name, get_device_attr, msg_type, initial_period, self._print_publish
|
||
)
|
||
|
||
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=self.callback_group,
|
||
)
|
||
|
||
self.lab_logger().trace(f"发布动作: {action_name}, 类型: {str_action_type}")
|
||
|
||
def get_real_function(self, instance, attr_name):
|
||
if hasattr(instance.__class__, attr_name):
|
||
obj = getattr(instance.__class__, attr_name)
|
||
if isinstance(obj, property):
|
||
return lambda *args, **kwargs: obj.fset(instance, *args, **kwargs), get_type_hints(obj.fset)
|
||
obj = getattr(instance, attr_name)
|
||
return obj, get_type_hints(obj)
|
||
else:
|
||
obj = getattr(instance, attr_name)
|
||
return obj, get_type_hints(obj)
|
||
|
||
def _create_execute_callback(self, action_name, action_value_mapping):
|
||
"""创建动作执行回调函数"""
|
||
|
||
async def execute_callback(goal_handle: ServerGoalHandle):
|
||
# 初始化结果信息变量
|
||
execution_error = ""
|
||
execution_success = False
|
||
action_return_value = None
|
||
|
||
##### self.lab_logger().info(f"执行动作: {action_name}")
|
||
goal = goal_handle.request
|
||
|
||
# 从目标消息中提取参数, 并调用对应的方法
|
||
if "sequence" in action_value_mapping:
|
||
# 如果一个指令对应函数的连续调用，如启动和等待结果，默认参数应该属于第一个函数调用
|
||
def ACTION(**kwargs):
|
||
for i, action in enumerate(action_value_mapping["sequence"]):
|
||
if i == 0:
|
||
self.lab_logger().info(f"执行序列动作第一步: {action}")
|
||
self.get_real_function(self.driver_instance, action)[0](**kwargs)
|
||
else:
|
||
self.lab_logger().info(f"执行序列动作后续步骤: {action}")
|
||
self.get_real_function(self.driver_instance, action)[0]()
|
||
|
||
action_paramtypes = self.get_real_function(self.driver_instance, action_value_mapping["sequence"][0])[
|
||
1
|
||
]
|
||
else:
|
||
ACTION, action_paramtypes = self.get_real_function(self.driver_instance, action_name)
|
||
|
||
action_kwargs = convert_from_ros_msg_with_mapping(goal, action_value_mapping["goal"])
|
||
self.lab_logger().debug(f"任务 {ACTION.__name__} 接收到原始目标: {action_kwargs}")
|
||
error_skip = False
|
||
# 向Host查询物料当前状态，如果是host本身的增加物料的请求，则直接跳过
|
||
if action_name not in ["create_resource_detailed", "create_resource"]:
|
||
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"{action_name} 查询资源状态: Key: {k} Type: {v}")
|
||
|
||
try:
|
||
# 统一处理单个或多个资源
|
||
is_sequence = v != "unilabos_msgs/Resource"
|
||
resource_inputs = action_kwargs[k] if is_sequence else [action_kwargs[k]]
|
||
|
||
# 批量查询资源
|
||
queried_resources = []
|
||
for resource_data in resource_inputs:
|
||
plr_resource = await self.get_resource_with_dir(
|
||
resource_id=resource_data["id"], with_children=True
|
||
)
|
||
queried_resources.append(plr_resource)
|
||
|
||
self.lab_logger().debug(f"资源查询结果: 共 {len(queried_resources)} 个资源")
|
||
|
||
# 通过资源跟踪器获取本地实例
|
||
final_resources = queried_resources if is_sequence else queried_resources[0]
|
||
final_resources = (
|
||
self.resource_tracker.figure_resource({"name": final_resources.name}, try_mode=False)
|
||
if not is_sequence
|
||
else [
|
||
self.resource_tracker.figure_resource({"name": res.name}, try_mode=False)
|
||
for res in queried_resources
|
||
]
|
||
)
|
||
action_kwargs[k] = final_resources
|
||
|
||
except Exception as e:
|
||
self.lab_logger().error(f"{action_name} 物料实例获取失败: {e}\n{traceback.format_exc()}")
|
||
error_skip = True
|
||
execution_error = traceback.format_exc()
|
||
break
|
||
|
||
##### self.lab_logger().info(f"准备执行: {action_kwargs}, 函数: {ACTION.__name__}")
|
||
time_start = time.time()
|
||
time_overall = 100
|
||
future = None
|
||
if not error_skip:
|
||
# 将阻塞操作放入线程池执行
|
||
if asyncio.iscoroutinefunction(ACTION):
|
||
try:
|
||
##### self.lab_logger().info(f"异步执行动作 {ACTION}")
|
||
future = ROS2DeviceNode.run_async_func(ACTION, trace_error=False, **action_kwargs)
|
||
|
||
def _handle_future_exception(fut):
|
||
nonlocal execution_error, execution_success, action_return_value
|
||
try:
|
||
action_return_value = fut.result()
|
||
execution_success = True
|
||
except Exception as e:
|
||
execution_error = traceback.format_exc()
|
||
error(
|
||
f"异步任务 {ACTION.__name__} 报错了\n{traceback.format_exc()}\n原始输入：{action_kwargs}"
|
||
)
|
||
|
||
future.add_done_callback(_handle_future_exception)
|
||
except Exception as e:
|
||
execution_error = traceback.format_exc()
|
||
execution_success = False
|
||
self.lab_logger().error(f"创建异步任务失败: {traceback.format_exc()}")
|
||
else:
|
||
##### self.lab_logger().info(f"同步执行动作 {ACTION}")
|
||
future = self._executor.submit(ACTION, **action_kwargs)
|
||
|
||
def _handle_future_exception(fut):
|
||
nonlocal execution_error, execution_success, action_return_value
|
||
try:
|
||
action_return_value = fut.result()
|
||
execution_success = True
|
||
except Exception as e:
|
||
execution_error = traceback.format_exc()
|
||
error(
|
||
f"同步任务 {ACTION.__name__} 报错了\n{traceback.format_exc()}\n原始输入：{action_kwargs}"
|
||
)
|
||
|
||
future.add_done_callback(_handle_future_exception)
|
||
|
||
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 future is not None and 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 is not None and future.cancelled():
|
||
self.lab_logger().info(f"动作 {action_name} 已取消")
|
||
return action_type.Result()
|
||
|
||
# self.lab_logger().info(f"动作执行完成: {action_name}")
|
||
del future
|
||
|
||
# 向Host更新物料当前状态
|
||
if action_name not in ["create_resource_detailed", "create_resource"]:
|
||
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}")
|
||
# 仅当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:
|
||
# 去重：使用 seen 集合获取唯一的资源对象
|
||
seen = set()
|
||
unique_resources = []
|
||
for rs in akv: # todo: 这里目前只支持plr的类型
|
||
res = self.resource_tracker.parent_resource(rs) # 获取 resource 对象
|
||
if id(res) not in seen:
|
||
seen.add(id(res))
|
||
unique_resources.append(res)
|
||
|
||
# 使用新的资源树接口
|
||
if unique_resources:
|
||
await self.update_resource(unique_resources)
|
||
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)
|
||
elif attr_name == "return_info":
|
||
setattr(
|
||
result_msg,
|
||
attr_name,
|
||
get_result_info_str(execution_error, execution_success, action_return_value),
|
||
)
|
||
|
||
##### self.lab_logger().info(f"动作 {action_name} 完成并返回结果")
|
||
return result_msg
|
||
|
||
return execute_callback
|
||
|
||
def _execute_driver_command(self, string: str):
|
||
try:
|
||
target = json.loads(string)
|
||
except Exception as ex:
|
||
try:
|
||
target = yaml.safe_load(io.StringIO(string))
|
||
except Exception as ex2:
|
||
raise JsonCommandInitError(
|
||
f"执行动作时JSON/YAML解析失败: \n{ex}\n{ex2}\n原内容: {string}\n{traceback.format_exc()}"
|
||
)
|
||
try:
|
||
function_name = target["function_name"]
|
||
function_args = target["function_args"]
|
||
assert isinstance(function_args, dict), "执行动作时JSON必须为dict类型\n原JSON: {string}"
|
||
function = getattr(self.driver_instance, function_name)
|
||
assert callable(
|
||
function
|
||
), f"执行动作时JSON中的function_name对应的函数不可调用: {function_name}\n原JSON: {string}"
|
||
|
||
# 处理 ResourceSlot 类型参数
|
||
args_list = default_manager._analyze_method_signature(function)["args"]
|
||
for arg in args_list:
|
||
arg_name = arg["name"]
|
||
arg_type = arg["type"]
|
||
|
||
# 跳过不在 function_args 中的参数
|
||
if arg_name not in function_args:
|
||
continue
|
||
|
||
# 处理单个 ResourceSlot
|
||
if arg_type == "unilabos.registry.placeholder_type:ResourceSlot":
|
||
resource_data = function_args[arg_name]
|
||
if isinstance(resource_data, dict) and "id" in resource_data:
|
||
try:
|
||
converted_resource = self._convert_resource_sync(resource_data)
|
||
function_args[arg_name] = converted_resource
|
||
except Exception as e:
|
||
self.lab_logger().error(
|
||
f"转换ResourceSlot参数 {arg_name} 失败: {e}\n{traceback.format_exc()}"
|
||
)
|
||
raise JsonCommandInitError(f"ResourceSlot参数转换失败: {arg_name}")
|
||
|
||
# 处理 ResourceSlot 列表
|
||
elif isinstance(arg_type, tuple) and len(arg_type) == 2:
|
||
resource_slot_type = "unilabos.registry.placeholder_type:ResourceSlot"
|
||
if arg_type[0] == "list" and arg_type[1] == resource_slot_type:
|
||
resource_list = function_args[arg_name]
|
||
if isinstance(resource_list, list):
|
||
try:
|
||
converted_resources = []
|
||
for resource_data in resource_list:
|
||
if isinstance(resource_data, dict) and "id" in resource_data:
|
||
converted_resource = self._convert_resource_sync(resource_data)
|
||
converted_resources.append(converted_resource)
|
||
function_args[arg_name] = converted_resources
|
||
except Exception as e:
|
||
self.lab_logger().error(
|
||
f"转换ResourceSlot列表参数 {arg_name} 失败: {e}\n{traceback.format_exc()}"
|
||
)
|
||
raise JsonCommandInitError(f"ResourceSlot列表参数转换失败: {arg_name}")
|
||
|
||
return function(**function_args)
|
||
except KeyError as ex:
|
||
raise JsonCommandInitError(
|
||
f"执行动作时JSON缺少function_name或function_args: {ex}\n原JSON: {string}\n{traceback.format_exc()}"
|
||
)
|
||
|
||
def _convert_resource_sync(self, resource_data: Dict[str, Any]):
|
||
"""同步转换资源数据为实例"""
|
||
# 创建资源查询请求
|
||
r = SerialCommand.Request()
|
||
r.command = json.dumps(
|
||
{
|
||
"id": resource_data.get("id", None),
|
||
"uuid": resource_data.get("uuid", None),
|
||
"with_children": True,
|
||
}
|
||
)
|
||
|
||
# 同步调用资源查询服务
|
||
future = self._resource_clients["resource_get"].call_async(r)
|
||
|
||
# 等待结果（使用while循环，每次sleep 0.05秒，最多等待30秒）
|
||
timeout = 30.0
|
||
elapsed = 0.0
|
||
while not future.done() and elapsed < timeout:
|
||
time.sleep(0.05)
|
||
elapsed += 0.05
|
||
|
||
if not future.done():
|
||
raise Exception(f"资源查询超时: {resource_data}")
|
||
|
||
response = future.result()
|
||
if response is None:
|
||
raise Exception(f"资源查询返回空结果: {resource_data}")
|
||
|
||
raw_data = json.loads(response.response)
|
||
|
||
# 转换为 PLR 资源
|
||
tree_set = ResourceTreeSet.from_raw_list(raw_data)
|
||
plr_resource = tree_set.to_plr_resources()[0]
|
||
|
||
# 通过资源跟踪器获取本地实例
|
||
res = self.resource_tracker.figure_resource(plr_resource, try_mode=True)
|
||
if len(res) == 0:
|
||
self.lab_logger().warning(f"资源转换未能索引到实例: {resource_data}，返回新建实例")
|
||
return plr_resource
|
||
elif len(res) == 1:
|
||
return res[0]
|
||
else:
|
||
raise ValueError(f"资源转换得到多个实例: {res}")
|
||
|
||
async def _execute_driver_command_async(self, string: str):
|
||
try:
|
||
target = json.loads(string)
|
||
except Exception as ex:
|
||
try:
|
||
target = yaml.safe_load(io.StringIO(string))
|
||
except Exception as ex2:
|
||
raise JsonCommandInitError(
|
||
f"执行动作时JSON/YAML解析失败: \n{ex}\n{ex2}\n原内容: {string}\n{traceback.format_exc()}"
|
||
)
|
||
try:
|
||
function_name = target["function_name"]
|
||
function_args = target["function_args"]
|
||
assert isinstance(function_args, dict), "执行动作时JSON必须为dict类型\n原JSON: {string}"
|
||
function = getattr(self.driver_instance, function_name)
|
||
assert callable(
|
||
function
|
||
), f"执行动作时JSON中的function_name对应的函数不可调用: {function_name}\n原JSON: {string}"
|
||
assert asyncio.iscoroutinefunction(
|
||
function
|
||
), f"执行动作时JSON中的function并非异步: {function_name}\n原JSON: {string}"
|
||
|
||
# 处理 ResourceSlot 类型参数
|
||
args_list = default_manager._analyze_method_signature(function)["args"]
|
||
for arg in args_list:
|
||
arg_name = arg["name"]
|
||
arg_type = arg["type"]
|
||
|
||
# 跳过不在 function_args 中的参数
|
||
if arg_name not in function_args:
|
||
continue
|
||
|
||
# 处理单个 ResourceSlot
|
||
if arg_type == "unilabos.registry.placeholder_type:ResourceSlot":
|
||
resource_data = function_args[arg_name]
|
||
if isinstance(resource_data, dict) and "id" in resource_data:
|
||
try:
|
||
converted_resource = await self._convert_resource_async(resource_data)
|
||
function_args[arg_name] = converted_resource
|
||
except Exception as e:
|
||
self.lab_logger().error(
|
||
f"转换ResourceSlot参数 {arg_name} 失败: {e}\n{traceback.format_exc()}"
|
||
)
|
||
raise JsonCommandInitError(f"ResourceSlot参数转换失败: {arg_name}")
|
||
|
||
# 处理 ResourceSlot 列表
|
||
elif isinstance(arg_type, tuple) and len(arg_type) == 2:
|
||
resource_slot_type = "unilabos.registry.placeholder_type:ResourceSlot"
|
||
if arg_type[0] == "list" and arg_type[1] == resource_slot_type:
|
||
resource_list = function_args[arg_name]
|
||
if isinstance(resource_list, list):
|
||
try:
|
||
converted_resources = []
|
||
for resource_data in resource_list:
|
||
if isinstance(resource_data, dict) and "id" in resource_data:
|
||
converted_resource = await self._convert_resource_async(resource_data)
|
||
converted_resources.append(converted_resource)
|
||
function_args[arg_name] = converted_resources
|
||
except Exception as e:
|
||
self.lab_logger().error(
|
||
f"转换ResourceSlot列表参数 {arg_name} 失败: {e}\n{traceback.format_exc()}"
|
||
)
|
||
raise JsonCommandInitError(f"ResourceSlot列表参数转换失败: {arg_name}")
|
||
|
||
return await function(**function_args)
|
||
except KeyError as ex:
|
||
raise JsonCommandInitError(
|
||
f"执行动作时JSON缺少function_name或function_args: {ex}\n原JSON: {string}\n{traceback.format_exc()}"
|
||
)
|
||
|
||
async def _convert_resource_async(self, resource_data: Dict[str, Any]):
|
||
"""异步转换资源数据为实例"""
|
||
# 使用封装的get_resource_with_dir方法获取PLR资源
|
||
plr_resource = await self.get_resource_with_dir(resource_ids=resource_data["id"], with_children=True)
|
||
|
||
# 通过资源跟踪器获取本地实例
|
||
res = self.resource_tracker.figure_resource(plr_resource, try_mode=True)
|
||
if len(res) == 0:
|
||
# todo: 后续通过decoration来区分，减少warning
|
||
self.lab_logger().warning(f"资源转换未能索引到实例: {resource_data}，返回新建实例")
|
||
return plr_resource
|
||
elif len(res) == 1:
|
||
return res[0]
|
||
else:
|
||
raise ValueError(f"资源转换得到多个实例: {res}")
|
||
|
||
# 异步上下文管理方法
|
||
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 JsonCommandInitError(Exception):
|
||
pass
|
||
|
||
|
||
class ROS2DeviceNode:
|
||
"""
|
||
ROS2设备节点类
|
||
|
||
这个类封装了设备类实例和ROS2节点的功能，提供ROS2接口。
|
||
它不继承设备类，而是通过代理模式访问设备类的属性和方法。
|
||
"""
|
||
|
||
@classmethod
|
||
def run_async_func(cls, func, trace_error=True, **kwargs) -> Task:
|
||
def _handle_future_exception(fut):
|
||
try:
|
||
fut.result()
|
||
except Exception as e:
|
||
error(f"异步任务 {func.__name__} 报错了")
|
||
error(traceback.format_exc())
|
||
|
||
future = rclpy.get_global_executor().create_task(func(**kwargs))
|
||
if trace_error:
|
||
future.add_done_callback(_handle_future_exception)
|
||
return future
|
||
|
||
@classmethod
|
||
async def async_wait_for(cls, node: Node, wait_time: float, callback_group=None):
|
||
future = Future()
|
||
timer = node.create_timer(
|
||
wait_time, lambda: future.set_result(None), callback_group=callback_group, clock=node.get_clock()
|
||
)
|
||
await future
|
||
timer.cancel()
|
||
node.destroy_timer(timer)
|
||
|
||
@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,
|
||
device_uuid: str,
|
||
driver_class: Type[T],
|
||
device_config: ResourceDictInstance,
|
||
driver_params: Dict[str, Any],
|
||
status_types: Dict[str, Any],
|
||
action_value_mappings: Dict[str, Any],
|
||
hardware_interface: Dict[str, Any],
|
||
print_publish: bool = True,
|
||
driver_is_ros: bool = False,
|
||
):
|
||
"""
|
||
初始化ROS2设备节点
|
||
|
||
Args:
|
||
device_id: 设备标识符
|
||
device_uuid: 设备uuid
|
||
driver_class: 设备类
|
||
device_config: 原始初始化的ResourceDictInstance
|
||
driver_params: driver初始化的参数
|
||
status_types: 状态类型映射
|
||
action_value_mappings: 动作值映射
|
||
hardware_interface: 硬件接口配置
|
||
children:
|
||
print_publish: 是否打印发布信息
|
||
driver_is_ros:
|
||
"""
|
||
|
||
# 保存设备类是否支持异步上下文
|
||
self._has_async_context = hasattr(driver_class, "__aenter__") and hasattr(driver_class, "__aexit__")
|
||
self._driver_class = driver_class
|
||
self.device_config = device_config
|
||
children: List[ResourceDictInstance] = device_config.children
|
||
self.driver_is_ros = driver_is_ros
|
||
self.driver_is_workstation = False
|
||
self.resource_tracker = DeviceNodeResourceTracker()
|
||
|
||
# use_pylabrobot_creator 使用 cls的包路径检测
|
||
use_pylabrobot_creator = (
|
||
driver_class.__module__.startswith("pylabrobot")
|
||
or driver_class.__name__ == "LiquidHandlerAbstract"
|
||
or driver_class.__name__ == "LiquidHandlerBiomek"
|
||
or driver_class.__name__ == "PRCXI9300Handler"
|
||
or driver_class.__name__ == "TransformXYZHandler"
|
||
)
|
||
|
||
# 创建设备类实例
|
||
if use_pylabrobot_creator:
|
||
# 先对pylabrobot的子资源进行加载，不然subclass无法认出
|
||
# 在下方对于加载Deck等Resource要手动import
|
||
register()
|
||
self._driver_creator = PyLabRobotCreator(
|
||
driver_class, children=children, resource_tracker=self.resource_tracker
|
||
)
|
||
else:
|
||
from unilabos.devices.workstation.workstation_base import WorkstationBase
|
||
|
||
if issubclass(
|
||
self._driver_class, WorkstationBase
|
||
): # 是WorkstationNode的子节点，就要调用WorkstationNodeCreator
|
||
self.driver_is_workstation = True
|
||
self._driver_creator = WorkstationNodeCreator(
|
||
driver_class, children=children, resource_tracker=self.resource_tracker
|
||
)
|
||
else:
|
||
self._driver_creator = DeviceClassCreator(
|
||
driver_class, children=children, resource_tracker=self.resource_tracker
|
||
)
|
||
|
||
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
|
||
elif self.driver_is_workstation:
|
||
from unilabos.ros.nodes.presets.workstation import ROS2WorkstationNode
|
||
|
||
self._ros_node = ROS2WorkstationNode(
|
||
protocol_type=driver_params["protocol_type"],
|
||
children=children,
|
||
driver_instance=self._driver_instance, # type: ignore
|
||
device_id=device_id,
|
||
device_uuid=device_uuid,
|
||
status_types=status_types,
|
||
action_value_mappings=action_value_mappings,
|
||
hardware_interface=hardware_interface,
|
||
print_publish=print_publish,
|
||
resource_tracker=self.resource_tracker,
|
||
)
|
||
else:
|
||
self._ros_node = BaseROS2DeviceNode(
|
||
driver_instance=self._driver_instance,
|
||
device_id=device_id,
|
||
device_uuid=device_uuid,
|
||
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}")
|
||
self.driver_instance._ros_node = self._ros_node # type: ignore
|
||
self.driver_instance._execute_driver_command = self._ros_node._execute_driver_command # type: ignore
|
||
self.driver_instance._execute_driver_command_async = self._ros_node._execute_driver_command_async # type: ignore
|
||
if hasattr(self.driver_instance, "post_init"):
|
||
try:
|
||
self.driver_instance.post_init(self._ros_node) # type: ignore
|
||
except Exception as e:
|
||
self._ros_node.lab_logger().error(f"设备后初始化失败: {e}")
|
||
|
||
|
||
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
|