diff --git a/hello_plugin/CMakeLists.txt b/hello_plugin/CMakeLists.txt index 75722dccbde1b387a589a419b75be6a9d146fbe9..ff20e63759550f5690f84ae02140361386eeb33f 100644 --- a/hello_plugin/CMakeLists.txt +++ b/hello_plugin/CMakeLists.txt @@ -4,7 +4,11 @@ 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) -target_link_libraries(hello_plugin class_loader boost_system console_bridge wolf yaml-cpp ${CERES_LIBRARIES}) +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 ) +target_link_libraries(hello_plugin boost_system wolf yaml-cpp ${CERES_LIBRARIES} dl) +target_link_libraries(params_autoconf boost_system wolf yaml-cpp dl) # 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 86baf7eb83c957293828e3a10f98d5a5f083b7b1..0f73b2ffe48ba95d3fbcc9838427ae2fd20ec71c 100644 --- a/hello_plugin/hello_plugin.cpp +++ b/hello_plugin/hello_plugin.cpp @@ -4,25 +4,24 @@ * Created on: Nov 12, 2018 * 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/yaml/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 "loader.hpp" #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; @@ -40,10 +39,15 @@ int main(int argc, char** argv) { 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*>(); + // vector<ClassLoader*> class_loaders = vector<ClassLoader*>(); + // for(auto it : parser.getFiles()) { + // auto c = new ClassLoader(it); + // class_loaders.push_back(c); + // } + auto loaders = vector<Loader*>(); for(auto it : parser.getFiles()) { - auto c = new ClassLoader(it); - class_loaders.push_back(c); + auto l = new LoaderRaw(it); + loaders.push_back(l); } ProblemPtr problem = Problem::create("PO 2D"); auto sensorMap = map<string, SensorBasePtr>(); @@ -56,6 +60,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/loader.hpp b/hello_plugin/loader.hpp new file mode 100644 index 0000000000000000000000000000000000000000..4f36eeecb9cda277e16bb495349ea047b546b6ce --- /dev/null +++ b/hello_plugin/loader.hpp @@ -0,0 +1,43 @@ +#ifndef LOADER_HPP +#define LOADER_HPP +#include <dlfcn.h> +class Loader{ +protected: + std::string path_; +public: + Loader(std::string _file){ + this->path_ = _file; + } + virtual void load() = 0; + virtual void close() = 0; +}; +class LoaderRaw: public Loader{ + void* resource_; +public: + LoaderRaw(std::string _file): + Loader(_file) + { + // + } + ~LoaderRaw(){ + this->close(); + } + void load(){ + this->resource_ = dlopen(this->path_.c_str(), RTLD_LAZY); + if(not this->resource_) + throw std::runtime_error("Couldn't load resource with path " + this->path_); + } + void close(){ + if(this->resource_) dlclose(this->resource_); + } +}; +// class LoaderPlugin: public Loader{ +// ClassLoader* resource_; +// void load(){ +// this->resource_ = new ClassLoader(this->path_); +// } +// void close(){ +// delete this->resource_; +// } +// }; +#endif \ No newline at end of file diff --git a/hello_plugin/params.yaml b/hello_plugin/params.yaml index ff190b7334b2cd1941da436b0846499bf965446a..db5fab3faaa036d7ba02f7f30c6a21468098330f 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 0000000000000000000000000000000000000000..e987769c16d943771665420f3816784bc7d49b5b --- /dev/null +++ b/hello_plugin/params_autoconf.cpp @@ -0,0 +1,70 @@ +/* + * params_autoconf.cpp + * + * Created on: Feb 15, 2019 + * Author: jcasals + */ +#include "base/sensor/sensor_base.h" +#include "base/wolf.h" +// #include "sensor_odom_2D.cpp" +#include <yaml-cpp/yaml.h> +#include "base/yaml/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 "loader.hpp" +#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 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. + **/ + auto loaders = vector<Loader*>(); + for(auto it : parser.getFiles()) { + auto l = new LoaderRaw(it); + loaders.push_back(l); + } + 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 0000000000000000000000000000000000000000..0d80a5802f5da772f59c29adf12257c8f79663d3 --- /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/include/base/converter.h b/include/base/converter.h index 357a5f81d9fa31f014353317b445e5d61b586e4d..2bff920a96c853c460a6aabc3fd863ecc1c6212c 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/hello_plugin/parser_yaml.hpp b/include/base/yaml/parser_yaml.hpp similarity index 79% rename from hello_plugin/parser_yaml.hpp rename to include/base/yaml/parser_yaml.hpp index 455561e01219fed6a80ea41118ed787af92fc6e7..491c15f6b0411f3961b3da09819e4f82889d0227 100644 --- a/hello_plugin/parser_yaml.hpp +++ b/include/base/yaml/parser_yaml.hpp @@ -62,6 +62,8 @@ class parserYAML { vector<ParamsInitProcessor> _paramsProc; vector<string> _files; string _file; + bool _relative_path; + string _path_root; public: parserYAML(){ _params = map<string, string>(); @@ -70,6 +72,8 @@ public: _paramsProc = vector<ParamsInitProcessor>(); _file = ""; _files = vector<string>(); + _path_root = ""; + _relative_path = false; } parserYAML(string file){ _params = map<string, string>(); @@ -78,6 +82,18 @@ public: _paramsProc = vector<ParamsInitProcessor>(); _files = vector<string>(); _file = file; + _path_root = ""; + _relative_path = false; + } + parserYAML(string file, string path_root){ + _params = map<string, string>(); + _active_name = ""; + _paramsSens = vector<ParamsInitSensor>(); + _paramsProc = vector<ParamsInitProcessor>(); + _files = vector<string>(); + _file = file; + _path_root = path_root; + _relative_path = true; } ~parserYAML(){ // @@ -104,16 +120,25 @@ string parserYAML::tagsToString(vector<std::string> &tags){ return hdr; } void parserYAML::walkTree(string file){ - YAML::Node n = YAML::LoadFile(file); + YAML::Node n; + // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl; + if(not _relative_path) n = YAML::LoadFile(file); + else n = YAML::LoadFile(_path_root + file); vector<string> hdrs = vector<string>(); walkTreeR(n, hdrs, ""); } void parserYAML::walkTree(string file, vector<string>& tags){ - YAML::Node n = YAML::LoadFile(file); + YAML::Node n; + // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl; + if(not _relative_path) n = YAML::LoadFile(file); + else n = YAML::LoadFile(_path_root + file); walkTreeR(n, tags, ""); } void parserYAML::walkTree(string file, vector<string>& tags, string hdr){ - YAML::Node n = YAML::LoadFile(file); + YAML::Node n; + // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl; + if(not _relative_path) n = YAML::LoadFile(file); + else n = YAML::LoadFile(_path_root + file); walkTreeR(n, tags, hdr); } void parserYAML::walkTreeR(YAML::Node n, vector<string>& tags, string hdr){ @@ -125,8 +150,8 @@ void parserYAML::walkTreeR(YAML::Node n, vector<string>& tags, string hdr){ // cout << "SUBSTR " << str.substr(1,str.size() - 1); walkTree(str.substr(1,str.size() - 1), tags, hdr); }else{ - std::copy(tags.begin(), tags.end(), std::ostream_iterator<string>(std::cout, "¬")); - cout << "«»" << n.Scalar() << endl; + // std::copy(tags.begin(), tags.end(), std::ostream_iterator<string>(std::cout, "¬")); + // cout << "«»" << n.Scalar() << endl; _params.insert(pair<string,string>(hdr, n.Scalar())); } break; @@ -140,13 +165,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); @@ -165,7 +197,10 @@ void parserYAML::updateActiveName(string tag){ this->_active_name = tag; } void parserYAML::parseFirstLevel(string file){ - YAML::Node n = YAML::LoadFile(file); + YAML::Node n; + // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl; + if(not _relative_path) n = YAML::LoadFile(file); + else n = YAML::LoadFile(_path_root + file); YAML::Node n_config = n["config"]; assert(n_config.Type() == YAML::NodeType::Map && "trying to parse config node but found a non-Map node"); for(const auto& kv : n_config["sensors"]){ diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 37859c19ea9713f5ca503c3b9fd9ac7abb7441db..9a3910693ca3a0408a3b00bc145442b28089c5cb 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -84,6 +84,14 @@ 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}) +# Parameters server +wolf_add_gtest(gtest_param_server gtest_param_server.cpp ${CMAKE_CURRENT_LIST_DIR}) +target_link_libraries(gtest_param_server ${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_param_server.cpp b/test/gtest_param_server.cpp new file mode 100644 index 0000000000000000000000000000000000000000..9dc28c7e4af19f090db038a384eb0cd7ade1198b --- /dev/null +++ b/test/gtest_param_server.cpp @@ -0,0 +1,32 @@ +#include "utils_gtest.h" +#include "base/converter.h" +#include "base/wolf.h" +#include "base/yaml/parser_yaml.hpp" +#include "base/params_server.hpp" + +using namespace std; +using namespace wolf; + +std::string wolf_root = _WOLF_ROOT_DIR; + +parserYAML parse(string _file, string _path_root) +{ + parserYAML parser = parserYAML(_file, _path_root); + parser.parse(); + return parser; +} + +TEST(ParamsServer, Default) +{ + auto parser = parse("/test/params1.yaml", wolf_root); + auto params = parser.getParams(); + paramsServer server = paramsServer(params, parser.sensorsSerialization(), parser.processorsSerialization()); + EXPECT_EQ(server.getParam<double>("should_not_exist", "2.6"), 2.6); + EXPECT_EQ(server.getParam<bool>("my_proc_test/voting_active", "true"), false); + EXPECT_NE(server.getParam<unsigned int>("my_proc_test/time_tolerance", "23"), 23); +} +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/gtest_parser_yaml.cpp b/test/gtest_parser_yaml.cpp new file mode 100644 index 0000000000000000000000000000000000000000..4ed91f95bacb06e69601dd233b2b9084af1bf2a9 --- /dev/null +++ b/test/gtest_parser_yaml.cpp @@ -0,0 +1,44 @@ +#include "utils_gtest.h" +#include "base/converter.h" +#include "base/wolf.h" +#include "base/yaml/parser_yaml.hpp" + +using namespace std; +using namespace wolf; + +std::string wolf_root = _WOLF_ROOT_DIR; + +parserYAML parse(string _file, string _path_root) +{ + parserYAML parser = parserYAML(_file, _path_root); + parser.parse(); + return parser; +} + +TEST(ParserYAML, RegularParse) +{ + auto parser = parse("/test/params1.yaml", wolf_root); + 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/sensorname"], "odom"); +} +TEST(ParserYAML, ParseMap) +{ + auto parser = parse("/test/params2.yaml", wolf_root); + auto params = parser.getParams(); + EXPECT_EQ(params["processor1/mymap"], "[{k1:v1},{k2:v2},{k3:[v3,v4,v5]}]"); +} +TEST(ParserYAML, JumpFile) +{ + auto parser = parse("/test/params3.yaml", wolf_root); + auto params = parser.getParams(); + EXPECT_EQ(params["my_proc_test/max_buff_length"], "100"); + EXPECT_EQ(params["my_proc_test/jump/voting_active"], "false"); +} +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 0000000000000000000000000000000000000000..5f89b4307ef8c86a350f24c39203d5ba3738c3f9 --- /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: "/test/params3.1.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/test/params2.yaml b/test/params2.yaml new file mode 100644 index 0000000000000000000000000000000000000000..30167145d3bf2c775ef497182de2e9a7022be0dd --- /dev/null +++ b/test/params2.yaml @@ -0,0 +1,33 @@ +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" + $mymap: + k1: v1 + k2: v2 + k3: [v3,v4,v5] + - + type: "RANGE BEARING" + name: "rb_processor" + sensorname: "rb" + - + type: "ODOM 2D" + name: "my_proc_test" + sensorname: "odom" +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/test/params3.1.yaml b/test/params3.1.yaml new file mode 100644 index 0000000000000000000000000000000000000000..bd9b495c830e56f17f11abbb5b2109d780f4e438 --- /dev/null +++ b/test/params3.1.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/test/params3.yaml b/test/params3.yaml new file mode 100644 index 0000000000000000000000000000000000000000..3917ee1cd386b0c1ef0ff29a4b9b1460ad43e421 --- /dev/null +++ b/test/params3.yaml @@ -0,0 +1,31 @@ +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: "/test/params3.1.yaml" + jump: "@/test/params3.1.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