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 "LinearMath/btMatrix3x3.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)
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 btMatrix3x3 mat(btQuaternion(orientation.x, orientation.y, orientation.z, orientation.w));
00106 double yaw, pitch, roll;
00107 mat.getEulerYPR(yaw, pitch, roll);
00108
00109 fprintf(yaml, "image: %s\nresolution: %f\norigin: [%f, %f, %f]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n\n",
00110 mapdatafile.c_str(), map->info.resolution, map->info.origin.position.x, map->info.origin.position.y, yaw);
00111
00112 fclose(yaml);
00113
00114 ROS_INFO("Done\n");
00115 }
00116
00117 std::string mapname_;
00118 ros::Subscriber map_sub_;
00119 };
00120
00121 #define USAGE "Usage: \n" \
00122 " map_saver -h\n"\
00123 " map_saver [-f <mapname>] [ROS remapping args]"
00124
00125 int main(int argc, char** argv)
00126 {
00127 ros::init(argc, argv, "map_saver");
00128 std::string mapname = "map";
00129
00130 for(int i=1; i<argc; i++)
00131 {
00132 if(!strcmp(argv[i], "-h"))
00133 {
00134 puts(USAGE);
00135 return 0;
00136 }
00137 else if(!strcmp(argv[i], "-f"))
00138 {
00139 if(++i < argc)
00140 mapname = argv[i];
00141 else
00142 {
00143 puts(USAGE);
00144 return 1;
00145 }
00146 }
00147 else
00148 {
00149 puts(USAGE);
00150 return 1;
00151 }
00152 }
00153
00154 MapGenerator mg(mapname);
00155
00156 ros::spin();
00157
00158 return 0;
00159 }
00160
00161