prepare_graph.cpp
Go to the documentation of this file.
00001 #include <bwi_mapper/topological_mapper.h>
00002 
00003 #include <fstream>
00004 #include <yaml-cpp/yaml.h>
00005 #include <opencv2/core/core.hpp>
00006 #include <opencv2/highgui/highgui.hpp>
00007 
00008 #ifdef HAVE_NEW_YAMLCPP
00009 namespace YAML {
00010   // The >> operator disappeared in yaml-cpp 0.5, so this function is
00011   // added to provide support for code written under the yaml-cpp 0.3 API.
00012   template<typename T>
00013   void operator >> (const YAML::Node& node, T& i)
00014   {
00015     i = node.as<T>();
00016   }
00017 }
00018 #endif
00019 
00020 using namespace bwi_mapper;
00021 
00022 void drawElementsFile(const std::string& elements_file, cv::Mat& image,
00023     const Graph& graph, const nav_msgs::MapMetaData& info) {
00024 
00025   std::ifstream fin(elements_file.c_str());
00026 
00027   YAML::Node node;
00028   bool put_text = true;
00029   bool put_all_edges = true;
00030   std::vector<std::pair<size_t, size_t> > edges;
00031   std::vector<std::pair<size_t, float> > arrows;
00032   std::vector<int> circles, squares;
00033 #ifdef HAVE_NEW_YAMLCPP
00034   std::vector<YAML::Node> node_vec = YAML::LoadAll(fin);
00035   for(std::vector<YAML::Node>::iterator it = node_vec.begin(); it != node_vec.end(); it++) {
00036     node = *it;
00037 #else
00038   YAML::Parser parser(fin);
00039   while(parser.GetNextDocument(node)) {
00040 #endif
00041 
00042     // Get Edges
00043 #ifdef HAVE_NEW_YAMLCPP
00044     if (node["edges"]) {
00045 #else
00046     if (node.FindValue("edges")) {
00047 #endif
00048       put_all_edges = false;
00049       const YAML::Node& edges_node = node["edges"];
00050       for(unsigned i = 0; i < edges_node.size(); ++i) {
00051         std::pair<size_t, size_t> edge;
00052         edges_node[i][0] >> edge.first;
00053         edges_node[i][1] >> edge.second;
00054         edges.push_back(edge);
00055       }
00056     }
00057 
00058     // Get Circles
00059 #ifdef HAVE_NEW_YAMLCPP
00060     if (node["circles"]) {
00061 #else
00062     if (node.FindValue("circles")) {
00063 #endif
00064       const YAML::Node& circles_node = node["circles"];
00065       for(unsigned i = 0; i < circles_node.size(); ++i) {
00066         int circle;
00067         circles_node[i] >> circle;
00068         circles.push_back(circle);
00069       }
00070     }
00071 
00072     // Get Squares
00073 #ifdef HAVE_NEW_YAMLCPP
00074     if (node["squares"]) {
00075 #else
00076     if (node.FindValue("squares")) {
00077 #endif
00078       const YAML::Node& squares_node = node["squares"];
00079       for(unsigned i = 0; i < squares_node.size(); ++i) {
00080         int square;
00081         squares_node[i] >> square;
00082         squares.push_back(square);
00083       }
00084     }
00085 
00086     // Get Directional arrows
00087 #ifdef HAVE_NEW_YAMLCPP
00088     if (node["arrows"]) {
00089 #else
00090     if (node.FindValue("arrows")) {
00091 #endif
00092       const YAML::Node& arrows_node = node["arrows"];
00093       for(unsigned i = 0; i < arrows_node.size(); ++i) {
00094         std::pair<size_t, float> arrow;
00095         arrows_node[i][0] >> arrow.first;
00096         size_t direction;
00097         arrows_node[i][1] >> direction;
00098         arrow.second = ((2 * M_PI) / 16) * direction + M_PI/2;
00099         arrows.push_back(arrow);
00100       }
00101     }
00102 
00103 #ifdef HAVE_NEW_YAMLCPP
00104     if (node["put_text"]) {
00105 #else
00106     if (node.FindValue("put_text")) {
00107 #endif
00108       const YAML::Node& put_text_node = node["put_text"];
00109       put_text_node >> put_text;
00110     }
00111   }
00112 
00113   bwi_mapper::drawGraph(image, graph, 0, 0, put_text, 
00114       put_all_edges, edges);
00115   for(unsigned i = 0; i < circles.size(); ++i) {
00116     drawCircleOnGraph(image, graph, circles[i]);  
00117   }
00118   for(unsigned i = 0; i < squares.size(); ++i) {
00119     drawSquareOnGraph(image, graph, squares[i]);//, cv::Scalar(0,0,255),0,0,30,-1);  
00120   }
00121   for(unsigned i = 0; i < arrows.size(); ++i) {
00122     drawArrowOnGraph(image, graph, arrows[i], info.width, info.height);  
00123   }
00124 
00125 }
00126 
00127 int main(int argc, char** argv) {
00128 
00129   if (argc < 4) {
00130     std::cerr << "USAGE: " << argv[0] 
00131         << " <yaml-map-file> <yaml-graph-file> <yaml-elements-file>" << std::endl;
00132     return -1;
00133   }
00134 
00135   bwi_mapper::TopologicalMapper mapper(argv[1]);
00136   bwi_mapper::Graph graph;
00137   nav_msgs::MapMetaData info;
00138   mapper.getMapInfo(info);
00139   bwi_mapper::readGraphFromFile(argv[2], info, graph);
00140 
00141   cv::Mat image;
00142   mapper.drawMap(image);
00143   drawElementsFile(argv[3], image, graph, info);
00144 
00145   cv::namedWindow("Display window", CV_WINDOW_AUTOSIZE);
00146   cv::imshow("Display window", image);                
00147   cv::imwrite("out.png", image);                
00148 
00149   cv::waitKey(0); // Wait for a keystroke in the window
00150   return 0;
00151 }
00152 


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