Do only consider enabled costmap plugins/filters in the isCurrent() m…

…ethod of a LayeredCostmap (ros-navigation#3356)

* Do only consider enabled costmap plugins/filters in the isCurrent() method of a LayeredCostmap

When clearing entirely a costmap (see the `clear_entirely_*` service),
its layers are reset. This sets their `current_` attribute to False,
until they are updated (`updateCosts()`). However, for disabled
plugins/filters, the `updateCosts()` is bypassed so that the `current_`
attribute is never reset to True, and the costmap never becomes
"current" again; because of that the controller_server used to get stuck
in an endless loop (see `while (!costmap_ros_->isCurrent())`
in `ControllerServer::computeControl()`).

This patch fixes that by adding a condition for not considering disabled
plugins/filters in the `LayeredCostmap::isCurrent()`.

* Add forgotten Layer::isEnabled() definition

nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp

@@ -139,6 +139,12 @@ class Layer
     return current_;
   }
 
+  /**@brief Gets whether the layer is enabled. */
+  bool isEnabled() const
+  {
+    return enabled_;
+  }
+
   /** @brief Convenience function for layered_costmap_->getFootprint(). */
   const std::vector<geometry_msgs::msg::Point> & getFootprint() const;
 

nav2_costmap_2d/src/layered_costmap.cpp

@@ -256,12 +256,12 @@ bool LayeredCostmap::isCurrent()
   for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin();
     plugin != plugins_.end(); ++plugin)
   {
-    current_ = current_ && (*plugin)->isCurrent();
+    current_ = current_ && ((*plugin)->isCurrent() || !(*plugin)->isEnabled());
   }
   for (vector<std::shared_ptr<Layer>>::iterator filter = filters_.begin();
     filter != filters_.end(); ++filter)
   {
-    current_ = current_ && (*filter)->isCurrent();
+    current_ = current_ && ((*filter)->isCurrent() || !(*filter)->isEnabled());
   }
   return current_;
 }