Skip to content

Commit

Permalink
Do only consider enabled costmap plugins/filters in the isCurrent() m…
Browse files Browse the repository at this point in the history
…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
  • Loading branch information
milidam authored and Andrew Lycas committed Feb 23, 2023
1 parent 3278e72 commit ee36ef4
Show file tree
Hide file tree
Showing 2 changed files with 8 additions and 2 deletions.
6 changes: 6 additions & 0 deletions nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
4 changes: 2 additions & 2 deletions nav2_costmap_2d/src/layered_costmap.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
}
Expand Down

0 comments on commit ee36ef4

Please sign in to comment.