text: // Copyright 2017 Intermodalics All Rights Reserved.
  //
  // 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.
  #include "tango_ros_native/occupancy_grid_file_io.h"
  
  #include <cmath>
  #include <fstream>
  #include <iostream>
  #include <sys/stat.h>
  #include <unistd.h>
  
  #include <geometry_msgs/Quaternion.h>
  #include <tf/LinearMath/Matrix3x3.h>
  
  #include <glog/logging.h>
  
  #include <yaml-cpp/yaml.h>
  
  namespace {
  void AddTrailingSlashToDirectoryPathIfNeeded(std::string* directory_path) {
    if (!directory_path->empty() && directory_path->back() != '/') {
      (*directory_path) += '/';
    }
  }
  bool DirectoryPathExists(const std::string& path) {
    struct stat st;
    return (stat(path.c_str(), &st) == 0 && (st.st_mode & S_IFDIR));
  }
  bool MakeDirectoryPath(const std::string& input_path, mode_t mode) {
    if (input_path.empty()) {
      // Path is empty, there is nothing to create so we return success.
      return 0;
    }
  
    size_t previous_slash_pos = 0;
    size_t current_slash_pos;
    std::string dir;
    int make_dir_status = 0;
  
    std::string path = input_path;
    AddTrailingSlashToDirectoryPathIfNeeded(&path);
  
    while ((current_slash_pos = path.find_first_of('/', previous_slash_pos)) !=
           std::string::npos) {
      dir = path.substr(0, current_slash_pos++);
      previous_slash_pos = current_slash_pos;
      if (dir.empty() || dir == ".")
        continue;  // If leading / first time is 0 length.
  
      if ((make_dir_status = mkdir(dir.c_str(), mode)) && errno != EEXIST) {
        return false;
      }
    }
    return true;
  }
  } // namespace
  
  namespace occupancy_grid_file_io {
  bool SaveOccupancyGridToFiles(
      const std::string& map_name, const std::string& map_uuid,
      const std::string& map_directory, const nav_msgs::OccupancyGrid& occupancy_grid) {
    return SaveOccupancyGridDataToPgmFile(map_name, map_directory, occupancy_grid)
        && SaveOccupancyGridMetadataToYamlFile(map_name, map_uuid, map_directory, occupancy_grid.info);
  }
  
  bool SaveOccupancyGridDataToPgmFile(
      const std::string& map_name, const std::string& map_directory,
      const nav_msgs::OccupancyGrid& occupancy_grid) {
    std::string map_directory_with_trailing_slash = map_directory;
    AddTrailingSlashToDirectoryPathIfNeeded(&map_directory_with_trailing_slash);
    if (!DirectoryPathExists(map_directory_with_trailing_slash)) {
      LOG(INFO) << "Directory " << map_directory_with_trailing_slash << " does not exist, creating it now.";
      if (!MakeDirectoryPath(map_directory_with_trailing_slash, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH)) {
        LOG(ERROR) << "Could not create directory: " << map_directory_with_trailing_slash;
        return false;
      }
    }
  
    std::string map_pgm_file_path = map_directory_with_trailing_slash + map_name + ".pgm";
    FILE* pgm_file = fopen(map_pgm_file_path.c_str(), "w");
    if (!pgm_file) {
      LOG(ERROR) << "Could no open file " << map_pgm_file_path;
      return false;
    }
  
    fprintf(pgm_file, "P5\n# CREATOR: TangoRosStreamer %.3f m/pix\n%d %d\n255\n",
            occupancy_grid.info.resolution,
            occupancy_grid.info.width, occupancy_grid.info.height);
    for (size_t i = 0; i < occupancy_grid.info.height; ++i) {
      for (size_t j = 0; j < occupancy_grid.info.width; ++j) {
        // Need to invert the ordering of the cells to
        // produce an image with pixel (0,0) in the top-left corner.
        int value = occupancy_grid.data[j + (occupancy_grid.info.height - i - 1) * occupancy_grid.info.width];
        if (value == 0) {
          fputc(254, pgm_file);
        } else if (value == +100) {
          fputc(000, pgm_file);
        } else {
          fputc(205, pgm_file);
        }
      }
    }
    fclose(pgm_file);
    LOG(INFO) << "Map image successfully saved to " << map_pgm_file_path;
    return true;
  }
  
  bool SaveOccupancyGridMetadataToYamlFile(
      const std::string& map_name, const std::string& map_uuid,
      const std::string& map_directory, const nav_msgs::MapMetaData& map_metadata) {
    std::string map_directory_with_trailing_slash = map_directory;
    AddTrailingSlashToDirectoryPathIfNeeded(&map_directory_with_trailing_slash);
    if (!DirectoryPathExists(map_directory_with_trailing_slash)) {
      LOG(INFO) << "Directory " << map_directory_with_trailing_slash << " does not exist, creating it now.";
      if (!MakeDirectoryPath(map_directory_with_trailing_slash, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH)) {
        LOG(ERROR) << "Could not create directory: " << map_directory_with_trailing_slash;
        return false;
      }
    }
  
    std::string image_name = map_name;
    if (image_name.empty())
      image_name = "\"\"";
    std::string uuid = map_uuid;
    if (uuid.empty())
      uuid = "\"\"";
  
    std::string map_yaml_file_path = map_directory_with_trailing_slash + map_name + ".yaml";
    FILE* yaml_file = fopen(map_yaml_file_path.c_str(), "w");
    if (!yaml_file) {
      LOG(ERROR) << "Could no open file " << map_yaml_file_path;
      return false;
    }
  
    tf::Matrix3x3 mat(tf::Quaternion(map_metadata.origin.orientation.x,
                                     map_metadata.origin.orientation.y,
                                     map_metadata.origin.orientation.z,
                                     map_metadata.origin.orientation.w));
    double yaw, pitch, roll;
    mat.getEulerYPR(yaw, pitch, roll);
    if (std::isnan(yaw))
      yaw = 0.0;
  
    fprintf(yaml_file, "image: %s.pgm\nresolution: %f\norigin: [%f, %f, %f]\nnegate: 0\n"
        "occupied_thresh: 0.65\nfree_thresh: 0.196\nuuid: %s\n\n",
        map_name.c_str(), map_metadata.resolution, map_metadata.origin.position.x,
        map_metadata.origin.position.y, yaw, uuid.c_str());
  
    fclose(yaml_file);
    LOG(INFO) << "Map yaml file successfully saved to " << map_yaml_file_path;
    return true;
  }
  
  bool LoadOccupancyGridFromFiles(
      const std::string&  map_name, const std::string& map_directory,
      nav_msgs::OccupancyGrid* occupancy_grid, std::string* map_uuid) {
    int negate;
    double occupied_threshold;
    double free_threshold;
    bool result = LoadOccupancyGridMetadataFromYamlFile(
        map_name, map_directory, &(occupancy_grid->info), &negate,
        &occupied_threshold, &free_threshold, map_uuid);
    return result && LoadOccupancyGridDataFromPgmFile(
        map_name, map_directory, negate, occupied_threshold, free_threshold,
        occupancy_grid);
  }
  
  bool LoadOccupancyGridDataFromPgmFile(
      const std::string&  map_name, const std::string& map_directory,
      bool negate, double occupied_threshold, double free_threshold,
      nav_msgs::OccupancyGrid* occupancy_grid) {
    std::string map_directory_with_trailing_slash = map_directory;
    AddTrailingSlashToDirectoryPathIfNeeded(&map_directory_with_trailing_slash);
  
    std::string map_pgm_file_path = map_directory_with_trailing_slash + map_name + ".pgm";
    std::ifstream pgm_file(map_pgm_file_path, std::ios::binary);
    if (pgm_file.fail()) {
      LOG(ERROR) << "Could no open file " << map_pgm_file_path;
      return false;
    }
  
    // First line contains file type.
    std::string file_type;
    getline(pgm_file, file_type);
    LOG(INFO) << file_type;
    if (file_type.compare("P5") != 0) {
      LOG(ERROR) << "Pgm file type error. Type is " << file_type
          << " while supported type is P5.";
      return false;
    }
    // Second line contains comment.
    std::string comment;
    getline(pgm_file, comment);
    LOG(INFO) << comment;
    // Third line contains size.
    std::string image_size;
    getline(pgm_file, image_size);
    std::stringstream size_stream(image_size);
    size_stream >> occupancy_grid->info.width >> occupancy_grid->info.height;
    LOG(INFO) << "Image size is " << occupancy_grid->info.width << "x" << occupancy_grid->info.height;
    // Fourth line contains max value.
    std::string max_value_string;
    getline(pgm_file, max_value_string);
    std::stringstream max_val_stream(max_value_string);
    int max_value = 0;
    max_val_stream >> max_value;
    // Following lines contain binary data.
    int pixel_array[occupancy_grid->info.height * occupancy_grid->info.width];
    for (size_t i = 0; i < occupancy_grid->info.height * occupancy_grid->info.width; ++i) {
      char pixel = pgm_file.get();
      pixel_array[i] = static_cast<int>(pixel);
    }
    // Need to invert the ordering of the pixels to
    // produce a map with cell (0,0) in the lower-left corner.
    occupancy_grid->data.reserve(occupancy_grid->info.height * occupancy_grid->info.width);
    for (size_t i = 0; i < occupancy_grid->info.height; ++i) {
      for (size_t j = 0; j < occupancy_grid->info.width; ++j) {
        int value = pixel_array[j + (occupancy_grid->info.height - i - 1) * occupancy_grid->info.width];
        if (negate)
          value = max_value - value;
        double occupancy = (max_value - value) / static_cast<double>(max_value);
        if (occupancy < free_threshold) {
          occupancy_grid->data.push_back(0);
        } else if (occupancy > occupied_threshold) {
          occupancy_grid->data.push_back(100);
        } else {
          occupancy_grid->data.push_back(-1);
        }
      }
    }
    pgm_file.close();
    LOG(INFO) << "Map image successfully loaded from " << map_pgm_file_path;
    return true;
  }
  
  bool LoadOccupancyGridMetadataFromYamlFile(
      const std::string&  map_name, const std::string& map_directory,
      nav_msgs::MapMetaData* map_metadata, int* negate,
      double* occupied_threshold, double* free_threshold, std::string* map_uuid) {
    std::string map_directory_with_trailing_slash = map_directory;
    AddTrailingSlashToDirectoryPathIfNeeded(&map_directory_with_trailing_slash);
  
    std::string map_yam_file_path = map_directory_with_trailing_slash + map_name + ".yaml";
    std::ifstream yaml_file(map_yam_file_path.c_str());
    if (yaml_file.fail()) {
      LOG(ERROR) << "Could no open file " << map_yam_file_path;
      return false;
    }
  
    YAML::Node node = YAML::Load(yaml_file);
    try {
      map_metadata->resolution = node["resolution"].as<double>();
    } catch (YAML::RepresentationException& e) {
      LOG(ERROR) << "The map does not contain a resolution tag or it is invalid. " << e.msg;
      return false;
    }
    try {
      *negate = node["negate"].as<int>();
    } catch (YAML::RepresentationException& e) {
      LOG(ERROR) << "The map does not contain a negate tag or it is invalid. " << e.msg;
      return false;
    }
    try {
      *occupied_threshold = node["occupied_thresh"].as<double>();
    } catch (YAML::RepresentationException& e) {
      LOG(ERROR) << "The map does not contain an occupied_thresh tag or it is invalid. " << e.msg;
      return false;
    }
    try {
      *free_threshold = node["free_thresh"].as<double>();
    } catch (YAML::RepresentationException& e) {
      LOG(ERROR) << "The map does not contain a free_thresh tag or it is invalid. " << e.msg;
      return false;
    }
    try {
      map_metadata->origin.position.x = node["origin"][0].as<double>();
      map_metadata->origin.position.y = node["origin"][1].as<double>();
      map_metadata->origin.position.z = 0.0;
      tf::Quaternion quaternion;
      double yaw = node["origin"][2].as<double>();
      quaternion.setRPY(0., 0., yaw);
      map_metadata->origin.orientation.x = quaternion.x();
      map_metadata->origin.orientation.y = quaternion.y();
      map_metadata->origin.orientation.z = quaternion.z();
      map_metadata->origin.orientation.w = quaternion.w();
    } catch (YAML::RepresentationException& e) {
      LOG(ERROR) << "The map does not contain an origin tag or it is invalid. " << e.msg;
      return false;
    }
    try {
      *map_uuid = node["uuid"].as<std::string>();
    } catch (YAML::RepresentationException& e) {
      LOG(WARNING) << "The map does not contain a uuid tag or it is invalid. " << e.msg;
    }
    yaml_file.close();
    LOG(INFO) << "Map yaml file successfully loaded from " << map_yam_file_path;
    return true;
  }
  } // namespace occupancy_grid_file_io
  