cherry-picked melodic fix #200 (#201)

Author: Tiago Silva

include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp

@@ -48,6 +48,7 @@
 // STL
 #include <math.h>
 #include <unordered_map>
+#include <unordered_set>
 #include <ctime>
 #include <iostream>
 #include <utility>
@@ -131,7 +132,8 @@ 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(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<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

@@ -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<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);
   void ResetGrid(void);

src/spatio_temporal_voxel_grid.cpp

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

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<volume_grid::occupany_cell>& 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<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);
+  }
 }
 
 /*****************************************************************************/
@@ -733,10 +741,12 @@ 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
   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)