34 #include "nav_msgs/GetMap.h" 36 #include "geometry_msgs/Quaternion.h" 47 MapGenerator(
const std::string& mapname,
int threshold_occupied,
int threshold_free)
48 : mapname_(mapname), saved_map_(false), threshold_occupied_(threshold_occupied), threshold_free_(threshold_free)
57 ROS_INFO(
"Received a %d X %d map @ %.3f m/pix",
60 map->info.resolution);
63 std::string mapdatafile = mapname_ +
".pgm";
64 ROS_INFO(
"Writing map occupancy data to %s", mapdatafile.c_str());
65 FILE* out = fopen(mapdatafile.c_str(),
"w");
68 ROS_ERROR(
"Couldn't save map file to %s", mapdatafile.c_str());
72 fprintf(out,
"P5\n# CREATOR: map_saver.cpp %.3f m/pix\n%d %d\n255\n",
73 map->info.resolution, map->info.width, map->info.height);
74 for(
unsigned int y = 0; y < map->info.height; y++) {
75 for(
unsigned int x = 0; x < map->info.width; x++) {
76 unsigned int i = x + (map->info.height - y - 1) * map->info.width;
77 if (map->data[i] >= 0 && map->data[i] <= threshold_free_) {
79 }
else if (map->data[i] >= threshold_occupied_) {
90 std::string mapmetadatafile = mapname_ +
".yaml";
91 ROS_INFO(
"Writing map occupancy data to %s", mapmetadatafile.c_str());
92 FILE* yaml = fopen(mapmetadatafile.c_str(),
"w");
105 geometry_msgs::Quaternion orientation = map->info.origin.orientation;
112 double yaw, pitch, roll;
115 fprintf(yaml,
"image: %s\nresolution: %f\norigin: [%f, %f, %f]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n\n",
116 mapdatafile.c_str(), map->info.resolution, map->info.origin.position.x, map->info.origin.position.y, yaw);
132 #define USAGE "Usage: \n" \ 134 " map_saver [--occ <threshold_occupied>] [--free <threshold_free>] [-f <mapname>] [ROS remapping args]" 136 int main(
int argc,
char** argv)
139 std::string mapname =
"map";
140 int threshold_occupied = 65;
141 int threshold_free = 25;
143 for(
int i=1; i<argc; i++)
145 if(!strcmp(argv[i],
"-h"))
150 else if(!strcmp(argv[i],
"-f"))
160 else if (!strcmp(argv[i],
"--occ"))
164 threshold_occupied = std::atoi(argv[i]);
165 if (threshold_occupied < 1 || threshold_occupied > 100)
167 ROS_ERROR(
"threshold_occupied must be between 1 and 100");
178 else if (!strcmp(argv[i],
"--free"))
182 threshold_free = std::atoi(argv[i]);
183 if (threshold_free < 0 || threshold_free > 100)
185 ROS_ERROR(
"threshold_free must be between 0 and 100");
203 if (threshold_occupied <= threshold_free)
205 ROS_ERROR(
"threshold_free must be smaller than threshold_occupied");
209 MapGenerator mg(mapname, threshold_occupied, threshold_free);
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int main(int argc, char **argv)
MapGenerator(const std::string &mapname, int threshold_occupied, int threshold_free)
void mapCallback(const nav_msgs::OccupancyGridConstPtr &map)
ROSCPP_DECL void spinOnce()
void getEulerYPR(tf2Scalar &yaw, tf2Scalar &pitch, tf2Scalar &roll, unsigned int solution_number=1) const