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

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
+        footprint_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.h"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "tf2/buffer_core.h"
-
+#include "tf2_ros/buffer.h"
 namespace spatio_temporal_voxel_layer
 {
 
@@ -180,18 +180,19 @@ 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, _footprint_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;
   boost::recursive_mutex _voxel_grid_lock;
 
   std::string _topics_string;
 
+
   // Dynamic parameters handler
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler;
 };

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,89 @@ 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 =
+   // transform the cloud in the global frame, directly or indirectly to apply relative z filter
+    point_cloud_ptr cld_transformed(new sensor_msgs::msg::PointCloud2());
+
+    geometry_msgs::msg::TransformStamped tf_direct_global =
       _buffer.lookupTransform(
       _global_frame, cloud.header.frame_id,
       tf2_ros::fromMsg(cloud.header.stamp));
-    tf2::doTransform(cloud, *cld_global, tf_stamped);
+
+    geometry_msgs::msg::TransformStamped tf_z_reference =
+      _buffer.lookupTransform(
+      _z_reference_frame, cloud.header.frame_id,
+      tf2_ros::fromMsg(cloud.header.stamp));
+
+    geometry_msgs::msg::TransformStamped tf_global_from_z_reference =
+      _buffer.lookupTransform(
+      _global_frame, _z_reference_frame,
+      tf2_ros::fromMsg(cloud.header.stamp));
 
     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;
