Merge branch '219-autoconf-add-state-definition-and-dimension-to-yaml' into devel

include/core/problem/problem.h

@@ -61,7 +61,7 @@ class Problem : public std::enable_shared_from_this<Problem>
 
     public:
         static ProblemPtr create(const std::string& _frame_structure, SizeEigen _dim); // USE THIS AS A CONSTRUCTOR!
-        static ProblemPtr autoSetup(const std::string& _frame_structure, SizeEigen _dim, const std::string& _yaml_file);
+        static ProblemPtr autoSetup(const std::string& _yaml_file);
         virtual ~Problem();
 
         // Properties -----------------------------------------

include/core/yaml/parser_yaml.hpp

@@ -66,6 +66,8 @@ class parserYAML {
     bool _relative_path;
     string _path_root;
     vector<array<string, 3>> _callbacks;
+    YAML::Node problem;
+    std::string generatePath(std::string);
 public:
     parserYAML(){
         _params = map<string, string>();
@@ -96,7 +98,10 @@ public:
         _paramsProc = vector<ParamsInitProcessor>();
         _files = vector<string>();
         _file = file;
-        _path_root = path_root;
+
+        regex r("/$");
+        if(not regex_match(path_root, r)) _path_root = path_root + "/";
+        else _path_root = path_root;
         _relative_path = true;
         _callbacks = vector<array<string, 3>>();
     }
@@ -114,10 +119,21 @@ public:
     vector<array<string, 3>> processorsSerialization();
     vector<string> getFiles();
     vector<array<string, 3>> getCallbacks();
+    vector<array<string, 2>> getProblem();
     map<string,string> getParams();
     void parse();
     map<string, string> fetchAsMap(YAML::Node);
 };
