open_pub/Uni-Lab-OS
1
0
Fork 0
mirror of https://github.com/dptech-corp/Uni-Lab-OS.git synced 2025-12-17 04:51:10 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
修改物料放下时的方法，
如果选择drop_trash，则删除物料显示
如果选择drop，则让其解除连接
This commit is contained in:
zhangshixiang
2025-05-20 19:08:22 +08:00
parent aa85a1f3a2
commit 554766924e
2 changed files with 18 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
unilabos/devices/ros_dev/liquid_handler_joint_publisher.py
View File

@@ -165,9 +165,6 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
goal_handle.abort() goal_handle.abort()
result.success = False result.success = False
print('='*20)
print(result)
print('='*20)
return result return result
def inverse_kinematics(self, x, y, z, def inverse_kinematics(self, x, y, z,
parent_id, parent_id,
@@ -251,6 +248,9 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
link_name = f'{parent_id}_{link_name}' link_name = f'{parent_id}_{link_name}'
self.resource_move(resource_name_, link_name, [0,1,2,3,4,5,6,7]) self.resource_move(resource_name_, link_name, [0,1,2,3,4,5,6,7])
time.sleep(1) 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": elif option == "drop":
self.resource_move(resource_name_, "world", [0,1,2,3,4,5,6,7]) self.resource_move(resource_name_, "world", [0,1,2,3,4,5,6,7])
time.sleep(1) time.sleep(1)
@@ -345,7 +345,7 @@ class JointStatePublisher(Node):
try: try:
# 创建新的executor # 创建新的executor
executor = rclpy.executors.SingleThreadedExecutor() executor = rclpy.executors.MultiThreadedExecutor()
executor.add_node(self) executor.add_node(self)
# 发送目标 # 发送目标

23
unilabos/ros/nodes/presets/resource_mesh_manager.py
View File

@@ -390,10 +390,12 @@ class ResourceMeshManager(BaseROS2DeviceNode):
planning_scene.is_diff = True planning_scene.is_diff = True
planning_scene.robot_state.is_diff = True planning_scene.robot_state.is_diff = True
for resource_id, target_parent in cmd_dict.items(): for resource_id, target_parent in cmd_dict.items():
parent_id = target_parent
if target_parent == '__trash':
parent_id = 'world'
# 获取从resource_id到target_parent的转换 # 获取从resource_id到target_parent的转换
transform = self.tf_buffer.lookup_transform( transform = self.tf_buffer.lookup_transform(
target_parent, parent_id,
resource_id, resource_id,
rclpy.time.Time(seconds=0) rclpy.time.Time(seconds=0)
) )
@@ -413,7 +415,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
} }
self.resource_tf_dict[resource_id] = { self.resource_tf_dict[resource_id] = {
"parent": target_parent, "parent": parent_id,
"position": position, "position": position,
"rotation": rotation "rotation": rotation
} }
@@ -422,16 +424,19 @@ class ResourceMeshManager(BaseROS2DeviceNode):
# self.attach_collision_object(id=resource_id,link_name=target_parent) # self.attach_collision_object(id=resource_id,link_name=target_parent)
# time.sleep(0.02) # time.sleep(0.02)
operation_attach = CollisionObject.ADD operation_attach = CollisionObject.ADD
operation_remove = CollisionObject.REMOVE operation_world = CollisionObject.REMOVE
if target_parent == 'world': if target_parent == 'world':
operation_attach = CollisionObject.REMOVE 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, 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( collision_object = AttachedCollisionObject(
@@ -440,7 +445,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
operation=operation_attach operation=operation_attach
) )
) )
if target_parent != 'world': if target_parent != 'world' and target_parent != '__trash':
collision_object.link_name = target_parent collision_object.link_name = target_parent
planning_scene.robot_state.attached_collision_objects.append(collision_object) planning_scene.robot_state.attached_collision_objects.append(collision_object)