From 1ce92f91608c9949ca6aa01684fde8a5d25eece7 Mon Sep 17 00:00:00 2001
From: Paul Gesel <paul.gesel@picknik.ai>
Date: Mon, 18 Nov 2024 13:29:07 -0700
Subject: [PATCH] Fix pick ApriTag object

Signed-off-by: Paul Gesel <paul.gesel@picknik.ai>
---
 .../objectives/apriltag_detection_config.yaml |  6 +--
 .../objectives/apriltag_pick_object.xml       | 36 +++++++++++++---
 .../get_april_tag_pose_from_image.xml         | 41 +++++++++++++++++++
 src/lab_sim/objectives/plan_move_to_pose.xml  | 36 ++++++++++++++++
 src/lab_sim/objectives/wrist_snap.xml         | 15 +++++--
 src/lab_sim/package.xml                       |  2 +-
 6 files changed, 123 insertions(+), 13 deletions(-)
 create mode 100644 src/lab_sim/objectives/get_april_tag_pose_from_image.xml
 create mode 100644 src/lab_sim/objectives/plan_move_to_pose.xml

diff --git a/src/lab_sim/objectives/apriltag_detection_config.yaml b/src/lab_sim/objectives/apriltag_detection_config.yaml
index f1a96237..7af7bf1c 100644
--- a/src/lab_sim/objectives/apriltag_detection_config.yaml
+++ b/src/lab_sim/objectives/apriltag_detection_config.yaml
@@ -3,7 +3,7 @@ DetectAprilTags:
   apriltag_size: 0.04
   max_hamming: 0
   n_threads: 1
-  quad_decimate: 1
-  quad_sigma: 0.1
-  refine_edges: false
+  quad_decimate: 2
+  quad_sigma: 0.0
+  refine_edges: true
   z_up: false
diff --git a/src/lab_sim/objectives/apriltag_pick_object.xml b/src/lab_sim/objectives/apriltag_pick_object.xml
index 00a6ca45..df3f814f 100644
--- a/src/lab_sim/objectives/apriltag_pick_object.xml
+++ b/src/lab_sim/objectives/apriltag_pick_object.xml
@@ -1,3 +1,4 @@
+<?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Pick April Tag Labeled Object">
   <!--//////////-->
   <BehaviorTree
@@ -8,7 +9,7 @@
     <Control ID="Sequence" name="TopLevelSequence">
       <SubTree ID="Look at table" _collapsed="true" />
       <Action ID="ClearSnapshot" />
-      <SubTree ID="Wrist camera snapshot" _collapsed="true" />
+      <SubTree ID="Take wrist camera snapshot" _collapsed="true" />
       <Control ID="Sequence" name="Setup">
         <SubTree ID="Open Gripper" />
       </Control>
@@ -27,19 +28,39 @@
         topic_name="/wrist_camera/color"
         message_out="{image}"
       />
+      <SubTree
+        ID="Get AprilTag Pose from Image"
+        image="{image}"
+        camera_info="{camera_info}"
+        parameters="{parameters}"
+        _collapsed="true"
+        detection_pose="{detection_pose}"
+      />
       <Action
-        ID="DetectAprilTags"
+        ID="TransformPose"
+        output_pose="{view_pose}"
+        input_pose="{detection_pose}"
+        quaternion_xyzw="0;0;0;1"
+        translation_xyz="0;-0.1;-0.15"
+      />
+      <SubTree
+        ID="Plan Move To Pose"
+        target_pose="{view_pose}"
+        move_to_pose_solution="{move_to_pose_solution}"
+      />
+      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
+      <SubTree
+        ID="Get AprilTag Pose from Image"
         image="{image}"
         camera_info="{camera_info}"
         parameters="{parameters}"
-        detections="{detections}"
+        _collapsed="true"
+        detection_pose="{detection_pose}"
       />
-      <Action ID="GetDetectionPose" />
-      <Action ID="TransformPoseFrame" input_pose="{detection_pose}" />
       <Action
         ID="TransformPose"
         output_pose="{offset_pose}"
-        input_pose="{output_pose}"
+        input_pose="{detection_pose}"
         quaternion_xyzw="0;0;0;1"
         translation_xyz="0;0;-0.01"
       />
@@ -68,4 +89,7 @@
       <Action ID="ExecuteMTCTask" solution="{pick_object_solution}" />
     </Control>
   </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Pick April Tag Labeled Object" />
+  </TreeNodesModel>
 </root>
