From e373220ce33c3877d1a5ca88dc3a4dfdbfc101b2 Mon Sep 17 00:00:00 2001 From: wznln <18435084+Xuwznln@users.noreply.github.com> Date: Tue, 6 May 2025 20:18:49 +0800 Subject: [PATCH] feat: add more necessary params --- unilabos/app/main.py | 6 +-- unilabos/registry/registry.py | 3 ++ unilabos/resources/registry.py | 9 +++-- unilabos/ros/nodes/base_device_node.py | 40 ++++++++++++++++++- unilabos/ros/nodes/presets/host_node.py | 23 +++++++---- .../action/ResourceCreateFromOuter.action | 2 + 6 files changed, 66 insertions(+), 17 deletions(-) diff --git a/unilabos/app/main.py b/unilabos/app/main.py index a56d2d46..4bc65cf5 100644 --- a/unilabos/app/main.py +++ b/unilabos/app/main.py @@ -77,9 +77,9 @@ def parse_args(): ) parser.add_argument( "--visual", - choices=["rviz", "web", "disable"], + choices=["rviz", "web", "deck", "disable"], default="disable", - help="选择可视化工具: rviz, web", + help="选择可视化工具: rviz, web, deck(2D bird view)", ) return parser.parse_args() @@ -179,7 +179,7 @@ def main(): signal.signal(signal.SIGTERM, _exit) mqtt_client.start() args_dict["resources_mesh_config"] = {} - + # web visiualize 2D if args_dict["visual"] != "disable": enable_rviz = args_dict["visual"] == "rviz" if devices_and_resources is not None: diff --git a/unilabos/registry/registry.py b/unilabos/registry/registry.py index a4ef581c..d8459866 100644 --- a/unilabos/registry/registry.py +++ b/unilabos/registry/registry.py @@ -37,6 +37,9 @@ class Registry: "goal": { "resources": "resources", "device_ids": "device_ids", + "bind_parent_ids": "bind_parent_ids", + "bind_locations": "bind_locations", + "other_calling_params": "other_calling_params", }, "feedback": {}, "result": { diff --git a/unilabos/resources/registry.py b/unilabos/resources/registry.py index 435edb68..fae72622 100644 --- a/unilabos/resources/registry.py +++ b/unilabos/resources/registry.py @@ -1,5 +1,5 @@ -import sys - +import traceback +from unilabos.utils.log import logger resource_schema = { "workstation": {"type": "object", "properties": {}}, @@ -132,7 +132,8 @@ def add_schema(resources_config: list[dict]) -> list[dict]: try: if type(resource["children"][0]) == dict: resource["children"] = add_schema(resource["children"]) - except: - sys.exit(0) + except Exception as ex: + logger.error("添加物料schema时出错") + traceback.print_exc() return resources_config diff --git a/unilabos/ros/nodes/base_device_node.py b/unilabos/ros/nodes/base_device_node.py index 0f62d2ea..dc75af96 100644 --- a/unilabos/ros/nodes/base_device_node.py +++ b/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 -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, @@ -304,7 +305,42 @@ class BaseROS2DeviceNode(Node, Generic[T]): res.response = "" return res - def append_resource(req, res): + def append_resource(req: SerialCommand_Request, res): + # 物料传输到对应的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["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()) pass self._service_server: Dict[str, Service] = { diff --git a/unilabos/ros/nodes/presets/host_node.py b/unilabos/ros/nodes/presets/host_node.py index 53ce6ca0..6f94b813 100644 --- a/unilabos/ros/nodes/presets/host_node.py +++ b/unilabos/ros/nodes/presets/host_node.py @@ -7,6 +7,7 @@ import uuid from typing import Optional, Dict, Any, List, ClassVar, Set from action_msgs.msg import GoalStatus +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 @@ -267,21 +268,27 @@ 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]): - for resource, device_id, bind_parent_id in zip(resources, device_ids, bind_parent_ids): + 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({ - "machine_name": BasicConfig.machine_name, - "type": "slave", - "devices_config": devices_config_copy, - "registry_config": lab_registry.obtain_registry_device_info() - }, ensure_ascii=False, cls=TypeEncoder) + "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), + }, ensure_ascii=False) response = sclient.call(request) - print("111") pass def initialize_device(self, device_id: str, device_config: Dict[str, Any]) -> None: diff --git a/unilabos_msgs/action/ResourceCreateFromOuter.action b/unilabos_msgs/action/ResourceCreateFromOuter.action index daf67c7d..e0eeb1c7 100644 --- a/unilabos_msgs/action/ResourceCreateFromOuter.action +++ b/unilabos_msgs/action/ResourceCreateFromOuter.action @@ -1,6 +1,8 @@ Resource[] resources string[] device_ids string[] bind_parent_ids +geometry_msgs/Point[] bind_locations +string[] other_calling_params --- bool success --- \ No newline at end of file