Filter fingers from wrist camera point cloud

src/lab_sim/config/config.yaml

@@ -28,6 +28,9 @@ moveit_params:
   servo_joint_limits:
     package: "lab_sim"
     path: "config/moveit/hard_joint_limits.yaml"
+  sensors_3d:
+    package: "lab_sim"
+    path: "config/moveit/sensors_3d.yaml"
 
 # Configuration for loading behaviors and objectives.
 # [Required]

src/lab_sim/config/moveit/sensors_3d.yaml

@@ -1,21 +1,15 @@
 sensors:
-  - scene_scan_camera
-scene_scan_camera:
-  # The name of the Octomap updater plugin that we are using.
-  sensor_plugin: "moveit_studio_plugins/PointCloudServiceOctomapUpdater"
-  # Specifies the topic to listen on for a point cloud.
-  # Set to an empty topic to disable.
-  point_cloud_service_name: "/point_cloud_service"
-  # Points further than this will not be used (in meters).
-  max_range: 1.5
-  # Choose one of every 'point_subsample' points (select all if set to 1).
-  point_subsample: 1
-  # Should always be >= 1.0. Scale up collision shapes in the scene before excluding them from the octomap.
-  padding_scale: 1.0
-  # Absolute padding around scaled collision shapes when excluding them from the octomap (in meters).
-  padding_offset: 0.01
-  # The octomap representation will be updated at rate less than or equal to this value.
-  max_update_rate: 0.1
+  - wrist_camera
+
+wrist_camera:
+    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
+    point_cloud_topic: /wrist_camera/points # The topic on which the sensor data is received.
+    max_range: 3.0 #  Points further than this will not be used.
+    point_subsample: 1 # Choose one of every point_subsample points.
+    padding_offset: 0.01 # The size of the padding (in cm).
+    padding_scale: 1.0 # The scale of the padding.
+    max_update_rate: 0.5 # The maximum rate at which the sensor is allowed to update the octomap.
+    filtered_cloud_topic: /wrist_camera/filtered_points # The topic on which the filtered cloud will be published
 
 # Specifies the resolution at which the octomap is maintained (in meters).
 octomap_resolution: 0.01

src/lab_sim/objectives/wrist_snap.xml

@@ -1,3 +1,4 @@
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Take wrist camera snapshot">
   <!--//////////-->
   <BehaviorTree
@@ -7,8 +8,8 @@
     _subtreeOnly="false"
   >
     <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" />
+      <!--Do not clear snapshots first, so we can take multiple snapshots from different angles-->
+      <Action ID="GetPointCloud" topic_name="/wrist_camera/filtered_points" />
       <Action ID="SendPointCloudToUI" />
     </Control>
   </BehaviorTree>