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

修改模型方向，在yaml中添加变换属性

Browse Source
This commit is contained in:
zhangshixiang
2025-04-25 10:47:46 +08:00
committed by Junhan Chang
parent 0d24606d46
commit 2990e70c25
7 changed files with 1173 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
unilabos/app/main.py
View File

@@ -166,10 +166,9 @@ def main():
resource_visualization = ResourceVisualization(args_dict["devices_config"], args_dict["resources_config"],registry_dict)
start_backend(**args_dict)
print('-'*100)
print(resource_visualization.resource_model)
print(json.dumps(args_dict["resources_config"], indent=4, ensure_ascii=False))
print('-'*100)
# print('-'*100)
# print(resource_visualization.resource_model)
# print('-'*100)
server_thread = threading.Thread(target=start_server)
server_thread.start()

4
unilabos/device_mesh/devices/opentrons_liquid_handler/macro_device.xacro
View File

@@ -8,14 +8,14 @@
<joint name="${station_name}${device_name}base_link_joint" type="fixed">
<origin xyz="${x} ${y} ${z}" rpy="0 0 ${r}" />
<parent link="${station_name}${device_name}${parent_link}"/>
<parent link="${parent_link}"/>
<child link="${station_name}${device_name}device_link"/>
<axis xyz="0 0 0"/>
</joint>
<link name="${station_name}${device_name}device_link"/>
<joint name="${station_name}${device_name}device_link_joint" type="fixed">
<origin xyz="-0.31 0.28 0" rpy="0 0 0" />
<origin xyz="-0.11565 0.496 0" rpy="0 0 0" />
<parent link="${station_name}${device_name}device_link"/>
<child link="${station_name}${device_name}main_link"/>
<axis xyz="0 0 0"/>

40
unilabos/device_mesh/resource_visalization.py
View File

