From 29212703a5821f20e4754e0907883c23229d0f15 Mon Sep 17 00:00:00 2001 From: zhenpeng ge Date: Fri, 16 Jul 2021 16:32:24 +0800 Subject: [PATCH 1/3] reduce MapSaver nodes by using callback group/executor combo Signed-off-by: zhenpeng ge --- nav2_map_server/src/map_saver/map_saver.cpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index 3761c35db97..a7f6916261b 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -186,20 +186,29 @@ bool MapSaver::saveMapTopicToFile( map_msg = msg; }; - // Add new subscription for incoming map topic. - // Utilizing local rclcpp::Node (rclcpp_node_) from nav2_util::LifecycleNode - // as a map listener. rclcpp::QoS map_qos(10); // initialize to default if (map_subscribe_transient_local_) { map_qos.transient_local(); map_qos.reliable(); map_qos.keep_last(1); } - auto map_sub = rclcpp_node_->create_subscription( - map_topic_loc, map_qos, mapCallback); + // Create new CallbackGroup for map_sub + auto callback_group = create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + + auto option = rclcpp::SubscriptionOptions(); + option.callback_group = callback_group; + auto map_sub = create_subscription( + map_topic_loc, map_qos, mapCallback, option); + + // Create SingleThreadedExecutor to spin map_sub in callback_group + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_callback_group(callback_group, get_node_base_interface()); rclcpp::Time start_time = now(); while (rclcpp::ok()) { + executor.spin_some(); if ((now() - start_time) > *save_map_timeout_) { RCLCPP_ERROR(get_logger(), "Failed to save the map: timeout"); return false; From 972f6eaeda0b98ef7008fd4060dcee73c557b499 Mon Sep 17 00:00:00 2001 From: zhenpeng ge Date: Fri, 16 Jul 2021 20:25:43 +0800 Subject: [PATCH 2/3] set use_rclcpp_node false --- nav2_map_server/src/map_saver/map_saver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index a7f6916261b..2af6a6b1f3d 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -42,7 +42,7 @@ using namespace std::placeholders; namespace nav2_map_server { MapSaver::MapSaver() -: nav2_util::LifecycleNode("map_saver", "", true) +: nav2_util::LifecycleNode("map_saver", "") { RCLCPP_INFO(get_logger(), "Creating"); From 08cbde003bda50eb43012ba4acb89a5c4df0e5f2 Mon Sep 17 00:00:00 2001 From: zhenpeng ge Date: Tue, 20 Jul 2021 08:59:58 +0800 Subject: [PATCH 3/3] a cleaner solution using a future and spin_until_future_complete() Signed-off-by: zhenpeng ge --- nav2_map_server/src/map_saver/map_saver.cpp | 53 ++++++++------------- 1 file changed, 21 insertions(+), 32 deletions(-) diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index 2af6a6b1f3d..97e7dddc1f4 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -149,12 +149,6 @@ bool MapSaver::saveMapTopicToFile( map_topic_loc.c_str(), save_parameters_loc.map_file_name.c_str()); try { - // Pointer to map message received in the subscription callback - nav_msgs::msg::OccupancyGrid::SharedPtr map_msg = nullptr; - - // Mutex for handling map_msg shared resource - std::recursive_mutex access; - // Correct map_topic_loc if necessary if (map_topic_loc == "") { map_topic_loc = "map"; @@ -179,11 +173,12 @@ bool MapSaver::saveMapTopicToFile( save_parameters_loc.occupied_thresh = occupied_thresh_default_; } + std::promise prom; + std::future future_result = prom.get_future(); // A callback function that receives map message from subscribed topic - auto mapCallback = [&map_msg, &access]( + auto mapCallback = [&prom]( const nav_msgs::msg::OccupancyGrid::SharedPtr msg) -> void { - std::lock_guard guard(access); - map_msg = msg; + prom.set_value(msg); }; rclcpp::QoS map_qos(10); // initialize to default @@ -206,29 +201,23 @@ bool MapSaver::saveMapTopicToFile( // Create SingleThreadedExecutor to spin map_sub in callback_group rclcpp::executors::SingleThreadedExecutor executor; executor.add_callback_group(callback_group, get_node_base_interface()); - rclcpp::Time start_time = now(); - while (rclcpp::ok()) { - executor.spin_some(); - if ((now() - start_time) > *save_map_timeout_) { - RCLCPP_ERROR(get_logger(), "Failed to save the map: timeout"); - return false; - } - - if (map_msg) { - std::lock_guard guard(access); - // map_sub is no more needed - map_sub.reset(); - // Map message received. Saving it to file - if (saveMapToFile(*map_msg, save_parameters_loc)) { - RCLCPP_INFO(get_logger(), "Map saved successfully"); - return true; - } else { - RCLCPP_ERROR(get_logger(), "Failed to save the map"); - return false; - } - } - - rclcpp::sleep_for(std::chrono::milliseconds(100)); + // Spin until map message received + auto timeout = save_map_timeout_->to_chrono(); + auto status = executor.spin_until_future_complete(future_result, timeout); + if (status != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(get_logger(), "Failed to spin map subscription"); + return false; + } + // map_sub is no more needed + map_sub.reset(); + // Map message received. Saving it to file + nav_msgs::msg::OccupancyGrid::SharedPtr map_msg = future_result.get(); + if (saveMapToFile(*map_msg, save_parameters_loc)) { + RCLCPP_INFO(get_logger(), "Map saved successfully"); + return true; + } else { + RCLCPP_ERROR(get_logger(), "Failed to save the map"); + return false; } } catch (std::exception & e) { RCLCPP_ERROR(get_logger(), "Failed to save the map: %s", e.what());