Add Get Imarker Pose from Mesh Visualization Objective

PickNikRobotics · Jul 10, 2024 · 41e0c32 · 41e0c32
1 parent e76d2a3
commit 41e0c32
Showing 1 changed file with 19 additions and 0 deletions.
diff --git a/src/picknik_ur_base_config/objectives/get_imarker_pose_from_mesh_visualization.xml b/src/picknik_ur_base_config/objectives/get_imarker_pose_from_mesh_visualization.xml
@@ -0,0 +1,19 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Get IMarker Pose From Mesh Visualization">
+  <!--//////////-->
+  <BehaviorTree ID="Get IMarker Pose From Mesh Visualization" _description="Visualize a mesh in the UI and use an IMarker to get a grasp pose based on that mesh" _favorite="false">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="VisualizeMesh" marker_lifetime="0.0"/>
+      <Action ID="AddPoseStampedToVector" input="{mesh_pose}"/>
+      <Action ID="AdjustPoseWithIMarker" initial_poses="{pose_stamped_vector}" adjusted_poses="{adjusted_poses}" prompts="Adjust IMarker for Poses relative to the mesh"/>
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Get IMarker Pose From Mesh Visualization">
+      <output_port name="adjusted_poses" default="{adjusted_poses}"/>
+      <input_port name="mesh_path" default="{mesh_path}"/>
+      <input_port name="mesh_pose" default="{mesh_pose}"/>
+      <input_port name="_collapsed" default="false"/>
+    </SubTree>
+  </TreeNodesModel>
+</root>