diff --git a/test/commands/resource_add.md b/test/commands/resource_add.md index 9d5fe38f..d80e1557 100644 --- a/test/commands/resource_add.md +++ b/test/commands/resource_add.md @@ -1,5 +1,5 @@ 使用plr_test.json启动,将Well加入Plate中 ```bash -ros2 action send_goal /devices/host_node/add_resource_from_outer unilabos_msgs/action/_resource_create_from_outer/ResourceCreateFromOuter "{ resources: [ { 'category': '', 'children': [], 'config': { 'type': 'Well', 'size_x': 6.86, 'size_y': 6.86, 'size_z': 10.67, 'rotation': { 'x': 0, 'y': 0, 'z': 0, 'type': 'Rotation' }, 'category': 'well', 'model': null, 'max_volume': 360, 'material_z_thickness': 0.5, 'compute_volume_from_height': null, 'compute_height_from_volume': null, 'bottom_type': 'flat', 'cross_section_type': 'circle' }, 'data': { 'liquids': [], 'pending_liquids': [], 'liquid_history': [] }, 'id': 'plate_well_11_7', 'name': 'plate_well_11_7', 'pose': { 'orientation': { 'w': 1.0, 'x': 0.0, 'y': 0.0, 'z': 0.0 }, 'position': { 'x': 0.0, 'y': 0.0, 'z': 0.0 } }, 'sample_id': '', 'parent': 'plate', 'type': 'device' } ], device_ids: [ 'PLR_STATION' ], bind_parent_ids: [ 'plate' ], bind_locations: [ { 'x': 0.0, 'y': 0.0, 'z': 0.0 } ], other_calling_params: [ '{}' ] }" +ros2 action send_goal /devices/host_node/create_resource_detailed unilabos_msgs/action/_resource_create_from_outer/ResourceCreateFromOuter "{ resources: [ { 'category': '', 'children': [], 'config': { 'type': 'Well', 'size_x': 6.86, 'size_y': 6.86, 'size_z': 10.67, 'rotation': { 'x': 0, 'y': 0, 'z': 0, 'type': 'Rotation' }, 'category': 'well', 'model': null, 'max_volume': 360, 'material_z_thickness': 0.5, 'compute_volume_from_height': null, 'compute_height_from_volume': null, 'bottom_type': 'flat', 'cross_section_type': 'circle' }, 'data': { 'liquids': [], 'pending_liquids': [], 'liquid_history': [] }, 'id': 'plate_well_11_7', 'name': 'plate_well_11_7', 'pose': { 'orientation': { 'w': 1.0, 'x': 0.0, 'y': 0.0, 'z': 0.0 }, 'position': { 'x': 0.0, 'y': 0.0, 'z': 0.0 } }, 'sample_id': '', 'parent': 'plate', 'type': 'device' } ], device_ids: [ 'PLR_STATION' ], bind_parent_ids: [ 'plate' ], bind_locations: [ { 'x': 0.0, 'y': 0.0, 'z': 0.0 } ], other_calling_params: [ '{}' ] }" ``` \ No newline at end of file diff --git a/unilabos-linux-64.yaml b/unilabos-linux-64.yaml index 3f5b91c6..aeac636f 100644 --- a/unilabos-linux-64.yaml +++ b/unilabos-linux-64.yaml @@ -56,6 +56,8 @@ dependencies: - ros-humble-moveit-servo # simulation - ros-humble-simulation + - ros-humble-tf-transformations + - transforms3d # ros-humble-gazebo-ros // ignored because of the conflict with ign-gazebo # ilab equipments # - ros-humble-unilabos-msgs diff --git a/unilabos-osx-64.yaml b/unilabos-osx-64.yaml index 38981f0a..72ffb4c3 100644 --- a/unilabos-osx-64.yaml +++ b/unilabos-osx-64.yaml @@ -56,6 +56,8 @@ dependencies: # - ros-humble-moveit-servo # simulation - ros-humble-simulation + - ros-humble-tf-transformations + - transforms3d # ros-humble-gazebo-ros // ignored because of the conflict with ign-gazebo # ilab equipments # - ros-humble-unilabos-msgs diff --git a/unilabos-osx-arm64.yaml b/unilabos-osx-arm64.yaml index 05333a39..83ffe1b3 100644 --- a/unilabos-osx-arm64.yaml +++ b/unilabos-osx-arm64.yaml @@ -58,6 +58,8 @@ dependencies: - ros-humble-moveit-servo # simulation - ros-humble-simulation + - ros-humble-tf-transformations + - transforms3d # ros-humble-gazebo-ros // ignored because of the conflict with ign-gazebo # ilab equipments # - ros-humble-unilabos-msgs diff --git a/unilabos-win64.yaml b/unilabos-win64.yaml index 2e26fa39..19a40080 100644 --- a/unilabos-win64.yaml +++ b/unilabos-win64.yaml @@ -56,6 +56,8 @@ dependencies: - ros-humble-moveit-servo # simulation - ros-humble-simulation # ignored because of NO python3.11 package in WIN64 + - ros-humble-tf-transformations + - transforms3d # ros-humble-gazebo-ros // ignored because of the conflict with ign-gazebo # ilab equipments # ros-humble-unilabos-msgs diff --git a/unilabos/registry/registry.py b/unilabos/registry/registry.py index 5831dd8d..59f84abc 100644 --- a/unilabos/registry/registry.py +++ b/unilabos/registry/registry.py @@ -20,10 +20,10 @@ class Registry: if registry_paths: self.registry_paths.extend(registry_paths) self.ResourceCreateFromOuter = self._replace_type_with_class( - "ResourceCreateFromOuter", "host_node", f"动作 add_resource_from_outer" + "ResourceCreateFromOuter", "host_node", f"动作 create_resource_detailed" ) self.ResourceCreateFromOuterEasy = self._replace_type_with_class( - "ResourceCreateFromOuterEasy", "host_node", f"动作 add_resource_from_outer_easy" + "ResourceCreateFromOuterEasy", "host_node", f"动作 create_resource" ) self.device_type_registry = { "host_node": { @@ -33,7 +33,7 @@ class Registry: "type": "python", "status_types": {}, "action_value_mappings": { - "add_resource_from_outer": { + "create_resource_detailed": { "type": msg_converter_manager.search_class("ResourceCreateFromOuter"), "goal": { "resources": "resources", @@ -48,7 +48,7 @@ class Registry: }, "schema": ros_action_to_json_schema(self.ResourceCreateFromOuter) }, - "add_resource_from_outer_easy": { + "create_resource": { "type": msg_converter_manager.search_class("ResourceCreateFromOuterEasy"), "goal": { "res_id": "res_id", @@ -56,9 +56,9 @@ class Registry: "parent": "parent", "device_id": "device_id", "bind_locations": "bind_locations", - "liquid_input_slot": "liquid_input_slot", - "liquid_type": "liquid_type", - "liquid_volume": "liquid_volume", + "liquid_input_slot": "liquid_input_slot[]", + "liquid_type": "liquid_type[]", + "liquid_volume": "liquid_volume[]", "slot_on_deck": "slot_on_deck", }, "feedback": {}, diff --git a/unilabos/resources/graphio.py b/unilabos/resources/graphio.py index 0a820131..9a31add5 100644 --- a/unilabos/resources/graphio.py +++ b/unilabos/resources/graphio.py @@ -189,6 +189,7 @@ def dict_from_graph(graph: nx.Graph) -> dict: def dict_to_tree(nodes: dict, devices_only: bool = False) -> list[dict]: # 将节点转换为字典,以便通过 ID 快速查找 nodes_list = [node for node in nodes.values() if node.get("type") == "device" or not devices_only] + id_list = [node["id"] for node in nodes_list] # 初始化每个节点的 children 为包含节点字典的列表 for node in nodes_list: @@ -196,7 +197,7 @@ def dict_to_tree(nodes: dict, devices_only: bool = False) -> list[dict]: # 找到根节点并返回 root_nodes = [ - node for node in nodes_list if len(nodes_list) == 1 or node.get("parent", node.get("parent_name")) in [None, "", "None", np.nan] + node for node in nodes_list if len(nodes_list) == 1 or node.get("parent", node.get("parent_name")) in [None, "", "None", np.nan] or node.get("parent", node.get("parent_name")) not in id_list ] # 如果存在多个根节点,返回所有根节点 diff --git a/unilabos/ros/msgs/message_converter.py b/unilabos/ros/msgs/message_converter.py index c9f8af3c..11c7afd5 100644 --- a/unilabos/ros/msgs/message_converter.py +++ b/unilabos/ros/msgs/message_converter.py @@ -348,7 +348,10 @@ def convert_to_ros_msg(ros_msg_type: Union[Type, Any], obj: Any) -> Any: if isinstance(td, NamespacedType): target_class = msg_converter_manager.get_class(f"{'.'.join(td.namespaces)}.{td.name}") setattr(ros_msg, key, [convert_to_ros_msg(target_class, v) for v in value]) + elif isinstance(td, UnboundedString): + setattr(ros_msg, key, value) else: + logger.warning(f"Not Supported type: {td}") setattr(ros_msg, key, []) # FIXME elif "array.array" in str(type(attr)): if attr.typecode == "f": diff --git a/unilabos/ros/nodes/base_device_node.py b/unilabos/ros/nodes/base_device_node.py index 0fd13c45..35513c8c 100644 --- a/unilabos/ros/nodes/base_device_node.py +++ b/unilabos/ros/nodes/base_device_node.py @@ -1,3 +1,4 @@ +import copy import json import threading import time @@ -19,7 +20,7 @@ from unilabos_msgs.action import SendCmd from unilabos_msgs.srv._serial_command import SerialCommand_Request, SerialCommand_Response from unilabos.resources.graphio import convert_resources_to_type, convert_resources_from_type, resource_ulab_to_plr, \ - initialize_resources + initialize_resources, list_to_nested_dict, dict_to_tree, resource_plr_to_ulab, tree_to_list from unilabos.ros.msgs.message_converter import ( convert_to_ros_msg, convert_from_ros_msg, @@ -311,7 +312,10 @@ class BaseROS2DeviceNode(Node, Generic[T]): # 物料传输到对应的node节点 rclient = self.create_client(ResourceAdd, "/resources/add") rclient.wait_for_service() + rclient2 = self.create_client(ResourceAdd, "/resources/add") + rclient2.wait_for_service() request = ResourceAdd.Request() + request2 = ResourceAdd.Request() command_json = json.loads(req.command) namespace = command_json["namespace"] bind_parent_id = command_json["bind_parent_id"] @@ -320,16 +324,23 @@ class BaseROS2DeviceNode(Node, Generic[T]): other_calling_param = command_json["other_calling_param"] resources = command_json["resource"] initialize_full = other_calling_param.pop("initialize_full", False) + # 用来增加液体 ADD_LIQUID_TYPE = other_calling_param.pop("ADD_LIQUID_TYPE", []) LIQUID_VOLUME = other_calling_param.pop("LIQUID_VOLUME", []) + LIQUID_INPUT_SLOT = other_calling_param.pop("LIQUID_INPUT_SLOT", []) slot = other_calling_param.pop("slot", -1) if slot >= 0: # slot为负数的时候采用assign方法 other_calling_param["slot"] = slot # 本地拿到这个物料,可能需要先做初始化? if isinstance(resources, list): - if initialize_full: + if len(resources) == 1 and isinstance(resources[0], list) and not initialize_full: # 取消,不存在的情况 + # 预先initialize过,以整组的形式传入 + request.resources = [convert_to_ros_msg(Resource, resource_) for resource_ in resources[0]] + elif initialize_full: resources = initialize_resources(resources) - request.resources = [convert_to_ros_msg(Resource, resource) for resource in resources] + request.resources = [convert_to_ros_msg(Resource, resource) for resource in resources] + else: + request.resources = [convert_to_ros_msg(Resource, resource) for resource in resources] else: if initialize_full: resources = initialize_resources([resources]) @@ -339,20 +350,31 @@ class BaseROS2DeviceNode(Node, Generic[T]): res.response = "OK" # 接下来该根据bind_parent_id进行assign了,目前只有plr可以进行assign,不然没有办法输入到物料系统中 resource = self.resource_tracker.figure_resource({"name": bind_parent_id}) - request.resources = [convert_to_ros_msg(Resource, resources)] + # request.resources = [convert_to_ros_msg(Resource, resources)] try: from pylabrobot.resources.resource import Resource as ResourcePLR from pylabrobot.resources.deck import Deck from pylabrobot.resources import Coordinate from pylabrobot.resources import OTDeck + from pylabrobot.resources import Plate contain_model = not isinstance(resource, Deck) if isinstance(resource, ResourcePLR): # resources.list() - plr_instance = resource_ulab_to_plr(resources, contain_model) + resources_tree = dict_to_tree(copy.deepcopy({r["id"]: r for r in resources})) + plr_instance = resource_ulab_to_plr(resources_tree[0], contain_model) + if isinstance(plr_instance, Plate): + empty_liquid_info_in = [(None, 0)] * plr_instance.num_items + for liquid_type, liquid_volume, liquid_input_slot in zip(ADD_LIQUID_TYPE, LIQUID_VOLUME, LIQUID_INPUT_SLOT): + empty_liquid_info_in[liquid_input_slot] = (liquid_type, liquid_volume) + plr_instance.set_well_liquids(empty_liquid_info_in) if isinstance(resource, OTDeck) and "slot" in other_calling_param: resource.assign_child_at_slot(plr_instance, **other_calling_param) - resource.assign_child_resource(plr_instance, Coordinate(location["x"], location["y"], location["z"]), **other_calling_param) + else: + _discard_slot = other_calling_param.pop("slot", -1) + resource.assign_child_resource(plr_instance, Coordinate(location["x"], location["y"], location["z"]), **other_calling_param) + request2.resources = [convert_to_ros_msg(Resource, r) for r in tree_to_list([resource_plr_to_ulab(resource)])] + rclient2.call(request2) # 发送给ResourceMeshManager action_client = ActionClient( self, SendCmd, "/devices/resource_mesh_manager/add_resource_mesh", callback_group=self.callback_group @@ -515,7 +537,7 @@ 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查询物料当前状态,如果是host本身的增加物料的请求,则直接跳过 - if action_name not in ["add_resource_from_outer", "add_resource_from_outer_easy"]: + 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"]: self.lab_logger().info(f"查询资源状态: Key: {k} Type: {v}") @@ -614,7 +636,7 @@ class BaseROS2DeviceNode(Node, Generic[T]): del future # 向Host更新物料当前状态 - if action_name not in ["add_resource_from_outer", "add_resource_from_outer_easy"]: + 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"]: continue diff --git a/unilabos/ros/nodes/presets/host_node.py b/unilabos/ros/nodes/presets/host_node.py index 86bffd61..5fe3d9c9 100644 --- a/unilabos/ros/nodes/presets/host_node.py +++ b/unilabos/ros/nodes/presets/host_node.py @@ -102,11 +102,11 @@ class HostNode(BaseROS2DeviceNode): self.devices_instances: Dict[str, ROS2DeviceNode] = {} # 存储设备实例 self.device_machine_names: Dict[str, str] = {device_id: "本地", } # 存储设备ID到机器名称的映射 self._action_clients: Dict[str, ActionClient] = { # 为了方便了解实际的数据类型,host的默认写好 - "/devices/host_node/add_resource_from_outer_easy": ActionClient( - self, lab_registry.ResourceCreateFromOuterEasy, "/devices/host_node/add_resource_from_outer_easy", callback_group=self.callback_group + "/devices/host_node/create_resource": ActionClient( + self, lab_registry.ResourceCreateFromOuterEasy, "/devices/host_node/create_resource", callback_group=self.callback_group ), - "/devices/host_node/add_resource_from_outer": ActionClient( - self, lab_registry.ResourceCreateFromOuter, "/devices/host_node/add_resource_from_outer", callback_group=self.callback_group + "/devices/host_node/create_resource_detailed": ActionClient( + self, lab_registry.ResourceCreateFromOuter, "/devices/host_node/create_resource_detailed", callback_group=self.callback_group ) } # 用来存储多个ActionClient实例 self._action_value_mappings: Dict[str, Dict] = {} # 用来存储多个ActionClient的type, goal, feedback, result的变量名映射关系 @@ -290,7 +290,7 @@ class HostNode(BaseROS2DeviceNode): 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]): + def create_resource_detailed(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 @@ -299,7 +299,7 @@ class HostNode(BaseROS2DeviceNode): sclient.wait_for_service() request = SerialCommand.Request() request.command = json.dumps({ - "resource": resource, + "resource": resource, # 单个/单组 可为 list[list[Resource]] "namespace": namespace, "edge_device_id": device_id, "bind_parent_id": bind_parent_id, @@ -314,7 +314,7 @@ class HostNode(BaseROS2DeviceNode): pass pass - def add_resource_from_outer_easy(self, device_id: str, res_id: str, class_name: str, parent: str, bind_locations: Point, liquid_input_slot: list[int], liquid_type: list[str], liquid_volume: list[int], slot_on_deck: int): + def create_resource(self, device_id: str, res_id: str, class_name: str, parent: str, bind_locations: Point, liquid_input_slot: list[int], liquid_type: list[str], liquid_volume: list[int], slot_on_deck: int): init_new_res = initialize_resource({ "name": res_id, "class": class_name, @@ -324,8 +324,8 @@ class HostNode(BaseROS2DeviceNode): "y": bind_locations.y, "z": bind_locations.z, } - }) - resources = init_new_res + }) # flatten的格式 + resources = [init_new_res] device_id = [device_id] bind_parent_id = [parent] bind_location = [bind_locations] @@ -337,7 +337,7 @@ class HostNode(BaseROS2DeviceNode): "slot": slot_on_deck })] - return self.add_resource_from_outer(resources, device_id, bind_parent_id, bind_location, other_calling_param) + return self.create_resource_detailed(resources, device_id, bind_parent_id, bind_location, other_calling_param) def initialize_device(self, device_id: str, device_config: Dict[str, Any]) -> None: """