yocs_ar_marker_tracking: yaml.cpp Source File

Go to the documentation of this file.
00001 /*
00002    AR marker yaml parser
00003 
00004    LICENSE : BSD - https://raw.github.com/yujinrobot/yujin_ocs/license/LICENSE
00005 
00006    Author : Jihoon Lee
00007    Date   : Dec 2013
00008  */
00009 
00010 #include "yocs_ar_marker_tracking/yaml.hpp"
00011 
00012 namespace yocs {
00013 
00014   bool loadAlvarMarkersFromYaml(const std::string& filename,ar_track_alvar_msgs::AlvarMarkers& ams)
00015   {
00016 
00017     ams.markers.clear(); 
00018 
00019     // Yaml File Parsing
00020     try
00021     {
00022       YAML::Node doc;
00023 
00024       getYamlNode(filename, doc);
00025       parseMarkers(doc, ams);
00026     }
00027     catch(YAML::ParserException& e)
00028     {
00029       ROS_ERROR("Parse AR markers file failed: %s", e.what());
00030       return false;
00031     }
00032     catch(YAML::RepresentationException& e)
00033     {
00034       ROS_ERROR("Parse AR Markers file failed: %s", e.what());
00035       return false;
00036     }
00037     catch(std::string& e) {
00038       ROS_ERROR("Parse AR Markers file failed: %s",e.c_str());
00039       return false;
00040     }
00041     return true;
00042   }
00043 
00044   void getYamlNode(const std::string& filename, YAML::Node& node) 
00045   {
00046     std::ifstream ifs(filename.c_str(), std::ifstream::in);
00047     if (ifs.good() == false)
00048     {                                                            
00049       throw std::string("file not found");
00050     }
00051 
00052     #ifdef HAVE_NEW_YAMLCPP
00053     node = YAML::Load(ifs);
00054     #else
00055     YAML::Parser parser(ifs);
00056     parser.GetNextDocument(node);
00057     #endif
00058   }
00059 
00060   void parseMarkers(const YAML::Node& node, ar_track_alvar_msgs::AlvarMarkers& ams) 
00061   {
00062     unsigned int i;
00063 
00064     for(i = 0; i < node.size(); i++) 
00065     {
00066         // Parse AlvarMarker entries on YAML
00067         ar_track_alvar_msgs::AlvarMarker m;
00068 
00069         node[i]["id"] >> m.id;
00070         node[i]["frame_id"] >> m.header.frame_id;
00071         node[i]["frame_id"] >> m.pose.header.frame_id;
00072         node[i]["confidence"] >> m.confidence;
00073         node[i]["pose"]["position"]["x"] >> m.pose.pose.position.x;
00074         node[i]["pose"]["position"]["y"] >> m.pose.pose.position.y;
00075         node[i]["pose"]["position"]["z"] >> m.pose.pose.position.z;
00076         node[i]["pose"]["orientation"]["x"] >> m.pose.pose.orientation.x;
00077         node[i]["pose"]["orientation"]["y"] >> m.pose.pose.orientation.y;
00078         node[i]["pose"]["orientation"]["z"] >> m.pose.pose.orientation.z;
00079         node[i]["pose"]["orientation"]["w"] >> m.pose.pose.orientation.w;
00080 
00081         ams.markers.push_back(m);
00082     }
00083   }
00084 }