save_maps.cpp
Go to the documentation of this file.
1 /*
2  * map_saver
3  * Copyright (c) 2008, Willow Garage, Inc.
4  * Copyright (c) 2016, the neonavigation authors
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions are met:
9  *
10  * * Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * * Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in the
14  * documentation and/or other materials provided with the distribution.
15  * * Neither the name of the <ORGANIZATION> nor the names of its
16  * contributors may be used to endorse or promote products derived from
17  * this software without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
23  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29  * POSSIBILITY OF SUCH DAMAGE.
30  */
31 
32 #include <ros/ros.h>
33 #include <ros/console.h>
34 #include <nav_msgs/GetMap.h>
36 #include <geometry_msgs/Quaternion.h>
37 
38 #include <cstdio>
39 #include <string>
40 
41 #include <map_organizer_msgs/OccupancyGridArray.h>
42 
47 {
48 protected:
50  std::string mapname_;
52  bool saved_map_;
53 
54 public:
55  explicit MapGeneratorNode(const std::string& mapname)
56  : nh_()
57  , mapname_(mapname)
58  , saved_map_(false)
59  {
60  ROS_INFO("Waiting for the map");
61  map_sub_ = nh_.subscribe("maps", 1, &MapGeneratorNode::mapsCallback, this);
62  }
63 
64  bool done() const
65  {
66  return saved_map_;
67  }
68  void mapsCallback(const map_organizer_msgs::OccupancyGridArrayConstPtr& maps)
69  {
70  int i = 0;
71  for (auto& map : maps->maps)
72  {
73  mapCallback(&map, i);
74  i++;
75  }
76  saved_map_ = true;
77  }
78  void mapCallback(const nav_msgs::OccupancyGrid* map, const int floor)
79  {
80  ROS_INFO("Received a %d X %d map @ %.3f m/pix",
81  map->info.width,
82  map->info.height,
83  map->info.resolution);
84 
85  std::string mapdatafile = mapname_ + std::to_string(floor) + ".pgm";
86  ROS_INFO("Writing map occupancy data to %s", mapdatafile.c_str());
87  FILE* out = fopen(mapdatafile.c_str(), "w");
88  if (!out)
89  {
90  ROS_ERROR("Couldn't save map file to %s", mapdatafile.c_str());
91  return;
92  }
93 
94  fprintf(out, "P5\n# CREATOR: Map_generator.cpp %.3f m/pix\n%d %d\n255\n",
95  map->info.resolution, map->info.width, map->info.height);
96  for (unsigned int y = 0; y < map->info.height; y++)
97  {
98  for (unsigned int x = 0; x < map->info.width; x++)
99  {
100  unsigned int i = x + (map->info.height - y - 1) * map->info.width;
101  if (map->data[i] == 0)
102  { // occ [0,0.1)
103  fputc(254, out);
104  }
105  else if (map->data[i] == +100)
106  { // occ (0.65,1]
107  fputc(000, out);
108  }
109  else
110  { // occ [0.1,0.65]
111  fputc(205, out);
112  }
113  }
114  }
115 
116  fclose(out);
117 
118  std::string mapmetadatafile = mapname_ + std::to_string(floor) + ".yaml";
119  ROS_INFO("Writing map occupancy data to %s", mapmetadatafile.c_str());
120  FILE* yaml = fopen(mapmetadatafile.c_str(), "w");
121 
122  geometry_msgs::Quaternion orientation = map->info.origin.orientation;
123  tf2::Matrix3x3 mat(tf2::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w));
124  double yaw, pitch, roll;
125  mat.getEulerYPR(yaw, pitch, roll);
126 
127  fprintf(yaml, "image: %s\nresolution: %f\n"
128  "origin: [%f, %f, %f]\nheight: %f\n"
129  "negate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n\n",
130  mapdatafile.c_str(), map->info.resolution,
131  map->info.origin.position.x, map->info.origin.position.y, yaw, map->info.origin.position.z);
132 
133  fclose(yaml);
134 
135  ROS_INFO("Done\n");
136  }
137 };
138 
139 #define USAGE "Usage: \n" \
140  " map_saver -h\n" \
141  " map_saver [-f <mapname>] [ROS remapping args]"
142 
143 int main(int argc, char** argv)
144 {
145  ros::init(argc, argv, "save_maps");
146  std::string mapname = "map";
147 
148  for (int i = 1; i < argc; i++)
149  {
150  if (!strcmp(argv[i], "-h"))
151  {
152  puts(USAGE);
153  return 0;
154  }
155  else if (!strcmp(argv[i], "-f"))
156  {
157  if (++i < argc)
158  mapname = argv[i];
159  else
160  {
161  puts(USAGE);
162  return 1;
163  }
164  }
165  else
166  {
167  puts(USAGE);
168  return 1;
169  }
170  }
171 
172  MapGeneratorNode mg(mapname);
173 
174  while (!mg.done() && ros::ok())
175  ros::spinOnce();
176 
177  return 0;
178 }
map_organizer_msgs::OccupancyGridArray maps
Definition: select_map.cpp:41
int main(int argc, char **argv)
Definition: save_maps.cpp:143
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)
bool done() const
Definition: save_maps.cpp:64
#define ROS_INFO(...)
ros::NodeHandle nh_
Definition: save_maps.cpp:49
ROSCPP_DECL bool ok()
std::string mapname_
Definition: save_maps.cpp:50
ros::Subscriber map_sub_
Definition: save_maps.cpp:51
MapGeneratorNode(const std::string &mapname)
Definition: save_maps.cpp:55
void mapCallback(const nav_msgs::OccupancyGrid *map, const int floor)
Definition: save_maps.cpp:78
Map generation node.
Definition: save_maps.cpp:46
#define USAGE
Definition: save_maps.cpp:139
void mapsCallback(const map_organizer_msgs::OccupancyGridArrayConstPtr &maps)
Definition: save_maps.cpp:68
ROSCPP_DECL void spinOnce()
#define ROS_ERROR(...)
void getEulerYPR(tf2Scalar &yaw, tf2Scalar &pitch, tf2Scalar &roll, unsigned int solution_number=1) const


map_organizer
Author(s): Atsushi Watanabe
autogenerated on Tue Jul 9 2019 04:59:56