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

Revert "修改物体attach时，多次赋值当前时间导致卡顿问题，"

Browse Source 
This reverts commit 56d45b94f5.
This commit is contained in:
zhangshixiang
2025-06-07 22:52:03 +08:00
parent 56d45b94f5
commit 07d9db20c3
3 changed files with 36 additions and 143 deletions
Download Patch File Download Diff File Expand all files Collapse all files

139
unilabos/device_mesh/view_robot.rviz
View File

@@ -4,12 +4,15 @@ Panels:
Name: Displays
Property Tree Widget:
Expanded:
- /TF1/Frames1
- /TF1/Tree1
- /PlanningScene1/Scene Geometry1
- /MotionPlanning1
- /MotionPlanning1/Scene Geometry1
- /MotionPlanning1/Scene Robot1
- /MotionPlanning1/Planning Request1
Splitter Ratio: 0.5016146302223206
Tree Height: 1112
Splitter Ratio: 0.4940796494483948
Tree Height: 602
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
@@ -53,7 +56,7 @@ Visualization Manager:
Name: TF
Show Arrows: true
Show Axes: true
Show Names: false
Show Names: true
Tree:
{}
Update Interval: 0
@@ -85,7 +88,7 @@ Visualization Manager:
Value: false
Visual Enabled: true
- Class: moveit_rviz_plugin/PlanningScene
Enabled: true
Enabled: false
Move Group Namespace: ""
Name: PlanningScene
Planning Scene Topic: /monitored_planning_scene
@@ -96,7 +99,7 @@ Visualization Manager:
Scene Display Time: 0.009999999776482582
Show Scene Geometry: true
Voxel Coloring: Z-Axis
Voxel Rendering: Disabled
Voxel Rendering: Occupied Voxels
Scene Robot:
Attached Body Color: 150; 50; 150
Links:
@@ -105,108 +108,10 @@ Visualization Manager:
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
PLR_STATION_deck_device_link:
Alpha: 1
Show Axes: false
Show Trail: false
PLR_STATION_deck_first_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
PLR_STATION_deck_fourth_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
PLR_STATION_deck_main_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
PLR_STATION_deck_second_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
PLR_STATION_deck_socketTypeGenericSbsFootprint:
Alpha: 1
Show Axes: false
Show Trail: false
PLR_STATION_deck_socketTypeHEPAModule:
Alpha: 1
Show Axes: false
Show Trail: false
PLR_STATION_deck_third_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_arm_base:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_arm_link_1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_arm_link_2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_arm_link_3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_arm_slideway:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_device_link:
Alpha: 1
Show Axes: false
Show Trail: false
benyao_gripper_base:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_gripper_left:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_gripper_right:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
hotel_base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
hotel_device_link:
Alpha: 1
Show Axes: false
Show Trail: false
hotel_socketTypeGenericSbsFootprint:
Alpha: 1
Show Axes: false
Show Trail: false
world:
Alpha: 1
Show Axes: false
Show Trail: false
Robot Alpha: 1
Show Robot Collision: false
Show Robot Visual: false
Value: true
Value: false
- Attached Body Color: 150; 50; 150
Class: moveit_rviz_plugin/RobotState
Collision Enabled: false
@@ -389,9 +294,9 @@ Visualization Manager:
Scene Alpha: 0.8999999761581421
Scene Color: 50; 230; 50
Scene Display Time: 0.009999999776482582
Show Scene Geometry: false
Voxel Coloring: Cell Probability
Voxel Rendering: All Voxels
Show Scene Geometry: true
Voxel Coloring: Z-Axis
Voxel Rendering: Occupied Voxels
Scene Robot:
Attached Body Color: 150; 50; 150
Links:
@@ -564,28 +469,28 @@ Visualization Manager:
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.48479583859443665
Pitch: 0.634795606136322
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.042561568319797516
Yaw: 5.590744495391846
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 2032
Hide Left Dock: false
collapsed: true
Height: 1897
Hide Left Dock: true
Hide Right Dock: true
MotionPlanning:
collapsed: false
collapsed: true
MotionPlanning - Trajectory Slider:
collapsed: false
QMainWindow State: 000000ff00000000fd0000000400000000000003a30000079bfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000027000004c60000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000002600000026fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000004f9000002c9000002b800ffffff000000010000010f00000387fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003870000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000bc50000079b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd0000000400000000000003a300000714fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000027000002c80000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000002600000026fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000002fb00000440000002b800ffffff000000010000010f00000387fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003870000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000ddb0000071400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 3956
X: 140
Y: 54
Width: 3547
X: -857
Y: 149

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

