add humble-navigation2

navigation2/nav2_map_server/src/costmap_filter_info/costmap_filter_info_server.cpp

@@ -0,0 +1,115 @@
+// Copyright (c) 2020 Samsung Research Russia
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// TODO(AlexeyMerzlyakov): This dummy info publisher should be removed
+// after Semantic Map Server having the same functionality will be developed.
+
+#include "nav2_map_server/costmap_filter_info_server.hpp"
+
+#include <string>
+#include <memory>
+#include <utility>
+
+namespace nav2_map_server
+{
+
+CostmapFilterInfoServer::CostmapFilterInfoServer(const rclcpp::NodeOptions & options)
+: nav2_util::LifecycleNode("costmap_filter_info_server", "", options)
+{
+  declare_parameter("filter_info_topic", "costmap_filter_info");
+  declare_parameter("type", 0);
+  declare_parameter("mask_topic", "filter_mask");
+  declare_parameter("base", 0.0);
+  declare_parameter("multiplier", 1.0);
+}
+
+CostmapFilterInfoServer::~CostmapFilterInfoServer()
+{
+}
+
+nav2_util::CallbackReturn
+CostmapFilterInfoServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Configuring");
+
+  std::string filter_info_topic = get_parameter("filter_info_topic").as_string();
+
+  publisher_ = this->create_publisher<nav2_msgs::msg::CostmapFilterInfo>(
+    filter_info_topic, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
+
+  msg_ = nav2_msgs::msg::CostmapFilterInfo();
+  msg_.header.frame_id = "";
+  msg_.header.stamp = now();
+  msg_.type = get_parameter("type").as_int();
+  msg_.filter_mask_topic = get_parameter("mask_topic").as_string();
+  msg_.base = static_cast<float>(get_parameter("base").as_double());
+  msg_.multiplier = static_cast<float>(get_parameter("multiplier").as_double());
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+CostmapFilterInfoServer::on_activate(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Activating");
+
+  publisher_->on_activate();
+  auto costmap_filter_info = std::make_unique<nav2_msgs::msg::CostmapFilterInfo>(msg_);
+  publisher_->publish(std::move(costmap_filter_info));
+
+  // create bond connection
+  createBond();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+CostmapFilterInfoServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Deactivating");
+
+  publisher_->on_deactivate();
+
+  // destroy bond connection
+  destroyBond();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+CostmapFilterInfoServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Cleaning up");
+
+  publisher_.reset();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+CostmapFilterInfoServer::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Shutting down");
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+}  // namespace nav2_map_server
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(nav2_map_server::CostmapFilterInfoServer)

navigation2/nav2_map_server/src/costmap_filter_info/main.cpp

@@ -0,0 +1,32 @@
+// Copyright (c) 2020 Samsung Research Russia
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include "nav2_map_server/costmap_filter_info_server.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+int main(int argc, char * argv[])
+{
+  auto logger = rclcpp::get_logger("costmap_filter_info_server");
+
+  RCLCPP_INFO(logger, "This is costmap filter info publisher");
+
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<nav2_map_server::CostmapFilterInfoServer>();
+  rclcpp::spin(node->get_node_base_interface());
+  rclcpp::shutdown();
+
+  return 0;
+}

navigation2/nav2_map_server/src/map_io.cpp

@@ -0,0 +1,602 @@
+/* Copyright 2019 Rover Robotics
+ * Copyright 2010 Brian Gerkey
+ * Copyright (c) 2008, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the <ORGANIZATION> nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "nav2_map_server/map_io.hpp"
+
+#ifndef _WIN32
+#include <libgen.h>
+#endif
+#include <iostream>
+#include <string>
+#include <vector>
+#include <fstream>
+#include <stdexcept>
+#include <cstdlib>
+
+#include "Magick++.h"
+#include "nav2_util/geometry_utils.hpp"
+
+#include "yaml-cpp/yaml.h"
+#include "tf2/LinearMath/Matrix3x3.h"
+#include "tf2/LinearMath/Quaternion.h"
+#include "nav2_util/occ_grid_values.hpp"
+
+#ifdef _WIN32
+// https://github.com/rtv/Stage/blob/master/replace/dirname.c
+static
+char * dirname(char * path)
+{
+  static const char dot[] = ".";
+  char * last_slash;
+
+  if (path == NULL) {
+    return path;
+  }
+
+  /* Replace all "\" with "/" */
+  char * c = path;
+  while (*c != '\0') {
+    if (*c == '\\') {*c = '/';}
+    ++c;
+  }
+
+  /* Find last '/'.  */
+  last_slash = path != NULL ? strrchr(path, '/') : NULL;
+
+  if (last_slash != NULL && last_slash == path) {
+    /* The last slash is the first character in the string.  We have to
+       return "/".  */
+    ++last_slash;
+  } else if (last_slash != NULL && last_slash[1] == '\0') {
+    /* The '/' is the last character, we have to look further.  */
+    last_slash = reinterpret_cast<char *>(memchr(path, last_slash - path, '/'));
+  }
+
+  if (last_slash != NULL) {
+    /* Terminate the path.  */
+    last_slash[0] = '\0';
+  } else {
+    /* This assignment is ill-designed but the XPG specs require to
+       return a string containing "." in any case no directory part is
+       found and so a static and constant string is required.  */
+    path = reinterpret_cast<char *>(dot);
+  }
+
+  return path;
+}
+#endif
+
+namespace nav2_map_server
+{
+using nav2_util::geometry_utils::orientationAroundZAxis;
+
+// === Map input part ===
+
+/// Get the given subnode value.
+/// The only reason this function exists is to wrap the exceptions in slightly nicer error messages,
+/// including the name of the failed key
+/// @throw YAML::Exception
+template<typename T>
+T yaml_get_value(const YAML::Node & node, const std::string & key)
+{
+  try {
+    return node[key].as<T>();
+  } catch (YAML::Exception & e) {
+    std::stringstream ss;
+    ss << "Failed to parse YAML tag '" << key << "' for reason: " << e.msg;
+    throw YAML::Exception(e.mark, ss.str());
+  }
+}
+
+std::string get_home_dir()
+{
+  if (const char * home_dir = std::getenv("HOME")) {
+    return std::string{home_dir};
+  }
+  return std::string{};
+}
+
+std::string expand_user_home_dir_if_needed(
+  std::string yaml_filename,
+  std::string home_variable_value)
+{
+  if (yaml_filename.size() < 2 || !(yaml_filename[0] == '~' && yaml_filename[1] == '/')) {
+    return yaml_filename;
+  }
+  if (home_variable_value.empty()) {
+    RCLCPP_INFO_STREAM(
+      rclcpp::get_logger(
+        "map_io"), "Map yaml file name starts with '~/' but no HOME variable set. \n"
+        << "[INFO] [map_io] User home dir will be not expanded \n");
+    return yaml_filename;
+  }
+  const std::string prefix{home_variable_value};
+  return yaml_filename.replace(0, 1, prefix);
+}
+
+LoadParameters loadMapYaml(const std::string & yaml_filename)
+{
+  YAML::Node doc = YAML::LoadFile(expand_user_home_dir_if_needed(yaml_filename, get_home_dir()));
+  LoadParameters load_parameters;
+
+  auto image_file_name = yaml_get_value<std::string>(doc, "image");
+  if (image_file_name.empty()) {
+    throw YAML::Exception(doc["image"].Mark(), "The image tag was empty.");
+  }
+  if (image_file_name[0] != '/') {
+    // dirname takes a mutable char *, so we copy into a vector
+    std::vector<char> fname_copy(yaml_filename.begin(), yaml_filename.end());
+    fname_copy.push_back('\0');
+    image_file_name = std::string(dirname(fname_copy.data())) + '/' + image_file_name;
+  }
+  load_parameters.image_file_name = image_file_name;
+
+  load_parameters.resolution = yaml_get_value<double>(doc, "resolution");
+  load_parameters.origin = yaml_get_value<std::vector<double>>(doc, "origin");
+  if (load_parameters.origin.size() != 3) {
+    throw YAML::Exception(
+            doc["origin"].Mark(), "value of the 'origin' tag should have 3 elements, not " +
+            std::to_string(load_parameters.origin.size()));
+  }
+
+  load_parameters.free_thresh = yaml_get_value<double>(doc, "free_thresh");
+  load_parameters.occupied_thresh = yaml_get_value<double>(doc, "occupied_thresh");
+
+  auto map_mode_node = doc["mode"];
+  if (!map_mode_node.IsDefined()) {
+    load_parameters.mode = MapMode::Trinary;
+  } else {
+    load_parameters.mode = map_mode_from_string(map_mode_node.as<std::string>());
+  }
+
+  try {
+    load_parameters.negate = yaml_get_value<int>(doc, "negate");
+  } catch (YAML::Exception &) {
+    load_parameters.negate = yaml_get_value<bool>(doc, "negate");
+  }
+
+  RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "resolution: " << load_parameters.resolution);
+  RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "origin[0]: " << load_parameters.origin[0]);
+  RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "origin[1]: " << load_parameters.origin[1]);
+  RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "origin[2]: " << load_parameters.origin[2]);
+  RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "free_thresh: " << load_parameters.free_thresh);
+  RCLCPP_INFO_STREAM(
+    rclcpp::get_logger(
+      "map_io"), "occupied_thresh: " << load_parameters.occupied_thresh);
+  RCLCPP_INFO_STREAM(
+    rclcpp::get_logger("map_io"),
+    "mode: " << map_mode_to_string(load_parameters.mode));
+  RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "negate: " << load_parameters.negate);
+
+  return load_parameters;
+}
+
+void loadMapFromFile(
+  const LoadParameters & load_parameters,
+  nav_msgs::msg::OccupancyGrid & map)
+{
+  Magick::InitializeMagick(nullptr);
+  nav_msgs::msg::OccupancyGrid msg;
+
+  RCLCPP_INFO_STREAM(
+    rclcpp::get_logger("map_io"), "Loading image_file: " <<
+      load_parameters.image_file_name);
+  Magick::Image img(load_parameters.image_file_name);
+
+  // Copy the image data into the map structure
+  msg.info.width = img.size().width();
+  msg.info.height = img.size().height();
+
+  msg.info.resolution = load_parameters.resolution;
+  msg.info.origin.position.x = load_parameters.origin[0];
+  msg.info.origin.position.y = load_parameters.origin[1];
+  msg.info.origin.position.z = 0.0;
+  msg.info.origin.orientation = orientationAroundZAxis(load_parameters.origin[2]);
+
+  // Allocate space to hold the data
+  msg.data.resize(msg.info.width * msg.info.height);
+
+  // Copy pixel data into the map structure
+  for (size_t y = 0; y < msg.info.height; y++) {
+    for (size_t x = 0; x < msg.info.width; x++) {
+      auto pixel = img.pixelColor(x, y);
+
+      std::vector<Magick::Quantum> channels = {pixel.redQuantum(), pixel.greenQuantum(),
+        pixel.blueQuantum()};
+      if (load_parameters.mode == MapMode::Trinary && img.matte()) {
+        // To preserve existing behavior, average in alpha with color channels in Trinary mode.
+        // CAREFUL. alpha is inverted from what you might expect. High = transparent, low = opaque
+        channels.push_back(MaxRGB - pixel.alphaQuantum());
+      }
+      double sum = 0;
+      for (auto c : channels) {
+        sum += c;
+      }
+      /// on a scale from 0.0 to 1.0 how bright is the pixel?
+      double shade = Magick::ColorGray::scaleQuantumToDouble(sum / channels.size());
+
+      // If negate is true, we consider blacker pixels free, and whiter
+      // pixels occupied. Otherwise, it's vice versa.
+      /// on a scale from 0.0 to 1.0, how occupied is the map cell (before thresholding)?
+      double occ = (load_parameters.negate ? shade : 1.0 - shade);
+
+      int8_t map_cell;
+      switch (load_parameters.mode) {
+        case MapMode::Trinary:
+          if (load_parameters.occupied_thresh < occ) {
+            map_cell = nav2_util::OCC_GRID_OCCUPIED;
+          } else if (occ < load_parameters.free_thresh) {
+            map_cell = nav2_util::OCC_GRID_FREE;
+          } else {
+            map_cell = nav2_util::OCC_GRID_UNKNOWN;
+          }
+          break;
+        case MapMode::Scale:
+          if (pixel.alphaQuantum() != OpaqueOpacity) {
+            map_cell = nav2_util::OCC_GRID_UNKNOWN;
+          } else if (load_parameters.occupied_thresh < occ) {
+            map_cell = nav2_util::OCC_GRID_OCCUPIED;
+          } else if (occ < load_parameters.free_thresh) {
+            map_cell = nav2_util::OCC_GRID_FREE;
+          } else {
+            map_cell = std::rint(
+              (occ - load_parameters.free_thresh) /
+              (load_parameters.occupied_thresh - load_parameters.free_thresh) * 100.0);
+          }
+          break;
+        case MapMode::Raw: {
+            double occ_percent = std::round(shade * 255);
+            if (nav2_util::OCC_GRID_FREE <= occ_percent &&
+              occ_percent <= nav2_util::OCC_GRID_OCCUPIED)
+            {
+              map_cell = static_cast<int8_t>(occ_percent);
+            } else {
+              map_cell = nav2_util::OCC_GRID_UNKNOWN;
+            }
+            break;
+          }
+        default:
+          throw std::runtime_error("Invalid map mode");
+      }
+      msg.data[msg.info.width * (msg.info.height - y - 1) + x] = map_cell;
+    }
+  }
+
+  // Since loadMapFromFile() does not belong to any node, publishing in a system time.
+  rclcpp::Clock clock(RCL_SYSTEM_TIME);
+  msg.info.map_load_time = clock.now();
+  msg.header.frame_id = "map";
+  msg.header.stamp = clock.now();
+
+  RCLCPP_INFO_STREAM(
+    rclcpp::get_logger(
+      "map_io"), "Read map " << load_parameters.image_file_name
+                             << ": " << msg.info.width << " X " << msg.info.height << " map @ "
+                             << msg.info.resolution << " m/cell");
+
+  map = msg;
+}
+
+LOAD_MAP_STATUS loadMapFromYaml(
+  const std::string & yaml_file,
+  nav_msgs::msg::OccupancyGrid & map)
+{
+  if (yaml_file.empty()) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_io"), "YAML file name is empty, can't load!");
+    return MAP_DOES_NOT_EXIST;
+  }
+  RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "Loading yaml file: " << yaml_file);
+  LoadParameters load_parameters;
+  try {
+    load_parameters = loadMapYaml(yaml_file);
+  } catch (YAML::Exception & e) {
+    RCLCPP_ERROR_STREAM(
+      rclcpp::get_logger(
+        "map_io"), "Failed processing YAML file " << yaml_file << " at position (" <<
+        e.mark.line << ":" << e.mark.column << ") for reason: " << e.what());
+    return INVALID_MAP_METADATA;
+  } catch (std::exception & e) {
+    RCLCPP_ERROR_STREAM(
+      rclcpp::get_logger("map_io"), "Failed to parse map YAML loaded from file " << yaml_file <<
+        " for reason: " << e.what());
+    return INVALID_MAP_METADATA;
+  }
+  try {
+    loadMapFromFile(load_parameters, map);
+  } catch (std::exception & e) {
+    RCLCPP_ERROR_STREAM(
+      rclcpp::get_logger(
+        "map_io"), "Failed to load image file " << load_parameters.image_file_name <<
+        " for reason: " << e.what());
+    return INVALID_MAP_DATA;
+  }
+
+  return LOAD_MAP_SUCCESS;
+}
+
+// === Map output part ===
+
+/**
+ * @brief Checks map saving parameters for consistency
+ * @param save_parameters Map saving parameters.
+ * NOTE: save_parameters could be updated during function execution.
+ * @throw std::exception in case of inconsistent parameters
+ */
+void checkSaveParameters(SaveParameters & save_parameters)
+{
+  // Magick must me initialized before any activity with images
+  Magick::InitializeMagick(nullptr);
+
+  // Checking map file name
+  if (save_parameters.map_file_name == "") {
+    rclcpp::Clock clock(RCL_SYSTEM_TIME);
+    save_parameters.map_file_name = "map_" +
+      std::to_string(static_cast<int>(clock.now().seconds()));
+    RCLCPP_WARN_STREAM(
+      rclcpp::get_logger("map_io"), "Map file unspecified. Map will be saved to " <<
+        save_parameters.map_file_name << " file");
+  }
+
+  // Checking thresholds
+  if (save_parameters.occupied_thresh == 0.0) {
+    save_parameters.occupied_thresh = 0.65;
+    RCLCPP_WARN_STREAM(
+      rclcpp::get_logger(
+        "map_io"), "Occupied threshold unspecified. Setting it to default value: " <<
+        save_parameters.occupied_thresh);
+  }
+  if (save_parameters.free_thresh == 0.0) {
+    save_parameters.free_thresh = 0.25;
+    RCLCPP_WARN_STREAM(
+      rclcpp::get_logger("map_io"), "Free threshold unspecified. Setting it to default value: " <<
+        save_parameters.free_thresh);
+  }
+  if (1.0 < save_parameters.occupied_thresh) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_io"), "Threshold_occupied must be 1.0 or less");
+    throw std::runtime_error("Incorrect thresholds");
+  }
+  if (save_parameters.free_thresh < 0.0) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_io"), "Free threshold must be 0.0 or greater");
+    throw std::runtime_error("Incorrect thresholds");
+  }
+  if (save_parameters.occupied_thresh <= save_parameters.free_thresh) {
+    RCLCPP_ERROR_STREAM(
+      rclcpp::get_logger(
+        "map_io"), "Threshold_free must be smaller than threshold_occupied");
+    throw std::runtime_error("Incorrect thresholds");
+  }
+
+  // Checking image format
+  if (save_parameters.image_format == "") {
+    save_parameters.image_format = save_parameters.mode == MapMode::Scale ? "png" : "pgm";
+    RCLCPP_WARN_STREAM(
+      rclcpp::get_logger("map_io"), "Image format unspecified. Setting it to: " <<
+        save_parameters.image_format);
+  }
+
+  std::transform(
+    save_parameters.image_format.begin(),
+    save_parameters.image_format.end(),
+    save_parameters.image_format.begin(),
+    [](unsigned char c) {return std::tolower(c);});
+
+  const std::vector<std::string> BLESSED_FORMATS{"bmp", "pgm", "png"};
+  if (
+    std::find(BLESSED_FORMATS.begin(), BLESSED_FORMATS.end(), save_parameters.image_format) ==
+    BLESSED_FORMATS.end())
+  {
+    std::stringstream ss;
+    bool first = true;
+    for (auto & format_name : BLESSED_FORMATS) {
+      if (!first) {
+        ss << ", ";
+      }
+      ss << "'" << format_name << "'";
+      first = false;
+    }
+    RCLCPP_WARN_STREAM(
+      rclcpp::get_logger("map_io"), "Requested image format '" << save_parameters.image_format <<
+        "' is not one of the recommended formats: " << ss.str());
+  }
+  const std::string FALLBACK_FORMAT = "png";
+
+  try {
+    Magick::CoderInfo info(save_parameters.image_format);
+    if (!info.isWritable()) {
+      RCLCPP_WARN_STREAM(
+        rclcpp::get_logger("map_io"), "Format '" << save_parameters.image_format <<
+          "' is not writable. Using '" << FALLBACK_FORMAT << "' instead");
+      save_parameters.image_format = FALLBACK_FORMAT;
+    }
+  } catch (Magick::ErrorOption & e) {
+    RCLCPP_WARN_STREAM(
+      rclcpp::get_logger(
+        "map_io"), "Format '" << save_parameters.image_format << "' is not usable. Using '" <<
+        FALLBACK_FORMAT << "' instead:" << std::endl << e.what());
+    save_parameters.image_format = FALLBACK_FORMAT;
+  }
+
+  // Checking map mode
+  if (
+    save_parameters.mode == MapMode::Scale &&
+    (save_parameters.image_format == "pgm" ||
+    save_parameters.image_format == "jpg" ||
+    save_parameters.image_format == "jpeg"))
+  {
+    RCLCPP_WARN_STREAM(
+      rclcpp::get_logger("map_io"), "Map mode 'scale' requires transparency, but format '" <<
+        save_parameters.image_format <<
+        "' does not support it. Consider switching image format to 'png'.");
+  }
+}
+
+/**
+ * @brief Tries to write map data into a file
+ * @param map Occupancy grid data
+ * @param save_parameters Map saving parameters
+ * @throw std::expection in case of problem
+ */
+void tryWriteMapToFile(
+  const nav_msgs::msg::OccupancyGrid & map,
+  const SaveParameters & save_parameters)
+{
+  RCLCPP_INFO_STREAM(
+    rclcpp::get_logger(
+      "map_io"), "Received a " << map.info.width << " X " << map.info.height << " map @ " <<
+      map.info.resolution << " m/pix");
+
+  std::string mapdatafile = save_parameters.map_file_name + "." + save_parameters.image_format;
+  {
+    // should never see this color, so the initialization value is just for debugging
+    Magick::Image image({map.info.width, map.info.height}, "red");
+
+    // In scale mode, we need the alpha (matte) channel. Else, we don't.
+    // NOTE: GraphicsMagick seems to have trouble loading the alpha channel when saved with
+    // Magick::GreyscaleMatte, so we use TrueColorMatte instead.
+    image.type(
+      save_parameters.mode == MapMode::Scale ?
+      Magick::TrueColorMatteType : Magick::GrayscaleType);
+
+    // Since we only need to support 100 different pixel levels, 8 bits is fine
+    image.depth(8);
+
+    int free_thresh_int = std::rint(save_parameters.free_thresh * 100.0);
+    int occupied_thresh_int = std::rint(save_parameters.occupied_thresh * 100.0);
+
+    for (size_t y = 0; y < map.info.height; y++) {
+      for (size_t x = 0; x < map.info.width; x++) {
+        int8_t map_cell = map.data[map.info.width * (map.info.height - y - 1) + x];
+
+        Magick::Color pixel;
+
+        switch (save_parameters.mode) {
+          case MapMode::Trinary:
+            if (map_cell < 0 || 100 < map_cell) {
+              pixel = Magick::ColorGray(205 / 255.0);
+            } else if (map_cell <= free_thresh_int) {
+              pixel = Magick::ColorGray(254 / 255.0);
+            } else if (occupied_thresh_int <= map_cell) {
+              pixel = Magick::ColorGray(0 / 255.0);
+            } else {
+              pixel = Magick::ColorGray(205 / 255.0);
+            }
+            break;
+          case MapMode::Scale:
+            if (map_cell < 0 || 100 < map_cell) {
+              pixel = Magick::ColorGray{0.5};
+              pixel.alphaQuantum(TransparentOpacity);
+            } else {
+              pixel = Magick::ColorGray{(100.0 - map_cell) / 100.0};
+            }
+            break;
+          case MapMode::Raw:
+            Magick::Quantum q;
+            if (map_cell < 0 || 100 < map_cell) {
+              q = MaxRGB;
+            } else {
+              q = map_cell / 255.0 * MaxRGB;
+            }
+            pixel = Magick::Color(q, q, q);
+            break;
+          default:
+            RCLCPP_ERROR_STREAM(
+              rclcpp::get_logger(
+                "map_io"), "Map mode should be Trinary, Scale or Raw");
+            throw std::runtime_error("Invalid map mode");
+        }
+        image.pixelColor(x, y, pixel);
+      }
+    }
+
+    RCLCPP_INFO_STREAM(
+      rclcpp::get_logger("map_io"),
+      "Writing map occupancy data to " << mapdatafile);
+    image.write(mapdatafile);
+  }
+
+  std::string mapmetadatafile = save_parameters.map_file_name + ".yaml";
+  {
+    std::ofstream yaml(mapmetadatafile);
+
+    geometry_msgs::msg::Quaternion orientation = map.info.origin.orientation;
+    tf2::Matrix3x3 mat(tf2::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w));
+    double yaw, pitch, roll;
+    mat.getEulerYPR(yaw, pitch, roll);
+
+    const int file_name_index = mapdatafile.find_last_of("/\\");
+    std::string image_name = mapdatafile.substr(file_name_index + 1);
+
+    YAML::Emitter e;
+    e << YAML::Precision(3);
+    e << YAML::BeginMap;
+    e << YAML::Key << "image" << YAML::Value << image_name;
+    e << YAML::Key << "mode" << YAML::Value << map_mode_to_string(save_parameters.mode);
+    e << YAML::Key << "resolution" << YAML::Value << map.info.resolution;
+    e << YAML::Key << "origin" << YAML::Flow << YAML::BeginSeq << map.info.origin.position.x <<
+      map.info.origin.position.y << yaw << YAML::EndSeq;
+    e << YAML::Key << "negate" << YAML::Value << 0;
+    e << YAML::Key << "occupied_thresh" << YAML::Value << save_parameters.occupied_thresh;
+    e << YAML::Key << "free_thresh" << YAML::Value << save_parameters.free_thresh;
+
+    if (!e.good()) {
+      RCLCPP_ERROR_STREAM(
+        rclcpp::get_logger("map_io"), "YAML writer failed with an error " << e.GetLastError() <<
+          ". The map metadata may be invalid.");
+    }
+
+    RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "Writing map metadata to " << mapmetadatafile);
+    std::ofstream(mapmetadatafile) << e.c_str();
+  }
+  RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "Map saved");
+}
+
+bool saveMapToFile(
+  const nav_msgs::msg::OccupancyGrid & map,
+  const SaveParameters & save_parameters)
+{
+  // Local copy of SaveParameters that might be modified by checkSaveParameters()
+  SaveParameters save_parameters_loc = save_parameters;
+
+  try {
+    // Checking map parameters for consistency
+    checkSaveParameters(save_parameters_loc);
+
+    tryWriteMapToFile(map, save_parameters_loc);
+  } catch (std::exception & e) {
+    RCLCPP_ERROR_STREAM(
+      rclcpp::get_logger("map_io"),
+      "Failed to write map for reason: " << e.what());
+    return false;
+  }
+  return true;
+}
+
+}  // namespace nav2_map_server

