rviz (occupancy grid) map doesn't match the one I save as pgm

asked 2018-04-26 05:07:40 -0500

OperationCrossbow gravatar image

Kinetic, Linux 16.04

I've done an experiment with a SLAM algorithm (gmapping) which makes a 'map' in rviz, from black and white cells (this is the occupancy grid). I then use a little script to save that map to a pgm.

The map looks like this: image description.

As you can see, a nice little map. The algorithm figured out its data and printed it as a reasonably straight, accurate map.

So I use this script from the 'occupancy_grid_server' package (I got from I know not where) to save the map:

#include <cstdio>
#include "ros/ros.h"
#include "ros/console.h"
#include "nav_msgs/OccupancyGrid.h"
#include "tf2/LinearMath/Matrix3x3.h"
#include "geometry_msgs/Quaternion.h"
using namespace std;
/**
 * @brief Map generation node.
 */
class MapGenerator
{
  public:
    MapGenerator(const std::string& mapname) : mapname_(mapname), saved_map_(false)
    {
      ros::NodeHandle n;
      ROS_INFO("Waiting for the map");
      map_sub_ = n.subscribe("/map", 10000, &MapGenerator::mapCallback, this);    }
    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);
      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_generator.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] == -1) { //occ [0,0.1)
            fputc(127, out);
          } else if (map->data[i] <= +100) { //occ (0.65,1]
            float value = 254-(map->data[i]*2.54);
            fputc(value, out);
          } else { //occ [0.1,0.65]
            fputc(127, out);
          }
        }
      }
      fclose(out);
      std::string mapmetadatafile = mapname_ + ".yaml";
      ROS_INFO("Writing map occupancy data to %s", mapmetadatafile.c_str());
      FILE* yaml = fopen(mapmetadatafile.c_str(), "w");
      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;
    }
    std::string mapname_;
    ros::Subscriber map_sub_;
    bool saved_map_;
};
#define USAGE "Usage: \n" \
              "  map_saver -h\n"\
              "  map_saver [-f <mapname>] [ROS remapping args]"
int main(int argc, char** argv)
{
  ros::init(argc, argv, "map_saver");
  time_t thetime = time(0);
  std::string dt = ctime(&thetime);
  std::string mapname = dt+"map";
  for(int i=1; i<argc; i++)
  {
    if(!strcmp(argv[i], "-h"))
    {
      puts(USAGE);
      return 0;    }
    else if(!strcmp(argv[i], "-f"))
    {
      if(++i < argc)
        mapname = argv[i];
      else
      {
        puts(USAGE);
        return 1;      }
    }
    else
    {
      puts(USAGE);
      return 1;    }
  }
  MapGenerator mg(mapname);
  while(!mg.saved_map_ && ros::ok())
    ros::spinOnce();
  return 0;
}

From what I understand, it ... (more)

edit retag flag offensive close merge delete