@@ -118,7 +118,7 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
str_dict[resource] = link_name
goal_msg.command = json.dumps(str_dict)
self.resource_action_client.send_goal(goal_msg)
self.resource_action_client.send_goal_async(goal_msg)
def resource_move(self, resource_id:str, link_name:str, channels:list[int]):
resource = resource_id.rsplit("_",1)
@@ -247,10 +247,13 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
link_name = self.lh_devices[parent_id]['joint_config']['link_names'][z_index]
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)
self.move_to(joint_positions_target_zero, speed, parent_id)

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

@@ -161,7 +161,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
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 'children_mesh' in model_config.keys():
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']
@@ -300,7 +300,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
"world",
resource_id,
rclpy.time.Time(seconds=0),
# rclpy.duration.Duration(seconds=5)
rclpy.duration.Duration(seconds=5)
)
# 提取当前位姿信息
@@ -391,7 +391,6 @@ class ResourceMeshManager(BaseROS2DeviceNode):
planning_scene = PlanningScene()
planning_scene.is_diff = True
planning_scene.robot_state.is_diff = True
time_start = self.get_clock().now()
for resource_id, target_parent in cmd_dict.items():
parent_id = target_parent
if target_parent == '__trash':
@@ -400,7 +399,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
transform = self.tf_buffer.lookup_transform(
parent_id,
resource_id,
time_start,
self.get_clock().now(),
timeout=rclpy.duration.Duration(seconds=10)
)
@@ -464,12 +463,11 @@ class ResourceMeshManager(BaseROS2DeviceNode):
# self.__planning_scene.robot_state.attached_collision_objects.append(collision_object)
req = ApplyPlanningScene.Request()
req.scene = planning_scene
self.publish_resource_tf()
self._apply_planning_scene_service.call(req)
self._apply_planning_scene_service.call_async(req)
self.__planning_scene_publisher.publish(planning_scene)
# self.__collision_object_publisher.publish(CollisionObject())
self.publish_resource_tf()
except Exception as e:
self.get_logger().error(f"更新资源TF字典失败: {e}")
@@ -485,7 +483,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
遍历资源配置字典，为每个在resource_model中有对应模型的资源添加碰撞网格
该方法检查每个资源ID是否在self.resource_model中有对应的3D模型文件路径，
如果有，则调用add_collision_mesh方法将其添加到碰撞环境中。
"""
self.get_logger().info('开始添加资源碰撞网格')
@@ -494,7 +492,6 @@ class ResourceMeshManager(BaseROS2DeviceNode):
).scene
planning_scene = PlanningScene()
planning_scene.is_diff = True
count = 0
for resource_id, tf_info in resource_tf_dict.items():
if resource_id in self.resource_model:
@@ -522,7 +519,6 @@ class ResourceMeshManager(BaseROS2DeviceNode):
quat_xyzw=q,
frame_id=resource_id
)
count += 1
planning_scene.world.collision_objects.append(collision_object)
elif f"{tf_info['parent']}_" in self.resource_model:
# 获取资源的父级框架ID
@@ -551,26 +547,15 @@ class ResourceMeshManager(BaseROS2DeviceNode):
quat_xyzw=q,
frame_id=resource_id
)
count += 1
planning_scene.world.collision_objects.append(collision_object)
if count > 30:
req = ApplyPlanningScene.Request()
req.scene = planning_scene
self.publish_resource_tf()
self._apply_planning_scene_service.call(req)
self.__planning_scene_publisher.publish(planning_scene)
count = 0
planning_scene = PlanningScene()
planning_scene.is_diff = True
planning_scene.world.collision_objects.append(collision_object)
req = ApplyPlanningScene.Request()
req.scene = planning_scene
self.publish_resource_tf()
self._apply_planning_scene_service.call(req)
self._apply_planning_scene_service.call_async(req)
self.__planning_scene_publisher.publish(planning_scene)
self.publish_resource_tf()
self.get_logger().info('资源碰撞网格添加完成')