add humble-navigation2

navigation2/nav2_map_server/CMakeLists.txt

@@ -0,0 +1,164 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_map_server)
+
+list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake_modules)
+
+find_package(ament_cmake REQUIRED)
+find_package(nav2_common REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(nav2_msgs REQUIRED)
+find_package(yaml_cpp_vendor REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(nav2_util REQUIRED)
+find_package(GRAPHICSMAGICKCPP REQUIRED)
+
+nav2_package()
+
+include_directories(include)
+
+set(map_server_executable map_server)
+
+set(map_saver_cli_executable map_saver_cli)
+
+set(map_saver_server_executable map_saver_server)
+
+set(costmap_filter_info_server_executable costmap_filter_info_server)
+
+add_executable(${map_server_executable}
+  src/map_server/main.cpp)
+
+add_executable(${map_saver_cli_executable}
+  src/map_saver/main_cli.cpp)
+
+add_executable(${map_saver_server_executable}
+  src/map_saver/main_server.cpp)
+
+add_executable(${costmap_filter_info_server_executable}
+  src/costmap_filter_info/main.cpp)
+
+set(map_io_library_name map_io)
+
+set(library_name ${map_server_executable}_core)
+
+add_library(${map_io_library_name} SHARED
+  src/map_mode.cpp
+  src/map_io.cpp)
+
+add_library(${library_name} SHARED
+  src/map_server/map_server.cpp
+  src/map_saver/map_saver.cpp
+  src/costmap_filter_info/costmap_filter_info_server.cpp)
+
+set(map_io_dependencies
+  yaml_cpp_vendor
+  nav_msgs
+  nav2_util
+  tf2)
+
+set(map_server_dependencies
+  rclcpp
+  rclcpp_lifecycle
+  rclcpp_components
+  nav_msgs
+  nav2_msgs
+  yaml_cpp_vendor
+  std_msgs
+  nav2_util)
+
+set(map_saver_dependencies
+  rclcpp
+  rclcpp_lifecycle
+  nav_msgs
+  nav2_msgs
+  nav2_util)
+
+ament_target_dependencies(${map_server_executable}
+  ${map_server_dependencies})
+
+ament_target_dependencies(${map_saver_cli_executable}
+  ${map_saver_dependencies})
+
+ament_target_dependencies(${map_saver_server_executable}
+  ${map_saver_dependencies})
+
+ament_target_dependencies(${costmap_filter_info_server_executable}
+  ${map_saver_dependencies})
+
+ament_target_dependencies(${library_name}
+  ${map_server_dependencies})
+
+ament_target_dependencies(${map_io_library_name}
+  ${map_io_dependencies})
+
+target_link_libraries(${library_name}
+  ${map_io_library_name})
+
+target_link_libraries(${map_server_executable}
+  ${library_name})
+
+if(WIN32)
+  target_compile_definitions(${map_server_executable} PRIVATE
+    YAML_CPP_DLL)
+endif()
+
+target_link_libraries(${map_saver_cli_executable}
+  ${library_name})
+
+target_link_libraries(${map_saver_server_executable}
+  ${library_name})
+
+target_link_libraries(${costmap_filter_info_server_executable}
+  ${library_name})
+
+target_include_directories(${map_io_library_name} SYSTEM PRIVATE
+  ${GRAPHICSMAGICKCPP_INCLUDE_DIRS})
+
+target_link_libraries(${map_io_library_name}
+  ${GRAPHICSMAGICKCPP_LIBRARIES})
+
+if(WIN32)
+  target_compile_definitions(${map_io_library_name} PRIVATE
+    YAML_CPP_DLL)
+endif()
+
+rclcpp_components_register_nodes(${library_name} "nav2_map_server::CostmapFilterInfoServer")
+rclcpp_components_register_nodes(${library_name} "nav2_map_server::MapSaver")
+rclcpp_components_register_nodes(${library_name} "nav2_map_server::MapServer")
+
+install(TARGETS
+    ${library_name} ${map_io_library_name}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin)
+
+install(TARGETS
+    ${map_server_executable} ${map_saver_cli_executable} ${map_saver_server_executable}
+    ${costmap_filter_info_server_executable}
+  RUNTIME DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY include/
+  DESTINATION include/)
+
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  set(ament_cmake_copyright_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gtest REQUIRED)
+  add_subdirectory(test)
+endif()
+
+ament_export_include_directories(include)
+ament_export_libraries(
+  ${library_name}
+  ${map_io_library_name}
+)
+ament_export_dependencies(${map_io_dependencies} ${map_server_dependencies})
+ament_package()

navigation2/nav2_map_server/README.md

@@ -0,0 +1,143 @@
+# Map Server
+
+The `Map Server` provides maps to the rest of the Nav2 system using both topic and
+service interfaces. Map server will expose maps on the node bringup, but can also change maps using a `load_map` service during run-time, as well as save maps using a `save_map` server.
+
+See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-map-server.html) for additional parameter descriptions.
+
+### Architecture
+
+In contrast to the ROS1 navigation map server, the nav2 map server will support a variety
+of map types, and thus some aspects of the original code have been refactored to support
+this new extensible framework.
+
+Currently map server divides into tree parts:
+
+- `map_server`
+- `map_saver`
+- `map_io` library
+
+`map_server` is responsible for loading the map from a file through command-line interface
+or by using service requests.
+
+`map_saver` saves the map into a file. Like `map_server`, it has an ability to save the map from
+command-line or by calling a service.
+
+`map_io` - is a map input-output library. The library is designed to be an object-independent
+in order to allow easily save/load map from external code just by calling necessary function.
+This library is also used by `map_loader` and `map_saver` to work. Currently it contains
+OccupancyGrid saving/loading functions moved from the rest part of map server code.
+It is designed to be replaceable for a new IO library (e.g. for library with new map encoding
+method or any other library supporting costmaps, multifloor maps, etc...).
+
+### CLI-usage
+
+#### Map Server
+
+The `Map Server` is a composable ROS2 node. By default, there is a `map_server` executable that
+instances one of these nodes, but it is possible to compose multiple map server nodes into
+a single process, if desired.
+
+The command line for the map server executable is slightly different that it was with ROS1.
+With ROS1, one invoked the map server and passing the map YAML filename, like this:
+
+```
+$ map_server map.yaml
+```
+
+Where the YAML file specified contained the various map metadata, such as:
+
+```
+image: testmap.png
+resolution: 0.1
+origin: [2.0, 3.0, 1.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+```
+
+The Nav2 software retains the map YAML file format from Nav1, but uses the ROS2 parameter
+mechanism to get the name of the YAML file to use. This effectively introduces a
+level of indirection to get the map yaml filename. For example, for a node named 'map_server',
+the parameter file would look like this:
+
+```
+# map_server_params.yaml
+map_server:
+    ros__parameters:
+        yaml_filename: "map.yaml"
+```
+
+One can invoke the map service executable directly, passing the params file on the command line,
+like this:
+
+```
+$ map_server __params:=map_server_params.yaml
+```
+
+There is also possibility of having multiple map server nodes in a single process, where the parameters file would separate the parameters by node name, like this:
+
+```
+# combined_params.yaml
+map_server1:
+    ros__parameters:
+        yaml_filename: "some_map.yaml"
+
+map_server2:
+    ros__parameters:
+        yaml_filename: "another_map.yaml"
+```
+
+Then, one would invoke this process with the params file that contains the parameters for both nodes:
+
+```
+$ process_with_multiple_map_servers __params:=combined_params.yaml
+```
+
+
+The parameter for the initial map (yaml_filename) has to be set, but an empty string can be used if no initial map should be loaded. In this case, no map is loaded during
+on_configure or published during on_activate. The _load_map_-service should the be used to load and publish a map. 
+
+
+#### Map Saver
+
+Like in ROS1 `map_saver` could be used as CLI-executable. It was renamed to `map_saver_cli`
+and could be invoked by following command:
+
+```
+$ ros2 run nav2_map_server map_saver_cli [arguments] [--ros-args ROS remapping args]
+```
+
+## Currently Supported Map Types
+
+- Occupancy grid (nav_msgs/msg/OccupancyGrid)
+
+## MapIO library
+
+`MapIO` library contains following API functions declared in `map_io.hpp` to work with
+OccupancyGrid maps:
+
+- loadMapYaml(): Load and parse the given YAML file
+- loadMapFromFile(): Load the image from map file and generate an OccupancyGrid
+- loadMapFromYaml(): Load the map YAML, image from map file and generate an OccupancyGrid
+- saveMapToFile(): Write OccupancyGrid map to file
+
+## Services
+
+As in ROS navigation, the `map_server` node provides a "map" service to get the map. See the nav_msgs/srv/GetMap.srv file for details.
+
+NEW in ROS2 Eloquent, `map_server` also now provides a "load_map" service and `map_saver` -
+a "save_map" service. See nav2_msgs/srv/LoadMap.srv and nav2_msgs/srv/SaveMap.srv for details.
+
+For using these services `map_server`/`map_saver` should be launched as a continuously running
+`nav2::LifecycleNode` node. In addition to the CLI, `Map Saver` has a functionality of server
+handling incoming services. To run `Map Saver` in a server mode
+`nav2_map_server/launch/map_saver_server.launch.py` launch-file could be used.
+
+Service usage examples:
+
+```
+$ ros2 service call /map_server/load_map nav2_msgs/srv/LoadMap "{map_url: /ros/maps/map.yaml}"
+$ ros2 service call /map_saver/save_map nav2_msgs/srv/SaveMap "{map_topic: map, map_url: my_map, image_format: pgm, map_mode: trinary, free_thresh: 0.25, occupied_thresh: 0.65}"
+```
+

navigation2/nav2_map_server/cmake_modules/FindGRAPHICSMAGICKCPP.cmake

@@ -0,0 +1,34 @@
+# 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.
+
+# CMake script for finding Magick++, the C++ interface for the
+# GraphicsMagick library
+#
+# Output variables:
+#  GRAPHICSMAGICKCPP_FOUND        - system has GraphicsMagick Magick++
+#  GRAPHICSMAGICKCPP_INCLUDE_DIRS - include directories for Magick++
+#  GRAPHICSMAGICKCPP_LIBRARIES    - libraries you need to link to
+include(FindPackageHandleStandardArgs)
+
+find_path(GRAPHICSMAGICKCPP_INCLUDE_DIRS
+  NAMES "Magick++.h"
+  PATH_SUFFIXES GraphicsMagick)
+
+find_library(GRAPHICSMAGICKCPP_LIBRARIES
+  NAMES "GraphicsMagick++" "graphicsmagick")
+
+find_package_handle_standard_args(
+  GRAPHICSMAGICKCPP
+  GRAPHICSMAGICKCPP_LIBRARIES
+  GRAPHICSMAGICKCPP_INCLUDE_DIRS)

navigation2/nav2_map_server/include/nav2_map_server/costmap_filter_info_server.hpp

@@ -0,0 +1,81 @@
+// 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.
+
+#ifndef NAV2_MAP_SERVER__COSTMAP_FILTER_INFO_SERVER_HPP_
+#define NAV2_MAP_SERVER__COSTMAP_FILTER_INFO_SERVER_HPP_
+
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_msgs/msg/costmap_filter_info.hpp"
+
+namespace nav2_map_server
+{
+
+class CostmapFilterInfoServer : public nav2_util::LifecycleNode
+{
+public:
+  /**
+   * @brief Constructor for the nav2_map_server::CostmapFilterInfoServer
+   * @param options Additional options to control creation of the node.
+   */
+  explicit CostmapFilterInfoServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+
+  /**
+   * @brief Destructor for the nav2_map_server::CostmapFilterInfoServer
+   */
+  ~CostmapFilterInfoServer();
+
+protected:
+  /**
+   * @brief Creates CostmapFilterInfo publisher and forms published message from ROS parameters
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Publishes a CostmapFilterInfo message
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Deactivates publisher
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Resets publisher
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Called when in Shutdown state
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
+
+private:
+  rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::CostmapFilterInfo>::SharedPtr publisher_;
+
+  nav2_msgs::msg::CostmapFilterInfo msg_;
+};  // CostmapFilterInfoServer
+
+}  // namespace nav2_map_server
+
+#endif  // NAV2_MAP_SERVER__COSTMAP_FILTER_INFO_SERVER_HPP_

navigation2/nav2_map_server/include/nav2_map_server/map_io.hpp

@@ -0,0 +1,114 @@
+// 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.
+
+/* OccupancyGrid map input-output library */
+
+#ifndef NAV2_MAP_SERVER__MAP_IO_HPP_
+#define NAV2_MAP_SERVER__MAP_IO_HPP_
+
+#include <string>
+#include <vector>
+
+#include "nav2_map_server/map_mode.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+
+/* Map input part */
+
+namespace nav2_map_server
+{
+
+struct LoadParameters
+{
+  std::string image_file_name;
+  double resolution{0};
+  std::vector<double> origin{0, 0, 0};
+  double free_thresh;
+  double occupied_thresh;
+  MapMode mode;
+  bool negate;
+};
+
+typedef enum
+{
+  LOAD_MAP_SUCCESS,
+  MAP_DOES_NOT_EXIST,
+  INVALID_MAP_METADATA,
+  INVALID_MAP_DATA
+} LOAD_MAP_STATUS;
+
+/**
+ * @brief Load and parse the given YAML file
+ * @param yaml_filename Name of the map file passed though parameter
+ * @return Map loading parameters obtained from YAML file
+ * @throw YAML::Exception
+ */
+LoadParameters loadMapYaml(const std::string & yaml_filename);
+
+/**
+ * @brief Load the image from map file and generate an OccupancyGrid
+ * @param load_parameters Parameters of loading map
+ * @param map Output loaded map
+ * @throw std::exception
+ */
+void loadMapFromFile(
+  const LoadParameters & load_parameters,
+  nav_msgs::msg::OccupancyGrid & map);
+
+/**
+ * @brief Load the map YAML, image from map file and
+ * generate an OccupancyGrid
+ * @param yaml_file Name of input YAML file
+ * @param map Output loaded map
+ * @return status of map loaded
+ */
+LOAD_MAP_STATUS loadMapFromYaml(
+  const std::string & yaml_file,
+  nav_msgs::msg::OccupancyGrid & map);
+
+
+/* Map output part */
+
+struct SaveParameters
+{
+  std::string map_file_name{""};
+  std::string image_format{""};
+  double free_thresh{0.0};
+  double occupied_thresh{0.0};
+  MapMode mode{MapMode::Trinary};
+};
+
+/**
+ * @brief Write OccupancyGrid map to file
+ * @param map OccupancyGrid map data
+ * @param save_parameters Map saving parameters.
+ * @return true or false
+ */
+bool saveMapToFile(
+  const nav_msgs::msg::OccupancyGrid & map,
+  const SaveParameters & save_parameters);
+
+/**
+ * @brief Expand ~/ to home user dir.
+ * @param yaml_filename Name of input YAML file.
+ * @param home_dir Expanded `~/`home dir or empty string if HOME not set
+ *
+ * @return Expanded string or input string if `~/` not expanded
+ */
+std::string expand_user_home_dir_if_needed(
+  std::string yaml_filename,
+  std::string home_dir);
+
+}  // namespace nav2_map_server
+
+#endif  // NAV2_MAP_SERVER__MAP_IO_HPP_

navigation2/nav2_map_server/include/nav2_map_server/map_mode.hpp

@@ -0,0 +1,71 @@
+// Copyright (c) 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.
+
+#ifndef NAV2_MAP_SERVER__MAP_MODE_HPP_
+#define NAV2_MAP_SERVER__MAP_MODE_HPP_
+
+#include <string>
+#include <vector>
+namespace nav2_map_server
+{
+/**
+ * @enum nav2_map_server::MapMode
+ * @brief Describes the relation between image pixel values and map occupancy
+ * status (0-100; -1). Lightness refers to the mean of a given pixel's RGB
+ * channels on a scale from 0 to 255.
+ */
+enum class MapMode
+{
+  /**
+   * Together with associated threshold values (occupied and free):
+   *   lightness >= occupied threshold - Occupied (100)
+   *             ... (anything in between) - Unknown (-1)
+   *    lightness <= free threshold - Free (0)
+   */
+  Trinary,
+  /**
+   * Together with associated threshold values (occupied and free):
+   *   alpha < 1.0 - Unknown (-1)
+   *   lightness >= occ_th - Occupied (100)
+   *             ... (linearly interpolate to)
+   *   lightness <= free_th - Free (0)
+   */
+  Scale,
+  /**
+   * Lightness = 0 - Free (0)
+   *          ... (linearly interpolate to)
+   * Lightness = 100 - Occupied (100)
+   * Lightness >= 101 - Unknown
+   */
+  Raw,
+};
+
+/**
+ * @brief Convert a MapMode enum to the name of the map mode
+ * @param map_mode Mode for the map
+ * @return String identifier of the given map mode
+ * @throw std::invalid_argument if the given value is not a defined map mode
+ */
+const char * map_mode_to_string(MapMode map_mode);
+
+/**
+ * @brief Convert the name of a map mode to a MapMode enum
+ * @param map_mode_name Name of the map mode
+ * @throw std::invalid_argument if the name does not name a map mode
+ * @return map mode corresponding to the string
+ */
+MapMode map_mode_from_string(std::string map_mode_name);
+}  // namespace nav2_map_server
+
+#endif  // NAV2_MAP_SERVER__MAP_MODE_HPP_

navigation2/nav2_map_server/include/nav2_map_server/map_saver.hpp

@@ -0,0 +1,118 @@
+// 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.
+
+#ifndef NAV2_MAP_SERVER__MAP_SAVER_HPP_
+#define NAV2_MAP_SERVER__MAP_SAVER_HPP_
+
+#include <string>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_msgs/srv/save_map.hpp"
+
+#include "map_io.hpp"
+
+namespace nav2_map_server
+{
+
+/**
+ * @class nav2_map_server::MapSaver
+ * @brief A class that provides map saving methods and services
+ */
+class MapSaver : public nav2_util::LifecycleNode
+{
+public:
+  /**
+   * @brief Constructor for the nav2_map_server::MapSaver
+   * @param options Additional options to control creation of the node.
+   */
+  explicit MapSaver(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+
+  /**
+   * @brief Destructor for the nav2_map_server::MapServer
+   */
+  ~MapSaver();
+
+  /**
+   * @brief Read a message from incoming map topic and save map to a file
+   * @param map_topic Incoming map topic name
+   * @param save_parameters Map saving parameters.
+   * @return true of false
+   */
+  bool saveMapTopicToFile(
+    const std::string & map_topic,
+    const SaveParameters & save_parameters);
+
+  /**
+   * @brief Sets up map saving service
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Called when node switched to active state
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Called when node switched to inactive state
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Called when it is required node clean-up
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Called when in Shutdown state
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
+
+protected:
+  /**
+   * @brief Map saving service callback
+   * @param request_header Service request header
+   * @param request Service request
+   * @param response Service response
+   */
+  void 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);
+
+  // The timeout for saving the map in service
+  std::shared_ptr<rclcpp::Duration> save_map_timeout_;
+  // Default values for map thresholds
+  double free_thresh_default_;
+  double occupied_thresh_default_;
+  // param for handling QoS configuration
+  bool map_subscribe_transient_local_;
+
+  // The name of the service for saving a map from topic
+  const std::string save_map_service_name_{"save_map"};
+  // A service to save the map to a file at run time (SaveMap)
+  rclcpp::Service<nav2_msgs::srv::SaveMap>::SharedPtr save_map_service_;
+};
+
+}  // namespace nav2_map_server
+
+#endif  // NAV2_MAP_SERVER__MAP_SAVER_HPP_

navigation2/nav2_map_server/include/nav2_map_server/map_server.hpp

@@ -0,0 +1,148 @@
+// 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.
+
+#ifndef NAV2_MAP_SERVER__MAP_SERVER_HPP_
+#define NAV2_MAP_SERVER__MAP_SERVER_HPP_
+
+#include <string>
+#include <memory>
+#include <functional>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "nav_msgs/srv/get_map.hpp"
+#include "nav2_msgs/srv/load_map.hpp"
+
+namespace nav2_map_server
+{
+
+/**
+ * @class nav2_map_server::MapServer
+ * @brief Parses the map yaml file and creates a service and a publisher that
+ * provides occupancy grid
+ */
+class MapServer : public nav2_util::LifecycleNode
+{
+public:
+  /**
+   * @brief A constructor for nav2_map_server::MapServer
+   * @param options Additional options to control creation of the node.
+   */
+  explicit MapServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+
+  /**
+   * @brief A Destructor for nav2_map_server::MapServer
+   */
+  ~MapServer();
+
+protected:
+  /**
+   * @brief Sets up required params and services. Loads map and its parameters from the file
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Start publishing the map using the latched topic
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Stops publishing the latched topic
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Resets the member variables
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Called when in Shutdown state
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
+
+  /**
+   * @brief Load the map YAML, image from map file name and
+   * generate output response containing an OccupancyGrid.
+   * Update msg_ class variable.
+   * @param yaml_file name of input YAML file
+   * @param response Output response with loaded OccupancyGrid map
+   * @return true or false
+   */
+  bool loadMapResponseFromYaml(
+    const std::string & yaml_file,
+    std::shared_ptr<nav2_msgs::srv::LoadMap::Response> response);
+
+  /**
+   * @brief Method correcting msg_ header when it belongs to instantiated object
+   */
+  void updateMsgHeader();
+
+  /**
+   * @brief Map getting service callback
+   * @param request_header Service request header
+   * @param request Service request
+   * @param response Service response
+   */
+  void 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);
+
+  /**
+   * @brief Map loading service callback
+   * @param request_header Service request header
+   * @param request Service request
+   * @param response Service response
+   */
+  void 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);
+
+  // The name of the service for getting a map
+  const std::string service_name_{"map"};
+
+  // The name of the service for loading a map
+  const std::string load_map_service_name_{"load_map"};
+
+  // A service to provide the occupancy grid (GetMap) and the message to return
+  rclcpp::Service<nav_msgs::srv::GetMap>::SharedPtr occ_service_;
+
+  // A service to load the occupancy grid from file at run time (LoadMap)
+  rclcpp::Service<nav2_msgs::srv::LoadMap>::SharedPtr load_map_service_;
+
+  // A topic on which the occupancy grid will be published
+  rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::OccupancyGrid>::SharedPtr occ_pub_;
+
+  // The frame ID used in the returned OccupancyGrid message
+  std::string frame_id_;
+
+  // The message to publish on the occupancy grid topic
+  nav_msgs::msg::OccupancyGrid msg_;
+
+  // true if msg_ was initialized
+  bool map_available_;
+};
+
+}  // namespace nav2_map_server
+
+#endif  // NAV2_MAP_SERVER__MAP_SERVER_HPP_

navigation2/nav2_map_server/launch/map_saver_server.launch.py

@@ -0,0 +1,55 @@
+#!/usr/bin/env python3
+
+# 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.
+
+from launch import LaunchDescription
+import launch_ros.actions
+
+
+def generate_launch_description():
+    # Parameters
+    lifecycle_nodes = ['map_saver']
+    use_sim_time = True
+    autostart = True
+    save_map_timeout = 2.0
+    free_thresh_default = 0.25
+    occupied_thresh_default = 0.65
+
+    # Nodes launching commands
+    start_map_saver_server_cmd = launch_ros.actions.Node(
+            package='nav2_map_server',
+            executable='map_saver_server',
+            output='screen',
+            emulate_tty=True,  # https://github.com/ros2/launch/issues/188
+            parameters=[{'save_map_timeout': save_map_timeout},
+                        {'free_thresh_default': free_thresh_default},
+                        {'occupied_thresh_default': occupied_thresh_default}])
+
+    start_lifecycle_manager_cmd = launch_ros.actions.Node(
+            package='nav2_lifecycle_manager',
+            executable='lifecycle_manager',
+            name='lifecycle_manager',
+            output='screen',
+            emulate_tty=True,  # https://github.com/ros2/launch/issues/188
+            parameters=[{'use_sim_time': use_sim_time},
+                        {'autostart': autostart},
+                        {'node_names': lifecycle_nodes}])
+
+    ld = LaunchDescription()
+
+    ld.add_action(start_map_saver_server_cmd)
+    ld.add_action(start_lifecycle_manager_cmd)
+
+    return ld

navigation2/nav2_map_server/package.xml

@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_map_server</name>
+  <version>1.1.18</version>
+  <description>
+    Refactored map server for ROS2 Navigation
+  </description>
+  <maintainer email="brian.wilcox@intel.com">Brian Wilcox</maintainer>
+  <license>Apache-2.0</license>
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>nav2_common</build_depend>
+
+  <depend>rclcpp_lifecycle</depend>
+  <depend>nav_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>yaml_cpp_vendor</depend>
+  <depend>launch_ros</depend>
+  <depend>launch_testing</depend>
+  <depend>tf2</depend>
+  <depend>nav2_msgs</depend>
+  <depend>nav2_util</depend>
+  <depend>graphicsmagick</depend>
+
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
+  <test_depend>launch</test_depend>
+  <test_depend>launch_testing</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+</package>

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)

