{

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());

}

{

if (const char * home_dir = std::getenv("HOME")) {

return std::string{home_dir};

}

return std::string{};

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);

{

YAML::Node doc = YAML::LoadFile(expand_user_home_dir_if_needed(yaml_filename, get_home_dir()));

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(yaml_filename.begin(), 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;

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();

// 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

Eigen::Matrix<int8_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> result(height, width);

if (load_parameters.mode == MapMode::Trinary || load_parameters.mode == MapMode::Scale) {

// Convert grayscale to float in range [0.0, 1.0]

Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic,

Eigen::RowMajor> normalized = gray_matrix.cast<float>() / 255.0f;

// Negate the image if specified (e.g. for black=occupied vs. white=occupied convention)

if (!load_parameters.negate) {

normalized = (1.0f - normalized.array()).matrix();

}

// Compute binary occupancy masks

Eigen::Matrix<uint8_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> occupied =

(normalized.array() >= load_parameters.occupied_thresh).cast<uint8_t>();

Eigen::Matrix<uint8_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> free =

(normalized.array() <= load_parameters.free_thresh).cast<uint8_t>();

// Initialize occupancy grid with UNKNOWN values (-1)

result.setConstant(nav2_util::OCC_GRID_UNKNOWN);

// Apply occupied and free cell updates

result = (occupied.array() > 0).select(nav2_util::OCC_GRID_OCCUPIED, result);

result = (free.array() > 0).select(nav2_util::OCC_GRID_FREE, result);

// Handle intermediate (gray) values if in Scale mode

if (load_parameters.mode == MapMode::Scale) {

// Create in-between mask

auto in_between_mask = (normalized.array() > load_parameters.free_thresh) &&

(normalized.array() < load_parameters.occupied_thresh);

if (in_between_mask.any()) {

// Scale in-between values to [0,100] range

Eigen::ArrayXXf scaled_float = ((normalized.array() - load_parameters.free_thresh) /

(load_parameters.occupied_thresh - load_parameters.free_thresh)) * 100.0f;

// Round and cast to int8_t

Eigen::Matrix<int8_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> scaled_int =

scaled_float.array().round().cast<int8_t>();

result = in_between_mask.select(scaled_int, result);

}

}

// 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);

// Apply mask directly with Eigen::select

result = (alpha_array < 255).select(nav2_util::OCC_GRID_UNKNOWN, result);

}

} else if (load_parameters.mode == MapMode::Raw) {

// Raw mode: interpret raw image pixel values directly as occupancy values

result = gray_matrix.cast<int8_t>();

// Clamp out-of-bound values (outside [-1, 100]) to UNKNOWN (-1)

auto out_of_bounds = (result.array() < nav2_util::OCC_GRID_FREE) ||

(result.array() > nav2_util::OCC_GRID_OCCUPIED);

result = out_of_bounds.select(nav2_util::OCC_GRID_UNKNOWN, result);

} else {

// If the map mode is not recognized, throw an error

throw std::runtime_error("Invalid map mode");

}

// Flip image vertically (as ROS expects origin at bottom-left)

Eigen::Matrix<int8_t, Eigen::Dynamic, Eigen::Dynamic,

Eigen::RowMajor> flipped = result.colwise().reverse();

std::memcpy(msg.data.data(), flipped.data(), width * height);

// 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;

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;

{

// 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'.");

}

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");

const nav_msgs::msg::OccupancyGrid & map,

const SaveParameters & save_parameters)

{

// 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::ostringstream out;

out << std::fixed << std::setprecision(precision) << value;

return out.str();