From 7c06d5410ed14cd54e72af967f663fd4e63e73fe Mon Sep 17 00:00:00 2001 From: Tiago Silva Date: Wed, 26 May 2021 19:46:05 +0100 Subject: [PATCH] cherry-picked melodic fix #200 (#201) --- .../spatio_temporal_voxel_grid.hpp | 7 ++-- .../spatio_temporal_voxel_layer.hpp | 3 +- src/spatio_temporal_voxel_grid.cpp | 32 +++++++++++++------ src/spatio_temporal_voxel_layer.cpp | 18 ++++++++--- 4 files changed, 44 insertions(+), 16 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 e4deef4d..4ec6ad96 100644 --- a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp +++ b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp @@ -48,6 +48,7 @@ // STL #include #include +#include #include #include #include @@ -131,7 +132,8 @@ 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(sensor_msgs::PointCloud2::Ptr& pc2); @@ -158,7 +160,8 @@ 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 77ae8bba..917d858c 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 costmap_2d::CostmapLayer void ObservationsResetAfterReading() const; // Functions to interact with maps - void UpdateROSCostmap(double* min_x, double* min_y, double* max_x, double* max_y); + void UpdateROSCostmap(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); void ResetGrid(void); diff --git a/src/spatio_temporal_voxel_grid.cpp b/src/spatio_temporal_voxel_grid.cpp index 4a67d2c0..032123d8 100644 --- a/src/spatio_temporal_voxel_grid.cpp +++ b/src/spatio_temporal_voxel_grid.cpp @@ -98,8 +98,9 @@ void SpatioTemporalVoxelGrid::InitializeGrid(void) } /*****************************************************************************/ -void SpatioTemporalVoxelGrid::ClearFrustums(const \ - std::vector& clearing_readings) +void SpatioTemporalVoxelGrid::ClearFrustums( \ + const std::vector& clearing_readings, \ + std::unordered_set& cleared_cells) /*****************************************************************************/ { boost::unique_lock lock(_grid_lock); @@ -117,7 +118,7 @@ void SpatioTemporalVoxelGrid::ClearFrustums(const \ if(clearing_readings.size() == 0) { - TemporalClearAndGenerateCostmap(obs_frustums); + TemporalClearAndGenerateCostmap(obs_frustums, cleared_cells); return; } @@ -156,13 +157,14 @@ void SpatioTemporalVoxelGrid::ClearFrustums(const \ frustum->TransformModel(); obs_frustums.emplace_back(frustum, it->_decay_acceleration); } - TemporalClearAndGenerateCostmap(obs_frustums); + TemporalClearAndGenerateCostmap(obs_frustums, cleared_cells); return; } /*****************************************************************************/ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \ - std::vector& frustums) + std::vector& frustums, \ + std::unordered_set& cleared_cells) /*****************************************************************************/ { // sample time once for all clearing readings @@ -173,9 +175,11 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \ for (cit_grid; 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( \ @@ -188,7 +192,7 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \ continue; } - if ( frustum_it->frustum->IsInside(this->IndexToWorld(pt_index)) ) + if ( frustum_it->frustum->IsInside(pose_world) ) { frustum_cycle = true; @@ -200,10 +204,12 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \ 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 { @@ -224,15 +230,23 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \ 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 a8f99a85..ad3dcf52 100644 --- a/src/spatio_temporal_voxel_layer.cpp +++ b/src/spatio_temporal_voxel_layer.cpp @@ -679,8 +679,10 @@ void SpatioTemporalVoxelLayer::updateCosts( \ } /*****************************************************************************/ -void SpatioTemporalVoxelLayer::UpdateROSCostmap(double* min_x, double* min_y, \ - double* max_x, double* max_y) +void SpatioTemporalVoxelLayer::UpdateROSCostmap( \ + 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_ @@ -698,6 +700,12 @@ void SpatioTemporalVoxelLayer::UpdateROSCostmap(double* min_x, double* min_y, \ 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); + } } /*****************************************************************************/ @@ -733,10 +741,12 @@ void SpatioTemporalVoxelLayer::updateBounds( \ ObservationsResetAfterReading(); current_ = current; + std::unordered_set cleared_cells; + // navigation mode: clear observations, mapping mode: save maps and publish if (!_mapping_mode) { - _voxel_grid->ClearFrustums(clearing_observations); + _voxel_grid->ClearFrustums(clearing_observations, cleared_cells); } else if (ros::Time::now() - _last_map_save_time > _map_save_duration) { @@ -757,7 +767,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)