navigation2/nav2_map_server/test/CMakeLists.txt

@@ -0,0 +1,8 @@
+set(TEST_LAUNCH_DIR ${CMAKE_CURRENT_SOURCE_DIR}/test_launch_files)
+set(TEST_DIR ${CMAKE_CURRENT_SOURCE_DIR})
+
+add_definitions( -DTEST_DIRECTORY=\"${CMAKE_CURRENT_SOURCE_DIR}\")
+
+add_subdirectory(unit)
+add_subdirectory(component)
+add_subdirectory(map_saver_cli)

navigation2/nav2_map_server/test/component/CMakeLists.txt

@@ -0,0 +1,51 @@
+include_directories(${PROJECT_SOURCE_DIR}/test)
+
+# map_server component test
+ament_add_gtest_executable(test_map_server_node
+  test_map_server_node.cpp
+  ${PROJECT_SOURCE_DIR}/test/test_constants.cpp
+)
+ament_target_dependencies(test_map_server_node rclcpp nav_msgs)
+target_link_libraries(test_map_server_node
+  ${library_name}
+)
+
+ament_add_test(test_map_server_node
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_map_server_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  ENV
+    TEST_DIR=${TEST_DIR}
+    TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR}
+    TEST_EXECUTABLE=$<TARGET_FILE:test_map_server_node>
+)
+
+# map_saver component test
+ament_add_gtest_executable(test_map_saver_node
+  test_map_saver_node.cpp
+  ${PROJECT_SOURCE_DIR}/test/test_constants.cpp
+)
+
+ament_target_dependencies(test_map_saver_node rclcpp nav_msgs)
+target_link_libraries(test_map_saver_node
+  ${library_name}
+)
+
+add_executable(test_map_saver_publisher
+  test_map_saver_publisher.cpp
+  ${PROJECT_SOURCE_DIR}/test/test_constants.cpp
+)
+
+target_link_libraries(test_map_saver_publisher
+  ${map_io_library_name}
+)
+
+ament_add_test(test_map_saver_node
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_map_saver_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  ENV
+    TEST_DIR=${TEST_DIR}
+    TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR}
+    TEST_EXECUTABLE=$<TARGET_FILE:test_map_saver_node>
+)

