Partially closes #210. The demos related part of the issue is moved to issue #209

include/core/yaml/parser_yaml.hpp

@@ -180,7 +180,7 @@ void parserYAML::walkTreeR(YAML::Node n, vector<string>& tags, string hdr){
                 The following example shows how the header remains unafected:
                 @my_main_config                |  @some_path
                 - cov_det: 1                   |  - my_value : 23
-                - follow: "some_path"          |
+                - follow: "@some_path"         |
                 - var: 1.2                     |
                 Resulting map:
                    cov_det -> 1
@@ -193,7 +193,7 @@ void parserYAML::walkTreeR(YAML::Node n, vector<string>& tags, string hdr){
                  Which would result from the following yaml files
                  @my_main_config                |  @some_path
                  - cov_det: 1                   |  - my_value : 23
-                 - $follow: "some_path"         |
+                 - $follow: "@some_path"        |
                  - var: 1.2                     |
                 */
                 regex rr("follow");

test/gtest_param_server.cpp

@@ -18,7 +18,7 @@ parserYAML parse(string _file, string _path_root)
 
 TEST(ParamsServer, Default)
 {
-  auto parser = parse("/test/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/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,13 +26,13 @@ TEST(ParserYAML, RegularParse)
 }
 TEST(ParserYAML, ParseMap)
 {
-  auto parser = parse("/test/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/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");

test/gtest_problem.cpp

@@ -111,7 +111,7 @@ TEST(Problem, Installers)
     ProblemPtr P = Problem::create("PO", 3);
     Eigen::Vector7s xs;
 
-    SensorBasePtr    S = P->installSensor   ("ODOM 3D", "odometer",        xs,         wolf_root + "/demos/sensor_odom_3D.yaml");
+    SensorBasePtr    S = P->installSensor   ("ODOM 3D", "odometer",        xs,         wolf_root + "/test/yaml/sensor_odom_3D.yaml");
 
     // install processor tracker (dummy installation under an Odometry sensor -- it's OK for this test)
     ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
@@ -126,7 +126,7 @@ TEST(Problem, Installers)
     ASSERT_FALSE(P->getProcessorMotion());
 
     // install processor motion
-    ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/demos/processor_odom_3D.yaml");
+    ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
 
     // check motion processor is set
     ASSERT_TRUE(P->getProcessorMotion());
@@ -243,7 +243,7 @@ TEST(Problem, StateBlocks)
     Eigen::Vector3s xs2d;
 
     // 2 state blocks, fixed
-    SensorBasePtr    Sm = P->installSensor   ("ODOM 3D", "odometer",xs3d, wolf_root + "/demos/sensor_odom_3D.yaml");
+    SensorBasePtr    Sm = P->installSensor   ("ODOM 3D", "odometer",xs3d, wolf_root + "/test/yaml/sensor_odom_3D.yaml");
     ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
 
     // 2 state blocks, fixed
@@ -256,7 +256,7 @@ TEST(Problem, StateBlocks)
     params->min_features_for_keyframe = 10;
     
     auto pt = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(St, ProcessorTrackerFeatureDummy(params));
-    ProcessorBasePtr pm = P->installProcessor("ODOM 3D",            "odom integrator",      "odometer", wolf_root + "/demos/processor_odom_3D.yaml");
+    ProcessorBasePtr pm = P->installProcessor("ODOM 3D",            "odom integrator",      "odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
 
     // 2 state blocks, estimated
     auto KF = P->emplaceFrame("PO", 3, KEY, xs3d, 0);
@@ -302,7 +302,7 @@ TEST(Problem, Covariances)
     Eigen::Vector7s xs3d;
     Eigen::Vector3s xs2d;
 
-    SensorBasePtr    Sm = P->installSensor   ("ODOM 3D", "odometer",xs3d, wolf_root + "/demos/sensor_odom_3D.yaml");
+    SensorBasePtr    Sm = P->installSensor   ("ODOM 3D", "odometer",xs3d, wolf_root + "/test/yaml/sensor_odom_3D.yaml");
     SensorBasePtr    St = P->installSensor   ("ODOM 2D", "other odometer", xs2d, "");
 
     ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
@@ -313,7 +313,7 @@ TEST(Problem, Covariances)
     auto pt = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(St, ProcessorTrackerFeatureDummy(params));
 
     // St->addProcessor(pt);
-    ProcessorBasePtr pm = P->installProcessor("ODOM 3D",            "odom integrator",      "odometer", wolf_root + "/demos/processor_odom_3D.yaml");
+    ProcessorBasePtr pm = P->installProcessor("ODOM 3D",            "odom integrator",      "odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
 
     // 4 state blocks, estimated
     St->unfixExtrinsics();

test/params1.yaml

-config:
-  sensors: 
+      follow: "/test/yaml/params3.1.yaml"
\ No newline at end of file
-    -
-      type: "ODOM 2D"
-      name: "odom"
-      intrinsic:
-        k_disp_to_disp: 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/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/params3.yaml

-config:
-  sensors: 
+      jump: "@/test/yaml/params3.1.yaml"
\ No newline at end of file
-    -
-      type: "ODOM 2D"
-      name: "odom"
-      intrinsic:
-        k_disp_to_disp: 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/params3.1.yaml"
-      jump: "@/test/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/params1.yaml

+config:
+  sensors: 
+    -
+      type: "ODOM 2D"
+      name: "odom"
+      intrinsic:
+        k_disp_to_disp: 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"
+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/params3.yaml

+config:
+  sensors: 
+    -
+      type: "ODOM 2D"
+      name: "odom"
+      intrinsic:
+        k_disp_to_disp: 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"
+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/processor_odom_3D.yaml

+processor type: "ODOM 3D"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+processor name: "Main odometer"        # This is ignored. The name provided to the SensorFactory prevails
+time tolerance:         0.01  # seconds
+keyframe vote:
+    max time span:      0.2   # seconds
+    max buffer length:  10    # motion deltas
+    dist traveled:      0.5   # meters
+    angle turned:       0.1   # radians (1 rad approx 57 deg, approx 60 deg)
\ No newline at end of file

test/yaml/sensor_odom_3D.yaml

+sensor type: "ODOM 3D"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+sensor name: "Main odometer"        # This is ignored. The name provided to the SensorFactory prevails
+motion variances: 
+    disp_to_disp:   0.02  # m^2   / m
+    disp_to_rot:    0.02  # rad^2 / m
+    rot_to_rot:     0.01  # rad^2 / rad
+    min_disp_var:   0.01  # m^2
+    min_rot_var:    0.01  # rad^2