diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp index 08fed09145a..4acf05bd558 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -40,6 +40,7 @@ #include #include +#include #include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/layer.hpp" @@ -77,7 +78,7 @@ class InflationLayer : public Layer public: InflationLayer(); - ~InflationLayer() override = default; + ~InflationLayer(); void onInitialize() override; void updateBounds( @@ -115,6 +116,13 @@ class InflationLayer : public Layer return cost; } + // Provide a typedef to ease future code maintenance + typedef std::recursive_mutex mutex_t; + mutex_t * getMutex() + { + return access_; + } + protected: void onFootprintChanged() override; @@ -184,6 +192,7 @@ class InflationLayer : public Layer // Indicates that the entire costmap should be reinflated next time around. bool need_reinflation_; + mutex_t * access_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index df8405a3c91..4316da9ecb9 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -71,6 +71,12 @@ InflationLayer::InflationLayer() last_max_x_(std::numeric_limits::max()), last_max_y_(std::numeric_limits::max()) { + access_ = new mutex_t(); +} + +InflationLayer::~InflationLayer() +{ + delete access_; } void @@ -160,6 +166,7 @@ InflationLayer::updateCosts( int max_i, int max_j) { + std::lock_guard guard(*getMutex()); if (!enabled_ || (cell_inflation_radius_ == 0)) { return; } @@ -305,6 +312,7 @@ InflationLayer::enqueue( void InflationLayer::computeCaches() { + std::lock_guard guard(*getMutex()); if (cell_inflation_radius_ == 0) { return; }