diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index 3761c35db97..97e7dddc1f4 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"); @@ -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 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); }; - // 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); - - 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 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( + 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(); + 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());