map_server refactor and cleanup (#1624)

* [WIP] map_server refactor and cleanup

nav2_map_server/mapio (map input-optput library):
* Move OccupancyGrid messages saving code from MapSaver::mapCallback()
  to saveMapToFile() function
* Rename and move try_write_map_to_file() MapSaver method
  to tryWriteMapToFile() function
* Move map saving parameters into one SaveParameters struct
* Reorganize map saving parameters verification code from MapSaver
  to new checkSaveParameters() function
* Correct logging for incorrect input cases in checkSaveParameters()
* Copy loadMapFromYaml() method from OccupancyGridLoader
* Move loadMapFromFile() method from OccupancyGridLoader
* Rename and move load_map_yaml() OccupancyGrid method
  to loadMapYaml() function
* Move LoadParameters struct from OccupancyGridLoader

nav2_map_server/map_saver:
* Completely re-work MapSaver node:
 - Switch MapSaver from rclcpp::Node to nav2_utils::LifecycleNode
 - Revise MapSaver node parameters model
 - Add saveMapTopicToFile() method for saving map from topic
 - Remove future-promise synchronization model as unnecessary
* Add "save_map" service with new SaveMap service messages
* Rename map_saver_cli.cpp -> map_saver_cli_main.cpp file
  and map_saver -> to map_saver_cli executable
* Add ability to save a map from custom topic ("-t" cli-option)
* Restore support of "--ros-args" remappings
* Update help message in map_saver_cli
* New map_saver_server_main.cpp file and map_saver_server executable:
  continuously running server node
* New launch/map_saver_server.launch.py: map_saver_server launcher

nav2_map_server/map_server:
* Revise MapServer node parameters model
* Rename loadMapFromYaml() -> loadMapResponseFromYaml()
* Add node prefix to "map" and "load_map" service names
* Fix crash: dereferencing nullptr in map_server running as a node
  while handling of incorrect input map name
* Add updateMsgHeader() method for correcting map message header
  when it belongs to instantiated object
* Rename main.cpp -> map_server_main.cpp file
* Minor changes and renames to keep unified code style

nav2_util/map_loader:
* Remove as duplicating of loadMapFromFile() from MapIO library

other:
* Update nav2_map_server/README
* Fix testcases

* Fixes for cpplint, uncrustify, flake8 and test_occ_grid_node failures

* Fixing review comments

* Rename mapio -> map_io
* Move all OccGridLoader functionality into MapServer. Remove OccGridLoader
* Switch all thresholds to be floating-point
* Switch loadMapFromYaml() returning type to LOAD_MAP_STATUS and remove
  duplicating code from loadMapResponseFromYaml()
* Make mapCallback() to be lambda-function
* Make saveMapCallback() to be class method
* Utilize local rclcpp_node_ from LifecycleNode instead of map_listener_.
  Remove map_listener_ and got_map_msg_ variables.
* Rename load_map_callback() -> loadMapCallback() and make it to be class method
* Rename handle_occ_callback() -> getMapCallback() and make it to be class method
* Force saveMapTopicToFile() and saveMapToFile() to work with constant arguments
* map_saver_cli: move arguments parsing code into new parse_arguments() function
* Rename test_occ_grid_node -> test_map_server_node and fix test
* Rename test_occ_grid -> test_occ_grid and fix test
* Fix copyrights
* Fix comments
* Update README

* Increase test coverage

* Fixing review comments

* Separate map_server and map_saver sources
* Fix copyrights
* Suppress false-positive uncrustify failure

nav2_map_server/CMakeLists.txt

@@ -21,21 +21,36 @@ include_directories(include)
 
 set(map_server_executable map_server)
 
-set(map_saver_executable map_saver)
+set(map_saver_cli_executable map_saver_cli)
+
+set(map_saver_server_executable map_saver_server)
 
 add_executable(${map_server_executable}
-  src/main.cpp)
+  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(${map_saver_executable}
-  src/map_saver_cli.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/occ_grid_loader.cpp
-  src/map_server.cpp
-  src/map_saver.cpp
-  src/map_mode.cpp)
+  src/map_server/map_server.cpp
+  src/map_saver/map_saver.cpp)
+
+set(map_io_dependencies
+  yaml_cpp_vendor
+  nav_msgs
+  nav2_util
+  tf2)
 
 set(map_server_dependencies
   rclcpp
@@ -44,43 +59,58 @@ set(map_server_dependencies
   nav2_msgs
   yaml_cpp_vendor
   std_msgs
-  tf2
   nav2_util)
 
 set(map_saver_dependencies
   rclcpp
   nav_msgs
-  tf2)
+  nav2_msgs
+  nav2_util)
 
 ament_target_dependencies(${map_server_executable}
   ${map_server_dependencies})
 
