diff --git a/CMakeLists.txt b/CMakeLists.txt
index 38eb02c21cd954fd5c5790ff9f94d63a5198a735..b547600921758e7f029cf96ceb126c954e0f4e70 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -164,6 +164,7 @@ SET(HDRS_COMMON
include/core/common/node_base.h
include/core/common/time_stamp.h
include/core/common/wolf.h
+ include/core/common/params_base.h
)
SET(HDRS_MATH
include/core/math/SE3.h
diff --git a/include/core/common/params_base.h b/include/core/common/params_base.h
new file mode 100644
index 0000000000000000000000000000000000000000..66034e29fd3e91ad28c2a08fea6a93eb0557bd90
--- /dev/null
+++ b/include/core/common/params_base.h
@@ -0,0 +1,22 @@
+#ifndef PARAMS_BASE_H_
+#define PARAMS_BASE_H_
+
+#include "core/utils/params_server.hpp"
+
+namespace wolf {
+ struct ParamsBase
+ {
+ ParamsBase() = default;
+ ParamsBase(std::string _unique_name, const paramsServer&)
+ {
+ //
+ }
+
+ virtual ~ParamsBase() = default;
+ std::string print()
+ {
+ return "";
+ }
+ };
+}
+#endif
\ No newline at end of file
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index d79f9e143eef67e982233b92df7a3cf289f9c100..09a6c11b1cd0cd685fdb6db40ea0c3ad64a20142 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -11,7 +11,6 @@
// Enable project-specific definitions and macros
#include "core/internal/config.h"
#include "core/utils/logging.h"
-#include "core/utils/params_server.hpp"
//includes from Eigen lib
#include <Eigen/Dense>
@@ -365,21 +364,6 @@ bool makePosDef(Eigen::Matrix<T,N,N,RC>& M, const T& eps = Constants::EPS)
return false;
}
-//===================================================
-struct ParamsBase
-{
- ParamsBase() = default;
- ParamsBase(std::string _unique_name, const paramsServer&)
- {
- //
- }
-
- virtual ~ParamsBase() = default;
- std::string print()
- {
- return "";
- }
-};
} // namespace wolf
#endif /* WOLF_H_ */
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index cf2bf2c7eb911522fc78e1d4a1b92a9d4a30da6c..114867765b722c5620d5e8a7c70abc6c43c792f8 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -11,7 +11,7 @@ class SensorBase;
#include "core/common/node_base.h"
#include "core/common/time_stamp.h"
#include "core/frame/frame_base.h"
-#include "core/utils/params_server.hpp"
+#include "core/common/params_base.h"
// std
#include <memory>
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 093f257064e1b16b4647bd3dbf83353bf287c8f8..25f5072ced7205d4de6a63361f7a25768cf411bf 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -29,24 +29,24 @@ struct ProcessorParamsMotion : public ProcessorParamsBase
Scalar dist_traveled = 5;
Scalar angle_turned = 0.5;
Scalar unmeasured_perturbation_std = 1e-4;
- ProcessorParamsMotion() = default;
- ProcessorParamsMotion(std::string _unique_name, const paramsServer& _server):
- ProcessorParamsBase(_unique_name, _server)
- {
- max_time_span = _server.getParam<Scalar>(_unique_name + "/max_time_span", "0.5");
- max_buff_length = _server.getParam<unsigned int>(_unique_name + "/max_buff_length", "10");
- dist_traveled = _server.getParam<Scalar>(_unique_name + "/dist_traveled", "5");
- angle_turned = _server.getParam<Scalar>(_unique_name + "/angle_turned", "0.5");
- unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std", "1e-4");
- }
- std::string print()
- {
- return "\n" + ProcessorParamsBase::print() + "max_time_span: " + std::to_string(max_time_span) + "\n"
- + "max_buff_length: " + std::to_string(max_buff_length) + "\n"
- + "dist_traveled: " + std::to_string(dist_traveled) + "\n"
- + "angle_turned: " +std::to_string(angle_turned) + "\n"
- + "unmeasured_perturbation_std: " + std::to_string(unmeasured_perturbation_std) + "\n";
- }
+ ProcessorParamsMotion() = default;
+ ProcessorParamsMotion(std::string _unique_name, const paramsServer& _server):
+ ProcessorParamsBase(_unique_name, _server)
+ {
+ max_time_span = _server.getParam<Scalar>(_unique_name + "/max_time_span", "0.5");
+ max_buff_length = _server.getParam<unsigned int>(_unique_name + "/max_buff_length", "10");
+ dist_traveled = _server.getParam<Scalar>(_unique_name + "/dist_traveled", "5");
+ angle_turned = _server.getParam<Scalar>(_unique_name + "/angle_turned", "0.5");
+ unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std", "1e-4");
+ }
+ std::string print()
+ {
+ return "\n" + ProcessorParamsBase::print() + "max_time_span: " + std::to_string(max_time_span) + "\n"
+ + "max_buff_length: " + std::to_string(max_buff_length) + "\n"
+ + "dist_traveled: " + std::to_string(dist_traveled) + "\n"
+ + "angle_turned: " +std::to_string(angle_turned) + "\n"
+ + "unmeasured_perturbation_std: " + std::to_string(unmeasured_perturbation_std) + "\n";
+ }
};
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 99a227a4010dffb1bdcddc18c390b94e5dafc2e9..3db21f9fe7245e54fcd6637adc8c5606ce9bd6a9 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -12,6 +12,7 @@ class StateBlock;
#include "core/common/wolf.h"
#include "core/common/node_base.h"
#include "core/common/time_stamp.h"
+#include "core/common/params_base.h"
//std includes
diff --git a/include/core/sensor/sensor_odom_3D.h b/include/core/sensor/sensor_odom_3D.h
index 827e4e2cf59d4d660a7d72ea5811f6a66d27f775..b5ec6717fe701acf3c8b4ca8c49b3bd81d892b7c 100644
--- a/include/core/sensor/sensor_odom_3D.h
+++ b/include/core/sensor/sensor_odom_3D.h
@@ -36,14 +36,14 @@ struct IntrinsicsOdom3D : public IntrinsicsBase
min_disp_var = _server.getParam<Scalar>(_unique_name + "/min_disp_var");
min_rot_var = _server.getParam<Scalar>(_unique_name + "/min_rot_var");
}
- std::string print()
- {
- return "\n" + IntrinsicsBase::print() + "k_disp_to_disp: " + std::to_string(k_disp_to_disp) + "\n"
- + "k_disp_to_rot: " + std::to_string(k_disp_to_rot) + "\n"
- + "k_rot_to_rot: " + std::to_string(k_rot_to_rot) + "\n"
- + "min_disp_var: " + std::to_string(min_disp_var) + "\n"
- + "min_rot_var: " + std::to_string(min_rot_var) + "\n";
- }
+ std::string print()
+ {
+ return "\n" + IntrinsicsBase::print() + "k_disp_to_disp: " + std::to_string(k_disp_to_disp) + "\n"
+ + "k_disp_to_rot: " + std::to_string(k_disp_to_rot) + "\n"
+ + "k_rot_to_rot: " + std::to_string(k_rot_to_rot) + "\n"
+ + "min_disp_var: " + std::to_string(min_disp_var) + "\n"
+ + "min_rot_var: " + std::to_string(min_rot_var) + "\n";
+ }
virtual ~IntrinsicsOdom3D() = default;
};
diff --git a/include/core/yaml/parser_yaml.hpp b/include/core/yaml/parser_yaml.hpp
index c721a163ac59abb487cfd01edbd3235f95d875d7..dbbba4103fa461d6f10468bcb6aafb79ed9874fa 100644
--- a/include/core/yaml/parser_yaml.hpp
+++ b/include/core/yaml/parser_yaml.hpp
@@ -2,6 +2,7 @@
#define PARSER_YAML_HPP
#include "yaml-cpp/yaml.h"
#include "core/utils/converter.h"
+#include "core/common/wolf.h"
#include <vector>
#include <regex>
#include <map>
@@ -10,27 +11,48 @@
#include <numeric>
namespace {
- // std::string remove_ws( const std::string& str ){
- // std::string str_no_ws ;
- // for( char c : str ) if( !std::isspace(c) ) str_no_ws += c ;
- // return str_no_ws ;
- // }
/** @Brief Generates a std::string [v1,v2,v3,...] representing the YAML sequence node
- * @param n a YAML::Node
+ * @param n a vector of YAML::Node where each node should be of type YAML::Node::Scalar
* @return <b>{std::string}</b> [v1,v2,v3,...]
*/
- std::string parseSequence(YAML::Node n){
- assert(n.Type() != YAML::NodeType::Map && "Trying to parse as a Sequence a Map node");
- if(n.Type() == YAML::NodeType::Scalar) return n.Scalar();
+ std::string fromSequenceToString(std::vector<YAML::Node> n){
+ std::string aux = "[";
+ bool first = true;
+ for(auto it : n){
+ assert(it.Type() == YAML::NodeType::Scalar && "fromSequenceToString requires that the sequence be a sequence of Scalars");
+ if(first) {
+ aux = aux + it.Scalar();
+ first = false;
+ }else{
+ aux = aux + "," + it.Scalar();
+ }
+ }
+ aux = aux + "]";
+ return aux;
+ }
+ /** @Brief Generates a std::string representing a YAML sequence. The sequence is assumed to be scalar or at most be a sequence of sequences of scalars.
+ * @param n a vector of YAML::Node that represents a YAML::Sequence
+ * @return <b>{std::string}</b> representing the YAML sequence
+ */
+ std::string parseScalarSequence(std::vector<YAML::Node> n){
std::string aux = "[";
bool first = true;
+ std::string separator = "";
for(auto it : n){
- if(first) {
- aux = aux + parseSequence(it);
+ if(it.Type() == YAML::NodeType::Scalar) aux = aux + separator + it.Scalar();
+ else {
+ auto seq = std::vector<YAML::Node>();
+ for(auto itt : it){
+ //I'm going to assume that the node is a sequence of scalars
+ assert(itt.Type() == YAML::NodeType::Scalar && "The sequence should be a sequence of Scalars");
+ seq.push_back(itt);
+ }
+ aux = aux + separator + fromSequenceToString(seq);
+ }
+ if(first){
+ separator = ",";
first = false;
- }else{
- aux = aux + "," + parseSequence(it);
- }
+ }
}
aux = aux + "]";
return aux;
@@ -186,13 +208,25 @@ void parserYAML::walkTreeR(YAML::Node n, std::vector<std::string>& tags, std::st
break;
}
case YAML::NodeType::Sequence : {
- std::string aux = parseSequence(n);
- _params.insert(std::pair<std::string,std::string>(hdr, aux));
+ std::vector<YAML::Node> scalar_and_seqs;
+ std::vector<YAML::Node> maps;
+ for(const auto& kv : n){
+ if(kv.Type() == YAML::NodeType::Scalar or kv.Type() == YAML::NodeType::Sequence) scalar_and_seqs.push_back(kv);
+ else if(kv.Type() == YAML::NodeType::Map) maps.push_back(kv);
+ }
+ assert(scalar_and_seqs.size() * maps.size() == 0);
+ if(scalar_and_seqs.size() > 0 and maps.size() == 0){
+ _params.insert(std::pair<std::string,std::string>(hdr, parseScalarSequence(scalar_and_seqs)));
+ }else if(scalar_and_seqs.size() == 0 and maps.size() > 0){
+ for(const auto& kv : maps){
+ walkTreeR(kv, tags, hdr);
+ }
+ }
break;
}
case YAML::NodeType::Map : {
for(const auto& kv : n){
- //If the key's value starts with a $ (i.e. $key) then its value is parsed as a literal map,
+ //If the key's value starts with a $ (i.e. $key) then its value is parsed as an atomic map,
//otherwise the parser recursively parses the map.
std::regex r("^\\$.*");
if(not std::regex_match(kv.first.as<std::string>(), r)){
@@ -313,6 +347,32 @@ void parserYAML::parse(){
this->walkTreeR(it.n , tags , it._name);
}
}
+
+std::string fetchMapEntry(YAML::Node n){
+ switch (n.Type()) {
+ case YAML::NodeType::Scalar : {
+ return n.Scalar();
+ break;
+ }
+ case YAML::NodeType::Sequence : {
+ std::vector<YAML::Node> nodes;
+ for(auto it : n){
+ nodes.push_back(it);
+ }
+ return parseScalarSequence(nodes);
+ break;
+ }
+ default: {
+ assert(1 == 0 && "Unsupported node Type at fetchMapEntry");
+ return "";
+ break;
+ }
+ }
+}
+/** @Brief Interprets a map as being atomic and thus parses it as a single entity. We assume that the map has as values only scalars and sequences.
+ * @param n the node representing a map
+ * @return std::map<std::string, std::string> populated with the key,value pairs in n
+ */
std::map<std::string, std::string> parserYAML::fetchAsMap(YAML::Node n){
assert(n.Type() == YAML::NodeType::Map && "trying to fetch as Map a non-Map node");
auto m = std::map<std::string, std::string>();
@@ -325,23 +385,21 @@ std::map<std::string, std::string> parserYAML::fetchAsMap(YAML::Node n){
break;
}
case YAML::NodeType::Sequence : {
- std::string aux = parseSequence(kv.second);
+ std::vector<YAML::Node> scalars;
+ std::vector<YAML::Node> non_scalars;
+ for(const auto& kvv : kv.second){
+ if(kvv.Type() == YAML::NodeType::Scalar or kvv.Type() == YAML::NodeType::Sequence) scalars.push_back(kvv);
+ else non_scalars.push_back(kvv);
+ }
+ if(non_scalars.size() > 0) WOLF_WARN("Ignoring non-scalar members of sequence in atomic map...");
+ std::string aux = parseScalarSequence(scalars);
m.insert(std::pair<std::string,std::string>(key, aux));
break;
}
case YAML::NodeType::Map : {
- m = fetchAsMap(kv.second);
- auto rtn = std::vector<std::string>();
- std::transform(m.begin(), m.end(), back_inserter(rtn), [](const std::pair<std::string,std::string> v){return "{" + v.first + ":" + v.second + "}";});
- auto concat = [](std::string ac, std::string str)-> std::string {
- return ac + str + ",";
- };
- std::string aux = "";
- std::string accumulated = std::accumulate(rtn.begin(), rtn.end(), aux, concat);
- if(accumulated.size() > 1) accumulated = accumulated.substr(0,accumulated.size() - 1);
- else accumulated = "";
- m.insert(std::pair<std::string,std::string>(key, "[" + accumulated + "]"));
- break;
+ std::string value = fetchMapEntry(kv.second);
+ m.insert(std::pair<std::string,std::string>(key, value));
+ break;
}
default:
assert(1 == 0 && "Unsupported node Type at fetchAsMap");
diff --git a/test/gtest_converter.cpp b/test/gtest_converter.cpp
index d8378df4616f90af772fe967e06ba892ecb516b5..d36d8665d7c3f9226e945a445ff922c20b8c1e00 100644
--- a/test/gtest_converter.cpp
+++ b/test/gtest_converter.cpp
@@ -39,6 +39,14 @@ TEST(Converter, ParseToEigenMatrix)
EXPECT_THROW(([=, &v]{v = converter<Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, 3, 5, 5>>::convert(v2);}()), std::runtime_error);
string v3 = "[[3],3,4,5,6,7,8,9,10,11]";
EXPECT_THROW(([=, &v]{v = converter<Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, 3, 5, 5>>::convert(v3);}()), std::runtime_error);
+ string v4 = "[[3,3],3,4,5,6,7,8,9,10,11]";
+ auto v41 = Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, 3, 5, 5>();
+ v41 = converter<Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, 3, 5, 5>>::convert(v4);
+ ASSERT_EQ(v41(0,0), 3);
+ ASSERT_EQ(v41(1,1), 7);
+ ASSERT_EQ(v41(2,2), 11);
+ string v5 = "[[3,3],3,4,5,6,7,[8,9],10,11]";
+ EXPECT_THROW(([=, &v]{v = converter<Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, 3, 5, 5>>::convert(v5);}()), std::runtime_error);
}
TEST(Converter, ParseToMap)
{