Hackathon count boxes objective

src/hangar_sim/objectives/count_boxes.xml

@@ -0,0 +1,33 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Count Boxes">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Count Boxes"
+    _description="Count the number of boxes in a given area."
+    _favorite="false"
+    name="Show images taken by the robot to a human for box count verification"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action
+        ID="LogMessage"
+        log_level="error"
+        message="Make sure to update the absolute path by replacing studio-user with your username, then you can delete this message"
+      />
+      <SubTree
+        ID="Count Boxes Objective"
+        _collapsed="true"
+        pose_path="/home/studio-user/user_ws/src/hangar_sim/poses"
+        scene_camera_folder="/home/studio-user/user_ws/src/hangar_sim/scene_camera_images"
+        wrist_camera_folder="/home/studio-user/user_ws/src/hangar_sim/wrist_camera_images"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Count Boxes">
+      <MetadataFields>
+        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Advanced Examples" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>

src/hangar_sim/objectives/count_boxes_objective.xml

@@ -0,0 +1,130 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Count Boxes Objective">
+  <BehaviorTree
+    ID="Count Boxes Objective"
+    _description="Count the number of boxes in a given area."
+    _favorite="false"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <SubTree
+        ID="CreatePoseVectorFromDirectory"
+        _collapsed="false"
+        pose_vector="{pose_vector}"
+        directory_path="{pose_path}"
+        name="given a path, load a PoseStamped from each yaml in that directory"
+      />
+      <!--Crop outliers from our region of interest-->
+      <Action
+        ID="CreateStampedPose"
+        reference_frame="world"
+        stamped_pose="{crop_pose_centroid}"
+        position_xyz="-11;2.75;0"
+      />
+      <Action
+        ID="VisualizePath"
+        marker_lifetime="0.000000"
+        marker_name="path_initial"
+        path="{pose_vector}"
+        pose_marker_size="1"
+        show_poses="true"
+      />
+      <Action
+        ID="CropPosesInBox"
+        crop_box_centroid_pose="{crop_pose_centroid}"
+        poses="{pose_vector}"
+        poses_cropped="{path_cropped}"
+        crop_box_size="12;12;1"
+      />
+      <SubTree
+        ID="Order Poses"
+        _collapsed="false"
+        centroid="{crop_pose_centroid}"
+        poses_in="{path_cropped}"
+        poses_ordered="{poses_ordered}"
+        x_extent="12"
+        y_extent="12"
+      />
+      <Action
+        ID="VisualizePath"
+        marker_lifetime="0.000000"
+        marker_name="poses_ordered"
+        path="{poses_ordered}"
+        pose_marker_size="1"
+        show_poses="true"
+      />
+      <Action
+        ID="SwitchController"
+        activate_asap="true"
+        automatic_deactivation="true"
+        controller_list_action_name="/controller_manager/list_controllers"
+        controller_switch_action_name="/controller_manager/switch_controller"
+        strictness="1"
+        timeout="-1.000000"
+        activate_controllers="platform_velocity_controller_nav2"
+        deactivate_controllers="platform_velocity_controller"
+      />
+      <Action ID="Script" code="total_box_count:=0" />
+      <!--Drive through our newly ordered list of poses and count boxes at each Pose-->
+      <Decorator
+        ID="ForEach"
+        index="{index}"
+        out="{out}"
+        vector_in="{poses_ordered}"
+      >
+        <Control ID="Sequence">
+          <Action
+            ID="NavigateToPoseAction"
+            action_name="/navigate_to_pose"
+            current_pose="{current_pose}"
+            distance_remaining="{distance_remaining}"
+            estimated_time_remaining="{estimated_time_remaining}"
+            ignore_stamp_time="true"
+            navigation_time="{navigation_time}"
+            number_of_recoveries="{number_of_recoveries}"
+            pose_stamped="{out}"
+            timeout_sec="-1.000000"
+            behavior_tree_path="/opt/ros/humble/share/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml"
+          />
+          <SubTree
+            ID="Count boxes subtree"
+            _collapsed="false"
+            box_count="{box_count}"
+            scene_camera_folder="{scene_camera_folder}"
+            wrist_camera_folder="{wrist_camera_folder}"
+          />
+          <Action ID="Script" code="total_box_count+=box_count" />
+        </Control>
+      </Decorator>
+      <Action ID="LogMessage" log_level="info" message="{total_box_count}" />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Count Boxes Objective">
+      <MetadataFields>
+        <Metadata runnable="false" />
+        <Metadata subcategory="Application - Advanced Examples" />
+      </MetadataFields>
+      <inout_port
+        name="wrist_camera_folder"
+        default="/home/studio-user/user_ws/src/hangar_sim/wrist_camera_images"
+        type="std::string"
+      >
+        Absolute path to a directory for saving wrist camera pictures
+      </inout_port>
+      <inout_port
+        name="scene_camera_folder"
+        default="/home/studio-user/user_ws/src/hangar_sim/scene_camera_images"
+        type="std::string"
+      >
+        Absolute path to a directory for saving scene camera pictures
+      </inout_port>
+      <inout_port
+        name="pose_path"
+        default="/home/studio-user/user_ws/src/hangar_sim/poses"
+        type="std::string"
+      >
+        Absolute path to a directory of pose YAML files
+      </inout_port>
+    </SubTree>
+  </TreeNodesModel>
+</root>

