Add objectives to teach and pick objects relative to AprilTag pose

src/picknik_ur_gazebo_config/objectives/apriltag1_grasp_offset.yaml

@@ -0,0 +1,8 @@
+GraspOffset:
+  x: -0.1
+  y: 0.0
+  z: 0.05
+  qx: 0.707
+  qy: -0.707
+  qz: 0.0
+  qw: 0.0

src/picknik_ur_gazebo_config/objectives/apriltag2_grasp_offset.yaml

@@ -0,0 +1,8 @@
+GraspOffset:
+  x: -0.1
+  y: 0.0
+  z: 0.05
+  qx: 0.707
+  qy: -0.707
+  qz: 0.0
+  qw: 0.0

src/picknik_ur_gazebo_config/objectives/apriltag3_grasp_offset.yaml

@@ -0,0 +1,8 @@
+GraspOffset:
+  x: -0.1
+  y: 0.0
+  z: 0.05
+  qx: 0.707
+  qy: -0.707
+  qz: 0.0
+  qw: 0.0

src/picknik_ur_gazebo_config/objectives/loop_detect_apriltag.xml

@@ -1,7 +1,7 @@
 <?xml version='1.0' encoding='UTF-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Loop Detect AprilTag">
   <!-- ////////// -->
-  <BehaviorTree ID="Loop Detect AprilTag" _description="Loops finding April Tag markers until failure." _favorite="true">
+  <BehaviorTree ID="Loop Detect AprilTag" _description="Loops finding April Tag markers until failure." _favorite="false">
     <Control ID="Sequence">
       <!-- Keep executing until failure -->
       <Decorator ID="KeepRunningUntilFailure">
@@ -10,8 +10,6 @@
           <Action ID="GetCameraInfo" topic_name="/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>
           <Action ID="GetImage" topic_name="/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
           <Action ID="DetectApriltags" image="{image}" camera_info="{camera_info}" parameters="{parameters}" detections="{detections}"/>
-          <!-- if you don't want this behavior to fail when it can't find an AprilTag comment out the line below -->
-          <Action ID="GetDetectionPose" detections="{detections}" target_id="0" target_label="" detection_pose="{detection_pose}"/>
         </Control>
       </Decorator>  
     </Control>

src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml

@@ -1,19 +1,18 @@
 <?xml version='1.0' encoding='UTF-8'?>
-<root BTCPP_format="4" main_tree_to_execute="Pick Apriltag Labeled Object">
+<root BTCPP_format="4" main_tree_to_execute="Pick April Tag Labeled Object">
   <!-- ////////// -->
-  <BehaviorTree ID="Pick Apriltag Labeled Object" _description="Picks up an object that is at a known pose offset from an AprilTag marker." _favorite="true">
+  <BehaviorTree ID="Pick April Tag Labeled Object" _description="Picks up an object that is at a known pose offset from an AprilTag marker." _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
       <Control ID="Sequence" name="Setup">
         <SubTree ID="Open Gripper"/>
         <Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
       </Control>
-      <SubTree ID="Sample April Tag" num_cycles="5" tag_id="1" apriltag_config="apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
+      <SubTree ID="Sample April Tag" num_samples="5" tag_id="1" apriltag_config="apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
+      <Action ID="LoadObjectiveParameters" config_file_name="apriltag1_grasp_offset.yaml" parameters="{pose_parameters}"/>
       <Control ID="Sequence" name="GraspObject">
         <Action ID="TransformPoseFrame" input_pose="{tag_pose}" target_frame_id="world" output_pose="{tag_pose_world}"/>
