Grid Points and Cost Map not updating when Grid is Empty

[Xavi]

Hi,

We are experiencing an issue where the costmap and the voxel map are not updating after resetting the layer through the move_base clear_costmap service. The issue happens in melodic (both in the last release 1.3.5 and in melodic-devel).

The case is the following: the robot sees an obstacle and it gets registered in the grid. The robot rotates in place up to a point where the obstacle is out of the FOV of the depth camera but it remains in the costmap. In the new position, the robot does not see any obstacle so the marking pointcloud is empty. When I call the move_base clear_costmap service, the openvdb::DoubleGrid::Ptr gets cleared through the ResetGrid (L412) function and the function IsGridEmpty (L478) returns true. But, since the input marking pointcloud is empty because we see no obstacles, when we update the SpatioTemporalVoxelGrid through the ClearFrustums (L101) function, the check of IsGridEmpty returns true and we do not update neither _grid_points nor _cost_map objects.

Code snippet:

/*****************************************************************************/
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);

  // accelerate the decay of voxels interior to the frustum
  if(this->IsGridEmpty())
  {
    return;
  }

  _grid_points->clear();
  _cost_map->clear();

My problem will be solved if the code was like this:

/*****************************************************************************/
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);

 // accelerate the decay of voxels interior to the frustum
 if(this->IsGridEmpty())
 {
    _grid_points->clear();
    _cost_map->clear();
   return;
 }

 _grid_points->clear();
 _cost_map->clear();

Not sure if this is the intended behavior, if it might be a bug or I am missing something.
I can also provide videos of the detailed behavior and/or config files.

I issued a PR on this: #262

Thanks a lot in advance.

[Steve Macenski]

Merged