bugfix (ros-navigation#3109) deadlock when costmap receives new map

Signed-off-by: Daisuke Sato <[email protected]>
CMU-cabot · Aug 23, 2022 · 7073e8d · 7073e8d
1 parent 1028873
commit 7073e8d
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 17 deletions.
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp
@@ -184,7 +184,6 @@ class StaticLayer : public CostmapLayer
   bool trinary_costmap_;
   bool map_received_{false};
   tf2::Duration transform_tolerance_;
-  std::atomic<bool> update_in_progress_;
   nav_msgs::msg::OccupancyGrid::SharedPtr map_buffer_;
   // Dynamic parameters handler
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;

diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp
@@ -161,7 +161,6 @@ StaticLayer::getParameters()
   // Enforce bounds
   lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
   map_received_ = false;
-  update_in_progress_.store(false);
 
   transform_tolerance_ = tf2::durationFromSec(temp_tf_tol);
 
@@ -277,17 +276,12 @@ StaticLayer::interpretValue(unsigned char value)
 void
 StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map)
 {
-  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
   if (!map_received_) {
-    map_received_ = true;
-    processMap(*new_map);
-  }
-  if (update_in_progress_.load()) {
-    map_buffer_ = new_map;
-  } else {
     processMap(*new_map);
-    map_buffer_ = nullptr;
+    map_received_ = true;
   }
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+  map_buffer_ = new_map;
 }
 
 void
@@ -340,9 +334,7 @@ StaticLayer::updateBounds(
   if (!map_received_) {
     return;
   }
-
   std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
-  update_in_progress_.store(true);
 
   // If there is a new available map, load it.
   if (map_buffer_) {
@@ -376,9 +368,7 @@ StaticLayer::updateCosts(
   nav2_costmap_2d::Costmap2D & master_grid,
   int min_i, int min_j, int max_i, int max_j)
 {
-  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
   if (!enabled_) {
-    update_in_progress_.store(false);
     return;
   }
   if (!map_received_) {
@@ -388,9 +378,9 @@ StaticLayer::updateCosts(
       RCLCPP_WARN(logger_, "Can't update static costmap layer, no map received");
       count = 0;
     }
-    update_in_progress_.store(false);
     return;
   }
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
 
   if (!layered_costmap_->isRolling()) {
     // if not rolling, the layered costmap (master_grid) has same coordinates as this layer
@@ -411,7 +401,6 @@ StaticLayer::updateCosts(
         transform_tolerance_);
     } catch (tf2::TransformException & ex) {
       RCLCPP_ERROR(logger_, "StaticLayer: %s", ex.what());
-      update_in_progress_.store(false);
       return;
     }
     // Copy map data given proper transformations
@@ -436,7 +425,6 @@ StaticLayer::updateCosts(
       }
     }
   }
-  update_in_progress_.store(false);
   current_ = true;
 }