+
+    switch (_filter)
+    {
+    case Filters::PASSTHROUGH :
+      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 :
+      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 :
+      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 :
+      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();

spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp

@@ -78,7 +78,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
   RCLCPP_INFO(
     logger_, "%s's global frame is %s.",
     getName().c_str(), _global_frame.c_str());
-
+  
   bool track_unknown_space;
   double transform_tolerance, map_save_time;
   int decay_model_int;
@@ -108,6 +108,12 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
   // clear under robot footprint
   declareParameter("update_footprint_enabled", rclcpp::ParameterValue(true));
   node->get_parameter(name_ + ".update_footprint_enabled", _update_footprint_enabled);
+  // use 3d transform for clearing footprint
+  declareParameter("footprint_projection_enabled", rclcpp::ParameterValue(false));
+  node->get_parameter(name_ + ".footprint_projection_enabled", _footprint_projection_enabled);
+  // footprint frame ( used only with footprint projection enabled )
+  declareParameter("footprint_frame", rclcpp::ParameterValue(std::string("")));
+  node->get_parameter(name_ + ".footprint_frame", _footprint_frame);
   // keep tabs on unknown space
   declareParameter(
     "track_unknown_space",
@@ -169,7 +175,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
     double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height;
     double min_z, max_z, vFOV, vFOVPadding;
     double hFOV, decay_acceleration, obstacle_range;
-    std::string topic, sensor_frame, data_type, filter_str;
+    std::string topic, sensor_frame, z_reference_frame, data_type, filter_str;
     bool inf_is_valid = false, clearing, marking;
     bool clear_after_reading, enabled;
     int voxel_min_points;
@@ -196,6 +202,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
     declareParameter(source + "." + "horizontal_fov_angle", rclcpp::ParameterValue(1.04));
     declareParameter(source + "." + "decay_acceleration", rclcpp::ParameterValue(0.0));
     declareParameter(source + "." + "filter", rclcpp::ParameterValue(std::string("passthrough")));
+    declareParameter(source + "." + "z_reference_frame", rclcpp::ParameterValue(_global_frame));    
     declareParameter(source + "." + "voxel_min_points", rclcpp::ParameterValue(0));
     declareParameter(source + "." + "clear_after_reading", rclcpp::ParameterValue(false));
     declareParameter(source + "." + "enabled", rclcpp::ParameterValue(true));
@@ -231,6 +238,8 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
     node->get_parameter(name_ + "." + source + "." + "decay_acceleration", decay_acceleration);
     // performs an approximate voxel filter over the data to reduce
     node->get_parameter(name_ + "." + source + "." + "filter", filter_str);
+    // frame in which to calculate z bounds if relative filter is applied
+    node->get_parameter(name_ + "." + source + "." + "z_reference_frame", z_reference_frame);
     // 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
@@ -242,13 +251,20 @@ 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(logger_, "Passthough filter activated.");
       filter = buffer::Filters::PASSTHROUGH;
     } else if (filter_str == "voxel") {
       RCLCPP_INFO(logger_, "Voxel filter activated.");
       filter = buffer::Filters::VOXEL;
-    } else {
+    } else if (filter_str == "passthrough_relative") {
+      RCLCPP_INFO(logger_, "Relative Passthough filter activated.");
+      filter = buffer::Filters::PASSTHROUGH_RELATIVE;
+    } else if (filter_str == "voxel_relative") {
+      RCLCPP_INFO(logger_, "Relative Voxel filter activated.");
+      filter = buffer::Filters::VOXEL_RELATIVE;
+    }  else {
       RCLCPP_INFO(logger_, "No filters activated.");
       filter = buffer::Filters::NONE;
     }
@@ -264,7 +280,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
         new buffer::MeasurementBuffer(
           source, topic,
           observation_keep_time, expected_update_rate, min_obstacle_height,
-          max_obstacle_height, obstacle_range, *tf_, _global_frame, sensor_frame,
+          max_obstacle_height, obstacle_range, *tf_, _global_frame, sensor_frame, z_reference_frame,
           transform_tolerance, min_z, max_z, vFOV, vFOVPadding, hFOV,
           decay_acceleration, marking, clearing, _voxel_size,
           filter, voxel_min_points, enabled, clear_after_reading, model_type,
@@ -533,17 +549,43 @@ bool SpatioTemporalVoxelLayer::updateFootprint(
   double * min_y, double * max_x, double * max_y)
 /*****************************************************************************/
 {
-  // updates layer costmap to include footprint for clearing in voxel grid
+  // Updates layer costmap to include footprint for clearing in voxel grid
   if (!_update_footprint_enabled) {
     return false;
   }
-  nav2_costmap_2d::transformFootprint(
-    robot_x, robot_y, robot_yaw,
-    getFootprint(), _transformed_footprint);
-  for (unsigned int i = 0; i < _transformed_footprint.size(); i++) {
-    touch(
-      _transformed_footprint[i].x, _transformed_footprint[i].y,
-      min_x, min_y, max_x, max_y);
+
+  if (!_footprint_projection_enabled) {
+    // Simple 2D transformation
+    nav2_costmap_2d::transformFootprint(
+      robot_x, robot_y, robot_yaw,
+      getFootprint(), _transformed_footprint);
+    for (unsigned int i = 0; i < _transformed_footprint.size(); i++) {
+      touch(
+        _transformed_footprint[i].x, _transformed_footprint[i].y,
+        min_x, min_y, max_x, max_y);
+    }
+  } else {
+    // Using tf2 for 3d rotation to provide accurate projection of the footprint
+    try {
+      geometry_msgs::msg::TransformStamped tf_footprint_stamped =
+        tf_->lookupTransform(
+          _global_frame, _footprint_frame,
+          rclcpp::Time(0)); 
+
+      _transformed_footprint = getFootprint();
+      for (unsigned int i = 0; i < _transformed_footprint.size(); i++) {
+        tf2::doTransform(_transformed_footprint[i], _transformed_footprint[i], tf_footprint_stamped);       
+        touch(
+          _transformed_footprint[i].x, _transformed_footprint[i].y,
+          min_x, min_y, max_x, max_y);
+      }
+
+    } catch (tf2::TransformException &ex) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("SpatioTemporalVoxelLayer"),
+        "Could not perform a transform for footprint projection: %s", ex.what());
+      return false;
+    }
   }
 
   return true;