add outside duration threshold

Author: Guillaume

cfg/SpatioTemporalVoxelLayer.cfg

@@ -22,6 +22,7 @@ gen.add("mark_threshold", double_t, 0, "Over this threshold, the measurement wil
 gen.add("update_footprint_enabled", bool_t, 0, "Cleans where the robot's footprint is", True)
 gen.add("track_unknown_space", bool_t, 1, "If true, marks will be UNKNOWN (255) otherwise, FREE (0)", True)
 gen.add("decay_model", int_t, 1, "Decay models", 0, edit_method=decay_model_enum)
+gen.add("outside_duration_threshold", double_t, 1, "Seconds", 0.0, 0.0, 1.0)
 gen.add("voxel_decay", double_t, 1, "Seconds if linear, e^n if exponential", 10.0, -1, 200.0)
 gen.add("mapping_mode", bool_t, 0, "Mapping mode", False)
 gen.add("map_save_duration", double_t, 0, "f mapping, how often to save a map for safety", 60.0, 1.0, 2000.0)

include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp

@@ -124,7 +124,7 @@ class SpatioTemporalVoxelGrid
   typedef openvdb::math::Ray<openvdb::Real>::Vec3T Vec3Type;
 
   SpatioTemporalVoxelGrid(const float& voxel_size, const double& background_value,
-                          const int& decay_model, const double& voxel_decay,
+                          const int& decay_model, const double& outside_duration_threshold, const double& voxel_decay,
                           const bool& pub_voxels);
   ~SpatioTemporalVoxelGrid(void);
 
@@ -169,7 +169,7 @@ class SpatioTemporalVoxelGrid
 
   mutable openvdb::DoubleGrid::Ptr _grid;
   int                             _decay_model;
-  double                          _background_value, _voxel_size, _voxel_decay;
+  double                          _background_value, _voxel_size, _outside_duration_threshold, _voxel_decay;
   bool                            _pub_voxels;
   std::vector<geometry_msgs::Point32>*   _grid_points;
   std::unordered_map<occupany_cell, uint>* _cost_map;

include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp

@@ -161,7 +161,7 @@ class SpatioTemporalVoxelLayer : public costmap_2d::CostmapLayer
   ros::Duration                        _map_save_duration;
   ros::Time                            _last_map_save_time;
   std::string                          _global_frame;
-  double                               _voxel_size, _voxel_decay;
+  double                               _voxel_size, _outside_duration_threshold, _voxel_decay;
   int                                  _combination_method, _mark_threshold;
   volume_grid::GlobalDecayModel        _decay_model;
   bool                                 _update_footprint_enabled, _enabled;

src/spatio_temporal_voxel_grid.cpp

@@ -43,10 +43,12 @@ namespace volume_grid
 /*****************************************************************************/
 SpatioTemporalVoxelGrid::SpatioTemporalVoxelGrid(const float& voxel_size, \
                    const double& background_value, const int& decay_model,\
+                   const double& outside_duration_threshold,              \
                    const double& voxel_decay, const bool& pub_voxels) :
                    _background_value(background_value),                   \
                    _voxel_size(voxel_size),                               \
                    _decay_model(decay_model),                             \
+                   _outside_duration_threshold(outside_duration_threshold), \
                    _voxel_decay(voxel_decay),                             \
                    _pub_voxels(pub_voxels),                               \
                    _grid_points(new std::vector<geometry_msgs::Point32>),   \
@@ -168,6 +170,8 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap(                \
   // sample time once for all clearing readings
   const double cur_time = ros::WallTime::now().toSec();
 
+
+
   // check each point in the grid for inclusion in a frustum
   openvdb::DoubleGrid::ValueOnCIter cit_grid = _grid->cbeginValueOn();
   for (cit_grid; cit_grid.test(); ++cit_grid)
@@ -218,10 +222,13 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap(                \
       }
     }
 
-    // if not inside any, check against nominal decay model
+    // if not inside any, check against nominal decay model, also if
+    // too young, it 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 < _outside_duration_threshold)
       {
         // expired by temporal clearing
         if(!this->ClearGridPoint(pt_index))

src/spatio_temporal_voxel_layer.cpp

@@ -99,6 +99,8 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
                                   layered_costmap_->isTrackingUnknown());
   nh.param("decay_model", decay_model_int, 0);
   _decay_model = static_cast<volume_grid::GlobalDecayModel>(decay_model_int);
+  // outside_duration_threshold param
+  nh.param("outside_duration_threshold", _outside_duration_threshold, 0.05);
   // decay param
   nh.param("voxel_decay", _voxel_decay, -1.);
   // whether to map or navigate
@@ -130,6 +132,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
   _voxel_grid = new volume_grid::SpatioTemporalVoxelGrid(_voxel_size, \
                                                         (double)default_value_, \
                                                         _decay_model, \
+                                                        _outside_duration_threshold, \
                                                         _voxel_decay, \
                                                         _publish_voxels);
   matchSize();
@@ -620,14 +623,15 @@ void SpatioTemporalVoxelLayer::DynamicReconfigureCallback( \
                             costmap_2d::NO_INFORMATION : costmap_2d::FREE_SPACE;
     default_value_ = default_value;
     _voxel_size = config.voxel_size;
+    _outside_duration_threshold = config.outside_duration_threshold;
     _voxel_decay = config.voxel_decay;
     _decay_model = static_cast<volume_grid::GlobalDecayModel>(config.decay_model);
     _publish_voxels = config.publish_voxel_map;
 
     delete _voxel_grid;
     _voxel_grid = new volume_grid::SpatioTemporalVoxelGrid(_voxel_size, \
       static_cast<double>(default_value_), _decay_model, \
-      _voxel_decay, _publish_voxels);
+      _outside_duration_threshold, _voxel_decay, _publish_voxels);
   }
 }