From f1957a1932f50e0363bc9dd866504ffac9522176 Mon Sep 17 00:00:00 2001
From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com>
Date: Wed, 29 May 2024 11:08:37 -0700
Subject: [PATCH] adding mutex lock around map resizing due to dynamic
 parameter changes and associated processes (#4373) (#4378)

(cherry picked from commit b0abc78ace48d21ca5b8b58afb0482d759733cb0)

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
---
 nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp | 1 +
 nav2_costmap_2d/src/costmap_2d_ros.cpp                     | 5 +++++
 2 files changed, 6 insertions(+)

diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp
index af93f4c7e0c..f4f365e8488 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp
@@ -333,6 +333,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode
   std::atomic<bool> stop_updates_{false};
   std::atomic<bool> initialized_{false};
   std::atomic<bool> stopped_{true};
+  std::mutex _dynamic_parameter_mutex;
   std::unique_ptr<std::thread> map_update_thread_;  ///< @brief A thread for updating the map
   rclcpp::Time last_publish_{0, 0, RCL_ROS_TIME};
   rclcpp::Duration publish_cycle_{1, 0};
diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp
index 6c57949062e..98bcb0ff368 100644
--- a/nav2_costmap_2d/src/costmap_2d_ros.cpp
+++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp
@@ -430,6 +430,9 @@ Costmap2DROS::mapUpdateLoop(double frequency)
 
     // Execute after start() will complete plugins activation
     if (!stopped_) {
+      // Lock while modifying layered costmap and publishing values
+      std::scoped_lock<std::mutex> lock(_dynamic_parameter_mutex);
+
       // Measure the execution time of the updateMap method
       timer.start();
       updateMap();
@@ -619,6 +622,7 @@ Costmap2DROS::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameter
 {
   auto result = rcl_interfaces::msg::SetParametersResult();
   bool resize_map = false;
+  std::lock_guard<std::mutex> lock_reinit(_dynamic_parameter_mutex);
 
   for (auto parameter : parameters) {
     const auto & type = parameter.get_type();
@@ -714,6 +718,7 @@ Costmap2DROS::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameter
     layered_costmap_->resizeMap(
       (unsigned int)(map_width_meters_ / resolution_),
       (unsigned int)(map_height_meters_ / resolution_), resolution_, origin_x_, origin_y_);
+    updateMap();
   }
 
   result.successful = true;