Relative Variants of Voxel and Passthrough Filters and Footprint Projection for Slope Navigation

README.md

@@ -101,7 +101,7 @@ 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. 
+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. Filters have "relative" variants made to use robot's frame for obstacle classification instead of the global frame in case of navigating sloped surfaces.
 
 ```
 rgbd_obstacle_layer:
@@ -113,7 +113,9 @@ rgbd_obstacle_layer:
   observation_persistence: 0.0  #seconds
   max_obstacle_height:   2.0    #meters
   mark_threshold:        0      #voxel height
-  update_footprint_enabled: true
+  update_footprint_enabled: true 
+  footprint_projection_enabled: true #default off, recommended when using 3d IMU data. Uses tf2 to transform the footprint
+  robot_base_frame: "base_link" #frame to use with footprint_projection_enabled
   combination_method:    1      #1=max, 0=override
   obstacle_range:        3.0    #meters
   origin_z:              0.0    #meters
@@ -133,7 +135,8 @@ 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
-    filter: "voxel"              #default passthrough, apply "voxel", "passthrough", or no filter to sensor data, recommended to have at one filter on
+    filter: "voxel"              #default passthrough, apply "voxel", "passthrough", "voxel_relative", "passthrough_relative", or no filter to sensor data, recommended to have at one filter on
+    z_reference_frame: "base_link" #default global_frame, use with *_relative filters as a frame for obstacle classification based on z coordinate
     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

spatio_temporal_voxel_layer/example/uneven_terrain_config.yaml

@@ -0,0 +1,47 @@
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      rgbd_obstacle_layer:
+        plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer"
+        enabled:                  true
+        voxel_decay:              1.0  # seconds if linear, e^n if exponential
+        decay_model:              0     # 0=linear, 1=exponential, -1=persistent
+        voxel_size:               0.05  # meters
+        track_unknown_space:      true  # default space is known
+        mark_threshold:           0     # voxel height
+        update_footprint_enabled: true
+        footprint_projection_enabled: true # default off, strongly recommended when using 3d IMU data
+        robot_base_frame: "base_link" # necessary for use with footprint projection, 
+        combination_method:       1     # 1=max, 0=override
+        origin_z:                 0.0   # meters
+        publish_voxel_map:        false # default off
+        transform_tolerance:      0.2   # seconds
+        mapping_mode:             false # default off, saves map not for navigation
+        map_save_duration:        60.0  # default 60s, how often to autosave
+        observation_sources:      rgbd1_mark rgbd1_clear
+        rgbd1_mark:
+          data_type: PointCloud2
+          topic: camera1/depth/points
+          marking: true
+          clearing: false
+          obstacle_range: 3.0          # meters
+          min_obstacle_height: 0.3     # default 0, meters
+          max_obstacle_height: 2.0     # default 3, meters
+          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
+          filter: "passthrough_relative"        # default passthrough, apply "voxel", "passthrough", their relative variants or no filter to sensor data, recommend on 
+          z_reference_frame: "base_link" # if unspecified keeps using global frame for z filter, frame to calcualte the z coordinate before transforming pointcloud to global frame
+          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:
+          data_type: PointCloud2
+          topic: camera1/depth/points
+          marking: false
+          clearing: true
+          max_z: 7.0                  # default 0, meters
+          min_z: 0.1                  # default 10, meters
+          vertical_fov_angle: 0.8745  # default 0.7, radians
+          horizontal_fov_angle: 1.048 # default 1.04, radians
+          decay_acceleration: 1.0     # default 0, 1/s^2. If laser scanner MUST be 0
+          model_type: 0                # default 0, model type for frustum. 0=depth camera, 1=3d lidar like VLP16 or similar

spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/measurement_buffer.hpp

@@ -73,7 +73,9 @@ enum class Filters
 {
   NONE = 0,
   VOXEL = 1,
-  PASSTHROUGH = 2
+  PASSTHROUGH = 2,
+  VOXEL_RELATIVE = 3,
+  PASSTHROUGH_RELATIVE = 4
 };
 
 // conveniences for line lengths
@@ -95,6 +97,7 @@ class MeasurementBuffer
     tf2_ros::Buffer & tf,
     const std::string & global_frame,
     const std::string & sensor_frame,
+    const std::string & z_reference_frame,
     const double & tf_tolerance,
     const double & min_d,
     const double & max_d,
@@ -155,7 +158,7 @@ class MeasurementBuffer
   const rclcpp::Duration _observation_keep_time, _expected_update_rate;
   rclcpp::Time _last_updated;
   boost::recursive_mutex _lock;
-  std::string _global_frame, _sensor_frame, _source_name, _topic_name;
+  std::string _global_frame, _sensor_frame, _source_name, _topic_name, _z_reference_frame;
   std::list<observation::MeasurementReading> _observation_list;
   double _min_obstacle_height, _max_obstacle_height, _obstacle_range, _tf_tolerance;
   double _min_z, _max_z, _vertical_fov, _vertical_fov_padding, _horizontal_fov;

spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp

@@ -76,7 +76,7 @@
 #include "message_filters/subscriber.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "tf2/buffer_core.h"
-
+#include "tf2_ros/buffer.h"
 namespace spatio_temporal_voxel_layer
 {
 
@@ -135,6 +135,9 @@ class SpatioTemporalVoxelLayer : public nav2_costmap_2d::CostmapLayer
     std::shared_ptr<spatio_temporal_voxel_layer::srv::SaveGrid::Response> resp);
 
 private:
+  // clock
+  rclcpp::Clock::SharedPtr _clock;
+
   // Sensor callbacks
   void LaserScanCallback(
     sensor_msgs::msg::LaserScan::ConstSharedPtr message,
@@ -180,11 +183,11 @@ class SpatioTemporalVoxelLayer : public nav2_costmap_2d::CostmapLayer
   rclcpp::Service<spatio_temporal_voxel_layer::srv::SaveGrid>::SharedPtr _grid_saver;
   std::unique_ptr<rclcpp::Duration> _map_save_duration;
   rclcpp::Time _last_map_save_time;
-  std::string _global_frame;
+  std::string _global_frame, _robot_base_frame;
   double _voxel_size, _voxel_decay;
   int _combination_method, _mark_threshold;
   volume_grid::GlobalDecayModel _decay_model;
-  bool _update_footprint_enabled, _enabled;
+  bool _update_footprint_enabled, _footprint_projection_enabled, _enabled;
   std::vector<geometry_msgs::msg::Point> _transformed_footprint;
   std::vector<observation::MeasurementReading> _static_observations;
   std::unique_ptr<volume_grid::SpatioTemporalVoxelGrid> _voxel_grid;

spatio_temporal_voxel_layer/src/measurement_buffer.cpp

@@ -54,7 +54,7 @@ MeasurementBuffer::MeasurementBuffer(
   const double & observation_keep_time, const double & expected_update_rate,
   const double & min_obstacle_height, const double & max_obstacle_height,
   const double & obstacle_range, tf2_ros::Buffer & tf, const std::string & global_frame,
-  const std::string & sensor_frame, const double & tf_tolerance,
+  const std::string & sensor_frame, const std::string & z_reference_frame, const double & tf_tolerance,
   const double & min_d, const double & max_d, const double & vFOV,
   const double & vFOVPadding, const double & hFOV,
   const double & decay_acceleration, const bool & marking,
@@ -67,7 +67,7 @@ MeasurementBuffer::MeasurementBuffer(
   _expected_update_rate(rclcpp::Duration::from_seconds(expected_update_rate)),
   _last_updated(clock->now()),
   _global_frame(global_frame), _sensor_frame(sensor_frame), _source_name(source_name),
-  _topic_name(topic_name), _min_obstacle_height(min_obstacle_height),
+  _topic_name(topic_name), _z_reference_frame(z_reference_frame), _min_obstacle_height(min_obstacle_height),
   _max_obstacle_height(max_obstacle_height), _obstacle_range(obstacle_range),
   _tf_tolerance(tf_tolerance), _min_z(min_d), _max_z(max_d),
   _vertical_fov(vFOV), _vertical_fov_padding(vFOVPadding),
@@ -137,44 +137,98 @@ void MeasurementBuffer::BufferROSCloud(
       return;
     }
 
-    // transform the cloud in the global frame
-    point_cloud_ptr cld_global(new sensor_msgs::msg::PointCloud2());
-    geometry_msgs::msg::TransformStamped tf_stamped =
-      _buffer.lookupTransform(
-      _global_frame, cloud.header.frame_id,
-      tf2_ros::fromMsg(cloud.header.stamp));
-    tf2::doTransform(cloud, *cld_global, tf_stamped);
-
+    point_cloud_ptr cld_transformed(new sensor_msgs::msg::PointCloud2());
     pcl::PCLPointCloud2::Ptr cloud_pcl(new pcl::PCLPointCloud2());
     pcl::PCLPointCloud2::Ptr cloud_filtered(new pcl::PCLPointCloud2());
-
+    pcl::PassThrough<pcl::PCLPointCloud2> pass_through_filter;
+    pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
+    float v_s = static_cast<float>(_voxel_size);
     // 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 (_filter == Filters::VOXEL) {
-      pcl_conversions::toPCL(*cld_global, *cloud_pcl);
-      pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
+
+    geometry_msgs::msg::TransformStamped tf_direct_global, tf_z_reference, tf_global_from_z_reference;
+
+
+    switch (_filter)
+    {
+    case Filters::PASSTHROUGH :
+      tf_direct_global =
+        _buffer.lookupTransform(
+        _global_frame, cloud.header.frame_id,
+        tf2_ros::fromMsg(cloud.header.stamp));
+      tf2::doTransform(cloud, *cld_transformed, tf_direct_global);
+      pcl_conversions::toPCL(*cld_transformed, *cloud_pcl);
+      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_transformed);
+      break;
+    case Filters::VOXEL :
+      tf_direct_global =
+        _buffer.lookupTransform(
+        _global_frame, cloud.header.frame_id,
+        tf2_ros::fromMsg(cloud.header.stamp));
+      tf2::doTransform(cloud, *cld_transformed, tf_direct_global);
+      pcl_conversions::toPCL(*cld_transformed, *cloud_pcl);
       sor.setInputCloud(cloud_pcl);
       sor.setFilterFieldName("z");
       sor.setFilterLimits(_min_obstacle_height, _max_obstacle_height);
       sor.setDownsampleAllData(false);
-      float v_s = static_cast<float>(_voxel_size);
       sor.setLeafSize(v_s, v_s, v_s);
       sor.setMinimumPointsNumberPerVoxel(static_cast<unsigned int>(_voxel_min_points));
       sor.filter(*cloud_filtered);
-      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;
+      pcl_conversions::fromPCL(*cloud_filtered, *cld_transformed);
+      break;
+    case Filters::PASSTHROUGH_RELATIVE :
+      tf_z_reference =
+        _buffer.lookupTransform(
+        _z_reference_frame, cloud.header.frame_id,
+        tf2_ros::fromMsg(cloud.header.stamp));
+      tf_global_from_z_reference =
+        _buffer.lookupTransform(
+        _global_frame, _z_reference_frame,
+        tf2_ros::fromMsg(cloud.header.stamp));
+      tf2::doTransform(cloud, *cld_transformed, tf_z_reference);
+      pcl_conversions::toPCL(*cld_transformed, *cloud_pcl);
       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_transformed);
+      tf2::doTransform(*cld_transformed, *cld_transformed, tf_global_from_z_reference);
+      break;
+    case Filters::VOXEL_RELATIVE :
+      tf_z_reference =
+        _buffer.lookupTransform(
+        _z_reference_frame, cloud.header.frame_id,
+        tf2_ros::fromMsg(cloud.header.stamp));
+      tf_global_from_z_reference =
+        _buffer.lookupTransform(
+        _global_frame, _z_reference_frame,
+        tf2_ros::fromMsg(cloud.header.stamp));
+      tf2::doTransform(cloud, *cld_transformed, tf_z_reference);
+      pcl_conversions::toPCL(*cld_transformed, *cloud_pcl);
+      sor.setInputCloud(cloud_pcl);
+      sor.setFilterFieldName("z");
+      sor.setFilterLimits(_min_obstacle_height, _max_obstacle_height);
+      sor.setDownsampleAllData(false);
+      sor.setLeafSize(v_s, v_s, v_s);
+      sor.setMinimumPointsNumberPerVoxel(static_cast<unsigned int>(_voxel_min_points));
+      sor.filter(*cloud_filtered);
+      pcl_conversions::fromPCL(*cloud_filtered, *cld_transformed);
+      tf2::doTransform(*cld_transformed, *cld_transformed, tf_global_from_z_reference);
+      break;    
+    default:
+      tf2::doTransform(cloud, *cld_transformed, tf_direct_global);
+      break;
     }
 
-    _observation_list.front()._cloud.reset(cld_global.release());
+    _observation_list.front()._cloud.reset(cld_transformed.release());
   } catch (tf2::TransformException & ex) {
     // if fails, remove the empty observation
     _observation_list.pop_front();