From 554766924e4048ef3d06374ffcb289f89dadb06a Mon Sep 17 00:00:00 2001 From: zhangshixiang <@zhangshixiang> Date: Tue, 20 May 2025 19:08:22 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E7=89=A9=E6=96=99=E6=94=BE?= =?UTF-8?q?=E4=B8=8B=E6=97=B6=E7=9A=84=E6=96=B9=E6=B3=95=EF=BC=8C=E5=A6=82?= =?UTF-8?q?=E6=9E=9C=E9=80=89=E6=8B=A9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 修改物料放下时的方法， 如果选择drop_trash，则删除物料显示 如果选择drop，则让其解除连接 --- .../ros_dev/liquid_handler_joint_publisher.py | 8 +++---- .../nodes/presets/resource_mesh_manager.py | 23 +++++++++++-------- 2 files changed, 18 insertions(+), 13 deletions(-) diff --git a/unilabos/devices/ros_dev/liquid_handler_joint_publisher.py b/unilabos/devices/ros_dev/liquid_handler_joint_publisher.py index 9a1f1f04..f5fa62a7 100644 --- a/unilabos/devices/ros_dev/liquid_handler_joint_publisher.py +++ b/unilabos/devices/ros_dev/liquid_handler_joint_publisher.py @@ -165,9 +165,6 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode): goal_handle.abort() result.success = False - print('='*20) - print(result) - print('='*20) return result def inverse_kinematics(self, x, y, z, parent_id, @@ -251,6 +248,9 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode): link_name = f'{parent_id}_{link_name}' self.resource_move(resource_name_, link_name, [0,1,2,3,4,5,6,7]) time.sleep(1) + elif option == "drop_trash": + self.resource_move(resource_name_, "__trash", [0,1,2,3,4,5,6,7]) + time.sleep(1) elif option == "drop": self.resource_move(resource_name_, "world", [0,1,2,3,4,5,6,7]) time.sleep(1) @@ -345,7 +345,7 @@ class JointStatePublisher(Node): try: # 创建新的executor - executor = rclpy.executors.SingleThreadedExecutor() + executor = rclpy.executors.MultiThreadedExecutor() executor.add_node(self) # 发送目标 diff --git a/unilabos/ros/nodes/presets/resource_mesh_manager.py b/unilabos/ros/nodes/presets/resource_mesh_manager.py index 0ceb5807..afa90568 100644 --- a/unilabos/ros/nodes/presets/resource_mesh_manager.py +++ b/unilabos/ros/nodes/presets/resource_mesh_manager.py @@ -390,10 +390,12 @@ class ResourceMeshManager(BaseROS2DeviceNode): planning_scene.is_diff = True planning_scene.robot_state.is_diff = True for resource_id, target_parent in cmd_dict.items(): - + parent_id = target_parent + if target_parent == '__trash': + parent_id = 'world' # 获取从resource_id到target_parent的转换 transform = self.tf_buffer.lookup_transform( - target_parent, + parent_id, resource_id, rclpy.time.Time(seconds=0) ) @@ -413,7 +415,7 @@ class ResourceMeshManager(BaseROS2DeviceNode): } self.resource_tf_dict[resource_id] = { - "parent": target_parent, + "parent": parent_id, "position": position, "rotation": rotation } @@ -422,16 +424,19 @@ class ResourceMeshManager(BaseROS2DeviceNode): # self.attach_collision_object(id=resource_id,link_name=target_parent) # time.sleep(0.02) operation_attach = CollisionObject.ADD - operation_remove = CollisionObject.REMOVE + operation_world = CollisionObject.REMOVE if target_parent == 'world': operation_attach = CollisionObject.REMOVE - operation_remove = CollisionObject.ADD + operation_world = CollisionObject.ADD + elif target_parent == '__trash': + operation_attach = CollisionObject.REMOVE - remove_object = CollisionObject( + world_object = CollisionObject( id=resource_id, - operation=operation_remove + operation=operation_world ) - planning_scene.world.collision_objects.append(remove_object) + if target_parent != '__trash': + planning_scene.world.collision_objects.append(world_object) collision_object = AttachedCollisionObject( @@ -440,7 +445,7 @@ class ResourceMeshManager(BaseROS2DeviceNode): operation=operation_attach ) ) - if target_parent != 'world': + if target_parent != 'world' and target_parent != '__trash': collision_object.link_name = target_parent planning_scene.robot_state.attached_collision_objects.append(collision_object)