-        <Action ID="TransformPose" input_pose="{tag_pose_world}" translation_xyz="-0.1;0.0;0.05" quaternion_xyzw="-0.707;0.707;0.0;0.0" output_pose="{grasp_pose}"/>
-        <Action ID="MoveToPose" target_pose="{grasp_pose}" ik_frame="manual_grasp_link" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
-        <SubTree ID="Close Gripper"/>
-        <Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+        <Action ID="TransformPoseFromYaml" input_pose="{tag_pose_world}" pose_parameters="{pose_parameters}" parameter_namespace="GraspOffset" output_pose="{grasp_pose}"/>
+        <SubTree ID="PickObject" grasp_pose="{grasp_pose}"/>
       </Control>
     </Control>
   </BehaviorTree>

src/picknik_ur_gazebo_config/objectives/sample_april_tag.xml

@@ -1,8 +1,8 @@
 <?xml version='1.0' encoding='UTF-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Sample April Tag">
   <!-- ////////// -->
-  <BehaviorTree ID="Sample April Tag" _description="Samples the pose of an AprilTag and returns the average measurement." _favorite="true">
-      <Decorator ID="Repeat" num_cycles="{num_cycles}" name="SamplePose">
+  <BehaviorTree ID="Sample April Tag" _description="Samples the pose of an AprilTag and returns the average measurement." _favorite="false">
+      <Decorator ID="Repeat" num_cycles="{num_samples}" name="SamplePose">
         <Control ID="Sequence">
         <Action ID="LoadObjectiveParameters" config_file_name="{apriltag_config}" parameters="{parameters}"/>
         <Action ID="GetCameraInfo" topic_name="/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>         

src/picknik_ur_gazebo_config/objectives/teach_apriltag_grasp_offset.xml

@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<root BTCPP_format="4" main_tree_to_execute="Teach Grasp Offset From Apriltag">
+    <BehaviorTree ID="Teach Grasp Offset From Apriltag" _description="Saves a grasp offset to yaml file from Apriltag observation using an interactive marker" _favorite="true">
+        <Control ID="Sequence" name="root">
+            <Action ID="GetPoseFromUser" parameter_name="calculate_offset.grasp_pose" parameter_value="{user_grasp_pose}" />
+            <SubTree ID="Sample April Tag" num_samples="5" tag_id="1" apriltag_config="apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
+            <Action ID="TransformPoseFrame" input_pose="{tag_pose}" target_frame_id="world" output_pose="{tag_pose_world}"/>
+            <Action ID="CalculatePoseOffset" source_pose="{tag_pose_world}" destination_pose="{user_grasp_pose}" source_to_destination_pose="{tag_to_user_grasp_pose}"/>
+            <Action ID="WritePoseToYAML" yaml_filename="apriltag1_grasp_offset" namespace="GraspOffset" pose="{tag_to_user_grasp_pose}"/>
+        </Control>
+    </BehaviorTree>
+</root>

src/picknik_ur_gazebo_config/objectives/tree_nodes_model.xml

@@ -0,0 +1,25 @@
+<root>
+  <TreeNodesModel>
+    <SubTree ID="Sample April Tag">
+        <description>
+          <p>
+              Samples the pose of an AprilTag and returns the average measurement.
+          </p>
+        </description>
+        <input_port name="num_samples" default="1">The number of times to sample the pose.</input_port>
+        <input_port name="tag_id" default="0">The AprilTag ID to detect.</input_port>
+        <input_port name="apriltag_config" default="apriltag_detection_config.yaml">The configuration file for detecting AprilTags.</input_port>
+        <input_port name="max_distance" default="0.02">The maximum distance allow between the average pose and the new sample (Euclidean distance between pose positions).</input_port>
+        <input_port name="max_rotation" default="0.2">The maximum angle allow between the average pose and the new sample (shortest angle between two pose quaternions).</input_port>
+        <output_port name="avg_pose" default="{avg_pose}">The calculated average pose.</output_port>
+    </SubTree>
+    <SubTree ID="PickObject">
+        <description>
+          <p>
+              Picks an object at the specified grasp pose.
+          </p>
+        </description>
+        <input_port name="grasp_pose" default="{grasp_pose}">The desired grasp pose.</input_port>
+    </SubTree>
+  </TreeNodesModel>
+</root>