navigation2/nav2_map_server/test/component/test_map_saver_launch.py

@@ -0,0 +1,47 @@
+#!/usr/bin/env python3
+
+# 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.
+
+import os
+import sys
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import ExecuteProcess
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_testing.legacy import LaunchTestService
+
+
+def main(argv=sys.argv[1:]):
+    launchFile = os.path.join(os.getenv('TEST_LAUNCH_DIR'), 'map_saver_node.launch.py')
+    testExecutable = os.getenv('TEST_EXECUTABLE')
+    ld = LaunchDescription([
+        IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])),
+    ])
+    test1_action = ExecuteProcess(
+        cmd=[testExecutable],
+        name='test_map_saver_node',
+    )
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    os.chdir(os.getenv('TEST_LAUNCH_DIR'))
+    return lts.run(ls)
+
+
+if __name__ == '__main__':
+    sys.exit(main())

navigation2/nav2_map_server/test/component/test_map_saver_node.cpp

@@ -0,0 +1,222 @@
+// 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 <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+#include <filesystem>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "test_constants/test_constants.h"
+#include "nav2_map_server/map_saver.hpp"
+#include "nav2_util/lifecycle_service_client.hpp"
+#include "nav2_msgs/srv/save_map.hpp"
+
+#define TEST_DIR TEST_DIRECTORY
+
+using std::filesystem::path;
+using lifecycle_msgs::msg::Transition;
+using namespace nav2_map_server;  // NOLINT
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+
+RclCppFixture g_rclcppfixture;
+
+class MapSaverTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = rclcpp::Node::make_shared("map_client_test");
+    lifecycle_client_ =
+      std::make_shared<nav2_util::LifecycleServiceClient>("map_saver", node_);
+    RCLCPP_INFO(node_->get_logger(), "Creating Test Node");
+
+    std::this_thread::sleep_for(std::chrono::seconds(5));  // allow node to start up
+    const std::chrono::seconds timeout(5);
+    lifecycle_client_->change_state(Transition::TRANSITION_CONFIGURE, timeout);
+    lifecycle_client_->change_state(Transition::TRANSITION_ACTIVATE, timeout);
+  }
+
+  static void TearDownTestCase()
+  {
+    lifecycle_client_->change_state(Transition::TRANSITION_DEACTIVATE);
+    lifecycle_client_->change_state(Transition::TRANSITION_CLEANUP);
+    lifecycle_client_.reset();
+    node_.reset();
+  }
+
+  template<class T>
+  typename T::Response::SharedPtr send_request(
+
+    rclcpp::Node::SharedPtr node,
+    typename rclcpp::Client<T>::SharedPtr client,
+    typename T::Request::SharedPtr request)
+  {
+    auto result = client->async_send_request(request);
+
+    // Wait for the result
+    if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) {
+      return result.get();
+    } else {
+      return nullptr;
+    }
+  }
+
+protected:
+  // Check that map_msg corresponds to reference pattern
+  // Input: map_msg
+  void verifyMapMsg(const nav_msgs::msg::OccupancyGrid & map_msg)
+  {
+    ASSERT_FLOAT_EQ(map_msg.info.resolution, g_valid_image_res);
+    ASSERT_EQ(map_msg.info.width, g_valid_image_width);
+    ASSERT_EQ(map_msg.info.height, g_valid_image_height);
+    for (unsigned int i = 0; i < map_msg.info.width * map_msg.info.height; i++) {
+      ASSERT_EQ(g_valid_image_content[i], map_msg.data[i]);
+    }
+  }
+
+  static rclcpp::Node::SharedPtr node_;
+  static std::shared_ptr<nav2_util::LifecycleServiceClient> lifecycle_client_;
+};
+
+
+rclcpp::Node::SharedPtr MapSaverTestFixture::node_ = nullptr;
+std::shared_ptr<nav2_util::LifecycleServiceClient> MapSaverTestFixture::lifecycle_client_ =
+  nullptr;
+
+// Send map saving service request.
+// Load saved map and verify obtained OccupancyGrid.
+TEST_F(MapSaverTestFixture, SaveMap)
+{
+  RCLCPP_INFO(node_->get_logger(), "Testing SaveMap service");
+  auto req = std::make_shared<nav2_msgs::srv::SaveMap::Request>();
+  auto client = node_->create_client<nav2_msgs::srv::SaveMap>(
+    "/map_saver/save_map");
+
+  RCLCPP_INFO(node_->get_logger(), "Waiting for save_map service");
+  ASSERT_TRUE(client->wait_for_service());
+
+  // 1. Send valid save_map serivce request
+  req->map_topic = "map";
+  req->map_url = path(g_tmp_dir) / path(g_valid_map_name);
+  req->image_format = "png";
+  req->map_mode = "trinary";
+  req->free_thresh = g_default_free_thresh;
+  req->occupied_thresh = g_default_occupied_thresh;
+  auto resp = send_request<nav2_msgs::srv::SaveMap>(node_, client, req);
+  ASSERT_EQ(resp->result, true);
+
+  // 2. Load saved map and verify it
+  nav_msgs::msg::OccupancyGrid map_msg;
+  LOAD_MAP_STATUS status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg);
+  ASSERT_EQ(status, LOAD_MAP_SUCCESS);
+  verifyMapMsg(map_msg);
+}
+
+// Send map saving service request with default parameters.
+// Load saved map and verify obtained OccupancyGrid.
+TEST_F(MapSaverTestFixture, SaveMapDefaultParameters)
+{
+  RCLCPP_INFO(node_->get_logger(), "Testing SaveMap service");
+  auto req = std::make_shared<nav2_msgs::srv::SaveMap::Request>();
+  auto client = node_->create_client<nav2_msgs::srv::SaveMap>(
+    "/map_saver/save_map");
+
+  RCLCPP_INFO(node_->get_logger(), "Waiting for save_map service");
+  ASSERT_TRUE(client->wait_for_service());
+
+  // 1. Send save_map serivce request with default parameters
+  req->map_topic = "";
+  req->map_url = path(g_tmp_dir) / path(g_valid_map_name);
+  req->image_format = "";
+  req->map_mode = "";
+  req->free_thresh = 0.0;
+  req->occupied_thresh = 0.0;
+  auto resp = send_request<nav2_msgs::srv::SaveMap>(node_, client, req);
+  ASSERT_EQ(resp->result, true);
+
+  // 2. Load saved map and verify it
+  nav_msgs::msg::OccupancyGrid map_msg;
+  LOAD_MAP_STATUS status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg);
+  ASSERT_EQ(status, LOAD_MAP_SUCCESS);
+  verifyMapMsg(map_msg);
+}
+
+// Send map saving service requests with different sets of parameters.
+// In case of map is expected to be saved correctly, load map from a saved
+// file and verify obtained OccupancyGrid.
+TEST_F(MapSaverTestFixture, SaveMapInvalidParameters)
+{
+  RCLCPP_INFO(node_->get_logger(), "Testing SaveMap service");
+  auto req = std::make_shared<nav2_msgs::srv::SaveMap::Request>();
+  auto client = node_->create_client<nav2_msgs::srv::SaveMap>(
+    "/map_saver/save_map");
+
+  RCLCPP_INFO(node_->get_logger(), "Waiting for save_map service");
+  ASSERT_TRUE(client->wait_for_service());
+
+  // 1. Trying to send save_map serivce request with different sets of parameters
+  // In case of map is expected to be saved correctly, verify it
+  req->map_topic = "invalid_map";
+  req->map_url = path(g_tmp_dir) / path(g_valid_map_name);
+  req->image_format = "png";
+  req->map_mode = "trinary";
+  req->free_thresh = g_default_free_thresh;
+  req->occupied_thresh = g_default_occupied_thresh;
+  auto resp = send_request<nav2_msgs::srv::SaveMap>(node_, client, req);
+  ASSERT_EQ(resp->result, false);
+
+  req->map_topic = "map";
+  req->image_format = "invalid_format";
+  resp = send_request<nav2_msgs::srv::SaveMap>(node_, client, req);
+  ASSERT_EQ(resp->result, true);
+  nav_msgs::msg::OccupancyGrid map_msg;
+  LOAD_MAP_STATUS status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg);
+  ASSERT_EQ(status, LOAD_MAP_SUCCESS);
+  verifyMapMsg(map_msg);
+
+  req->image_format = "png";
+  req->map_mode = "invalid_mode";
+  resp = send_request<nav2_msgs::srv::SaveMap>(node_, client, req);
+  ASSERT_EQ(resp->result, true);
+  status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg);
+  ASSERT_EQ(status, LOAD_MAP_SUCCESS);
+  verifyMapMsg(map_msg);
+
+  req->map_mode = "trinary";
+  req->free_thresh = 2.0;
+  req->occupied_thresh = 2.0;
+  resp = send_request<nav2_msgs::srv::SaveMap>(node_, client, req);
+  ASSERT_EQ(resp->result, false);
+
+  req->free_thresh = -2.0;
+  req->occupied_thresh = -2.0;
+  resp = send_request<nav2_msgs::srv::SaveMap>(node_, client, req);
+  ASSERT_EQ(resp->result, false);
+
+  req->free_thresh = 0.7;
+  req->occupied_thresh = 0.2;
+  resp = send_request<nav2_msgs::srv::SaveMap>(node_, client, req);
+  ASSERT_EQ(resp->result, false);
+}

