00001 00009 /***************************************************************************** 00010 ** Includes 00011 *****************************************************************************/ 00012 00013 #include <fstream> 00014 00015 #include "yocs_cmd_vel_mux/cmd_vel_subscribers.hpp" 00016 #include "yocs_cmd_vel_mux/exceptions.hpp" 00017 00018 /***************************************************************************** 00019 ** Namespaces 00020 *****************************************************************************/ 00021 00022 namespace yocs_cmd_vel_mux { 00023 00024 /***************************************************************************** 00025 ** Implementation 00026 *****************************************************************************/ 00027 00028 void CmdVelSubscribers::CmdVelSubs::operator << (const YAML::Node& node) 00029 { 00030 node["name"] >> name; 00031 node["topic"] >> topic; 00032 node["timeout"] >> timeout; 00033 node["priority"] >> priority; 00034 #ifdef HAVE_NEW_YAMLCPP 00035 if (node["short_desc"]) { 00036 #else 00037 if (node.FindValue("short_desc") != NULL) { 00038 #endif 00039 node["short_desc"] >> short_desc; 00040 } 00041 } 00042 00043 void CmdVelSubscribers::configure(const YAML::Node& node) { 00044 00045 list.clear(); 00046 try 00047 { 00048 if ( node.size() == 0 ) { 00049 throw EmptyCfgException(); 00050 } 00051 00052 for (unsigned int i = 0; i < node.size(); i++) 00053 { 00054 // Parse every entries on YAML 00055 CmdVelSubs subscriber(i); 00056 subscriber << node[i]; 00057 list.push_back(subscriber); 00058 } 00059 } 00060 catch(EmptyCfgException& e) { 00061 throw e; 00062 } 00063 catch(YAML::ParserException& e) 00064 { 00065 throw YamlException(e.what()); 00066 } 00067 catch(YAML::RepresentationException& e) 00068 { 00069 throw YamlException(e.what()); 00070 } 00071 } 00072 00073 00074 } // namespace yocs_cmd_vel_mux