main.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
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 Willow Garage, Inc. 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 /* Author: Brian Gerkey */
31 
32 #define USAGE "\nUSAGE: map_server <map.yaml>\n" \
33  " map.yaml: map description file\n" \
34  "DEPRECATED USAGE: map_server <map> <resolution>\n" \
35  " map: image file to load\n"\
36  " resolution: map resolution [meters/pixel]"
37 
38 #include <stdio.h>
39 #include <stdlib.h>
40 #include <libgen.h>
41 #include <fstream>
42 
43 #include "ros/ros.h"
44 #include "ros/console.h"
46 #include "nav_msgs/MapMetaData.h"
47 #include "yaml-cpp/yaml.h"
48 
49 #ifdef HAVE_YAMLCPP_GT_0_5_0
50 // The >> operator disappeared in yaml-cpp 0.5, so this function is
51 // added to provide support for code written under the yaml-cpp 0.3 API.
52 template<typename T>
53 void operator >> (const YAML::Node& node, T& i)
54 {
55  i = node.as<T>();
56 }
57 #endif
58 
59 class MapServer
60 {
61  public:
63  MapServer(const std::string& fname, double res)
64  {
65  std::string mapfname = "";
66  double origin[3];
67  int negate;
68  double occ_th, free_th;
69  MapMode mode = TRINARY;
70  std::string frame_id;
71  ros::NodeHandle private_nh("~");
72  private_nh.param("frame_id", frame_id, std::string("map"));
73  deprecated = (res != 0);
74  if (!deprecated) {
75  //mapfname = fname + ".pgm";
76  //std::ifstream fin((fname + ".yaml").c_str());
77  std::ifstream fin(fname.c_str());
78  if (fin.fail()) {
79  ROS_ERROR("Map_server could not open %s.", fname.c_str());
80  exit(-1);
81  }
82 #ifdef HAVE_YAMLCPP_GT_0_5_0
83  // The document loading process changed in yaml-cpp 0.5.
84  YAML::Node doc = YAML::Load(fin);
85 #else
86  YAML::Parser parser(fin);
87  YAML::Node doc;
88  parser.GetNextDocument(doc);
89 #endif
90  try {
91  doc["resolution"] >> res;
92  } catch (YAML::InvalidScalar &) {
93  ROS_ERROR("The map does not contain a resolution tag or it is invalid.");
94  exit(-1);
95  }
96  try {
97  doc["negate"] >> negate;
98  } catch (YAML::InvalidScalar &) {
99  ROS_ERROR("The map does not contain a negate tag or it is invalid.");
100  exit(-1);
101  }
102  try {
103  doc["occupied_thresh"] >> occ_th;
104  } catch (YAML::InvalidScalar &) {
105  ROS_ERROR("The map does not contain an occupied_thresh tag or it is invalid.");
106  exit(-1);
107  }
108  try {
109  doc["free_thresh"] >> free_th;
110  } catch (YAML::InvalidScalar &) {
111  ROS_ERROR("The map does not contain a free_thresh tag or it is invalid.");
112  exit(-1);
113  }
114  try {
115  std::string modeS = "";
116  doc["mode"] >> modeS;
117 
118  if(modeS=="trinary")
119  mode = TRINARY;
120  else if(modeS=="scale")
121  mode = SCALE;
122  else if(modeS=="raw")
123  mode = RAW;
124  else{
125  ROS_ERROR("Invalid mode tag \"%s\".", modeS.c_str());
126  exit(-1);
127  }
128  } catch (YAML::Exception &) {
129  ROS_DEBUG("The map does not contain a mode tag or it is invalid... assuming Trinary");
130  mode = TRINARY;
131  }
132  try {
133  doc["origin"][0] >> origin[0];
134  doc["origin"][1] >> origin[1];
135  doc["origin"][2] >> origin[2];
136  } catch (YAML::InvalidScalar &) {
137  ROS_ERROR("The map does not contain an origin tag or it is invalid.");
138  exit(-1);
139  }
140  try {
141  doc["image"] >> mapfname;
142  // TODO: make this path-handling more robust
143  if(mapfname.size() == 0)
144  {
145  ROS_ERROR("The image tag cannot be an empty string.");
146  exit(-1);
147  }
148  if(mapfname[0] != '/')
149  {
150  // dirname can modify what you pass it
151  char* fname_copy = strdup(fname.c_str());
152  mapfname = std::string(dirname(fname_copy)) + '/' + mapfname;
153  free(fname_copy);
154  }
155  } catch (YAML::InvalidScalar &) {
156  ROS_ERROR("The map does not contain an image tag or it is invalid.");
157  exit(-1);
158  }
159  } else {
160  private_nh.param("negate", negate, 0);
161  private_nh.param("occupied_thresh", occ_th, 0.65);
162  private_nh.param("free_thresh", free_th, 0.196);
163  mapfname = fname;
164  origin[0] = origin[1] = origin[2] = 0.0;
165  }
166 
167  ROS_INFO("Loading map from image \"%s\"", mapfname.c_str());
168  try
169  {
170  map_server::loadMapFromFile(&map_resp_,mapfname.c_str(),res,negate,occ_th,free_th, origin, mode);
171  }
172  catch (std::runtime_error e)
173  {
174  ROS_ERROR("%s", e.what());
175  exit(-1);
176  }
177  // To make sure get a consistent time in simulation
179  map_resp_.map.info.map_load_time = ros::Time::now();
180  map_resp_.map.header.frame_id = frame_id;
181  map_resp_.map.header.stamp = ros::Time::now();
182  ROS_INFO("Read a %d X %d map @ %.3lf m/cell",
183  map_resp_.map.info.width,
184  map_resp_.map.info.height,
185  map_resp_.map.info.resolution);
186  meta_data_message_ = map_resp_.map.info;
187 
188  service = n.advertiseService("static_map", &MapServer::mapCallback, this);
189  //pub = n.advertise<nav_msgs::MapMetaData>("map_metadata", 1,
190 
191  // Latched publisher for metadata
192  metadata_pub= n.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
194 
195  // Latched publisher for data
196  map_pub = n.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
197  map_pub.publish( map_resp_.map );
198  }
199 
200  private:
206 
208  bool mapCallback(nav_msgs::GetMap::Request &req,
209  nav_msgs::GetMap::Response &res )
210  {
211  // request is empty; we ignore it
212 
213  // = operator is overloaded to make deep copy (tricky!)
214  res = map_resp_;
215  ROS_INFO("Sending map");
216 
217  return true;
218  }
219 
222  nav_msgs::MapMetaData meta_data_message_;
223  nav_msgs::GetMap::Response map_resp_;
224 
225  /*
226  void metadataSubscriptionCallback(const ros::SingleSubscriberPublisher& pub)
227  {
228  pub.publish( meta_data_message_ );
229  }
230  */
231 
232 };
233 
234 int main(int argc, char **argv)
235 {
236  ros::init(argc, argv, "map_server", ros::init_options::AnonymousName);
237  if(argc != 3 && argc != 2)
238  {
239  ROS_ERROR("%s", USAGE);
240  exit(-1);
241  }
242  if (argc != 2) {
243  ROS_WARN("Using deprecated map server interface. Please switch to new interface.");
244  }
245  std::string fname(argv[1]);
246  double res = (argc == 2) ? 0.0 : atof(argv[2]);
247 
248  try
249  {
250  MapServer ms(fname, res);
251  ros::spin();
252  }
253  catch(std::runtime_error& e)
254  {
255  ROS_ERROR("map_server exception: %s", e.what());
256  return -1;
257  }
258 
259  return 0;
260 }
261 
ros::Publisher metadata_pub
Definition: main.cpp:203
bool deprecated
Definition: main.cpp:205
MapMode
Definition: image_loader.h:52
void publish(const boost::shared_ptr< M > &message) const
int main(int argc, char **argv)
Definition: main.cpp:234
nav_msgs::MapMetaData meta_data_message_
Definition: main.cpp:222
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
#define ROS_WARN(...)
ros::ServiceServer service
Definition: main.cpp:204
#define USAGE
Definition: main.cpp:32
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
MapServer(const std::string &fname, double res)
Definition: main.cpp:63
nav_msgs::GetMap::Response map_resp_
Definition: main.cpp:223
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROSCPP_DECL void spin()
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)
static Time now()
ros::Publisher map_pub
Definition: main.cpp:202
ros::NodeHandle n
Definition: main.cpp:201
bool mapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
Definition: main.cpp:208
static bool waitForValid()
#define ROS_ERROR(...)
#define ROS_DEBUG(...)


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