Merge pull request #12 from PickNikRobotics/fix-pick-apriTag-object

Fix pick ApriTag object

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

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>

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>

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>

src/lab_sim/objectives/wrist_snap.xml

@@ -1,13 +1,22 @@
-<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" />
       <Action ID="SendPointCloudToUI" />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
-    <SubTree ID="Wrist camera snapshot" />
+    <SubTree ID="Take wrist camera snapshot">
+      <MetadataFields>
+        <Metadata subcategory="Application - Basic Examples" />
+      </MetadataFields>
+    </SubTree>
   </TreeNodesModel>
 </root>