main.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /* Author: Brian Gerkey */
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 #ifdef HAVE_NEW_YAMLCPP
00051 // The >> operator disappeared in yaml-cpp 0.5, so this function is
00052 // added to provide support for code written under the yaml-cpp 0.3 API.
00053 template<typename T>
00054 void operator >> (const YAML::Node& node, T& i)
00055 {
00056   i = node.as<T>();
00057 }
00058 #endif
00059 
00060 class MapServer
00061 {
00062   public:
00064     MapServer(const std::string& fname, double res)
00065     {
00066       std::string mapfname = "";
00067       double origin[3];
00068       int negate;
00069       double occ_th, free_th;
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         //mapfname = fname + ".pgm";
00076         //std::ifstream fin((fname + ".yaml").c_str());
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         // The document loading process changed in yaml-cpp 0.5.
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           doc["map_id"] >> map_id;
00116         } catch (YAML::InvalidScalar) {
00117           ROS_WARN("The map does not contain a map_id tag or it is invalid.");
00118           map_id = fname;
00119         }
00120         try { 
00121           doc["origin"][0] >> origin[0]; 
00122           doc["origin"][1] >> origin[1]; 
00123           doc["origin"][2] >> origin[2]; 
00124         } catch (YAML::InvalidScalar) { 
00125           ROS_ERROR("The map does not contain an origin tag or it is invalid.");
00126           exit(-1);
00127         }
00128         try { 
00129           doc["image"] >> mapfname; 
00130           // TODO: make this path-handling more robust
00131           if(mapfname.size() == 0)
00132           {
00133             ROS_ERROR("The image tag cannot be an empty string.");
00134             exit(-1);
00135           }
00136           if(mapfname[0] != '/')
00137           {
00138             // dirname can modify what you pass it
00139             char* fname_copy = strdup(fname.c_str());
00140             mapfname = std::string(dirname(fname_copy)) + '/' + mapfname;
00141             free(fname_copy);
00142           }
00143         } catch (YAML::InvalidScalar) { 
00144           ROS_ERROR("The map does not contain an image tag or it is invalid.");
00145           exit(-1);
00146         }
00147       } else {
00148         private_nh.param("negate", negate, 0);
00149         private_nh.param("occupied_thresh", occ_th, 0.65);
00150         private_nh.param("free_thresh", free_th, 0.196);
00151         mapfname = fname;
00152         origin[0] = origin[1] = origin[2] = 0.0;
00153       }
00154 
00155       ROS_INFO("Loading map from image \"%s\"", mapfname.c_str());
00156       map_server::loadMapFromFile(&map_resp_,mapfname.c_str(),res,negate,occ_th,free_th, origin);
00157       map_resp_.map.info.map_load_time = ros::Time::now();
00158       map_resp_.map.header.frame_id = frame_id;
00159       map_resp_.map.header.stamp = ros::Time::now();
00160       ROS_INFO("Read a %d X %d map @ %.3lf m/cell",
00161                map_resp_.map.info.width,
00162                map_resp_.map.info.height,
00163                map_resp_.map.info.resolution);
00164       meta_data_message_ = map_resp_.map.info;
00165 
00166       // Latched publisher for metadata
00167       metadata_pub= n.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
00168       metadata_pub.publish( meta_data_message_ );
00169 
00170     }
00171 
00172   void SwitchPublisher(bool up) {
00173     if( up ) {
00174       // Latched publisher for data
00175       map_pub = n.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
00176       map_pub.publish( map_resp_.map );
00177     } else {
00178       map_pub.shutdown();
00179     }
00180   }
00181 
00183     bool mapCallback(nav_msgs::GetMap::Request  &req,
00184                      nav_msgs::GetMap::Response &res )
00185     {
00186       // request is empty; we ignore it
00187 
00188       // = operator is overloaded to make deep copy (tricky!)
00189       res = map_resp_;
00190       ROS_INFO("Sending map");
00191 
00192       return true;
00193     }
00194 
00195     std::string map_id;
00196 
00197   private:
00198     ros::NodeHandle n;
00199     ros::Publisher metadata_pub;
00200     ros::Publisher map_pub;
00201     bool deprecated;
00202 
00205     nav_msgs::MapMetaData meta_data_message_;
00206     nav_msgs::GetMap::Response map_resp_;
00207 
00208     /*
00209     void metadataSubscriptionCallback(const ros::SingleSubscriberPublisher& pub)
00210     {
00211       pub.publish( meta_data_message_ );
00212     }
00213     */
00214 
00215 };
00216 
00217 std::map< std::string, MapServer > msv;
00218 std::string current_map;
00219 
00220 bool mapCallback(nav_msgs::GetMap::Request  &req,
00221                  nav_msgs::GetMap::Response &res ) {
00222   if(msv.find(current_map) != msv.end()) {
00223     msv.find(current_map)->second.mapCallback(req,res);
00224   }
00225   return true;
00226 }
00227 
00228 void MapSelect(const std_msgs::StringConstPtr& msg) {
00229   if(msv.find(msg->data) != msv.end()) {
00230     current_map = msg->data;
00231     msv.find(current_map)->second.SwitchPublisher(true);
00232     ROS_INFO("map[%s] is available.", msg->data.c_str());
00233   }
00234 }
00235 
00236 int main(int argc, char **argv)
00237 {
00238   ros::init(argc, argv, "map_server", ros::init_options::AnonymousName);
00239 
00240   ros::NodeHandle n;
00241   ros::ServiceServer service;
00242   ros::Subscriber sub;
00243   service = n.advertiseService("static_map", &mapCallback);
00244   sub = n.subscribe("/map_reload", 1, MapSelect);
00245 
00246   try
00247   {
00248     for(int i=1; i < argc; i++) {
00249       MapServer ms = MapServer(argv[i],0.0);
00250       std::string id = ms.map_id;
00251       msv.insert(make_pair(id, ms));
00252       if(i==1) {
00253         current_map = id;
00254         msv.find(id)->second.SwitchPublisher(true);
00255       }
00256     }
00257 
00258     ros::spin();
00259   }
00260   catch(std::runtime_error& e)
00261   {
00262     ROS_ERROR("map_server exception: %s", e.what());
00263     return -1;
00264   }
00265 
00266   return 0;
00267 }
00268 


multi_map_server
Author(s): Manabu Saito
autogenerated on Mon Oct 6 2014 10:48:38