From 29f5f0180fb16711154c4c40a7414669ff528e2f Mon Sep 17 00:00:00 2001
From: Dan Rose <dan@digilabs.io>
Date: Thu, 8 Aug 2019 13:34:44 -0500
Subject: [PATCH] Better format handling

1. Warn when an unsupported image format is requested (anything other than bmp, pgm, png)
2. Warn of format problems when node is created, before map is received.
3. Don't crash if an unknown format is requested. Instead fall back to png.
---
 nav2_map_server/src/map_saver.cpp             | 65 ++++++++++++++-----
 .../test/component/test_occ_grid_node.cpp     |  3 +-
 2 files changed, 50 insertions(+), 18 deletions(-)

diff --git a/nav2_map_server/src/map_saver.cpp b/nav2_map_server/src/map_saver.cpp
index 251e53d160b..b9b223af983 100644
--- a/nav2_map_server/src/map_saver.cpp
+++ b/nav2_map_server/src/map_saver.cpp
@@ -79,6 +79,53 @@ MapSaver::MapSaver(const rclcpp::NodeOptions & options)
     }
 
     image_format = declare_parameter("image_format", map_mode == MapMode::Scale ? "png" : "pgm");
+    std::transform(
+      image_format.begin(), image_format.end(), 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(), 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(
+        get_logger(), "Requested image format '%s' is not one of the recommended formats: %s",
+        image_format.c_str(), ss.str().c_str());
+    }
+    const std::string FALLBACK_FORMAT = "png";
+
+    try {
+      Magick::CoderInfo info(image_format);
+      if (!info.isWritable()) {
+        RCLCPP_WARN(
+          get_logger(), "Format '%s' is not writable. Using '%s' instead",
+          image_format.c_str(), FALLBACK_FORMAT.c_str());
+        image_format = FALLBACK_FORMAT;
+      }
+    } catch (Magick::ErrorOption & e) {
+      RCLCPP_WARN(
+        get_logger(), "Format '%s' is not usable. Using '%s' instead:\n%s",
+        image_format.c_str(), FALLBACK_FORMAT.c_str(), e.what());
+      image_format = FALLBACK_FORMAT;
+    }
+    if (
+      map_mode == MapMode::Scale &&
+      (image_format == "pgm" || image_format == "jpg" || image_format == "jpeg"))
+    {
+      RCLCPP_WARN(
+        get_logger(),
+        "Map mode 'scale' requires transparency, but format '%s' does not support it. Consider "
+        "switching image format to 'png'.",
+        image_format.c_str());
+    }
 
     RCLCPP_INFO(get_logger(), "Waiting for the map");
     map_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
@@ -89,17 +136,11 @@ MapSaver::MapSaver(const rclcpp::NodeOptions & options)
 
 void MapSaver::try_write_map_to_file(const nav_msgs::msg::OccupancyGrid & map)
 {
-  rclcpp::Logger logger = this->get_logger();
+  auto logger = get_logger();
   RCLCPP_INFO(
     logger, "Received a %d X %d map @ %.3f m/pix", map.info.width, map.info.height,
     map.info.resolution);
 
-  Magick::CoderInfo info(image_format);
-  if (!info.isWritable()) {
-    image_format = "png";
-    RCLCPP_WARN(logger, "Format is not writable. Falling back to %s", image_format.c_str());
-  }
-
   std::string mapdatafile = mapname_ + "." + image_format;
   {
     // should never see this color, so the initialization value is just for debugging
@@ -113,16 +154,6 @@ void MapSaver::try_write_map_to_file(const nav_msgs::msg::OccupancyGrid & map)
     // Since we only need to support 100 different pixel levels, 8 bits is fine
     image.depth(8);
 
-    if (
-      map_mode == MapMode::Scale &&
-      (image_format == "pgm" || image_format == "jpg" || image_format == "jpeg"))
-    {
-      RCLCPP_WARN(
-        logger,
-        "Map mode 'scale' requires transparency, but format '%s' does not support it. Consider "
-        "switching image format to 'png'.",
-        image_format.c_str());
-    }
     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];
diff --git a/nav2_map_server/test/component/test_occ_grid_node.cpp b/nav2_map_server/test/component/test_occ_grid_node.cpp
index fc7197622c6..37918323aa4 100644
--- a/nav2_map_server/test/component/test_occ_grid_node.cpp
+++ b/nav2_map_server/test/component/test_occ_grid_node.cpp
@@ -60,7 +60,8 @@ class TestNode : public ::testing::Test
     auto result = client->async_send_request(request);
 
     // Wait for the result
-    if (rclcpp::spin_until_future_complete(node, result) ==
+    if (
+      rclcpp::spin_until_future_complete(node, result) ==
       rclcpp::executor::FutureReturnCode::SUCCESS)
     {
       return result.get();