From e6a6b8cbdb211ded9ec3f50e97d1c31e19e4fbab Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joaquim=20Casals=20Bu=C3=B1uel?= <jcasals@iri.upc.edu>
Date: Fri, 15 Feb 2019 13:27:17 +0100
Subject: [PATCH] WIP, added first gtest parser
---
hello_plugin/CMakeLists.txt | 2 +
hello_plugin/hello_plugin.cpp | 1 +
hello_plugin/params.yaml | 5 ++
hello_plugin/params_autoconf.cpp | 71 +++++++++++++++++++
hello_plugin/params_conf.yaml | 8 +++
hello_plugin/parser_yaml.hpp | 17 +++--
include/base/converter.h | 6 ++
include/base/processor/processor_IMU.h | 7 +-
include/base/processor/processor_base.h | 7 +-
include/base/processor/processor_diff_drive.h | 19 ++---
include/base/processor/processor_motion.h | 10 +++
include/base/processor/processor_odom_2D.h | 8 +++
include/base/processor/processor_odom_3D.h | 7 +-
test/CMakeLists.txt | 4 ++
test/gtest_parser_yaml.cpp | 24 +++++++
test/params1.yaml | 30 ++++++++
16 files changed, 206 insertions(+), 20 deletions(-)
create mode 100644 hello_plugin/params_autoconf.cpp
create mode 100644 hello_plugin/params_conf.yaml
create mode 100644 test/gtest_parser_yaml.cpp
create mode 100644 test/params1.yaml
diff --git a/hello_plugin/CMakeLists.txt b/hello_plugin/CMakeLists.txt
index 75722dccb..4582cc409 100644
--- a/hello_plugin/CMakeLists.txt
+++ b/hello_plugin/CMakeLists.txt
@@ -4,7 +4,9 @@ INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
# ADD_EXECUTABLE(hello_plugin hello_plugin.cpp sensor_odom_2D.cpp processor_odom_2D.cpp)
ADD_EXECUTABLE(hello_plugin hello_plugin.cpp)
+ADD_EXECUTABLE(params_autoconf params_autoconf.cpp)
target_link_libraries(hello_plugin class_loader boost_system console_bridge wolf yaml-cpp ${CERES_LIBRARIES})
+target_link_libraries(params_autoconf class_loader boost_system console_bridge wolf yaml-cpp )
# These lines always at the end
SET(HDRS_PLUGIN ${HDRS_PLUGIN} PARENT_SCOPE )
SET(SRCS_PLUGIN ${SRCS_PLUGIN} PARENT_SCOPE )
diff --git a/hello_plugin/hello_plugin.cpp b/hello_plugin/hello_plugin.cpp
index 86baf7eb8..9077d04f3 100644
--- a/hello_plugin/hello_plugin.cpp
+++ b/hello_plugin/hello_plugin.cpp
@@ -56,6 +56,7 @@ int main(int argc, char** argv) {
cout << s._name << " " << s._type << " " << s._name_assoc_sensor << endl;
procesorMap.insert(pair<string, ProcessorBasePtr>(s._name,problem->installProcessor(s._type, s._name, s._name_assoc_sensor, server)));
}
+
problem->print(4,1,1,1);
Vector2s motion_data(1.0, 0.0); // Will advance 1m at each keyframe, will not turn.
Matrix2s motion_cov = 0.1 * Matrix2s::Identity();
diff --git a/hello_plugin/params.yaml b/hello_plugin/params.yaml
index ff190b733..db5fab3fa 100644
--- a/hello_plugin/params.yaml
+++ b/hello_plugin/params.yaml
@@ -20,6 +20,11 @@ config:
type: "RANGE BEARING"
name: "rb_processor"
sensorname: "rb"
+ -
+ type: "ODOM 2D"
+ name: "my_proc_test"
+ sensorname: "odom"
+ follow: "../hello_plugin/params_conf.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
diff --git a/hello_plugin/params_autoconf.cpp b/hello_plugin/params_autoconf.cpp
new file mode 100644
index 000000000..cf95715f8
--- /dev/null
+++ b/hello_plugin/params_autoconf.cpp
@@ -0,0 +1,71 @@
+/*
+ * params_autoconf.cpp
+ *
+ * Created on: Feb 15, 2019
+ * Author: jcasals
+ */
+#include <class_loader/class_loader.hpp>
+#include "base/sensor/sensor_base.h"
+#include "base/wolf.h"
+// #include "sensor_odom_2D.cpp"
+#include <yaml-cpp/yaml.h>
+#include "parser_yaml.hpp"
+#include "base/params_server.hpp"
+
+#include "../hello_wolf/capture_range_bearing.h"
+#include "../hello_wolf/feature_range_bearing.h"
+#include "../hello_wolf/constraint_range_bearing.h"
+#include "../hello_wolf/landmark_point_2D.h"
+#include "base/processor/processor_odom_2D.h"
+
+#include "base/solver/solver_factory.h"
+#include "base/ceres_wrapper/ceres_manager.h"
+
+using namespace std;
+using namespace class_loader;
+using namespace wolf;
+using namespace Eigen;
+
+int main(int argc, char** argv) {
+ string file = "";
+ if(argc > 1) file = argv[1];
+ parserYAML parser = parserYAML(file);
+ parser.parse();
+ paramsServer server = paramsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization());
+ cout << "PRINTING SERVER MAP" << endl;
+ server.print();
+ cout << "-----------------------------------" << endl;
+ /**
+ It seems to be a requirement that each file is loaded by its own ClassLoader object, otherwise I get
+ a segmentation fault. Likewise, it seems that these ClassLoaders must be allocated at the heap, because
+ the constructor refuses to build an object if I try to do local (stack) allocation, i.e `ClassLoader(it)` is not allowed but `new ClassLoader(it)` is.
+ **/
+ vector<ClassLoader*> class_loaders = vector<ClassLoader*>();
+ for(auto it : parser.getFiles()) {
+ auto c = new ClassLoader(it);
+ class_loaders.push_back(c);
+ }
+ ProblemPtr problem = Problem::create("PO 2D");
+ auto sensorMap = map<string, SensorBasePtr>();
+ auto procesorMap = map<string, ProcessorBasePtr>();
+ for(auto s : server.getSensors()){
+ cout << s._name << " " << s._type << endl;
+ sensorMap.insert(pair<string, SensorBasePtr>(s._name,problem->installSensor(s._type, s._name, server)));
+ }
+ for(auto s : server.getProcessors()){
+ cout << s._name << " " << s._type << " " << s._name_assoc_sensor << endl;
+ procesorMap.insert(pair<string, ProcessorBasePtr>(s._name,problem->installProcessor(s._type, s._name, s._name_assoc_sensor, server)));
+ }
+ auto prc = ProcessorParamsOdom2D("my_proc_test", server);
+
+ std::cout << "prc.cov_det " << prc.cov_det << std::endl;
+ std::cout << "prc.unmeasured_perturbation_std " << prc.unmeasured_perturbation_std << std::endl;
+ std::cout << "prc.angle_turned " << prc.angle_turned << std::endl;
+ std::cout << "prc.dist_traveled " << prc.dist_traveled << std::endl;
+ std::cout << "prc.max_buff_length " << prc.max_buff_length << std::endl;
+ std::cout << "prc.max_time_span " << prc.max_time_span << std::endl;
+ std::cout << "prc.time_tolerance " << prc.time_tolerance << std::endl;
+ std::cout << "prc.voting_active " << prc.voting_active << std::endl;
+
+ return 0;
+}
diff --git a/hello_plugin/params_conf.yaml b/hello_plugin/params_conf.yaml
new file mode 100644
index 000000000..0d80a5802
--- /dev/null
+++ b/hello_plugin/params_conf.yaml
@@ -0,0 +1,8 @@
+cov_det: 100
+unmeasured_perturbation_std: 100
+angle_turned: 100
+dist_traveled: 100
+max_buff_length: 100
+max_time_span: 100
+time_tolerance: 100
+voting_active: false
\ No newline at end of file
diff --git a/hello_plugin/parser_yaml.hpp b/hello_plugin/parser_yaml.hpp
index 455561e01..ffe2adc5f 100644
--- a/hello_plugin/parser_yaml.hpp
+++ b/hello_plugin/parser_yaml.hpp
@@ -140,13 +140,20 @@ void parserYAML::walkTreeR(YAML::Node n, vector<string>& tags, string hdr){
}
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,
+ //otherwise the parser recursively parses the map
regex r("^\\$.*");
if(not regex_match(kv.first.as<string>(), r)){
- tags.push_back(kv.first.as<string>());
- if(tags.size() == 2) this->updateActiveName(kv.first.as<string>());
- walkTreeR(kv.second, tags, hdr +"/"+ kv.first.as<string>());
- tags.pop_back();
- if(tags.size() == 1) this->updateActiveName("");
+ regex rr("follow");
+ if(not regex_match(kv.first.as<string>(), rr)) {
+ tags.push_back(kv.first.as<string>());
+ if(tags.size() == 2) this->updateActiveName(kv.first.as<string>());
+ walkTreeR(kv.second, tags, hdr +"/"+ kv.first.as<string>());
+ tags.pop_back();
+ if(tags.size() == 1) this->updateActiveName("");
+ }else{
+ walkTree(kv.second.as<string>(), tags, hdr);
+ }
}else{
string key = kv.first.as<string>();
key = key.substr(1,key.size() - 1);
diff --git a/include/base/converter.h b/include/base/converter.h
index 357a5f81d..2bff920a9 100644
--- a/include/base/converter.h
+++ b/include/base/converter.h
@@ -116,6 +116,12 @@ struct converter<int>{
}
};
template<>
+struct converter<unsigned int>{
+ static unsigned int convert(std::string val){
+ return stod(val);
+ }
+};
+template<>
struct converter<double>{
static double convert(std::string val){
return stod(val);
diff --git a/include/base/processor/processor_IMU.h b/include/base/processor/processor_IMU.h
index 98c7cffee..64d97861c 100644
--- a/include/base/processor/processor_IMU.h
+++ b/include/base/processor/processor_IMU.h
@@ -11,7 +11,12 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsIMU);
struct ProcessorParamsIMU : public ProcessorParamsMotion
{
- //
+ ProcessorParamsIMU() = default;
+ ProcessorParamsIMU(std::string _unique_name, const paramsServer& _server):
+ ProcessorParamsMotion(_unique_name, _server)
+ {
+ //
+ }
};
WOLF_PTR_TYPEDEFS(ProcessorIMU);
diff --git a/include/base/processor/processor_base.h b/include/base/processor/processor_base.h
index 23ac82daa..fb5c9e647 100644
--- a/include/base/processor/processor_base.h
+++ b/include/base/processor/processor_base.h
@@ -11,6 +11,7 @@ class SensorBase;
#include "base/node_base.h"
#include "base/time_stamp.h"
#include "base/frame_base.h"
+#include "base/params_server.hpp"
// std
#include <memory>
@@ -108,7 +109,6 @@ class PackKeyFrameBuffer
struct ProcessorParamsBase
{
ProcessorParamsBase() = default;
-
ProcessorParamsBase(bool _voting_active,
Scalar _time_tolerance)
: voting_active(_voting_active)
@@ -116,6 +116,11 @@ struct ProcessorParamsBase
{
//
}
+ ProcessorParamsBase(std::string _unique_name, const paramsServer& _server)
+ {
+ voting_active = _server.getParam<bool>(_unique_name + "/voting_active", "false");
+ time_tolerance = _server.getParam<Scalar>(_unique_name + "/time_tolerance", "0");
+ }
virtual ~ProcessorParamsBase() = default;
diff --git a/include/base/processor/processor_diff_drive.h b/include/base/processor/processor_diff_drive.h
index e219b552f..dc7f77c80 100644
--- a/include/base/processor/processor_diff_drive.h
+++ b/include/base/processor/processor_diff_drive.h
@@ -10,6 +10,7 @@
#include "base/processor/processor_motion.h"
#include "base/diff_drive_tools.h"
+#include "base/params_server.hpp"
namespace wolf {
@@ -17,19 +18,13 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsDiffDrive);
struct ProcessorParamsDiffDrive : public ProcessorParamsMotion
{
-// ProcessorParamsDiffDrive(const Scalar _time_tolerance,
-// const Scalar _dist_travel_th,
-// const Scalar _theta_traveled_th,
-// const Scalar _cov_det_th,
-// const Scalar _unmeasured_perturbation_std = 0.0001) :
-// dist_traveled_th_(_dist_travel_th),
-// theta_traveled_th_(_theta_traveled_th),
-// cov_det_th_(_cov_det_th),
-// unmeasured_perturbation_std_(_unmeasured_perturbation_std)
-// {
-// time_tolerance = _time_tolerance;
-// }
Scalar unmeasured_perturbation_std = 0.0001;
+ ProcessorParamsDiffDrive();
+ ProcessorParamsDiffDrive(std::string _unique_name, const paramsServer& _server):
+ ProcessorParamsMotion(_unique_name, _server)
+ {
+ unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std", "0.0001");
+ }
};
/**
diff --git a/include/base/processor/processor_motion.h b/include/base/processor/processor_motion.h
index df43f9f84..cb7957e1a 100644
--- a/include/base/processor/processor_motion.h
+++ b/include/base/processor/processor_motion.h
@@ -12,6 +12,7 @@
#include "base/capture/capture_motion.h"
#include "base/processor/processor_base.h"
#include "base/time_stamp.h"
+#include "base/params_server.hpp"
// std
#include <iomanip>
@@ -27,6 +28,15 @@ struct ProcessorParamsMotion : public ProcessorParamsBase
unsigned int max_buff_length = 10;
Scalar dist_traveled = 5;
Scalar angle_turned = 0.5;
+ 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");
+ }
};
/** \brief class for Motion processors
diff --git a/include/base/processor/processor_odom_2D.h b/include/base/processor/processor_odom_2D.h
index af8856455..a8a8942c7 100644
--- a/include/base/processor/processor_odom_2D.h
+++ b/include/base/processor/processor_odom_2D.h
@@ -23,6 +23,14 @@ struct ProcessorParamsOdom2D : public ProcessorParamsMotion
{
Scalar cov_det = 1.0; // 1 rad^2
Scalar unmeasured_perturbation_std = 0.001; // no particular dimension: the same for displacement and angle
+
+ ProcessorParamsOdom2D() = default;
+ ProcessorParamsOdom2D(std::string _unique_name, const paramsServer& _server):
+ ProcessorParamsMotion(_unique_name, _server)
+ {
+ cov_det = _server.getParam<Scalar>(_unique_name + "/cov_det", "1.0");
+ unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std", "0.001");
+ }
};
class ProcessorOdom2D : public ProcessorMotion
diff --git a/include/base/processor/processor_odom_3D.h b/include/base/processor/processor_odom_3D.h
index 491ae23c7..21bad23ea 100644
--- a/include/base/processor/processor_odom_3D.h
+++ b/include/base/processor/processor_odom_3D.h
@@ -21,7 +21,12 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom3D);
struct ProcessorParamsOdom3D : public ProcessorParamsMotion
{
- //
+ ProcessorParamsOdom3D() = default;
+ ProcessorParamsOdom3D(std::string _unique_name, const paramsServer& _server):
+ ProcessorParamsMotion(_unique_name, _server)
+ {
+ //
+ }
};
WOLF_PTR_TYPEDEFS(ProcessorOdom3D);
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 37859c19e..c4e1ce8be 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -84,6 +84,10 @@ target_link_libraries(gtest_motion_buffer ${PROJECT_NAME})
wolf_add_gtest(gtest_pack_KF_buffer gtest_pack_KF_buffer.cpp)
target_link_libraries(gtest_pack_KF_buffer ${PROJECT_NAME})
+# YAML parser
+wolf_add_gtest(gtest_parser_yaml gtest_parser_yaml.cpp)
+target_link_libraries(gtest_parser_yaml ${PROJECT_NAME})
+
# Problem class test
wolf_add_gtest(gtest_problem gtest_problem.cpp)
target_link_libraries(gtest_problem ${PROJECT_NAME})
diff --git a/test/gtest_parser_yaml.cpp b/test/gtest_parser_yaml.cpp
new file mode 100644
index 000000000..b25e7a4b3
--- /dev/null
+++ b/test/gtest_parser_yaml.cpp
@@ -0,0 +1,24 @@
+#include "utils_gtest.h"
+#include "base/converter.h"
+#include "../hello_plugin/parser_yaml.hpp"
+
+using namespace std;
+using namespace wolf;
+
+TEST(ParserYAML, RegularParse)
+{
+ string file = "";
+ file = "../test/params1.yaml";
+ parserYAML parser = parserYAML(file);
+ parser.parse();
+ auto params = parser.getParams();
+ for(auto it : params)
+ cout << it.first << " %% " << it.second << endl;
+ ASSERT_EQ(params["odom/intrinsic/k_rot_to_rot"], "0.1");
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/params1.yaml b/test/params1.yaml
new file mode 100644
index 000000000..db5fab3fa
--- /dev/null
+++ b/test/params1.yaml
@@ -0,0 +1,30 @@
+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"
+ sensorname: "odom"
+ -
+ type: "RANGE BEARING"
+ name: "rb_processor"
+ sensorname: "rb"
+ -
+ type: "ODOM 2D"
+ name: "my_proc_test"
+ sensorname: "odom"
+ follow: "../hello_plugin/params_conf.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
--
GitLab