Merge dev branch: Add battery resources, bioyond_cell device registry, and fix file path resolution

unilabos/ros/nodes/base_device_node.py

@@ -5,7 +5,6 @@ import json
 import threading
 import time
 import traceback
-import uuid
 from typing import get_type_hints, TypeVar, Generic, Dict, Any, Type, TypedDict, Optional, List, TYPE_CHECKING, Union
 
 from concurrent.futures import ThreadPoolExecutor
@@ -39,7 +38,6 @@ from unilabos.ros.msgs.message_converter import (
 )
 from unilabos_msgs.srv import (
     ResourceAdd,
-    ResourceGet,
     ResourceDelete,
     ResourceUpdate,
     ResourceList,
@@ -49,7 +47,8 @@ from unilabos_msgs.msg import Resource  # type: ignore
 
 from unilabos.ros.nodes.resource_tracker import (
     DeviceNodeResourceTracker,
-    ResourceTreeSet, ResourceTreeInstance,
+    ResourceTreeSet,
+    ResourceTreeInstance, ResourceDictInstance,
 )
 from unilabos.ros.x.rclpyx import get_event_loop
 from unilabos.ros.utils.driver_creator import WorkstationNodeCreator, PyLabRobotCreator, DeviceClassCreator
@@ -134,12 +133,11 @@ def init_wrapper(
     device_id: str,
     device_uuid: str,
     driver_class: type[T],
-    device_config: Dict[str, Any],
+    device_config: ResourceTreeInstance,
     status_types: Dict[str, Any],
     action_value_mappings: Dict[str, Any],
     hardware_interface: Dict[str, Any],
     print_publish: bool,
-    children: Optional[list] = None,
     driver_params: Optional[Dict[str, Any]] = None,
     driver_is_ros: bool = False,
     *args,
@@ -148,8 +146,6 @@ def init_wrapper(
     """初始化设备节点的包装函数，和ROS2DeviceNode初始化保持一致"""
     if driver_params is None:
         driver_params = kwargs.copy()
-    if children is None:
-        children = []
     kwargs["device_id"] = device_id
     kwargs["device_uuid"] = device_uuid
     kwargs["driver_class"] = driver_class
@@ -158,7 +154,6 @@ def init_wrapper(
     kwargs["status_types"] = status_types
     kwargs["action_value_mappings"] = action_value_mappings
     kwargs["hardware_interface"] = hardware_interface
-    kwargs["children"] = children
     kwargs["print_publish"] = print_publish
     kwargs["driver_is_ros"] = driver_is_ros
     super(type(self), self).__init__(*args, **kwargs)
@@ -340,10 +335,16 @@ class BaseROS2DeviceNode(Node, Generic[T]):
         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_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),
+            "c2s_update_resource_tree": self.create_client(
+                SerialCommand, "/c2s_update_resource_tree", callback_group=self.callback_group
+            ),
         }
 
         def re_register_device(req, res):
@@ -466,8 +467,9 @@ class BaseROS2DeviceNode(Node, Generic[T]):
                 contain_model = not isinstance(resource, Deck)
                 if isinstance(resource, ResourcePLR):
                     # resources.list()
-                    resources_tree = dict_to_tree(copy.deepcopy({r["id"]: r for r in resources}))
-                    plr_instance = resource_ulab_to_plr(resources_tree[0], contain_model)
+                    plr_instance = ResourceTreeSet.from_raw_list(resources).to_plr_resources()[0]
+                    # resources_tree = dict_to_tree(copy.deepcopy({r["id"]: r for r in resources}))
+                    # plr_instance = resource_ulab_to_plr(resources_tree[0], contain_model)
 
                     if isinstance(plr_instance, Plate):
                         empty_liquid_info_in = [(None, 0)] * plr_instance.num_items
@@ -580,9 +582,66 @@ class BaseROS2DeviceNode(Node, Generic[T]):
         except Exception as e:
             self.lab_logger().error(f"更新资源uuid失败: {e}")
             self.lab_logger().error(traceback.format_exc())
-        self.lab_logger().debug(f"资源更新结果: {response}")
+        self.lab_logger().trace(f"资源更新结果: {response}")
 
-    def transfer_to_new_resource(self, plr_resource: "ResourcePLR", tree: ResourceTreeInstance, additional_add_params: Dict[str, Any]):
+    async def get_resource(self, resources_uuid: List[str], with_children: bool = True) -> ResourceTreeSet:
+        """
+        根据资源UUID列表获取资源树
+
+        Args:
+            resources_uuid: 资源UUID列表
+            with_children: 是否包含子节点，默认为True
+
+        Returns:
+            ResourceTreeSet: 资源树集合
+        """
+        response: SerialCommand.Response = await self._resource_clients["c2s_update_resource_tree"].call_async(
+            SerialCommand.Request(
+                command=json.dumps(
+                    {
+                        "data": {"data": resources_uuid, "with_children": with_children},
+                        "action": "get",
+                    }
+                )
+            )
+        )  # type: ignore
+        raw_nodes = json.loads(response.response)
+        tree_set = ResourceTreeSet.from_raw_list(raw_nodes)
+        self.lab_logger().debug(f"获取资源结果: {len(tree_set.trees)} 个资源树")
+        return tree_set
+
+    async def get_resource_with_dir(self, resource_id: str, with_children: bool = True) -> "ResourcePLR":
+        """
+        根据资源ID获取单个资源实例
+
+        Args:
+            resource_ids: 资源ID字符串
+            with_children: 是否包含子节点，默认为True
+
+        Returns:
+            ResourcePLR: PLR资源实例
+        """
+        r = SerialCommand.Request()
+        r.command = json.dumps(
+            {
+                "id": resource_id,
+                "uuid": None,
+                "with_children": with_children,
+            }
+        )
+        # 发送请求并等待响应
+        response: SerialCommand_Response = await self._resource_clients["resource_get"].call_async(r)
+        raw_data = json.loads(response.response)
+
+        # 转换为 PLR 资源
+        tree_set = ResourceTreeSet.from_raw_list(raw_data)
+        plr_resource = tree_set.to_plr_resources()[0]
+        self.lab_logger().debug(f"获取资源 {resource_id} 成功")
+        return plr_resource
+
+    def transfer_to_new_resource(
+        self, plr_resource: "ResourcePLR", tree: ResourceTreeInstance, additional_add_params: Dict[str, Any]
+    ):
         parent_uuid = tree.root_node.res_content.parent_uuid
         if parent_uuid:
             parent_resource: ResourcePLR = self.resource_tracker.uuid_to_resources.get(parent_uuid)
@@ -609,16 +668,26 @@ class BaseROS2DeviceNode(Node, Generic[T]):
                     old_parent = plr_resource.parent
                     if old_parent is not None:
                         # plr并不支持同一个deck的加载和卸载
-                        self.lab_logger().warning(
-                            f"物料{plr_resource}请求从{old_parent}卸载"
-                        )
+                        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}"
                     )
