map_io.cpp

/* 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"
#include "nav2_ros_common/validate_messages.hpp"
#include "rclcpp/clock.hpp"
#include "rclcpp/logger.hpp"
#include "rclcpp/logging.hpp"
 
#ifndef _WIN32
#include <libgen.h>
#endif
 
#include <Eigen/Dense>
 
#include <array>
#include <cmath>
#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.hpp"
#include "tf2/LinearMath/Quaternion.hpp"
#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 = const_cast<char *>(dot);
  }
 
  return path;
}
#endif
 
namespace nav2_map_server
{
using nav2_util::geometry_utils::orientationAroundZAxis;
 
// === Map input part ===
 
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)
{
  const auto expanded_yaml_filename =
    expand_user_home_dir_if_needed(yaml_filename, get_home_dir());
  YAML::Node doc = YAML::LoadFile(expanded_yaml_filename);
  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(expanded_yaml_filename.begin(), expanded_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);
 
  // Convert the image to grayscale
  Magick::Image gray = img;
  gray.type(Magick::GrayscaleType);
 
  // Prepare grayscale matrix from image
  size_t width = gray.columns();
  size_t height = gray.rows();
 
  std::vector<uint8_t> buffer(width * height);
  gray.write(0, 0, width, height, "I", Magick::CharPixel, buffer.data());
 
  // Map the grayscale buffer to an Eigen matrix (row-major layout)
  Eigen::Map<Eigen::Matrix<uint8_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
  gray_matrix(buffer.data(), height, width);
 
  bool has_alpha = img.matte();
 
  // ROS expects the origin at the bottom-left, so the image is flipped vertically on
  // the way out. Mapping msg.data let's the classified values land straight in the
  // message, with no width*height intermediate of our own.
  Eigen::Map<Eigen::Matrix<int8_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
  output_map(msg.data.data(), height, width);
 
  // Handle different map modes with if else condition
  // Trinary and Scale modes are handled together
  // because they share a lot of code
  // Raw mode is handled separately in else if block
  std::array<int8_t, 256> lut;
 
  if (load_parameters.mode == MapMode::Trinary || load_parameters.mode == MapMode::Scale) {
    // A grayscale pixel has only 256 possible values, so classification collapses
    // into a lookup table, avoiding every width*height intermediate buffer. The
    // float arithmetic is kept in its original order so that boundary pixels
    // classify bit-for-bit identically.
    const float free_thresh = static_cast<float>(load_parameters.free_thresh);
    const float occupied_thresh = static_cast<float>(load_parameters.occupied_thresh);
    const float scale_span =
      static_cast<float>(load_parameters.occupied_thresh - load_parameters.free_thresh);
 
    for (int g = 0; g < 256; ++g) {
      float occ = static_cast<float>(g) / 255.0f;
      if (!load_parameters.negate) {
        occ = 1.0f - occ;
      }
 
      int8_t value = nav2_util::OCC_GRID_UNKNOWN;
      // Free after occupied so it wins when the thresholds overlap, as in the original.
      if (occ >= occupied_thresh) {
        value = nav2_util::OCC_GRID_OCCUPIED;
      }
      if (occ <= free_thresh) {
        value = nav2_util::OCC_GRID_FREE;
      }
      if (load_parameters.mode == MapMode::Scale &&
        occ > free_thresh && occ < occupied_thresh)
      {
        value = static_cast<int8_t>(std::round(((occ - free_thresh) / scale_span) * 100.0f));
      }
      lut[g] = value;
    }
 
    output_map = gray_matrix.unaryExpr([&lut](uint8_t g) -> int8_t {return lut[g];})
      .colwise().reverse();
 
    // Apply alpha transparency mask: mark transparent cells as UNKNOWN
    if (has_alpha) {
      // Allocate buffer only once and map directly to Eigen without extra copy
      std::vector<uint8_t> alpha_buf(width * height);
      img.write(0, 0, width, height, "A", Magick::CharPixel, alpha_buf.data());
 
      // Map alpha buffer as Eigen::Array for efficient elementwise ops
      Eigen::Map<Eigen::Array<uint8_t, Eigen::Dynamic, Eigen::Dynamic,
        Eigen::RowMajor>> alpha_array(
        alpha_buf.data(), height, width);
 
      // Reversed to line up with the already-flipped output
      output_map = (alpha_array.colwise().reverse() < 255)
        .select(nav2_util::OCC_GRID_UNKNOWN, output_map);
    }
 
  } else if (load_parameters.mode == MapMode::Raw) {
    // Raw mode: interpret raw image pixel values directly as occupancy values,
    // clamping anything outside [0, 100] to UNKNOWN. Also a pure function of the
    // gray level, so it collapses into a lookup table the same way.
    for (int g = 0; g < 256; ++g) {
      const int8_t value = static_cast<int8_t>(g);
      const bool in_bounds = value >= nav2_util::OCC_GRID_FREE &&
        value <= nav2_util::OCC_GRID_OCCUPIED;
      lut[g] = in_bounds ? value : nav2_util::OCC_GRID_UNKNOWN;
    }
 
    output_map = gray_matrix.unaryExpr([&lut](uint8_t g) -> int8_t {return lut[g];})
      .colwise().reverse();
 
  } else {
    // If the map mode is not recognized, throw an error
    throw std::runtime_error("Invalid map mode");
  }
 
  // 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 ===
 
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'.");
  }
}
 
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(7);
    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 << to_string_with_precision(
      map.info.resolution,
      3);
    e << YAML::Key << "origin" << YAML::Flow << YAML::BeginSeq <<
      to_string_with_precision(map.info.origin.position.x, 3) <<
      to_string_with_precision(map.info.origin.position.y, 3) << yaw << YAML::EndSeq;
    e << YAML::Key << "negate" << YAML::Value << 0;
 
    if (save_parameters.mode == MapMode::Trinary) {
      // For Trinary mode, the thresholds depend on the pixel values in the saved map,
      // not on the thresholds used to threshold the map.
      // As these values are fixed above, the thresholds must also be fixed to separate the
      // pixel values into occupied, free and unknown.
      e << YAML::Key << "occupied_thresh" << YAML::Value << 0.65;
      e << YAML::Key << "free_thresh" << YAML::Value << 0.196;
    } else {
      e << YAML::Key << "occupied_thresh" << YAML::Value <<
        to_string_with_precision(save_parameters.occupied_thresh, 3);
      e << YAML::Key << "free_thresh" << YAML::Value <<
        to_string_with_precision(save_parameters.free_thresh, 3);
    }
 
    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)
{
  if (!nav2::validateMsg(map)) {
    RCLCPP_ERROR_STREAM(
      rclcpp::get_logger("map_io"),
      "Failed to write map for reason: invalid OccupancyGrid message");
    return false;
  }
 
  // 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;
}
 
std::string to_string_with_precision(double value, int precision)
{
  std::ostringstream out;
  out << std::fixed << std::setprecision(precision) << value;
 
  return out.str();
}
 
}  // namespace nav2_map_server