From f240e9a01337e66b8edf62afe293e0afbe7dc39b Mon Sep 17 00:00:00 2001
From: David Sobek <david.sobek@picknik.ai>
Date: Thu, 14 Nov 2024 09:37:00 -0700
Subject: [PATCH 1/2] Fix register cad part objective

---
 .../objectives/load_mesh_as_green_poincloud.xml             | 1 -
 .../objectives/load_mesh_as_red_pointcloud.xml              | 1 -
 src/arm_on_rail_sim/objectives/register_cad_part.xml        | 6 +++---
 3 files changed, 3 insertions(+), 5 deletions(-)

diff --git a/src/arm_on_rail_sim/objectives/load_mesh_as_green_poincloud.xml b/src/arm_on_rail_sim/objectives/load_mesh_as_green_poincloud.xml
index ddf320b3..575430ef 100644
--- a/src/arm_on_rail_sim/objectives/load_mesh_as_green_poincloud.xml
+++ b/src/arm_on_rail_sim/objectives/load_mesh_as_green_poincloud.xml
@@ -29,7 +29,6 @@
   <TreeNodesModel>
     <SubTree ID="Load Mesh as Green Pointcloud">
       <output_port name="point_cloud" default="{green_cloud}" />
-      <input_port name="_collapsed" default="true" />
       <input_port name="initial_pose" default="{stamped_pose}" />
       <MetadataFields>
         <Metadata subcategory="Perception - 3D Point Cloud" />
diff --git a/src/arm_on_rail_sim/objectives/load_mesh_as_red_pointcloud.xml b/src/arm_on_rail_sim/objectives/load_mesh_as_red_pointcloud.xml
index 5a155ccf..c96d49af 100644
--- a/src/arm_on_rail_sim/objectives/load_mesh_as_red_pointcloud.xml
+++ b/src/arm_on_rail_sim/objectives/load_mesh_as_red_pointcloud.xml
@@ -29,7 +29,6 @@
   <TreeNodesModel>
     <SubTree ID="Load Mesh as Red Pointcloud">
       <output_port name="point_cloud" default="{red_cloud}" />
-      <input_port name="_collapsed" default="false" />
       <input_port name="initial_pose" default="{stamped_pose}" />
       <MetadataFields>
         <Metadata subcategory="Perception - 3D Point Cloud" />
diff --git a/src/arm_on_rail_sim/objectives/register_cad_part.xml b/src/arm_on_rail_sim/objectives/register_cad_part.xml
index 1443fcc6..33bbdebb 100644
--- a/src/arm_on_rail_sim/objectives/register_cad_part.xml
+++ b/src/arm_on_rail_sim/objectives/register_cad_part.xml
@@ -4,7 +4,7 @@
   <BehaviorTree ID="Register CAD Part" _description="" _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
       <Action ID="ClearSnapshot" />
-      <SubTree ID="Look at table" _collapsed="true" />
+      <SubTree ID="Look at table" _collapsed="false" />
       <Action
         ID="CreateStampedPose"
         position_xyz="0.02;.75;0.58"
@@ -15,7 +15,7 @@
         ID="Load Mesh as Red Pointcloud"
         _collapsed="false"
         output_cloud="{red_cloud}"
-        initial_pose="{stamped_pose}"
+        initial_pose="{pose_stamped}"
         point_cloud="{red_cloud}"
       />
       <Action ID="SendPointCloudToUI" point_cloud="{red_cloud}" />
@@ -41,7 +41,7 @@
         ID="Load Mesh as Green Pointcloud"
         _collapsed="false"
         output_cloud="{green_cloud}"
-        initial_pose="{stamped_pose}"
+        initial_pose="{pose_stamped}"
         point_cloud="{green_cloud}"
       />
       <Action

From 9bdbbd26577e2bbeb4c6525a2c4588bbad3edeaf Mon Sep 17 00:00:00 2001
From: mewtwoshaurd <me@shaurkumar.com>
Date: Fri, 15 Nov 2024 10:41:29 -0700
Subject: [PATCH 2/2] fix: pose_stamped port for output on Blackboard

---
 src/arm_on_rail_sim/objectives/register_cad_part.xml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/arm_on_rail_sim/objectives/register_cad_part.xml b/src/arm_on_rail_sim/objectives/register_cad_part.xml
index 33bbdebb..7dba9f7a 100644
--- a/src/arm_on_rail_sim/objectives/register_cad_part.xml
+++ b/src/arm_on_rail_sim/objectives/register_cad_part.xml
@@ -10,6 +10,7 @@
         position_xyz="0.02;.75;0.58"
         orientation_xyzw="0;0;0;1"
         name="Initial Guess"
+        stamped_pose="{pose_stamped}"
       />
       <SubTree
         ID="Load Mesh as Red Pointcloud"