bugfix (#3109) deadlock when costmap receives new map (#3145)

* bugfix (#3109) deadlock when costmap receives new map Signed-off-by: Daisuke Sato <daisukes@cmu.edu> * introduce map_received_in_update_bounds_ flag to make sure processMap will not be called between updateBounds and updateCosts Signed-off-by: Daisuke Sato <daisukes@cmu.edu> Signed-off-by: Daisuke Sato <daisukes@cmu.edu>
ros-navigation · Nov 8, 2022 · 86211fe · 86211fe
1 parent 804af4f
commit 86211fe
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 16 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
@@ -183,8 +183,8 @@ class StaticLayer : public CostmapLayer
   unsigned char unknown_cost_value_;
   bool trinary_costmap_;
   bool map_received_{false};
+  bool map_received_in_update_bounds_{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,7 @@ StaticLayer::getParameters()
   // Enforce bounds
   lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
   map_received_ = false;
-  update_in_progress_.store(false);
+  map_received_in_update_bounds_ = false;
 
   transform_tolerance_ = tf2::durationFromSec(temp_tf_tol);
 
@@ -277,17 +277,13 @@ 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;
+    return;
   }
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+  map_buffer_ = new_map;
 }
 
 void
@@ -338,11 +334,12 @@ StaticLayer::updateBounds(
   double * max_y)
 {
   if (!map_received_) {
+    map_received_in_update_bounds_ = false;
     return;
   }
+  map_received_in_update_bounds_ = true;
 
   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_) {
@@ -378,17 +375,15 @@ StaticLayer::updateCosts(
 {
   std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
   if (!enabled_) {
-    update_in_progress_.store(false);
     return;
   }
-  if (!map_received_) {
+  if (!map_received_in_update_bounds_) {
     static int count = 0;
     // throttle warning down to only 1/10 message rate
     if (++count == 10) {
       RCLCPP_WARN(logger_, "Can't update static costmap layer, no map received");
       count = 0;
     }
-    update_in_progress_.store(false);
     return;
   }
 
@@ -411,7 +406,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 +430,6 @@ StaticLayer::updateCosts(
       }
     }
   }
-  update_in_progress_.store(false);
   current_ = true;
 }