[ROS2] Adding optional filter (#173)

* adding filter options to ROS2

* changing macros for ros

* remove deprecated foxy header

README.md

@@ -101,6 +101,8 @@ Required dependencies ROS Kinetic, navigation, OpenVDB, TBB.
 
 An example fully-described configuration is shown below.
 
+Note: We supply two PCL filters within STVL to massage the data to lower compute overhead. STVL has an approximate voxel filter to make the data more sparse if very dense. It also has a passthrough filter to limit processing data within the valid minimum to maximum height bounds. The voxel filter is recommended if it lowers CPU overhead, otherwise, passthrough filter. No filter is also available if you pre-process your data or are not interested in performance optimizations. 
+
 ```
 rgbd_obstacle_layer:
   enabled:               true
@@ -132,7 +134,7 @@ rgbd_obstacle_layer:
     observation_persistence: 0.0 #default 0, use all measurements taken during now-value, 0=latest 
     inf_is_valid: false          #default false, for laser scans
     clear_after_reading: true    #default false, clear the buffer after the layer gets readings from it
-    voxel_filter: true           #default off, apply voxel filter to sensor, recommend on 
+    filter: "voxel"              #default passthrough, apply "voxel", "passthrough", or no filter to sensor data, recommended to have at one filter on
     voxel_min_points: 0          #default 0, minimum points per voxel for voxel filter
   rgbd1_clear:
     enabled: true                #default true, can be toggled on/off with associated service call

example/constrained_indoor_environment_config.yaml

@@ -29,7 +29,7 @@ global_costmap:
           expected_update_rate: 0.0    # default 0, if not updating at this rate at least, remove from buffer
           observation_persistence: 0.0 # default 0, use all measurements taken during now-value, 0=latest
           inf_is_valid: false          # default false, for laser scans
-          voxel_filter: false          # default off, apply voxel filter to sensor, recommend on
+          filter: "passthrough"        # default passthrough, apply "voxel", "passthrough", or no filter to sensor data, recommend on 
           voxel_min_points: 0          # default 0, minimum points per voxel for voxel filter
           clear_after_reading: true    # default false, clear the buffer after the layer gets readings from it
         rgbd1_clear:

example/outdoor_environment_config.yaml

@@ -29,7 +29,7 @@ global_costmap:
           expected_update_rate: 0.0    # default 0, if not updating at this rate at least, remove from buffer
           observation_persistence: 0.0 # default 0, use all measurements taken during now-value, 0=latest
           inf_is_valid: false          # default false, for laser scans
-          voxel_filter: false          # default off, apply voxel filter to sensor, recommend on
+          filter: "passthrough"        # default passthrough, apply "voxel", "passthrough", or no filter to sensor data, recommend on 
           voxel_min_points: 0          # default 0, minimum points per voxel for voxel filter
           clear_after_reading: true    # default false, clear the buffer after the layer gets readings from it
         rgbd1_clear:

example/standard_indoor_environment_config.yaml

@@ -29,7 +29,7 @@ global_costmap:
           expected_update_rate: 0.0    # default 0, if not updating at this rate at least, remove from buffer
           observation_persistence: 0.0 # default 0, use all measurements taken during now-value, 0=latest
           inf_is_valid: false          # default false, for laser scans
-          voxel_filter: false          # default off, apply voxel filter to sensor, recommend on
+          filter: "passthrough"        # default passthrough, apply "voxel", "passthrough", or no filter to sensor data, recommend on 
           voxel_min_points: 0          # default 0, minimum points per voxel for voxel filter
           clear_after_reading: true    # default false, clear the buffer after the layer gets readings from it
         rgbd1_clear:

example/vlp16_config.yaml

@@ -26,8 +26,7 @@ rgbd_obstacle_layer:
     expected_update_rate: 0.0    # default 0, if not updating at this rate at least, remove from buffer
     observation_persistence: 0.0 # default 0, use all measurements taken during now-value, 0=latest
     inf_is_valid: false          # default false, for laser scans
-    voxel_filter: false          # default off, apply voxel filter to sensor, recommend on
-    voxel_min_points: 0          # default 0, minimum points per voxel for voxel filter
+    filter: "voxel"        # default passthrough, apply "voxel", "passthrough", or no filter to sensor data, recommend on     voxel_min_points: 0          # default 0, minimum points per voxel for voxel filter
     clear_after_reading: true    # default false, clear the buffer after the layer gets readings from it
   rgbd1_clear:
     enabled: true                #default true, can be toggled on/off with associated service call

include/spatio_temporal_voxel_layer/measurement_buffer.hpp

@@ -69,6 +69,13 @@
 namespace buffer
 {
 
+enum class Filters
+{
+  NONE = 0,
+  VOXEL = 1,
+  PASSTHROUGH = 2
+};
+
 // conveniences for line lengths
 typedef std::list<observation::MeasurementReading>::iterator readings_iter;
 typedef std::unique_ptr<sensor_msgs::msg::PointCloud2> point_cloud_ptr;
@@ -97,7 +104,7 @@ class MeasurementBuffer
     const bool & marking,
     const bool & clearing,
     const double & voxel_size,
-    const bool & voxel_filter,
+    const Filters & filter,
     const int & voxel_min_points,
     const bool & enabled,
     const bool & clear_buffer_after_reading,
@@ -139,7 +146,8 @@ class MeasurementBuffer
   double _min_obstacle_height, _max_obstacle_height, _obstacle_range, _tf_tolerance;
   double _min_z, _max_z, _vertical_fov, _vertical_fov_padding, _horizontal_fov;
   double _decay_acceleration, _voxel_size;
-  bool _marking, _clearing, _voxel_filter;
+  bool _marking, _clearing;
+  Filters _filter;
   int _voxel_min_points;
   bool _clear_buffer_after_reading, _enabled;
   ModelType _model_type;

include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp

@@ -67,6 +67,7 @@
 #include "openvdb/openvdb.h"
 #include "openvdb/tools/GridTransformer.h"
 #include "openvdb/tools/RayIntersector.h"
+
 // measurement struct and buffer
 #include "spatio_temporal_voxel_layer/measurement_buffer.hpp"
 #include "spatio_temporal_voxel_layer/frustum_models/depth_camera_frustum.hpp"

include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp

@@ -61,7 +61,6 @@
 // msgs
 #include "sensor_msgs/msg/laser_scan.hpp"
 #include "sensor_msgs/msg/point_cloud2.hpp"
-#include "sensor_msgs/point_cloud_conversion.hpp"
 #include "geometry_msgs/msg/point.hpp"
 #include "spatio_temporal_voxel_layer/srv/save_grid.hpp"
 #include "std_srvs/srv/set_bool.hpp"

src/measurement_buffer.cpp

@@ -56,7 +56,7 @@ MeasurementBuffer::MeasurementBuffer(
   const double & min_d, const double & max_d, const double & vFOV,
   const double & vFOVPadding, const double & hFOV,
   const double & decay_acceleration, const bool & marking,
-  const bool & clearing, const double & voxel_size, const bool & voxel_filter,
+  const bool & clearing, const double & voxel_size, const Filters & filter,
   const int & voxel_min_points, const bool & enabled,
   const bool & clear_buffer_after_reading, const ModelType & model_type,
   std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node)
@@ -69,7 +69,7 @@ MeasurementBuffer::MeasurementBuffer(
   _vertical_fov(vFOV), _vertical_fov_padding(vFOVPadding),
   _horizontal_fov(hFOV), _decay_acceleration(decay_acceleration),
   _voxel_size(voxel_size), _marking(marking), _clearing(clearing),
-  _voxel_filter(voxel_filter), _voxel_min_points(voxel_min_points),
+  _filter(filter), _voxel_min_points(voxel_min_points),
   _clear_buffer_after_reading(clear_buffer_after_reading),
   _enabled(enabled), _model_type(model_type), node_(node)
 /*****************************************************************************/
@@ -142,11 +142,10 @@ void MeasurementBuffer::BufferROSCloud(
     pcl::PCLPointCloud2::Ptr cloud_pcl(new pcl::PCLPointCloud2());
     pcl::PCLPointCloud2::Ptr cloud_filtered(new pcl::PCLPointCloud2());
 
-    pcl_conversions::toPCL(*cld_global, *cloud_pcl);
-
     // remove points that are below or above our height restrictions, and
     // in the same time, remove NaNs and if user wants to use it, combine with a
-    if (_voxel_filter) {
+    if (_filter == Filters::VOXEL) {
+      pcl_conversions::toPCL(*cld_global, *cloud_pcl);
       pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
       sor.setInputCloud(cloud_pcl);
       sor.setFilterFieldName("z");
@@ -156,17 +155,19 @@ void MeasurementBuffer::BufferROSCloud(
       sor.setLeafSize(v_s, v_s, v_s);
       sor.setMinimumPointsNumberPerVoxel(static_cast<unsigned int>(_voxel_min_points));
       sor.filter(*cloud_filtered);
-    } else {
+      pcl_conversions::fromPCL(*cloud_filtered, *cld_global);
+    } else if (_filter == Filters::PASSTHROUGH) {
+      pcl_conversions::toPCL(*cld_global, *cloud_pcl);
       pcl::PassThrough<pcl::PCLPointCloud2> pass_through_filter;
       pass_through_filter.setInputCloud(cloud_pcl);
       pass_through_filter.setKeepOrganized(false);
       pass_through_filter.setFilterFieldName("z");
       pass_through_filter.setFilterLimits(
         _min_obstacle_height, _max_obstacle_height);
       pass_through_filter.filter(*cloud_filtered);
+      pcl_conversions::fromPCL(*cloud_filtered, *cld_global);
     }
 
-    pcl_conversions::fromPCL(*cloud_filtered, *cld_global);
     _observation_list.front()._cloud.reset(cld_global.release());
   } catch (tf2::TransformException & ex) {
     // if fails, remove the empty observation

src/spatio_temporal_voxel_layer.cpp

@@ -151,10 +151,11 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
     double observation_keep_time, expected_update_rate, min_obstacle_height;
     double max_obstacle_height, min_z, max_z, vFOV, vFOVPadding;
     double hFOV, decay_acceleration, obstacle_range;
-    std::string topic, sensor_frame, data_type;
+    std::string topic, sensor_frame, data_type, filter_str;
     bool inf_is_valid = false, clearing, marking;
-    bool voxel_filter, clear_after_reading, enabled;
+    bool clear_after_reading, enabled;
     int voxel_min_points;
+    buffer::Filters filter;
 
     declareParameter(source + "." + "topic", rclcpp::ParameterValue(std::string("")));
     declareParameter(source + "." + "sensor_frame", rclcpp::ParameterValue(std::string("")));
@@ -175,7 +176,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
     declareParameter(source + "." + "vertical_fov_padding", rclcpp::ParameterValue(0.0));
     declareParameter(source + "." + "horizontal_fov_angle", rclcpp::ParameterValue(1.04));
     declareParameter(source + "." + "decay_acceleration", rclcpp::ParameterValue(0.0));
-    declareParameter(source + "." + "voxel_filter", rclcpp::ParameterValue(false));
+    declareParameter(source + "." + "filter", rclcpp::ParameterValue(std::string("passthrough")));
     declareParameter(source + "." + "voxel_min_points", rclcpp::ParameterValue(0));
     declareParameter(source + "." + "clear_after_reading", rclcpp::ParameterValue(false));
     declareParameter(source + "." + "enabled", rclcpp::ParameterValue(true));
@@ -208,7 +209,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
     // acceleration scales the model's decay in presence of readings
     node_->get_parameter(name_ + "." + source + "." + "decay_acceleration", decay_acceleration);
     // performs an approximate voxel filter over the data to reduce
-    node_->get_parameter(name_ + "." + source + "." + "voxel_filter", voxel_filter);
+    node_->get_parameter(name_ + "." + source + "." + "filter", filter_str);
     // minimum points per voxel for voxel filter
     node_->get_parameter(name_ + "." + source + "." + "voxel_min_points", voxel_min_points);
     // clears measurement buffer after reading values from it
@@ -220,6 +221,22 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
     node_->get_parameter(name_ + "." + source + "." + "model_type", model_type_int);
     ModelType model_type = static_cast<ModelType>(model_type_int);
 
+    if (filter_str == "passthrough")
+    {
+      RCLCPP_INFO(node_->get_logger(), "Passthough filter activated.");
+      filter = buffer::Filters::PASSTHROUGH;
+    }
+    else if (filter_str == "voxel")
+    {
+      RCLCPP_INFO(node_->get_logger(), "Voxel filter activated.");
+      filter = buffer::Filters::VOXEL;
+    }
+    else
+    {
+      RCLCPP_INFO(node_->get_logger(), "No filters activated.");
+      filter = buffer::Filters::NONE;
+    }
+
     if (!(data_type == "PointCloud2" || data_type == "LaserScan")) {
       throw std::runtime_error(
               "Only topics that use pointclouds or laser scans are supported.");
@@ -232,7 +249,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
       max_obstacle_height, obstacle_range, *tf_, _global_frame, sensor_frame,
       transform_tolerance, min_z, max_z, vFOV, vFOVPadding, hFOV,
       decay_acceleration, marking, clearing, _voxel_size,
-      voxel_filter, voxel_min_points, enabled, clear_after_reading, model_type,
+      filter, voxel_min_points, enabled, clear_after_reading, model_type,
       node_)));
 
     // Add buffer to marking observation buffers