navigation2/nav2_map_server/test/component/test_map_saver_publisher.cpp

@@ -0,0 +1,59 @@
+// 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 <filesystem>
+#include <string>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_map_server/map_io.hpp"
+#include "test_constants/test_constants.h"
+
+#define TEST_DIR TEST_DIRECTORY
+
+using namespace nav2_map_server;  // NOLINT
+using std::filesystem::path;
+
+class TestPublisher : public rclcpp::Node
+{
+public:
+  TestPublisher()
+  : Node("map_publisher")
+  {
+    std::string pub_map_file = path(TEST_DIR) / path(g_valid_yaml_file);
+    nav_msgs::msg::OccupancyGrid msg;
+    LOAD_MAP_STATUS status = loadMapFromYaml(pub_map_file, msg);
+    if (status != LOAD_MAP_SUCCESS) {
+      RCLCPP_ERROR(get_logger(), "Can not load %s map file", pub_map_file.c_str());
+      return;
+    }
+
+    map_pub_ = create_publisher<nav_msgs::msg::OccupancyGrid>(
+      "map",
+      rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
+    map_pub_->publish(msg);
+  }
+
+protected:
+  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_pub_;
+};
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto pub_node = std::make_shared<TestPublisher>();
+  rclcpp::spin(pub_node);
+  rclcpp::shutdown();
+  return 0;
+}

navigation2/nav2_map_server/test/component/test_map_server_launch.py

@@ -0,0 +1,47 @@
+#!/usr/bin/env python3
+
+# 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.
+
+import os
+import sys
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import ExecuteProcess
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_testing.legacy import LaunchTestService
+
+
+def main(argv=sys.argv[1:]):
+    launchFile = os.path.join(os.getenv('TEST_LAUNCH_DIR'), 'map_server_node.launch.py')
+    testExecutable = os.getenv('TEST_EXECUTABLE')
+    ld = LaunchDescription([
+        IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])),
+    ])
+    test1_action = ExecuteProcess(
+        cmd=[testExecutable],
+        name='test_map_server_node',
+    )
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    os.chdir(os.getenv('TEST_LAUNCH_DIR'))
+    return lts.run(ls)
+
+
+if __name__ == '__main__':
+    sys.exit(main())

navigation2/nav2_map_server/test/component/test_map_server_node.cpp

