From e297fec1739ee0b7a38bbe56082b97697f6d360d Mon Sep 17 00:00:00 2001 From: Tiago Silva Date: Wed, 26 May 2021 19:46:38 +0100 Subject: [PATCH] cherry-picked melodic fix #200 (#202) --- .../spatio_temporal_voxel_grid.hpp | 9 ++++-- .../spatio_temporal_voxel_layer.hpp | 3 +- src/spatio_temporal_voxel_grid.cpp | 30 ++++++++++++++----- src/spatio_temporal_voxel_layer.cpp | 15 ++++++++-- 4 files changed, 43 insertions(+), 14 deletions(-) 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 ed9c1632..4f170bf2 100644 --- a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp +++ b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp @@ -43,6 +43,7 @@ // STL #include #include +#include #include #include #include @@ -137,7 +138,9 @@ class SpatioTemporalVoxelGrid // Core making and clearing functions void Mark(const std::vector & marking_observations); void operator()(const observation::MeasurementReading & obs) const; - void ClearFrustums(const std::vector & clearing_observations); + void ClearFrustums( + const std::vector & clearing_observations, + std::unordered_set & cleared_cells); // Get the pointcloud of the underlying occupancy grid void GetOccupancyPointCloud(std::unique_ptr & pc2); @@ -164,7 +167,9 @@ class SpatioTemporalVoxelGrid double GetTemporalClearingDuration(const double & time_delta); double GetFrustumAcceleration( const double & time_delta, const double & acceleration_factor); - void TemporalClearAndGenerateCostmap(std::vector & frustums); + void TemporalClearAndGenerateCostmap( + std::vector & frustums, + std::unordered_set & cleared_cells); // Populate the costmap ROS api and pointcloud with a marked point void PopulateCostmapAndPointcloud(const openvdb::Coord & pt); 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 79d15f17..e5218a80 100644 --- a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp +++ b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp @@ -114,7 +114,8 @@ class SpatioTemporalVoxelLayer : public nav2_costmap_2d::CostmapLayer // Functions to interact with maps void UpdateROSCostmap( - double * min_x, double * min_y, double * max_x, double * max_y); + double * min_x, double * min_y, double * max_x, double * max_y, + std::unordered_set & cleared_cells); bool updateFootprint( double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y, double * max_x, double * max_y); diff --git a/src/spatio_temporal_voxel_grid.cpp b/src/spatio_temporal_voxel_grid.cpp index 8de00ec1..65ab0033 100644 --- a/src/spatio_temporal_voxel_grid.cpp +++ b/src/spatio_temporal_voxel_grid.cpp @@ -94,7 +94,8 @@ void SpatioTemporalVoxelGrid::InitializeGrid(void) /*****************************************************************************/ void SpatioTemporalVoxelGrid::ClearFrustums( - const std::vector & clearing_readings) + const std::vector & clearing_readings, + std::unordered_set & cleared_cells) /*****************************************************************************/ { boost::unique_lock lock(_grid_lock); @@ -110,7 +111,7 @@ void SpatioTemporalVoxelGrid::ClearFrustums( std::vector obs_frustums; if (clearing_readings.size() == 0) { - TemporalClearAndGenerateCostmap(obs_frustums); + TemporalClearAndGenerateCostmap(obs_frustums, cleared_cells); return; } @@ -138,12 +139,13 @@ void SpatioTemporalVoxelGrid::ClearFrustums( frustum->TransformModel(); obs_frustums.emplace_back(frustum, it->_decay_acceleration); } - TemporalClearAndGenerateCostmap(obs_frustums); + TemporalClearAndGenerateCostmap(obs_frustums, cleared_cells); } /*****************************************************************************/ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( - std::vector & frustums) + std::vector & frustums, + std::unordered_set & cleared_cells) /*****************************************************************************/ { // sample time once for all clearing readings @@ -153,9 +155,11 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( openvdb::DoubleGrid::ValueOnCIter cit_grid = _grid->cbeginValueOn(); for (; cit_grid.test(); ++cit_grid) { const openvdb::Coord pt_index(cit_grid.getCoord()); + const openvdb::Vec3d pose_world = this->IndexToWorld(pt_index); std::vector::iterator frustum_it = frustums.begin(); bool frustum_cycle = false; + bool cleared_point = false; const double time_since_marking = cur_time - cit_grid.getValue(); const double base_duration_to_decay = GetTemporalClearingDuration( @@ -166,7 +170,7 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( continue; } - if (frustum_it->frustum->IsInside(this->IndexToWorld(pt_index)) ) { + if (frustum_it->frustum->IsInside(pose_world) ) { frustum_cycle = true; const double frustum_acceleration = GetFrustumAcceleration( @@ -176,9 +180,11 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( frustum_acceleration; if (time_until_decay < 0.) { // expired by acceleration + cleared_point = true; if (!this->ClearGridPoint(pt_index)) { std::cout << "Failed to clear point." << std::endl; } + break; } else { const double updated_mark = cit_grid.getValue() - frustum_acceleration; @@ -194,14 +200,22 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( if (!frustum_cycle) { if (base_duration_to_decay < 0.) { // expired by temporal clearing + cleared_point = true; if (!this->ClearGridPoint(pt_index)) { std::cout << "Failed to clear point." << std::endl; } - continue; } } - // if here, we can add to costmap and PC2 - PopulateCostmapAndPointcloud(pt_index); + + if (cleared_point) + { + cleared_cells.insert(occupany_cell(pose_world[0], pose_world[1])); + } + else + { + // if here, we can add to costmap and PC2 + PopulateCostmapAndPointcloud(pt_index); + } } } diff --git a/src/spatio_temporal_voxel_layer.cpp b/src/spatio_temporal_voxel_layer.cpp index adbc6fd8..18c7ec5b 100644 --- a/src/spatio_temporal_voxel_layer.cpp +++ b/src/spatio_temporal_voxel_layer.cpp @@ -640,7 +640,8 @@ void SpatioTemporalVoxelLayer::updateCosts( /*****************************************************************************/ void SpatioTemporalVoxelLayer::UpdateROSCostmap( - double * min_x, double * min_y, double * max_x, double * max_y) + double * min_x, double * min_y, double * max_x, double * max_y, + std::unordered_set & cleared_cells) /*****************************************************************************/ { // grabs map of occupied cells from grid and adds to costmap_ @@ -658,6 +659,12 @@ void SpatioTemporalVoxelLayer::UpdateROSCostmap( touch(it->first.x, it->first.y, min_x, min_y, max_x, max_y); } } + + std::unordered_set::iterator cell; + for (cell = cleared_cells.begin(); cell != cleared_cells.end(); ++cell) + { + touch(cell->x, cell->y, min_x, min_y, max_x, max_y); + } } /*****************************************************************************/ @@ -692,13 +699,15 @@ void SpatioTemporalVoxelLayer::updateBounds( ObservationsResetAfterReading(); current_ = current; + std::unordered_set cleared_cells; + // navigation mode: clear observations, mapping mode: save maps and publish bool should_save = false; if (_map_save_duration) { should_save = node_->now() - _last_map_save_time > *_map_save_duration; } if (!_mapping_mode) { - _voxel_grid->ClearFrustums(clearing_observations); + _voxel_grid->ClearFrustums(clearing_observations, cleared_cells); } else if (should_save) { _last_map_save_time = node_->now(); time_t rawtime; @@ -720,7 +729,7 @@ void SpatioTemporalVoxelLayer::updateBounds( _voxel_grid->Mark(marking_observations); // update the ROS Layered Costmap - UpdateROSCostmap(min_x, min_y, max_x, max_y); + UpdateROSCostmap(min_x, min_y, max_x, max_y, cleared_cells); // publish point cloud in navigation mode if (_publish_voxels && !_mapping_mode) {