LayeredCostmap isCurrent() does not consider whether the plugins are enabled or not

[milidam]

Bug report

Required Info:

• Operating System:
  • Ubuntu 22.04
• ROS2 Version:
  • Humble (apt binaries)
• Version or commit hash:
  • 1.1.3-1jammy.20221207.081347
• DDS implementation:
  • CycloneDDS

Steps to reproduce issue

• configure nav2 with the InflationLayer and ObstacleLayer plugins configured for the local costmap; the InflationLayer is enabled, but the ObstacleLayer is disabled
• implement a "go to pose" behavior tree, executed so that a follow_path is executed after having cleared the local costmap (e.g. after the execution of a recovery)

Expected behavior

nav2's controller server properly follows the computed path.

Actual behavior

nav2's controller server gets stuck in a endless loop after Received a goal, begin computing control effort..

Additional information

The endless loop is here.

After clearing the local costmap, calling the reset() of the different layered_costmap plugins, the current_ attribute of the ObstacleLayer plugin is set to False, so that the isCurrent() of the plugins returns False, and the LayeredCostmap's isCurrent() returns False as well, even is the plugin is disabled.
ObstacleLayer's current_ is never set back to True, as normally done in the the updateCosts(), that is "bypassed" when the plugin is not enabled.

Feature request

Feature description

The isCurrent() of the LayeredCostmap class should take into account whether the plugins are enabled or not.

Implementation considerations

Add a isEnabled() method to the Layer class, and modify

current_ = current_ && (*plugin)->isCurrent();

by

current_ = current_ && ((*plugin)->isCurrent() || !(*plugin)->isEnabled();

in LayeredCostmap's isCurrent() implementation.

Or simply replace Layer's isCurrent() implementation by

    return current_ || !enabled_;

[SteveMacenski]

in LayeredCostmap's isCurrent() implementation

That sounds good to me and makes sense! Can you open the PR?