@@ -0,0 +1,237 @@
+// 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 <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+#include <filesystem>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "test_constants/test_constants.h"
+#include "nav2_map_server/map_server.hpp"
+#include "nav2_util/lifecycle_service_client.hpp"
+#include "nav2_msgs/srv/load_map.hpp"
+using namespace std::chrono_literals;
+using namespace rclcpp;  // NOLINT
+
+#define TEST_DIR TEST_DIRECTORY
+
+using std::filesystem::path;
+
+using lifecycle_msgs::msg::Transition;
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+
+RclCppFixture g_rclcppfixture;
+
+class MapServerTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = rclcpp::Node::make_shared("map_client_test");
+    lifecycle_client_ =
+      std::make_shared<nav2_util::LifecycleServiceClient>("map_server", node_);
+    RCLCPP_INFO(node_->get_logger(), "Creating Test Node");
+
+    std::this_thread::sleep_for(std::chrono::seconds(5));  // allow node to start up
+    const std::chrono::seconds timeout(5);
+    lifecycle_client_->change_state(Transition::TRANSITION_CONFIGURE, timeout);
+    lifecycle_client_->change_state(Transition::TRANSITION_ACTIVATE, timeout);
+  }
+
+  static void TearDownTestCase()
+  {
+    lifecycle_client_->change_state(Transition::TRANSITION_DEACTIVATE);
+    lifecycle_client_->change_state(Transition::TRANSITION_CLEANUP);
+    lifecycle_client_->change_state(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN);
+    lifecycle_client_.reset();
+    node_.reset();
+  }
+
+  template<class T>
+  typename T::Response::SharedPtr send_request(
+    rclcpp::Node::SharedPtr node,
+    typename rclcpp::Client<T>::SharedPtr client,
+    typename T::Request::SharedPtr request)
+  {
+    auto result = client->async_send_request(request);
+
+    // Wait for the result
+    if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) {
+      return result.get();
+    } else {
+      return nullptr;
+    }
+  }
+
+protected:
+  // Check that map_msg corresponds to reference pattern
+  // Input: map_msg
+  void verifyMapMsg(const nav_msgs::msg::OccupancyGrid & map_msg)
+  {
+    ASSERT_FLOAT_EQ(map_msg.info.resolution, g_valid_image_res);
+    ASSERT_EQ(map_msg.info.width, g_valid_image_width);
+    ASSERT_EQ(map_msg.info.height, g_valid_image_height);
+    for (unsigned int i = 0; i < map_msg.info.width * map_msg.info.height; i++) {
+      ASSERT_EQ(g_valid_image_content[i], map_msg.data[i]);
+    }
+  }
+
+  static rclcpp::Node::SharedPtr node_;
+  static std::shared_ptr<nav2_util::LifecycleServiceClient> lifecycle_client_;
+};
+
+
+rclcpp::Node::SharedPtr MapServerTestFixture::node_ = nullptr;
+std::shared_ptr<nav2_util::LifecycleServiceClient> MapServerTestFixture::lifecycle_client_ =
+  nullptr;
+
+
+// Send map getting service request and verify obtained OccupancyGrid
+TEST_F(MapServerTestFixture, GetMap)
+{
+  RCLCPP_INFO(node_->get_logger(), "Testing GetMap service");
+  auto req = std::make_shared<nav_msgs::srv::GetMap::Request>();
+  auto client = node_->create_client<nav_msgs::srv::GetMap>(
+    "/map_server/map");
+
+  RCLCPP_INFO(node_->get_logger(), "Waiting for map service");
+  ASSERT_TRUE(client->wait_for_service());
+
+  auto resp = send_request<nav_msgs::srv::GetMap>(node_, client, req);
+
+  verifyMapMsg(resp->map);
+}
+
+// Send map loading service request and verify obtained OccupancyGrid
+TEST_F(MapServerTestFixture, LoadMap)
+{
+  RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service");
+  auto req = std::make_shared<nav2_msgs::srv::LoadMap::Request>();
+  auto client = node_->create_client<nav2_msgs::srv::LoadMap>(
+    "/map_server/load_map");
+
+  RCLCPP_INFO(node_->get_logger(), "Waiting for load_map service");
+  ASSERT_TRUE(client->wait_for_service());
+
+  req->map_url = path(TEST_DIR) / path(g_valid_yaml_file);
+  auto resp = send_request<nav2_msgs::srv::LoadMap>(node_, client, req);
+
+  ASSERT_EQ(resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS);
+  verifyMapMsg(resp->map);
+}
+
+// Send map loading service request without specifying which map to load
+TEST_F(MapServerTestFixture, LoadMapNull)
+{
+  RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service");
+  auto req = std::make_shared<nav2_msgs::srv::LoadMap::Request>();
+  auto client = node_->create_client<nav2_msgs::srv::LoadMap>(
+    "/map_server/load_map");
+
+  RCLCPP_INFO(node_->get_logger(), "Waiting for load_map service");
+  ASSERT_TRUE(client->wait_for_service());
+
+  req->map_url = "";
+  RCLCPP_INFO(node_->get_logger(), "Sending load_map request with null file name");
+  auto resp = send_request<nav2_msgs::srv::LoadMap>(node_, client, req);
+
+  ASSERT_EQ(resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_MAP_DOES_NOT_EXIST);
+}
+
+// Send map loading service request with non-existing yaml file
+TEST_F(MapServerTestFixture, LoadMapInvalidYaml)
+{
+  RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service");
+  auto req = std::make_shared<nav2_msgs::srv::LoadMap::Request>();
+  auto client = node_->create_client<nav2_msgs::srv::LoadMap>(
+    "/map_server/load_map");
+
+  RCLCPP_INFO(node_->get_logger(), "Waiting for load_map service");
+  ASSERT_TRUE(client->wait_for_service());
+
+  req->map_url = "invalid_file.yaml";
+  RCLCPP_INFO(node_->get_logger(), "Sending load_map request with invalid yaml file name");
+  auto resp = send_request<nav2_msgs::srv::LoadMap>(node_, client, req);
+
+  ASSERT_EQ(resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_METADATA);
+}
+
+// Send map loading service request with yaml file containing non-existing map
+TEST_F(MapServerTestFixture, LoadMapInvalidImage)
+{
+  RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service");
+  auto req = std::make_shared<nav2_msgs::srv::LoadMap::Request>();
+  auto client = node_->create_client<nav2_msgs::srv::LoadMap>(
+    "/map_server/load_map");
+
+  RCLCPP_INFO(node_->get_logger(), "Waiting for load_map service");
+  ASSERT_TRUE(client->wait_for_service());
+
+  req->map_url = path(TEST_DIR) / "invalid_image.yaml";
+  RCLCPP_INFO(node_->get_logger(), "Sending load_map request with invalid image file name");
+  auto resp = send_request<nav2_msgs::srv::LoadMap>(node_, client, req);
+
+  ASSERT_EQ(resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_DATA);
+}
+
+/**
+ * Test behaviour of server if yaml_filename is set to an empty string.
+ */
+TEST_F(MapServerTestFixture, NoInitialMap)
+{
+  // turn off node into unconfigured state
+  lifecycle_client_->change_state(Transition::TRANSITION_DEACTIVATE);
+  lifecycle_client_->change_state(Transition::TRANSITION_CLEANUP);
+
+  auto client = node_->create_client<nav_msgs::srv::GetMap>("/map_server/map");
+  auto req = std::make_shared<nav_msgs::srv::GetMap::Request>();
+
+  auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(node_, "map_server");
+  ASSERT_TRUE(parameters_client->wait_for_service(3s));
+
+  // set yaml_filename-parameter to empty string (essentially restart the node)
+  RCLCPP_INFO(node_->get_logger(), "Removing yaml_filename-parameter before restarting");
+  parameters_client->set_parameters({Parameter("yaml_filename", ParameterValue(""))});
+
+  // only configure node, to test behaviour of service while node is not active
+  lifecycle_client_->change_state(Transition::TRANSITION_CONFIGURE, 3s);
+
+  RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service while not being active");
+  auto load_map_req = std::make_shared<nav2_msgs::srv::LoadMap::Request>();
+  auto load_map_cl = node_->create_client<nav2_msgs::srv::LoadMap>("/map_server/load_map");
+
+  ASSERT_TRUE(load_map_cl->wait_for_service(3s));
+  auto resp = send_request<nav2_msgs::srv::LoadMap>(node_, load_map_cl, load_map_req);
+
+  ASSERT_EQ(resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_UNDEFINED_FAILURE);
+
+  // activate server and load map:
+  lifecycle_client_->change_state(Transition::TRANSITION_ACTIVATE, 3s);
+  RCLCPP_INFO(node_->get_logger(), "active again");
+
+  load_map_req->map_url = path(TEST_DIR) / path(g_valid_yaml_file);
+  auto load_res = send_request<nav2_msgs::srv::LoadMap>(node_, load_map_cl, load_map_req);
+
+  ASSERT_EQ(load_res->result, nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS);
+  verifyMapMsg(load_res->map);
+}

navigation2/nav2_map_server/test/invalid_image.yaml

@@ -0,0 +1,7 @@
+image: "invalid.png"
+resolution: 0.1
+origin: [2.0, 3.0, 1.0]
+occupied_thresh: 0.65
+free_thresh: 0.196
+negate: 0
+mode: "trinary"

navigation2/nav2_map_server/test/map_saver_cli/CMakeLists.txt

@@ -0,0 +1,12 @@
+include_directories(${PROJECT_SOURCE_DIR}/test)
+
+# map_saver CLI
+ament_add_gtest(test_map_saver_cli
+  test_map_saver_cli.cpp
+  ${PROJECT_SOURCE_DIR}/test/test_constants.cpp
+)
+
+ament_target_dependencies(test_map_saver_cli rclcpp nav_msgs)
+target_link_libraries(test_map_saver_cli
+  ${dependencies}
+)

navigation2/nav2_map_server/test/map_saver_cli/test_map_saver_cli.cpp