-                    parent_resource.assign_child_resource(
-                        plr_resource, location=None, **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了）
@@ -790,17 +859,9 @@ class BaseROS2DeviceNode(Node, Generic[T]):
                 )
                 tree_set = None
                 if action in ["add", "update"]:
-                    response: SerialCommand.Response = await self._resource_clients[
-                        "c2s_update_resource_tree"
-                    ].call_async(
-                        SerialCommand.Request(
-                            command=json.dumps(
-                                {"data": {"data": resources_uuid, "with_children": True if action == "add" else False}, "action": "get"}
-                            )
-                        )
-                    )  # type: ignore
-                    raw_nodes = json.loads(response.response)
-                    tree_set = ResourceTreeSet.from_raw_list(raw_nodes)
+                    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:
@@ -1078,17 +1139,9 @@ class BaseROS2DeviceNode(Node, Generic[T]):
                             # 批量查询资源
                             queried_resources = []
                             for resource_data in resource_inputs:
-                                r = SerialCommand.Request()
-                                r.command = json.dumps({"id": resource_data["id"], "uuid": resource_data.get("uuid", None), "with_children": True})
-                                # 发送请求并等待响应
-                                response: SerialCommand_Response = await self._resource_clients[
-                                    "resource_get"
-                                ].call_async(r)
-                                raw_data = json.loads(response.response)
-
-                                # 转换为 PLR 资源
-                                tree_set = ResourceTreeSet.from_raw_list(raw_data)
-                                plr_resource = tree_set.to_plr_resources()[0]
+                                plr_resource = await self.get_resource_with_dir(
+                                    resource_id=resource_data["id"], with_children=True
+                                )
                                 queried_resources.append(plr_resource)
 
                             self.lab_logger().debug(f"资源查询结果: 共 {len(queried_resources)} 个资源")
@@ -1111,7 +1164,6 @@ class BaseROS2DeviceNode(Node, Generic[T]):
                             execution_error = traceback.format_exc()
                             break
 
-            ##### self.lab_logger().info(f"准备执行: {action_kwargs}, 函数: {ACTION.__name__}")
             time_start = time.time()
             time_overall = 100
             future = None
@@ -1119,35 +1171,36 @@ class BaseROS2DeviceNode(Node, Generic[T]):
                 # 将阻塞操作放入线程池执行
                 if asyncio.iscoroutinefunction(ACTION):
                     try:
