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       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         //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           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           // TODO: make this path-handling more robust
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             // dirname can modify what you pass it
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       //pub = n.advertise<nav_msgs::MapMetaData>("map_metadata", 1,
00180 
00181       // Latched publisher for metadata
00182       metadata_pub= n.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
00183       metadata_pub.publish( meta_data_message_ );
00184       
00185       // Latched publisher for data
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       // request is empty; we ignore it
00202 
00203       // = operator is overloaded to make deep copy (tricky!)
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     void metadataSubscriptionCallback(const ros::SingleSubscriberPublisher& pub)
00217     {
00218       pub.publish( meta_data_message_ );
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 


map_server
Author(s): Brian Gerkey, Tony Pratkanis, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:45:59