mirror of
https://github.com/dptech-corp/Uni-Lab-OS.git
synced 2025-12-17 04:51:10 +00:00
修改模型方向，在yaml中添加变换属性
This commit is contained in:
zhangshixiang
@@ -154,10 +154,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()
|
||||
|
||||
|
||||
@@ -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"/>
|
||||
|
||||
@@ -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
923
unilabos/device_mesh/view_robot.rviz
Normal 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
|
||||
@@ -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)
|
||||
@@ -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:
|
||||
|
||||
Reference in New Issue
Block a user