-                        ##### self.lab_logger().info(f"异步执行动作 {ACTION}")
-                        future = ROS2DeviceNode.run_async_func(ACTION, trace_error=False, **action_kwargs)
-
-                        def _handle_future_exception(fut):
+                        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 e:
+                            except Exception as _:
                                 execution_error = traceback.format_exc()
                                 error(
                                     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().info(f"同步执行动作 {ACTION}")
+                    self.lab_logger().trace(f"同步执行动作 {ACTION}")
                     future = self._executor.submit(ACTION, **action_kwargs)
 
-                    def _handle_future_exception(fut):
+                    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 e:
+                        except Exception as _:
                             execution_error = traceback.format_exc()
                             error(
                                 f"同步任务 {ACTION.__name__} 报错了\n{traceback.format_exc()}\n原始输入：{action_kwargs}"
@@ -1252,7 +1305,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
                         get_result_info_str(execution_error, execution_success, action_return_value),
                     )
 
-            ##### self.lab_logger().info(f"动作 {action_name} 完成并返回结果")
+            self.lab_logger().trace(f"动作 {action_name} 完成并返回结果")
             return result_msg
 
         return execute_callback
@@ -1328,12 +1381,18 @@ class BaseROS2DeviceNode(Node, Generic[T]):
         """同步转换资源数据为实例"""
         # 创建资源查询请求
         r = SerialCommand.Request()
-        r.command = json.dumps({"id": resource_data["id"], "with_children": True})
+        r.command = json.dumps(
+            {
+                "id": resource_data.get("id", None),
+                "uuid": resource_data.get("uuid", None),
+                "with_children": True,
+            }
+        )
 
         # 同步调用资源查询服务
         future = self._resource_clients["resource_get"].call_async(r)
 
-        # 等待结果（使用while循环，每次sleep 0.5秒，最多等待5秒）
+        # 等待结果（使用while循环，每次sleep 0.05秒，最多等待30秒）
         timeout = 30.0
         elapsed = 0.0
         while not future.done() and elapsed < timeout:
@@ -1341,16 +1400,16 @@ class BaseROS2DeviceNode(Node, Generic[T]):
             elapsed += 0.05
 
         if not future.done():
-            raise Exception(f"资源查询超时: {resource_data['id']}")
+            raise Exception(f"资源查询超时: {resource_data}")
 
         response = future.result()
         if response is None:
-            raise Exception(f"资源查询返回空结果: {resource_data['id']}")
+            raise Exception(f"资源查询返回空结果: {resource_data}")
 
-        current_resources = json.loads(response.response)
+        raw_data = json.loads(response.response)
 
         # 转换为 PLR 资源
-        tree_set = ResourceTreeSet.from_raw_list(current_resources)
+        tree_set = ResourceTreeSet.from_raw_list(raw_data)
         plr_resource = tree_set.to_plr_resources()[0]
 
         # 通过资源跟踪器获取本地实例
@@ -1435,17 +1494,8 @@ class BaseROS2DeviceNode(Node, Generic[T]):
 
     async def _convert_resource_async(self, resource_data: Dict[str, Any]):
         """异步转换资源数据为实例"""
-        # 创建资源查询请求
-        r = SerialCommand.Request()
-        r.command = json.dumps({"id": resource_data["id"], "with_children": True})
-
-        # 异步调用资源查询服务
-        response: SerialCommand_Response = await self._resource_clients["resource_get"].call_async(r)
-        current_resources = json.loads(response.response)
-
-        # 转换为 PLR 资源
-        tree_set = ResourceTreeSet.from_raw_list(current_resources)
-        plr_resource = tree_set.to_plr_resources()[0]
+        # 使用封装的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)
@@ -1490,17 +1540,29 @@ class ROS2DeviceNode:
     这个类封装了设备类实例和ROS2节点的功能，提供ROS2接口。
     它不继承设备类，而是通过代理模式访问设备类的属性和方法。
     """
+    @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, **kwargs) -> Task:
-        def _handle_future_exception(fut):
+    def run_async_func(cls, func, trace_error=True, inner_trace_callback=None, **kwargs) -> Task:
+        def _handle_future_exception(fut: Future):
             try:
-                fut.result()
+                ret = fut.result()
+                if isinstance(ret, BaseException):
+                    raise ret
             except Exception as e:
-                error(f"异步任务 {func.__name__} 报错了")
+                error(f"异步任务 {func.__name__} 获取结果失败")
                 error(traceback.format_exc())
 
-        future = rclpy.get_global_executor().create_task(func(**kwargs))
+        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
@@ -1508,7 +1570,9 @@ class ROS2DeviceNode:
     @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())
+        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)
@@ -1526,12 +1590,11 @@ class ROS2DeviceNode:
         device_id: str,
         device_uuid: str,
         driver_class: Type[T],
-        device_config: Dict[str, Any],
+        device_config: ResourceDictInstance,
         driver_params: Dict[str, Any],
         status_types: Dict[str, Any],
         action_value_mappings: Dict[str, Any],
         hardware_interface: Dict[str, Any],
-        children: Dict[str, Any],
         print_publish: bool = True,
         driver_is_ros: bool = False,
     ):
@@ -1542,7 +1605,7 @@ class ROS2DeviceNode:
             device_id: 设备标识符
             device_uuid: 设备uuid
             driver_class: 设备类
-            device_config: 原始初始化的json
+            device_config: 原始初始化的ResourceDictInstance
             driver_params: driver初始化的参数
             status_types: 状态类型映射
             action_value_mappings: 动作值映射
@@ -1556,6 +1619,7 @@ class ROS2DeviceNode:
         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()
@@ -1566,6 +1630,7 @@ class ROS2DeviceNode:
             or driver_class.__name__ == "LiquidHandlerAbstract"
             or driver_class.__name__ == "LiquidHandlerBiomek"
             or driver_class.__name__ == "PRCXI9300Handler"
+            or driver_class.__name__ == "TransformXYZHandler"
         )
 
         # 创建设备类实例

unilabos/ros/nodes/presets/camera.py

@@ -6,12 +6,13 @@ from cv_bridge import CvBridge
 from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode, DeviceNodeResourceTracker
 
 class VideoPublisher(BaseROS2DeviceNode):
-    def __init__(self, device_id='video_publisher', camera_index=0, period: float = 0.1, resource_tracker: DeviceNodeResourceTracker = None):
+    def __init__(self, device_id='video_publisher', device_uuid='', camera_index=0, period: float = 0.1, resource_tracker: DeviceNodeResourceTracker = None):
         # 初始化BaseROS2DeviceNode，使用自身作为driver_instance
         BaseROS2DeviceNode.__init__(
             self,
             driver_instance=self,
             device_id=device_id,
+            device_uuid=device_uuid,
             status_types={},
             action_value_mappings={},
             hardware_interface="camera",

unilabos/ros/nodes/presets/host_node.py

@@ -5,7 +5,7 @@ import threading
 import time
 import traceback
 import uuid
-from typing import TYPE_CHECKING, Optional, Dict, Any, List, ClassVar, Set, Union
+from typing import TYPE_CHECKING, Optional, Dict, Any, List, ClassVar, Set, TypedDict, Union
 
 from action_msgs.msg import GoalStatus
 from geometry_msgs.msg import Point
@@ -38,6 +38,7 @@ from unilabos.ros.msgs.message_converter import (
 from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode, ROS2DeviceNode, DeviceNodeResourceTracker
 from unilabos.ros.nodes.presets.controller_node import ControllerNode
 from unilabos.ros.nodes.resource_tracker import (
+    ResourceDict,
     ResourceDictInstance,
     ResourceTreeSet,
     ResourceTreeInstance,
@@ -48,7 +49,7 @@ from unilabos.utils.type_check import serialize_result_info
 from unilabos.registry.placeholder_type import ResourceSlot, DeviceSlot
 
 if TYPE_CHECKING:
-    from unilabos.app.ws_client import QueueItem, WSResourceChatData
+    from unilabos.app.ws_client import QueueItem
 
 
 @dataclass
@@ -56,6 +57,11 @@ class DeviceActionStatus:
     job_ids: Dict[str, float] = field(default_factory=dict)
 
 
+class TestResourceReturn(TypedDict):
+    resources: List[List[ResourceDict]]
+    devices: List[DeviceSlot]
+
+
 class HostNode(BaseROS2DeviceNode):
     """
     主机节点类，负责管理设备、资源和控制器
@@ -283,6 +289,12 @@ class HostNode(BaseROS2DeviceNode):
         self.lab_logger().info("[Host Node] Host node initialized.")
         HostNode._ready_event.set()
 
+        # 发送host_node ready信号到所有桥接器
+        for bridge in self.bridges:
+            if hasattr(bridge, "publish_host_ready"):
+                bridge.publish_host_ready()
+                self.lab_logger().debug(f"Host ready signal sent via {bridge.__class__.__name__}")
+
     def _send_re_register(self, sclient):
         sclient.wait_for_service()
         request = SerialCommand.Request()
@@ -526,7 +538,7 @@ class HostNode(BaseROS2DeviceNode):
         self.lab_logger().info(f"[Host Node] Initializing device: {device_id}")
 
         try:
-            d = initialize_device_from_dict(device_id, device_config.get_nested_dict())
+            d = initialize_device_from_dict(device_id, device_config)
         except DeviceClassInvalid as e:
             self.lab_logger().error(f"[Host Node] Device class invalid: {e}")
             d = None
@@ -706,7 +718,7 @@ class HostNode(BaseROS2DeviceNode):
             feedback_callback=lambda feedback_msg: self.feedback_callback(item, action_id, feedback_msg),
             goal_uuid=goal_uuid_obj,
         )
