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)