Add mutex lock into inflation layer to avoid thread issue in updating footprint (#1952)

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

Author: daisukes

nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp

@@ -40,6 +40,7 @@
 
 #include <map>
 #include <vector>
+#include <mutex>
 
 #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

nav2_costmap_2d/plugins/inflation_layer.cpp

@@ -71,6 +71,12 @@ InflationLayer::InflationLayer()
   last_max_x_(std::numeric_limits<double>::max()),
   last_max_y_(std::numeric_limits<double>::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<Costmap2D::mutex_t> guard(*getMutex());
   if (!enabled_ || (cell_inflation_radius_ == 0)) {
     return;
   }
@@ -305,6 +312,7 @@ InflationLayer::enqueue(
 void
 InflationLayer::computeCaches()
 {
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
   if (cell_inflation_radius_ == 0) {
     return;
   }