map_loader.cpp
Go to the documentation of this file.
00001 
00038 #include <iostream>
00039 #include <fstream>
00040 #include <stdio.h>
00041 #include <libgen.h>
00042 
00043 #include <map_server/image_loader.h>
00044 #include <bwi_mapper/map_loader.h>
00045 #include <bwi_mapper/map_utils.h>
00046 
00047 #ifdef HAVE_NEW_YAMLCPP
00048 namespace YAML {
00049   // The >> operator disappeared in yaml-cpp 0.5, so this function is
00050   // added to provide support for code written under the yaml-cpp 0.3 API.
00051   template<typename T>
00052   void operator >> (const YAML::Node& node, T& i)
00053   {
00054     i = node.as<T>();
00055   }
00056 }
00057 #endif
00058 
00059 namespace bwi_mapper {
00060 
00064   MapLoader::MapLoader (const std::string& fname) {
00065 
00066     std::string mapfname = "";   
00067     double origin[3];
00068     int negate;
00069     double occ_th, free_th;
00070     double res;
00071 
00072     // open supplied yaml file
00073     std::ifstream fin(fname.c_str());
00074     if (fin.fail()) {
00075       std::cerr << "FATAL: Map_server could not open: " 
00076         << fname << std::endl;
00077       exit(-1);
00078     }
00079 
00080     // Initilize parameters
00081     YAML::Node doc;
00082 #ifdef HAVE_NEW_YAMLCPP
00083     doc = YAML::Load(fin);
00084 #else
00085     YAML::Parser parser(fin);   
00086     parser.GetNextDocument(doc);
00087 #endif
00088     try { 
00089       doc["resolution"] >> res; 
00090     } catch (YAML::InvalidScalar) { 
00091       std::cerr << "FATAL: The map does not contain a resolution tag "
00092         << "or it is invalid." << std::endl;
00093       exit(-1);
00094     }
00095     try { 
00096       doc["negate"] >> negate; 
00097     } catch (YAML::InvalidScalar) { 
00098       std::cerr << "FATAL: The map does not contain a negate tag "
00099         << "or it is invalid." << std::endl;
00100       exit(-1);
00101     }
00102     try { 
00103       doc["occupied_thresh"] >> occ_th; 
00104     } catch (YAML::InvalidScalar) { 
00105       std::cerr << "FATAL: The map does not contain occupied_thresh tag "
00106         << "or it is invalid." << std::endl;
00107       exit(-1);
00108     }
00109     try { 
00110       doc["free_thresh"] >> free_th; 
00111     } catch (YAML::InvalidScalar) { 
00112       std::cerr << "FATAL: The map does not contain free_thresh tag "
00113         << "or it is invalid." << std::endl;
00114       exit(-1);
00115     }
00116     try { 
00117       doc["origin"][0] >> origin[0]; 
00118       doc["origin"][1] >> origin[1]; 
00119       doc["origin"][2] >> origin[2]; 
00120     } catch (YAML::InvalidScalar) { 
00121       std::cerr << "FATAL: The map does not contain origin tag "
00122         << "or it is invalid." << std::endl;
00123       exit(-1);
00124     }
00125 
00126     // Get image data
00127     try { 
00128       doc["image"] >> mapfname; 
00129       if(mapfname.size() == 0) {
00130         std::cerr << "FATAL: The image tag cannot be an empty string."
00131           << std::endl;
00132         exit(-1);
00133       }
00134       if(mapfname[0] != '/') {
00135         // dirname can modify what you pass it
00136         char* fname_copy = strdup(fname.c_str());
00137         mapfname = std::string(dirname(fname_copy)) + '/' + mapfname;
00138         free(fname_copy);
00139       }
00140     } catch (YAML::InvalidScalar) { 
00141       std::cerr << "FATAL: The map does not contain an image tag "
00142         << "or it is invalid." << std::endl;
00143       exit(-1);
00144     }
00145 
00146     std::cout << "MapLoader: Loading map from image " << mapfname << std::endl;
00147     map_server::loadMapFromFile(&map_resp_, mapfname.c_str(), res, negate, 
00148         free_th, free_th, origin);
00149 
00150   }
00151 
00156   void MapLoader::drawMap(cv::Mat &image, uint32_t orig_x, uint32_t orig_y) {
00157     drawMap(image, map_resp_.map, orig_x, orig_y);
00158   }
00159 
00160   void MapLoader::getMapInfo(nav_msgs::MapMetaData& map) const {
00161     map = map_resp_.map.info;
00162   }
00163 
00164   void MapLoader::getMap(nav_msgs::OccupancyGrid &map) const {
00165     map = map_resp_.map;
00166   }
00167 
00171   void MapLoader::drawMap(cv::Mat &image, const nav_msgs::OccupancyGrid& map, 
00172       uint32_t orig_x, uint32_t orig_y) {
00173 
00174     // Check if matrix has enough space, otherwise make it larger
00175     if (image.data == NULL ||
00176         (uint32_t) image.cols < orig_x + map.info.width ||
00177         (uint32_t) image.rows < orig_y + map.info.height) {
00178       cv::Mat old_mat = image.clone();
00179       image.create(orig_y + map.info.height, orig_x + map.info.width, CV_8UC3);
00180       for (uint32_t j = 0; j < (uint32_t) old_mat.rows; ++j) {
00181         const cv::Vec3b* old_row_j = old_mat.ptr<cv::Vec3b>(j);
00182         cv::Vec3b* row_j = image.ptr<cv::Vec3b>(j);
00183         for (uint32_t i = 0; i < (uint32_t) old_mat.cols; ++i) {
00184           row_j[i] = old_row_j[i];
00185         }
00186       }
00187     }
00188 
00189     // Put map onto image
00190     for (uint32_t j = 0; j < map.info.height; ++j) {
00191       cv::Vec3b* image_row_j = image.ptr<cv::Vec3b>(j + orig_y);
00192       for (uint32_t i = 0; i < map.info.width; ++i) {
00193         uint8_t val = map.data[MAP_IDX(map.info.width, i, j)];
00194         cv::Vec3b& pixel = image_row_j[i + orig_x];
00195         switch (val) {
00196           case 100:
00197             pixel[0] = pixel[1] = pixel[2] = 0;
00198             break;
00199           case 0:
00200             pixel[0] = pixel[1] = pixel[2] = 255;
00201             break;
00202           default:
00203             pixel[0] = pixel[1] = pixel[2] = 128;
00204         }
00205       }
00206     }
00207   }
00208 
00209 } /* bwi_mapper */
00210 
00211 


bwi_mapper
Author(s): Piyush Khandelwal
autogenerated on Thu Jun 6 2019 17:57:21