Merge remote-tracking branch 'upstream/dev' into device_visualization

unilabos/ros/nodes/base_device_node.py

@@ -15,8 +15,9 @@ 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.srv._serial_command import SerialCommand_Request, SerialCommand_Response
 
-from unilabos.resources.graphio import convert_resources_to_type, convert_resources_from_type
+from unilabos.resources.graphio import convert_resources_to_type, convert_resources_from_type, resource_ulab_to_plr
 from unilabos.ros.msgs.message_converter import (
     convert_to_ros_msg,
     convert_from_ros_msg,
@@ -101,6 +102,7 @@ def init_wrapper(
     self,
     device_id: str,
     driver_class: type[T],
+    device_config: Dict[str, Any],
     status_types: Dict[str, Any],
     action_value_mappings: Dict[str, Any],
     hardware_interface: Dict[str, Any],
@@ -118,6 +120,7 @@ def init_wrapper(
         children = []
     kwargs["device_id"] = device_id
     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
@@ -302,10 +305,52 @@ class BaseROS2DeviceNode(Node, Generic[T]):
             res.response = ""
             return res
 
+        def append_resource(req: SerialCommand_Request, res: SerialCommand_Response):
+            # 物料传输到对应的node节点
+            rclient = self.create_client(ResourceAdd, "/resources/add")
+            rclient.wait_for_service()
+            request = ResourceAdd.Request()
+            command_json = json.loads(req.command)
+            namespace = command_json["namespace"]
+            bind_parent_id = command_json["bind_parent_id"]
+            edge_device_id = command_json["edge_device_id"]
+            location = command_json["bind_location"]
+            other_calling_param = command_json["other_calling_param"]
+            resources = command_json["resource"]
+            # 本地拿到这个物料，可能需要先做初始化?
+            if isinstance(resources, list):
+                request.resources = [convert_to_ros_msg(Resource, resource) for resource in resources]
+            else:
+                request.resources = [convert_to_ros_msg(Resource, resources)]
+            response = rclient.call(request)
+            # 应该本地先add_resource？
+            res.response = "OK"
+            # 接下来该根据bind_parent_id进行assign了，目前只有plr可以进行assign，不然没有办法输入到物料系统中
+            resource = self.resource_tracker.figure_resource({"name": bind_parent_id})
+            try:
+                from pylabrobot.resources.resource import Resource as ResourcePLR
+                from pylabrobot.resources.deck import Deck
+                from pylabrobot.resources import Coordinate
+                contain_model = not isinstance(resource, Deck)
+                if isinstance(resource, ResourcePLR):
+                    # resources.list()
+                    plr_instance = resource_ulab_to_plr(resources, contain_model)
+                    resource.assign_child_resource(plr_instance, Coordinate(location["x"], location["y"], location["z"]), **other_calling_param)
+            except ImportError:
+                self.lab_logger().error("Host请求添加物料时，本环境并不存在pylabrobot")
+            except Exception as e:
+                self.lab_logger().error("Host请求添加物料时出错")
+                self.lab_logger().error(traceback.format_exc())
+            return res
+
+        # noinspection PyTypeChecker
         self._service_server: Dict[str, Service] = {
             "query_host_name": self.create_service(
                 SerialCommand, f"/srv{self.namespace}/query_host_name", query_host_name_cb, callback_group=self.callback_group
             ),
+            "append_resource": self.create_service(
+                SerialCommand, f"/srv{self.namespace}/append_resource", append_resource, callback_group=self.callback_group
+            ),
         }
 
         # 向全局在线设备注册表添加设备信息
@@ -437,26 +482,26 @@ class BaseROS2DeviceNode(Node, Generic[T]):
 
             action_kwargs = convert_from_ros_msg_with_mapping(goal, action_value_mapping["goal"])
             self.lab_logger().debug(f"接收到原始目标: {action_kwargs}")
-
-            # 向Host查询物料当前状态
-            for k, v in goal.get_fields_and_field_types().items():
-                if v in ["unilabos_msgs/Resource", "sequence<unilabos_msgs/Resource>"]:
-                    self.lab_logger().info(f"查询资源状态: Key: {k} Type: {v}")
-                    try:
-                        r = ResourceGet.Request()
-                        r.id = action_kwargs[k]["id"] if v == "unilabos_msgs/Resource" else action_kwargs[k][0]["id"]
-                        r.with_children = True
-                        response = await self._resource_clients["resource_get"].call_async(r)
-                    except Exception:
-                        logger.error(f"资源查询失败，默认使用本地资源")
-                    # 删除对response.resources的检查，因为它总是存在
-                    resources_list = [convert_from_ros_msg(rs) for rs in response.resources]  # type: ignore  # FIXME
-                    self.lab_logger().debug(f"资源查询结果: {len(resources_list)} 个资源")
-                    type_hint = action_paramtypes[k]
-                    final_type = get_type_class(type_hint)
-                    # 判断 ACTION 是否需要特殊的物料类型如 pylabrobot.resources.Resource，并做转换
-                    final_resource = convert_resources_to_type(resources_list, final_type)
-                    action_kwargs[k] = self.resource_tracker.figure_resource(final_resource)
+            # 向Host查询物料当前状态，如果是host本身的增加物料的请求，则直接跳过
+            if action_name != "add_resource_from_outer":
+                for k, v in goal.get_fields_and_field_types().items():
+                    if v in ["unilabos_msgs/Resource", "sequence<unilabos_msgs/Resource>"]:
+                        self.lab_logger().info(f"查询资源状态: Key: {k} Type: {v}")
+                        try:
+                            r = ResourceGet.Request()
+                            r.id = action_kwargs[k]["id"] if v == "unilabos_msgs/Resource" else action_kwargs[k][0]["id"]
+                            r.with_children = True
+                            response = await self._resource_clients["resource_get"].call_async(r)
+                        except Exception:
+                            logger.error(f"资源查询失败，默认使用本地资源")
+                        # 删除对response.resources的检查，因为它总是存在
+                        resources_list = [convert_from_ros_msg(rs) for rs in response.resources]  # type: ignore  # FIXME
+                        self.lab_logger().debug(f"资源查询结果: {len(resources_list)} 个资源")
+                        type_hint = action_paramtypes[k]
+                        final_type = get_type_class(type_hint)
+                        # 判断 ACTION 是否需要特殊的物料类型如 pylabrobot.resources.Resource，并做转换
+                        final_resource = convert_resources_to_type(resources_list, final_type)
+                        action_kwargs[k] = self.resource_tracker.figure_resource(final_resource)
 
             self.lab_logger().info(f"准备执行: {action_kwargs}, 函数: {ACTION.__name__}")
             time_start = time.time()
@@ -527,27 +572,28 @@ class BaseROS2DeviceNode(Node, Generic[T]):
             del future
 
             # 向Host更新物料当前状态
-            for k, v in goal.get_fields_and_field_types().items():
-                if v not in ["unilabos_msgs/Resource", "sequence<unilabos_msgs/Resource>"]:
-                    continue
-                self.lab_logger().info(f"更新资源状态: {k}")
-                r = ResourceUpdate.Request()
-                # 仅当action_kwargs[k]不为None时尝试转换
-                akv = action_kwargs[k]
-                apv = action_paramtypes[k]
-                final_type = get_type_class(apv)
-                if final_type is None:
-                    continue
-                try:
-                    r.resources = [
-                        convert_to_ros_msg(Resource, self.resource_tracker.root_resource(rs))
-                        for rs in convert_resources_from_type(akv, final_type)  # type: ignore  # FIXME  # 考虑反查到最大的
-                    ]
-                    response = await self._resource_clients["resource_update"].call_async(r)
-                    self.lab_logger().debug(f"资源更新结果: {response}")
-                except Exception as e:
-                    self.lab_logger().error(f"资源更新失败: {e}")
-                    self.lab_logger().error(traceback.format_exc())
+            if action_name != "add_resource_from_outer":
+                for k, v in goal.get_fields_and_field_types().items():
+                    if v not in ["unilabos_msgs/Resource", "sequence<unilabos_msgs/Resource>"]:
+                        continue
+                    self.lab_logger().info(f"更新资源状态: {k}")
+                    r = ResourceUpdate.Request()
+                    # 仅当action_kwargs[k]不为None时尝试转换
+                    akv = action_kwargs[k]
+                    apv = action_paramtypes[k]
+                    final_type = get_type_class(apv)
+                    if final_type is None:
+                        continue
+                    try:
+                        r.resources = [
+                            convert_to_ros_msg(Resource, self.resource_tracker.root_resource(rs))
+                            for rs in convert_resources_from_type(akv, final_type)  # type: ignore  # FIXME  # 考虑反查到最大的
+                        ]
+                        response = await self._resource_clients["resource_update"].call_async(r)
+                        self.lab_logger().debug(f"资源更新结果: {response}")
+                    except Exception as e:
+                        self.lab_logger().error(f"资源更新失败: {e}")
+                        self.lab_logger().error(traceback.format_exc())
 
             # 发布结果
             goal_handle.succeed()
@@ -627,6 +673,7 @@ class ROS2DeviceNode:
         self,
         device_id: str,
         driver_class: Type[T],
+        device_config: Dict[str, Any],
         driver_params: Dict[str, Any],
         status_types: Dict[str, Any],
         action_value_mappings: Dict[str, Any],
@@ -641,6 +688,8 @@ class ROS2DeviceNode:
         Args:
             device_id: 设备标识符
             driver_class: 设备类
+            device_config: 原始初始化的json
+            driver_params: driver初始化的参数
             status_types: 状态类型映射
             action_value_mappings: 动作值映射
             hardware_interface: 硬件接口配置
@@ -657,11 +706,12 @@ 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
         self.driver_is_ros = driver_is_ros
         self.resource_tracker = DeviceNodeResourceTracker()
 
         # use_pylabrobot_creator 使用 cls的包路径检测
-        use_pylabrobot_creator = driver_class.__module__.startswith("pylabrobot")
+        use_pylabrobot_creator = driver_class.__module__.startswith("pylabrobot") or driver_class.__name__ == "DPLiquidHandler"
 
         # TODO: 要在创建之前预先请求服务器是否有当前id的物料，放到resource_tracker中，让pylabrobot进行创建
         # 创建设备类实例

unilabos/ros/nodes/presets/host_node.py

@@ -7,11 +7,13 @@ import uuid
 from typing import Optional, Dict, Any, List, ClassVar, Set
 
 from action_msgs.msg import GoalStatus
-from unilabos_msgs.msg import Resource  # type: ignore
-from unilabos_msgs.srv import ResourceAdd, ResourceGet, ResourceDelete, ResourceUpdate, ResourceList, SerialCommand  # type: ignore
+from geometry_msgs.msg import Point
 from rclpy.action import ActionClient, get_action_server_names_and_types_by_node
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.service import Service
+from unilabos_msgs.msg import Resource  # type: ignore
+from unilabos_msgs.srv import ResourceAdd, ResourceGet, ResourceDelete, ResourceUpdate, ResourceList, \
+    SerialCommand  # type: ignore
 from unique_identifier_msgs.msg import UUID
 
 from unilabos.registry.registry import lab_registry
@@ -23,11 +25,9 @@ from unilabos.ros.msgs.message_converter import (
     convert_from_ros_msg,
     convert_to_ros_msg,
     msg_converter_manager,
-    ros_action_to_json_schema,
 )
 from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode, ROS2DeviceNode, DeviceNodeResourceTracker
 from unilabos.ros.nodes.presets.controller_node import ControllerNode
-from unilabos.utils.type_check import TypeEncoder
 
 
 class HostNode(BaseROS2DeviceNode):
@@ -50,7 +50,7 @@ class HostNode(BaseROS2DeviceNode):
         self,
         device_id: str,
         devices_config: Dict[str, Any],
-        resources_config: Any,
+        resources_config: list,
         physical_setup_graph: Optional[Dict[str, Any]] = None,
         controllers_config: Optional[Dict[str, Any]] = None,
         bridges: Optional[List[Any]] = None,
@@ -76,7 +76,7 @@ class HostNode(BaseROS2DeviceNode):
             driver_instance=self,
             device_id=device_id,
             status_types={},
-            action_value_mappings={},
+            action_value_mappings=lab_registry.device_type_registry["host_node"]["class"]["action_value_mappings"],
             hardware_interface={},
             print_publish=False,
             resource_tracker=DeviceNodeResourceTracker(),  # host node并不是通过initialize 包一层传进来的
@@ -97,15 +97,13 @@ class HostNode(BaseROS2DeviceNode):
         self.bridges = bridges
 
         # 创建设备、动作客户端和目标存储
-        self.devices_names: Dict[str, str] = {}  # 存储设备名称和命名空间的映射
+        self.devices_names: Dict[str, str] = {device_id: self.namespace}  # 存储设备名称和命名空间的映射
         self.devices_instances: Dict[str, ROS2DeviceNode] = {}  # 存储设备实例
         self.device_machine_names: Dict[str, str] = {device_id: "本地", }  # 存储设备ID到机器名称的映射
         self._action_clients: Dict[str, ActionClient] = {}  # 用来存储多个ActionClient实例
-        self._action_value_mappings: Dict[str, Dict] = (
-            {}
-        )  # 用来存储多个ActionClient的type, goal, feedback, result的变量名映射关系
+        self._action_value_mappings: Dict[str, Dict] = {}  # 用来存储多个ActionClient的type, goal, feedback, result的变量名映射关系
         self._goals: Dict[str, Any] = {}  # 用来存储多个目标的状态
-        self._online_devices: Set[str] = set()  # 用于跟踪在线设备
+        self._online_devices: Set[str] = {f"{self.namespace}/{device_id}"}  # 用于跟踪在线设备
         self._last_discovery_time = 0.0  # 上次设备发现的时间
         self._discovery_lock = threading.Lock()  # 设备发现的互斥锁
         self._subscribed_topics = set()  # 用于跟踪已订阅的话题
@@ -259,16 +257,41 @@ class HostNode(BaseROS2DeviceNode):
                     self.lab_logger().debug(f"[Host Node] Created ActionClient (Discovery): {action_id}")
                     action_name = action_id[len(namespace) + 1:]
                     edge_device_id = namespace[9:]
-                    from unilabos.app.mq import mqtt_client
-                    info_with_schema = ros_action_to_json_schema(action_type)
-                    mqtt_client.publish_actions(action_name, {
-                        "device_id": edge_device_id,
-                        "action_name": action_name,
-                        "schema": info_with_schema,
-                    })
+                    # from unilabos.app.mq import mqtt_client
+                    # info_with_schema = ros_action_to_json_schema(action_type)
+                    # mqtt_client.publish_actions(action_name, {
+                    #     "device_id": edge_device_id,
+                    #     "device_type": "",
+                    #     "action_name": action_name,
+                    #     "schema": info_with_schema,
+                    # })
                 except Exception as e:
                     self.lab_logger().error(f"[Host Node] Failed to create ActionClient for {action_id}: {str(e)}")
 
+    def add_resource_from_outer(self, resources: list["Resource"], device_ids: list[str], bind_parent_ids: list[str], bind_locations: list[Point], other_calling_params: list[str]):
+        for resource, device_id, bind_parent_id, bind_location, other_calling_param in zip(resources, device_ids, bind_parent_ids, bind_locations, other_calling_params):
+            # 这里要求device_id传入必须是edge_device_id
+            namespace = "/devices/" + device_id
+            srv_address = f"/srv{namespace}/append_resource"
+            sclient = self.create_client(SerialCommand, srv_address)
+            sclient.wait_for_service()
+            request = SerialCommand.Request()
+            request.command = json.dumps({
+                "resource": resource,
+                "namespace": namespace,
+                "edge_device_id": device_id,
+                "bind_parent_id": bind_parent_id,
+                "bind_location": {
+                    "x": bind_location.x,
+                    "y": bind_location.y,
+                    "z": bind_location.z,
+                },
+                "other_calling_param": json.loads(other_calling_param) if other_calling_param else {},
+            }, ensure_ascii=False)
+            response = sclient.call(request)
+            pass
+        pass
+
     def initialize_device(self, device_id: str, device_config: Dict[str, Any]) -> None:
         """
         根据配置初始化设备，
@@ -297,13 +320,14 @@ class HostNode(BaseROS2DeviceNode):
                 action_type = action_value_mapping["type"]
                 self._action_clients[action_id] = ActionClient(self, action_type, action_id)
                 self.lab_logger().debug(f"[Host Node] Created ActionClient (Local): {action_id}")  # 子设备再创建用的是Discover发现的
-                from unilabos.app.mq import mqtt_client
-                info_with_schema = ros_action_to_json_schema(action_type)
-                mqtt_client.publish_actions(action_name, {
-                    "device_id": device_id,
-                    "action_name": action_name,
-                    "schema": info_with_schema,
-                })
+                # from unilabos.app.mq import mqtt_client
+                # info_with_schema = ros_action_to_json_schema(action_type)
+                # mqtt_client.publish_actions(action_name, {
+                #     "device_id": device_id,
+                #     "device_type": device_config["class"],
+                #     "action_name": action_name,
+                #     "schema": info_with_schema,
+                # })
             else:
                 self.lab_logger().warning(f"[Host Node] ActionClient {action_id} already exists.")
         device_key = f"{self.devices_names[device_id]}/{device_id}"  # 这里不涉及二级device_id

unilabos/ros/nodes/resource_tracker.py

@@ -1,7 +1,7 @@
 from unilabos.utils.log import logger
 
 
-class DeviceNodeResourceTracker:
+class DeviceNodeResourceTracker(object):
 
     def __init__(self):
         self.resources = []
@@ -15,44 +15,46 @@ class DeviceNodeResourceTracker:
             return resource
 
     def add_resource(self, resource):
-        # 使用内存地址跟踪是否为同一个resource
         for r in self.resources:
             if id(r) == id(resource):
                 return
-        # 添加资源到跟踪器
         self.resources.append(resource)
 
     def clear_resource(self):
         self.resources = []
 
-    def figure_resource(self, resource):
-        # 使用内存地址跟踪是否为同一个resource
-        if isinstance(resource, list):
-            return [self.figure_resource(r) for r in resource]
-        res_id = resource.id if hasattr(resource, "id") else None
-        res_name = resource.name if hasattr(resource, "name") else None
+    def figure_resource(self, query_resource):
+        if isinstance(query_resource, list):
+            return [self.figure_resource(r) for r in query_resource]
+        res_id = query_resource.id if hasattr(query_resource, "id") else (query_resource.get("id") if isinstance(query_resource, dict) else None)
+        res_name = query_resource.name if hasattr(query_resource, "name") else (query_resource.get("name") if isinstance(query_resource, dict) else None)
         res_identifier = res_id if res_id else res_name
         identifier_key = "id" if res_id else "name"
-        resource_cls_type = type(resource)
+        resource_cls_type = type(query_resource)
         if res_identifier is None:
-            logger.warning(f"resource {resource} 没有id或name，暂不能对应figure")
+            logger.warning(f"resource {query_resource} 没有id或name，暂不能对应figure")
         res_list = []
         for r in self.resources:
-            res_list.extend(
-                self.loop_find_resource(r, resource_cls_type, identifier_key, getattr(resource, identifier_key))
-            )
+            if isinstance(query_resource, dict):
+                res_list.extend(
+                    self.loop_find_resource(r, resource_cls_type, identifier_key, query_resource[identifier_key])
+                )
+            else:
+                res_list.extend(
+                    self.loop_find_resource(r, resource_cls_type, identifier_key, getattr(query_resource, identifier_key))
+                )
         assert len(res_list) == 1, f"找到多个资源，请检查资源是否唯一: {res_list}"
-        self.root_resource2resource[id(resource)] = res_list[0]
+        self.root_resource2resource[id(query_resource)] = res_list[0]
         # 后续加入其他对比方式
         return res_list[0]
 
-    def loop_find_resource(self, resource, resource_cls_type, identifier_key, compare_value):
+    def loop_find_resource(self, resource, target_resource_cls_type, identifier_key, compare_value):
         res_list = []
-        print(resource, resource_cls_type, identifier_key, compare_value)
+        # print(resource, target_resource_cls_type, identifier_key, compare_value)
         children = getattr(resource, "children", [])
         for child in children:
-            res_list.extend(self.loop_find_resource(child, resource_cls_type, identifier_key, compare_value))
-        if resource_cls_type == type(resource):
+            res_list.extend(self.loop_find_resource(child, target_resource_cls_type, identifier_key, compare_value))
+        if target_resource_cls_type == type(resource) or target_resource_cls_type == dict:
             if hasattr(resource, identifier_key):
                 if getattr(resource, identifier_key) == compare_value:
                     res_list.append(resource)