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]" 46 #include "nav_msgs/MapMetaData.h" 47 #include "yaml-cpp/yaml.h" 49 #ifdef HAVE_YAMLCPP_GT_0_5_0 53 void operator >> (
const YAML::Node& node, T& i)
65 std::string mapfname =
"";
68 double occ_th, free_th;
72 private_nh.
param(
"frame_id", frame_id, std::string(
"map"));
77 std::ifstream fin(fname.c_str());
79 ROS_ERROR(
"Map_server could not open %s.", fname.c_str());
82 #ifdef HAVE_YAMLCPP_GT_0_5_0 84 YAML::Node doc = YAML::Load(fin);
86 YAML::Parser parser(fin);
88 parser.GetNextDocument(doc);
91 doc[
"resolution"] >> res;
92 }
catch (YAML::InvalidScalar &) {
93 ROS_ERROR(
"The map does not contain a resolution tag or it is invalid.");
97 doc[
"negate"] >> negate;
98 }
catch (YAML::InvalidScalar &) {
99 ROS_ERROR(
"The map does not contain a negate tag or it is invalid.");
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.");
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.");
115 std::string modeS =
"";
116 doc[
"mode"] >> modeS;
120 else if(modeS==
"scale")
122 else if(modeS==
"raw")
125 ROS_ERROR(
"Invalid mode tag \"%s\".", modeS.c_str());
128 }
catch (YAML::Exception &) {
129 ROS_DEBUG(
"The map does not contain a mode tag or it is invalid... assuming Trinary");
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.");
141 doc[
"image"] >> mapfname;
143 if(mapfname.size() == 0)
145 ROS_ERROR(
"The image tag cannot be an empty string.");
148 if(mapfname[0] !=
'/')
151 char* fname_copy = strdup(fname.c_str());
152 mapfname = std::string(dirname(fname_copy)) +
'/' + mapfname;
155 }
catch (YAML::InvalidScalar &) {
156 ROS_ERROR(
"The map does not contain an image tag or it is invalid.");
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);
164 origin[0] = origin[1] = origin[2] = 0.0;
167 ROS_INFO(
"Loading map from image \"%s\"", mapfname.c_str());
172 catch (std::runtime_error e)
180 map_resp_.map.header.frame_id = frame_id;
182 ROS_INFO(
"Read a %d X %d map @ %.3lf m/cell",
209 nav_msgs::GetMap::Response &res )
234 int main(
int argc,
char **argv)
237 if(argc != 3 && argc != 2)
243 ROS_WARN(
"Using deprecated map server interface. Please switch to new interface.");
245 std::string fname(argv[1]);
246 double res = (argc == 2) ? 0.0 : atof(argv[2]);
253 catch(std::runtime_error& e)
255 ROS_ERROR(
"map_server exception: %s", e.what());
ros::Publisher metadata_pub
void publish(const boost::shared_ptr< M > &message) const
int main(int argc, char **argv)
nav_msgs::MapMetaData meta_data_message_
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)
ros::ServiceServer service
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
MapServer(const std::string &fname, double res)
nav_msgs::GetMap::Response map_resp_
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)
bool mapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
static bool waitForValid()