Rewrite nav2_map_server

Add GraphicsMagick API and use it to implement map_saver, map_loader
Removes dependency on SDL, creates installation dependency on libgraphicsmagick++; apt package libgraphicsmagick++1-dev
Allows saving in trinary, raw, or scale mode.
Added configurable image format pgm, png, etc.
Add warning when using scale mode with opaque format.
Remove SDL from map server
Extract repeated MapMode to shared enum class
Switch writing YAML to same library that reads it (yaml-cpp)
Add --help alias to map saver -h
Extract map yaml loading to its own function, with better error messages. Also fixed crash due to misuse of std::string constructor.

nav2_map_server/CMakeLists.txt

@@ -1,45 +1,40 @@
 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(nav_msgs REQUIRED)
-find_package(SDL REQUIRED)
-find_package(SDL_image 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
-  ${SDL_INCLUDE_DIR}
-  ${SDL_IMAGE_INCLUDE_DIRS}
-)
+include_directories(include)
 
 set(map_server_executable map_server)
 
 set(map_saver_executable map_saver)
 
 add_executable(${map_server_executable}
-  src/main.cpp
-)
+  src/main.cpp)
 
 add_executable(${map_saver_executable}
-  src/map_saver.cpp
-)
+  src/map_saver_cli.cpp)
 
 set(library_name ${map_server_executable}_core)
 
 add_library(${library_name} SHARED
   src/occ_grid_loader.cpp
   src/map_server.cpp
-  src/map_generator.cpp
-)
+  src/map_saver.cpp
+  src/map_mode.cpp)
 
 set(map_server_dependencies
   rclcpp
@@ -48,49 +43,41 @@ set(map_server_dependencies
   yaml_cpp_vendor
   std_msgs
   tf2
-  nav2_util
-)
+  nav2_util)
 
 set(map_saver_dependencies
   rclcpp
   nav_msgs
-  tf2
-)
+  tf2)
 
 ament_target_dependencies(${map_server_executable}
-  ${map_server_dependencies}
-)
+  ${map_server_dependencies})
 
 ament_target_dependencies(${map_saver_executable}
-  ${map_saver_dependencies}
-)
+  ${map_saver_dependencies})
 
 ament_target_dependencies(${library_name}
-  ${map_server_dependencies}
-)
+  ${map_server_dependencies})
 
 target_link_libraries(${map_server_executable}
-  ${library_name}
-)
+  ${library_name})
 
 target_link_libraries(${map_saver_executable}
-  ${library_name}
-)
+  ${library_name})
+
+target_include_directories(${library_name} SYSTEM PRIVATE
+  ${GRAPHICSMAGICKCPP_INCLUDE_DIRS})
 
 target_link_libraries(${library_name}
-  ${SDL_LIBRARY}
-  ${SDL_IMAGE_LIBRARIES}
-)
+  ${GRAPHICSMAGICKCPP_LIBRARIES})
 
 install(TARGETS ${map_server_executable} ${library_name} ${map_saver_executable}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
-  RUNTIME DESTINATION lib/${PROJECT_NAME}
-)
+  RUNTIME DESTINATION lib/${PROJECT_NAME})
 
 install(DIRECTORY include/
-  DESTINATION include/
-)
+  DESTINATION include/)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

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++")
+
+find_package_handle_standard_args(
+  GRAPHICSMAGICKCPP
+  GRAPHICSMAGICKCPP__LIBRARIES
+  GRAPHICSMAGICKCPP_INCLUDE_DIRS)

nav2_map_server/include/nav2_map_server/map_mode.hpp

@@ -0,0 +1,55 @@
+// 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.
+
+#ifndef NAV2_MAP_SERVER__MAP_MODE_HPP_
+#define NAV2_MAP_SERVER__MAP_MODE_HPP_
+
+#include <string>
+#include <vector>
+namespace nav2_map_server
+{
+/// 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,
+};
+
+/// Convert a MapMode enum to the name of the map mode
+/// @throw std::invalid_argument if the given value is not a defined map mode
+/// @return String identifier of the given map mode
+const char * map_mode_to_string(MapMode map_mode);
+
+/// Convert the name of a map mode to a MapMode enum
+/// @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_

