Merge branch 'devel' into write-state-tracker-demo

include/core/utils/converter.h

@@ -189,7 +189,7 @@ struct converter<Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCo
             int ncols = (int) values.size() / _Rows;
             m.resize(_Rows, ncols);
         }
-        if(m.rows()*m.cols() != values.size()) throw std::runtime_error("The literal string provides " + std::to_string(values.size()) + " values but "
+        if(m.rows()*m.cols() != (int) values.size()) throw std::runtime_error("The literal string provides " + std::to_string(values.size()) + " values but "
                                                                         + "the Eigen matrix is of dimensions "
                                                                         + std::to_string(m.rows()) + "x" + std::to_string(m.cols()));
         else{

include/core/yaml/parser_yaml.hpp

@@ -191,10 +191,8 @@ class ParserYAML {
     std::string _active_name;
     std::vector<ParamsInitSensor> _paramsSens;
     std::vector<ParamsInitProcessor> _paramsProc;
-    std::vector<std::string> _raw_libs;
     std::stack<std::string> _parsing_file;
     std::string _file;
-    bool _relative_path;
     std::string _path_root;
     std::vector<SubscriberManager> _subscriber_managers;
     YAML::Node problem;
@@ -202,34 +200,27 @@ class ParserYAML {
     YAML::Node loadYAML(std::string);
     void insert_register(std::string, std::string);
 public:
-    ParserYAML(bool freely_parse = false): ParserYAML("", "", freely_parse){
-    }
-    ParserYAML(std::string file, bool freely_parse = false)
-        : ParserYAML(file, "", freely_parse) {}
-    ParserYAML(std::string file, std::string path_root,
+    ParserYAML(std::string file, std::string path_root = "",
                bool freely_parse = false) {
       _params = std::map<std::string, std::string>();
       _active_name = "";
       _paramsSens = std::vector<ParamsInitSensor>();
       _paramsProc = std::vector<ParamsInitProcessor>();
       _subscriber_managers = std::vector<SubscriberManager>();
-      _raw_libs = std::vector<std::string>();
       _parsing_file = std::stack<std::string>();
       _file = file;
       if (path_root != "") {
-        std::regex r("/$");
+        std::regex r(".*/ *$");
         if (not std::regex_match(path_root, r))
           _path_root = path_root + "/";
         else
           _path_root = path_root;
-        _relative_path = true;
-      } else {
-        _relative_path = false;
       }
       if(not freely_parse) this->parse();
       else this->parse_freely();
     }
-    ~ParserYAML(){
+    ~ParserYAML()
+    {
         //
     }
     void parse_freely();
@@ -245,12 +236,12 @@ public:
     std::string tagsToString(std::vector<std::string>& tags);
     void parse();
 };
-std::string ParserYAML::generatePath(std::string path){
+std::string ParserYAML::generatePath(std::string file){
     std::regex r("^/.*");
-    if(std::regex_match(path, r)){
-        return path;
+    if(std::regex_match(file, r)){
+        return file;
     }else{
-        return _path_root + path;
+        return _path_root + file;
     }
 }
 YAML::Node ParserYAML::loadYAML(std::string file){
@@ -430,15 +421,8 @@ void ParserYAML::parseFirstLevel(std::string file){
       insert_register("ROS subscriber managers", wolf::converter<std::string>::convert(map_container));
       map_container.clear();
     } catch (YAML::InvalidNode &e) {
-        throw std::runtime_error("Error parsing subscriber managers @" + generatePath(file) + ". Please make sure that each manager has 'package', 'subscriber', 'topic' and 'sensor_name' entries.");
-    }
-
-    //TODO: Defined for testing purposes, maybe should be removed when releasing
-    YAML::Node n_raw_libs = n["raw_libs"];
-    for (const auto &kv : n_raw_libs) {
-      _raw_libs.push_back(kv.Scalar());
+        throw std::runtime_error("Error parsing subscriber managers @" + generatePath(file) + ". Please make sure that each manager has 'package', 'type', 'topic' and 'sensor_name' entries.");
     }
-    insert_register("raw_libs", wolf::converter<std::string>::convert(_raw_libs));
 }
 std::map<std::string,std::string> ParserYAML::getParams(){
     std::map<std::string,std::string> rtn = _params;
@@ -463,17 +447,52 @@ void ParserYAML::parse(){
       this->walkTreeR(it.n , tags , "processor/" + it._name);
     }
     std::list<std::string> plugins, packages;
-    for(const auto &it : this->_paramsSens) plugins.push_back(it._plugin);
-    for (const auto &it : this->_paramsProc) plugins.push_back(it._plugin);
-    for(const auto &it : this->_subscriber_managers) packages.push_back(it._package);
+    for (const auto& it : this->_paramsSens)
+        plugins.push_back(it._plugin);
+    for (const auto& it : this->_paramsProc)
+        plugins.push_back(it._plugin);
+    for (const auto& it : this->_subscriber_managers)
+        packages.push_back(it._package);
     plugins.sort();
     plugins.unique();
     packages.sort();
     packages.unique();
     insert_register("plugins", wolf::converter<std::string>::convert(plugins));
     insert_register("packages", wolf::converter<std::string>::convert(packages));
+
+    YAML::Node n;
+    n = loadYAML(generatePath(this->_file));
+
+    if (n.Type() == YAML::NodeType::Map)
+        {
+            for (auto it : n)
+                {
+                    auto node = it.second;
+                    // WOLF_INFO("WUT ", it.first);
+                    std::vector<std::string> tags = std::vector<std::string>();
+                    if(it.first.as<std::string>() != "config")
+                        this->walkTreeR(node, tags, it.first.as<std::string>());
+                    else
+                        {
+                            for (auto itt : node)
+                                {
+                                    std::string node_key = itt.first.as<std::string>();
+                                    // WOLF_INFO("node_key ", node_key);
+                                    if (node_key != "problem" and node_key != "sensors" and node_key != "processors" and
+                                        node_key != "ROS subscriber managers")
+                                        {
+                                            this->walkTreeR(itt.second, tags, node_key);
+                                        }
+                                }
+                        }
+                }
+        }else
+        {
+            std::vector<std::string> tags = std::vector<std::string>();
+            this->walkTreeR(n, tags, "");
+        }
     this->_parsing_file.pop();
-}
+    }
 void ParserYAML::parse_freely(){
     this->_parsing_file.push(generatePath(this->_file));
     std::vector<std::string> tags = std::vector<std::string>();
@@ -481,6 +500,7 @@ void ParserYAML::parse_freely(){
     this->_parsing_file.pop();
 }
 void ParserYAML::insert_register(std::string key, std::string value){
+    if(key.substr(0,1) == "/") key = key.substr(1,key.size()-1);
   auto inserted_it = _params.insert(std::pair<std::string, std::string>(key, value));
   if(not inserted_it.second) WOLF_WARN("Skipping key '", key, "' with value '", value, "'. There already exists the register: (", inserted_it.first->first, ",", inserted_it.first->second, ")");
 }