Hello wolf autoconf

The hello_wolf demo now uses the autoconf capabilities of WOLF.

A YAML file is provided to describe all the hello_wolf robot configuration.

The file is parsed to create a parameter server.

The WOLF problem is automatically configured using the server.

The rest of the demo is exactly the same as the original hello_wolf. Both versions coexist.

src/problem/problem.cpp

@@ -73,14 +73,19 @@ ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim)
     p->setup();
     return p->shared_from_this();
 }
+
 ProblemPtr Problem::autoSetup(paramsServer &_server)
 {
-    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);
+    // Problem structure and dimension
+    std::string frame_structure = _server.getParam<std::string> ("problem/frame structure", "PO");
+    int dim                     = _server.getParam<int>         ("problem/dimension", "2");
+    auto problem                = Problem::create(frame_structure, dim);
     // cout << "PRINTING SERVER MAP" << endl;
     // _server.print();
     // cout << "-----------------------------------" << endl;
+    WOLF_TRACE("Setting up problem with frame structure {" + frame_structure + "} and dimension " + std::to_string(dim) + "D");
+
+    // Load plugins
     auto loaders = std::vector<Loader*>();
     for(auto it : _server.getParam<std::vector<std::string>>("files")) {
         WOLF_TRACE("Loading file " + it)
@@ -88,27 +93,37 @@ ProblemPtr Problem::autoSetup(paramsServer &_server)
         l->load();
         loaders.push_back(l);
     }
-    WOLF_TRACE("Setting up problem with frame structure {" + frame_structure + "} and dimension {" + std::to_string(dim) + "}");
-    Eigen::VectorXs prior_state = _server.getParam<Eigen::VectorXs>("problem/prior state");
-    Eigen::MatrixXs prior_cov = _server.getParam<Eigen::MatrixXs>("problem/prior cov");
-    Scalar time_tolerance = _server.getParam<Scalar>("problem/time tolerance");
-    auto ts = TimeStamp();
-    WOLF_TRACE("prior timestamp:\n",ts);
-    WOLF_TRACE("prior state:\n", prior_state.transpose());
-    WOLF_TRACE("prior covariance:\n", prior_cov);
-    WOLF_TRACE("prior time tolerance:\n", time_tolerance);
-    p->setPrior(prior_state, prior_cov, ts, time_tolerance);
+
+    // Install sensors and processors
     auto sensorMap = std::map<std::string, SensorBasePtr>();
     auto procesorMap = std::map<std::string, ProcessorBasePtr>();
-    for(auto s : _server.getSensors()){
-      sensorMap.insert(std::pair<std::string, SensorBasePtr>(s._name,p->installSensor(s._type, s._name, _server)));
+
+    for(auto sen : _server.getSensors()){
+        sensorMap.insert(std::pair<std::string, SensorBasePtr>(sen._name, problem->installSensor(sen._type, sen._name, _server)));
     }
-    for(auto s : _server.getProcessors()){
-      procesorMap.insert(std::pair<std::string, ProcessorBasePtr>(s._name,p->installProcessor(s._type, s._name, s._name_assoc_sensor, _server)));
+
+    for(auto prc : _server.getProcessors()){
+        procesorMap.insert(std::pair<std::string, ProcessorBasePtr>(prc._name, problem->installProcessor(prc._type, prc._name, prc._name_assoc_sensor, _server)));
     }
-    return p;
+
+    // Prior
+    Eigen::VectorXs prior_state = _server.getParam<Eigen::VectorXs>("problem/prior state");
+    Eigen::MatrixXs prior_cov   = _server.getParam<Eigen::MatrixXs>("problem/prior cov");
+    Scalar prior_time_tolerance = _server.getParam<Scalar>("problem/prior time tolerance");
+    Scalar prior_ts             = _server.getParam<Scalar>("problem/prior timestamp");
+
+    WOLF_TRACE("prior timestamp:\n"     , prior_ts);
+    WOLF_TRACE("prior state:\n"         , prior_state.transpose());
+    WOLF_TRACE("prior covariance:\n"    , prior_cov);
+    WOLF_TRACE("prior time tolerance:\n", prior_time_tolerance);
+
+    problem->setPrior(prior_state, prior_cov, prior_ts, prior_time_tolerance);
+
+    // Done
+    return problem;
 }
 
+
 Problem::~Problem()
 {
     //    WOLF_DEBUG("destructed -P");