Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031 #include <cstdio>
00032 #include "ros/ros.h"
00033 #include "ros/console.h"
00034 #include "nav_msgs/GetMap.h"
00035 #include "tf/LinearMath/Matrix3x3.h"
00036 #include "geometry_msgs/Quaternion.h"
00037
00038 using namespace std;
00039
00043 class MapGenerator
00044 {
00045
00046 public:
00047 MapGenerator(const std::string& mapname) : mapname_(mapname), saved_map_(false)
00048 {
00049 ros::NodeHandle n;
00050 ROS_INFO("Waiting for the map");
00051 map_sub_ = n.subscribe("map", 1, &MapGenerator::mapCallback, this);
00052 }
00053
00054 void mapCallback(const nav_msgs::OccupancyGridConstPtr& map)
00055 {
00056 ROS_INFO("Received a %d X %d map @ %.3f m/pix",
00057 map->info.width,
00058 map->info.height,
00059 map->info.resolution);
00060
00061
00062 std::string mapdatafile = mapname_ + ".pgm";
00063 ROS_INFO("Writing map occupancy data to %s", mapdatafile.c_str());
00064 FILE* out = fopen(mapdatafile.c_str(), "w");
00065 if (!out)
00066 {
00067 ROS_ERROR("Couldn't save map file to %s", mapdatafile.c_str());
00068 return;
00069 }
00070
00071 fprintf(out, "P5\n# CREATOR: Map_generator.cpp %.3f m/pix\n%d %d\n255\n",
00072 map->info.resolution, map->info.width, map->info.height);
00073 for(unsigned int y = 0; y < map->info.height; y++) {
00074 for(unsigned int x = 0; x < map->info.width; x++) {
00075 unsigned int i = x + (map->info.height - y - 1) * map->info.width;
00076 if (map->data[i] == 0) {
00077 fputc(254, out);
00078 } else if (map->data[i] == +100) {
00079 fputc(000, out);
00080 } else {
00081 fputc(205, out);
00082 }
00083 }
00084 }
00085
00086 fclose(out);
00087
00088
00089 std::string mapmetadatafile = mapname_ + ".yaml";
00090 ROS_INFO("Writing map occupancy data to %s", mapmetadatafile.c_str());
00091 FILE* yaml = fopen(mapmetadatafile.c_str(), "w");
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104 geometry_msgs::Quaternion orientation = map->info.origin.orientation;
00105 tf::Matrix3x3 mat(tf::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w));
00106 double yaw, pitch, roll;
00107 mat.getEulerYPR(yaw, pitch, roll);
00108
00109
00110 char* mapname_copy = strdup(mapname_.c_str());
00111 char* base = basename(mapname_copy);
00112 std::string relative_mapdatafile = std::string(base) + ".pgm";
00113 free(mapname_copy);
00114
00115 fprintf(yaml, "image: %s\nresolution: %f\norigin: [%f, %f, %f]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n\n",
00116 relative_mapdatafile.c_str(), map->info.resolution, map->info.origin.position.x, map->info.origin.position.y, yaw);
00117
00118 fclose(yaml);
00119
00120 ROS_INFO("Done\n");
00121 saved_map_ = true;
00122 }
00123
00124 std::string mapname_;
00125 ros::Subscriber map_sub_;
00126 bool saved_map_;
00127
00128 };
00129
00130 #define USAGE "Usage: \n" \
00131 " map_saver -h\n"\
00132 " map_saver [-f <mapname>] [ROS remapping args]"
00133
00134 int main(int argc, char** argv)
00135 {
00136 ros::init(argc, argv, "map_saver");
00137 std::string mapname = "map";
00138
00139 for(int i=1; i<argc; i++)
00140 {
00141 if(!strcmp(argv[i], "-h"))
00142 {
00143 puts(USAGE);
00144 return 0;
00145 }
00146 else if(!strcmp(argv[i], "-f"))
00147 {
00148 if(++i < argc)
00149 mapname = argv[i];
00150 else
00151 {
00152 puts(USAGE);
00153 return 1;
00154 }
00155 }
00156 else
00157 {
00158 puts(USAGE);
00159 return 1;
00160 }
00161 }
00162
00163 MapGenerator mg(mapname);
00164
00165 while(!mg.saved_map_)
00166 ros::spinOnce();
00167
00168 return 0;
00169 }
00170
00171