@@ -0,0 +1,146 @@
+// Copyright (c) 2020 Samsung Research America
+//
+// 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 <gtest/gtest.h>
+#include <filesystem>
+#include <string>
+#include <memory>
+#include <utility>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+
+TEST(MapSaverCLI, CLITest)
+{
+  std::string path = "/tmp/";
+  std::string file = "test_map";
+  std::string file_path = path + file;
+
+  rclcpp::init(0, nullptr);
+
+  auto node = std::make_shared<rclcpp::Node>("CLI_Test_Node");
+  RCLCPP_INFO(node->get_logger(), "Testing Map Saver CLI");
+
+  auto publisher = node->create_publisher<nav_msgs::msg::OccupancyGrid>(
+    "/map",
+    rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
+
+  auto msg = std::make_unique<nav_msgs::msg::OccupancyGrid>();
+  msg->header.frame_id = "map";
+  msg->header.stamp = node->now();
+  msg->info.map_load_time = node->now();
+  msg->info.resolution = 0.05;
+  msg->info.width = 3;
+  msg->info.height = 3;
+  msg->info.origin.position.x = 0.0;
+  msg->info.origin.position.y = 0.0;
+  msg->info.origin.orientation.w = 1.0;
+  msg->data.resize(9);
+  msg->data[0] = 0;
+  msg->data[2] = 100;
+  msg->data[1] = 101;
+  msg->data[3] = 50;
+
+  RCLCPP_INFO(node->get_logger(), "Publishing occupancy grid...");
+
+  publisher->publish(std::move(msg));
+
+  rclcpp::Rate(1).sleep();
+
+  // succeed on real map
+  RCLCPP_INFO(node->get_logger(), "Calling saver...");
+
+  EXPECT_FALSE(std::filesystem::exists(file_path + ".yaml"));
+
+  std::string command =
+    std::string(
+    "ros2 run nav2_map_server map_saver_cli -f ") + file_path;
+  auto return_code = system(command.c_str());
+  EXPECT_EQ(return_code, 0);
+
+  rclcpp::Rate(0.25).sleep();
+
+  RCLCPP_INFO(node->get_logger(), "Checking on file...");
+
+  EXPECT_TRUE(std::filesystem::exists(file_path + ".pgm"));
+  EXPECT_EQ(std::filesystem::file_size(file_path + ".pgm"), 20ul);
+
+  if (std::filesystem::exists(file_path + ".yaml")) {
+    std::filesystem::remove(file_path + ".yaml");
+  }
+  if (std::filesystem::exists(file_path + ".pgm")) {
+    std::filesystem::remove(file_path + ".pgm");
+  }
+
+  // fail on bogus map
+  RCLCPP_INFO(node->get_logger(), "Calling saver...");
+
+  EXPECT_FALSE(std::filesystem::exists(file_path + ".yaml"));
+
+  command =
+    std::string(
+    "ros2 run nav2_map_server map_saver_cli "
+    "-t map_failure --occ 100 --free 2 --mode trinary --fmt png -f ") + file_path +
+    std::string("--ros-args __node:=map_saver_test_node");
+  return_code = system(command.c_str());
+  EXPECT_EQ(return_code, 65280);
+
+  rclcpp::Rate(0.25).sleep();
+
+  RCLCPP_INFO(node->get_logger(), "Checking on file...");
+
+  EXPECT_FALSE(std::filesystem::exists(file_path + ".yaml"));
+
+  RCLCPP_INFO(node->get_logger(), "Testing help...");
+  command =
+    std::string(
+    "ros2 run nav2_map_server map_saver_cli -h");
+  return_code = system(command.c_str());
+  EXPECT_EQ(return_code, 0);
+
+  rclcpp::Rate(0.5).sleep();
+
+  RCLCPP_INFO(node->get_logger(), "Testing invalid mode...");
+  command =
+    std::string(
+    "ros2 run nav2_map_server map_saver_cli --mode fake_mode");
+  return_code = system(command.c_str());
+  EXPECT_EQ(return_code, 0);
+
+  rclcpp::Rate(0.5).sleep();
+
+  RCLCPP_INFO(node->get_logger(), "Testing missing argument...");
+  command =
+    std::string(
+    "ros2 run nav2_map_server map_saver_cli --mode");
+  return_code = system(command.c_str());
+  EXPECT_EQ(return_code, 65280);
+
+  rclcpp::Rate(0.5).sleep();
+
+  RCLCPP_INFO(node->get_logger(), "Testing wrong argument...");
+  command =
+    std::string(
+    "ros2 run nav2_map_server map_saver_cli --free 0 0");
+  return_code = system(command.c_str());
+  EXPECT_EQ(return_code, 65280);
+
+  rclcpp::Rate(0.5).sleep();
+
+  command =
+    std::string(
+    "ros2 run nav2_map_server map_saver_cli --ros-args -r __node:=map_saver_test_node");
+  return_code = system(command.c_str());
+  EXPECT_EQ(return_code, 0);
+}

navigation2/nav2_map_server/test/map_saver_params.yaml

@@ -0,0 +1,5 @@
+map_saver:
+    ros__parameters:
+        save_map_timeout: 5.0
+        free_thresh_default: 0.196
+        occupied_thresh_default: 0.65

navigation2/nav2_map_server/test/map_server_params.yaml

@@ -0,0 +1,3 @@
+map_server:
+    ros__parameters:
+        yaml_filename: "../testmap.yaml"

navigation2/nav2_map_server/test/test_constants.cpp

@@ -0,0 +1,70 @@
+/*
+ * 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.
+ */
+
+/* Author: Brian Gerkey */
+
+/* This file contains global constants shared among tests */
+
+/* Note that these must be changed if the test image changes */
+
+#include "test_constants/test_constants.h"
+
+#include <vector>
+
+const unsigned int g_valid_image_width = 10;
+const unsigned int g_valid_image_height = 10;
+// Note that the image content is given in row-major order, with the
+// lower-left pixel first.  This is different from a graphics coordinate
+// system, which starts with the upper-left pixel.  The loadMapFromFile
+// call converts from the latter to the former when it loads the image, and
+// we want to compare against the result of that conversion.
+const char g_valid_image_content[] = {
+  0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+  100, 100, 100, 100, 0, 0, 100, 100, 100, 0,
+  100, 100, 100, 100, 0, 0, 100, 100, 100, 0,
+  100, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+  100, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+  100, 0, 0, 0, 0, 0, 100, 100, 0, 0,
+  100, 0, 0, 0, 0, 0, 100, 100, 0, 0,
+  100, 0, 0, 0, 0, 0, 100, 100, 0, 0,
+  100, 0, 0, 0, 0, 0, 100, 100, 0, 0,
+  100, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+};
+
+const char * g_valid_map_name = "testmap";
+const char * g_valid_png_file = "testmap.png";
+const char * g_valid_bmp_file = "testmap.bmp";
+const char * g_valid_pgm_file = "testmap.pgm";
+const char * g_valid_yaml_file = "testmap.yaml";
+const char * g_tmp_dir = "/tmp";
+
+const double g_valid_image_res = 0.1;
+const std::vector<double> g_valid_origin{2.0, 3.0, 1.0};
+const double g_default_free_thresh = 0.196;
+const double g_default_occupied_thresh = 0.65;

navigation2/nav2_map_server/test/test_constants/test_constants.h

@@ -0,0 +1,56 @@
+/*
+ * 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.
+ */
+#ifndef TEST_CONSTANTS__TEST_CONSTANTS_H_
+#define TEST_CONSTANTS__TEST_CONSTANTS_H_
+
+/* Author: Brian Gerkey */
+
+/* This file externs global constants shared among tests */
+
+#include <vector>
+
+extern const unsigned int g_valid_image_width;
+extern const unsigned int g_valid_image_height;
+extern const char g_valid_image_content[];
+extern const char * g_valid_map_name;
+extern const char * g_valid_png_file;
+extern const char * g_valid_bmp_file;
+extern const char * g_valid_pgm_file;
+extern const char * g_valid_yaml_file;
+extern const char * g_tmp_dir;
+
+extern const double g_valid_image_res;
+// *INDENT-OFF*
+// Uncrustify may incorrectly guide to add extra spaces in < double > during CI tests
+extern const std::vector<double> g_valid_origin;
+// *INDENT-ON*
+extern const double g_default_free_thresh;
+extern const double g_default_occupied_thresh;
+
+#endif  // TEST_CONSTANTS__TEST_CONSTANTS_H_

navigation2/nav2_map_server/test/test_launch_files/map_saver_node.launch.py

@@ -0,0 +1,42 @@
+#!/usr/bin/env python3
+
+# 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.
+
+import os
+
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess
+import launch_ros.actions
+
+
+def generate_launch_description():
+    map_publisher = f"{os.path.dirname(os.getenv('TEST_EXECUTABLE'))}/test_map_saver_publisher"
+
+    ld = LaunchDescription()
+
+    map_saver_server_cmd = launch_ros.actions.Node(
+        package='nav2_map_server',
+        executable='map_saver_server',
+        output='screen',
+        parameters=[os.path.join(os.getenv('TEST_DIR'),
+                    'map_saver_params.yaml')])
+
+    map_publisher_cmd = ExecuteProcess(
+        cmd=[map_publisher])
+
+    ld.add_action(map_saver_server_cmd)
+    ld.add_action(map_publisher_cmd)
+
+    return ld

navigation2/nav2_map_server/test/test_launch_files/map_server_node.launch.py

@@ -0,0 +1,31 @@
+#!/usr/bin/env python3
+
+# 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.
+
+import os
+
+from launch import LaunchDescription
+import launch_ros.actions
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        launch_ros.actions.Node(
+            package='nav2_map_server',
+            executable='map_server',
+            output='screen',
+            parameters=[os.path.join(os.getenv('TEST_DIR'),
+                        'map_server_params.yaml')])
+    ])

navigation2/nav2_map_server/test/testmap.yaml

