/*

* map_saver

* Copyright (c) 2008, Willow Garage, Inc.

* All rights reserved.

*

* Redistribution and use in source and binary forms, with or without

* modification, are permitted provided that the following conditions are met:

*

* * Redistributions of source code must retain the above copyright

* notice, this list of conditions and the following disclaimer.

* * Redistributions in binary form must reproduce the above copyright

* notice, this list of conditions and the following disclaimer in the

* documentation and/or other materials provided with the distribution.

* * Neither the name of the <ORGANIZATION> nor the names of its

* contributors may be used to endorse or promote products derived from

* this software without specific prior written permission.

*

* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE

* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

* POSSIBILITY OF SUCH DAMAGE.

*/

#include <cstdio>

#include "ros/ros.h"

#include "ros/console.h"

#include "nav_msgs/GetMap.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, int threshold_occupied, int threshold_free)

: mapname_(mapname), saved_map_(false), threshold_occupied_(threshold_occupied), threshold_free_(threshold_free)

{

ros::NodeHandle n;

ROS_INFO("Waiting for the map");

map_sub_ = n.subscribe("map", 1, &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_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);

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;

}

std::string mapname_;

ros::Subscriber map_sub_;

bool saved_map_;

int threshold_occupied_;

int threshold_free_;

};

#define USAGE "Usage: \n" \

" map_saver -h\n"\

" map_saver [--occ <threshold_occupied>] [--free <threshold_free>] [-f <mapname>] [ROS remapping args]"

int main(int argc, char** argv)

{

ros::init(argc, argv, "map_saver");

std::string mapname = "map";

int threshold_occupied = 65;

int threshold_free = 25;

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 if (!strcmp(argv[i], "--occ"))

{

if (++i < argc)

{

threshold_occupied = std::atoi(argv[i]);

if (threshold_occupied < 1 || threshold_occupied > 100)

{

ROS_ERROR("threshold_occupied must be between 1 and 100");

return 1;

}

}

else

{

puts(USAGE);

return 1;

}

}

else if (!strcmp(argv[i], "--free"))

{

if (++i < argc)

{

threshold_free = std::atoi(argv[i]);

if (threshold_free < 0 || threshold_free > 100)

{

ROS_ERROR("threshold_free must be between 0 and 100");

return 1;

}

}

else

{

puts(USAGE);

return 1;

}

}

else

{

puts(USAGE);

return 1;

}

}

if (threshold_occupied <= threshold_free)

{

ROS_ERROR("threshold_free must be smaller than threshold_occupied");

return 1;

}

MapGenerator mg(mapname, threshold_occupied, threshold_free);

while(!mg.saved_map_ && ros::ok())

ros::spinOnce();

return 0;

}