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,8 +99,7 @@ class InflationLayer : public Layer
 
   virtual void reset()
   {
-    undeclareAllParameters();
-    onInitialize();
+    matchSize();
   }
 
   /** @brief  Given a distance, compute a cost.

nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp

@@ -67,7 +67,7 @@ class Layer  // TODO(mjeronimo): public nav2_util::LifecycleHelperInterface
     rclcpp::Node::SharedPtr rclcpp_node);
   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

@@ -588,13 +588,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()
 {
@@ -620,11 +616,19 @@ ObstacleLayer::updateRaytraceBounds(
 void
 ObstacleLayer::reset()
 {
-  deactivate();
   resetMaps();
+  resetBuffersLastUpdated();
   current_ = true;
-  activate();
-  undeclareAllParameters();
+}
+
+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

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

nav2_costmap_2d/plugins/voxel_layer.cpp

@@ -110,17 +110,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();
-  undeclareAllParameters();
-  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();
 }
 

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -245,7 +245,6 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
 {
   RCLCPP_INFO(get_logger(), "Cleaning up");
 
-  resetLayers();
   delete layered_costmap_;
   layered_costmap_ = nullptr;
 

nav2_costmap_2d/test/integration/obstacle_tests.cpp

@@ -357,20 +357,9 @@ TEST_F(TestNode, testRepeatedResets) {
       return plugin->hasParameter(layer_param);
     }));
 
-  // Reset all layers. This will un-declare all params and might re-declare internal ones
-  // Should run without throwing exceptions
+  // Reset all layers. Parameters should be declared if not declared, otherwise skipped.
   ASSERT_NO_THROW(
     for_each(begin(*plugins), end(*plugins), [](const auto & plugin) {
       plugin->reset();
     }));
-
-  // Check for node-level param
-  ASSERT_TRUE(node_->has_parameter(node_dummy.first));
-
-  // Layer-level parameters shouldn't be found
-  ASSERT_TRUE(
-    none_of(begin(*plugins), end(*plugins), [&layer_dummy](const auto & plugin) {
-      string layer_param = layer_dummy.first + "_" + plugin->getName();
-      return plugin->hasParameter(layer_param);
-    }));
 }