Reduce map saver nodes (#2454)

* reduce MapSaver nodes by using callback group/executor combo

Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com>

* set use_rclcpp_node false

* a cleaner solution using a future and spin_until_future_complete()

Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com>

Author: gezp

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");
 
@@ -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,47 +173,51 @@ bool MapSaver::saveMapTopicToFile(
       save_parameters_loc.occupied_thresh = occupied_thresh_default_;
     }
 
+    std::promise<nav_msgs::msg::OccupancyGrid::SharedPtr> prom;
+    std::future<nav_msgs::msg::OccupancyGrid::SharedPtr> 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<std::recursive_mutex> guard(access);
-        map_msg = msg;
+        prom.set_value(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<nav_msgs::msg::OccupancyGrid>(
-      map_topic_loc, map_qos, mapCallback);
-
-    rclcpp::Time start_time = now();
-    while (rclcpp::ok()) {
-      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<std::recursive_mutex> 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));
+
+    // 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<nav_msgs::msg::OccupancyGrid>(
+      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());
+    // Spin until map message received
+    auto timeout = save_map_timeout_->to_chrono<std::chrono::nanoseconds>();
+    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());