-        future.add_done_callback(lambda future: self.goal_response_callback(item, action_id, future))
+        future.add_done_callback(lambda f: self.goal_response_callback(item, action_id, f))
 
     def goal_response_callback(self, item: "QueueItem", action_id: str, future) -> None:
         """目标响应回调"""
@@ -717,9 +729,11 @@ class HostNode(BaseROS2DeviceNode):
 
         self.lab_logger().info(f"[Host Node] Goal {action_id} ({item.job_id}) accepted")
         self._goals[item.job_id] = goal_handle
-        goal_handle.get_result_async().add_done_callback(
-            lambda future: self.get_result_callback(item, action_id, future)
+        goal_future = goal_handle.get_result_async()
+        goal_future.add_done_callback(
+            lambda f: self.get_result_callback(item, action_id, f)
         )
+        goal_future.result()
 
     def feedback_callback(self, item: "QueueItem", action_id: str, feedback_msg) -> None:
         """反馈回调"""
@@ -734,46 +748,133 @@ class HostNode(BaseROS2DeviceNode):
     def get_result_callback(self, item: "QueueItem", action_id: str, future) -> None:
         """获取结果回调"""
         job_id = item.job_id
-        result_msg = future.result().result
-        result_data = convert_from_ros_msg(result_msg)
-        status = "success"
-        return_info_str = result_data.get("return_info")
-        if return_info_str is not None:
-            try:
-                return_info = json.loads(return_info_str)
-                suc = return_info.get("suc", False)
-                if not suc:
-                    status = "failed"
-            except json.JSONDecodeError:
+
+        try:
+            result = future.result()
+            result_msg = result.result
+            goal_status = result.status
+
+            # 检查是否是被取消的任务
+            if goal_status == GoalStatus.STATUS_CANCELED:
+                self.lab_logger().info(f"[Host Node] Goal {action_id} ({job_id[:8]}) was cancelled")
                 status = "failed"
-                return_info = serialize_result_info("", False, result_data)
-                self.lab_logger().critical("错误的return_info类型，请断点修复")
-        else:
-            # 无 return_info 字段时，回退到 success 字段（若存在）
-            suc_field = result_data.get("success")
-            if isinstance(suc_field, bool):
-                status = "success" if suc_field else "failed"
-                return_info = serialize_result_info("", suc_field, result_data)
+                return_info = serialize_result_info("Job was cancelled", False, {})
             else:
-                # 最保守的回退：标记失败并返回空JSON
-                status = "failed"
-                return_info = serialize_result_info("缺少return_info", False, result_data)
+                result_data = convert_from_ros_msg(result_msg)
+                status = "success"
+                return_info_str = result_data.get("return_info")
+                if return_info_str is not None:
+                    try:
+                        return_info = json.loads(return_info_str)
+                        # 适配后端的一些额外处理
+                        return_value = return_info.get("return_value")
+                        if isinstance(return_value, dict):
+                            unilabos_samples = return_info.get("unilabos_samples")
+                            if isinstance(unilabos_samples, list):
+                                return_info["unilabos_samples"] = unilabos_samples
+                        suc = return_info.get("suc", False)
+                        if not suc:
+                            status = "failed"
+                    except json.JSONDecodeError:
+                        status = "failed"
+                        return_info = serialize_result_info("", False, result_data)
+                        self.lab_logger().critical("错误的return_info类型，请断点修复")
+                else:
+                    # 无 return_info 字段时，回退到 success 字段（若存在）
+                    suc_field = result_data.get("success")
+                    if isinstance(suc_field, bool):
+                        status = "success" if suc_field else "failed"
+                        return_info = serialize_result_info("", suc_field, result_data)
+                    else:
+                        # 最保守的回退：标记失败并返回空JSON
+                        status = "failed"
+                        return_info = serialize_result_info("缺少return_info", False, result_data)
 
-        self.lab_logger().info(f"[Host Node] Result for {action_id} ({job_id}): {status}")
-        self.lab_logger().debug(f"[Host Node] Result data: {result_data}")
+            self.lab_logger().info(f"[Host Node] Result for {action_id} ({job_id[:8]}): {status}")
+            if goal_status != GoalStatus.STATUS_CANCELED:
+                self.lab_logger().debug(f"[Host Node] Result data: {result_data}")
 
-        if job_id:
+            # 清理 _goals 中的记录
+            if job_id in self._goals:
+                del self._goals[job_id]
+                self.lab_logger().debug(f"[Host Node] Removed goal {job_id[:8]} from _goals")
+
+            # 存储结果供 HTTP API 查询
+            try:
+                from unilabos.app.web.controller import store_job_result
+
+                if goal_status == GoalStatus.STATUS_CANCELED:
+                    store_job_result(job_id, status, return_info, {})
+                else:
+                    store_job_result(job_id, status, return_info, result_data)
+            except ImportError:
+                pass  # controller 模块可能未加载
+
+            # 发布状态到桥接器
+            if job_id:
+                for bridge in self.bridges:
+                    if hasattr(bridge, "publish_job_status"):
+                        if goal_status == GoalStatus.STATUS_CANCELED:
+                            bridge.publish_job_status({}, item, status, return_info)
+                        else:
+                            bridge.publish_job_status(result_data, item, status, return_info)
+
+        except Exception as e:
+            self.lab_logger().error(
+                f"[Host Node] Error in get_result_callback for {action_id} ({job_id[:8]}): {str(e)}"
+            )
+            import traceback
+
+            self.lab_logger().error(traceback.format_exc())
+
+            # 清理 _goals 中的记录
+            if job_id in self._goals:
+                del self._goals[job_id]
+
+            # 发布失败状态
             for bridge in self.bridges:
                 if hasattr(bridge, "publish_job_status"):
-                    bridge.publish_job_status(result_data, item, status, return_info)
+                    bridge.publish_job_status(
+                        {}, item, "failed", serialize_result_info(f"Callback error: {str(e)}", False, {})
+                    )
 
-    def cancel_goal(self, goal_uuid: str) -> None:
-        """取消目标"""
+    def cancel_goal(self, goal_uuid: str) -> bool:
+        """
+        取消目标
+
+        Args:
+            goal_uuid: 目标UUID（job_id）
+
+        Returns:
+            bool: 如果找到目标并发起取消请求返回True，否则返回False
+        """
         if goal_uuid in self._goals:
-            self.lab_logger().info(f"[Host Node] Cancelling goal {goal_uuid}")
-            self._goals[goal_uuid].cancel_goal_async()
+            self.lab_logger().info(f"[Host Node] Cancelling goal {goal_uuid[:8]}")
+            goal_handle = self._goals[goal_uuid]
+
+            # 发起异步取消请求
+            cancel_future = goal_handle.cancel_goal_async()
+
+            # 添加取消完成的回调
+            cancel_future.add_done_callback(lambda future: self._cancel_goal_callback(goal_uuid, future))
+            return True
         else:
-            self.lab_logger().warning(f"[Host Node] Goal {goal_uuid} not found, cannot cancel")
+            self.lab_logger().warning(f"[Host Node] Goal {goal_uuid[:8]} not found in _goals, cannot cancel")
+            return False
+
+    def _cancel_goal_callback(self, goal_uuid: str, future) -> None:
+        """取消目标的回调"""
+        try:
+            cancel_response = future.result()
+            if cancel_response.goals_canceling:
+                self.lab_logger().info(f"[Host Node] Goal {goal_uuid[:8]} cancel request accepted")
+            else:
+                self.lab_logger().warning(f"[Host Node] Goal {goal_uuid[:8]} cancel request rejected")
+        except Exception as e:
+            self.lab_logger().error(f"[Host Node] Error cancelling goal {goal_uuid[:8]}: {str(e)}")
+            import traceback
+
+            self.lab_logger().error(traceback.format_exc())
 
     def get_goal_status(self, job_id: str) -> int:
         """获取目标状态"""
@@ -1056,11 +1157,12 @@ class HostNode(BaseROS2DeviceNode):
             响应对象，包含查询到的资源
         """
         try:
+            from unilabos.app.web import http_client
             data = json.loads(request.command)
             if "uuid" in data and data["uuid"] is not None:
-                http_req = self.bridges[-1].resource_tree_get([data["uuid"]], data["with_children"])
+                http_req = http_client.resource_tree_get([data["uuid"]], data["with_children"])
             elif "id" in data and data["id"].startswith("/"):
-                http_req = self.bridges[-1].resource_get(data["id"], data["with_children"])
+                http_req = http_client.resource_get(data["id"], data["with_children"])
             else:
                 raise ValueError("没有使用正确的物料 id 或 uuid")
             response.response = json.dumps(http_req["data"])
@@ -1270,7 +1372,7 @@ class HostNode(BaseROS2DeviceNode):
 
     def test_resource(
         self, resource: ResourceSlot, resources: List[ResourceSlot], device: DeviceSlot, devices: List[DeviceSlot]
-    ):
+    ) -> TestResourceReturn:
         return {
             "resources": ResourceTreeSet.from_plr_resources([resource, *resources]).dump(),
             "devices": [device, *devices],

unilabos/ros/nodes/presets/joint_republisher.py

@@ -1,3 +1,4 @@
+import uuid
 import rclpy,json
 from rclpy.node import Node
 from sensor_msgs.msg import JointState 
@@ -6,7 +7,7 @@ from rclpy.callback_groups import ReentrantCallbackGroup
 from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode
 
 class JointRepublisher(BaseROS2DeviceNode):
-    def __init__(self,device_id,resource_tracker):
+    def __init__(self,device_id,resource_tracker, **kwargs):
         super().__init__(
             driver_instance=self,
             device_id=device_id,
@@ -15,6 +16,7 @@ class JointRepublisher(BaseROS2DeviceNode):
             hardware_interface={},
             print_publish=False,
             resource_tracker=resource_tracker,  
+            device_uuid=kwargs.get("uuid", str(uuid.uuid4())),
         )  
         
         # print('-'*20,device_id)

unilabos/ros/nodes/presets/resource_mesh_manager.py

@@ -1,5 +1,6 @@
 from pathlib import Path
 import time
+import uuid
 import rclpy,json
 from rclpy.node import Node
 from std_msgs.msg import String,Header
@@ -25,7 +26,7 @@ from unilabos.resources.graphio import initialize_resources
 from unilabos.registry.registry import lab_registry
 
 class ResourceMeshManager(BaseROS2DeviceNode):
-    def __init__(self, resource_model: dict, resource_config: list,resource_tracker, device_id: str = "resource_mesh_manager", rate=50):
+    def __init__(self, resource_model: dict, resource_config: list,resource_tracker, device_id: str = "resource_mesh_manager", rate=50, **kwargs):
         """初始化资源网格管理器节点
         
         Args:
@@ -41,10 +42,11 @@ class ResourceMeshManager(BaseROS2DeviceNode):
             hardware_interface={},
             print_publish=False,
             resource_tracker=resource_tracker, 
+            device_uuid=kwargs.get("uuid", str(uuid.uuid4())),
         ) 
 
         self.resource_model         = resource_model