@@ -0,0 +1,7 @@
+image: "testmap.png"
+resolution: 0.1
+origin: [2.0, 3.0, 1.0]
+occupied_thresh: 0.65
+free_thresh: 0.196
+negate: 0
+mode: "trinary"

navigation2/nav2_map_server/test/unit/CMakeLists.txt

@@ -0,0 +1,21 @@
+include_directories(${PROJECT_SOURCE_DIR}/test)
+
+# map_io unit test
+ament_add_gtest(test_map_io test_map_io.cpp ${PROJECT_SOURCE_DIR}/test/test_constants.cpp)
+
+ament_target_dependencies(test_map_io rclcpp nav_msgs)
+
+target_link_libraries(test_map_io
+  ${map_io_library_name}
+)
+
+# costmap_filter_info_server unit test
+ament_add_gtest(test_costmap_filter_info_server
+  test_costmap_filter_info_server.cpp
+)
+
+ament_target_dependencies(test_costmap_filter_info_server rclcpp)
+
+target_link_libraries(test_costmap_filter_info_server
+  ${library_name}
+)

navigation2/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp

@@ -0,0 +1,177 @@
+// 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 <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+#include <chrono>
+#include <limits>
+#include <mutex>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "nav2_map_server/costmap_filter_info_server.hpp"
+
+using namespace std::chrono_literals;
+
+typedef std::recursive_mutex mutex_t;
+
+static const char FILTER_INFO_TOPIC[] = "filter_info";
+static const int TYPE = 1;
+static const char MASK_TOPIC[] = "mask";
+static const double BASE = 0.1;
+static const double MULTIPLIER = 0.2;
+
+static const double EPSILON = std::numeric_limits<float>::epsilon();
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+class InfoServerWrapper : public nav2_map_server::CostmapFilterInfoServer
+{
+public:
+  void start()
+  {
+    on_configure(get_current_state());
+    on_activate(get_current_state());
+  }
+
+  void stop()
+  {
+    on_deactivate(get_current_state());
+    on_cleanup(get_current_state());
+    on_shutdown(get_current_state());
+  }
+
+  void deactivate()
+  {
+    on_deactivate(get_current_state());
+  }
+
+  void activate()
+  {
+    on_activate(get_current_state());
+  }
+};
+
+class InfoServerTester : public ::testing::Test
+{
+public:
+  InfoServerTester()
+  : info_server_(nullptr), info_(nullptr), subscription_(nullptr)
+  {
+    access_ = new mutex_t();
+
+    info_server_ = std::make_shared<InfoServerWrapper>();
+    try {
+      info_server_->set_parameter(rclcpp::Parameter("filter_info_topic", FILTER_INFO_TOPIC));
+      info_server_->set_parameter(rclcpp::Parameter("type", TYPE));
+      info_server_->set_parameter(rclcpp::Parameter("mask_topic", MASK_TOPIC));
+      info_server_->set_parameter(rclcpp::Parameter("base", BASE));
+      info_server_->set_parameter(rclcpp::Parameter("multiplier", MULTIPLIER));
+    } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) {
+      RCLCPP_ERROR(
+        info_server_->get_logger(),
+        "Error while setting parameters for CostmapFilterInfoServer: %s", ex.what());
+      throw;
+    }
+
+    info_server_->start();
+
+    subscription_ = info_server_->create_subscription<nav2_msgs::msg::CostmapFilterInfo>(
+      FILTER_INFO_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
+      std::bind(&InfoServerTester::infoCallback, this, std::placeholders::_1));
+  }
+
+  ~InfoServerTester()
+  {
+    info_server_->stop();
+    info_server_.reset();
+    subscription_.reset();
+  }
+
+  bool isReceived()
+  {
+    std::lock_guard<mutex_t> guard(*getMutex());
+    if (info_) {
+      return true;
+    } else {
+      return false;
+    }
+  }
+
+  mutex_t * getMutex()
+  {
+    return access_;
+  }
+
+protected:
+  std::shared_ptr<InfoServerWrapper> info_server_;
+  nav2_msgs::msg::CostmapFilterInfo::SharedPtr info_;
+
+private:
+  void infoCallback(const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg)
+  {
+    std::lock_guard<mutex_t> guard(*getMutex());
+    info_ = msg;
+  }
+
+  rclcpp::Subscription<nav2_msgs::msg::CostmapFilterInfo>::SharedPtr subscription_;
+
+  mutex_t * access_;
+};
+
+TEST_F(InfoServerTester, testCostmapFilterInfoPublish)
+{
+  rclcpp::Time start_time = info_server_->now();
+  while (!isReceived()) {
+    rclcpp::spin_some(info_server_->get_node_base_interface());
+    std::this_thread::sleep_for(100ms);
+    // Waiting no more than 5 seconds
+    ASSERT_TRUE((info_server_->now() - start_time) <= rclcpp::Duration(5000ms));
+  }
+
+  // Checking received CostmapFilterInfo for consistency
+  EXPECT_EQ(info_->type, TYPE);
+  EXPECT_EQ(info_->filter_mask_topic, MASK_TOPIC);
+  EXPECT_NEAR(info_->base, BASE, EPSILON);
+  EXPECT_NEAR(info_->multiplier, MULTIPLIER, EPSILON);
+}
+
+TEST_F(InfoServerTester, testCostmapFilterInfoDeactivateActivate)
+{
+  info_server_->deactivate();
+  info_ = nullptr;
+  info_server_->activate();
+
+  rclcpp::Time start_time = info_server_->now();
+  while (!isReceived()) {
+    rclcpp::spin_some(info_server_->get_node_base_interface());
+    std::this_thread::sleep_for(100ms);
+    // Waiting no more than 5 seconds
+    ASSERT_TRUE((info_server_->now() - start_time) <= rclcpp::Duration(5000ms));
+  }
+
+  // Checking received CostmapFilterInfo for consistency
+  EXPECT_EQ(info_->type, TYPE);
+  EXPECT_EQ(info_->filter_mask_topic, MASK_TOPIC);
+  EXPECT_NEAR(info_->base, BASE, EPSILON);
+  EXPECT_NEAR(info_->multiplier, MULTIPLIER, EPSILON);
+}

navigation2/nav2_map_server/test/unit/test_map_io.cpp

