map_saver.cpp
Go to the documentation of this file.
1 /*
2  * map_saver
3  * Copyright (c) 2008, Willow Garage, Inc.
4  * All rights reserved.
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions are met:
8  *
9  * * Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * * Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in the
13  * documentation and/or other materials provided with the distribution.
14  * * Neither the name of the <ORGANIZATION> nor the names of its
15  * contributors may be used to endorse or promote products derived from
16  * this software without specific prior written permission.
17  *
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
22  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28  * POSSIBILITY OF SUCH DAMAGE.
29  */
30 
31 #include <cstdio>
32 #include "ros/ros.h"
33 #include "ros/console.h"
34 #include "nav_msgs/GetMap.h"
36 #include "geometry_msgs/Quaternion.h"
37 
38 using namespace std;
39 
44 {
45 
46  public:
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)
49  {
51  ROS_INFO("Waiting for the map");
52  map_sub_ = n.subscribe("map", 1, &MapGenerator::mapCallback, this);
53  }
54 
55  void mapCallback(const nav_msgs::OccupancyGridConstPtr& map)
56  {
57  ROS_INFO("Received a %d X %d map @ %.3f m/pix",
58  map->info.width,
59  map->info.height,
60  map->info.resolution);
61 
62 
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");
66  if (!out)
67  {
68  ROS_ERROR("Couldn't save map file to %s", mapdatafile.c_str());
69  return;
70  }
71 
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_) { // [0,free)
78  fputc(254, out);
79  } else if (map->data[i] >= threshold_occupied_) { // (occ,255]
80  fputc(000, out);
81  } else { //occ [0.25,0.65]
82  fputc(205, out);
83  }
84  }
85  }
86 
87  fclose(out);
88 
89 
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");
93 
94 
95  /*
96 resolution: 0.100000
97 origin: [0.000000, 0.000000, 0.000000]
98 #
99 negate: 0
100 occupied_thresh: 0.65
101 free_thresh: 0.196
102 
103  */
104 
105  geometry_msgs::Quaternion orientation = map->info.origin.orientation;
107  orientation.x,
108  orientation.y,
109  orientation.z,
110  orientation.w
111  ));
112  double yaw, pitch, roll;
113  mat.getEulerYPR(yaw, pitch, roll);
114 
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);
117 
118  fclose(yaml);
119 
120  ROS_INFO("Done\n");
121  saved_map_ = true;
122  }
123 
124  std::string mapname_;
129 
130 };
131 
132 #define USAGE "Usage: \n" \
133  " map_saver -h\n"\
134  " map_saver [--occ <threshold_occupied>] [--free <threshold_free>] [-f <mapname>] [ROS remapping args]"
135 
136 int main(int argc, char** argv)
137 {
138  ros::init(argc, argv, "map_saver");
139  std::string mapname = "map";
140  int threshold_occupied = 65;
141  int threshold_free = 25;
142 
143  for(int i=1; i<argc; i++)
144  {
145  if(!strcmp(argv[i], "-h"))
146  {
147  puts(USAGE);
148  return 0;
149  }
150  else if(!strcmp(argv[i], "-f"))
151  {
152  if(++i < argc)
153  mapname = argv[i];
154  else
155  {
156  puts(USAGE);
157  return 1;
158  }
159  }
160  else if (!strcmp(argv[i], "--occ"))
161  {
162  if (++i < argc)
163  {
164  threshold_occupied = std::atoi(argv[i]);
165  if (threshold_occupied < 1 || threshold_occupied > 100)
166  {
167  ROS_ERROR("threshold_occupied must be between 1 and 100");
168  return 1;
169  }
170 
171  }
172  else
173  {
174  puts(USAGE);
175  return 1;
176  }
177  }
178  else if (!strcmp(argv[i], "--free"))
179  {
180  if (++i < argc)
181  {
182  threshold_free = std::atoi(argv[i]);
183  if (threshold_free < 0 || threshold_free > 100)
184  {
185  ROS_ERROR("threshold_free must be between 0 and 100");
186  return 1;
187  }
188 
189  }
190  else
191  {
192  puts(USAGE);
193  return 1;
194  }
195  }
196  else
197  {
198  puts(USAGE);
199  return 1;
200  }
201  }
202 
203  if (threshold_occupied <= threshold_free)
204  {
205  ROS_ERROR("threshold_free must be smaller than threshold_occupied");
206  return 1;
207  }
208 
209  MapGenerator mg(mapname, threshold_occupied, threshold_free);
210 
211  while(!mg.saved_map_ && ros::ok())
212  ros::spinOnce();
213 
214  return 0;
215 }
216 
217 
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 threshold_free_
Definition: map_saver.cpp:128
int main(int argc, char **argv)
Definition: map_saver.cpp:136
Map generation node.
Definition: map_saver.cpp:43
#define ROS_INFO(...)
ROSCPP_DECL bool ok()
MapGenerator(const std::string &mapname, int threshold_occupied, int threshold_free)
Definition: map_saver.cpp:47
std::string mapname_
Definition: map_saver.cpp:124
#define USAGE
Definition: map_saver.cpp:132
ros::Subscriber map_sub_
Definition: map_saver.cpp:125
void mapCallback(const nav_msgs::OccupancyGridConstPtr &map)
Definition: map_saver.cpp:55
int threshold_occupied_
Definition: map_saver.cpp:127
ROSCPP_DECL void spinOnce()
#define ROS_ERROR(...)
void getEulerYPR(tf2Scalar &yaw, tf2Scalar &pitch, tf2Scalar &roll, unsigned int solution_number=1) const


map_server
Author(s): Brian Gerkey, Tony Pratkanis, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:44:06