diff --git a/cfg/SpatioTemporalVoxelLayer.cfg b/cfg/SpatioTemporalVoxelLayer.cfg index d9bc1458..d9cb9a8a 100755 --- a/cfg/SpatioTemporalVoxelLayer.cfg +++ b/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("min_age_outside_frustum", double_t, 1, "the minimum age that a voxel should have to be allow to live outside frustrums", 0.0, 0.0, 100.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) diff --git a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp index e4deef4d..95ec688f 100644 --- a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp +++ b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp @@ -124,7 +124,7 @@ class SpatioTemporalVoxelGrid typedef openvdb::math::Ray::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& min_age_outside_frustum, 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, _min_age_outside_frustum, _voxel_decay; bool _pub_voxels; std::vector* _grid_points; std::unordered_map* _cost_map; diff --git a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp index 77ae8bba..3aca104f 100644 --- a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp +++ b/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, _min_age_outside_frustum, _voxel_decay; int _combination_method, _mark_threshold; volume_grid::GlobalDecayModel _decay_model; bool _update_footprint_enabled, _enabled; diff --git a/src/spatio_temporal_voxel_grid.cpp b/src/spatio_temporal_voxel_grid.cpp index 4a67d2c0..e3707d5d 100644 --- a/src/spatio_temporal_voxel_grid.cpp +++ b/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& min_age_outside_frustum, \ const double& voxel_decay, const bool& pub_voxels) : _background_value(background_value), \ _voxel_size(voxel_size), \ _decay_model(decay_model), \ + _min_age_outside_frustum(min_age_outside_frustum), \ _voxel_decay(voxel_decay), \ _pub_voxels(pub_voxels), \ _grid_points(new std::vector), \ @@ -218,10 +220,15 @@ 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 if(!this->ClearGridPoint(pt_index)) diff --git a/src/spatio_temporal_voxel_layer.cpp b/src/spatio_temporal_voxel_layer.cpp index a8f99a85..adac6c9e 100644 --- a/src/spatio_temporal_voxel_layer.cpp +++ b/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(decay_model_int); + // min_age_outside_frustum param + nh.param("min_age_outside_frustum", _min_age_outside_frustum, 0.0); // 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, \ + _min_age_outside_frustum, \ _voxel_decay, \ _publish_voxels); matchSize(); @@ -620,6 +623,7 @@ void SpatioTemporalVoxelLayer::DynamicReconfigureCallback( \ costmap_2d::NO_INFORMATION : costmap_2d::FREE_SPACE; default_value_ = default_value; _voxel_size = config.voxel_size; + _min_age_outside_frustum = config.min_age_outside_frustum; _voxel_decay = config.voxel_decay; _decay_model = static_cast(config.decay_model); _publish_voxels = config.publish_voxel_map; @@ -627,7 +631,7 @@ void SpatioTemporalVoxelLayer::DynamicReconfigureCallback( \ delete _voxel_grid; _voxel_grid = new volume_grid::SpatioTemporalVoxelGrid(_voxel_size, \ static_cast(default_value_), _decay_model, \ - _voxel_decay, _publish_voxels); + _min_age_outside_frustum, _voxel_decay, _publish_voxels); } }