-        self.resource_config_dict   = {item['id']: item for item in resource_config}
+        self.resource_config_dict   = {item['uuid']: item for item in resource_config}
         self.move_group_ready       = False
         self.resource_tf_dict       = {}
         self.tf_broadcaster         = TransformBroadcaster(self)
@@ -182,14 +184,16 @@ class ResourceMeshManager(BaseROS2DeviceNode):
         self.get_logger().info('开始设置资源网格管理器')
         #遍历resource_config中的资源配置，判断panent是否在resource_model中，
         resource_tf_dict = {}
-        for resource_id, resource_config in resource_config_dict.items():
+        for resource_uuid, resource_config in resource_config_dict.items():
+            parent = None
+            resource_id = resource_config['id']
+            if resource_config['parent_uuid'] is not None and resource_config['parent_uuid'] != "":
+                parent = resource_config_dict[resource_config['parent_uuid']]['id']
 
-            parent = resource_config['parent']
             parent_link = 'world'
             if parent in self.resource_model:
                 parent_link = parent
             elif parent is None and resource_id in self.resource_model:
-
                 pass
             elif parent is not None and resource_id in self.resource_model:
                 # parent_link = f"{self.resource_config_dict[parent]['parent']}_{parent}_device_link".replace("None_","")
@@ -199,9 +203,9 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                 continue
             # 提取位置信息并转换单位
             position = {
-                "x": float(resource_config['position']['x'])/1000,
-                "y": float(resource_config['position']['y'])/1000,
-                "z": float(resource_config['position']['z'])/1000
+                "x": float(resource_config['position']['position']['x'])/1000,
+                "y": float(resource_config['position']['position']['y'])/1000,
+                "z": float(resource_config['position']['position']['z'])/1000
             }
             
             rotation_dict = {
@@ -210,8 +214,8 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                 "z": 0
             }
 