+std::string parserYAML::generatePath(std::string path){
+    regex r("^/.*");
+    if(regex_match(path, r)){
+        std::cout << "Generating path " << path << std::endl;
+        return path;
+    }else{
+        std::cout << "Generating path " << _path_root + path << std::endl;
+        return _path_root + path;
+    }
+}
 string parserYAML::tagsToString(vector<std::string> &tags){
     string hdr = "";
     for(auto it : tags){
@@ -127,24 +143,18 @@ string parserYAML::tagsToString(vector<std::string> &tags){
 }
 void parserYAML::walkTree(string file){
     YAML::Node n;
-    // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl;
-    if(not _relative_path) n = YAML::LoadFile(file);
-    else n = YAML::LoadFile(_path_root + file);
+    n = YAML::LoadFile(generatePath(file));
     vector<string> hdrs = vector<string>();
     walkTreeR(n, hdrs, "");
 }
 void parserYAML::walkTree(string file, vector<string>& tags){
     YAML::Node n;
-    // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl;
-    if(not _relative_path) n = YAML::LoadFile(file);
-    else n = YAML::LoadFile(_path_root + file);
+    n = YAML::LoadFile(generatePath(file));
     walkTreeR(n, tags, "");
 }
 void parserYAML::walkTree(string file, vector<string>& tags, string hdr){
     YAML::Node n;
-    // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl;
-    if(not _relative_path) n = YAML::LoadFile(file);
-    else n = YAML::LoadFile(_path_root + file);
+    n = YAML::LoadFile(generatePath(file));
     walkTreeR(n, tags, hdr);
 }
 void parserYAML::walkTreeR(YAML::Node n, vector<string>& tags, string hdr){
@@ -153,18 +163,13 @@ void parserYAML::walkTreeR(YAML::Node n, vector<string>& tags, string hdr){
         regex r("^@.*");
         if(regex_match(n.Scalar(), r)){
             string str = n.Scalar();
-            // cout << "SUBSTR " << str.substr(1,str.size() - 1);
             walkTree(str.substr(1,str.size() - 1), tags, hdr);
         }else{
-            // std::copy(tags.begin(), tags.end(), std::ostream_iterator<string>(std::cout, "¬"));
-            // cout << "«»" << n.Scalar() << endl;
             _params.insert(pair<string,string>(hdr, n.Scalar()));
         }
         break;
     }
     case YAML::NodeType::Sequence : {
-        // cout << tags[tags.size() - 1] << "«»" << kv << endl;
-        // std::vector<double> vi = n.as<std::vector<double>>();
         string aux = parseSequence(n);
         _params.insert(pair<string,string>(hdr, aux));
         break;
@@ -217,7 +222,7 @@ void parserYAML::walkTreeR(YAML::Node n, vector<string>& tags, string hdr){
         break;
     }
     default:
-        assert(1 == 0 && "Unsupported node Type at walkTreeR");
+        assert(1 == 0 && "Unsupported node Type at walkTreeR.");
         break;
     }
 }
@@ -226,11 +231,11 @@ void parserYAML::updateActiveName(string tag){
 }
 void parserYAML::parseFirstLevel(string file){
     YAML::Node n;
-    // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl;
-    if(not _relative_path) n = YAML::LoadFile(file);
-    else n = YAML::LoadFile(_path_root + file);
+    n = YAML::LoadFile(generatePath(file));
+
     YAML::Node n_config = n["config"];
     assert(n_config.Type() == YAML::NodeType::Map && "trying to parse config node but found a non-Map node");
+    this->problem = n_config["problem"];
     for(const auto& kv : n_config["sensors"]){
         ParamsInitSensor pSensor = {kv["type"].Scalar(), kv["name"].Scalar(), kv};
         _paramsSens.push_back(pSensor);
@@ -243,7 +248,6 @@ void parserYAML::parseFirstLevel(string file){
         _callbacks.push_back({{kv[0].as<std::string>(), kv[1].as<std::string>(), kv[2].as<std::string>()}});
     }
     YAML::Node n_files = n["files"];
-    assert(n_files.Type() == YAML::NodeType::Sequence && "trying to parse files node but found a non-Sequence node");
     for(const auto& kv : n_files){
         _files.push_back(kv.Scalar());
     }
@@ -266,20 +270,26 @@ vector<string> parserYAML::getFiles(){
 vector<array<string, 3>> parserYAML::getCallbacks(){
     return this->_callbacks;
 }
+vector<array<string, 2>> parserYAML::getProblem(){
+    return vector<array<string, 2>>();
+}
 map<string,string> parserYAML::getParams(){
     map<string,string> rtn = _params;
     return rtn;
 }
 void parserYAML::parse(){
     this->parseFirstLevel(this->_file);
+
+    if(this->problem.Type() != YAML::NodeType::Undefined){
+        vector<string> tags = vector<string>();
+        this->walkTreeR(this->problem, tags , "problem");
+    }
     for(auto it : _paramsSens){
         vector<string> tags = vector<string>();
-        // this->walkTreeR(it.n , tags , it._type + "/" + it._name);
         this->walkTreeR(it.n , tags , it._name);
     }
     for(auto it : _paramsProc){
         vector<string> tags = vector<string>();
-        // this->walkTreeR(it.n , tags , it._type + "/" + it._name);
         this->walkTreeR(it.n , tags , it._name);
     }
 }
@@ -295,8 +305,6 @@ map<string, string> parserYAML::fetchAsMap(YAML::Node n){
             break;
         }
         case YAML::NodeType::Sequence : {
-            // cout << tags[tags.size() - 1] << "«»" << kv << endl;
-            // std::vector<double> vi = n.as<std::vector<double>>();
             string aux = parseSequence(kv.second);
             m.insert(pair<string,string>(key, aux));
             break;

src/problem/problem.cpp

@@ -72,13 +72,15 @@ ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim)
     p->setup();
     return p->shared_from_this();
 }
-ProblemPtr Problem::autoSetup(const std::string& _frame_structure, SizeEigen _dim, const std::string& _yaml_file)
+ProblemPtr Problem::autoSetup(const std::string& _yaml_file)
 {
-    auto p = Problem::create(_frame_structure, _dim);
     // string file = "/home/jcasals/catkin_ws/src/wolf_ros_wrapper/src/params.yaml";
     parserYAML parser = parserYAML(_yaml_file);
     parser.parse();
     paramsServer server = paramsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization());
+    std::string frame_structure = server.getParam<std::string>("problem/frame structure", "PO");
+    int dim = server.getParam<int>("problem/dimension", "2");
+    auto p = Problem::create(frame_structure, dim);
     // cout << "PRINTING SERVER MAP" << endl;
     // server.print();
     // cout << "-----------------------------------" << endl;

test/gtest_param_server.cpp

@@ -18,7 +18,7 @@ parserYAML parse(string _file, string _path_root)
 
 TEST(ParamsServer, Default)
 {
-  auto parser = parse("/test/yaml/params1.yaml", wolf_root);
+  auto parser = parse("test/yaml/params1.yaml", wolf_root);
   auto params = parser.getParams();
   paramsServer server = paramsServer(params, parser.sensorsSerialization(), parser.processorsSerialization());
   EXPECT_EQ(server.getParam<double>("should_not_exist", "2.6"), 2.6);

test/gtest_parser_yaml.cpp

@@ -17,7 +17,7 @@ parserYAML parse(string _file, string _path_root)
 
 TEST(ParserYAML, RegularParse)
 {
-  auto parser = parse("/test/yaml/params1.yaml", wolf_root);
+  auto parser = parse("test/yaml/params1.yaml", wolf_root);
   auto params = parser.getParams();
   // for(auto it : params)
   //   cout << it.first << " %% " << it.second << endl;
@@ -26,16 +26,23 @@ TEST(ParserYAML, RegularParse)
 }
 TEST(ParserYAML, ParseMap)
 {
-  auto parser = parse("/test/yaml/params2.yaml", wolf_root);
+  auto parser = parse("test/yaml/params2.yaml", wolf_root);
   auto params = parser.getParams();
   EXPECT_EQ(params["processor1/mymap"], "[{k1:v1},{k2:v2},{k3:[v3,v4,v5]}]");
 }
 TEST(ParserYAML, JumpFile)
 {
-  auto parser = parse("/test/yaml/params3.yaml", wolf_root);
+  auto parser = parse("test/yaml/params3.yaml", wolf_root);
   auto params = parser.getParams();
-  EXPECT_EQ(params["my_proc_test/max_buff_length"], "100");
-  EXPECT_EQ(params["my_proc_test/jump/voting_active"], "false");
+  EXPECT_EQ(params["my_proc_test/extern params/max_buff_length"], "100");
+  EXPECT_EQ(params["my_proc_test/extern params/voting_active"], "false");
+}
+TEST(ParserYAML, ProblemConfig)
+{
+  auto parser = parse("test/yaml/params2.yaml", wolf_root);
+  auto params = parser.getParams();
+  EXPECT_EQ(params["problem/frame structure"], "POV");
+  EXPECT_EQ(params["problem/dimension"], "2");
 }
 int main(int argc, char **argv)
 {

test/params1.yaml

-      follow: "/test/yaml/params3.1.yaml"
\ No newline at end of file

test/params3.yaml

-      jump: "@/test/yaml/params3.1.yaml"
\ No newline at end of file

test/yaml/params1.yaml

 config:
+  problem:
+    frame structure: "POV"
+    dimension: 3
   sensors: 
     -
       type: "ODOM 2D"
@@ -24,7 +27,7 @@ config:
       type: "ODOM 2D"
       name: "my_proc_test"
       sensor name: "odom"
-      follow: "/test/yaml/params3.1.yaml"
+      follow: "test/yaml/params3.1.yaml"
 files:
   - "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so"
   - "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so"
\ No newline at end of file

test/yaml/params2.yaml

 config:
+  problem:
+    frame structure: "POV"
+    dimension: 2
   sensors: 
     -
       type: "ODOM 2D"

test/yaml/params3.yaml

 config:
-  sensors: 
+  # problem:
+  #   frame structure: "POV"
+  #   dimension: 2
+  sensors:
     -
       type: "ODOM 2D"
       name: "odom"
       intrinsic:
         k_disp_to_disp: 0.1
-        k_rot_to_rot: 0.1 
+        k_rot_to_rot: 0.1
       extrinsic:
         pos: [1,2,3]
-    -
-      type: "RANGE BEARING"
-      name: "rb"
   processors:
-    -
-      type: "ODOM 2D"
-      name: "processor1"
-      sensor name: "odom"
-    -
-      type: "RANGE BEARING"
-      name: "rb_processor"
-      sensor name: "rb"
     -
       type: "ODOM 2D"
       name: "my_proc_test"
       sensor name: "odom"
-      follow: "/test/yaml/params3.1.yaml"
-      jump: "@/test/yaml/params3.1.yaml"
+      extern params: "@test/yaml/params3.1.yaml"
\ No newline at end of file
-files:
-  - "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so"
-  - "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so"
\ No newline at end of file