map_loader.cpp
Go to the documentation of this file.
00001 /******************************************************************************
00002    STDR Simulator - Simple Two DImensional Robot Simulator
00003    Copyright (C) 2013 STDR Simulator
00004    This program is free software; you can redistribute it and/or modify
00005    it under the terms of the GNU General Public License as published by
00006    the Free Software Foundation; either version 3 of the License, or
00007    (at your option) any later version.
00008    This program is distributed in the hope that it will be useful,
00009    but WITHOUT ANY WARRANTY; without even the implied warranty of
00010    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00011    GNU General Public License for more details.
00012    You should have received a copy of the GNU General Public License
00013    along with this program; if not, write to the Free Software Foundation,
00014    Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301  USA
00015 
00016    Authors :
00017    * Manos Tsardoulias, etsardou@gmail.com
00018    * Aris Thallas, aris.thallas@gmail.com
00019    * Chris Zalidis, zalidis@gmail.com
00020 ******************************************************************************/
00021 
00022 #include "stdr_server/map_loader.h"
00023 
00024 namespace stdr_server {
00025 
00026   namespace map_loader {
00027 
00033     nav_msgs::OccupancyGrid loadMap(const std::string& fname) {
00034 
00035       nav_msgs::GetMap::Response map_resp_;
00036 
00037       std::string mapfname = "";
00038       std::ifstream fin(fname.c_str());
00039       double origin[3];
00040       double res;
00041       int negate;
00042       double occ_th, free_th;
00043       std::string frame_id = "map";
00044 
00045     #ifdef HAVE_NEW_YAMLCPP
00046       // The document loading process changed in yaml-cpp 0.5.
00047       YAML::Node doc = YAML::Load(fin);
00048     #else
00049       YAML::Parser parser(fin);
00050       YAML::Node doc;
00051       parser.GetNextDocument(doc);
00052     #endif
00053       try {
00054         doc["resolution"] >> res;
00055       } catch (YAML::InvalidScalar) {
00056         ROS_ERROR(
00057           "The map does not contain a resolution tag or it is invalid.");
00058         exit(-1);
00059       }
00060       try {
00061         doc["negate"] >> negate;
00062       } catch (YAML::InvalidScalar) {
00063         ROS_ERROR("The map does not contain a negate tag or it is invalid.");
00064         exit(-1);
00065       }
00066       try {
00067         doc["occupied_thresh"] >> occ_th;
00068       } catch (YAML::InvalidScalar) {
00069         ROS_ERROR(
00070           "The map does not contain an occupied_thresh tag or it is invalid.");
00071         exit(-1);
00072       }
00073       try {
00074         doc["free_thresh"] >> free_th;
00075       } catch (YAML::InvalidScalar) {
00076         ROS_ERROR(
00077           "The map does not contain a free_thresh tag or it is invalid.");
00078         exit(-1);
00079       }
00080       try {
00081         doc["origin"][0] >> origin[0];
00082         doc["origin"][1] >> origin[1];
00083         doc["origin"][2] >> origin[2];
00084       } catch (YAML::InvalidScalar) {
00085         ROS_ERROR("The map does not contain an origin tag or it is invalid.");
00086         exit(-1);
00087       }
00088       try {
00089         doc["image"] >> mapfname;
00090         // TODO: make this path-handling more robust
00091         if(mapfname.size() == 0)
00092         {
00093         ROS_ERROR("The image tag cannot be an empty string.");
00094         exit(-1);
00095         }
00096         if(mapfname[0] != '/')
00097         {
00098         // dirname can modify what you pass it
00099         char* fname_copy = strdup(fname.c_str());
00100         mapfname = std::string(dirname(fname_copy)) + '/' + mapfname;
00101         free(fname_copy);
00102         }
00103       } catch (YAML::InvalidScalar) {
00104         ROS_ERROR("The map does not contain an image tag or it is invalid.");
00105         exit(-1);
00106       }
00107 
00108       ROS_INFO("Loading map from image \"%s\"", mapfname.c_str());
00109       map_server::loadMapFromFile(&map_resp_,mapfname.c_str(),
00110         res,negate,occ_th,free_th, origin);
00111 
00112       map_resp_.map.info.map_load_time = ros::Time::now();
00113       map_resp_.map.header.frame_id = frame_id;
00114       map_resp_.map.header.stamp = ros::Time::now();
00115       ROS_INFO("Read a %d X %d map @ %.3lf m/cell",
00116            map_resp_.map.info.width,
00117            map_resp_.map.info.height,
00118            map_resp_.map.info.resolution);
00119 
00120       return map_resp_.map;
00121     }
00122 
00123   } // end of namespace map_loader
00124 
00125 } // end of namespace stdr_server


stdr_server
Author(s): Chris Zalidis
autogenerated on Tue Feb 7 2017 03:46:33