cherry-picked melodic fix #200 (#204)

Author: Tiago Silva

include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp

@@ -43,6 +43,7 @@
 // STL
 #include <math.h>
 #include <unordered_map>
+#include <unordered_set>
 #include <ctime>
 #include <iostream>
 #include <utility>
@@ -135,7 +136,9 @@ class SpatioTemporalVoxelGrid
   // Core making and clearing functions
   void Mark(const std::vector<observation::MeasurementReading> & marking_observations);
   void operator()(const observation::MeasurementReading & obs) const;
-  void ClearFrustums(const std::vector<observation::MeasurementReading> & clearing_observations);
+  void ClearFrustums(
+    const std::vector<observation::MeasurementReading> & clearing_observations,
+    std::unordered_set<occupany_cell> & cleared_cells);
 
   // Get the pointcloud of the underlying occupancy grid
   void GetOccupancyPointCloud(std::unique_ptr<sensor_msgs::msg::PointCloud2> & 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<frustum_model> & frustums);
+  void TemporalClearAndGenerateCostmap(
+    std::vector<frustum_model> & frustums,
+    std::unordered_set<occupany_cell> & cleared_cells);
 
   // Populate the costmap ROS api and pointcloud with a marked point
   void PopulateCostmapAndPointcloud(const openvdb::Coord & pt);

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<volume_grid::occupany_cell> & 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);

src/spatio_temporal_voxel_grid.cpp

@@ -94,7 +94,8 @@ void SpatioTemporalVoxelGrid::InitializeGrid(void)
 
 /*****************************************************************************/
 void SpatioTemporalVoxelGrid::ClearFrustums(
-  const std::vector<observation::MeasurementReading> & clearing_readings)
+  const std::vector<observation::MeasurementReading> & clearing_readings,
+  std::unordered_set<occupany_cell> & cleared_cells)
 /*****************************************************************************/
 {
   boost::unique_lock<boost::mutex> lock(_grid_lock);
@@ -110,7 +111,7 @@ void SpatioTemporalVoxelGrid::ClearFrustums(
   std::vector<frustum_model> 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<frustum_model> & frustums)
+  std::vector<frustum_model> & frustums,
+  std::unordered_set<occupany_cell> & 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<frustum_model>::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);
+    }
   }
 }
 

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<volume_grid::occupany_cell> & 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<volume_grid::occupany_cell>::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,14 +730,16 @@ void SpatioTemporalVoxelLayer::updateBounds(
   ObservationsResetAfterReading();
   current_ = current;
 
+  std::unordered_set<volume_grid::occupany_cell> cleared_cells;
+
   // navigation mode: clear observations, mapping mode: save maps and publish
   bool should_save = false;
   auto node = node_.lock();
   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;
@@ -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) {