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 "std_msgs/String.h"
00047 #include "nav_msgs/MapMetaData.h"
00048 #include "yaml-cpp/yaml.h"
00049
00050 class MapServer
00051 {
00052 public:
00054 MapServer(const std::string& fname, double res)
00055 {
00056 std::string mapfname = "";
00057 double origin[3];
00058 int negate;
00059 double occ_th, free_th;
00060 std::string frame_id;
00061 ros::NodeHandle private_nh("~");
00062 private_nh.param("frame_id", frame_id, std::string("map"));
00063 deprecated = (res != 0);
00064 if (!deprecated) {
00065
00066
00067 std::ifstream fin(fname.c_str());
00068 if (fin.fail()) {
00069 ROS_ERROR("Map_server could not open %s.", fname.c_str());
00070 exit(-1);
00071 }
00072 YAML::Parser parser(fin);
00073 YAML::Node doc;
00074 parser.GetNextDocument(doc);
00075 try {
00076 doc["resolution"] >> res;
00077 } catch (YAML::InvalidScalar) {
00078 ROS_ERROR("The map does not contain a resolution tag or it is invalid.");
00079 exit(-1);
00080 }
00081 try {
00082 doc["negate"] >> negate;
00083 } catch (YAML::InvalidScalar) {
00084 ROS_ERROR("The map does not contain a negate tag or it is invalid.");
00085 exit(-1);
00086 }
00087 try {
00088 doc["occupied_thresh"] >> occ_th;
00089 } catch (YAML::InvalidScalar) {
00090 ROS_ERROR("The map does not contain an occupied_thresh tag or it is invalid.");
00091 exit(-1);
00092 }
00093 try {
00094 doc["free_thresh"] >> free_th;
00095 } catch (YAML::InvalidScalar) {
00096 ROS_ERROR("The map does not contain a free_thresh tag or it is invalid.");
00097 exit(-1);
00098 }
00099 try {
00100 doc["map_id"] >> map_id;
00101 } catch (YAML::InvalidScalar) {
00102 ROS_WARN("The map does not contain a map_id tag or it is invalid.");
00103 map_id = fname;
00104 }
00105 try {
00106 doc["origin"][0] >> origin[0];
00107 doc["origin"][1] >> origin[1];
00108 doc["origin"][2] >> origin[2];
00109 } catch (YAML::InvalidScalar) {
00110 ROS_ERROR("The map does not contain an origin tag or it is invalid.");
00111 exit(-1);
00112 }
00113 try {
00114 doc["image"] >> mapfname;
00115
00116 if(mapfname.size() == 0)
00117 {
00118 ROS_ERROR("The image tag cannot be an empty string.");
00119 exit(-1);
00120 }
00121 if(mapfname[0] != '/')
00122 {
00123
00124 char* fname_copy = strdup(fname.c_str());
00125 mapfname = std::string(dirname(fname_copy)) + '/' + mapfname;
00126 free(fname_copy);
00127 }
00128 } catch (YAML::InvalidScalar) {
00129 ROS_ERROR("The map does not contain an image tag or it is invalid.");
00130 exit(-1);
00131 }
00132 } else {
00133 private_nh.param("negate", negate, 0);
00134 private_nh.param("occupied_thresh", occ_th, 0.65);
00135 private_nh.param("free_thresh", free_th, 0.196);
00136 mapfname = fname;
00137 origin[0] = origin[1] = origin[2] = 0.0;
00138 }
00139
00140 ROS_INFO("Loading map from image \"%s\"", mapfname.c_str());
00141 map_server::loadMapFromFile(&map_resp_,mapfname.c_str(),res,negate,occ_th,free_th, origin);
00142 map_resp_.map.info.map_load_time = ros::Time::now();
00143 map_resp_.map.header.frame_id = frame_id;
00144 map_resp_.map.header.stamp = ros::Time::now();
00145 ROS_INFO("Read a %d X %d map @ %.3lf m/cell",
00146 map_resp_.map.info.width,
00147 map_resp_.map.info.height,
00148 map_resp_.map.info.resolution);
00149 meta_data_message_ = map_resp_.map.info;
00150
00151
00152 metadata_pub= n.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
00153 metadata_pub.publish( meta_data_message_ );
00154
00155 }
00156
00157 void SwitchPublisher(bool up) {
00158 if( up ) {
00159
00160 map_pub = n.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
00161 map_pub.publish( map_resp_.map );
00162 } else {
00163 map_pub.shutdown();
00164 }
00165 }
00166
00168 bool mapCallback(nav_msgs::GetMap::Request &req,
00169 nav_msgs::GetMap::Response &res )
00170 {
00171
00172
00173
00174 res = map_resp_;
00175 ROS_INFO("Sending map");
00176
00177 return true;
00178 }
00179
00180 std::string map_id;
00181
00182 private:
00183 ros::NodeHandle n;
00184 ros::Publisher metadata_pub;
00185 ros::Publisher map_pub;
00186 bool deprecated;
00187
00190 nav_msgs::MapMetaData meta_data_message_;
00191 nav_msgs::GetMap::Response map_resp_;
00192
00193
00194
00195
00196
00197
00198
00199
00200 };
00201
00202 std::map< std::string, MapServer > msv;
00203 std::string current_map;
00204
00205 bool mapCallback(nav_msgs::GetMap::Request &req,
00206 nav_msgs::GetMap::Response &res ) {
00207 if(msv.find(current_map) != msv.end()) {
00208 msv.find(current_map)->second.mapCallback(req,res);
00209 }
00210 return true;
00211 }
00212
00213 void MapSelect(const std_msgs::StringConstPtr& msg) {
00214 if(msv.find(msg->data) != msv.end()) {
00215 current_map = msg->data;
00216 msv.find(current_map)->second.SwitchPublisher(true);
00217 ROS_INFO("map[%s] is available.", msg->data.c_str());
00218 }
00219 }
00220
00221 int main(int argc, char **argv)
00222 {
00223 ros::init(argc, argv, "map_server", ros::init_options::AnonymousName);
00224
00225 ros::NodeHandle n;
00226 ros::ServiceServer service;
00227 ros::Subscriber sub;
00228 service = n.advertiseService("static_map", &mapCallback);
00229 sub = n.subscribe("/map_reload", 1, MapSelect);
00230
00231 try
00232 {
00233 for(int i=1; i < argc; i++) {
00234 MapServer ms = MapServer(argv[i],0.0);
00235 std::string id = ms.map_id;
00236 msv.insert(make_pair(id, ms));
00237 if(i==1) {
00238 current_map = id;
00239 msv.find(id)->second.SwitchPublisher(true);
00240 }
00241 }
00242
00243 ros::spin();
00244 }
00245 catch(std::runtime_error& e)
00246 {
00247 ROS_ERROR("map_server exception: %s", e.what());
00248 return -1;
00249 }
00250
00251 return 0;
00252 }
00253