nav2_map_server/include/nav2_map_server/map_generator.hpp → nav2_map_server/include/nav2_map_server/map_saver.hpp

@@ -12,32 +12,39 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_MAP_SERVER__MAP_GENERATOR_HPP_
-#define NAV2_MAP_SERVER__MAP_GENERATOR_HPP_
+#ifndef NAV2_MAP_SERVER__MAP_SAVER_HPP_
+#define NAV2_MAP_SERVER__MAP_SAVER_HPP_
 
 #include <string>
-#include "rclcpp/rclcpp.hpp"
+#include "map_mode.hpp"
 #include "nav_msgs/srv/get_map.hpp"
+#include "rclcpp/rclcpp.hpp"
 
 namespace nav2_map_server
 {
-
-class MapGenerator : public rclcpp::Node
+class MapSaver : public rclcpp::Node
 {
 public:
-  MapGenerator(const std::string & mapname, int threshold_occupied, int threshold_free);
+  explicit MapSaver(const rclcpp::NodeOptions & options);
 
   void mapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr map);
 
-  bool saved_map_;
+  std::shared_future<void> map_saved_future() {return save_next_map_promise.get_future().share();}
 
-private:
-  std::string mapname_;
+protected:
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::ConstSharedPtr map_sub_;
+
+  void try_write_map_to_file(const nav_msgs::msg::OccupancyGrid & map);
+
+  std::promise<void> save_next_map_promise;
+
+  std::string image_format;
+  std::string mapname_;
   int threshold_occupied_;
   int threshold_free_;
+  nav2_map_server::MapMode map_mode;
 };
 
 }  // namespace nav2_map_server
 
-#endif  // NAV2_MAP_SERVER__MAP_GENERATOR_HPP_
+#endif  // NAV2_MAP_SERVER__MAP_SAVER_HPP_

nav2_map_server/include/nav2_map_server/occ_grid_loader.hpp

@@ -18,15 +18,16 @@
 #include <memory>
 #include <string>
 #include <vector>
+#include "nav2_map_server/occ_grid_loader.hpp"
 
-#include "rclcpp/rclcpp.hpp"
+#include "map_mode.hpp"
+#include "nav2_util/lifecycle_node.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
 #include "nav_msgs/srv/get_map.hpp"
-#include "nav2_util/lifecycle_node.hpp"
+#include "rclcpp/rclcpp.hpp"
 
 namespace nav2_map_server
 {
-
 class OccGridLoader : public nav2_util::LifecycleHelperInterface
 {
 public:
@@ -46,19 +47,23 @@ class OccGridLoader : public nav2_util::LifecycleHelperInterface
   // The name of the YAML file from which to get the conversion parameters
   std::string yaml_filename_;
 
-  typedef enum { TRINARY, SCALE, RAW } MapMode;
   typedef struct
   {
+    std::string image_file_name;
     double resolution{0};
     std::vector<double> origin{0, 0, 0};
-    double free_thresh{0};
-    double occupied_thresh{0};
-    MapMode mode{TRINARY};
-    int negate{0};
+    double free_thresh;
+    double occupied_thresh;
+    MapMode mode;
+    bool negate;
   } LoadParameters;
 
+  // Load and parse the given YAML file
+  /// @throw YAML::Exception
+  LoadParameters load_map_yaml(const std::string & yaml_filename_);
+
   // Load the image and generate an OccupancyGrid
-  void loadMapFromFile(const std::string & filename, LoadParameters * loadParameters);
+  void loadMapFromFile(const LoadParameters & loadParameters);
 
   // A service to provide the occupancy grid (GetMap) and the message to return
   rclcpp::Service<nav_msgs::srv::GetMap>::SharedPtr occ_service_;

nav2_map_server/package.xml

@@ -7,22 +7,22 @@
     Refactored map server for ROS2 Navigation
   </description>
   <maintainer email="brian.wilcox@intel.com">Brian Wilcox</maintainer>
-  <license>Apache License 2.0</license>
+  <license>BSD License 2.0</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>sdl</depend>
-  <depend>sdl-image</depend>
   <depend>yaml_cpp_vendor</depend>
   <depend>launch_ros</depend>
   <depend>launch_testing</depend>
   <depend>tf2</depend>
   <depend>nav2_util</depend>
+  <depend>graphicsmagick</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>