From 6f581cebef02bd46eb3e6b7b641f844597bca03f Mon Sep 17 00:00:00 2001
From: Paul Gesel <paul.gesel@picknik.ai>
Date: Mon, 18 Nov 2024 12:50:14 -0700
Subject: [PATCH 1/6] Fix depend of moveit_studio_behavior

Signed-off-by: Paul Gesel <paul.gesel@picknik.ai>
---
 src/example_behaviors/package.xml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/example_behaviors/package.xml b/src/example_behaviors/package.xml
index d68b3219..86f625d8 100644
--- a/src/example_behaviors/package.xml
+++ b/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>

From 15baa7f26f41e99155425b8639943776b4fa518e Mon Sep 17 00:00:00 2001
From: bgill92 <bilal.gill@picknik.ai>
Date: Mon, 18 Nov 2024 14:55:53 -0500
Subject: [PATCH 2/6] Update ports explicitly

---
 src/lab_sim/objectives/create_pose_vector.xml | 18 ++++++++++++++++++
 src/lab_sim/objectives/pick_object.xml        |  1 +
 src/lab_sim/objectives/place_object.xml       |  1 +
 src/lab_sim/objectives/register_cad_part.xml  |  1 +
 4 files changed, 21 insertions(+)

diff --git a/src/lab_sim/objectives/create_pose_vector.xml b/src/lab_sim/objectives/create_pose_vector.xml
index af7432b9..787f42ea 100644
--- a/src/lab_sim/objectives/create_pose_vector.xml
+++ b/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>
diff --git a/src/lab_sim/objectives/pick_object.xml b/src/lab_sim/objectives/pick_object.xml
index 66dcbacd..481097e8 100644
--- a/src/lab_sim/objectives/pick_object.xml
+++ b/src/lab_sim/objectives/pick_object.xml
@@ -15,6 +15,7 @@
           ID="CreateStampedPose"
           orientation_xyzw="0;1;0;0"
           position_xyz="0.01;.75;0.515"
+          stamped_pose="{stamped_pose}"
         />
         <Action
           ID="AddPoseStampedToVector"
diff --git a/src/lab_sim/objectives/place_object.xml b/src/lab_sim/objectives/place_object.xml
index 61c14dc3..6ef0149e 100644
--- a/src/lab_sim/objectives/place_object.xml
+++ b/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"
diff --git a/src/lab_sim/objectives/register_cad_part.xml b/src/lab_sim/objectives/register_cad_part.xml
index f43db67f..3a301798 100644
--- a/src/lab_sim/objectives/register_cad_part.xml
+++ b/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

From 9b97cd4a7a89647de89e47db75a9c618a94cae85 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 3/6] 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 +++++--
 5 files changed, 122 insertions(+), 12 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>

From ad79604eae475267e7e2dddd09c0ef3ba048c7c9 Mon Sep 17 00:00:00 2001
From: Paul Gesel <paul.gesel@picknik.ai>
Date: Mon, 18 Nov 2024 14:41:36 -0700
Subject: [PATCH 4/6] update submodule

Signed-off-by: Paul Gesel <paul.gesel@picknik.ai>
---
 src/picknik_accessories | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/picknik_accessories b/src/picknik_accessories
index c9b1e709..18b1e9f5 160000
--- a/src/picknik_accessories
+++ b/src/picknik_accessories
@@ -1 +1 @@
-Subproject commit c9b1e709df2de65c1087d4433cbc7f079601cf70
+Subproject commit 18b1e9f50f8f566b979e7251baf8744fa6387b29

From b090cff5e8bf5bc6a6076e444c037718c44c2c41 Mon Sep 17 00:00:00 2001
From: Michael Wrock <mike.wrock@gmail.com>
Date: Mon, 18 Nov 2024 13:58:02 -0800
Subject: [PATCH 5/6] more fixes

---
 .../objectives/add_waypoints_to_mtc_task.xml  | 35 +++--------
 src/lab_sim/objectives/pick_object.xml        | 34 +----------
 .../objectives/plan_and_save_trajectory.xml   | 60 ++++---------------
 3 files changed, 22 insertions(+), 107 deletions(-)

