Merge pull request #431 from PickNikRobotics/octomap-example

feat: octomap example working in lab_sim

Author: shaur-k

src/lab_sim/config/config.yaml

@@ -32,6 +32,9 @@ moveit_params:
   pose_jog:
     package: "lab_sim"
     path: "config/moveit/pose_jog.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

@@ -7,18 +7,18 @@ scene_scan_camera:
   # 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
+  max_range: 6.0
   # 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
+  padding_offset: 0.1
   # The octomap representation will be updated at rate less than or equal to this value.
   max_update_rate: 0.1
 
 # Specifies the resolution at which the octomap is maintained (in meters).
-octomap_resolution: 0.01
+octomap_resolution: 0.05
 # Specifies the coordinate frame in which the Octomap representation will be stored.
 # Note! When an OccupancyMonitor instance is initialized by the PlanningSceneMonitor,
 # this frame parameter will not be used. Instead, the frame defaults to the planning frame.

src/lab_sim/objectives/octomap_example.xml

@@ -0,0 +1,38 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Octomap Example">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Octomap Example"
+    _description="Takes a snapshot of the scene camera and then updates the planning scene in RViz with an ocotomap based on the `sensors_3d.yaml` file settings."
+    _favorite="false"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="ClearSnapshot" />
+      <Action
+        ID="GetPointCloud"
+        message_out="{point_cloud}"
+        publisher_timeout_sec="5.000000"
+        timeout_sec="5.000000"
+        topic_name="/scene_camera/points"
+      />
+      <Action
+        ID="UpdatePlanningSceneService"
+        point_cloud="{point_cloud}"
+        point_cloud_service="/point_cloud_service"
+      />
+      <Action
+        ID="SendPointCloudToUI"
+        pcd_topic="/pcd_pointcloud_captures"
+        point_cloud="{point_cloud}"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Octomap Example">
+      <MetadataFields>
+        <Metadata runnable="true" />
+        <Metadata subcategory="Perception - 3D Point Cloud" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>