@@ -0,0 +1,340 @@
+// 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 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.
+ */
+
+/* Author: Brian Gerkey */
+
+#include <gtest/gtest.h>
+#include <filesystem>
+#include <stdexcept>
+#include <string>
+#include <vector>
+#include <memory>
+#include <iostream>
+#include <fstream>
+
+#include "yaml-cpp/yaml.h"
+#include "nav2_map_server/map_io.hpp"
+#include "nav2_map_server/map_server.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "test_constants/test_constants.h"
+
+#define TEST_DIR TEST_DIRECTORY
+
+using namespace std;  // NOLINT
+using namespace nav2_map_server;  // NOLINT
+using std::filesystem::path;
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+
+RclCppFixture g_rclcppfixture;
+
+class MapIOTester : public ::testing::Test
+{
+protected:
+  // Fill LoadParameters with standard for testing values
+  // Input: image_file_name
+  // Output: load_parameters
+  void fillLoadParameters(
+    const std::string & image_file_name,
+    LoadParameters & load_parameters)
+  {
+    load_parameters.image_file_name = image_file_name;
+    load_parameters.resolution = g_valid_image_res;
+    load_parameters.origin = g_valid_origin;
+    load_parameters.free_thresh = g_default_free_thresh;
+    load_parameters.occupied_thresh = g_default_occupied_thresh;
+    load_parameters.mode = MapMode::Trinary;
+    load_parameters.negate = 0;
+  }
+
+  // Fill SaveParameters with standard for testing values
+  // Input: map_file_name, image_format
+  // Output: save_parameters
+  void fillSaveParameters(
+    const std::string & map_file_name,
+    const std::string & image_format,
+    SaveParameters & save_parameters)
+  {
+    save_parameters.map_file_name = map_file_name;
+    save_parameters.image_format = image_format;
+    save_parameters.free_thresh = g_default_free_thresh;
+    save_parameters.occupied_thresh = g_default_occupied_thresh;
+    save_parameters.mode = MapMode::Trinary;
+  }
+
+  // Check that map_msg corresponds to reference pattern
+  // Input: map_msg
+  void verifyMapMsg(const nav_msgs::msg::OccupancyGrid & map_msg)
+  {
+    ASSERT_FLOAT_EQ(map_msg.info.resolution, g_valid_image_res);
+    ASSERT_EQ(map_msg.info.width, g_valid_image_width);
+    ASSERT_EQ(map_msg.info.height, g_valid_image_height);
+    for (unsigned int i = 0; i < map_msg.info.width * map_msg.info.height; i++) {
+      ASSERT_EQ(g_valid_image_content[i], map_msg.data[i]);
+    }
+  }
+};
+
+// Load a valid reference PGM file. Check obtained OccupancyGrid message for consistency:
+// loaded image should match the known dimensions and content of the file.
+// Save obtained OccupancyGrid message into a tmp PGM file. Then load back saved tmp file
+// and check for consistency.
+// Succeeds all steps were passed without a problem or expection.
+TEST_F(MapIOTester, loadSaveValidPGM)
+{
+  // 1. Load reference map file and verify obtained OccupancyGrid
+  LoadParameters loadParameters;
+  fillLoadParameters(path(TEST_DIR) / path(g_valid_pgm_file), loadParameters);
+
+  nav_msgs::msg::OccupancyGrid map_msg;
+  ASSERT_NO_THROW(loadMapFromFile(loadParameters, map_msg));
+
+  verifyMapMsg(map_msg);
+
+  // 2. Save OccupancyGrid into a tmp file
+  SaveParameters saveParameters;
+  fillSaveParameters(path(g_tmp_dir) / path(g_valid_map_name), "pgm", saveParameters);
+
+  ASSERT_TRUE(saveMapToFile(map_msg, saveParameters));
+
+  // 3. Load saved map and verify it
+  LOAD_MAP_STATUS status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg);
+  ASSERT_EQ(status, LOAD_MAP_SUCCESS);
+
+  verifyMapMsg(map_msg);
+}
+
+// Load a valid reference PNG file. Check obtained OccupancyGrid message for consistency:
+// loaded image should match the known dimensions and content of the file.
+// Save obtained OccupancyGrid message into a tmp PNG file. Then load back saved tmp file
+// and check for consistency.
+// Succeeds all steps were passed without a problem or expection.
+TEST_F(MapIOTester, loadSaveValidPNG)
+{
+  // 1. Load reference map file and verify obtained OccupancyGrid
+  LoadParameters loadParameters;
+  fillLoadParameters(path(TEST_DIR) / path(g_valid_png_file), loadParameters);
+
+  nav_msgs::msg::OccupancyGrid map_msg;
+  ASSERT_NO_THROW(loadMapFromFile(loadParameters, map_msg));
+
+  verifyMapMsg(map_msg);
+
+  // 2. Save OccupancyGrid into a tmp file
+  SaveParameters saveParameters;
+  fillSaveParameters(path(g_tmp_dir) / path(g_valid_map_name), "png", saveParameters);
+
+  ASSERT_TRUE(saveMapToFile(map_msg, saveParameters));
+
+  // 3. Load saved map and verify it
+  LOAD_MAP_STATUS status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg);
+  ASSERT_EQ(status, LOAD_MAP_SUCCESS);
+
+  verifyMapMsg(map_msg);
+}
+
+// Load a valid reference BMP file. Check obtained OccupancyGrid message for consistency:
+// loaded image should match the known dimensions and content of the file.
+// Save obtained OccupancyGrid message into a tmp BMP file. Then load back saved tmp file
+// and check for consistency.
+// Succeeds all steps were passed without a problem or expection.
+TEST_F(MapIOTester, loadSaveValidBMP)
+{
+  // 1. Load reference map file and verify obtained OccupancyGrid
+  auto test_bmp = path(TEST_DIR) / path(g_valid_bmp_file);
+
+  LoadParameters loadParameters;
+  fillLoadParameters(test_bmp, loadParameters);
+
+  nav_msgs::msg::OccupancyGrid map_msg;
+  ASSERT_NO_THROW(loadMapFromFile(loadParameters, map_msg));
+
+  verifyMapMsg(map_msg);
+
+  // 2. Save OccupancyGrid into a tmp file
+  SaveParameters saveParameters;
+  fillSaveParameters(path(g_tmp_dir) / path(g_valid_map_name), "bmp", saveParameters);
+
+  ASSERT_TRUE(saveMapToFile(map_msg, saveParameters));
+
+  // 3. Load saved map and verify it
+  LOAD_MAP_STATUS status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg);
+  ASSERT_EQ(status, LOAD_MAP_SUCCESS);
+
+  verifyMapMsg(map_msg);
+}
+
+// Load map from a valid file. Trying to save map with different modes.
+// Succeeds all steps were passed without a problem or expection.
+TEST_F(MapIOTester, loadSaveMapModes)
+{
+  // 1. Load map from YAML file
+  nav_msgs::msg::OccupancyGrid map_msg;
+  LOAD_MAP_STATUS status = loadMapFromYaml(path(TEST_DIR) / path(g_valid_yaml_file), map_msg);
+  ASSERT_EQ(status, LOAD_MAP_SUCCESS);
+
+  // No need to check Trinary mode. This already verified in previous testcases.
+  // 2. Save map in Scale mode.
+  SaveParameters saveParameters;
+  fillSaveParameters(path(g_tmp_dir) / path(g_valid_map_name), "png", saveParameters);
+  saveParameters.mode = MapMode::Scale;
+
+  ASSERT_TRUE(saveMapToFile(map_msg, saveParameters));
+
+  // 3. Load saved map and verify it
+  status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg);
+  ASSERT_EQ(status, LOAD_MAP_SUCCESS);
+
+  verifyMapMsg(map_msg);
+
+  // 4. Save map in Raw mode.
+  saveParameters.mode = MapMode::Raw;
+
+  ASSERT_TRUE(saveMapToFile(map_msg, saveParameters));
+
+  // 5. Load saved map and verify it
+  status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg);
+  ASSERT_EQ(status, LOAD_MAP_SUCCESS);
+
+  verifyMapMsg(map_msg);
+}
+
+// Try to load an invalid file with different ways.
+// Succeeds if all cases are got expected fail behaviours.
+TEST_F(MapIOTester, loadInvalidFile)
+{
+  // 1. Trying to load incorrect map by loadMapFromFile()
+  auto test_invalid = path(TEST_DIR) / path("foo");
+
+  LoadParameters loadParameters;
+  fillLoadParameters(test_invalid, loadParameters);
+
+  nav_msgs::msg::OccupancyGrid map_msg;
+  ASSERT_ANY_THROW(loadMapFromFile(loadParameters, map_msg));
+
+  // 2. Trying to load incorrect map by loadMapFromYaml()
+  LOAD_MAP_STATUS status = loadMapFromYaml("", map_msg);
+  ASSERT_EQ(status, MAP_DOES_NOT_EXIST);
+
+  status = loadMapFromYaml(std::string(test_invalid) + ".yaml", map_msg);
+  ASSERT_EQ(status, INVALID_MAP_METADATA);
+}
+
+// Load map from a valid file. Trying to save map with different sets of parameters.
+// Succeeds if all cases got expected behaviours.
+TEST_F(MapIOTester, saveInvalidParameters)
+{
+  // 1. Load map from YAML file
+  nav_msgs::msg::OccupancyGrid map_msg;
+  LOAD_MAP_STATUS status = loadMapFromYaml(path(TEST_DIR) / path(g_valid_yaml_file), map_msg);
+  ASSERT_EQ(status, LOAD_MAP_SUCCESS);
+
+  // 2. Trying to save map with different sets of parameters
+  SaveParameters saveParameters;
+
+  saveParameters.map_file_name = path(g_tmp_dir) / path(g_valid_map_name);
+  saveParameters.image_format = "";
+  saveParameters.free_thresh = 2.0;
+  saveParameters.occupied_thresh = 2.0;
+  saveParameters.mode = MapMode::Trinary;
+  ASSERT_FALSE(saveMapToFile(map_msg, saveParameters));
+
+  saveParameters.free_thresh = -2.0;
+  saveParameters.occupied_thresh = -2.0;
+  ASSERT_FALSE(saveMapToFile(map_msg, saveParameters));
+
+  saveParameters.free_thresh = 0.7;
+  saveParameters.occupied_thresh = 0.2;
+  ASSERT_FALSE(saveMapToFile(map_msg, saveParameters));
+
+  saveParameters.free_thresh = 0.0;
+  saveParameters.occupied_thresh = 0.0;
+  ASSERT_TRUE(saveMapToFile(map_msg, saveParameters));
+
+  saveParameters.map_file_name = path("/invalid_path") / path(g_valid_map_name);
+  ASSERT_FALSE(saveMapToFile(map_msg, saveParameters));
+}
+
+// Load valid YAML file and check for consistency
+TEST_F(MapIOTester, loadValidYAML)
+{
+  LoadParameters loadParameters;
+  ASSERT_NO_THROW(loadParameters = loadMapYaml(path(TEST_DIR) / path(g_valid_yaml_file)));
+
+  LoadParameters refLoadParameters;
+  fillLoadParameters(path(TEST_DIR) / path(g_valid_png_file), refLoadParameters);
+  ASSERT_EQ(loadParameters.image_file_name, refLoadParameters.image_file_name);
+  ASSERT_FLOAT_EQ(loadParameters.resolution, refLoadParameters.resolution);
+  ASSERT_EQ(loadParameters.origin, refLoadParameters.origin);
+  ASSERT_FLOAT_EQ(loadParameters.free_thresh, refLoadParameters.free_thresh);
+  ASSERT_FLOAT_EQ(loadParameters.occupied_thresh, refLoadParameters.occupied_thresh);
+  ASSERT_EQ(loadParameters.mode, refLoadParameters.mode);
+  ASSERT_EQ(loadParameters.negate, refLoadParameters.negate);
+}
+
+// Try to load invalid YAML file
+TEST_F(MapIOTester, loadInvalidYAML)
+{
+  LoadParameters loadParameters;
+  ASSERT_ANY_THROW(loadParameters = loadMapYaml(path(TEST_DIR) / path("invalid_file.yaml")));
+}
+
+TEST(HomeUserExpanderTestSuite, homeUserExpanderShouldNotChangeInputStringWhenShorterThanTwo)
+{
+  const std::string emptyFileName{};
+  ASSERT_EQ(emptyFileName, expand_user_home_dir_if_needed(emptyFileName, "/home/user"));
+}
+
+TEST(
+  HomeUserExpanderTestSuite,
+  homeUserExpanderShouldNotChangeInputStringWhenInputStringDoesNotStartWithHomeSequence)
+{
+  const std::string fileName{"valid_file.yaml"};
+  ASSERT_EQ(fileName, expand_user_home_dir_if_needed(fileName, "/home/user"));
+}
+
+TEST(HomeUserExpanderTestSuite, homeUserExpanderShouldNotChangeInputStringWhenHomeVariableNotFound)
+{
+  const std::string fileName{"~/valid_file.yaml"};
+  ASSERT_EQ(fileName, expand_user_home_dir_if_needed(fileName, ""));
+}
+
+TEST(HomeUserExpanderTestSuite, homeUserExpanderShouldExpandHomeSequenceWhenHomeVariableSet)
+{
+  const std::string fileName{"~/valid_file.yaml"};
+  const std::string expectedOutputFileName{"/home/user/valid_file.yaml"};
+  ASSERT_EQ(expectedOutputFileName, expand_user_home_dir_if_needed(fileName, "/home/user"));
+}