From 7a25610e3e5160158c4c3b575dfb63ca47117f16 Mon Sep 17 00:00:00 2001 From: Dirk Braunschweiger <1677757+dirkmb@users.noreply.github.com> Date: Fri, 31 Mar 2023 23:40:06 +0200 Subject: [PATCH] fix data race: prohibit resizeMap() during plugin/filter initialization (#3522) Co-authored-by: Dirk Braunschweiger --- nav2_costmap_2d/src/costmap_2d_ros.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 7f1d582ffb..1fd02a084d 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -153,6 +153,10 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Using plugin \"%s\"", plugin_names_[i].c_str()); std::shared_ptr plugin = plugin_loader_.createSharedInstance(plugin_types_[i]); + + // lock the costmap because no update is allowed until the plugin is initialized + std::unique_lock lock(*(layered_costmap_->getCostmap()->getMutex())); + layered_costmap_->addPlugin(plugin); // TODO(mjeronimo): instead of get(), use a shared ptr @@ -160,6 +164,8 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) layered_costmap_.get(), plugin_names_[i], tf_buffer_.get(), shared_from_this(), callback_group_); + lock.unlock(); + RCLCPP_INFO(get_logger(), "Initialized plugin \"%s\"", plugin_names_[i].c_str()); } // and costmap filters as well @@ -167,12 +173,18 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Using costmap filter \"%s\"", filter_names_[i].c_str()); std::shared_ptr filter = plugin_loader_.createSharedInstance(filter_types_[i]); + + // lock the costmap because no update is allowed until the filter is initialized + std::unique_lock lock(*(layered_costmap_->getCostmap()->getMutex())); + layered_costmap_->addFilter(filter); filter->initialize( layered_costmap_.get(), filter_names_[i], tf_buffer_.get(), shared_from_this(), callback_group_); + lock.unlock(); + RCLCPP_INFO(get_logger(), "Initialized costmap filter \"%s\"", filter_names_[i].c_str()); }