From d199fda9a57d038f07f7e04bb82e4a85ba8d3644 Mon Sep 17 00:00:00 2001 From: zhangshixiang <@zhangshixiang> Date: Tue, 6 May 2025 22:01:23 +0800 Subject: [PATCH] =?UTF-8?q?=E7=BC=96=E5=86=99mesh=E6=B7=BB=E5=8A=A0action?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- test/experiments/test_copy.json | 41 ---- unilabos/device_mesh/resource_visalization.py | 1 + unilabos/resources/graphio.py | 11 +- unilabos/ros/main_slave_run.py | 21 +- .../nodes/presets/resource_mesh_manager.py | 210 +++++++++++++++--- unilabos/ros/utils/driver_creator.py | 1 + 6 files changed, 212 insertions(+), 73 deletions(-) diff --git a/test/experiments/test_copy.json b/test/experiments/test_copy.json index 9b1debed..df40affc 100644 --- a/test/experiments/test_copy.json +++ b/test/experiments/test_copy.json @@ -62,7 +62,6 @@ "name": "teaching_carrier", "sample_id": null, "children": [ - "teaching_carrier_A1" ], "parent": "deck", "type": "plate", @@ -87,46 +86,6 @@ "model": null }, "data": {} - }, - { - "id": "teaching_carrier_A1", - "name": "teaching_carrier_A1", - "sample_id": null, - "children": [], - "parent": "teaching_carrier", - "type": "device", - "class": "", - "position": { - "x": 10.87, - "y": 70.77, - "z": 10 - }, - "config": { - "type": "TipSpot", - "size_x": 6.86, - "size_y": 6.86, - "size_z": 10.67, - "rotation": { - "x": 0, - "y": 0, - "z": 0, - "type": "Rotation" - }, - "category": "tip_spot", - "model": null, - "prototype_tip": { - "type": "Tip", - "total_tip_length": 39.2, - "has_filter": true, - "maximal_volume": 20.0, - "fitting_depth": 3.29 - } - }, - "data": { - "liquids": [], - "pending_liquids": [], - "liquid_history": [] - } } ], "links": [ diff --git a/unilabos/device_mesh/resource_visalization.py b/unilabos/device_mesh/resource_visalization.py index 2ec28b01..196baf58 100644 --- a/unilabos/device_mesh/resource_visalization.py +++ b/unilabos/device_mesh/resource_visalization.py @@ -76,6 +76,7 @@ class ResourceVisualization: new_dev.set("parent_link", "world") new_dev.set("mesh_path", str(self.mesh_path)) new_dev.set("device_name", node["id"]+"_") + new_dev.set("station_name", node["parent"]+'_') new_dev.set("x",str(float(node["position"]["x"])/1000)) new_dev.set("y",str(float(node["position"]["y"])/1000)) new_dev.set("z",str(float(node["position"]["z"])/1000)) diff --git a/unilabos/resources/graphio.py b/unilabos/resources/graphio.py index 71e4aaeb..d6f64e46 100644 --- a/unilabos/resources/graphio.py +++ b/unilabos/resources/graphio.py @@ -394,14 +394,14 @@ def resource_ulab_to_plr(resource: dict, plr_model=False) -> "ResourcePLR": return resource_plr -def resource_plr_to_ulab(resource_plr: "ResourcePLR"): +def resource_plr_to_ulab(resource_plr: "ResourcePLR", parent_name: str = None): def resource_plr_to_ulab_inner(d: dict, all_states: dict) -> dict: r = { "id": d["name"], "name": d["name"], "sample_id": None, "children": [resource_plr_to_ulab_inner(child, all_states) for child in d["children"]], - "parent": d["parent_name"] if d["parent_name"] else None, + "parent": d["parent_name"] if d["parent_name"] else parent_name if parent_name else None, "type": "device", # FIXME plr自带的type是python class name "class": d.get("class", ""), "position": ( @@ -417,6 +417,7 @@ def resource_plr_to_ulab(resource_plr: "ResourcePLR"): d = resource_plr.serialize() all_states = resource_plr.serialize_all_state() r = resource_plr_to_ulab_inner(d, all_states) + return r @@ -451,7 +452,8 @@ def initialize_resource(resource_config: dict, lab_registry: dict) -> list[dict] if resource_class_config["type"] == "pylabrobot": resource_plr = RESOURCE(name=resource_config["name"]) - r = resource_plr_to_ulab(resource_plr=resource_plr) + r = resource_plr_to_ulab(resource_plr=resource_plr, parent_name=resource_config.get("parent", None)) + # r = resource_plr_to_ulab(resource_plr=resource_plr) if resource_config.get("position") is not None: r["position"] = resource_config["position"] r = tree_to_list([r]) @@ -475,8 +477,9 @@ def initialize_resources(resources_config) -> list[dict]: """ from unilabos.registry.registry import lab_registry - resources = [] for resource_config in resources_config: + resources.extend(initialize_resource(resource_config, lab_registry)) + return resources diff --git a/unilabos/ros/main_slave_run.py b/unilabos/ros/main_slave_run.py index 8560d376..4451831a 100644 --- a/unilabos/ros/main_slave_run.py +++ b/unilabos/ros/main_slave_run.py @@ -72,12 +72,12 @@ def main( resource_mesh_manager = ResourceMeshManager( resources_mesh_config, resources_config, - resource_tracker= DeviceNodeResourceTracker(), + resource_tracker= host_node.resource_tracker, device_id = 'resource_mesh_manager', ) joint_republisher = JointRepublisher( 'joint_republisher', - DeviceNodeResourceTracker() + host_node.resource_tracker ) executor.add_node(resource_mesh_manager) @@ -96,6 +96,8 @@ def slave( graph: Optional[Dict[str, Any]] = None, controllers_config: Dict[str, Any] = {}, bridges: List[Any] = [], + visual: str = "None", + resources_mesh_config: dict = {}, args: List[str] = ["--log-level", "debug"], ) -> None: """从节点函数""" @@ -118,6 +120,21 @@ def slave( n = Node(f"slaveMachine_{BasicConfig.machine_name}", parameter_overrides=[]) executor.add_node(n) + if visual != "None": + resource_mesh_manager = ResourceMeshManager( + resources_mesh_config, + resources_config, + resource_tracker= DeviceNodeResourceTracker(), + device_id = 'resource_mesh_manager', + ) + joint_republisher = JointRepublisher( + 'joint_republisher', + DeviceNodeResourceTracker() + ) + + executor.add_node(resource_mesh_manager) + executor.add_node(joint_republisher) + thread = threading.Thread(target=executor.spin, daemon=True, name="slave_executor_thread") thread.start() diff --git a/unilabos/ros/nodes/presets/resource_mesh_manager.py b/unilabos/ros/nodes/presets/resource_mesh_manager.py index 012404b3..9bd2c3ae 100644 --- a/unilabos/ros/nodes/presets/resource_mesh_manager.py +++ b/unilabos/ros/nodes/presets/resource_mesh_manager.py @@ -1,3 +1,4 @@ +from pathlib import Path import time import rclpy,json from rclpy.node import Node @@ -6,7 +7,7 @@ import numpy as np from moveit_msgs.srv import GetPlanningScene, ApplyPlanningScene from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy, QoSHistoryPolicy -from moveit_msgs.msg import CollisionObject, AttachedCollisionObject, AllowedCollisionEntry +from moveit_msgs.msg import CollisionObject, AttachedCollisionObject, AllowedCollisionEntry, RobotState, PlanningScene from shape_msgs.msg import Mesh, MeshTriangle, SolidPrimitive from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion, TransformStamped from rclpy.callback_groups import ReentrantCallbackGroup @@ -20,6 +21,8 @@ from rclpy.action import ActionServer from unilabos_msgs.action import SendCmd from rclpy.action.server import ServerGoalHandle from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode,DeviceNodeResourceTracker +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): @@ -51,10 +54,10 @@ class ResourceMeshManager(BaseROS2DeviceNode): self.zero_count = 0 self.old_resource_pose = {} - self.__planning_scene = None + self.__planning_scene = PlanningScene() self.__old_planning_scene = None self.__old_allowed_collision_matrix = None - + self.mesh_path = Path(__file__).parent.parent.parent.parent.absolute() callback_group = ReentrantCallbackGroup() self._get_planning_scene_service = self.create_client( @@ -101,11 +104,48 @@ class ResourceMeshManager(BaseROS2DeviceNode): callback_group=callback_group ) - self.resource_mesh_setup() + # 创建一个Action Server用于添加新的资源模型与resource_tf_dict + self._add_resource_mesh_action_server = ActionServer( + self, + SendCmd, + f"add_resource_mesh", + self.add_resource_mesh_callback, + callback_group=callback_group + ) + + self.resource_tf_dict = self.resource_mesh_setup(self.resource_config_dict) self.create_timer(1/self.rate, self.publish_resource_tf) self.create_timer(1/self.rate, self.check_resource_pose_changes) + def add_resource_mesh_callback(self,resource_config_str:str): + """刷新资源配置""" + + registry = lab_registry + resource_config = json.loads(resource_config_str) + + if resource_config['id'] in self.resource_config_dict: + self.get_logger().info(f'资源 {resource_config["id"]} 已存在') + return + if resource_config['class'] in registry.resource_type_registry.keys(): + model_config = registry.resource_type_registry[resource_config['class']]['model'] + if model_config['type'] == 'resource': + self.resource_model[resource_config['id']] = { + 'mesh': f"{str(self.mesh_path)}/device_mesh/resources/{model_config['mesh']}", + 'mesh_tf': model_config['mesh_tf']} + if model_config['children_mesh'] is not None: + self.resource_model[f"{resource_config['id']}_"] = { + 'mesh': f"{str(self.mesh_path)}/device_mesh/resources/{model_config['children_mesh']}", + 'mesh_tf': model_config['children_mesh_tf'] + } + resources = initialize_resources([resource_config]) + resource_dict = {item['id']: item for item in resources} + self.resource_config_dict = {**self.resource_config_dict,**resource_dict} + tf_dict = self.resource_mesh_setup(resource_dict) + self.resource_tf_dict = {**self.resource_tf_dict,**tf_dict} + self.publish_resource_tf() + self.add_resource_collision_meshes(tf_dict) + def check_move_group_ready(self): """检查move_group节点是否已初始化完成""" @@ -113,33 +153,35 @@ class ResourceMeshManager(BaseROS2DeviceNode): # 获取当前可用的节点列表 tf_ready = self.tf_buffer.can_transform("world", next(iter(self.resource_tf_dict.keys())), rclpy.time.Time(),rclpy.duration.Duration(seconds=2)) - + # if tf_ready: if self._get_planning_scene_service.service_is_ready() and self._apply_planning_scene_service.service_is_ready() and tf_ready: self.move_group_ready = True self.publish_resource_tf() - self.add_resource_collision_meshes() + self.add_resource_collision_meshes(self.resource_tf_dict) # time.sleep(1) - - def resource_mesh_setup(self): + def resource_mesh_setup(self, resource_config_dict:dict): """move_group初始化完成后的设置""" self.get_logger().info('开始设置资源网格管理器') #遍历resource_config中的资源配置，判断panent是否在resource_model中， - - for resource_id, resource_config in self.resource_config_dict.items(): + resource_tf_dict = {} + for resource_id, resource_config in resource_config_dict.items(): 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 not in self.resource_model and parent is not None and resource_config['class'] != '': + 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","") + else: + continue # 提取位置信息并转换单位 position = { @@ -163,12 +205,7 @@ class ResourceMeshManager(BaseROS2DeviceNode): float(rotation_dict['y']), float(rotation_dict['z']) ) - # print("-"*20) - # print(f"resource_id: {resource_id}") - # print(f"parent: {parent}") - # print(f"resource_config: {resource_config}") - # print(f"parent_link: {parent_link}") - # print("-"*20) + rotation = { "x": q[0], "y": q[1], @@ -177,12 +214,14 @@ class ResourceMeshManager(BaseROS2DeviceNode): } # 更新资源TF字典 - self.resource_tf_dict[resource_id] = { + resource_tf_dict[resource_id] = { "parent": parent_link, "position": position, "rotation": rotation } + return resource_tf_dict + def publish_resource_tf(self): """ @@ -376,7 +415,7 @@ class ResourceMeshManager(BaseROS2DeviceNode): return SendCmd.Result(success=True) - def add_resource_collision_meshes(self): + def add_resource_collision_meshes(self,resource_tf_dict:dict): """ 遍历资源配置字典，为每个在resource_model中有对应模型的资源添加碰撞网格 @@ -384,8 +423,11 @@ class ResourceMeshManager(BaseROS2DeviceNode): 如果有，则调用add_collision_mesh方法将其添加到碰撞环境中。 """ self.get_logger().info('开始添加资源碰撞网格') - - for resource_id, tf_info in self.resource_tf_dict.items(): + + self.__planning_scene = self._get_planning_scene_service.call( + GetPlanningScene.Request() + ).scene + for resource_id, tf_info in resource_tf_dict.items(): if resource_id in self.resource_model: # 获取位置信息 @@ -405,14 +447,14 @@ class ResourceMeshManager(BaseROS2DeviceNode): ) # 添加碰撞网格 - self.add_collision_mesh( + collision_object = self.get_collision_mesh( filepath=self.resource_model[resource_id]['mesh'], id=resource_id, position=position, quat_xyzw=q, frame_id=resource_id ) - + self.__planning_scene.world.collision_objects.append(collision_object) elif f"{tf_info['parent']}_" in self.resource_model: # 获取资源的父级框架ID id_ = f"{tf_info['parent']}_" @@ -433,15 +475,21 @@ class ResourceMeshManager(BaseROS2DeviceNode): ) # 添加碰撞网格 - self.add_collision_mesh( + collision_object = self.get_collision_mesh( filepath=self.resource_model[id_]['mesh'], id=resource_id, position=position, quat_xyzw=q, frame_id=resource_id ) - time.sleep(0.03) + self.__planning_scene.world.collision_objects.append(collision_object) + + req = ApplyPlanningScene.Request() + req.scene = self.__planning_scene + self._apply_planning_scene_service.call_async(req) + + self.get_logger().info('资源碰撞网格添加完成') @@ -637,6 +685,116 @@ class ResourceMeshManager(BaseROS2DeviceNode): operation=operation, ) + def get_collision_mesh( + self, + filepath: Optional[str], + id: str, + pose: Optional[Union[PoseStamped, Pose]] = None, + position: Optional[Union[Point, Tuple[float, float, float]]] = None, + quat_xyzw: Optional[ + Union[Quaternion, Tuple[float, float, float, float]] + ] = None, + frame_id: Optional[str] = None, + operation: int = CollisionObject.ADD, + scale: Union[float, Tuple[float, float, float]] = 1.0, + mesh: Optional[Any] = None, + ): + """ + Add collision object with a mesh geometry. Either `filepath` must be + specified or `mesh` must be provided. + Note: This function required 'trimesh' Python module to be installed. + """ + + # Load the mesh + try: + import trimesh + except ImportError as err: + raise ImportError( + "Python module 'trimesh' not found! Please install it manually in order " + "to add collision objects into the MoveIt 2 planning scene." + ) from err + + # Check the parameters + if (pose is None) and (position is None or quat_xyzw is None): + raise ValueError( + "Either `pose` or `position` and `quat_xyzw` must be specified!" + ) + if (filepath is None and mesh is None) or ( + filepath is not None and mesh is not None + ): + raise ValueError("Exactly one of `filepath` or `mesh` must be specified!") + if mesh is not None and not isinstance(mesh, trimesh.Trimesh): + raise ValueError("`mesh` must be an instance of `trimesh.Trimesh`!") + + if isinstance(pose, PoseStamped): + pose_stamped = pose + elif isinstance(pose, Pose): + pose_stamped = PoseStamped( + header=Header( + stamp=self.get_clock().now().to_msg(), + frame_id=( + frame_id if frame_id is not None else self.__base_link_name + ), + ), + pose=pose, + ) + else: + if not isinstance(position, Point): + position = Point( + x=float(position[0]), y=float(position[1]), z=float(position[2]) + ) + if not isinstance(quat_xyzw, Quaternion): + quat_xyzw = Quaternion( + x=float(quat_xyzw[0]), + y=float(quat_xyzw[1]), + z=float(quat_xyzw[2]), + w=float(quat_xyzw[3]), + ) + pose_stamped = PoseStamped( + header=Header( + stamp=self.get_clock().now().to_msg(), + frame_id=( + frame_id if frame_id is not None else self.__base_link_name + ), + ), + pose=Pose(position=position, orientation=quat_xyzw), + ) + + msg = CollisionObject( + header=pose_stamped.header, + id=id, + operation=operation, + pose=pose_stamped.pose, + ) + + if filepath is not None: + mesh = trimesh.load(filepath) + + # Scale the mesh + if isinstance(scale, float): + scale = (scale, scale, scale) + if not (scale[0] == scale[1] == scale[2] == 1.0): + # If the mesh was passed in as a parameter, make a copy of it to + # avoid transforming the original. + if filepath is not None: + mesh = mesh.copy() + # Transform the mesh + transform = np.eye(4) + np.fill_diagonal(transform, scale) + mesh.apply_transform(transform) + + msg.meshes.append( + Mesh( + triangles=[MeshTriangle(vertex_indices=face) for face in mesh.faces], + vertices=[ + Point(x=vert[0], y=vert[1], z=vert[2]) for vert in mesh.vertices + ], + ) + ) + + # self.__collision_object_publisher.publish(msg) + return msg + def add_collision_mesh( self, filepath: Optional[str], @@ -743,7 +901,7 @@ class ResourceMeshManager(BaseROS2DeviceNode): ], ) ) - + self.__collision_object_publisher.publish(msg) def remove_collision_object(self, id: str): diff --git a/unilabos/ros/utils/driver_creator.py b/unilabos/ros/utils/driver_creator.py index 2ea30856..a63781ee 100644 --- a/unilabos/ros/utils/driver_creator.py +++ b/unilabos/ros/utils/driver_creator.py @@ -6,6 +6,7 @@ """ import asyncio import inspect +import json import traceback from abc import abstractmethod from typing import Type, Any, Dict, Optional, TypeVar, Generic