ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Generate .pgm and .yaml from a "grid.txt"

asked 2021-01-29 04:18:18 -0600

thenart21 gravatar image

With the code below I load a map to memory and generate a "grid_inflate.txt" that contains a matrix of 0's and 1's, where i inflate the obstacles (change some 0's to 1 basically). Know I need to generate another .pgm and .yaml from this "grid_inflate.txt" and I don't know how.

#include <ros/ros.h>
#include <nav_msgs/GetMap.h>
#include <vector>
#include <fstream>

using namespace std;

// grid map
int rows;
int cols;
double mapResolution;
vector<vector<bool> > grid;

bool requestMap(ros::NodeHandle &nh);
void readMap(const nav_msgs::OccupancyGrid& msg);
void printGridToFile();

int main(int argc, char** argv)
{
    ros::init(argc, argv, "mapping");
    ros::NodeHandle nh;

    if (!requestMap(nh))
        exit(-1);

    printGridToFile();

    return 0;
}

bool requestMap(ros::NodeHandle &nh)
{
    nav_msgs::GetMap::Request req;
    nav_msgs::GetMap::Response res;

    while (!ros::service::waitForService("static_map", ros::Duration(3.0))) {
         ROS_INFO("Waiting for service static_map to become available");
    }

    ROS_INFO("Requesting the map...");
    ros::ServiceClient mapClient = nh.serviceClient<nav_msgs::GetMap>("static_map");

    if (mapClient.call(req, res)) {
        readMap(res.map);
        return true;
    }
    else {
        ROS_ERROR("Failed to call map service");
        return false;
    }
}

void readMap(const nav_msgs::OccupancyGrid& map)
{
    ROS_INFO("Received a %d X %d map @ %.3f m/px\n",
            map.info.width,
            map.info.height,
            map.info.resolution); 
    rows = map.info.height;
    cols = map.info.width;
    mapResolution = map.info.resolution;

    // Dynamically resize the grid
    grid.resize(rows);
    for (int i = 0; i < rows; i++) {
        grid[i].resize(cols);
    } 
    int currCell = 0;
    for (int i = 0; i < rows; i++)  {
        for (int j = 0; j < cols; j++)      {
            if (map.data[currCell] == 0) // unoccupied cell
                grid[i][j] = false;
            else
                grid[i][j] = true; // occupied (100) or unknown cell (-1)
            currCell++;
        }
    }

    for (int i = 5 ; i < grid.size(); i++) {        
        for (int j = 5; j < grid[0].size(); j++) {
            if(grid[i][j] == true){
                if(grid[i][j-1] == false){
                    grid[i][j-1] = true;
                }
                if(grid[i-1][j] == false){
                    grid[i-1][j] = true;
                }
                if(grid[i-1][j-1] == false){
                    grid[i-1][j-1] = true;
                }
            }
        }
    }

    for (int i = grid.size() - 5; i >= 0; i--) {        
        for (int j = grid[0].size()-5; j >= 0; j--) {
            if(grid[i][j] == true){
                if(grid[i][j+1] == false){
                    grid[i][j+1] = true;
                }
                if(grid[i+1][j] == false){
                    grid[i+1][j] = true;
                }
                if(grid[i+1][j+1] == false){
                    grid[i+1][j+1] = true;
                }
            }
        }
    }




}

void printGridToFile() {
    ofstream gridFile;
    gridFile.open("grid_inflate.txt");

    for (int i = grid.size() - 1; i >= 0; i--) {        
        for (int j = 0; j < grid[0].size(); j++) {
        gridFile << (grid[i][j] ? "1" : "0");           
        }
        gridFile << endl;
    }
    gridFile.close();
}
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2021-01-29 21:42:25 -0600

tryan gravatar image

Since you're already working with map messages, one option would be to publish the new map as a message on a topic and let the map_saver node (wiki) do it for you. That would also have the benefit (depends on your end goal) of making your new map available online. If you don't care about online vs. offline and you want everything in a single node, you can still reuse the map_saver.cpp source code. Here's the meat of it for convenience:

    void mapCallback(const nav_msgs::OccupancyGridConstPtr& map)
    {
      ROS_INFO("Received a %d X %d map @ %.3f m/pix",
               map->info.width,
               map->info.height,
               map->info.resolution);

/* ***************** Saves map (PGM) ******************* */
      std::string mapdatafile = mapname_ + ".pgm";
      ROS_INFO("Writing map occupancy data to %s", mapdatafile.c_str());
      FILE* out = fopen(mapdatafile.c_str(), "w");
      if (!out)
      {
        ROS_ERROR("Couldn't save map file to %s", mapdatafile.c_str());
        return;
      }

      fprintf(out, "P5\n# CREATOR: map_saver.cpp %.3f m/pix\n%d %d\n255\n",
              map->info.resolution, map->info.width, map->info.height);
      for(unsigned int y = 0; y < map->info.height; y++) {
        for(unsigned int x = 0; x < map->info.width; x++) {
          unsigned int i = x + (map->info.height - y - 1) * map->info.width;
          if (map->data[i] >= 0 && map->data[i] <= threshold_free_) { // [0,free)
            fputc(254, out);
          } else if (map->data[i] >= threshold_occupied_) { // (occ,255]
            fputc(000, out);
          } else { //occ [0.25,0.65]
            fputc(205, out);
          }
        }
      }

      fclose(out);


/* ************************* Saves metadata (YAML) ********************** */
      std::string mapmetadatafile = mapname_ + ".yaml";
      ROS_INFO("Writing map occupancy data to %s", mapmetadatafile.c_str());
      FILE* yaml = fopen(mapmetadatafile.c_str(), "w");

      /*
resolution: 0.100000
origin: [0.000000, 0.000000, 0.000000]
#
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
       */

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

      fprintf(yaml, "image: %s\nresolution: %f\norigin: [%f, %f, %f]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n\n",
              mapdatafile.c_str(), map->info.resolution, map->info.origin.position.x, map->info.origin.position.y, yaw);

      fclose(yaml);

      ROS_INFO("Done\n");
      saved_map_ = true;
    }
edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2021-01-29 04:18:18 -0600

Seen: 516 times

Last updated: Jan 29 '21