From f4f429935d748400932e5597b616efd75e33d120 Mon Sep 17 00:00:00 2001 From: siferati Date: Sun, 30 May 2021 16:25:33 +0100 Subject: [PATCH] cherry-picked melodic fix #200 --- .../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 0bba19a5..f45d24c5 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 @@ -135,7 +136,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); @@ -162,7 +165,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 729bf3d3..7db83938 100644 --- a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp +++ b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp @@ -117,7 +117,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 32815aad..41fe6b76 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; } @@ -139,12 +140,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 @@ -154,9 +156,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( @@ -167,7 +171,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( @@ -177,9 +181,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; @@ -195,14 +201,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 003208fd..1e022c48 100644 --- a/src/spatio_temporal_voxel_layer.cpp +++ b/src/spatio_temporal_voxel_layer.cpp @@ -670,7 +670,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_ @@ -688,6 +689,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); + } } /*****************************************************************************/ @@ -723,6 +730,8 @@ 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; auto node = node_.lock(); @@ -730,7 +739,7 @@ void SpatioTemporalVoxelLayer::updateBounds( 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; @@ -752,7 +761,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) {