From b32a42519ed58276cd7fd9ad95000245863bbb9c Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Tue, 23 Aug 2022 13:51:22 -0400 Subject: [PATCH 1/2] bugfix (#3109) deadlock when costmap receives new map Signed-off-by: Daisuke Sato --- .../include/nav2_costmap_2d/static_layer.hpp | 1 - nav2_costmap_2d/plugins/static_layer.cpp | 18 ++++-------------- 2 files changed, 4 insertions(+), 15 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 index def79e349f..42fb211ceb 100644 --- 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 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 index 073109c235..f3872298ce 100644 --- 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,13 @@ StaticLayer::interpretValue(unsigned char value) void StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map) { - std::lock_guard 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 guard(*getMutex()); + map_buffer_ = new_map; } void @@ -342,7 +337,6 @@ StaticLayer::updateBounds( } std::lock_guard guard(*getMutex()); - update_in_progress_.store(true); // If there is a new available map, load it. if (map_buffer_) { @@ -378,7 +372,6 @@ StaticLayer::updateCosts( { std::lock_guard guard(*getMutex()); if (!enabled_) { - update_in_progress_.store(false); return; } if (!map_received_) { @@ -388,7 +381,6 @@ StaticLayer::updateCosts( RCLCPP_WARN(logger_, "Can't update static costmap layer, no map received"); count = 0; } - update_in_progress_.store(false); return; } @@ -411,7 +403,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 +427,6 @@ StaticLayer::updateCosts( } } } - update_in_progress_.store(false); current_ = true; } From 7e92afef58e78ee42192cee391d320fbd807bccd Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Mon, 29 Aug 2022 19:31:25 -0400 Subject: [PATCH 2/2] introduce map_received_in_update_bounds_ flag to make sure processMap will not be called between updateBounds and updateCosts Signed-off-by: Daisuke Sato --- nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp | 1 + nav2_costmap_2d/plugins/static_layer.cpp | 5 ++++- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index 42fb211ceb..aec68fac1f 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -183,6 +183,7 @@ 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_; nav_msgs::msg::OccupancyGrid::SharedPtr map_buffer_; // Dynamic parameters handler diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index f3872298ce..18cd51164f 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -161,6 +161,7 @@ StaticLayer::getParameters() // Enforce bounds lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0); map_received_ = false; + map_received_in_update_bounds_ = false; transform_tolerance_ = tf2::durationFromSec(temp_tf_tol); @@ -333,8 +334,10 @@ StaticLayer::updateBounds( double * max_y) { if (!map_received_) { + map_received_in_update_bounds_ = false; return; } + map_received_in_update_bounds_ = true; std::lock_guard guard(*getMutex()); @@ -374,7 +377,7 @@ StaticLayer::updateCosts( if (!enabled_) { 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) {