Merge pull request #88 from PickNikRobotics/pr-use-core-sam-behavior

Use core segmentation behavior in `lab_sim`

Author: pac48

src/example_behaviors/CMakeLists.txt

@@ -15,7 +15,6 @@ example_interfaces)
 foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
   find_package(${package} REQUIRED)
 endforeach()
-find_package(moveit_pro_ml REQUIRED)
 
 add_library(
   example_behaviors
@@ -41,7 +40,6 @@ target_include_directories(
   PRIVATE ${PCL_INCLUDE_DIRS})
 ament_target_dependencies(example_behaviors
                           ${THIS_PACKAGE_INCLUDE_DEPENDS})
-target_link_libraries(example_behaviors onnx_sam2)
 
 # Install Libraries
 install(

src/lab_sim/CMakeLists.txt

@@ -33,6 +33,7 @@ install(
     config
     description
     launch
+    models
     objectives
     waypoints
   DESTINATION

src/example_behaviors/models/decoder.onnx src/lab_sim/models/decoder.onnxsrc/example_behaviors/models/decoder.onnx renamed to src/lab_sim/models/decoder.onnx

@@ -0,0 +1,88 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<root
+  BTCPP_format="4"
+  main_tree_to_execute="Segment Point Cloud from Clicked Point"
+>
+  <!--//////////-->
+  <BehaviorTree
+    ID="Segment Point Cloud from Clicked Point"
+    _description="Captures a point cloud and requests the user to click an object in the image to be segmented. The point cloud is then filtered to only include the selected object."
+    _favorite="true"
+  >
+    <Control ID="Sequence">
+      <Action ID="ClearSnapshot" />
+      <Action
+        ID="GetImage"
+        topic_name="/wrist_camera/color"
+        timeout_sec="5.000000"
+        message_out="{image}"
+      />
+      <Action
+        ID="GetPointsFromUser"
+        point_prompts="Select the object to be segmented;"
+        point_names="Point1;"
+        view_name="/wrist_camera/color"
+        pixel_coords="{pixel_coords}"
+      />
+      <Action
+        ID="GetMasks2DFromPointQuery"
+        image="{image}"
+        pixel_coords="{pixel_coords}"
+        masks2d="{masks2d}"
+        decoder_model_path="models/decoder.onnx"
+        encoder_model_path="models/sam2_hiera_large_encoder.onnx"
+        model_package="lab_sim"
+      />
+      <Action
+        ID="GetPointCloud"
+        topic_name="/wrist_camera/points"
+        timeout_sec="5.000000"
+        message_out="{point_cloud}"
+      />
+      <Action
+        ID="GetCameraInfo"
+        topic_name="/wrist_camera/camera_info"
+        message_out="{camera_info}"
+        timeout_sec="5.000000"
+      />
+      <Action
+        ID="GetMasks3DFromMasks2D"
+        camera_info="{camera_info}"
+        masks2d="{masks2d}"
+        point_cloud="{point_cloud}"
+        masks3d="{masks3d}"
+      />
+      <Decorator
+        ID="ForEach"
+        vector_in="{masks3d}"
+        out="{mask3d}"
+        index="{index}"
+      >
+        <Action
+          ID="GetPointCloudFromMask3D"
+          point_cloud="{point_cloud}"
+          mask3d="{mask3d}"
+          point_cloud_fragment="{point_cloud_fragment}"
+        />
+      </Decorator>
+      <Action
+        ID="SendPointCloudToUI"
+        point_cloud="{point_cloud_fragment}"
+        pcd_topic="/pcd_pointcloud_captures"
+      />
+      <Action
+        ID="PublishPointCloud"
+        point_cloud="{point_cloud_fragment}"
+        point_cloud_topic="/my_point_cloud"
+      />
+      <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization" />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Segment Point Cloud from Clicked Point">
+      <MetadataFields>
+        <Metadata runnable="true" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>