src/hangar_sim/objectives/count_boxes_subtree.xml

@@ -0,0 +1,168 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Count boxes subtree">
+  <BehaviorTree
+    ID="Count boxes subtree"
+    _description="Counts the boxes visible in a scene"
+    _favorite="false"
+  >
+    <Control ID="Fallback">
+      <Control ID="Sequence" name="TopLevelSequence">
+        <Action
+          ID="GetSyncedImages"
+          camera_info_1="{camera_info_1}"
+          camera_info_1_topic_name="/wrist_camera/camera_info"
+          camera_info_2="{camera_info_2}"
+          camera_info_2_topic_name="/scene_camera/camera_info"
+          image_1="{wrist_camera_image}"
+          image_1_topic_name="/wrist_camera/color"
+          image_2="{scene_camera_image}"
+          image_2_topic_name="/scene_camera/color"
+        />
+        <Action
+          ID="SaveImageToFile"
+          file_path="{wrist_camera_folder}"
+          file_prefix="box"
+          image="{wrist_camera_image}"
+        />
+        <Action
+          ID="SaveImageToFile"
+          file_path="{scene_camera_folder}"
+          file_prefix="box"
+          image="{scene_camera_image}"
+        />
+        <SubTree
+          ID="Segment Image from Text Prompt Subtree"
+          _collapsed="true"
+          clip_model_path="models/clip.onnx"
+          clipseg_model_path="models/clipseg.onnx"
+          erosion_size="0"
+          image_topic_name="/wrist_camera/color"
+          masks_visualization_topic="/masks_visualization"
+          masks2d="{masks2d}"
+          model_package="moveit_pro_clipseg"
+          prompts="small boxes"
+          negative_prompts="grey"
+          threshold="0.4"
+        />
+        <Action
+          ID="PublishMask2D"
+          image="{wrist_camera_image}"
+          masks="{masks2d}"
+          masks_visualization_topic="/masks_visualization"
+          opacity="0.500000"
+          bounding_box_detection_class="box"
+          _skipIf="true"
+        />
+        <Action
+          ID="SwitchUIPrimaryView"
+          primary_view_name="/masks_visualization"
+          _skipIf="true"
+        />
+        <Action
+          ID="GetPointCloud"
+          message_out="{point_cloud}"
+          publisher_timeout_sec="5.000000"
+          timeout_sec="5.000000"
+          topic_name="/wrist_camera/points"
+        />
+        <Action
+          ID="CreateStampedPose"
+          reference_frame="ridgeback_base_link"
+          stamped_pose="{robot_pose}"
+          orientation_xyzw="0;0;0;1"
+          position_xyz="0;0;0"
+        />
+        <Action
+          ID="FindMaskedObjects"
+          base_frame="world"
+          camera_info="{camera_info_1}"
+          detected_shapes="{detected_shapes}"
+          face_separation_threshold="0.010000"
+          masks2d="{masks2d}"
+          minimum_face_area="0.000625"
+          plane_inlier_threshold="0.010000"
+          point_cloud="{point_cloud}"
+        />
+        <Action ID="Script" code="box_count:=0" />
+        <Decorator
+          ID="ForEach"
+          index="{index}"
+          out="{out}"
+          vector_in="{detected_shapes}"
+        >
+          <Control ID="Sequence">
+            <Control ID="IfThenElse">
+              <Control ID="Sequence">
+                <Action
+                  ID="ExtractGraspableObjectPose"
+                  graspable_object="{out}"
+                  pose="{pose_stamped}"
+                />
+                <Action
+                  ID="CreateGraspableObject"
+                  cuboid_object="{reference_cuboid}"
+                  dx="0.25"
+                  dy="0.25"
+                  dz="0.25"
+                  generate_front_face="true"
+                  generate_side_faces="true"
+                  generate_top_face="true"
+                  object_id="reference_cuboid"
+                  pose="{pose_stamped}"
+                />
+                <Action
+                  ID="CheckCuboidSimilarity"
+                  base_frame="world"
+                  distance_threshold="0.020000"
+                  input_cuboid="{out}"
+                  orientation_threshold="3.142"
+                  reference_cuboid="{reference_cuboid}"
+                />
+              </Control>
+              <Action ID="Script" code="box_count+=1" />
+              <Action
+                ID="AlwaysSuccess"
+                name="If the cuboid isn't similar, just don't count it"
+              />
+            </Control>
+          </Control>
+        </Decorator>
+        <Action
+          ID="ResetPlanningSceneObjects"
+          apply_planning_scene_service="/apply_planning_scene"
+          name="CreateGraspableObject adds to the planning scene but we don't want that"
+        />
+      </Control>
+      <Action
+        ID="Script"
+        code="box_count:=0"
+        name="This can happen if CLIPSeg doesn't find any boxes"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Count boxes subtree">
+      <MetadataFields>
+        <Metadata runnable="False" />
+        <Metadata subcategory="Perception - ML" />
+      </MetadataFields>
+      <inout_port
+        name="scene_camera_folder"
+        default="/home/studio-user/user_ws/src/hangar_sim/scene_camera_images"
+        type="std::string"
+      >
+        Absolute path to a directory for saving scene camera pictures
+      </inout_port>
+      <inout_port
+        name="wrist_camera_folder"
+        default="/home/studio-user/user_ws/src/hangar_sim/wrist_camera_images"
+        type="std::string"
+      >
+        Absolute path to a directory for saving wrist camera pictures
+      </inout_port>
+      <inout_port name="box_count" default="{box_count}" type="int">
+        Number of boxes
+      </inout_port>
+    </SubTree>
+  </TreeNodesModel>
+</root>