navigation2/nav2_map_server/src/map_mode.cpp

@@ -0,0 +1,52 @@
+// Copyright 2019 Rover Robotics
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_map_server/map_mode.hpp"
+
+#include <stdexcept>
+#include <string>
+
+namespace nav2_map_server
+{
+const char * map_mode_to_string(MapMode map_mode)
+{
+  switch (map_mode) {
+    case MapMode::Trinary:
+      return "trinary";
+    case MapMode::Scale:
+      return "scale";
+    case MapMode::Raw:
+      return "raw";
+    default:
+      throw std::invalid_argument("map_mode");
+  }
+}
+
+MapMode map_mode_from_string(std::string map_mode_name)
+{
+  for (auto & c : map_mode_name) {
+    c = tolower(c);
+  }
+
+  if (map_mode_name == "scale") {
+    return MapMode::Scale;
+  } else if (map_mode_name == "raw") {
+    return MapMode::Raw;
+  } else if (map_mode_name == "trinary") {
+    return MapMode::Trinary;
+  } else {
+    throw std::invalid_argument("map_mode_name");
+  }
+}
+}  // namespace nav2_map_server

navigation2/nav2_map_server/src/map_saver/main_cli.cpp

@@ -0,0 +1,181 @@
+// Copyright 2019 Rover Robotics
+// Copyright (c) 2008, Willow Garage, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <stdexcept>
+
+#include "nav2_map_server/map_mode.hpp"
+#include "nav2_map_server/map_saver.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+
+using namespace nav2_map_server;  // NOLINT
+
+const char * USAGE_STRING{
+  "Usage:\n"
+  "  map_saver_cli [arguments] [--ros-args ROS remapping args]\n"
+  "\n"
+  "Arguments:\n"
+  "  -h/--help\n"
+  "  -t <map_topic>\n"
+  "  -f <mapname>\n"
+  "  --occ <threshold_occupied>\n"
+  "  --free <threshold_free>\n"
+  "  --fmt <image_format>\n"
+  "  --mode trinary(default)/scale/raw\n"
+  "\n"
+  "NOTE: --ros-args should be passed at the end of command line"};
+
+typedef enum
+{
+  COMMAND_MAP_TOPIC,
+  COMMAND_MAP_FILE_NAME,
+  COMMAND_IMAGE_FORMAT,
+  COMMAND_OCCUPIED_THRESH,
+  COMMAND_FREE_THRESH,
+  COMMAND_MODE
+} COMMAND_TYPE;
+
+struct cmd_struct
+{
+  const char * cmd;
+  COMMAND_TYPE command_type;
+};
+
+typedef enum
+{
+  ARGUMENTS_INVALID,
+  ARGUMENTS_VALID,
+  HELP_MESSAGE
+} ARGUMENTS_STATUS;
+
+// Arguments parser
+// Input parameters: logger, argc, argv
+// Output parameters: map_topic, save_parameters
+ARGUMENTS_STATUS parse_arguments(
+  const rclcpp::Logger & logger, int argc, char ** argv,
+  std::string & map_topic, SaveParameters & save_parameters)
+{
+  const struct cmd_struct commands[] = {
+    {"-t", COMMAND_MAP_TOPIC},
+    {"-f", COMMAND_MAP_FILE_NAME},
+    {"--occ", COMMAND_OCCUPIED_THRESH},
+    {"--free", COMMAND_FREE_THRESH},
+    {"--mode", COMMAND_MODE},
+    {"--fmt", COMMAND_IMAGE_FORMAT},
+  };
+
+  std::vector<std::string> arguments(argv + 1, argv + argc);
+  std::vector<rclcpp::Parameter> params_from_args;
+
+
+  size_t cmd_size = sizeof(commands) / sizeof(commands[0]);
+  size_t i;
+  for (auto it = arguments.begin(); it != arguments.end(); it++) {
+    if (*it == "-h" || *it == "--help") {
+      std::cout << USAGE_STRING << std::endl;
+      return HELP_MESSAGE;
+    }
+    if (*it == "--ros-args") {
+      break;
+    }
+    for (i = 0; i < cmd_size; i++) {
+      if (commands[i].cmd == *it) {
+        if ((it + 1) == arguments.end()) {
+          RCLCPP_ERROR(logger, "Wrong argument: %s should be followed by a value.", it->c_str());
+          return ARGUMENTS_INVALID;
+        }
+        it++;
+        switch (commands[i].command_type) {
+          case COMMAND_MAP_TOPIC:
+            map_topic = *it;
+            break;
+          case COMMAND_MAP_FILE_NAME:
+            save_parameters.map_file_name = *it;
+            break;
+          case COMMAND_FREE_THRESH:
+            save_parameters.free_thresh = atof(it->c_str());
+            break;
+          case COMMAND_OCCUPIED_THRESH:
+            save_parameters.occupied_thresh = atof(it->c_str());
+            break;
+          case COMMAND_IMAGE_FORMAT:
+            save_parameters.image_format = *it;
+            break;
+          case COMMAND_MODE:
+            try {
+              save_parameters.mode = map_mode_from_string(*it);
+            } catch (std::invalid_argument &) {
+              save_parameters.mode = MapMode::Trinary;
+              RCLCPP_WARN(
+                logger,
+                "Map mode parameter not recognized: %s, using default value (trinary)",
+                it->c_str());
+            }
+            break;
+        }
+        break;
+      }
+    }
+    if (i == cmd_size) {
+      RCLCPP_ERROR(logger, "Wrong argument: %s", it->c_str());
+      return ARGUMENTS_INVALID;
+    }
+  }
+
+  return ARGUMENTS_VALID;
+}
+
+int main(int argc, char ** argv)
+{
+  // ROS2 init
+  rclcpp::init(argc, argv);
+  auto logger = rclcpp::get_logger("map_saver_cli");
+
+  // Parse CLI-arguments
+  SaveParameters save_parameters;
+  std::string map_topic = "map";
+  switch (parse_arguments(logger, argc, argv, map_topic, save_parameters)) {
+    case ARGUMENTS_INVALID:
+      rclcpp::shutdown();
+      return -1;
+    case HELP_MESSAGE:
+      rclcpp::shutdown();
+      return 0;
+    case ARGUMENTS_VALID:
+      break;
+  }
+
+  // Call saveMapTopicToFile()
+  int retcode;
+  try {
+    auto map_saver = std::make_shared<nav2_map_server::MapSaver>();
+    map_saver->on_configure(rclcpp_lifecycle::State());
+    if (map_saver->saveMapTopicToFile(map_topic, save_parameters)) {
+      retcode = 0;
+    } else {
+      retcode = 1;
+    }
+  } catch (std::exception & e) {
+    RCLCPP_ERROR(logger, "Unexpected problem appear: %s", e.what());
+    retcode = -1;
+  }
+
+  // Exit
+  rclcpp::shutdown();
+  return retcode;
+}

