Added 'sensor' & 'processor' prefix in params server. Added verbose output for...

Added 'sensor' & 'processor' prefix in params server. Added verbose output for duplicated keys in parser.

hello_wolf/sensor_range_bearing.h

@@ -26,8 +26,8 @@ struct IntrinsicsRangeBearing : public IntrinsicsBase
     IntrinsicsRangeBearing(std::string _unique_name, const ParamsServer& _server):
         IntrinsicsBase(_unique_name, _server)
     {
-        noise_range_metres_std      = _server.getParam<Scalar>(_unique_name + "/noise_range_metres_std");
-        noise_bearing_degrees_std   = _server.getParam<Scalar>(_unique_name + "/noise_bearing_degrees_std");
+        noise_range_metres_std      = _server.getParam<Scalar>(prefix + _unique_name + "/noise_range_metres_std");
+        noise_bearing_degrees_std   = _server.getParam<Scalar>(prefix + _unique_name + "/noise_bearing_degrees_std");
     }
     std::string print() const
     {

include/core/processor/processor_base.h

@@ -204,13 +204,14 @@ class BufferCapture : public Buffer<CaptureBasePtr> {};
  */
 struct ProcessorParamsBase : public ParamsBase
 {
+    std::string prefix = "processor/";
     ProcessorParamsBase() = default;
     ProcessorParamsBase(std::string _unique_name, const ParamsServer& _server):
         ParamsBase(_unique_name, _server)
     {
-        time_tolerance      = _server.getParam<Scalar>(_unique_name + "/time_tolerance");
-        voting_active       = _server.getParam<bool>(_unique_name   + "/keyframe_vote/voting_active");
-        voting_aux_active   = _server.getParam<bool>(_unique_name   + "/keyframe_vote/voting_aux_active");
+        time_tolerance      = _server.getParam<Scalar>(prefix + _unique_name + "/time_tolerance");
+        voting_active       = _server.getParam<bool>(prefix + _unique_name   + "/keyframe_vote/voting_active");
+        voting_aux_active   = _server.getParam<bool>(prefix + _unique_name   + "/keyframe_vote/voting_aux_active");
     }
 
     virtual ~ProcessorParamsBase() = default;

include/core/processor/processor_motion.h

@@ -34,11 +34,11 @@ struct ProcessorParamsMotion : public ProcessorParamsBase
         ProcessorParamsMotion(std::string _unique_name, const ParamsServer& _server):
             ProcessorParamsBase(_unique_name, _server)
         {
-          max_time_span   = _server.getParam<Scalar>(_unique_name       + "/keyframe_vote/max_time_span");
-          max_buff_length = _server.getParam<unsigned int>(_unique_name + "/keyframe_vote/max_buff_length");
-          dist_traveled   = _server.getParam<Scalar>(_unique_name       + "/keyframe_vote/dist_traveled");
-          angle_turned    = _server.getParam<Scalar>(_unique_name       + "/keyframe_vote/angle_turned");
-          unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std");
+          max_time_span   = _server.getParam<Scalar>(prefix + _unique_name       + "/keyframe_vote/max_time_span");
+          max_buff_length = _server.getParam<unsigned int>(prefix + _unique_name + "/keyframe_vote/max_buff_length");
+          dist_traveled   = _server.getParam<Scalar>(prefix + _unique_name       + "/keyframe_vote/dist_traveled");
+          angle_turned    = _server.getParam<Scalar>(prefix + _unique_name       + "/keyframe_vote/angle_turned");
+          unmeasured_perturbation_std = _server.getParam<Scalar>(prefix + _unique_name + "/unmeasured_perturbation_std");
         }
         std::string print() const
         {

include/core/processor/processor_odom_2D.h

@@ -27,7 +27,7 @@ struct ProcessorParamsOdom2D : public ProcessorParamsMotion
         ProcessorParamsOdom2D(std::string _unique_name, const wolf::ParamsServer & _server) :
             ProcessorParamsMotion(_unique_name, _server)
         {
-            cov_det = _server.getParam<Scalar>(_unique_name + "/keyframe_vote/cov_det");
+            cov_det = _server.getParam<Scalar>(prefix + _unique_name + "/keyframe_vote/cov_det");
         }
 
         std::string print() const

include/core/processor/processor_tracker.h

@@ -24,8 +24,8 @@ struct ProcessorParamsTracker : public ProcessorParamsBase
     ProcessorParamsTracker(std::string _unique_name, const wolf::ParamsServer & _server):
         ProcessorParamsBase(_unique_name, _server)
     {
-        min_features_for_keyframe   = _server.getParam<unsigned int>(_unique_name   + "/min_features_for_keyframe");
-        max_new_features            = _server.getParam<int>(_unique_name            + "/max_new_features");
+        min_features_for_keyframe   = _server.getParam<unsigned int>(prefix + _unique_name   + "/min_features_for_keyframe");
+        max_new_features            = _server.getParam<int>(prefix + _unique_name            + "/max_new_features");
     }
     std::string print() const
     {

include/core/sensor/sensor_base.h

@@ -36,7 +36,7 @@ namespace wolf {
 static                                                                                                              \
 SensorBasePtr create(const std::string& _unique_name, const ParamsServer& _server)                                  \
 {                                                                                                                   \
-    Eigen::VectorXs extrinsics = _server.template getParam<Eigen::VectorXs>(_unique_name + "/extrinsic/pose");      \
+    Eigen::VectorXs extrinsics = _server.template getParam<Eigen::VectorXs>("sensor" + _unique_name + "/extrinsic/pose");      \
                                                                                                                     \
     assert(extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length.");                                 \
                                                                                                                     \
@@ -72,6 +72,7 @@ SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _ex
  */
 struct IntrinsicsBase: public ParamsBase
 {
+    std::string prefix = "sensor/";
     virtual ~IntrinsicsBase() = default;
     using ParamsBase::ParamsBase;
     std::string print() const

include/core/sensor/sensor_diff_drive.h

@@ -27,10 +27,10 @@ struct IntrinsicsDiffDrive : public IntrinsicsBase
         IntrinsicsDiffDrive(std::string _unique_name, const wolf::ParamsServer & _server) :
             IntrinsicsBase(_unique_name, _server)
         {
-            radius_left                 = _server.getParam<Scalar>(_unique_name + "/radius_left");
-            radius_right                = _server.getParam<Scalar>(_unique_name + "/radius_right");
-            wheel_separation            = _server.getParam<Scalar>(_unique_name + "/wheel_separation");
-            ticks_per_wheel_revolution  = _server.getParam<Scalar>(_unique_name + "/ticks_per_wheel_revolution");
+            radius_left                 = _server.getParam<Scalar>(prefix + _unique_name + "/radius_left");
+            radius_right                = _server.getParam<Scalar>(prefix + _unique_name + "/radius_right");
+            wheel_separation            = _server.getParam<Scalar>(prefix + _unique_name + "/wheel_separation");
+            ticks_per_wheel_revolution  = _server.getParam<Scalar>(prefix + _unique_name + "/ticks_per_wheel_revolution");
         }
         std::string print() const
         {

include/core/sensor/sensor_odom_2D.h

@@ -22,8 +22,8 @@ struct IntrinsicsOdom2D : public IntrinsicsBase
     IntrinsicsOdom2D(std::string _unique_name, const ParamsServer& _server):
         IntrinsicsBase(_unique_name, _server)
     {
-        k_disp_to_disp = _server.getParam<Scalar>(_unique_name + "/k_disp_to_disp");
-        k_rot_to_rot   = _server.getParam<Scalar>(_unique_name + "/k_rot_to_rot");
+        k_disp_to_disp = _server.getParam<Scalar>(prefix + _unique_name + "/k_disp_to_disp");
+        k_rot_to_rot   = _server.getParam<Scalar>(prefix + _unique_name + "/k_rot_to_rot");
     }
     std::string print() const
     {

include/core/sensor/sensor_odom_3D.h

@@ -30,11 +30,11 @@ struct IntrinsicsOdom3D : public IntrinsicsBase
     IntrinsicsOdom3D(std::string _unique_name, const ParamsServer& _server):
         IntrinsicsBase(_unique_name, _server)
     {
-        k_disp_to_disp = _server.getParam<Scalar>(_unique_name + "/k_disp_to_disp");
-        k_disp_to_rot  = _server.getParam<Scalar>(_unique_name + "/k_disp_to_rot");
-        k_rot_to_rot   = _server.getParam<Scalar>(_unique_name + "/k_rot_to_rot");
-        min_disp_var   = _server.getParam<Scalar>(_unique_name + "/min_disp_var");
-        min_rot_var    = _server.getParam<Scalar>(_unique_name + "/min_rot_var");
+        k_disp_to_disp = _server.getParam<Scalar>(prefix + _unique_name + "/k_disp_to_disp");
+        k_disp_to_rot  = _server.getParam<Scalar>(prefix + _unique_name + "/k_disp_to_rot");
+        k_rot_to_rot   = _server.getParam<Scalar>(prefix + _unique_name + "/k_rot_to_rot");
+        min_disp_var   = _server.getParam<Scalar>(prefix + _unique_name + "/min_disp_var");
+        min_rot_var    = _server.getParam<Scalar>(prefix + _unique_name + "/min_rot_var");
     }
     std::string print() const
     {

include/core/yaml/parser_yaml.hpp

@@ -189,6 +189,7 @@ class ParserYAML {
     YAML::Node problem;
     std::string generatePath(std::string);
     YAML::Node loadYAML(std::string);
+    void insert_register(std::string, std::string);
 public:
     ParserYAML(){
         _params = std::map<std::string, std::string>();
@@ -310,13 +311,15 @@ void ParserYAML::walkTreeR(YAML::Node n, std::vector<std::string>& tags, std::st
             std::string str = n.Scalar();
             walkTree(str.substr(1,str.size() - 1), tags, hdr);
         }else{
-            _params.insert(std::pair<std::string,std::string>(hdr, n.Scalar()));
+          insert_register(hdr, n.Scalar());
+          // _params.insert(std::pair<std::string,std::string>);
         }
         break;
     }
     case YAML::NodeType::Sequence : {
       if(isAtomic("", n)){
-          _params.insert(std::pair<std::string,std::string>(hdr, parseAtomicNode(n)));
+        insert_register(hdr, parseAtomicNode(n));
+        // _params.insert(std::pair<std::string,std::string>);
         }else{
           for(const auto& kv : n){
             walkTreeR(kv, tags, hdr);
@@ -330,7 +333,8 @@ void ParserYAML::walkTreeR(YAML::Node n, std::vector<std::string>& tags, std::st
           std::string key = kv.first.as<std::string>();
           //WOLF_DEBUG("KEY IN MAP ATOMIC ", hdr + "/" + key);
           key = key.substr(1,key.size() - 1);
-          _params.insert(std::pair<std::string,std::string>(hdr + "/" + key, parseAtomicNode(kv.second)));
+          insert_register(hdr + "/" + key, parseAtomicNode(kv.second));
+          // _params.insert(std::pair<std::string,std::string>);
       }else{
             /*
               If key=="follow" then the parser will assume that the value is a path and will parse
@@ -413,7 +417,8 @@ void ParserYAML::parseFirstLevel(std::string file){
     for(const auto& kv : n_files){
         _files.push_back(kv.Scalar());
     }
-    _params.insert(std::pair<std::string,std::string>("files", wolf::converter<std::string>::convert(_files)));
+    insert_register("files", wolf::converter<std::string>::convert(_files));
+    // _params.insert(std::pair<std::string,std::string>);
 }
 std::vector<std::array<std::string, 2>> ParserYAML::sensorsSerialization(){
     std::vector<std::array<std::string, 2>> aux = std::vector<std::array<std::string, 2>>();
@@ -450,11 +455,13 @@ void ParserYAML::parse(){
     }
     for(auto it : _paramsSens){
       std::vector<std::string> tags = std::vector<std::string>();
-      this->walkTreeR(it.n , tags , it._name);
+      tags.push_back("sensor");
+      this->walkTreeR(it.n , tags , "sensor/" + it._name);
     }
     for(auto it : _paramsProc){
       std::vector<std::string> tags = std::vector<std::string>();
-      this->walkTreeR(it.n , tags , it._name);
+      tags.push_back("processor");
+      this->walkTreeR(it.n , tags , "processor/" + it._name);
     }
     this->_parsing_file.pop();
 }
@@ -464,4 +471,9 @@ void ParserYAML::parse_freely(){
     this->walkTreeR(loadYAML(this->_file), tags, "");
     this->_parsing_file.pop();
 }
+void ParserYAML::insert_register(std::string key, std::string value){
+  // _params.insert(std::pair<std::string,std::string>);
+  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, ")");
+}
 #endif

src/problem/problem.cpp

@@ -199,7 +199,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
 {
     SensorBasePtr sen_ptr = getSensor(_corresponding_sensor_name);
     if (sen_ptr == nullptr)
-        throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, in must match in sensor and processor!");
+        throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, it must match in sensor and processor!");
     if (_params_filename == "")
         return installProcessor(_prc_type, _unique_processor_name, sen_ptr, nullptr);
     else
@@ -217,7 +217,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
 {
     SensorBasePtr sen_ptr = getSensor(_corresponding_sensor_name);
     if (sen_ptr == nullptr)
-        throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, in must match in sensor and processor!");
+        throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, it must match in sensor and processor!");
 
     ProcessorBasePtr prc_ptr = AutoConfProcessorFactory::get().create(uppercase(_prc_type), _unique_processor_name, _server);
     prc_ptr->configure(sen_ptr);

test/dummy/processor_tracker_feature_dummy.h

@@ -25,7 +25,7 @@ struct ProcessorParamsTrackerFeatureDummy : public ProcessorParamsTrackerFeature
     ProcessorParamsTrackerFeatureDummy(std::string _unique_name, const wolf::ParamsServer & _server):
         ProcessorParamsTrackerFeature(_unique_name, _server)
     {
-        n_tracks_lost = _server.getParam<unsigned int>(_unique_name + "/n_tracks_lost");
+        n_tracks_lost = _server.getParam<unsigned int>(prefix + _unique_name + "/n_tracks_lost");
     }
 };
 

test/dummy/processor_tracker_landmark_dummy.h

@@ -23,7 +23,7 @@ struct ProcessorParamsTrackerLandmarkDummy : public ProcessorParamsTrackerLandma
     ProcessorParamsTrackerLandmarkDummy(std::string _unique_name, const wolf::ParamsServer & _server):
         ProcessorParamsTrackerLandmark(_unique_name, _server)
     {
-        n_landmarks_lost = _server.getParam<unsigned int>(_unique_name + "/n_landmarks_lost");
+        n_landmarks_lost = _server.getParam<unsigned int>(prefix + _unique_name + "/n_landmarks_lost");
     }
 };
 

test/gtest_parser_yaml.cpp

@@ -7,6 +7,8 @@ using namespace std;
 using namespace wolf;
 
 std::string wolf_root = _WOLF_ROOT_DIR;
+std::string sensor_prefix = "sensor/";
+std::string processor_prefix = "processor/";
 
 ParserYAML parse(string _file, string _path_root)
 {
@@ -21,8 +23,8 @@ TEST(ParserYAML, RegularParse)
   auto params = parser.getParams();
   // for(auto it : params)
   //   cout << it.first << " %% " << it.second << endl;
-  EXPECT_EQ(params["odom/intrinsic/k_rot_to_rot"], "0.1");
-  EXPECT_EQ(params["processor1/sensor_name"], "odom");
+  EXPECT_EQ(params[sensor_prefix + "odom/intrinsic/k_rot_to_rot"], "0.1");
+  EXPECT_EQ(params[processor_prefix + "processor1/sensor_name"], "odom");
 }
 TEST(ParserYAML, ParseMap)
 {
@@ -30,29 +32,29 @@ TEST(ParserYAML, ParseMap)
   auto params = parser.getParams();
   // for(auto it : params)
   //   cout << it.first << " %% " << it.second << endl;
-  EXPECT_EQ(params["processor1/mymap"], "[{k1:v1},{k2:v2},{k3:[v3,v4,v5]}]");
+  EXPECT_EQ(params[processor_prefix + "processor1/mymap"], "[{k1:v1},{k2:v2},{k3:[v3,v4,v5]}]");
   // EXPECT_EQ(params["processor1/$mymap/k1"], "v1");
 }
 TEST(ParserYAML, FollowFile)
 {
   auto parser = parse("test/yaml/params1.yaml", wolf_root);
   auto params = parser.getParams();
-  EXPECT_EQ(params["my_proc_test/max_buff_length"], "100");
-  EXPECT_EQ(params["my_proc_test/voting_active"], "false");
+  EXPECT_EQ(params[processor_prefix + "my_proc_test/max_buff_length"], "100");
+  EXPECT_EQ(params[processor_prefix + "my_proc_test/voting_active"], "false");
 }
 TEST(ParserYAML, FollowOdom3D)
 {
   auto parser = parse("test/yaml/params1.yaml", wolf_root);
   auto params = parser.getParams();
-  EXPECT_EQ(params["my_proc_odom3d/keyframe_vote/max_buff_length"], "10");
-  EXPECT_EQ(params["my_proc_odom3d/keyframe_vote/max_time_span"], "0.2");
+  EXPECT_EQ(params[processor_prefix + "my_proc_odom3d/keyframe_vote/max_buff_length"], "10");
+  EXPECT_EQ(params[processor_prefix + "my_proc_odom3d/keyframe_vote/max_time_span"], "0.2");
 }
 TEST(ParserYAML, JumpFile)
 {
   auto parser = parse("test/yaml/params3.yaml", wolf_root);
   auto params = parser.getParams();
-  EXPECT_EQ(params["my_proc_test/extern_params/max_buff_length"], "100");
-  EXPECT_EQ(params["my_proc_test/extern_params/voting_active"], "false");
+  EXPECT_EQ(params[processor_prefix + "my_proc_test/extern_params/max_buff_length"], "100");
+  EXPECT_EQ(params[processor_prefix + "my_proc_test/extern_params/voting_active"], "false");
 }
 TEST(ParserYAML, ProblemConfig)
 {