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 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         //mapfname = fname + ".pgm";
00065         //std::ifstream fin((fname + ".yaml").c_str());
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           // TODO: make this path-handling more robust
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             // dirname can modify what you pass it
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       //pub = n.advertise<nav_msgs::MapMetaData>("map_metadata", 1,
00146 
00147       // Latched publisher for metadata
00148       metadata_pub= n.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
00149       metadata_pub.publish( meta_data_message_ );
00150       
00151       // Latched publisher for data
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       // request is empty; we ignore it
00168 
00169       // = operator is overloaded to make deep copy (tricky!)
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     void metadataSubscriptionCallback(const ros::SingleSubscriberPublisher& pub)
00183     {
00184       pub.publish( meta_data_message_ );
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 


map_server
Author(s): Brian Gerkey, Tony Pratkanis
autogenerated on Mon Oct 6 2014 02:44:29