src/hangar_sim/objectives/createposevectorfromdirectory.xml

@@ -0,0 +1,47 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="CreatePoseVectorFromDirectory">
+  <BehaviorTree
+    ID="CreatePoseVectorFromDirectory"
+    _description=""
+    _favorite="false"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action
+        ID="GetFilenamesFromDirectory"
+        result="{result}"
+        directory_path="{directory_path}"
+        file_type=".yaml"
+      />
+      <Action ID="ResetVector" vector="{pose_vector}" />
+      <Decorator ID="ForEach" index="{index}" out="{out}" vector_in="{result}">
+        <Control ID="Sequence">
+          <Action
+            ID="LoadPoseStampedFromYaml"
+            output="{pose_msg}"
+            file_path="{out}"
+          />
+          <Action
+            ID="PushBackVector"
+            input_vector="{pose_vector}"
+            output_vector="{pose_vector}"
+            element="{pose_msg}"
+          />
+        </Control>
+      </Decorator>
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="CreatePoseVectorFromDirectory">
+      <MetadataFields>
+        <Metadata runnable="false" />
+        <Metadata subcategory="Application - Advanced Examples" />
+      </MetadataFields>
+      <inout_port
+        name="directory_path"
+        default="{directory_path}"
+        type="std::string"
+      />
+      <inout_port name="pose_vector" default="{pose_vector}" />
+    </SubTree>
+  </TreeNodesModel>
+</root>