diff --git a/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml b/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml
index 04947544..35596a12 100644
--- a/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml
+++ b/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml
@@ -1,38 +1,19 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version="1.0" encoding="utf-8"?>
 <root BTCPP_format="4" main_tree_to_execute="Add Waypoints to MTC Task">
   <!--//////////-->
   <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" />
+      <input_port name="joint_group_name" default="manipulator"/>
+      <inout_port name="mtc_task" default="{mtc_task}"/>
+      <input_port name="planner_interface" default="moveit_default"/>
     </SubTree>
   </TreeNodesModel>
 </root>
diff --git a/src/lab_sim/objectives/pick_object.xml b/src/lab_sim/objectives/pick_object.xml
index 481097e8..ac46e08c 100644
--- a/src/lab_sim/objectives/pick_object.xml
+++ b/src/lab_sim/objectives/pick_object.xml
@@ -1,33 +1 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<root BTCPP_format="4" main_tree_to_execute="Pick object">
-  <!--//////////-->
-  <BehaviorTree
-    ID="Pick object"
-    _description=""
-    _favorite="false"
-    _subtreeOnly="true"
-  >
-    <Control ID="Sequence" name="root">
-      <SubTree ID="Open Gripper" />
-      <Control ID="Sequence">
-        <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization" />
-        <Action
-          ID="CreateStampedPose"
-          orientation_xyzw="0;1;0;0"
-          position_xyz="0.01;.75;0.515"
-          stamped_pose="{stamped_pose}"
-        />
-        <Action
-          ID="AddPoseStampedToVector"
-          input="{stamped_pose}"
-          vector="{pose_vector}"
-        />
-      </Control>
-      <SubTree
-        ID="Pick first object in vector"
-        pose_vector="{pose_vector}"
-        _collapsed="false"
-      />
-    </Control>
-  </BehaviorTree>
-</root>
+<root BTCPP_format="4" main_tree_to_execute="Pick object"><!--//////////--><BehaviorTree ID="Pick object" _description="" _favorite="false" _subtreeOnly="true"><Control ID="Sequence" name="root"><SubTree ID="Open Gripper"/><Control ID="Sequence"><Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/><Action ID="CreateStampedPose" orientation_xyzw="0;1;0;0" position_xyz="0.01;.75;0.515" stamped_pose="{stamped_pose}"/><Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_vector}"/></Control><SubTree ID="Pick first object in vector" pose_vector="{pose_vector}" _collapsed="false"/></Control></BehaviorTree></root>
\ No newline at end of file
diff --git a/src/lab_sim/objectives/plan_and_save_trajectory.xml b/src/lab_sim/objectives/plan_and_save_trajectory.xml
index 232e1f08..b3782ba2 100644
--- a/src/lab_sim/objectives/plan_and_save_trajectory.xml
+++ b/src/lab_sim/objectives/plan_and_save_trajectory.xml
@@ -1,53 +1,19 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version="1.0" encoding="utf-8"?>
 <root BTCPP_format="4" main_tree_to_execute="Plan and Save Trajectory">
   <!--//////////-->
