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 93c04af..52b6b84 100644 --- a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp +++ b/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> _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 324bf6e..9f177aa 100644 --- a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp +++ b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp @@ -181,7 +181,7 @@ class SpatioTemporalVoxelLayer : public nav2_costmap_2d::CostmapLayer std::unique_ptr _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; diff --git a/src/spatio_temporal_voxel_grid.cpp b/src/spatio_temporal_voxel_grid.cpp index ca89d95..47bd5f9 100644 --- a/src/spatio_temporal_voxel_grid.cpp +++ b/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>()), _cost_map(new std::unordered_map) @@ -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)) { diff --git a/src/spatio_temporal_voxel_layer.cpp b/src/spatio_temporal_voxel_layer.cpp index 6b86dea..82a5e7c 100644 --- a/src/spatio_temporal_voxel_layer.cpp +++ b/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(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( node->get_clock(), _voxel_size, static_cast(default_value_), _decay_model, - _voxel_decay, _publish_voxels); + _min_age_outside_frustum, _voxel_decay, _publish_voxels); matchSize();