From 7c630416751a61cd2d9fb0608634751b8fe95565 Mon Sep 17 00:00:00 2001 From: Aitor Miguel Blanco Date: Wed, 1 Jul 2020 01:09:50 +0000 Subject: [PATCH 1/3] Added new costmap buffer to resize map safely Signed-off-by: Aitor Miguel Blanco --- .../include/nav2_costmap_2d/static_layer.hpp | 2 ++ nav2_costmap_2d/plugins/static_layer.cpp | 17 ++++++++++++++--- 2 files changed, 16 insertions(+), 3 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 f6f01e8c56..a925fe052e 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -111,6 +111,8 @@ class StaticLayer : public CostmapLayer bool trinary_costmap_; bool map_received_{false}; tf2::Duration transform_tolerance_; + bool map_updated_{false}; + nav_msgs::msg::OccupancyGrid::SharedPtr map_buffer_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 8180e3f6ba..6bbcb77d97 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -140,6 +140,7 @@ StaticLayer::getParameters() // Enforce bounds lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0); map_received_ = false; + map_updated_ = false; transform_tolerance_ = tf2::durationFromSec(temp_tf_tol); } @@ -193,6 +194,9 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map) unsigned int index = 0; + // we have a new map, update full size of map + std::lock_guard guard(*getMutex()); + // initialize the costmap with static data for (unsigned int i = 0; i < size_y; ++i) { for (unsigned int j = 0; j < size_x; ++j) { @@ -204,8 +208,6 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map) map_frame_ = new_map.header.frame_id; - // we have a new map, update full size of map - std::lock_guard guard(*getMutex()); x_ = y_ = 0; width_ = size_x_; height_ = size_y_; @@ -249,9 +251,12 @@ void StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map) { std::lock_guard guard(*getMutex()); - processMap(*new_map); if (!map_received_) { map_received_ = true; + processMap(*new_map); + } else { + map_buffer_ = new_map; + map_updated_ = true; } } @@ -311,6 +316,12 @@ StaticLayer::updateBounds( } std::lock_guard guard(*getMutex()); + + if (map_updated_) { + processMap(*map_buffer_); + map_updated_ = false; + } + if (!layered_costmap_->isRolling() ) { if (!(has_updated_data_ || has_extra_bounds_)) { return; From 7e926b8c12f6838bac695ec7318ada1b9acd53e8 Mon Sep 17 00:00:00 2001 From: Aitor Miguel Blanco Date: Tue, 21 Jul 2020 10:02:00 +0000 Subject: [PATCH 2/3] Update map if the layer is not being processed. Signed-off-by: Aitor Miguel Blanco --- .../include/nav2_costmap_2d/static_layer.hpp | 2 +- nav2_costmap_2d/plugins/static_layer.cpp | 19 ++++++++++++++----- 2 files changed, 15 insertions(+), 6 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 a925fe052e..ce68d7a6bb 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -111,7 +111,7 @@ class StaticLayer : public CostmapLayer bool trinary_costmap_; bool map_received_{false}; tf2::Duration transform_tolerance_; - bool map_updated_{false}; + std::atomic processing_layer_; nav_msgs::msg::OccupancyGrid::SharedPtr map_buffer_; }; diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 6bbcb77d97..65f775fb23 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -140,7 +140,7 @@ StaticLayer::getParameters() // Enforce bounds lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0); map_received_ = false; - map_updated_ = false; + processing_layer_.store(false); transform_tolerance_ = tf2::durationFromSec(temp_tf_tol); } @@ -254,9 +254,12 @@ StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map) if (!map_received_) { map_received_ = true; processMap(*new_map); - } else { + } + if (processing_layer_.load()) { map_buffer_ = new_map; - map_updated_ = true; + } else { + processMap(*new_map); + map_buffer_ = nullptr; } } @@ -316,10 +319,12 @@ StaticLayer::updateBounds( } std::lock_guard guard(*getMutex()); + processing_layer_.store(true); - if (map_updated_) { + // If there is a new available map, load it. + if (map_buffer_ != nullptr) { processMap(*map_buffer_); - map_updated_ = false; + map_buffer_ = nullptr; } if (!layered_costmap_->isRolling() ) { @@ -349,6 +354,7 @@ StaticLayer::updateCosts( int min_i, int min_j, int max_i, int max_j) { if (!enabled_) { + processing_layer_.store(false); return; } if (!map_received_) { @@ -358,6 +364,7 @@ StaticLayer::updateCosts( RCLCPP_WARN(node_->get_logger(), "Can't update static costmap layer, no map received"); count = 0; } + processing_layer_.store(false); return; } @@ -380,6 +387,7 @@ StaticLayer::updateCosts( transform_tolerance_); } catch (tf2::TransformException & ex) { RCLCPP_ERROR(node_->get_logger(), "StaticLayer: %s", ex.what()); + processing_layer_.store(false); return; } // Copy map data given proper transformations @@ -404,6 +412,7 @@ StaticLayer::updateCosts( } } } + processing_layer_.store(false); } } // namespace nav2_costmap_2d From a241308e1915289e8b243afe5866804abecad354 Mon Sep 17 00:00:00 2001 From: Aitor Miguel Blanco Date: Thu, 23 Jul 2020 10:07:58 +0000 Subject: [PATCH 3/3] Updated bool name and added buffer initialization Signed-off-by: Aitor Miguel Blanco --- .../include/nav2_costmap_2d/static_layer.hpp | 2 +- nav2_costmap_2d/plugins/static_layer.cpp | 17 +++++++++-------- 2 files changed, 10 insertions(+), 9 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 ce68d7a6bb..bbd5daa757 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -111,7 +111,7 @@ class StaticLayer : public CostmapLayer bool trinary_costmap_; bool map_received_{false}; tf2::Duration transform_tolerance_; - std::atomic processing_layer_; + std::atomic update_in_progress_; nav_msgs::msg::OccupancyGrid::SharedPtr map_buffer_; }; diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 65f775fb23..411981813f 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -58,6 +58,7 @@ namespace nav2_costmap_2d { StaticLayer::StaticLayer() +: map_buffer_(nullptr) { } @@ -140,7 +141,7 @@ StaticLayer::getParameters() // Enforce bounds lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0); map_received_ = false; - processing_layer_.store(false); + update_in_progress_.store(false); transform_tolerance_ = tf2::durationFromSec(temp_tf_tol); } @@ -255,7 +256,7 @@ StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map) map_received_ = true; processMap(*new_map); } - if (processing_layer_.load()) { + if (update_in_progress_.load()) { map_buffer_ = new_map; } else { processMap(*new_map); @@ -319,10 +320,10 @@ StaticLayer::updateBounds( } std::lock_guard guard(*getMutex()); - processing_layer_.store(true); + update_in_progress_.store(true); // If there is a new available map, load it. - if (map_buffer_ != nullptr) { + if (map_buffer_) { processMap(*map_buffer_); map_buffer_ = nullptr; } @@ -354,7 +355,7 @@ StaticLayer::updateCosts( int min_i, int min_j, int max_i, int max_j) { if (!enabled_) { - processing_layer_.store(false); + update_in_progress_.store(false); return; } if (!map_received_) { @@ -364,7 +365,7 @@ StaticLayer::updateCosts( RCLCPP_WARN(node_->get_logger(), "Can't update static costmap layer, no map received"); count = 0; } - processing_layer_.store(false); + update_in_progress_.store(false); return; } @@ -387,7 +388,7 @@ StaticLayer::updateCosts( transform_tolerance_); } catch (tf2::TransformException & ex) { RCLCPP_ERROR(node_->get_logger(), "StaticLayer: %s", ex.what()); - processing_layer_.store(false); + update_in_progress_.store(false); return; } // Copy map data given proper transformations @@ -412,7 +413,7 @@ StaticLayer::updateCosts( } } } - processing_layer_.store(false); + update_in_progress_.store(false); } } // namespace nav2_costmap_2d