-            if 'rotation' in resource_config['config']:
-                rotation_dict = resource_config['config']['rotation']   
+            if 'rotation' in resource_config['position']:
+                rotation_dict = resource_config['position']['rotation']   
                 
             # 从欧拉角转换为四元数
             q = quaternion_from_euler(

unilabos/ros/nodes/presets/workstation.py

@@ -24,7 +24,7 @@ from unilabos.ros.msgs.message_converter import (
     convert_from_ros_msg_with_mapping,
 )
 from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode, DeviceNodeResourceTracker, ROS2DeviceNode
-from unilabos.ros.nodes.resource_tracker import ResourceTreeSet
+from unilabos.ros.nodes.resource_tracker import ResourceTreeSet, ResourceDictInstance
 from unilabos.utils.type_check import get_result_info_str
 
 if TYPE_CHECKING:
@@ -47,7 +47,7 @@ class ROS2WorkstationNode(BaseROS2DeviceNode):
     def __init__(
         self,
         protocol_type: List[str],
-        children: Dict[str, Any],
+        children: List[ResourceDictInstance],
         *,
         driver_instance: "WorkstationBase",
         device_id: str,
@@ -81,10 +81,11 @@ class ROS2WorkstationNode(BaseROS2DeviceNode):
         # 初始化子设备
         self.communication_node_id_to_instance = {}
 
-        for device_id, device_config in self.children.items():
-            if device_config.get("type", "device") != "device":
+        for device_config in self.children:
+            device_id = device_config.res_content.id
+            if device_config.res_content.type != "device":
                 self.lab_logger().debug(
-                    f"[Protocol Node] Skipping type {device_config['type']} {device_id} already existed, skipping."
+                    f"[Protocol Node] Skipping type {device_config.res_content.type} {device_id} already existed, skipping."
                 )
                 continue
             try:
@@ -101,8 +102,9 @@ class ROS2WorkstationNode(BaseROS2DeviceNode):
                 self.communication_node_id_to_instance[device_id] = d
                 continue
 
-        for device_id, device_config in self.children.items():
-            if device_config.get("type", "device") != "device":
+        for device_config in self.children:
+            device_id = device_config.res_content.id
+            if device_config.res_content.type != "device":
                 continue
             # 设置硬件接口代理
             if device_id not in self.sub_devices:

unilabos/ros/nodes/resource_tracker.py

@@ -1,9 +1,11 @@
+import inspect
 import traceback
 import uuid
 from pydantic import BaseModel, field_serializer, field_validator
 from pydantic import Field
 from typing import List, Tuple, Any, Dict, Literal, Optional, cast, TYPE_CHECKING, Union
 
+from unilabos.resources.plr_additional_res_reg import register
 from unilabos.utils.log import logger
 
 if TYPE_CHECKING:
@@ -60,13 +62,12 @@ class ResourceDict(BaseModel):
     icon: str = Field(description="Resource icon", default="")
     parent_uuid: Optional["str"] = Field(description="Parent resource uuid", default=None)  # 先设定parent_uuid
     parent: Optional["ResourceDict"] = Field(description="Parent resource object", default=None, exclude=True)
-    type: Literal["device"] | str = Field(description="Resource type")
+    type: Union[Literal["device"], str] = Field(description="Resource type")
     klass: str = Field(alias="class", description="Resource class name")
-    position: ResourceDictPosition = Field(description="Resource position", default_factory=ResourceDictPosition)
     pose: ResourceDictPosition = Field(description="Resource position", default_factory=ResourceDictPosition)
     config: Dict[str, Any] = Field(description="Resource configuration")
-    data: Dict[str, Any] = Field(description="Resource data")
-    extra: Dict[str, Any] = Field(description="Extra data")
+    data: Dict[str, Any] = Field(description="Resource data, eg: container liquid data")
+    extra: Dict[str, Any] = Field(description="Extra data, eg: slot index")
 
     @field_serializer("parent_uuid")
     def _serialize_parent(self, parent_uuid: Optional["ResourceDict"]):
@@ -146,15 +147,16 @@ class ResourceDictInstance(object):
         if not content.get("extra"):  # MagicCode
             content["extra"] = {}
         if "pose" not in content:
-            content["pose"] = content.get("position", {})
+            content["pose"] = content.pop("position", {})
         return ResourceDictInstance(ResourceDict.model_validate(content))
 
-    def get_nested_dict(self) -> Dict[str, Any]:
+    def get_plr_nested_dict(self) -> Dict[str, Any]:
         """获取资源实例的嵌套字典表示"""
         res_dict = self.res_content.model_dump(by_alias=True)
-        res_dict["children"] = {child.res_content.id: child.get_nested_dict() for child in self.children}
+        res_dict["children"] = {child.res_content.id: child.get_plr_nested_dict() for child in self.children}
         res_dict["parent"] = self.res_content.parent_instance_name
-        res_dict["position"] = self.res_content.position.position.model_dump()
+        res_dict["position"] = self.res_content.pose.position.model_dump()
+        del res_dict["pose"]
         return res_dict
 
 
@@ -429,9 +431,9 @@ class ResourceTreeSet(object):
         Returns:
             List[PLRResource]: PLR 资源实例列表
         """
+        register()
         from pylabrobot.resources import Resource as PLRResource
         from pylabrobot.utils.object_parsing import find_subclass
-        import inspect
 
         # 类型映射
         TYPE_MAP = {"plate": "Plate", "well": "Well", "deck": "Deck", "container": "RegularContainer"}
@@ -459,9 +461,9 @@ class ResourceTreeSet(object):
                 "size_y": res.config.get("size_y", 0),
                 "size_z": res.config.get("size_z", 0),
                 "location": {
-                    "x": res.position.position.x,
-                    "y": res.position.position.y,
-                    "z": res.position.position.z,
+                    "x": res.pose.position.x,
+                    "y": res.pose.position.y,
+                    "z": res.pose.position.z,
                     "type": "Coordinate",
                 },
                 "rotation": {"x": 0, "y": 0, "z": 0, "type": "Rotation"},
@@ -848,9 +850,13 @@ class DeviceNodeResourceTracker(object):
             extra: extra字典值
         """
         if isinstance(resource, dict):
-            resource["extra"] = extra
+            c_extra = resource.get("extra", {})
+            c_extra.update(extra)
+            resource["extra"] = c_extra
         else:
-            setattr(resource, "unilabos_extra", extra)
+            c_extra = getattr(resource, "unilabos_extra", {})
+            c_extra.update(extra)
+            setattr(resource, "unilabos_extra", c_extra)
 
     def _traverse_and_process(self, resource, process_func) -> int:
         """