Merge pull request #72 from PickNikRobotics/apriltag-update

Apriltag update
PickNikRobotics · Jan 2, 2025 · 40911fa · 40911fa
2 parents 014f237 + 141fea0
commit 40911fa
Show file tree

Hide file tree

Showing 2 changed files with 97 additions and 17 deletions.
diff --git 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
@@ -21,11 +22,13 @@
         ID="GetCameraInfo"
         topic_name="/wrist_camera/camera_info"
         message_out="{camera_info}"
+        timeout_sec="5.000000"
       />
       <Action
         ID="GetImage"
         topic_name="/wrist_camera/color"
         message_out="{image}"
+        timeout_sec="5.000000"
       />
       <SubTree
         ID="Get AprilTag Pose from Image"
@@ -47,7 +50,11 @@
         target_pose="{view_pose}"
         move_to_pose_solution="{move_to_pose_solution}"
       />
-      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
+      <Action
+        ID="ExecuteMTCTask"
+        solution="{move_to_pose_solution}"
+        goal_duration_tolerance="-1.000000"
+      />
       <SubTree
         ID="Get AprilTag Pose from Image"
         image="{image}"
@@ -66,26 +73,36 @@
       <Action
         ID="LoadObjectiveParameters"
         config_file_name="pick_object_config.yaml"
+        parameters="{parameters}"
       />
       <Action
         ID="InitializeMTCTask"
         task_id="pick_object"
         controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
         task="{pick_object_task}"
       />
-      <Action ID="SetupMTCCurrentState" task="{pick_object_task}" />
+      <Action
+        ID="SetupMTCCurrentState"
+        task="{pick_object_task}"
+        skip_collision_check="false"
+      />
       <Action
         ID="SetupMTCPickObject"
         grasp_pose="{offset_pose}"
         task="{pick_object_task}"
         parameters="{parameters}"
+        monitored_stage="current state"
       />
       <Action
         ID="PlanMTCTask"
         solution="{pick_object_solution}"
         task="{pick_object_task}"
       />
-      <Action ID="ExecuteMTCTask" solution="{pick_object_solution}" />
+      <Action
+        ID="ExecuteMTCTask"
+        solution="{pick_object_solution}"
+        goal_duration_tolerance="-1.000000"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>

diff --git a/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml b/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml
@@ -1,15 +1,17 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version="1.0" encoding="utf-8" ?>
 <root
   BTCPP_format="4"
-  main_tree_to_execute="Pick April Tag Object With Approval"
+  main_tree_to_execute="Pick April Tag Labeled Object With Approval"
 >
   <!--//////////-->
   <BehaviorTree
-    ID="Pick April Tag Object With Approval"
+    ID="Pick April Tag Labeled Object With Approval"
     _description="Picks up an object that has an AprilTag marker."
     _favorite="false"
   >
     <Control ID="Sequence" name="TopLevelSequence">
+      <SubTree ID="Look at table" _collapsed="true" />
+      <Action ID="ClearSnapshot" />
       <SubTree ID="Take wrist camera snapshot" _collapsed="true" />
       <Control ID="Sequence" name="Setup">
         <SubTree ID="Open Gripper" />
@@ -23,37 +25,98 @@
         ID="GetCameraInfo"
         topic_name="/wrist_camera/camera_info"
         message_out="{camera_info}"
+        timeout_sec="5.000000"
       />
       <Action
         ID="GetImage"
         topic_name="/wrist_camera/color"
         message_out="{image}"
+        timeout_sec="5.000000"
       />
-      <Action
-        ID="DetectAprilTags"
+      <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="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="GetDetectionPose" />
-      <Action ID="TransformPoseFrame" input_pose="{detection_pose}" />
       <Action
-        ID="AddPoseStampedToVector"
-        vector="{initial_poses}"
-        input="{output_pose}"
+        ID="ExecuteMTCTask"
+        solution="{move_to_pose_solution}"
+        goal_duration_tolerance="-1.000000"
       />
       <SubTree
-        ID="Pick first object in vector"
+        ID="Get AprilTag Pose from Image"
+        image="{image}"
+        camera_info="{camera_info}"
+        parameters="{parameters}"
         _collapsed="true"
-        pose_vector="{initial_poses}"
+        detection_pose="{detection_pose}"
+      />
+      <Action
+        ID="TransformPose"
+        output_pose="{offset_pose}"
+        input_pose="{detection_pose}"
+        quaternion_xyzw="0;0;0;1"
+        translation_xyz="0;0;-0.01"
+      />
+      <Action
+        ID="LoadObjectiveParameters"
+        config_file_name="pick_object_config.yaml"
+        parameters="{parameters}"
+      />
+      <Action
+        ID="InitializeMTCTask"
+        task_id="pick_object"
+        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+        task="{pick_object_task}"
+      />
+      <Action
+        ID="SetupMTCCurrentState"
+        task="{pick_object_task}"
+        skip_collision_check="false"
+      />
+      <Action
+        ID="SetupMTCPickObject"
+        grasp_pose="{offset_pose}"
+        task="{pick_object_task}"
+        parameters="{parameters}"
+        monitored_stage="current state"
+      />
+      <Action
+        ID="PlanMTCTask"
+        solution="{pick_object_solution}"
+        task="{pick_object_task}"
+      />
+      <Action
+        ID="WaitForUserTrajectoryApproval"
+        solution="{pick_object_solution}"
+      />
+      <Action
+        ID="ExecuteMTCTask"
+        solution="{pick_object_solution}"
+        goal_duration_tolerance="-1.000000"
       />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
-    <SubTree ID="Pick April Tag Object With Approval">
+    <SubTree ID="Pick April Tag Labeled Object With Approval">
       <MetadataFields>
         <Metadata subcategory="Application - Advanced Examples" />
+        <Metadata runnable="true" />
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>