fix for #1363 - use clearMap instead of reset to clear costmaps (#1412)

* -fix for #1363. -created clearMap function for each layer. -use clearMap instead of reset to clear the costmaps

* -function name change -infilation layer clearMap change -make resetMap a pure virtual function

* Changing clearMaps back to reset

* Fix uncrustify error

* Fixing lifecycle transitions and tests now that reset doesn't act as a cleanup function

* Updating this based on the approach taken in PR #1431

* Moving voxel_grid reset to the resetMaps function.

* Missed adding resetMaps back to header.

* Adding some comments to help navigate the inheritance hierarchy

Author: mlherd

nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp

@@ -99,7 +99,7 @@ class InflationLayer : public Layer
 
   virtual void reset()
   {
-    onInitialize();
+    matchSize();
   }
 
   /** @brief  Given a distance, compute a cost.

nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp

@@ -69,7 +69,7 @@ class Layer  // TODO(mjeronimo): public nav2_util::LifecycleHelperInterface
     std::shared_ptr<nav2_util::ParameterEventsSubscriber> param_subscriber = nullptr);
   virtual void deactivate() {} /** @brief Stop publishers. */
   virtual void activate() {}   /** @brief Restart publishers if they've been stopped. */
-  virtual void reset() {}
+  virtual void reset() = 0;
 
   /**
    * @brief This is called by the LayeredCostmap to poll this plugin as to how

nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp

@@ -83,6 +83,10 @@ class ObstacleLayer : public CostmapLayer
   virtual void activate();
   virtual void deactivate();
   virtual void reset();
+  /**
+   * @brief triggers the update of observations buffer
+   */
+  void resetBuffersLastUpdated();
 
   /**
    * @brief  A callback to handle buffering LaserScan messages

nav2_costmap_2d/plugins/obstacle_layer.cpp

@@ -619,13 +619,9 @@ ObstacleLayer::activate()
       observation_subscribers_[i]->subscribe();
     }
   }
-
-  for (unsigned int i = 0; i < observation_buffers_.size(); ++i) {
-    if (observation_buffers_[i]) {
-      observation_buffers_[i]->resetLastUpdated();
-    }
-  }
+  resetBuffersLastUpdated();
 }
+
 void
 ObstacleLayer::deactivate()
 {
@@ -651,10 +647,19 @@ ObstacleLayer::updateRaytraceBounds(
 void
 ObstacleLayer::reset()
 {
-  deactivate();
   resetMaps();
+  resetBuffersLastUpdated();
   current_ = true;
-  activate();
+}
+
+void
+ObstacleLayer::resetBuffersLastUpdated()
+{
+  for (unsigned int i = 0; i < observation_buffers_.size(); ++i) {
+    if (observation_buffers_[i]) {
+      observation_buffers_[i]->resetLastUpdated();
+    }
+  }
 }
 
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/plugins/static_layer.cpp

@@ -122,10 +122,7 @@ StaticLayer::deactivate()
 void
 StaticLayer::reset()
 {
-  map_sub_.reset();
-  map_update_sub_.reset();
-
-  onInitialize();
+  has_updated_data_ = true;
 }
 
 void

nav2_costmap_2d/plugins/voxel_layer.cpp

@@ -167,16 +167,18 @@ void VoxelLayer::matchSize()
 
 void VoxelLayer::reset()
 {
-  voxel_pub_->on_deactivate();
-  deactivate();
+  // Call the base class method before adding our own functionality
+  ObstacleLayer::reset();
   resetMaps();
-  activate();
-  voxel_pub_->on_activate();
 }
 
 void VoxelLayer::resetMaps()
 {
-  Costmap2D::resetMaps();
+  // Call the base class method before adding our own functionality
+  // Note: at the time this was written, ObstacleLayer doesn't implement
+  // resetMaps so this goes to the next layer down Costmap2DLayer which also
+  // doesn't implement this, so it actually goes all the way to Costmap2D
+  ObstacleLayer::resetMaps();
   voxel_grid_.reset();
 }