-  <BehaviorTree
-    ID="Plan and Save Trajectory"
-    _description=""
-    _favorite="true"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Plan and Save Trajectory" _description="" _favorite="true" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        waypoint_name="Workspace Right"
-        joint_group_name="manipulator"
-        planner_interface="moveit_default"
-        controller_names="/joint_trajectory_controller"
-      />
-      <SubTree
-        ID="Create Pose Vector"
-        _collapsed="false"
-        target_poses="{target_poses}"
-      />
-      <SubTree
-        ID="Add Poses to MTC Task"
-        _collapsed="true"
-        target_poses="{target_poses}"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        planning_group_name="manipulator"
-        ik_frame="grasp_link"
-        mtc_task="{mtc_task}"
-      />
-      <SubTree
-        ID="Add Waypoints to MTC Task"
-        _collapsed="true"
-        mtc_task="{mtc_task}"
-        waypoint_names="Pick Cube;Place Cube"
-        joint_group_name="manipulator"
-        planner_interface="moveit_default"
-      />
-      <Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}" />
-      <Action ID="WaitForUserTrajectoryApproval" />
-      <Action
-        ID="ConvertMtcSolutionToJointTrajectory"
-        joint_trajectory="{joint_trajectory_msg}"
-      />
-      <Action
-        ID="SaveJointTrajectoryToYaml"
-        yaml_filename="~/user_ws/src/lab_sim/objectives/joint_trajectory"
-      />
+      <SubTree ID="Move to Waypoint" _collapsed="true" waypoint_name="Workspace Right" joint_group_name="manipulator" planner_interface="moveit_default" controller_names="/joint_trajectory_controller"/>
+      <SubTree ID="Create Pose Vector" _collapsed="false" target_poses="{target_poses}"/>
+      <SubTree ID="Add Poses to MTC Task" _collapsed="true" target_poses="{target_poses}" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planning_group_name="manipulator" ik_frame="grasp_link" mtc_task="{mtc_task}"/>
+      <SubTree ID="Add Waypoints to MTC Task" _collapsed="false" mtc_task="{mtc_task}" waypoint_names="Pick Cube;Place Cube" joint_group_name="manipulator" planner_interface="moveit_default"/>
+      <Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}"/>
+      <Action ID="WaitForUserTrajectoryApproval"/>
+      <Action ID="ConvertMtcSolutionToJointTrajectory" joint_trajectory="{joint_trajectory_msg}"/>
+      <Action ID="SaveJointTrajectoryToYaml" yaml_filename="~/user_ws/src/lab_sim/objectives/joint_trajectory"/>
     </Control>
   </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Plan and Save Trajectory"/>
+  </TreeNodesModel>
 </root>

From 23d20c4b79d4797ac8e437808e8e76fc353d0da3 Mon Sep 17 00:00:00 2001
From: Michael Wrock <mike.wrock@gmail.com>
Date: Mon, 18 Nov 2024 13:59:01 -0800
Subject: [PATCH 6/6] formatting

---
 .../objectives/add_waypoints_to_mtc_task.xml  | 40 ++++++++++---
 src/lab_sim/objectives/pick_object.xml        | 33 ++++++++++-
 src/lab_sim/objectives/pick_up_cube.xml       | 10 ++--
 .../objectives/plan_and_save_trajectory.xml   | 59 +++++++++++++++----
 4 files changed, 117 insertions(+), 25 deletions(-)

diff --git a/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml b/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml
index 35596a12..31a2650f 100644
--- a/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml
+++ b/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml
@@ -1,19 +1,43 @@
-<?xml version="1.0" encoding="utf-8"?>
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Add Waypoints to MTC Task">
   <!--//////////-->
   <BehaviorTree ID="Add Waypoints to MTC Task">
     <Control ID="Sequence">
-      <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}"/>
+      <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"/>
-      <inout_port name="mtc_task" default="{mtc_task}"/>
-      <input_port name="planner_interface" default="moveit_default"/>
+      <input_port name="joint_group_name" default="manipulator" />
+      <inout_port name="mtc_task" default="{mtc_task}" />
+      <input_port name="planner_interface" default="moveit_default" />
     </SubTree>
   </TreeNodesModel>
 </root>
diff --git a/src/lab_sim/objectives/pick_object.xml b/src/lab_sim/objectives/pick_object.xml
index ac46e08c..2aa4018c 100644
--- a/src/lab_sim/objectives/pick_object.xml
+++ b/src/lab_sim/objectives/pick_object.xml
@@ -1 +1,32 @@
-<root BTCPP_format="4" main_tree_to_execute="Pick object"><!--//////////--><BehaviorTree ID="Pick object" _description="" _favorite="false" _subtreeOnly="true"><Control ID="Sequence" name="root"><SubTree ID="Open Gripper"/><Control ID="Sequence"><Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/><Action ID="CreateStampedPose" orientation_xyzw="0;1;0;0" position_xyz="0.01;.75;0.515" stamped_pose="{stamped_pose}"/><Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_vector}"/></Control><SubTree ID="Pick first object in vector" pose_vector="{pose_vector}" _collapsed="false"/></Control></BehaviorTree></root>
\ No newline at end of file
+<root BTCPP_format="4" main_tree_to_execute="Pick object">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Pick object"
+    _description=""
+    _favorite="false"
+    _subtreeOnly="true"
+  >
+    <Control ID="Sequence" name="root">
+      <SubTree ID="Open Gripper" />
+      <Control ID="Sequence">
+        <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization" />
+        <Action
+          ID="CreateStampedPose"
+          orientation_xyzw="0;1;0;0"
+          position_xyz="0.01;.75;0.515"
+          stamped_pose="{stamped_pose}"
+        />
+        <Action
+          ID="AddPoseStampedToVector"
+          input="{stamped_pose}"
+          vector="{pose_vector}"
+        />
+      </Control>
+      <SubTree
+        ID="Pick first object in vector"
+        pose_vector="{pose_vector}"
+        _collapsed="false"
+      />
+    </Control>
+  </BehaviorTree>
+</root>
diff --git a/src/lab_sim/objectives/pick_up_cube.xml b/src/lab_sim/objectives/pick_up_cube.xml
index e23c4dab..336abbae 100644
--- a/src/lab_sim/objectives/pick_up_cube.xml
+++ b/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>
diff --git a/src/lab_sim/objectives/plan_and_save_trajectory.xml b/src/lab_sim/objectives/plan_and_save_trajectory.xml
index b3782ba2..a3c28efd 100644
--- a/src/lab_sim/objectives/plan_and_save_trajectory.xml
+++ b/src/lab_sim/objectives/plan_and_save_trajectory.xml
@@ -1,19 +1,56 @@
-<?xml version="1.0" encoding="utf-8"?>
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Plan and Save Trajectory">
   <!--//////////-->
