From 4716301feeca380a33780712c9bb0c16f5333604 Mon Sep 17 00:00:00 2001
From: marqrazz <marq.razz@gmail.com>
Date: Fri, 8 Sep 2023 10:23:59 -0600
Subject: [PATCH] Add pose averaging to Pick Apriltag Labeled Object

---
 .../objectives/loop_detect_april.xml          | 19 +++++++++++++
 .../pick_apriltag_labeled_object.xml          | 13 +++------
 .../objectives/sample_april_tag.xml           | 28 +++++++++++++++++++
 3 files changed, 51 insertions(+), 9 deletions(-)
 create mode 100644 src/picknik_ur_gazebo_config/objectives/loop_detect_april.xml
 create mode 100644 src/picknik_ur_gazebo_config/objectives/sample_april_tag.xml

diff --git a/src/picknik_ur_gazebo_config/objectives/loop_detect_april.xml b/src/picknik_ur_gazebo_config/objectives/loop_detect_april.xml
new file mode 100644
index 00000000..f63c7903
--- /dev/null
+++ b/src/picknik_ur_gazebo_config/objectives/loop_detect_april.xml
@@ -0,0 +1,19 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Loop Detect April">
+  <!-- ////////// -->
+  <BehaviorTree ID="Loop Detect April" _description="Loops finding April Tag marker until failure." _favorite="true">
+    <Control ID="Sequence">
+      <!-- Keep executing until failure -->
+      <Decorator ID="KeepRunningUntilFailure">
+        <Control ID="Sequence" name="GetDetection">
+          <Action ID="LoadObjectiveParameters" config_file_name="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}"/>
+          <!-- if you don't want this behavior to fail when it can't find an April tag comment out the line below -->
+          <Action ID="GetDetectionPose" detections="{detections}" target_id="0" target_label="" detection_pose="{detection_pose}"/>
+        </Control>
+      </Decorator>  
+    </Control>
+  </BehaviorTree>
+</root>
diff --git a/src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml b/src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml
index 81b2d912..0dadffbd 100644
--- a/src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml
+++ b/src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml
@@ -8,19 +8,14 @@
         <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>
-      <Control ID="Sequence" name="GetDetection">
-        <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}"/>
-        <Action ID="GetDetectionPose" detections="{detections}" target_id="1" target_label="" detection_pose="{detection_pose}"/>
-      </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}"/>
       <Control ID="Sequence" name="GraspObject">
-        <Action ID="TransformPoseFrame" input_pose="{detection_pose}" target_frame_id="world" output_pose="{detection_pose_world}"/>
-        <Action ID="TransformPose" input_pose="{detection_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="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"/>
       </Control>
     </Control>
   </BehaviorTree>
-</root>
+</root>
\ No newline at end of file
diff --git a/src/picknik_ur_gazebo_config/objectives/sample_april_tag.xml b/src/picknik_ur_gazebo_config/objectives/sample_april_tag.xml
new file mode 100644
index 00000000..0fa3b6f0
--- /dev/null
+++ b/src/picknik_ur_gazebo_config/objectives/sample_april_tag.xml
@@ -0,0 +1,28 @@
+<?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 April tag and returns the average measurement." _favorite="true">
+      <Decorator ID="Repeat" num_cycles="{num_cycles}" 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}"/>
+        <!-- TODO: update this objective to allow rejecting noisy samples but fail after X attempts
+        <Fallback name="Average pose or sample more">
+            <Action ID="AveragePoseStamped"/>
+            <Control ID="Sequence">
+              <Script code="num_cycles={num_cycles - 1}" />
+              <Script code="num_failures={num_failures + 1}" />
+            </Control>
+        </Fallback> -->
+        </Control>
+      </Decorator>
+  </BehaviorTree>
+</root>