Fix register cad part objective (#396)

* Fix register cad part objective * fix: pose_stamped port for output on Blackboard --------- Co-authored-by: mewtwoshaurd <[email protected]>
PickNikRobotics · Nov 15, 2024 · 16c6636 · 16c6636
1 parent 1a6d0be
commit 16c6636
Show file tree

Hide file tree

Showing 3 changed files with 4 additions and 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
@@ -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
@@ -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
@@ -4,18 +4,19 @@
   <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"
         orientation_xyzw="0;0;0;1"
         name="Initial Guess"
+        stamped_pose="{pose_stamped}"
       />
       <SubTree
         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 +42,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