From 7ddaaf4b9b1ce9ac2ffbcef80684b8d21ff7bbb0 Mon Sep 17 00:00:00 2001
From: bpwilcox <bpwilcox@eng.ucsd.edu>
Date: Tue, 19 Nov 2019 17:24:02 -0800
Subject: [PATCH] add mutex lock for plugin param callbacks

---
 .../include/nav2_costmap_2d/inflation_layer.hpp          | 1 -
 nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp        | 2 ++
 nav2_costmap_2d/plugins/inflation_layer.cpp              | 2 ++
 nav2_costmap_2d/plugins/obstacle_layer.cpp               | 4 ++++
 nav2_costmap_2d/plugins/voxel_layer.cpp                  | 9 +++++++++
 5 files changed, 17 insertions(+), 1 deletion(-)

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 090e3c8715..49f21ebe44 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp
@@ -187,7 +187,6 @@ class InflationLayer : public Layer
 
   // Indicates that the entire costmap should be reinflated next time around.
   bool need_reinflation_;
-  std::recursive_mutex mutex_;
 };
 
 }  // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp
index 9819eb38d6..6cd9f3ac3d 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp
@@ -155,6 +155,8 @@ class Layer  // TODO(mjeronimo): public nav2_util::LifecycleHelperInterface
   // Names of the parameters declared on the ROS node
   std::unordered_set<std::string> local_params_;
 
+  std::recursive_mutex mutex_;
+
 private:
   std::vector<geometry_msgs::msg::Point> footprint_spec_;
 };
diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp
index 931de69539..10988cda84 100644
--- a/nav2_costmap_2d/plugins/inflation_layer.cpp
+++ b/nav2_costmap_2d/plugins/inflation_layer.cpp
@@ -101,12 +101,14 @@ InflationLayer::setParamCallbacks()
   if (param_subscriber_) {
     callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".enabled",
       [&](const rclcpp::Parameter & p) {
+        std::lock_guard<std::recursive_mutex> lock(mutex_);
         enabled_ = p.get_value<bool>();
         need_reinflation_ = true;
       }));
     callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
       ".inflate_unknown",
       [&](const rclcpp::Parameter & p) {
+        std::lock_guard<std::recursive_mutex> lock(mutex_);
         inflate_unknown_ = p.get_value<bool>();
         need_reinflation_ = true;
       }));
diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp
index 5f234a5289..3ab2e7da16 100644
--- a/nav2_costmap_2d/plugins/obstacle_layer.cpp
+++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp
@@ -252,21 +252,25 @@ ObstacleLayer::setParamCallbacks()
   if (param_subscriber_) {
     callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".enabled",
       [&](const rclcpp::Parameter & p) {
+        std::lock_guard<std::recursive_mutex> lock(mutex_);
         enabled_ = p.get_value<bool>();
       }));
     callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
       ".footprint_clearing_enabled",
       [&](const rclcpp::Parameter & p) {
+        std::lock_guard<std::recursive_mutex> lock(mutex_);
         footprint_clearing_enabled_ = p.get_value<bool>();
       }));
     callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
       ".max_obstacle_height",
       [&](const rclcpp::Parameter & p) {
+        std::lock_guard<std::recursive_mutex> lock(mutex_);
         max_obstacle_height_ = p.get_value<double>();
       }));
     callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
       ".combination_method",
       [&](const rclcpp::Parameter & p) {
+        std::lock_guard<std::recursive_mutex> lock(mutex_);
         combination_method_ = p.get_value<int>();
       }));
   }
diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp
index 1924e336f6..0b3afabf50 100644
--- a/nav2_costmap_2d/plugins/voxel_layer.cpp
+++ b/nav2_costmap_2d/plugins/voxel_layer.cpp
@@ -104,42 +104,51 @@ VoxelLayer::setParamCallbacks()
   if (param_subscriber_) {
     callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".enabled",
       [&](const rclcpp::Parameter & p) {
+        std::lock_guard<std::recursive_mutex> lock(mutex_);
         enabled_ = p.get_value<bool>();
       }));
     callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
       ".footprint_clearing_enabled",
       [&](const rclcpp::Parameter & p) {
+        std::lock_guard<std::recursive_mutex> lock(mutex_);
         footprint_clearing_enabled_ = p.get_value<bool>();
       }));
     callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
       ".max_obstacle_height",
       [&](const rclcpp::Parameter & p) {
+        std::lock_guard<std::recursive_mutex> lock(mutex_);
         max_obstacle_height_ = p.get_value<double>();
       }));
     callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
       ".combination_method",
       [&](const rclcpp::Parameter & p) {
+        std::lock_guard<std::recursive_mutex> lock(mutex_);
         combination_method_ = p.get_value<int>();
       }));
     callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".z_voxels",
       [&](const rclcpp::Parameter & p) {
+        std::lock_guard<std::recursive_mutex> lock(mutex_);
         size_z_ = p.get_value<int>();
       }));
     callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".origin_z",
       [&](const rclcpp::Parameter & p) {
+        std::lock_guard<std::recursive_mutex> lock(mutex_);
         origin_z_ = p.get_value<double>();
       }));
     callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".z_resolution",
       [&](const rclcpp::Parameter & p) {
+        std::lock_guard<std::recursive_mutex> lock(mutex_);
         z_resolution_ = p.get_value<double>();
       }));
     callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
       ".unknown_threshold",
       [&](const rclcpp::Parameter & p) {
+        std::lock_guard<std::recursive_mutex> lock(mutex_);
         unknown_threshold_ = p.get_value<int>() + (VOXEL_BITS - size_z_);
       }));
     callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".mark_threshold",
       [&](const rclcpp::Parameter & p) {
+        std::lock_guard<std::recursive_mutex> lock(mutex_);
         mark_threshold_ = p.get_value<int>();
       }));
   }