add mutex lock for plugin param callbacks

ros-navigation · Dec 4, 2019 · 7ddaaf4 · 7ddaaf4
1 parent a76e5fa
commit 7ddaaf4
Show file tree

Hide file tree

Showing 5 changed files with 17 additions and 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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>();
       }));
   }