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 class MapServer
00050 {
00051 public:
00053 MapServer(const std::string& fname, double res)
00054 {
00055 std::string mapfname = "";
00056 double origin[3];
00057 int negate;
00058 double occ_th, free_th;
00059 std::string frame_id;
00060 ros::NodeHandle private_nh("~");
00061 private_nh.param("frame_id", frame_id, std::string("map"));
00062 deprecated = (res != 0);
00063 if (!deprecated) {
00064
00065
00066 std::ifstream fin(fname.c_str());
00067 if (fin.fail()) {
00068 ROS_ERROR("Map_server could not open %s.", fname.c_str());
00069 exit(-1);
00070 }
00071 YAML::Parser parser(fin);
00072 YAML::Node doc;
00073 parser.GetNextDocument(doc);
00074 try {
00075 doc["resolution"] >> res;
00076 } catch (YAML::InvalidScalar) {
00077 ROS_ERROR("The map does not contain a resolution tag or it is invalid.");
00078 exit(-1);
00079 }
00080 try {
00081 doc["negate"] >> negate;
00082 } catch (YAML::InvalidScalar) {
00083 ROS_ERROR("The map does not contain a negate tag or it is invalid.");
00084 exit(-1);
00085 }
00086 try {
00087 doc["occupied_thresh"] >> occ_th;
00088 } catch (YAML::InvalidScalar) {
00089 ROS_ERROR("The map does not contain an occupied_thresh tag or it is invalid.");
00090 exit(-1);
00091 }
00092 try {
00093 doc["free_thresh"] >> free_th;
00094 } catch (YAML::InvalidScalar) {
00095 ROS_ERROR("The map does not contain a free_thresh tag or it is invalid.");
00096 exit(-1);
00097 }
00098 try {
00099 doc["origin"][0] >> origin[0];
00100 doc["origin"][1] >> origin[1];
00101 doc["origin"][2] >> origin[2];
00102 } catch (YAML::InvalidScalar) {
00103 ROS_ERROR("The map does not contain an origin tag or it is invalid.");
00104 exit(-1);
00105 }
00106 try {
00107 doc["image"] >> mapfname;
00108
00109 if(mapfname.size() == 0)
00110 {
00111 ROS_ERROR("The image tag cannot be an empty string.");
00112 exit(-1);
00113 }
00114 if(mapfname[0] != '/')
00115 {
00116
00117 char* fname_copy = strdup(fname.c_str());
00118 mapfname = std::string(dirname(fname_copy)) + '/' + mapfname;
00119 free(fname_copy);
00120 }
00121 } catch (YAML::InvalidScalar) {
00122 ROS_ERROR("The map does not contain an image tag or it is invalid.");
00123 exit(-1);
00124 }
00125 } else {
00126 private_nh.param("negate", negate, 0);
00127 private_nh.param("occupied_thresh", occ_th, 0.65);
00128 private_nh.param("free_thresh", free_th, 0.196);
00129 mapfname = fname;
00130 origin[0] = origin[1] = origin[2] = 0.0;
00131 }
00132
00133 ROS_INFO("Loading map from image \"%s\"", mapfname.c_str());
00134 map_server::loadMapFromFile(&map_resp_,mapfname.c_str(),res,negate,occ_th,free_th, origin);
00135 map_resp_.map.info.map_load_time = ros::Time::now();
00136 map_resp_.map.header.frame_id = frame_id;
00137 map_resp_.map.header.stamp = ros::Time::now();
00138 ROS_INFO("Read a %d X %d map @ %.3lf m/cell",
00139 map_resp_.map.info.width,
00140 map_resp_.map.info.height,
00141 map_resp_.map.info.resolution);
00142 meta_data_message_ = map_resp_.map.info;
00143
00144 service = n.advertiseService("static_map", &MapServer::mapCallback, this);
00145
00146
00147
00148 metadata_pub= n.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
00149 metadata_pub.publish( meta_data_message_ );
00150
00151
00152 map_pub = n.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
00153 map_pub.publish( map_resp_.map );
00154 }
00155
00156 private:
00157 ros::NodeHandle n;
00158 ros::Publisher map_pub;
00159 ros::Publisher metadata_pub;
00160 ros::ServiceServer service;
00161 bool deprecated;
00162
00164 bool mapCallback(nav_msgs::GetMap::Request &req,
00165 nav_msgs::GetMap::Response &res )
00166 {
00167
00168
00169
00170 res = map_resp_;
00171 ROS_INFO("Sending map");
00172
00173 return true;
00174 }
00175
00178 nav_msgs::MapMetaData meta_data_message_;
00179 nav_msgs::GetMap::Response map_resp_;
00180
00181
00182
00183
00184
00185
00186
00187
00188 };
00189
00190 int main(int argc, char **argv)
00191 {
00192 ros::init(argc, argv, "map_server", ros::init_options::AnonymousName);
00193 if(argc != 3 && argc != 2)
00194 {
00195 ROS_ERROR("%s", USAGE);
00196 exit(-1);
00197 }
00198 if (argc != 2) {
00199 ROS_WARN("Using deprecated map server interface. Please switch to new interface.");
00200 }
00201 std::string fname(argv[1]);
00202 double res = (argc == 2) ? 0.0 : atof(argv[2]);
00203
00204 try
00205 {
00206 MapServer ms(fname, res);
00207 ros::spin();
00208 }
00209 catch(std::runtime_error& e)
00210 {
00211 ROS_ERROR("map_server exception: %s", e.what());
00212 return -1;
00213 }
00214
00215 return 0;
00216 }
00217