-  <BehaviorTree ID="Plan and Save Trajectory" _description="" _favorite="true" _subtreeOnly="false">
+  <BehaviorTree
+    ID="Plan and Save Trajectory"
+    _description=""
+    _favorite="true"
+    _subtreeOnly="false"
+  >
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree ID="Move to Waypoint" _collapsed="true" waypoint_name="Workspace Right" joint_group_name="manipulator" planner_interface="moveit_default" controller_names="/joint_trajectory_controller"/>
-      <SubTree ID="Create Pose Vector" _collapsed="false" target_poses="{target_poses}"/>
-      <SubTree ID="Add Poses to MTC Task" _collapsed="true" target_poses="{target_poses}" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planning_group_name="manipulator" ik_frame="grasp_link" mtc_task="{mtc_task}"/>
-      <SubTree ID="Add Waypoints to MTC Task" _collapsed="false" mtc_task="{mtc_task}" waypoint_names="Pick Cube;Place Cube" joint_group_name="manipulator" planner_interface="moveit_default"/>
-      <Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}"/>
-      <Action ID="WaitForUserTrajectoryApproval"/>
-      <Action ID="ConvertMtcSolutionToJointTrajectory" joint_trajectory="{joint_trajectory_msg}"/>
-      <Action ID="SaveJointTrajectoryToYaml" yaml_filename="~/user_ws/src/lab_sim/objectives/joint_trajectory"/>
+      <SubTree
+        ID="Move to Waypoint"
+        _collapsed="true"
+        waypoint_name="Workspace Right"
+        joint_group_name="manipulator"
+        planner_interface="moveit_default"
+        controller_names="/joint_trajectory_controller"
+      />
+      <SubTree
+        ID="Create Pose Vector"
+        _collapsed="false"
+        target_poses="{target_poses}"
+      />
+      <SubTree
+        ID="Add Poses to MTC Task"
+        _collapsed="true"
+        target_poses="{target_poses}"
+        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+        planning_group_name="manipulator"
+        ik_frame="grasp_link"
+        mtc_task="{mtc_task}"
+      />
+      <SubTree
+        ID="Add Waypoints to MTC Task"
+        _collapsed="false"
+        mtc_task="{mtc_task}"
+        waypoint_names="Pick Cube;Place Cube"
+        joint_group_name="manipulator"
+        planner_interface="moveit_default"
+      />
+      <Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}" />
+      <Action ID="WaitForUserTrajectoryApproval" />
+      <Action
+        ID="ConvertMtcSolutionToJointTrajectory"
+        joint_trajectory="{joint_trajectory_msg}"
+      />
+      <Action
+        ID="SaveJointTrajectoryToYaml"
+        yaml_filename="~/user_ws/src/lab_sim/objectives/joint_trajectory"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
-    <SubTree ID="Plan and Save Trajectory"/>
+    <SubTree ID="Plan and Save Trajectory" />
   </TreeNodesModel>
 </root>