map_saver.cpp
Go to the documentation of this file.
00001 /*
00002  * map_saver
00003  * Copyright (c) 2008, Willow Garage, Inc.
00004  * All rights reserved.
00005  * 
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  * 
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *     * Neither the name of the <ORGANIZATION> nor the names of its
00015  *       contributors may be used to endorse or promote products derived from
00016  *       this software without specific prior written permission.
00017  * 
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028  * POSSIBILITY OF SUCH DAMAGE.
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) { //occ [0,0.1)
00077             fputc(254, out);
00078           } else if (map->data[i] == +100) { //occ (0.65,1]
00079             fputc(000, out);
00080           } else { //occ [0.1,0.65]
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 resolution: 0.100000
00096 origin: [0.000000, 0.000000, 0.000000]
00097 #
00098 negate: 0
00099 occupied_thresh: 0.65
00100 free_thresh: 0.196
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       // basename may modify content, so make copy
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 


map_server
Author(s): Brian Gerkey, Tony Pratkanis
autogenerated on Mon Oct 6 2014 02:44:29