map_server refactor and cleanup

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_