Update ports explicitly

src/example_behaviors/package.xml

@@ -19,6 +19,7 @@
   <depend>moveit_ros_planning_interface</depend>
   <depend>moveit_studio_vision_msgs</depend>
   <depend>moveit_studio_vision</depend>
+  <depend>moveit_studio_behavior</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>

src/lab_sim/objectives/add_waypoints_to_mtc_task.xml

@@ -3,34 +3,39 @@
   <!--//////////-->
   <BehaviorTree ID="Add Waypoints to MTC Task">
     <Control ID="Sequence">
-      <Decorator
-        ID="ForEachString"
-        vector_in="{waypoint_names}"
-        out="{waypoint_name}"
-      >
-        <Control ID="Sequence">
-          <Action
-            ID="RetrieveWaypoint"
-            waypoint_joint_state="{target_joint_state}"
-            waypoint_name="{waypoint_name}"
-            joint_group_name="{joint_group_name}"
-          />
-          <Action
-            ID="SetupMTCMoveToJointState"
-            joint_state="{target_joint_state}"
-            name="SetupMTCMoveToJointState"
-            planning_group_name="{joint_group_name}"
-            planner_interface="{planner_interface}"
-            task="{mtc_task}"
-          />
-        </Control>
-      </Decorator>
+      <Action
+        ID="RetrieveWaypoint"
+        waypoint_joint_state="{target_joint_state}"
+        waypoint_name="Pick Cube"
+        joint_group_name="{joint_group_name}"
+      />
+      <Action
+        ID="SetupMTCMoveToJointState"
+        joint_state="{target_joint_state}"
+        name="SetupMTCMoveToJointState"
+        planning_group_name="{joint_group_name}"
+        planner_interface="{planner_interface}"
+        task="{mtc_task}"
+      />
+      <Action
+        ID="RetrieveWaypoint"
+        waypoint_joint_state="{target_joint_state}"
+        waypoint_name="Place Cube"
+        joint_group_name="{joint_group_name}"
+      />
+      <Action
+        ID="SetupMTCMoveToJointState"
+        joint_state="{target_joint_state}"
+        name="SetupMTCMoveToJointState"
+        planning_group_name="{joint_group_name}"
+        planner_interface="{planner_interface}"
+        task="{mtc_task}"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Add Waypoints to MTC Task">
       <input_port name="joint_group_name" default="manipulator" />
-      <input_port name="waypoint_names" default="Pick Cube;Place Cube" />
       <inout_port name="mtc_task" default="{mtc_task}" />
       <input_port name="planner_interface" default="moveit_default" />
     </SubTree>

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/create_pose_vector.xml

@@ -6,82 +6,100 @@
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
         position_xyz="1;0.5;1"
+        stamped_pose="{stamped_pose}"
       />
       <Action
         ID="AddPoseStampedToVector"
         vector="{target_poses}"
+        input="{stamped_pose}"
       />
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
         position_xyz="0.75;0.5;0.9"
+        stamped_pose="{stamped_pose}"
       />
       <Action
         ID="AddPoseStampedToVector"
         vector="{target_poses}"
+        input="{stamped_pose}"
       />
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
         position_xyz="0.5;0.5;0.8"
+        stamped_pose="{stamped_pose}"
       />
       <Action
         ID="AddPoseStampedToVector"
         vector="{target_poses}"
+        input="{stamped_pose}"
       />
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
         position_xyz="0.25;0.5;0.7"
+        stamped_pose="{stamped_pose}"
       />
       <Action
         ID="AddPoseStampedToVector"
         vector="{target_poses}"
+        input="{stamped_pose}"
       />
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
         position_xyz="0;0.5;0.6"
+        stamped_pose="{stamped_pose}"
       />
       <Action
         ID="AddPoseStampedToVector"
         vector="{target_poses}"
+        input="{stamped_pose}"
       />
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
         position_xyz="-0.25;0.5;0.7"
+        stamped_pose="{stamped_pose}"
       />
       <Action
         ID="AddPoseStampedToVector"
         vector="{target_poses}"
+        input="{stamped_pose}"
       />
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
         position_xyz="-0.5;0.5;0.8"
+        stamped_pose="{stamped_pose}"
       />
       <Action
         ID="AddPoseStampedToVector"
         vector="{target_poses}"
+        input="{stamped_pose}"
       />
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
         position_xyz="-0.75;0.5;0.9"
+        stamped_pose="{stamped_pose}"
       />
       <Action
         ID="AddPoseStampedToVector"
         vector="{target_poses}"
+        input="{stamped_pose}"
       />
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
         position_xyz="-1;0.5;1"
+        stamped_pose="{stamped_pose}"
       />
       <Action
         ID="AddPoseStampedToVector"
         vector="{target_poses}"
+        input="{stamped_pose}"
       />
     </Control>
   </BehaviorTree>

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/pick_object.xml

@@ -1,4 +1,3 @@
-<?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Pick object">
   <!--//////////-->
   <BehaviorTree
@@ -15,6 +14,7 @@
           ID="CreateStampedPose"
           orientation_xyzw="0;1;0;0"
           position_xyz="0.01;.75;0.515"
+          stamped_pose="{stamped_pose}"
         />
         <Action
           ID="AddPoseStampedToVector"

src/lab_sim/objectives/pick_up_cube.xml

@@ -1,13 +1,13 @@
-<?xml version="1.0" encoding="utf-8"?>
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Pick up Cube">
   <!--//////////-->
   <BehaviorTree ID="Pick up Cube" _description="" _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree ID="Clear Snapshot" _collapsed="true"/>
-      <SubTree ID="Look at table" _collapsed="true"/>
+      <SubTree ID="Clear Snapshot" _collapsed="true" />
+      <SubTree ID="Look at table" _collapsed="true" />
       <!--Take a snapshot so we have a visualization of the cubes to use in grasp pose correction-->
-      <SubTree ID="Wrist camera snapshot" _collapsed="true"/>
-      <SubTree ID="Pick object" _collapsed="false"/>
+      <SubTree ID="Wrist camera snapshot" _collapsed="true" />
+      <SubTree ID="Pick object" _collapsed="false" />
     </Control>
   </BehaviorTree>
 </root>

src/lab_sim/objectives/place_object.xml

@@ -13,6 +13,7 @@
           ID="CreateStampedPose"
           orientation_xyzw="0;1;0;0"
           position_xyz="0.01;.75;0.5.15"
+          stamped_pose="{stamped_pose}"
         />
         <Action
           ID="AddPoseStampedToVector"

src/lab_sim/objectives/plan_and_save_trajectory.xml

@@ -32,7 +32,7 @@
       />
       <SubTree
         ID="Add Waypoints to MTC Task"
-        _collapsed="true"
+        _collapsed="false"
         mtc_task="{mtc_task}"
         waypoint_names="Pick Cube;Place Cube"
         joint_group_name="manipulator"
@@ -50,4 +50,7 @@
       />
     </Control>
   </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Plan and Save Trajectory" />
+  </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/register_cad_part.xml

@@ -8,6 +8,7 @@
         ID="CreateStampedPose"
         position_xyz="0.02;.75;0.58"
         orientation_xyzw="0;0;0;1"
+        stamped_pose="{stamped_pose}"
         name="Initial Guess"
       />
       <SubTree