Merge branch 'outside_duration_threshold' into HEAD

Author: RBT22

include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp

@@ -129,7 +129,7 @@ class SpatioTemporalVoxelGrid
   SpatioTemporalVoxelGrid(
     rclcpp::Clock::SharedPtr clock,
     const float & voxel_size, const double & background_value,
-    const int & decay_model, const double & voxel_decay,
+    const int & decay_model, const double & min_age_outside_frustum, const double & voxel_decay,
     const bool & pub_voxels);
   ~SpatioTemporalVoxelGrid(void);
 
@@ -181,7 +181,7 @@ class SpatioTemporalVoxelGrid
 
   mutable openvdb::DoubleGrid::Ptr _grid;
   int _decay_model;
-  double _background_value, _voxel_size, _voxel_decay;
+  double _background_value, _voxel_size, _min_age_outside_frustum, _voxel_decay;
   bool _pub_voxels;
   std::unique_ptr<std::vector<geometry_msgs::msg::Point32>> _grid_points;
   std::unordered_map<occupany_cell, uint> * _cost_map;

include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp

@@ -181,7 +181,7 @@ class SpatioTemporalVoxelLayer : public nav2_costmap_2d::CostmapLayer
   std::unique_ptr<rclcpp::Duration> _map_save_duration;
   rclcpp::Time _last_map_save_time;
   std::string _global_frame;
-  double _voxel_size, _voxel_decay;
+  double _voxel_size, _min_age_outside_frustum, _voxel_decay;
   int _mark_threshold;
   nav2_costmap_2d::CombinationMethod _combination_method;
   volume_grid::GlobalDecayModel _decay_model;

src/spatio_temporal_voxel_grid.cpp

@@ -49,8 +49,10 @@ namespace volume_grid
 SpatioTemporalVoxelGrid::SpatioTemporalVoxelGrid(
   rclcpp::Clock::SharedPtr clock,
   const float & voxel_size, const double & background_value,
-  const int & decay_model, const double & voxel_decay, const bool & pub_voxels)
-: _clock(clock), _decay_model(decay_model), _background_value(background_value),
+  const int & decay_model, const double & min_age_outside_frustum,
+  const double & voxel_decay, const bool & pub_voxels)
+: _clock(clock), _decay_model(decay_model), _min_age_outside_frustum(min_age_outside_frustum),
+  _background_value(background_value),
   _voxel_size(voxel_size), _voxel_decay(voxel_decay), _pub_voxels(pub_voxels),
   _grid_points(std::make_unique<std::vector<geometry_msgs::msg::Point32>>()),
   _cost_map(new std::unordered_map<occupany_cell, uint>)
@@ -199,9 +201,14 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap(
       }
     }
 
-    // if not inside any, check against nominal decay model
+    // if not inside any, check against nominal decay model, also clearing if
+    // too young.
+    // Depending on min_age_outside_frustum value and cycle frequency,
+    // a voxel too young  means that it was marked without being in a frustrum
+    // (or, edgy case, the point that marked the voxel was in a frustrum
+    // but the center of that voxel was not), in that case : clearing
     if (!frustum_cycle) {
-      if (base_duration_to_decay < 0.) {
+      if (base_duration_to_decay < 0. || time_since_marking < _min_age_outside_frustum) {
         // expired by temporal clearing
         cleared_point = true;
         if (!this->ClearGridPoint(pt_index)) {

src/spatio_temporal_voxel_layer.cpp

@@ -118,6 +118,8 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
   declareParameter("decay_model", rclcpp::ParameterValue(0));
   node->get_parameter(name_ + ".decay_model", decay_model_int);
   _decay_model = static_cast<volume_grid::GlobalDecayModel>(decay_model_int);
+  declareParameter("min_age_outside_frustum", rclcpp::ParameterValue(0.0));
+  node->get_parameter(name_ + ".min_age_outside_frustum", _min_age_outside_frustum);
   // decay param
   declareParameter("voxel_decay", rclcpp::ParameterValue(-1.0));
   node->get_parameter(name_ + ".voxel_decay", _voxel_decay);
@@ -158,7 +160,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
 
   _voxel_grid = std::make_unique<volume_grid::SpatioTemporalVoxelGrid>(
     node->get_clock(), _voxel_size, static_cast<double>(default_value_), _decay_model,
-    _voxel_decay, _publish_voxels);
+    _min_age_outside_frustum, _voxel_decay, _publish_voxels);
 
   matchSize();