diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index 2af6a6b1f3dfc2ffa39bc23b457a8a6132b2b262..97e7dddc1f448c2de7f1b67df85e5dbe16b35ef1 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<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); }; 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<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)); + // 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());