Add April tag support to picknik_ur_site_config (#179)

src/picknik_ur_site_config/objectives/51mm_apriltag_detection_config.yaml

@@ -0,0 +1,9 @@
+DetectApriltags:
+  apriltag_family_name: 36h11
+  apriltag_size: 0.051
+  max_hamming: 0
+  n_threads: 1
+  quad_decimate: 1
+  quad_sigma: 0.1
+  refine_edges: false
+  z_up: true

src/picknik_ur_site_config/objectives/apriltag1_grasp_offset.yaml

@@ -0,0 +1,10 @@
+GraspOffset:
+  position:
+    x: -0.1
+    y: 0.0
+    z: 0.05
+  orientation:
+    x: 0.707
+    y: -0.707
+    z: 0.0
+    w: 0.0

src/picknik_ur_site_config/objectives/loop_detect_apriltag.xml

@@ -0,0 +1,17 @@
+<?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="false">
+    <Control ID="Sequence">
+      <!-- Keep executing until failure -->
+      <Decorator ID="KeepRunningUntilFailure">
+        <Control ID="Sequence" name="GetDetection">
+          <Action ID="LoadObjectiveParameters" config_file_name="51mm_apriltag_detection_config.yaml" parameters="{parameters}"/>
+          <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}"/>
+        </Control>
+      </Decorator>  
+    </Control>
+  </BehaviorTree>
+</root>

src/picknik_ur_site_config/objectives/pick_apriltag_labeled_object.xml

@@ -0,0 +1,19 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Pick April Tag Labeled Object">
+  <!-- ////////// -->
+  <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 April Tag" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+      </Control>
+      <SubTree ID="Sample April Tag" num_samples="5" tag_id="1" apriltag_config="51mm_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="TransformPoseFromYaml" input_pose="{tag_pose_world}" pose_parameters="{pose_parameters}" parameter_namespace="GraspOffset" output_pose="{grasp_pose}"/>
+        <SubTree ID="Pick Object" grasp_pose="{grasp_pose}"/>
+      </Control>
+    </Control>
+  </BehaviorTree>
+</root>

src/picknik_ur_site_config/objectives/sample_april_tag.xml

@@ -0,0 +1,20 @@
+<?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="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}"/>         
+        <Decorator ID="RetryUntilSuccessful" num_attempts="5">
+          <Control ID="Sequence">                      
+            <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}"/> 
+            <Action ID="GetDetectionPose" detections="{detections}" target_id="{tag_id}" target_label="" detection_pose="{detection_pose}"/>
+          </Control>
+        </Decorator>
+        <Action ID="AveragePoseStamped" pose_sample="{detection_pose}" avg_pose="{avg_pose}" max_distance="{max_distance}" max_rotation="{max_rotation}" num_samples="{num_samples}" run_continuously="false"/>
+        </Control>
+      </Decorator>
+  </BehaviorTree>
+</root>

src/picknik_ur_site_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="51mm_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="SavePoseToYaml" yaml_filename="apriltag1_grasp_offset" namespace="GraspOffset" message="{tag_to_user_grasp_pose}"/>
+        </Control>
+    </BehaviorTree>
+</root>