import inspect
import io
import json
import threading
import time
import traceback
from typing import (
    get_type_hints,
    TypeVar,
    Generic,
    Dict,
    Any,
    Type,
    TypedDict,
    Optional,
    List,
    TYPE_CHECKING,
    Union,
    Tuple,
)

from concurrent.futures import ThreadPoolExecutor
import asyncio

import rclpy
import yaml
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.config.config import BasicConfig
from unilabos.registry.decorators import get_topic_config
from unilabos.utils.decorator import get_all_subscriptions

from unilabos.resources.container import RegularContainer
from unilabos.resources.graphio import (
    initialize_resources,
)
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.resources.resource_tracker import (
    DeviceNodeResourceTracker,
    ResourceDictType,
    ResourceTreeSet,
    ResourceTreeInstance,
    ResourceDictInstance,
    EXTRA_SAMPLE_UUID,
    PARAM_SAMPLE_UUIDS,
    JSON_UNILABOS_PARAM,
)
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: ResourceDictInstance,
    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,
        qos: int = 10,
    ):
        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.qos = qos

        self._value = None
        try:
            self.publisher_ = node.create_publisher(msg_type, f"{name}", qos)
        except Exception as e:
            self.node.lab_logger().error(
                f"StatusError, DeviceId: {self.node.device_id} 创建发布者 {name} 失败，可能由于注册表有误，类型: {msg_type}，错误: {e}"
            )
        self.timer = node.create_timer(self.timer_period, self.publish_property)
        self.__loop = ROS2DeviceNode.get_asyncio_loop()
        str_msg_type = str(msg_type)[8:-2]
        self.node.lab_logger().trace(f"发布属性: {name}, 类型: {str_msg_type}, 周期: {initial_period}秒, QoS: {qos}")

    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:
                pass
                # self.node.lab_logger().trace(f"【.publish_property】发布 {self.msg_type}: {value}")
            if value is not None:
                msg = convert_to_ros_msg(self.msg_type, value)
                self.publisher_.publish(msg)
                # self.node.lab_logger().trace(f"【.publish_property】属性 {self.name} 发布成功")
        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,
        registry_name: 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.registry_name = registry_name
        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)

        # 创建订阅者（通过 @subscribe 装饰器）
        self._topic_subscribers: Dict[str, Any] = {}
        self._setup_decorated_subscribers()

        # 创建线程池执行器
        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):
            from pylabrobot.resources.deck import Deck
            from pylabrobot.resources import Coordinate
            from pylabrobot.resources import Plate

            # 物料传输到对应的node节点
            client = self._resource_clients["c2s_update_resource_tree"]
            request = SerialCommand.Request()
            request2 = SerialCommand.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"]
            input_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: List[float] = other_calling_param.pop("LIQUID_VOLUME", [])
            LIQUID_INPUT_SLOT: List[int] = other_calling_param.pop("LIQUID_INPUT_SLOT", [])
            slot = other_calling_param.pop("slot", "-1")
            if slot != -1:  # slot为负数的时候采用assign方法
                other_calling_param["slot"] = slot
            # 本地拿到这个物料，可能需要先做初始化
            if isinstance(input_resources, list) and initialize_full:
                input_resources = initialize_resources(input_resources)
            elif initialize_full:
                input_resources = initialize_resources([input_resources])
            rts: ResourceTreeSet = ResourceTreeSet.from_raw_dict_list(input_resources)
            parent_resource = None
            if bind_parent_id != self.node_name:
                parent_resource = self.resource_tracker.figure_resource({"name": bind_parent_id})
                for r in rts.root_nodes:
                    # noinspection PyUnresolvedReferences
                    r.res_content.parent_uuid = parent_resource.unilabos_uuid
            else:
                for r in rts.root_nodes:
                    r.res_content.parent_uuid = self.uuid
            rts_plr_instances = rts.to_plr_resources()
            if len(rts.root_nodes) == 1 and isinstance(rts_plr_instances[0], RegularContainer):
                # noinspection PyTypeChecker
                container_instance: RegularContainer = rts_plr_instances[0]
                found_resources = self.resource_tracker.figure_resource(
                    {"name": container_instance.name}, try_mode=True
                )
                if not len(found_resources):
                    self.resource_tracker.add_resource(container_instance)
                    logger.info(f"添加物料{container_instance.name}到资源跟踪器")
                else:
                    assert len(found_resources) == 1, f"找到多个同名物料: {container_instance.name}, 请检查物料系统"
                    found_resource = found_resources[0]
                    if isinstance(found_resource, RegularContainer):
                        logger.info(f"更新物料{container_instance.name}的数据{found_resource.state}")
                        found_resource.state.update(container_instance.state)
                    elif isinstance(found_resource, dict):
                        raise ValueError("已不支持 字典 版本的RegularContainer")
                    else:
                        logger.info(
                            f"更新物料{container_instance.name}出现不支持的数据类型{type(found_resource)} {found_resource}"
                        )
            # noinspection PyUnresolvedReferences
            request.command = json.dumps(
                {
                    "action": "add",
                    "data": {
                        "data": rts.dump(),
                        "mount_uuid": parent_resource.unilabos_uuid if parent_resource is not None else self.uuid,
                        "first_add": False,
                    },
                }
            )
            tree_response: SerialCommand.Response = await client.call_async(request)
            uuid_maps = json.loads(tree_response.response)
            plr_instances = rts.to_plr_resources()
            for plr_instance in plr_instances:
                self.resource_tracker.loop_update_uuid(plr_instance, uuid_maps)
            rts: ResourceTreeSet = ResourceTreeSet.from_plr_resources(plr_instances)
            self.lab_logger().info(f"Resource tree added. UUID mapping: {len(uuid_maps)} nodes")
            final_response = {
                "created_resource_tree": rts.dump(),
                "liquid_input_resource_tree": [],
            }
            res.response = json.dumps(final_response)
            # 如果driver自己就有assign的方法，那就使用driver自己的assign方法
            if hasattr(self.driver_instance, "create_resource") and self.node_name != "host_node":
                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
            try:
                if len(rts.root_nodes) == 1 and parent_resource is not None:
                    plr_instance = plr_instances[0]
                    if isinstance(plr_instance, Plate):
                        if len(ADD_LIQUID_TYPE) == 1 and len(LIQUID_VOLUME) == 1 and len(LIQUID_INPUT_SLOT) > 1:
                            ADD_LIQUID_TYPE = ADD_LIQUID_TYPE * len(LIQUID_INPUT_SLOT)
                            LIQUID_VOLUME = LIQUID_VOLUME * len(LIQUID_INPUT_SLOT)
                            self.lab_logger().warning(
                                f"增加液体资源时，数量为1，自动补全为 {len(LIQUID_INPUT_SLOT)} 个"
                            )
                        try:
                            # noinspection PyProtectedMember
                            keys = list(plr_instance._ordering.keys())
                            for ind, r in enumerate(LIQUID_INPUT_SLOT[:]):
                                if isinstance(r, int):
                                    # noinspection PyTypeChecker
                                    LIQUID_INPUT_SLOT[ind] = keys[r]
                            input_wells = [plr_instance.get_well(r) for r in LIQUID_INPUT_SLOT]
                        except AttributeError:
                            # 按照id回去失败，回退到children
                            input_wells = []
                            for r in LIQUID_INPUT_SLOT:
                                input_wells.append(plr_instance.children[r])
                        for input_well, liquid_type, liquid_volume, liquid_input_slot in zip(
                            input_wells, ADD_LIQUID_TYPE, LIQUID_VOLUME, LIQUID_INPUT_SLOT
                        ):
                            input_well.set_liquids([(liquid_type, liquid_volume, "uL")])
                        final_response["liquid_input_resource_tree"] = ResourceTreeSet.from_plr_resources(
                            input_wells
                        ).dump()
                        res.response = json.dumps(final_response)
                    if (
                        issubclass(parent_resource.__class__, Deck)
                        and hasattr(parent_resource, "assign_child_at_slot")
                        and "slot" in other_calling_param
                    ):
                        other_calling_param["slot"] = int(other_calling_param["slot"])
                        parent_resource.assign_child_at_slot(plr_instance, **other_calling_param)
                    else:
                        _discard_slot = other_calling_param.pop("slot", -1)
                        parent_resource.assign_child_resource(
                            plr_instance,
                            Coordinate(location["x"], location["y"], location["z"]),
                            **other_calling_param,
                        )
                    # 调整了液体以及Deck之后要重新Assign
                    # noinspection PyUnresolvedReferences
                    rts_with_parent = ResourceTreeSet.from_plr_resources([parent_resource])
                    if rts_with_parent.root_nodes[0].res_content.uuid_parent is None:
                        rts_with_parent.root_nodes[0].res_content.parent_uuid = self.uuid
                    request.command = json.dumps(
                        {
                            "action": "add",
                            "data": {
                                "data": rts_with_parent.dump(),
                                "mount_uuid": rts_with_parent.root_nodes[0].res_content.uuid_parent,
                                "first_add": False,
                            },
                        }
                    )
                    tree_response: SerialCommand.Response = await client.call_async(request)
                    uuid_maps = json.loads(tree_response.response)
                    self.resource_tracker.loop_update_uuid(input_resources, uuid_maps)
                    self._lab_logger.info(f"Resource tree added. UUID mapping: {len(uuid_maps)} nodes")
                    # 这里created_resources不包含parent_resource
                # 发送给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": input_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")
                res.response = get_result_info_str(traceback.format_exc(), False, {})
            except Exception as e:
                self.lab_logger().error("Host请求添加物料时出错")
                self.lab_logger().error(traceback.format_exc())
                res.response = get_result_info_str(traceback.format_exc(), False, {})
            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,
            ),
            "s2c_device_manage": self.create_service(
                SerialCommand,
                f"/srv{self.namespace}/s2c_device_manage",
                self.s2c_device_manage,  # 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().trace(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_dict_list(raw_nodes)
        self.lab_logger().trace(f"获取资源结果: {len(tree_set.trees)} 个资源树 {tree_set.root_nodes}")
        return tree_set

    async def get_resource_with_dir(self, resource_id: str, with_children: bool = True) -> "ResourcePLR":
        """
        根据资源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_dict_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]
    ) -> Optional["ResourcePLR"]:
        parent_uuid = tree.root_node.res_content.parent_uuid
        if not parent_uuid:
            self.lab_logger().warning(
                f"物料{plr_resource} parent未知，挂载到当前节点下，额外参数：{additional_add_params}"
            )
            return None
        if parent_uuid == self.uuid:
            self.lab_logger().warning(
                f"物料{plr_resource}请求挂载到{self.identifier}，额外参数：{additional_add_params}"
            )
            return None
        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)
                return 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]
        ) -> Tuple[Dict[str, Any], List[ResourcePLR]]:
            """
            处理资源添加操作的内部函数

            Args:
                plr_resources: PLR资源列表
                tree_set: 资源树集合
                additional_add_params: 额外的添加参数

            Returns:
                操作结果字典
            """
            parents = []  # 放的是被变更的物料 / 被变更的物料父级
            for plr_resource, tree in zip(plr_resources, tree_set.trees):
                self.resource_tracker.add_resource(plr_resource)
                parent = self.transfer_to_new_resource(plr_resource, tree, additional_add_params)
                if parent is not None:
                    parents.append(parent)
                else:
                    parents.append(plr_resource)

            func = getattr(self.driver_instance, "resource_tree_add", None)
            if callable(func):
                func(plr_resources)

            return {"success": True, "action": "add"}, parents

        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[Union[ResourcePLR, ResourceDictInstance]],
            tree_set: ResourceTreeSet,
            additional_add_params: Dict[str, Any],
        ) -> Tuple[Dict[str, Any], List[ResourcePLR]]:
            """
            处理资源更新操作的内部函数

            Args:
                plr_resources: PLR资源列表（包含新状态）
                tree_set: 资源树集合
                additional_add_params: 额外的参数

            Returns:
                操作结果字典
            """
            original_instances = []
            for plr_resource, tree in zip(plr_resources, tree_set.trees):
                if isinstance(plr_resource, ResourceDictInstance):
                    self._lab_logger.info(f"跳过 非资源{plr_resource.res_content.name} 的更新")
                    continue
                states = plr_resource.serialize_all_state()
                original_instance: ResourcePLR = self.resource_tracker.figure_resource(
                    {"uuid": tree.root_node.res_content.uuid}, try_mode=False
                )
                original_parent_resource = original_instance.parent
                original_parent_resource_uuid = getattr(original_parent_resource, "unilabos_uuid", None)
                target_parent_resource_uuid = tree.root_node.res_content.uuid_parent
                not_same_parent = (
                    original_parent_resource_uuid != target_parent_resource_uuid
                    and original_parent_resource is not None
                )
                old_name = original_instance.name
                new_name = plr_resource.name
                parent_appended = False

                # Update操作中包含改名：需要先remove再add，这里更新父节点即可
                if not not_same_parent and old_name != new_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_instances.append(original_parent_resource)
                    parent_appended = True

                # 常规更新：不涉及改名
                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 not_same_parent:
                    parent = self.transfer_to_new_resource(original_instance, tree, additional_add_params)
                    original_instances.append(parent)
                    parent_appended = True
                else:
                    # 判断是否变更了resource_site，重新登记
                    target_site = original_instance.unilabos_extra.get("update_resource_site")
                    sites = (
                        original_instance.parent.sites
                        if original_instance.parent is not None and hasattr(original_instance.parent, "sites")
                        else None
                    )
                    site_names = (
                        list(original_instance.parent._ordering.keys())
                        if original_instance.parent is not None and hasattr(original_instance.parent, "sites")
                        else []
                    )
                    if target_site is not None and sites is not None and site_names is not None:
                        site_index = None
                        try:
                            # sites 可能是 Resource 列表或 dict 列表 (如 PRCXI9300Deck)
                            # 只有itemized_carrier在使用，准备弃用
                            site_index = sites.index(original_instance)
                        except ValueError:
                            # dict 类型的 sites: 通过name匹配
                            for idx, site in enumerate(sites):
                                if original_instance.name == site["occupied_by"]:
                                    site_index = idx
                                    break
                                elif (original_instance.location.x == site["position"]["x"] and original_instance.location.y == site["position"]["y"] and original_instance.location.z == site["position"]["z"]):
                                    site_index = idx
                                    break
                        if site_index is None:
                            site_name = None
                        else:
                            site_name = site_names[site_index]
                        if site_name != target_site:
                            parent = self.transfer_to_new_resource(original_instance, tree, additional_add_params)
                            if parent is not None:
                                original_instances.append(parent)
                                parent_appended = True

                # 加载状态
                # noinspection PyProtectedMember
                original_instance._size_x = plr_resource._size_x
                # noinspection PyProtectedMember
                original_instance._size_y = plr_resource._size_y
                # noinspection PyProtectedMember
                original_instance._size_z = plr_resource._size_z
                # noinspection PyProtectedMember
                original_instance._local_size_z = plr_resource._local_size_z
                original_instance.location = plr_resource.location
                original_instance.rotation = plr_resource.rotation
                original_instance.barcode = plr_resource.barcode
                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} 个"
                )
                if not parent_appended:
                    original_instances.append(original_instance)

            # 调用driver的update回调
            func = getattr(self.driver_instance, "resource_tree_update", None)
            if callable(func):
                func(original_instances)

            return {"success": True, "action": "update"}, original_instances

        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().trace(f"[资源同步] 处理 {action}, " 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, parents = _handle_add(plr_resources, tree_set, additional_add_params)
                        parents: List[Optional["ResourcePLR"]] = [i for i in parents if i is not None]
                        # de_dupe_parents = list(set(parents))
                        # Fix unhashable type error for WareHouse
                        de_dupe_parents = []
                        _seen_ids = set()
                        for p in parents:
                            if id(p) not in _seen_ids:
                                _seen_ids.add(id(p))
                                de_dupe_parents.append(p)
                        new_tree_set = ResourceTreeSet.from_plr_resources(de_dupe_parents)  # 去重
                        for tree in new_tree_set.trees:
                            if tree.root_node.res_content.uuid_parent is None and self.node_name != "host_node":
                                tree.root_node.res_content.parent_uuid = self.uuid
                        r = SerialCommand.Request()
                        r.command = json.dumps(
                            {"data": {"data": new_tree_set.dump()}, "action": "update"}
                        )  # 和Update Resource一致
                        response: SerialCommand_Response = await self._resource_clients[
                            "c2s_update_resource_tree"
                        ].call_async(
                            r
                        )  # type: ignore
                        self.lab_logger().trace(f"确认资源云端 Add 结果: {response.response}")
                        results.append(result)
                    elif action == "update":
                        if tree_set is None:
                            raise ValueError("tree_set不能为None")
                        plr_resources = []
                        for tree in tree_set.trees:
                            if tree.root_node.res_content.type == "device":
                                plr_resources.append(tree.root_node)
                            else:
                                plr_resources.append(ResourceTreeSet([tree]).to_plr_resources()[0])
                        result, original_instances = _handle_update(plr_resources, tree_set, additional_add_params)
                        if not BasicConfig.no_update_feedback:
                            new_tree_set = ResourceTreeSet.from_plr_resources(original_instances)  # 去重
                            for tree in new_tree_set.trees:
                                if tree.root_node.res_content.uuid_parent is None and self.node_name != "host_node":
                                    tree.root_node.res_content.parent_uuid = self.uuid
                            r = SerialCommand.Request()
                            r.command = json.dumps(
                                {"data": {"data": new_tree_set.dump()}, "action": "update"}
                            )  # 和Update Resource一致
                            response: SerialCommand_Response = await self._resource_clients[
                                "c2s_update_resource_tree"
                            ].call_async(
                                r
                            )  # type: ignore
                            self.lab_logger().trace(f"确认资源云端 Update 结果: {response.response}")
                        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"[资源同步] {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"[资源同步] {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 s2c_device_manage(self, req: SerialCommand_Request, res: SerialCommand_Response):
        """Handle add/remove device requests from HostNode via SerialCommand."""
        try:
            cmd = json.loads(req.command)
            action = cmd.get("action", "")
            data = cmd.get("data", {})
            device_id = data.get("device_id", "")

            if not device_id:
                res.response = json.dumps({"success": False, "error": "device_id required"})
                return res

            if action == "add":
                result = self.create_device(device_id, data)
            elif action == "remove":
                result = self.destroy_device(device_id)
            else:
                result = {"success": False, "error": f"Unknown action: {action}"}

            res.response = json.dumps(result, ensure_ascii=False)

        except NotImplementedError as e:
            self.lab_logger().warning(f"[DeviceManage] {e}")
            res.response = json.dumps({"success": False, "error": str(e)})
        except Exception as e:
            self.lab_logger().error(f"[DeviceManage] Error: {e}")
            res.response = json.dumps({"success": False, "error": str(e)})

        return res

    def create_device(self, device_id: str, config: "ResourceDictType") -> dict:
        """Create a sub-device dynamically. Override in HostNode / WorkstationNode."""
        raise NotImplementedError(
            f"{self.__class__.__name__} does not support dynamic device creation"
        )

    def destroy_device(self, device_id: str) -> dict:
        """Destroy a sub-device dynamically. Override in HostNode / WorkstationNode."""
        raise NotImplementedError(
            f"{self.__class__.__name__} does not support dynamic device removal"
        )

    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,
                    "registry_name": self.registry_name,
                }
            },
            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发布者。已在 status_types 中声明的属性直接创建；@topic_config 用于覆盖默认参数。"""
        topic_cfg = {}
        driver_class = type(self.driver_instance)

        # 区分 @property 和普通方法两种情况
        is_prop = hasattr(driver_class, attr_name) and isinstance(
            getattr(driver_class, attr_name), property
        )

        if is_prop:
            class_attr = getattr(driver_class, attr_name)
            if class_attr.fget is not None:
                topic_cfg = get_topic_config(class_attr.fget)
        else:
            if hasattr(self.driver_instance, attr_name):
                method = getattr(self.driver_instance, attr_name)
                if callable(method):
                    topic_cfg = get_topic_config(method)

        # 发布名称优先级: @topic_config(name=...) > get_ 前缀去除 > attr_name
        cfg_name = topic_cfg.get("name")
        if cfg_name:
            publish_name = cfg_name
        elif attr_name.startswith("get_"):
            publish_name = attr_name[4:]
        else:
            publish_name = attr_name

        # @topic_config 参数覆盖默认值
        cfg_period = topic_cfg.get("period")
        cfg_print = topic_cfg.get("print_publish")
        cfg_qos = topic_cfg.get("qos")
        period: float = cfg_period if cfg_period is not None else initial_period
        print_publish: bool = cfg_print if cfg_print is not None else self._print_publish
        qos: int = cfg_qos if cfg_qos is not None else 10

        # 获取属性值的方法
        def get_device_attr():
            try:
                if is_prop:
                    return getattr(self.driver_instance, 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[publish_name] = PropertyPublisher(
            self, publish_name, get_device_attr, msg_type, period, print_publish, qos
        )

    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]

        try:
            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,
            )
        except Exception as e:
            self.lab_logger().error(f"创建ActionServer失败，Device: {self.device_id}, Action Name: {action_name}, Action Type: {action_type}, Error: {e}")
            return
        self.lab_logger().trace(f"发布动作: {action_name}, 类型: {str_action_type}")

    def _setup_decorated_subscribers(self):
        """扫描 driver_instance 中带有 @subscribe 装饰器的方法并创建订阅者"""
        subscriptions = get_all_subscriptions(self.driver_instance)

        for method_name, method, config in subscriptions:
            topic_template = config.get("topic")
            msg_type = config.get("msg_type")
            qos = config.get("qos", 10)

            if not topic_template:
                self.lab_logger().warning(f"订阅方法 {method_name} 缺少 topic 配置，跳过")
                continue

            # 如果没有指定 msg_type，尝试从类型注解推断
            if msg_type is None:
                try:
                    hints = get_type_hints(method)
                    # 第一个参数是 self，第二个是 msg
                    param_names = list(hints.keys())
                    if param_names:
                        msg_type = hints[param_names[0]]
                except Exception:
                    pass

            if msg_type is None:
                self.lab_logger().warning(f"订阅方法 {method_name} 缺少 msg_type 配置且无法从类型注解推断，跳过")
                continue

            # 替换 topic 模板中的占位符
            topic = self._resolve_topic_template(topic_template)

            self.create_ros_subscriber(topic, msg_type, method, qos)

    def _resolve_topic_template(self, topic_template: str) -> str:
        """
        解析 topic 模板，替换占位符

        支持的占位符:
            - {device_id}: 设备ID
            - {namespace}: 完整命名空间
        """
        return topic_template.format(
            device_id=self.device_id,
            namespace=self.namespace,
        )

    def create_ros_subscriber(self, topic: str, msg_type, callback, qos: int = 10):
        """
        创建ROS订阅者

        Args:
            topic: Topic 名称
            msg_type: ROS 消息类型
            callback: 回调方法（会自动绑定到 driver_instance）
            qos: QoS 深度配置
        """
        try:
            subscription = self.create_subscription(
                msg_type,
                topic,
                callback,
                qos,
                callback_group=self.callback_group,
            )
            self._topic_subscribers[topic] = subscription
            str_msg_type = str(msg_type)[8:-2] if str(msg_type).startswith("<class") else str(msg_type)
            self.lab_logger().trace(f"订阅Topic: {topic}, 类型: {str_msg_type}, QoS: {qos}")
        except Exception as ex:
            self.lab_logger().error(f"创建订阅者 {topic} 失败，类型: {msg_type}，错误: {ex}\n{traceback.format_exc()}")

    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__} 接收到原始目标: {str(action_kwargs)[:1000]}")
            self.lab_logger().trace(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: list = [None] * len(resource_inputs)
                            uuid_indices: list[tuple[int, str, dict]] = []  # (index, uuid, resource_data)

                            # 第一遍：处理没有uuid的资源，收集有uuid的资源信息
                            for idx, resource_data in enumerate(resource_inputs):
                                unilabos_uuid = resource_data.get("data", {}).get("unilabos_uuid")
                                if unilabos_uuid is None:
                                    plr_resource = await self.get_resource_with_dir(
                                        resource_id=resource_data["id"], with_children=True
                                    )
                                    if "sample_id" in resource_data:
                                        plr_resource.unilabos_extra[EXTRA_SAMPLE_UUID] = resource_data["sample_id"]
                                    queried_resources[idx] = plr_resource
                                else:
                                    uuid_indices.append((idx, unilabos_uuid, resource_data))

                            # 第二遍：批量查询有uuid的资源
                            if uuid_indices:
                                uuids = [item[1] for item in uuid_indices]
                                resource_tree = await self.get_resource(uuids)
                                plr_resources = resource_tree.to_plr_resources()
                                for i, (idx, _, resource_data) in enumerate(uuid_indices):
                                    plr_resource = plr_resources[i]
                                    if "sample_id" in resource_data:
                                        plr_resource.unilabos_extra[EXTRA_SAMPLE_UUID] = resource_data["sample_id"]
                                    queried_resources[idx] = plr_resource

                            self.lab_logger().debug(f"资源查询结果: 共 {len(queried_resources)} 个资源")

                            # 通过资源跟踪器获取本地实例
                            final_resources = queried_resources if is_sequence else queried_resources[0]
                            if not is_sequence:
                                plr = self.resource_tracker.figure_resource(
                                    {"name": final_resources.name}, try_mode=False
                                )
                                # 保留unilabos_extra
                                if hasattr(final_resources, "unilabos_extra") and hasattr(plr, "unilabos_extra"):
                                    plr.unilabos_extra = getattr(final_resources, "unilabos_extra", {}).copy()
                                final_resources = plr
                            else:
                                new_resources = []
                                for res in queried_resources:
                                    plr = self.resource_tracker.figure_resource({"name": res.name}, try_mode=False)
                                    if hasattr(res, "unilabos_extra") and hasattr(plr, "unilabos_extra"):
                                        plr.unilabos_extra = getattr(res, "unilabos_extra", {}).copy()
                                    new_resources.append(plr)
                                final_resources = new_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

            time_start = time.time()
            time_overall = 100
            future = None
            if not error_skip:
                # 将阻塞操作放入线程池执行
                if asyncio.iscoroutinefunction(ACTION):
                    try:
                        self.lab_logger().trace(f"异步执行动作 {ACTION}")

                        def _handle_future_exception(fut: Future):
                            nonlocal execution_error, execution_success, action_return_value
                            try:
                                action_return_value = fut.result()
                                if isinstance(action_return_value, BaseException):
                                    raise action_return_value
                                execution_success = True
                            except Exception as _:
                                execution_error = traceback.format_exc()
                                error(
                                    f"异步任务 {ACTION.__name__} 报错了\n{traceback.format_exc()}\n原始输入：{str(action_kwargs)[:1000]}"
                                )
                                trace(
                                    f"异步任务 {ACTION.__name__} 报错了\n{traceback.format_exc()}\n原始输入：{action_kwargs}"
                                )

                        future = ROS2DeviceNode.run_async_func(ACTION, trace_error=False, **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().trace(f"同步执行动作 {ACTION}")
                    future = self._executor.submit(ACTION, **action_kwargs)

                    def _handle_future_exception(fut: Future):
                        nonlocal execution_error, execution_success, action_return_value
                        try:
                            action_return_value = fut.result()
                            execution_success = True
                        except Exception as _:
                            execution_error = traceback.format_exc()
                            error(
                                f"同步任务 {ACTION.__name__} 报错了\n{traceback.format_exc()}\n原始输入：{str(action_kwargs)[:1000]}"
                            )
                            trace(
                                f"同步任务 {ACTION.__name__} 报错了\n{traceback.format_exc()}\n原始输入：{action_kwargs}"
                            )

                    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的类型
                            if isinstance(rs, list):
                                for r in rs:
                                    res = self.resource_tracker.parent_resource(r)  # 获取 resource 对象
                                    if res is None:
                                        res = rs
                                    if id(res) not in seen:
                                        seen.add(id(res))
                                        unique_resources.append(res)
                            else:
                                res = self.resource_tracker.parent_resource(rs)
                                if res is None:
                                    res = rs
                                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().trace(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"]
            # 获取 unilabos 系统参数
            unilabos_param: Dict[str, Any] = target[JSON_UNILABOS_PARAM]

            assert isinstance(function_args, dict), "执行动作时JSON必须为dict类型\n原JSON: {string}"
            function = getattr(self.driver_instance, function_name)
            assert callable(
                function
            ), f"执行动作时JSON中的function_name对应的函数不可调用: {function_name}\n原JSON: {string}"

            # 处理参数（包含 unilabos 系统参数如 sample_uuids）
            args_list = default_manager._analyze_method_signature(function, skip_unilabos_params=False)["args"]
            for arg in args_list:
                arg_name = arg["name"]
                arg_type = arg["type"]

                # 跳过不在 function_args 中的参数
                if arg_name not in function_args:
                    # 处理 sample_uuids 参数注入
                    if arg_name == PARAM_SAMPLE_UUIDS:
                        raw_sample_uuids = unilabos_param.get(PARAM_SAMPLE_UUIDS, {})
                        # 将 material uuid 转换为 resource 实例
                        # key: sample_uuid, value: material_uuid -> resource 实例
                        resolved_sample_uuids: Dict[str, Any] = {}
                        for sample_uuid, material_uuid in raw_sample_uuids.items():
                            if material_uuid and self.resource_tracker:
                                resource = self.resource_tracker.uuid_to_resources.get(material_uuid)
                                resolved_sample_uuids[sample_uuid] = resource if resource else material_uuid
                            else:
                                resolved_sample_uuids[sample_uuid] = material_uuid
                        function_args[PARAM_SAMPLE_UUIDS] = resolved_sample_uuids
                        self.lab_logger().debug(f"[JsonCommand] 注入 {PARAM_SAMPLE_UUIDS}: {resolved_sample_uuids}")
                    continue

                # 处理单个 ResourceSlot
                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:
                            function_args[arg_name] = self._convert_resources_sync(resource_data["uuid"])[0]
                        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:
                                uuids = [r["uuid"] for r in resource_list if isinstance(r, dict) and "id" in r]
                                function_args[arg_name] = self._convert_resources_sync(*uuids) if uuids else []
                            except Exception as e:
                                self.lab_logger().error(
                                    f"转换ResourceSlot列表参数 {arg_name} 失败: {e}\n{traceback.format_exc()}"
                                )
                                raise JsonCommandInitError(f"ResourceSlot列表参数转换失败: {arg_name}")

            # todo: 默认反报送
            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_resources_sync(self, *uuids: str) -> List["ResourcePLR"]:
        """同步转换资源 UUID 为实例

        Args:
            *uuids: 一个或多个资源 UUID

        Returns:
            单个 UUID 时返回单个资源实例，多个 UUID 时返回资源实例列表
        """
        if not uuids:
            raise ValueError("至少需要提供一个 UUID")

        uuids_list = list(uuids)
        future = self._resource_clients["c2s_update_resource_tree"].call_async(
            SerialCommand.Request(
                command=json.dumps(
                    {
                        "data": {"data": uuids_list, "with_children": True},
                        "action": "get",
                    }
                )
            )
        )

        # 等待结果（使用while循环，每次sleep 0.05秒，最多等待30秒）
        timeout = 30.0
        elapsed = 0.0
        while not future.done() and elapsed < timeout:
            time.sleep(0.02)
            elapsed += 0.02

        if not future.done():
            raise Exception(f"资源查询超时: {uuids_list}")

        response = future.result()
        if response is None:
            raise Exception(f"资源查询返回空结果: {uuids_list}")

        raw_data = json.loads(response.response)

        # 转换为 PLR 资源
        tree_set = ResourceTreeSet.from_raw_dict_list(raw_data)
        if not len(tree_set.trees):
            raise Exception(f"资源查询返回空树: {raw_data}")
        plr_resources = tree_set.to_plr_resources()

        # 通过资源跟踪器获取本地实例
        figured_resources: List[ResourcePLR] = []
        for plr_resource, tree in zip(plr_resources, tree_set.trees):
            res = self.resource_tracker.figure_resource(plr_resource, try_mode=True)
            if len(res) == 0:
                self.lab_logger().warning(f"资源转换未能索引到实例: {tree.root_node.res_content}，返回新建实例")
                figured_resources.append(plr_resource)
            elif len(res) == 1:
                figured_resources.append(res[0])
            else:
                raise ValueError(f"资源转换得到多个实例: {res}")

        mapped_plr_resources = []
        for uuid in uuids_list:
            for plr_resource in figured_resources:
                r = self.resource_tracker.loop_find_with_uuid(plr_resource, uuid)
                mapped_plr_resources.append(r)
                break

        return mapped_plr_resources

    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"]
            # 获取 unilabos 系统参数
            unilabos_param: Dict[str, Any] = target.get(JSON_UNILABOS_PARAM, {})

            assert isinstance(function_args, dict), "执行动作时JSON必须为dict类型\n原JSON: {string}"
            function = getattr(self.driver_instance, function_name)
            assert callable(
                function
            ), f"执行动作时JSON中的function_name对应的函数不可调用: {function_name}\n原JSON: {string}"
            assert asyncio.iscoroutinefunction(
                function
            ), f"执行动作时JSON中的function并非异步: {function_name}\n原JSON: {string}"

            # 处理参数（包含 unilabos 系统参数如 sample_uuids）
            args_list = default_manager._analyze_method_signature(function, skip_unilabos_params=False)["args"]
            for arg in args_list:
                arg_name = arg["name"]
                arg_type = arg["type"]

                # 跳过不在 function_args 中的参数
                if arg_name not in function_args:
                    # 处理 sample_uuids 参数注入
                    if arg_name == PARAM_SAMPLE_UUIDS:
                        raw_sample_uuids = unilabos_param.get(PARAM_SAMPLE_UUIDS, {})
                        # 将 material uuid 转换为 resource 实例
                        # key: sample_uuid, value: material_uuid -> resource 实例
                        resolved_sample_uuids: Dict[str, Any] = {}
                        for sample_uuid, material_uuid in raw_sample_uuids.items():
                            if material_uuid and self.resource_tracker:
                                resource = self.resource_tracker.uuid_to_resources.get(material_uuid)
                                resolved_sample_uuids[sample_uuid] = resource if resource else material_uuid
                            else:
                                resolved_sample_uuids[sample_uuid] = material_uuid
                        function_args[PARAM_SAMPLE_UUIDS] = resolved_sample_uuids
                        self.lab_logger().debug(
                            f"[JsonCommandAsync] 注入 {PARAM_SAMPLE_UUIDS}: {resolved_sample_uuids}"
                        )
                    continue

                # 处理单个 ResourceSlot
                _is_resource_slot = isinstance(arg_type, str) and arg_type.endswith(":ResourceSlot")
                if _is_resource_slot:
                    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:
                    if arg_type[0] == "list" and isinstance(arg_type[1], str) and arg_type[1].endswith(":ResourceSlot"):
                        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接口。
    它不继承设备类，而是通过代理模式访问设备类的属性和方法。
    """

    # 类变量，用于循环管理
    _asyncio_loop = None
    _asyncio_loop_running = False
    _asyncio_loop_thread = None

    @classmethod
    def get_asyncio_loop(cls):
        return cls._asyncio_loop

    @staticmethod
    async def safe_task_wrapper(trace_callback, func, **kwargs):
        try:
            if callable(trace_callback):
                trace_callback(await func(**kwargs))
            return await func(**kwargs)
        except Exception as e:
            if callable(trace_callback):
                trace_callback(e)
            return e

    @classmethod
    def run_async_func(cls, func, trace_error=True, inner_trace_callback=None, **kwargs) -> Task:
        def _handle_future_exception(fut: Future):
            try:
                ret = fut.result()
                if isinstance(ret, BaseException):
                    raise ret
            except Exception as e:
                error(f"异步任务 {func.__name__} 获取结果失败")
                error(traceback.format_exc())

        future = rclpy.get_global_executor().create_task(
            ROS2DeviceNode.safe_task_wrapper(inner_trace_callback, 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:
        """
        # 在初始化时检查循环状态
        if ROS2DeviceNode._asyncio_loop_running and ROS2DeviceNode._asyncio_loop_thread is not None:
            pass
        elif ROS2DeviceNode._asyncio_loop_thread is None:
            self._start_loop()

        # 保存设备类是否支持异步上下文
        self._has_async_context = hasattr(driver_class, "__aenter__") and hasattr(driver_class, "__aexit__")
        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"
            or driver_class.__name__ == "OpcUaClient"
        )

        # 创建设备类实例
        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["registry_name"] = device_config.res_content.klass
            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,
                registry_name=device_config.res_content.klass,
                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,
                registry_name=device_config.res_content.klass,
                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
        # 将注册表类型名传递给BaseROS2DeviceNode,用于slave上报
        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}")

    def _start_loop(self):
        def run_event_loop():
            loop = asyncio.new_event_loop()
            ROS2DeviceNode._asyncio_loop = loop
            asyncio.set_event_loop(loop)
            loop.run_forever()

        ROS2DeviceNode._asyncio_loop_thread = threading.Thread(
            target=run_event_loop, daemon=True, name="ROS2DeviceNode"
        )
        ROS2DeviceNode._asyncio_loop_thread.start()
        logger.info(f"循环线程已启动")


class DeviceInfoType(TypedDict):
    id: str
    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
