tie_maps.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2014-2017, the neonavigation authors
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <ros/ros.h>
31 #include <nav_msgs/OccupancyGrid.h>
32 #include <map_organizer_msgs/OccupancyGridArray.h>
33 
34 #include <stdio.h>
35 #include <stdlib.h>
36 #include <libgen.h>
37 #include <fstream>
38 #include <sstream>
39 #include <string>
40 #include <vector>
41 
43 #include <yaml-cpp/yaml.h>
44 
45 #ifdef HAVE_NEW_YAMLCPP
46 // The >> operator disappeared in yaml-cpp 0.5, so this function is
47 // added to provide support for code written under the yaml-cpp 0.3 API.
48 template <typename T>
49 void operator>>(const YAML::Node& node, T& i)
50 {
51  i = node.as<T>();
52 }
53 #endif
54 
56 {
57 private:
61  std::vector<ros::Publisher> pub_map_;
62 
63 public:
65  : pnh_("~")
66  , nh_()
67  {
68  pub_map_array_ = nh_.advertise<map_organizer_msgs::OccupancyGridArray>("maps", 1, true);
69 
70  map_organizer_msgs::OccupancyGridArray maps;
71 
72  std::string files_str;
73  std::string mapfname;
74  double res;
75  double origin[3], height;
76  int negate;
77  double occ_th, free_th;
78  MapMode mode;
79  std::string frame_id;
80  pnh_.param("map_files", files_str, std::string(""));
81  pnh_.param("frame_id", frame_id, std::string("map"));
82 
83  int i = 0;
84  std::string file;
85  std::stringstream ss(files_str);
86  while (std::getline(ss, file, ','))
87  {
88  std::ifstream fin(file);
89  if (fin.fail())
90  {
91  ROS_ERROR("Map_server could not open %s.", file.c_str());
92  ros::shutdown();
93  return;
94  }
95 #ifdef HAVE_NEW_YAMLCPP
96  // The document loading process changed in yaml-cpp 0.5.
97  YAML::Node doc = YAML::Load(fin);
98 #else
99  YAML::Parser parser(fin);
100  YAML::Node doc;
101  parser.GetNextDocument(doc);
102 #endif
103  try
104  {
105  doc["resolution"] >> res;
106  }
107  catch (YAML::InvalidScalar& e)
108  {
109  ROS_ERROR("The map does not contain a resolution tag or it is invalid: %s", e.what());
110  ros::shutdown();
111  return;
112  }
113  try
114  {
115  doc["negate"] >> negate;
116  }
117  catch (YAML::InvalidScalar& e)
118  {
119  ROS_ERROR("The map does not contain a negate tag or it is invalid: %s", e.what());
120  ros::shutdown();
121  return;
122  }
123  try
124  {
125  doc["occupied_thresh"] >> occ_th;
126  }
127  catch (YAML::InvalidScalar& e)
128  {
129  ROS_ERROR("The map does not contain an occupied_thresh tag or it is invalid: %s", e.what());
130  ros::shutdown();
131  return;
132  }
133  try
134  {
135  doc["free_thresh"] >> free_th;
136  }
137  catch (YAML::InvalidScalar& e)
138  {
139  ROS_ERROR("The map does not contain a free_thresh tag or it is invalid: %s", e.what());
140  ros::shutdown();
141  return;
142  }
143  try
144  {
145  std::string modeS = "";
146  doc["mode"] >> modeS;
147 
148  if (modeS == "trinary")
149  mode = TRINARY;
150  else if (modeS == "scale")
151  mode = SCALE;
152  else if (modeS == "raw")
153  mode = RAW;
154  else
155  {
156  ROS_ERROR("Invalid mode tag \"%s\".", modeS.c_str());
157  exit(-1);
158  }
159  }
160  catch (YAML::Exception& e)
161  {
162  ROS_DEBUG("The map does not contain a mode tag or it is invalid... assuming trinary: %s", e.what());
163  mode = TRINARY;
164  }
165  try
166  {
167  doc["origin"][0] >> origin[0];
168  doc["origin"][1] >> origin[1];
169  doc["origin"][2] >> origin[2];
170  }
171  catch (YAML::InvalidScalar& e)
172  {
173  ROS_ERROR("The map does not contain an origin tag or it is invalid: %s", e.what());
174  ros::shutdown();
175  return;
176  }
177  try
178  {
179  doc["height"] >> height;
180  }
181  catch (YAML::Exception& e)
182  {
183  height = 0;
184  }
185  try
186  {
187  doc["image"] >> mapfname;
188  // TODO(at-wat): make this path-handling more robust
189  if (mapfname.size() == 0)
190  {
191  ROS_ERROR("The image tag cannot be an empty string.");
192  ros::shutdown();
193  return;
194  }
195  if (mapfname[0] != '/')
196  {
197  // dirname can modify what you pass it
198  char* fname_copy = strdup(file.c_str());
199  mapfname = std::string(dirname(fname_copy)) + '/' + mapfname;
200  free(fname_copy);
201  }
202  }
203  catch (YAML::InvalidScalar& e)
204  {
205  ROS_ERROR("The map does not contain an image tag or it is invalid: e.what()");
206  ros::shutdown();
207  return;
208  }
209 
210  ROS_INFO("Loading map from image \"%s\"", mapfname.c_str());
211 
212  nav_msgs::GetMap::Response map_resp;
213  map_server::loadMapFromFile(&map_resp,
214  mapfname.c_str(), res, negate, occ_th, free_th, origin, mode);
215  map_resp.map.info.origin.position.z = height;
216  map_resp.map.info.map_load_time = ros::Time::now();
217  map_resp.map.header.frame_id = frame_id;
218  map_resp.map.header.stamp = ros::Time::now();
219  ROS_INFO("Read a %d X %d map @ %.3lf m/cell",
220  map_resp.map.info.width,
221  map_resp.map.info.height,
222  map_resp.map.info.resolution);
223  maps.maps.push_back(map_resp.map);
224  pub_map_.push_back(nh_.advertise<nav_msgs::OccupancyGrid>(
225  "map" + std::to_string(i), 1, true));
226  pub_map_.back().publish(map_resp.map);
227  i++;
228  }
229  pub_map_array_.publish(maps);
230  }
231 };
232 
233 int main(int argc, char** argv)
234 {
235  ros::init(argc, argv, "tie_maps");
236 
237  TieMapNode tmn;
238  ros::spin();
239 
240  return 0;
241 }
int main(int argc, char **argv)
Definition: tie_maps.cpp:233
map_organizer_msgs::OccupancyGridArray maps
Definition: select_map.cpp:41
ros::NodeHandle pnh_
Definition: tie_maps.cpp:58
MapMode
void publish(const boost::shared_ptr< M > &message) const
std::istream & operator>>(std::istream &is, Vector &v)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
RAW
ROSCPP_DECL void spin(Spinner &spinner)
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
std::vector< ros::Publisher > pub_map_
Definition: tie_maps.cpp:61
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void loadMapFromFile(nav_msgs::GetMap::Response *resp, const char *fname, double res, bool negate, double occ_th, double free_th, double *origin, MapMode mode=TRINARY)
ros::NodeHandle nh_
Definition: tie_maps.cpp:59
ros::Publisher pub_map_array_
Definition: tie_maps.cpp:60
parser
static Time now()
ROSCPP_DECL void shutdown()
SCALE
TRINARY
#define ROS_ERROR(...)
#define ROS_DEBUG(...)


map_organizer
Author(s): Atsushi Watanabe
autogenerated on Wed May 12 2021 02:20:33