navigation2/nav2_map_server/src/map_saver/main_server.cpp

@@ -0,0 +1,32 @@
+// Copyright (c) 2020 Samsung Research Russia
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <stdexcept>
+#include <string>
+
+#include "nav2_map_server/map_saver.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+
+  auto logger = rclcpp::get_logger("map_saver_server");
+  auto service_node = std::make_shared<nav2_map_server::MapSaver>();
+  rclcpp::spin(service_node->get_node_base_interface());
+  rclcpp::shutdown();
+  return 0;
+}

navigation2/nav2_map_server/src/map_saver/map_saver.cpp

@@ -0,0 +1,242 @@
+/*
+ * Copyright (c) 2020 Samsung Research Russia
+ * Copyright 2019 Rover Robotics
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the <ORGANIZATION> nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "nav2_map_server/map_saver.hpp"
+
+#include <string>
+#include <memory>
+#include <stdexcept>
+#include <functional>
+#include <mutex>
+
+using namespace std::placeholders;
+
+namespace nav2_map_server
+{
+MapSaver::MapSaver(const rclcpp::NodeOptions & options)
+: nav2_util::LifecycleNode("map_saver", "", options)
+{
+  RCLCPP_INFO(get_logger(), "Creating");
+
+  // Declare the node parameters
+  declare_parameter("save_map_timeout", 2.0);
+  declare_parameter("free_thresh_default", 0.25);
+  declare_parameter("occupied_thresh_default", 0.65);
+  declare_parameter("map_subscribe_transient_local", true);
+}
+
+MapSaver::~MapSaver()
+{
+}
+
+nav2_util::CallbackReturn
+MapSaver::on_configure(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Configuring");
+
+  // Make name prefix for services
+  const std::string service_prefix = get_name() + std::string("/");
+
+  save_map_timeout_ = std::make_shared<rclcpp::Duration>(
+    rclcpp::Duration::from_seconds(get_parameter("save_map_timeout").as_double()));
+  free_thresh_default_ = get_parameter("free_thresh_default").as_double();
+  occupied_thresh_default_ = get_parameter("occupied_thresh_default").as_double();
+  map_subscribe_transient_local_ = get_parameter("map_subscribe_transient_local").as_bool();
+
+  // Create a service that saves the occupancy grid from map topic to a file
+  save_map_service_ = create_service<nav2_msgs::srv::SaveMap>(
+    service_prefix + save_map_service_name_,
+    std::bind(&MapSaver::saveMapCallback, this, _1, _2, _3));
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+MapSaver::on_activate(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Activating");
+
+  // create bond connection
+  createBond();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+MapSaver::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Deactivating");
+
+  // destroy bond connection
+  destroyBond();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+MapSaver::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Cleaning up");
+
+  save_map_service_.reset();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+MapSaver::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Shutting down");
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+void MapSaver::saveMapCallback(
+  const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+  const std::shared_ptr<nav2_msgs::srv::SaveMap::Request> request,
+  std::shared_ptr<nav2_msgs::srv::SaveMap::Response> response)
+{
+  // Set input arguments and call saveMapTopicToFile()
+  SaveParameters save_parameters;
+  save_parameters.map_file_name = request->map_url;
+  save_parameters.image_format = request->image_format;
+  save_parameters.free_thresh = request->free_thresh;
+  save_parameters.occupied_thresh = request->occupied_thresh;
+  try {
+    save_parameters.mode = map_mode_from_string(request->map_mode);
+  } catch (std::invalid_argument &) {
+    save_parameters.mode = MapMode::Trinary;
+    RCLCPP_WARN(
+      get_logger(), "Map mode parameter not recognized: '%s', using default value (trinary)",
+      request->map_mode.c_str());
+  }
+
+  response->result = saveMapTopicToFile(request->map_topic, save_parameters);
+}
+
+bool MapSaver::saveMapTopicToFile(
+  const std::string & map_topic,
+  const SaveParameters & save_parameters)
+{
+  // Local copies of map_topic and save_parameters that could be changed
+  std::string map_topic_loc = map_topic;
+  SaveParameters save_parameters_loc = save_parameters;
+
+  RCLCPP_INFO(
+    get_logger(), "Saving map from \'%s\' topic to \'%s\' file",
+    map_topic_loc.c_str(), save_parameters_loc.map_file_name.c_str());
+
+  try {
+    // Correct map_topic_loc if necessary
+    if (map_topic_loc == "") {
+      map_topic_loc = "map";
+      RCLCPP_WARN(
+        get_logger(), "Map topic unspecified. Map messages will be read from \'%s\' topic",
+        map_topic_loc.c_str());
+    }
+
+    // Set default for MapSaver node thresholds parameters
+    if (save_parameters_loc.free_thresh == 0.0) {
+      RCLCPP_WARN(
+        get_logger(),
+        "Free threshold unspecified. Setting it to default value: %f",
+        free_thresh_default_);
+      save_parameters_loc.free_thresh = free_thresh_default_;
+    }
+    if (save_parameters_loc.occupied_thresh == 0.0) {
+      RCLCPP_WARN(
+        get_logger(),
+        "Occupied threshold unspecified. Setting it to default value: %f",
+        occupied_thresh_default_);
+      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 = [&prom](
+      const nav_msgs::msg::OccupancyGrid::SharedPtr msg) -> void {
+        prom.set_value(msg);
+      };
+
+    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);
+    }
+
+    // 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());
+    return false;
+  }
+
+  return false;
+}
+
+}  // namespace nav2_map_server
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(nav2_map_server::MapSaver)

navigation2/nav2_map_server/src/map_server/main.cpp

@@ -0,0 +1,30 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <stdexcept>
+#include <string>
+
+#include "nav2_map_server/map_server.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+int main(int argc, char ** argv)
+{
+  std::string node_name("map_server");
+
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<nav2_map_server::MapServer>();
+  rclcpp::spin(node->get_node_base_interface());
+  rclcpp::shutdown();
+}

navigation2/nav2_map_server/src/map_server/map_server.cpp

@@ -0,0 +1,257 @@
+/* Copyright (c) 2018 Intel Corporation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/* Copyright 2019 Rover Robotics
+ * Copyright 2010 Brian Gerkey
+ * Copyright (c) 2008, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "nav2_map_server/map_server.hpp"
+
+#include <string>
+#include <memory>
+#include <fstream>
+#include <stdexcept>
+#include <utility>
+
+#include "yaml-cpp/yaml.h"
+#include "lifecycle_msgs/msg/state.hpp"
+#include "nav2_map_server/map_io.hpp"
+
+using namespace std::chrono_literals;
+using namespace std::placeholders;
+
+namespace nav2_map_server
+{
+
+MapServer::MapServer(const rclcpp::NodeOptions & options)
+: nav2_util::LifecycleNode("map_server", "", options), map_available_(false)
+{
+  RCLCPP_INFO(get_logger(), "Creating");
+
+  // Declare the node parameters
+  declare_parameter("yaml_filename", rclcpp::PARAMETER_STRING);
+  declare_parameter("topic_name", "map");
+  declare_parameter("frame_id", "map");
+}
+
+MapServer::~MapServer()
+{
+}
+
+nav2_util::CallbackReturn
+MapServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Configuring");
+
+  // Get the name of the YAML file to use (can be empty if no initial map should be used)
+  std::string yaml_filename = get_parameter("yaml_filename").as_string();
+  std::string topic_name = get_parameter("topic_name").as_string();
+  frame_id_ = get_parameter("frame_id").as_string();
+
+  // only try to load map if parameter was set
+  if (!yaml_filename.empty()) {
+    // Shared pointer to LoadMap::Response is also should be initialized
+    // in order to avoid null-pointer dereference
+    std::shared_ptr<nav2_msgs::srv::LoadMap::Response> rsp =
+      std::make_shared<nav2_msgs::srv::LoadMap::Response>();
+
+    if (!loadMapResponseFromYaml(yaml_filename, rsp)) {
+      throw std::runtime_error("Failed to load map yaml file: " + yaml_filename);
+    }
+  } else {
+    RCLCPP_INFO(
+      get_logger(),
+      "yaml-filename parameter is empty, set map through '%s'-service",
+      load_map_service_name_.c_str());
+  }
+
+  // Make name prefix for services
+  const std::string service_prefix = get_name() + std::string("/");
+
+  // Create a service that provides the occupancy grid
+  occ_service_ = create_service<nav_msgs::srv::GetMap>(
+    service_prefix + std::string(service_name_),
+    std::bind(&MapServer::getMapCallback, this, _1, _2, _3));
+
+  // Create a publisher using the QoS settings to emulate a ROS1 latched topic
+  occ_pub_ = create_publisher<nav_msgs::msg::OccupancyGrid>(
+    topic_name,
+    rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
+
+  // Create a service that loads the occupancy grid from a file
+  load_map_service_ = create_service<nav2_msgs::srv::LoadMap>(
+    service_prefix + std::string(load_map_service_name_),
+    std::bind(&MapServer::loadMapCallback, this, _1, _2, _3));
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+MapServer::on_activate(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Activating");
+
+  // Publish the map using the latched topic
+  occ_pub_->on_activate();
+  if (map_available_) {
+    auto occ_grid = std::make_unique<nav_msgs::msg::OccupancyGrid>(msg_);
+    occ_pub_->publish(std::move(occ_grid));
+  }
+
+  // create bond connection
+  createBond();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+MapServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Deactivating");
+
+  occ_pub_->on_deactivate();
+
+  // destroy bond connection
+  destroyBond();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+MapServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Cleaning up");
+
+  occ_pub_.reset();
+  occ_service_.reset();
+  load_map_service_.reset();
+  map_available_ = false;
+  msg_ = nav_msgs::msg::OccupancyGrid();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+MapServer::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Shutting down");
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+void MapServer::getMapCallback(
+  const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+  const std::shared_ptr<nav_msgs::srv::GetMap::Request>/*request*/,
+  std::shared_ptr<nav_msgs::srv::GetMap::Response> response)
+{
+  // if not in ACTIVE state, ignore request
+  if (get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
+    RCLCPP_WARN(
+      get_logger(),
+      "Received GetMap request but not in ACTIVE state, ignoring!");
+    return;
+  }
+  RCLCPP_INFO(get_logger(), "Handling GetMap request");
+  response->map = msg_;
+}
+
+void MapServer::loadMapCallback(
+  const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+  const std::shared_ptr<nav2_msgs::srv::LoadMap::Request> request,
+  std::shared_ptr<nav2_msgs::srv::LoadMap::Response> response)
+{
+  // if not in ACTIVE state, ignore request
+  if (get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
+    RCLCPP_WARN(
+      get_logger(),
+      "Received LoadMap request but not in ACTIVE state, ignoring!");
+    response->result = response->RESULT_UNDEFINED_FAILURE;
+    return;
+  }
+  RCLCPP_INFO(get_logger(), "Handling LoadMap request");
+  // Load from file
+  if (loadMapResponseFromYaml(request->map_url, response)) {
+    auto occ_grid = std::make_unique<nav_msgs::msg::OccupancyGrid>(msg_);
+    occ_pub_->publish(std::move(occ_grid));  // publish new map
+  }
+}
+
+bool MapServer::loadMapResponseFromYaml(
+  const std::string & yaml_file,
+  std::shared_ptr<nav2_msgs::srv::LoadMap::Response> response)
+{
+  switch (loadMapFromYaml(yaml_file, msg_)) {
+    case MAP_DOES_NOT_EXIST:
+      response->result = nav2_msgs::srv::LoadMap::Response::RESULT_MAP_DOES_NOT_EXIST;
+      return false;
+    case INVALID_MAP_METADATA:
+      response->result = nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_METADATA;
+      return false;
+    case INVALID_MAP_DATA:
+      response->result = nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_DATA;
+      return false;
+    case LOAD_MAP_SUCCESS:
+      // Correcting msg_ header when it belongs to specific node
+      updateMsgHeader();
+
+      map_available_ = true;
+      response->map = msg_;
+      response->result = nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS;
+  }
+
+  return true;
+}
+
+void MapServer::updateMsgHeader()
+{
+  msg_.info.map_load_time = now();
+  msg_.header.frame_id = frame_id_;
+  msg_.header.stamp = now();
+}
+
+}  // namespace nav2_map_server
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(nav2_map_server::MapServer)