Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #define USAGE "\nUSAGE: map_server <map.yaml>\n" \
00033 " map.yaml: map description file\n" \
00034 "DEPRECATED USAGE: map_server <map> <resolution>\n" \
00035 " map: image file to load\n"\
00036 " resolution: map resolution [meters/pixel]"
00037
00038 #include <stdio.h>
00039 #include <stdlib.h>
00040 #include <libgen.h>
00041 #include <fstream>
00042
00043 #include "ros/ros.h"
00044 #include "ros/console.h"
00045 #include "map_server/image_loader.h"
00046 #include "nav_msgs/MapMetaData.h"
00047 #include "yaml-cpp/yaml.h"
00048
00049 #ifdef HAVE_NEW_YAMLCPP
00050
00051
00052 template<typename T>
00053 void operator >> (const YAML::Node& node, T& i)
00054 {
00055 i = node.as<T>();
00056 }
00057 #endif
00058
00059 class MapServer
00060 {
00061 public:
00063 MapServer(const std::string& fname, double res)
00064 {
00065 std::string mapfname = "";
00066 double origin[3];
00067 int negate;
00068 double occ_th, free_th;
00069 MapMode mode = TRINARY;
00070 std::string frame_id;
00071 ros::NodeHandle private_nh("~");
00072 private_nh.param("frame_id", frame_id, std::string("map"));
00073 deprecated = (res != 0);
00074 if (!deprecated) {
00075
00076
00077 std::ifstream fin(fname.c_str());
00078 if (fin.fail()) {
00079 ROS_ERROR("Map_server could not open %s.", fname.c_str());
00080 exit(-1);
00081 }
00082 #ifdef HAVE_NEW_YAMLCPP
00083
00084 YAML::Node doc = YAML::Load(fin);
00085 #else
00086 YAML::Parser parser(fin);
00087 YAML::Node doc;
00088 parser.GetNextDocument(doc);
00089 #endif
00090 try {
00091 doc["resolution"] >> res;
00092 } catch (YAML::InvalidScalar) {
00093 ROS_ERROR("The map does not contain a resolution tag or it is invalid.");
00094 exit(-1);
00095 }
00096 try {
00097 doc["negate"] >> negate;
00098 } catch (YAML::InvalidScalar) {
00099 ROS_ERROR("The map does not contain a negate tag or it is invalid.");
00100 exit(-1);
00101 }
00102 try {
00103 doc["occupied_thresh"] >> occ_th;
00104 } catch (YAML::InvalidScalar) {
00105 ROS_ERROR("The map does not contain an occupied_thresh tag or it is invalid.");
00106 exit(-1);
00107 }
00108 try {
00109 doc["free_thresh"] >> free_th;
00110 } catch (YAML::InvalidScalar) {
00111 ROS_ERROR("The map does not contain a free_thresh tag or it is invalid.");
00112 exit(-1);
00113 }
00114 try {
00115 std::string modeS = "";
00116 doc["mode"] >> modeS;
00117
00118 if(modeS=="trinary")
00119 mode = TRINARY;
00120 else if(modeS=="scale")
00121 mode = SCALE;
00122 else if(modeS=="raw")
00123 mode = RAW;
00124 else{
00125 ROS_ERROR("Invalid mode tag \"%s\".", modeS.c_str());
00126 exit(-1);
00127 }
00128 } catch (YAML::Exception) {
00129 ROS_DEBUG("The map does not contain a mode tag or it is invalid... assuming Trinary");
00130 mode = TRINARY;
00131 }
00132 try {
00133 doc["origin"][0] >> origin[0];
00134 doc["origin"][1] >> origin[1];
00135 doc["origin"][2] >> origin[2];
00136 } catch (YAML::InvalidScalar) {
00137 ROS_ERROR("The map does not contain an origin tag or it is invalid.");
00138 exit(-1);
00139 }
00140 try {
00141 doc["image"] >> mapfname;
00142
00143 if(mapfname.size() == 0)
00144 {
00145 ROS_ERROR("The image tag cannot be an empty string.");
00146 exit(-1);
00147 }
00148 if(mapfname[0] != '/')
00149 {
00150
00151 char* fname_copy = strdup(fname.c_str());
00152 mapfname = std::string(dirname(fname_copy)) + '/' + mapfname;
00153 free(fname_copy);
00154 }
00155 } catch (YAML::InvalidScalar) {
00156 ROS_ERROR("The map does not contain an image tag or it is invalid.");
00157 exit(-1);
00158 }
00159 } else {
00160 private_nh.param("negate", negate, 0);
00161 private_nh.param("occupied_thresh", occ_th, 0.65);
00162 private_nh.param("free_thresh", free_th, 0.196);
00163 mapfname = fname;
00164 origin[0] = origin[1] = origin[2] = 0.0;
00165 }
00166
00167 ROS_INFO("Loading map from image \"%s\"", mapfname.c_str());
00168 map_server::loadMapFromFile(&map_resp_,mapfname.c_str(),res,negate,occ_th,free_th, origin, mode);
00169 map_resp_.map.info.map_load_time = ros::Time::now();
00170 map_resp_.map.header.frame_id = frame_id;
00171 map_resp_.map.header.stamp = ros::Time::now();
00172 ROS_INFO("Read a %d X %d map @ %.3lf m/cell",
00173 map_resp_.map.info.width,
00174 map_resp_.map.info.height,
00175 map_resp_.map.info.resolution);
00176 meta_data_message_ = map_resp_.map.info;
00177
00178 service = n.advertiseService("static_map", &MapServer::mapCallback, this);
00179
00180
00181
00182 metadata_pub= n.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
00183 metadata_pub.publish( meta_data_message_ );
00184
00185
00186 map_pub = n.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
00187 map_pub.publish( map_resp_.map );
00188 }
00189
00190 private:
00191 ros::NodeHandle n;
00192 ros::Publisher map_pub;
00193 ros::Publisher metadata_pub;
00194 ros::ServiceServer service;
00195 bool deprecated;
00196
00198 bool mapCallback(nav_msgs::GetMap::Request &req,
00199 nav_msgs::GetMap::Response &res )
00200 {
00201
00202
00203
00204 res = map_resp_;
00205 ROS_INFO("Sending map");
00206
00207 return true;
00208 }
00209
00212 nav_msgs::MapMetaData meta_data_message_;
00213 nav_msgs::GetMap::Response map_resp_;
00214
00215
00216
00217
00218
00219
00220
00221
00222 };
00223
00224 int main(int argc, char **argv)
00225 {
00226 ros::init(argc, argv, "map_server", ros::init_options::AnonymousName);
00227 if(argc != 3 && argc != 2)
00228 {
00229 ROS_ERROR("%s", USAGE);
00230 exit(-1);
00231 }
00232 if (argc != 2) {
00233 ROS_WARN("Using deprecated map server interface. Please switch to new interface.");
00234 }
00235 std::string fname(argv[1]);
00236 double res = (argc == 2) ? 0.0 : atof(argv[2]);
00237
00238 try
00239 {
00240 MapServer ms(fname, res);
00241 ros::spin();
00242 }
00243 catch(std::runtime_error& e)
00244 {
00245 ROS_ERROR("map_server exception: %s", e.what());
00246 return -1;
00247 }
00248
00249 return 0;
00250 }
00251