diff --git a/src/lab_sim/objectives/get_april_tag_pose_from_image.xml b/src/lab_sim/objectives/get_april_tag_pose_from_image.xml
new file mode 100644
index 00000000..789682be
--- /dev/null
+++ b/src/lab_sim/objectives/get_april_tag_pose_from_image.xml
@@ -0,0 +1,41 @@
+<root BTCPP_format="4" main_tree_to_execute="Get AprilTag Pose from Image">
+  <!-- ////////// -->
+  <BehaviorTree ID="Get AprilTag Pose from Image">
+    <Control ID="Sequence">
+      <!--Get tag pose estimate in world frame-->
+      <Action
+        ID="DetectAprilTags"
+        image="{image}"
+        camera_info="{camera_info}"
+        parameters="{parameters}"
+        detections="{detections}"
+      />
+      <Action
+        ID="GetImage"
+        topic_name="/wrist_camera/color"
+        message_out="{image}"
+      />
+      <Action
+        ID="DetectAprilTags"
+        image="{image}"
+        camera_info="{camera_info}"
+        parameters="{parameters}"
+        detections="{detections}"
+      />
+      <Action ID="GetDetectionPose" />
+      <Action
+        ID="TransformPoseFrame"
+        input_pose="{detection_pose}"
+        output_pose="{detection_pose}"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Get AprilTag Pose from Image">
+      <input_port name="image" default="{image}" />
+      <input_port name="camera_info" default="{camera_info}" />
+      <input_port name="parameters" default="{parameters}" />
+      <output_port name="detection_pose" default="{detection_pose}" />
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/lab_sim/objectives/plan_move_to_pose.xml b/src/lab_sim/objectives/plan_move_to_pose.xml
new file mode 100644
index 00000000..b5072d77
--- /dev/null
+++ b/src/lab_sim/objectives/plan_move_to_pose.xml
@@ -0,0 +1,36 @@
+<root BTCPP_format="4" main_tree_to_execute="Plan Move To Pose">
+  <!-- ////////// -->
+  <BehaviorTree ID="Plan Move To Pose">
+    <Control ID="Sequence">
+      <Action
+        ID="InitializeMTCTask"
+        task_id="move_to_pose"
+        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+        task="{move_to_pose_task}"
+      />
+      <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}" />
+      <Action
+        ID="SetupMTCMoveToPose"
+        ik_frame="grasp_link"
+        planning_group_name="manipulator"
+        target_pose="{target_pose}"
+        task="{move_to_pose_task}"
+        planner_interface="moveit_default"
+      />
+      <Action
+        ID="PlanMTCTask"
+        solution="{move_to_pose_solution}"
+        task="{move_to_pose_task}"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Plan Move To Pose">
+      <input_port name="target_pose" default="{target_pose}" />
+      <output_port
+        name="move_to_pose_solution"
+        default="{move_to_pose_solution}"
+      />
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/lab_sim/objectives/wrist_snap.xml b/src/lab_sim/objectives/wrist_snap.xml
index 83c58612..7c04c0c4 100644
--- a/src/lab_sim/objectives/wrist_snap.xml
+++ b/src/lab_sim/objectives/wrist_snap.xml
@@ -1,6 +1,11 @@
-<root BTCPP_format="4" main_tree_to_execute="Wrist camera snapshot">
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute=" Take wrist camera snapshot">
   <!--//////////-->
-  <BehaviorTree ID="Wrist camera snapshot" _description="" _favorite="true">
+  <BehaviorTree
+    ID="Take wrist camera snapshot"
+    _description=""
+    _favorite="true"
+  >
     <Control ID="Sequence" name="TopLevelSequence">
       <!-- Do not clear snapshots first, so we can take multiple snapshots from different angles -->
       <Action ID="GetPointCloud" topic_name="/wrist_camera/points" />
@@ -8,6 +13,10 @@
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
-    <SubTree ID="Wrist camera snapshot" />
+    <SubTree ID="Take wrist camera snapshot">
+      <MetadataFields>
+        <Metadata subcategory="Application - Basic Examples" />
+      </MetadataFields>
+    </SubTree>
   </TreeNodesModel>
 </root>
diff --git a/src/lab_sim/package.xml b/src/lab_sim/package.xml
index 1438d94e..c28b2cba 100644
--- a/src/lab_sim/package.xml
+++ b/src/lab_sim/package.xml
@@ -13,7 +13,7 @@
   <license>BSD-3-Clause</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
-
+  k
   <exec_depend>admittance_controller</exec_depend>
   <exec_depend>moveit_planners_stomp</exec_depend>
   <exec_depend>moveit_ros_perception</exec_depend>