-ament_target_dependencies(${map_saver_executable}
+ament_target_dependencies(${map_saver_cli_executable}
+  ${map_saver_dependencies})
+
+ament_target_dependencies(${map_saver_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})
 
-target_link_libraries(${map_saver_executable}
+target_link_libraries(${map_saver_cli_executable}
+  ${library_name})
+
+target_link_libraries(${map_saver_server_executable}
   ${library_name})
 
-target_include_directories(${library_name} SYSTEM PRIVATE
+target_include_directories(${map_io_library_name} SYSTEM PRIVATE
   ${GRAPHICSMAGICKCPP_INCLUDE_DIRS})
 
-target_link_libraries(${library_name}
+target_link_libraries(${map_io_library_name}
   ${GRAPHICSMAGICKCPP_LIBRARIES})
 
-install(TARGETS ${map_server_executable} ${library_name} ${map_saver_executable}
+install(TARGETS ${map_server_executable} ${library_name} ${map_io_library_name}
+  ${map_saver_cli_executable} ${map_saver_server_executable}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   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
@@ -94,5 +124,6 @@ endif()
 ament_export_include_directories(include)
 ament_export_libraries(
   ${library_name}
+  ${map_io_library_name}
 )
 ament_package()

nav2_map_server/README.md

@@ -8,21 +8,39 @@ service interfaces.
 While the nav2 map server provides the same general function as the nav1 map server, the new
 code has some changes to accomodate ROS2 as well as some architectural improvements.
 
-In addition, there is now a new "load_map" service which can be used to dynamically load a map.
+In addition, there is now two new "load_map" and "save_map" services which can be used to
+dynamically load and save a map.
 
 ### 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. In particular, there is now a `MapLoader` abstract base class
-and type-specific map loaders which derive from this class. There is currently one such
-derived class, the `OccGridLoader`, which converts an input image to an OccupancyGrid and
-makes this available via topic and service interfaces. The `MapServer` class is a ROS2 node
-that uses the appropriate loader, based on an input parameter.
+this new extensible framework.
 
-### Command-line arguments, ROS2 Node Parameters, and YAML files
+Currently map server divides into tree parts:
 
-The Map Server is a composable ROS2 node. By default, there is a map_server executable that
+- `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 serice 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 replaceble 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.
 
@@ -82,16 +100,45 @@ Then, one would invoke this process with the params file that contains the param
 $ process_with_multiple_map_servers __params:=combined_params.yaml
 ```
 
+#### 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), via the OccGridLoader
+
+- 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. See nav2_msgs/srv/LoadMap.srv for details.
+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:
 
-Example:
 ```
-$ ros2 service call /load_map nav2_msgs/srv/LoadMap "{type: 0, map_id: /ros/maps/map.yaml}
+$ 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}"
 ```
 

nav2_map_server/include/nav2_map_server/map_io.hpp

@@ -0,0 +1,103 @@
+// 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);
+
+}  // namespace nav2_map_server
+
+#endif  // NAV2_MAP_SERVER__MAP_IO_HPP_