Skip to content
Snippets Groups Projects
Commit e6a6b8cb authored by Joaquim Casals Buñuel's avatar Joaquim Casals Buñuel
Browse files

WIP, added first gtest parser

parent 4d345d0e
No related branches found
No related tags found
1 merge request!260WIP: params autoconf
This commit is part of merge request !260. Comments created here will be created in the context of that merge request.
Showing
with 206 additions and 20 deletions
...@@ -4,7 +4,9 @@ INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR}) ...@@ -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 sensor_odom_2D.cpp processor_odom_2D.cpp)
ADD_EXECUTABLE(hello_plugin hello_plugin.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(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 # These lines always at the end
SET(HDRS_PLUGIN ${HDRS_PLUGIN} PARENT_SCOPE ) SET(HDRS_PLUGIN ${HDRS_PLUGIN} PARENT_SCOPE )
SET(SRCS_PLUGIN ${SRCS_PLUGIN} PARENT_SCOPE ) SET(SRCS_PLUGIN ${SRCS_PLUGIN} PARENT_SCOPE )
......
...@@ -56,6 +56,7 @@ int main(int argc, char** argv) { ...@@ -56,6 +56,7 @@ int main(int argc, char** argv) {
cout << s._name << " " << s._type << " " << s._name_assoc_sensor << endl; 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))); procesorMap.insert(pair<string, ProcessorBasePtr>(s._name,problem->installProcessor(s._type, s._name, s._name_assoc_sensor, server)));
} }
problem->print(4,1,1,1); problem->print(4,1,1,1);
Vector2s motion_data(1.0, 0.0); // Will advance 1m at each keyframe, will not turn. Vector2s motion_data(1.0, 0.0); // Will advance 1m at each keyframe, will not turn.
Matrix2s motion_cov = 0.1 * Matrix2s::Identity(); Matrix2s motion_cov = 0.1 * Matrix2s::Identity();
......
...@@ -20,6 +20,11 @@ config: ...@@ -20,6 +20,11 @@ config:
type: "RANGE BEARING" type: "RANGE BEARING"
name: "rb_processor" name: "rb_processor"
sensorname: "rb" sensorname: "rb"
-
type: "ODOM 2D"
name: "my_proc_test"
sensorname: "odom"
follow: "../hello_plugin/params_conf.yaml"
files: files:
- "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so" - "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so"
- "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so" - "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so"
\ No newline at end of file
/*
* 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;
}
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
...@@ -140,13 +140,20 @@ void parserYAML::walkTreeR(YAML::Node n, vector<string>& tags, string hdr){ ...@@ -140,13 +140,20 @@ void parserYAML::walkTreeR(YAML::Node n, vector<string>& tags, string hdr){
} }
case YAML::NodeType::Map : { case YAML::NodeType::Map : {
for(const auto& kv : n){ 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("^\\$.*"); regex r("^\\$.*");
if(not regex_match(kv.first.as<string>(), r)){ if(not regex_match(kv.first.as<string>(), r)){
tags.push_back(kv.first.as<string>()); regex rr("follow");
if(tags.size() == 2) this->updateActiveName(kv.first.as<string>()); if(not regex_match(kv.first.as<string>(), rr)) {
walkTreeR(kv.second, tags, hdr +"/"+ kv.first.as<string>()); tags.push_back(kv.first.as<string>());
tags.pop_back(); if(tags.size() == 2) this->updateActiveName(kv.first.as<string>());
if(tags.size() == 1) this->updateActiveName(""); 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{ }else{
string key = kv.first.as<string>(); string key = kv.first.as<string>();
key = key.substr(1,key.size() - 1); key = key.substr(1,key.size() - 1);
......
...@@ -116,6 +116,12 @@ struct converter<int>{ ...@@ -116,6 +116,12 @@ struct converter<int>{
} }
}; };
template<> template<>
struct converter<unsigned int>{
static unsigned int convert(std::string val){
return stod(val);
}
};
template<>
struct converter<double>{ struct converter<double>{
static double convert(std::string val){ static double convert(std::string val){
return stod(val); return stod(val);
......
...@@ -11,7 +11,12 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsIMU); ...@@ -11,7 +11,12 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsIMU);
struct ProcessorParamsIMU : public ProcessorParamsMotion struct ProcessorParamsIMU : public ProcessorParamsMotion
{ {
// ProcessorParamsIMU() = default;
ProcessorParamsIMU(std::string _unique_name, const paramsServer& _server):
ProcessorParamsMotion(_unique_name, _server)
{
//
}
}; };
WOLF_PTR_TYPEDEFS(ProcessorIMU); WOLF_PTR_TYPEDEFS(ProcessorIMU);
......
...@@ -11,6 +11,7 @@ class SensorBase; ...@@ -11,6 +11,7 @@ class SensorBase;
#include "base/node_base.h" #include "base/node_base.h"
#include "base/time_stamp.h" #include "base/time_stamp.h"
#include "base/frame_base.h" #include "base/frame_base.h"
#include "base/params_server.hpp"
// std // std
#include <memory> #include <memory>
...@@ -108,7 +109,6 @@ class PackKeyFrameBuffer ...@@ -108,7 +109,6 @@ class PackKeyFrameBuffer
struct ProcessorParamsBase struct ProcessorParamsBase
{ {
ProcessorParamsBase() = default; ProcessorParamsBase() = default;
ProcessorParamsBase(bool _voting_active, ProcessorParamsBase(bool _voting_active,
Scalar _time_tolerance) Scalar _time_tolerance)
: voting_active(_voting_active) : voting_active(_voting_active)
...@@ -116,6 +116,11 @@ struct ProcessorParamsBase ...@@ -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; virtual ~ProcessorParamsBase() = default;
......
...@@ -10,6 +10,7 @@ ...@@ -10,6 +10,7 @@
#include "base/processor/processor_motion.h" #include "base/processor/processor_motion.h"
#include "base/diff_drive_tools.h" #include "base/diff_drive_tools.h"
#include "base/params_server.hpp"
namespace wolf { namespace wolf {
...@@ -17,19 +18,13 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsDiffDrive); ...@@ -17,19 +18,13 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsDiffDrive);
struct ProcessorParamsDiffDrive : public ProcessorParamsMotion 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; 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");
}
}; };
/** /**
......
...@@ -12,6 +12,7 @@ ...@@ -12,6 +12,7 @@
#include "base/capture/capture_motion.h" #include "base/capture/capture_motion.h"
#include "base/processor/processor_base.h" #include "base/processor/processor_base.h"
#include "base/time_stamp.h" #include "base/time_stamp.h"
#include "base/params_server.hpp"
// std // std
#include <iomanip> #include <iomanip>
...@@ -27,6 +28,15 @@ struct ProcessorParamsMotion : public ProcessorParamsBase ...@@ -27,6 +28,15 @@ struct ProcessorParamsMotion : public ProcessorParamsBase
unsigned int max_buff_length = 10; unsigned int max_buff_length = 10;
Scalar dist_traveled = 5; Scalar dist_traveled = 5;
Scalar angle_turned = 0.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 /** \brief class for Motion processors
......
...@@ -23,6 +23,14 @@ struct ProcessorParamsOdom2D : public ProcessorParamsMotion ...@@ -23,6 +23,14 @@ struct ProcessorParamsOdom2D : public ProcessorParamsMotion
{ {
Scalar cov_det = 1.0; // 1 rad^2 Scalar cov_det = 1.0; // 1 rad^2
Scalar unmeasured_perturbation_std = 0.001; // no particular dimension: the same for displacement and angle 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 class ProcessorOdom2D : public ProcessorMotion
......
...@@ -21,7 +21,12 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom3D); ...@@ -21,7 +21,12 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom3D);
struct ProcessorParamsOdom3D : public ProcessorParamsMotion struct ProcessorParamsOdom3D : public ProcessorParamsMotion
{ {
// ProcessorParamsOdom3D() = default;
ProcessorParamsOdom3D(std::string _unique_name, const paramsServer& _server):
ProcessorParamsMotion(_unique_name, _server)
{
//
}
}; };
WOLF_PTR_TYPEDEFS(ProcessorOdom3D); WOLF_PTR_TYPEDEFS(ProcessorOdom3D);
......
...@@ -84,6 +84,10 @@ target_link_libraries(gtest_motion_buffer ${PROJECT_NAME}) ...@@ -84,6 +84,10 @@ target_link_libraries(gtest_motion_buffer ${PROJECT_NAME})
wolf_add_gtest(gtest_pack_KF_buffer gtest_pack_KF_buffer.cpp) wolf_add_gtest(gtest_pack_KF_buffer gtest_pack_KF_buffer.cpp)
target_link_libraries(gtest_pack_KF_buffer ${PROJECT_NAME}) 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 # Problem class test
wolf_add_gtest(gtest_problem gtest_problem.cpp) wolf_add_gtest(gtest_problem gtest_problem.cpp)
target_link_libraries(gtest_problem ${PROJECT_NAME}) target_link_libraries(gtest_problem ${PROJECT_NAME})
......
#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();
}
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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment