修改物料放下时的方法，如果选择

修改物料放下时的方法，
如果选择drop_trash，则删除物料显示
如果选择drop，则让其解除连接

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)
             
             # 发送目标

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)