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 "nav_msgs/MapMetaData.h"
00047 #include "yaml-cpp/yaml.h"
00048 
00049 #ifdef HAVE_NEW_YAMLCPP
00050 // The >> operator disappeared in yaml-cpp 0.5, so this function is
00051 // added to provide support for code written under the yaml-cpp 0.3 API.
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       bool trinary = true;
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["trinary"] >> trinary; 
00116         } catch (YAML::Exception) { 
00117           ROS_DEBUG("The map does not contain a trinary tag or it is invalid... assuming true");
00118           trinary = true;
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, trinary);
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       service = n.advertiseService("static_map", &MapServer::mapCallback, this);
00167       //pub = n.advertise<nav_msgs::MapMetaData>("map_metadata", 1,
00168 
00169       // Latched publisher for metadata
00170       metadata_pub= n.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
00171       metadata_pub.publish( meta_data_message_ );
00172       
00173       // Latched publisher for data
00174       map_pub = n.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
00175       map_pub.publish( map_resp_.map );
00176     }
00177 
00178   private:
00179     ros::NodeHandle n;
00180     ros::Publisher map_pub;
00181     ros::Publisher metadata_pub;
00182     ros::ServiceServer service;
00183     bool deprecated;
00184 
00186     bool mapCallback(nav_msgs::GetMap::Request  &req,
00187                      nav_msgs::GetMap::Response &res )
00188     {
00189       // request is empty; we ignore it
00190 
00191       // = operator is overloaded to make deep copy (tricky!)
00192       res = map_resp_;
00193       ROS_INFO("Sending map");
00194 
00195       return true;
00196     }
00197 
00200     nav_msgs::MapMetaData meta_data_message_;
00201     nav_msgs::GetMap::Response map_resp_;
00202 
00203     /*
00204     void metadataSubscriptionCallback(const ros::SingleSubscriberPublisher& pub)
00205     {
00206       pub.publish( meta_data_message_ );
00207     }
00208     */
00209 
00210 };
00211 
00212 int main(int argc, char **argv)
00213 {
00214   ros::init(argc, argv, "map_server", ros::init_options::AnonymousName);
00215   if(argc != 3 && argc != 2)
00216   {
00217     ROS_ERROR("%s", USAGE);
00218     exit(-1);
00219   }
00220   if (argc != 2) {
00221     ROS_WARN("Using deprecated map server interface. Please switch to new interface.");
00222   }
00223   std::string fname(argv[1]);
00224   double res = (argc == 2) ? 0.0 : atof(argv[2]);
00225 
00226   try
00227   {
00228     MapServer ms(fname, res);
00229     ros::spin();
00230   }
00231   catch(std::runtime_error& e)
00232   {
00233     ROS_ERROR("map_server exception: %s", e.what());
00234     return -1;
00235   }
00236 
00237   return 0;
00238 }
00239 


map_server
Author(s): Brian Gerkey, Tony Pratkanis, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:06:52