@@ -28,7 +28,12 @@ class ResourceVisualization:
self.mesh_path = Path(__file__).parent.absolute()
self.enable_rviz = enable_rviz
self.srdf_str = '''
<?xml version="1.0" encoding="UTF-8"?>
<robot name="minimal">
</robot>
'''
self.robot_state_str= '''<?xml version="1.0" ?>
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="full_dev">
<link name="world"/>
@@ -56,6 +61,13 @@ class ResourceVisualization:
new_dev = etree.SubElement(self.root, f"{{{xacro_uri}}}{model_config['mesh']}")
new_dev.set("parent_link", "world")
new_dev.set("mesh_path", str(self.mesh_path))
new_dev.set("device_name", node["id"]+"_")
new_dev.set("x",str(float(node["position"]["x"])/1000))
new_dev.set("y",str(float(node["position"]["y"])/1000))
new_dev.set("z",str(float(node["position"]["z"])/1000))
if "rotation" in node["config"]:
new_dev.set("r",str(float(node["config"]["rotation"]["z"])/1000))
elif node['type'] in self.resource_type:
# print(registry.resource_type_registry)
@@ -65,9 +77,14 @@ class ResourceVisualization:
elif "model" in registry.resource_type_registry[resource_class].keys():
model_config = registry.resource_type_registry[resource_class]['model']
if model_config['type'] == 'resource':
self.resource_model[node['id']] = f"{str(self.mesh_path)}/resources/{model_config['mesh']}"
self.resource_model[node['id']] = {
'mesh': f"{str(self.mesh_path)}/resources/{model_config['mesh']}",
'mesh_tf': model_config['mesh_tf']}
if model_config['children_mesh'] is not None:
self.resource_model[f"{node['id']}_"] = f"{str(self.mesh_path)}/resources/{model_config['children_mesh']}"
self.resource_model[f"{node['id']}_"] = {
'mesh': f"{str(self.mesh_path)}/resources/{model_config['children_mesh']}",
'mesh_tf': model_config['children_mesh_tf']
}
re = etree.tostring(self.root, encoding="unicode")
doc = xacro.parse(re)
@@ -102,23 +119,35 @@ class ResourceVisualization:
}]
)
joint_state_publisher_node = nd(
package='joint_state_publisher_gui', # 或 joint_state_publisher
executable='joint_state_publisher_gui',
name='joint_state_publisher',
output='screen'
)
# 创建move_group节点
move_group = nd(
package='moveit_ros_move_group',
executable='move_group',
output='screen',
parameters=[{
'robot_description': robot_description,
'allow_trajectory_execution': True,
'robot_description': self.robot_state_str,
'robot_description_semantic': self.srdf_str,
'capabilities': '',
'disable_capabilities': '',
'monitor_dynamics': False,
'publish_monitored_planning_scene': True
'publish_monitored_planning_scene': True,
'publish_robot_description_semantic': True,
'publish_planning_scene': True,
'publish_geometry_updates': True,
'publish_state_updates': True,
'publish_transforms_updates': True,
}]
)
# 将节点添加到launch描述中
self.launch_description.add_action(robot_state_publisher)
self.launch_description.add_action(joint_state_publisher_node)
self.launch_description.add_action(move_group)
# 如果启用RViz,添加RViz节点
@@ -127,6 +156,7 @@ class ResourceVisualization:
package='rviz2',
executable='rviz2',
name='rviz2',
arguments=['-d', f"{str(self.mesh_path)}/view_robot.rviz"],
output='screen'
)
self.launch_description.add_action(rviz_node)

923
unilabos/device_mesh/view_robot.rviz Normal file
View File

@@ -0,0 +1,923 @@
Panels:
- Class: rviz_common/Displays
Help Height: 138
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /TF1
- /TF1/Tree1
- /RobotModel1
- /PlanningScene1
- /PlanningScene1/Scene Geometry1
- /RobotState1
- /RobotState1/Links1
Splitter Ratio: 0.5
Tree Height: 275
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Goal Pose1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Class: rviz_default_plugins/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: false
Gripper1_device_link:
Value: false
Gripper1_first_link:
Value: true
Gripper1_fourth_link:
Value: false
Gripper1_main_link:
Value: false
Gripper1_second_link:
Value: false
Gripper1_socketTypeGenericSbsFootprint:
Value: false
Gripper1_socketTypeHEPAModule:
Value: false
Gripper1_third_link:
Value: false
Plate1:
Value: false
Plate1_A1:
Value: false
Plate1_A10:
Value: false
Plate1_A11:
Value: false
Plate1_A12:
Value: false
Plate1_A2:
Value: false
Plate1_A3:
Value: false
Plate1_A4:
Value: false
Plate1_A5:
Value: false
Plate1_A6:
Value: false
Plate1_A7:
Value: false
Plate1_A8:
Value: false
Plate1_A9:
Value: false
Plate1_B1:
Value: false
Plate1_B10:
Value: false
Plate1_B11:
Value: false
Plate1_B12:
Value: false
Plate1_B2:
Value: false
Plate1_B3:
Value: false
Plate1_B4:
Value: false
Plate1_B5:
Value: false
Plate1_B6:
Value: false
Plate1_B7:
Value: false
Plate1_B8:
Value: false
Plate1_B9:
Value: false
Plate1_C1:
Value: false
Plate1_C10:
Value: false
Plate1_C11:
Value: false
Plate1_C12:
Value: false
Plate1_C2:
Value: false
Plate1_C3:
Value: false
Plate1_C4:
Value: false
Plate1_C5:
Value: false
Plate1_C6:
Value: false
Plate1_C7:
Value: false
Plate1_C8:
Value: false
Plate1_C9:
Value: false
Plate1_D1:
Value: false
Plate1_D10:
Value: false
Plate1_D11:
Value: false
Plate1_D12:
Value: false
Plate1_D2:
Value: false
Plate1_D3:
Value: false
Plate1_D4:
Value: false
Plate1_D5:
Value: false
Plate1_D6:
Value: false
Plate1_D7:
Value: false
Plate1_D8:
Value: false
Plate1_D9:
Value: false
Plate1_E1:
Value: false
Plate1_E10:
Value: false
Plate1_E11:
Value: false
Plate1_E12:
Value: false
Plate1_E2:
Value: false
Plate1_E3:
Value: false
Plate1_E4:
Value: false
Plate1_E5:
Value: false
Plate1_E6:
Value: false
Plate1_E7:
Value: false
Plate1_E8:
Value: false
Plate1_E9:
Value: false
Plate1_F1:
Value: false
Plate1_F10:
Value: false
Plate1_F11:
Value: false
Plate1_F12:
Value: false
Plate1_F2:
Value: false
Plate1_F3:
Value: false
Plate1_F4:
Value: false
Plate1_F5:
Value: false
Plate1_F6:
Value: false
Plate1_F7:
Value: false
Plate1_F8:
Value: false
Plate1_F9:
Value: false
Plate1_G1:
Value: false
Plate1_G10:
Value: false
Plate1_G11:
Value: false
Plate1_G12:
Value: false
Plate1_G2:
Value: false
Plate1_G3:
Value: false
Plate1_G4:
Value: false
Plate1_G5:
Value: false
Plate1_G6:
Value: false
Plate1_G7:
Value: false
Plate1_G8:
Value: false
Plate1_G9:
Value: false
Plate1_H1:
Value: false
Plate1_H10:
Value: false
Plate1_H11:
Value: false
Plate1_H12:
Value: false
Plate1_H2:
Value: false
Plate1_H3:
Value: false
Plate1_H4:
Value: false
Plate1_H5:
Value: false
Plate1_H6:
Value: false
Plate1_H7:
Value: false
Plate1_H8:
Value: false
Plate1_H9:
Value: false
world:
Value: false
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
world:
Gripper1_device_link:
Gripper1_main_link:
Gripper1_first_link:
Gripper1_second_link:
Gripper1_fourth_link:
{}
Gripper1_third_link:
{}
Plate1_C6:
{}
Gripper1_socketTypeGenericSbsFootprint:
{}
Gripper1_socketTypeHEPAModule:
{}
Plate1:
Plate1_A1:
{}
Plate1_A10:
{}
Plate1_A11:
{}
Plate1_A12:
{}
Plate1_A2:
{}
Plate1_A3:
{}
Plate1_A4:
{}
Plate1_A5:
{}
Plate1_A6:
{}
Plate1_A7:
{}
Plate1_A8:
{}
Plate1_A9:
{}
Plate1_B1:
{}
Plate1_B10:
{}
Plate1_B11:
{}
Plate1_B12:
{}
Plate1_B2:
{}
Plate1_B3:
{}
Plate1_B4:
{}
Plate1_B5:
{}
Plate1_B6:
{}
Plate1_B7:
{}
Plate1_B8:
{}
Plate1_B9:
{}
Plate1_C1:
{}
Plate1_C10:
{}
Plate1_C11:
{}
Plate1_C12:
{}
Plate1_C2:
{}
Plate1_C3:
{}
Plate1_C4:
{}
Plate1_C7:
{}
Plate1_C8:
{}
Plate1_C9:
{}
Plate1_D1:
{}
Plate1_D10:
{}
Plate1_D11:
{}
Plate1_D12:
{}
Plate1_D2:
{}
Plate1_D3:
{}
Plate1_D4:
{}
Plate1_D5:
{}
Plate1_D6:
{}
Plate1_D7:
{}
Plate1_D8:
{}
Plate1_D9:
{}
Plate1_E1:
{}
Plate1_E10:
{}
Plate1_E11:
{}
Plate1_E12:
{}
Plate1_E2:
{}
Plate1_E3:
{}
Plate1_E4:
{}
Plate1_E5:
{}
Plate1_E6:
{}
Plate1_E7:
{}
Plate1_E8:
{}
Plate1_E9:
{}
Plate1_F1:
{}
Plate1_F10:
{}
Plate1_F11:
{}
Plate1_F12:
{}
Plate1_F2:
{}
Plate1_F3:
{}
Plate1_F4:
{}
Plate1_F5:
{}
Plate1_F6:
{}
Plate1_F7:
{}
Plate1_F8:
{}
Plate1_F9:
{}
Plate1_G1:
{}
Plate1_G10:
{}
Plate1_G11:
{}
Plate1_G12:
{}
Plate1_G2:
{}
Plate1_G3:
{}
Plate1_G4:
{}
Plate1_G5:
{}
Plate1_G6:
{}
Plate1_G7:
{}
Plate1_G8:
{}
Plate1_G9:
{}
Plate1_H1:
{}
Plate1_H10:
{}
Plate1_H11:
{}
Plate1_H12:
{}
Plate1_H2:
{}
Plate1_H3:
{}
Plate1_H4:
{}
Plate1_H5:
{}
Plate1_H6:
{}
Plate1_H7:
{}
Plate1_H8:
{}
Plate1_H9:
{}
Plate1_C5:
{}
Update Interval: 0
Value: true
- Alpha: 1
Class: rviz_default_plugins/RobotModel
Collision Enabled: false
Description File: ""
Description Source: Topic
Description Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /robot_description
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Gripper1_device_link:
Alpha: 1
Show Axes: false
Show Trail: false
Gripper1_first_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_fourth_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_main_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_second_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_socketTypeGenericSbsFootprint:
Alpha: 1
Show Axes: false
Show Trail: false
Gripper1_socketTypeHEPAModule:
Alpha: 1
Show Axes: false
Show Trail: false
Gripper1_third_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Link Tree Style: Links in Alphabetic Order
world:
Alpha: 1
Show Axes: false
Show Trail: false
Mass Properties:
Inertia: false
Mass: false
Name: RobotModel
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: moveit_rviz_plugin/PlanningScene
Enabled: false
Move Group Namespace: ""
Name: PlanningScene
Planning Scene Topic: /monitored_planning_scene
Robot Description: robot_description
Scene Geometry:
Scene Alpha: 0.8999999761581421
Scene Color: 50; 230; 50
Scene Display Time: 0.009999999776482582
Show Scene Geometry: true
Voxel Coloring: Z-Axis
Voxel Rendering: Occupied Voxels
Scene Robot:
Attached Body Color: 150; 50; 150
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Gripper1_device_link:
Alpha: 1
Show Axes: false
Show Trail: false
Gripper1_first_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_fourth_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_main_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_second_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_socketTypeGenericSbsFootprint:
Alpha: 1
Show Axes: false
Show Trail: false
Gripper1_socketTypeHEPAModule:
Alpha: 1
Show Axes: false
Show Trail: false
Gripper1_third_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Link Tree Style: Links in Alphabetic Order
world:
Alpha: 1
Show Axes: false
Show Trail: false
Robot Alpha: 1
Show Robot Collision: false
Show Robot Visual: false
Value: false
- Attached Body Color: 150; 50; 150
Class: moveit_rviz_plugin/RobotState
Collision Enabled: false
Enabled: false
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Gripper1_device_link:
Alpha: 1
Show Axes: false
Show Trail: false
Gripper1_first_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_fourth_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_main_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_second_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_socketTypeGenericSbsFootprint:
Alpha: 1
Show Axes: false
Show Trail: false
Gripper1_socketTypeHEPAModule:
Alpha: 1
Show Axes: false
Show Trail: false
Gripper1_third_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Link Tree Style: Links in Alphabetic Order
world:
Alpha: 1
Show Axes: false
Show Trail: false
Name: RobotState
Robot Alpha: 1
Robot Description: robot_description
Robot State Topic: display_robot_state
Show All Links: true
Show Highlights: true
Value: false
Visual Enabled: true
- Acceleration_Scaling_Factor: 0.1
Class: moveit_rviz_plugin/MotionPlanning
Enabled: true
Move Group Namespace: ""
MoveIt_Allow_Approximate_IK: false
MoveIt_Allow_External_Program: false
MoveIt_Allow_Replanning: false
MoveIt_Allow_Sensor_Positioning: false
MoveIt_Planning_Attempts: 10
MoveIt_Planning_Time: 5
MoveIt_Use_Cartesian_Path: false
MoveIt_Use_Constraint_Aware_IK: false
MoveIt_Workspace:
Center:
X: 0
Y: 0
Z: 0
Size:
X: 2
Y: 2
Z: 2
Name: MotionPlanning
Planned Path:
Color Enabled: false
Interrupt Display: false
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Gripper1_device_link:
Alpha: 1
Show Axes: false
Show Trail: false
Gripper1_first_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_fourth_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_main_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_second_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_socketTypeGenericSbsFootprint:
Alpha: 1
Show Axes: false
Show Trail: false
Gripper1_socketTypeHEPAModule:
Alpha: 1
Show Axes: false
Show Trail: false
Gripper1_third_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Link Tree Style: Links in Alphabetic Order
world:
Alpha: 1
Show Axes: false
Show Trail: false
Loop Animation: false
Robot Alpha: 0.5
Robot Color: 150; 50; 150
Show Robot Collision: false
Show Robot Visual: true
Show Trail: false
State Display Time: 3x
Trail Step Size: 1
Trajectory Topic: /display_planned_path
Use Sim Time: false
Planning Metrics:
Payload: 1
Show Joint Torques: false
Show Manipulability: false
Show Manipulability Index: false
Show Weight Limit: false
TextHeight: 0.07999999821186066
Planning Request:
Colliding Link Color: 255; 0; 0
Goal State Alpha: 1
Goal State Color: 250; 128; 0
Interactive Marker Size: 0
Joint Violation Color: 255; 0; 255
Planning Group: ""
Query Goal State: true
Query Start State: false
Show Workspace: false
Start State Alpha: 1
Start State Color: 0; 255; 0
Planning Scene Topic: /monitored_planning_scene
Robot Description: robot_description
Scene Geometry:
Scene Alpha: 0.8999999761581421
Scene Color: 50; 230; 50
Scene Display Time: 0.009999999776482582
Show Scene Geometry: true
Voxel Coloring: Z-Axis
Voxel Rendering: Occupied Voxels
Scene Robot:
Attached Body Color: 150; 50; 150
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Gripper1_device_link:
Alpha: 1
Show Axes: false
Show Trail: false
Gripper1_first_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_fourth_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_main_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_second_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_socketTypeGenericSbsFootprint:
Alpha: 1
Show Axes: false
Show Trail: false
Gripper1_socketTypeHEPAModule:
Alpha: 1
Show Axes: false
Show Trail: false
Gripper1_third_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Link Tree Style: Links in Alphabetic Order
world:
Alpha: 1
Show Axes: false
Show Trail: false
Robot Alpha: 1
Show Robot Collision: false
Show Robot Visual: true
Value: true
Velocity_Scaling_Factor: 0.1
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: world
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Covariance x: 0.25
Covariance y: 0.25
Covariance yaw: 0.06853891909122467
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /goal_pose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 1.49553644657135
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 1.2573390007019043
Y: 1.1951926946640015
Z: 0.23975235223770142
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.5797955989837646
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 6.250748634338379
Saved: ~
Window Geometry:
"":
collapsed: false
" - Trajectory Slider":
collapsed: false
Displays:
collapsed: false
Height: 1656
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd0000000400000000000003a3000005dcfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e0000020b0000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000007a00fffffffbffffffff0100000285000003c5000002b700ffffff000000010000010f00000387fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003870000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000627000005dc00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 2518
X: 125
Y: 145

252
unilabos/devices/ros_dev/resource_mesh_manager.py
View File

@@ -15,8 +15,10 @@ from rclpy.task import Future
import copy
from typing import Tuple, Optional, Union, Any, List
from tf_transformations import quaternion_from_euler
from tf2_ros import StaticTransformBroadcaster
from tf2_ros import TransformBroadcaster, Buffer, TransformListener
from rclpy.action import ActionServer
from unilabos_msgs.action import SendCmd
from rclpy.action.server import ServerGoalHandle
class ResourceMeshManager(Node):
def __init__(self, resource_model: dict, resource_config: list, node_name: str):
@@ -30,13 +32,13 @@ class ResourceMeshManager(Node):
super().__init__(node_name)
self.resource_model = resource_model
self.resource_config = resource_config
self.resource_config_dict = {item['id']: item for item in self.resource_config}
self.resource_config_dict = {item['id']: item for item in resource_config}
self.move_group_ready = False
self.resource_tf_dict = {}
self.tf_broadcaster = StaticTransformBroadcaster(self)
self.create_timer(1, self.publish_resource_tf)
self.tf_broadcaster = TransformBroadcaster(self)
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
self.create_timer(0.02, self.publish_resource_tf)
callback_group = ReentrantCallbackGroup()
self._get_planning_scene_service = self.create_client(
@@ -74,9 +76,65 @@ class ResourceMeshManager(Node):
AttachedCollisionObject, "/attached_collision_object", 10
)
# 创建一个Action Server用于修改resource_tf_dict
self._action_server = ActionServer(
self,
SendCmd,
f'{node_name}/tf_update',
self.tf_update,
callback_group=callback_group
)
def tf_update(self, goal_handle : ServerGoalHandle):
tf_update_msg = goal_handle.request
try:
cmd_dict = json.loads(tf_update_msg.command.replace("'",'"'))
for resource_id, target_parent in cmd_dict.items():
# 获取从resource_id到target_parent的转换
transform = self.tf_buffer.lookup_transform(
target_parent,
resource_id,
rclpy.time.Time()
)
# 提取转换中的位置和旋转信息
position = {
"x": transform.transform.translation.x,
"y": transform.transform.translation.y,
"z": transform.transform.translation.z
}
rotation = {
"x": transform.transform.rotation.x,
"y": transform.transform.rotation.y,
"z": transform.transform.rotation.z,
"w": transform.transform.rotation.w
}
self.resource_tf_dict[resource_id] = {
"parent": target_parent,
"position": position,
"rotation": rotation
}
print(self.resource_tf_dict)
self.attach_collision_object(id=resource_id,link_name=target_parent)
self.publish_resource_tf()
except Exception as e:
self.get_logger().error(f"更新资源TF字典失败: {e}")
goal_handle.abort()
return SendCmd.Result(success=False)
goal_handle.succeed()
return SendCmd.Result(success=True)
"""检查move_group节点是否已初始化完成"""
def check_move_group_ready(self):
"""检查move_group节点是否已初始化完成"""
while not self.move_group_ready:
# 获取当前可用的节点列表
if self._get_planning_scene_service.service_is_ready() and self._apply_planning_scene_service.service_is_ready():
@@ -91,20 +149,54 @@ class ResourceMeshManager(Node):
#遍历resource_config中的资源配置，判断panent是否在resource_model中，
for resource_config in self.resource_config:
if resource_config['parent'] in self.resource_model:
# self.resource_tf_dict[resource_config['id']] = resource_config['parent']
self.resource_tf_dict.update({resource_config['id']:{"parent":resource_config['parent'],
"position":resource_config['position'],
"rotation":resource_config['config']['rotation']}})
elif resource_config['parent'] is None and resource_config['id'] in self.resource_model:
self.resource_tf_dict.update({resource_config['id']:{'parent':'world',
"position":resource_config['position'],
"rotation":resource_config['config']['rotation']}})
elif resource_config['parent'] not in self.resource_model and resource_config['parent'] is not None:
self.resource_tf_dict.update({resource_config['id']:{'parent':f"{self.resource_config_dict[resource_config['parent']]['parent']}{resource_config['parent']}_device_link".replace("None",""),
"position":resource_config['position'],
"rotation":resource_config['config']['rotation']}})
for resource_id, resource_config in self.resource_config_dict.items():
parent = resource_config['parent']
parent_link = 'world'
if parent in self.resource_model:
parent_link = parent
elif parent is None and resource_id in self.resource_model:
pass
elif parent not in self.resource_model and parent is not None:
parent_link = f"{self.resource_config_dict[parent]['parent']}{parent}_device_link".replace("None","")
else:
continue
# 提取位置信息并转换单位
position = {
"x": float(resource_config['position']['x'])/1000,
"y": float(resource_config['position']['y'])/1000,
"z": float(resource_config['position']['z'])/1000
}
rotation_dict = {
"x": 0,
"y": 0,
"z": 0
}
if 'rotation' in resource_config['config']:
rotation_dict = resource_config['config']['rotation']
# 从欧拉角转换为四元数
q = quaternion_from_euler(
float(rotation_dict['x']),
float(rotation_dict['y']),
float(rotation_dict['z'])
)
rotation = {
"x": q[0],
"y": q[1],
"z": q[2],
"w": q[3]
}
# 更新资源TF字典
self.resource_tf_dict[resource_id] = {
"parent": parent_link,
"position": position,
"rotation": rotation
}
def publish_resource_tf(self):
"""
@@ -113,12 +205,7 @@ class ResourceMeshManager(Node):
遍历self.resource_tf_dict中的每个元素，根据key，parent，以及position和rotation，
发布key和parent之间的tf关系
"""
self.get_logger().info('开始发布资源TF关系')
# 创建静态TF广播器
# 存储所有需要发布的静态变换
transforms = []
# 遍历资源TF字典
@@ -135,31 +222,92 @@ class ResourceMeshManager(Node):
transform.child_frame_id = resource_id
# 设置位置
transform.transform.translation.x = float(position['x'])/1000
transform.transform.translation.y = float(position['y'])/1000
transform.transform.translation.z = float(position['z'])/1000
# 从欧拉角转换为四元数
q = quaternion_from_euler(
float(rotation['x']),
float(rotation['y']),
float(rotation['z'])
)
transform.transform.translation.x = float(position['x'])
transform.transform.translation.y = float(position['y'])
transform.transform.translation.z = float(position['z'])
# 设置旋转
transform.transform.rotation.x = q[0]
transform.transform.rotation.y = q[1]
transform.transform.rotation.z = q[2]
transform.transform.rotation.w = q[3]
transform.transform.rotation.x = rotation['x']
transform.transform.rotation.y = rotation['y']
transform.transform.rotation.z = rotation['z']
transform.transform.rotation.w = rotation['w']
transforms.append(transform)
# 一次性发布所有静态变换
if transforms:
self.tf_broadcaster.sendTransform(transforms)
self.get_logger().info(f'已发布 {len(transforms)} 个资源TF关系')
# self.get_logger().info(f'已发布 {len(transforms)} 个资源TF关系')
def add_resource_collision_meshes(self):
"""
遍历资源配置字典，为每个在resource_model中有对应模型的资源添加碰撞网格
该方法检查每个资源ID是否在self.resource_model中有对应的3D模型文件路径，
如果有，则调用add_collision_mesh方法将其添加到碰撞环境中。
"""
self.get_logger().info('开始添加资源碰撞网格')
for resource_id, tf_info in self.resource_tf_dict.items():
if resource_id in self.resource_model:
# 获取位置信息
position = [
float(self.resource_model[resource_id]['mesh_tf'][0]),
float(self.resource_model[resource_id]['mesh_tf'][1]),
float(self.resource_model[resource_id]['mesh_tf'][2])
]
# 获取旋转信息并转换为四元数
q = quaternion_from_euler(
float(self.resource_model[resource_id]['mesh_tf'][3]),
float(self.resource_model[resource_id]['mesh_tf'][4]),
float(self.resource_model[resource_id]['mesh_tf'][5])
)
# 添加碰撞网格
self.add_collision_mesh(
filepath=self.resource_model[resource_id]['mesh'],
id=resource_id,
position=position,
quat_xyzw=q,
frame_id=resource_id
)
elif f"{tf_info['parent']}_" in self.resource_model:
# 获取资源的父级框架ID
id_ = f"{tf_info['parent']}_"
# 获取位置信息
position = [
float(self.resource_model[id_]['mesh_tf'][0]),
float(self.resource_model[id_]['mesh_tf'][1]),
float(self.resource_model[id_]['mesh_tf'][2])
]
# 获取旋转信息并转换为四元数
q = quaternion_from_euler(
float(self.resource_model[id_]['mesh_tf'][3]),
float(self.resource_model[id_]['mesh_tf'][4]),
float(self.resource_model[id_]['mesh_tf'][5])
)
# 添加碰撞网格
self.add_collision_mesh(
filepath=self.resource_model[id_]['mesh'],
id=resource_id,
position=position,
quat_xyzw=q,
frame_id=resource_id
)
time.sleep(0.01)
self.get_logger().info('资源碰撞网格添加完成')
def add_collision_primitive(
self,
id: str,
@@ -672,7 +820,8 @@ class ResourceMeshManager(Node):
if __name__ == '__main__':
model_s = '''
{'Plate1': '/home/z43/git_pj/Uni-Lab-OS/unilabos/device_mesh/resources/tecan_nested_tip_rack/meshes/plate.stl', 'Plate1_': '/home/z43/git_pj/Uni-Lab-OS/unilabos/device_mesh/resources/generic_labware_tube_10_75/meshes/0_base.stl'}
{'Plate1': {'mesh': '/home/z43/git_pj/Uni-Lab-OS/unilabos/device_mesh/resources/tecan_nested_tip_rack/meshes/plate.stl', 'mesh_tf': [0.064, 0.043, 0, -1.5708, 0, 1.5708]},
'Plate1_': {'mesh': '/home/z43/git_pj/Uni-Lab-OS/unilabos/device_mesh/resources/generic_labware_tube_10_75/meshes/0_base.stl', 'mesh_tf': [0.0018, 0.0018, 0, -1.5708,0, 0]}}
'''
resource_model = json.loads(model_s.replace("'",'"'))
@@ -686,8 +835,8 @@ if __name__ == '__main__':
"type": "device",
"class": "gripper.mock",
"position": {
"x": 620.6111111111111,
"y": 171,
"x": 0,
"y": 0,
"z": 0
},
"config": {},
@@ -799,9 +948,9 @@ if __name__ == '__main__':
"type": "device",
"class": "",
"position": {
"x": 620.6111111111111,
"y": 171,
"z": 0
"x": 0,
"y": 0,
"z": 69
},
"config": {
"type": "Plate",
@@ -4668,5 +4817,8 @@ if __name__ == '__main__':
rclpy.init()
resource_mesh_manager = ResourceMeshManager(resource_model, resource_config, 'resource_mesh_manager')
resource_mesh_manager.resource_mesh_setup()
print(json.dumps(resource_mesh_manager.resource_tf_dict, indent=4, ensure_ascii=False))
resource_mesh_manager.publish_resource_tf()
# resource_mesh_manager.clear_all_collision_objects()
# print(json.dumps(resource_mesh_manager.resource_tf_dict, indent=4, ensure_ascii=False))
resource_mesh_manager.add_resource_collision_meshes()
rclpy.spin(resource_mesh_manager)

4
unilabos/registry/resources/opentrons/plates.yaml
View File

@@ -54,8 +54,10 @@ nest_96_wellplate_100ul_pcr_full_skirt:
model:
type: resource
mesh: tecan_nested_tip_rack/meshes/plate.stl
mesh_tf: [0.064, 0.043, 0, -1.5708, 0, 1.5708]
children_mesh: generic_labware_tube_10_75/meshes/0_base.stl
children_mesh_tf: [0.0018, 0.0018, 0, -1.5708,0, 0]
appliedbiosystemsmicroamp_384_wellplate_40ul:
description: Applied Biosystems microamp 384 wellplate 40ul
class: