diff --git a/cmake_modules/wolfConfig.cmake b/cmake_modules/wolfConfig.cmake index f370972f1fd757b09c335756e8fd099c5e426102..90e622c1b964cfb78b1664b1ed9794f8004bff47 100644 --- a/cmake_modules/wolfConfig.cmake +++ b/cmake_modules/wolfConfig.cmake @@ -66,9 +66,6 @@ set(wolf_FOUND TRUE) # Now we gather all the required dependencies for Wolf -# FIND_PACKAGE(Eigen3 3.2.92 REQUIRED) -# list(APPEND wolf_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) - get_filename_component(WOLF_CURRENT_CONFIG_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) @@ -87,4 +84,9 @@ FIND_PACKAGE(YamlCpp REQUIRED) list(APPEND wolf_INCLUDE_DIRS ${YAMLCPP_INCLUDE_DIRS}) list(APPEND wolf_LIBRARIES ${YAMLCPP_LIBRARY}) +if(NOT Eigen_FOUND) + FIND_PACKAGE(Eigen3 REQUIRED) + list(APPEND wolf_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) +endif() + SET(CMAKE_MODULE_PATH ${BACKUP_MODULE_PATH}) \ No newline at end of file diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h index fdfb8a1052e4dabda74f7060c4e6c0128d6c29cd..935a887be9a1bbbf467335d54692204e0b6f62b0 100644 --- a/include/core/problem/problem.h +++ b/include/core/problem/problem.h @@ -61,7 +61,7 @@ class Problem : public std::enable_shared_from_this<Problem> public: static ProblemPtr create(const std::string& _frame_structure, SizeEigen _dim); // USE THIS AS A CONSTRUCTOR! - static ProblemPtr autoSetup(const std::string& _yaml_file); + static ProblemPtr autoSetup(paramsServer &_server); virtual ~Problem(); // Properties ----------------------------------------- diff --git a/include/core/utils/converter.h b/include/core/utils/converter.h index e6ffee4053501c652b197787207f9c8b1e922e50..49f003633c42e4e5b6dc051e2c0c2c0c560b14b3 100644 --- a/include/core/utils/converter.h +++ b/include/core/utils/converter.h @@ -107,7 +107,7 @@ struct converter<utils::list<A>>{ utils::list<A> result = utils::list<A>(); if(std::regex_match(val, rgxP)) { std::string aux = val.substr(1,val.size()-2); - auto l = utils::getMatches(aux, std::regex("([^,]+),?")); + auto l = utils::getMatches(aux, std::regex("([^,]+)")); for(auto it : l){ result.push_back(converter<A>::convert(it)); } diff --git a/include/core/yaml/parser_yaml.hpp b/include/core/yaml/parser_yaml.hpp index 9330ad79e000a9caff7d558929ad32c64b8b1408..c721a163ac59abb487cfd01edbd3235f95d875d7 100644 --- a/include/core/yaml/parser_yaml.hpp +++ b/include/core/yaml/parser_yaml.hpp @@ -1,6 +1,7 @@ #ifndef PARSER_YAML_HPP #define PARSER_YAML_HPP #include "yaml-cpp/yaml.h" +#include "core/utils/converter.h" #include <vector> #include <regex> #include <map> @@ -8,21 +9,20 @@ #include <algorithm> #include <numeric> -using namespace std; 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 string [v1,v2,v3,...] representing the YAML sequence node + /** @Brief Generates a std::string [v1,v2,v3,...] representing the YAML sequence node * @param n a YAML::Node * @return <b>{std::string}</b> [v1,v2,v3,...] */ - string parseSequence(YAML::Node n){ + 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(); - string aux = "["; + std::string aux = "["; bool first = true; for(auto it : n){ if(first) { @@ -35,19 +35,19 @@ namespace { aux = aux + "]"; return aux; } - /** @Brief Transforms a map<string,string> to its string representation [{k1:v1},{k2:v2},{k3:v3},...] - * @param _map just a map<string,string> + /** @Brief Transforms a std::map<std::string,std::string> to its std::string representation [{k1:v1},{k2:v2},{k3:v3},...] + * @param _map just a std::map<std::string,std::string> * @return <b>{std::string}</b> [{k1:v1},{k2:v2},{k3:v3},...] */ std::string mapToString(std::map<std::string,std::string> _map){ std::string result = ""; - auto v = std::vector<string>(); - std::transform(_map.begin(), _map.end(), back_inserter(v), [](const std::pair<string,string> p){return "{" + p.first + ":" + p.second + "}";}); - auto concat = [](string ac, string str)-> string { + auto v = std::vector<std::string>(); + std::transform(_map.begin(), _map.end(), back_inserter(v), [](const std::pair<std::string,std::string> p){return "{" + p.first + ":" + p.second + "}";}); + auto concat = [](std::string ac, std::string str)-> std::string { return ac + str + ","; }; - string aux = ""; - string accumulated = std::accumulate(v.begin(), v.end(), aux, concat); + std::string aux = ""; + std::string accumulated = std::accumulate(v.begin(), v.end(), aux, concat); if(accumulated.size() > 1) accumulated = accumulated.substr(0,accumulated.size() - 1); else accumulated = ""; return "[" + accumulated + "]"; @@ -55,110 +55,115 @@ namespace { } class parserYAML { struct ParamsInitSensor{ - string _type; - string _name; + std::string _type; + std::string _name; YAML::Node n; }; struct ParamsInitProcessor{ - string _type; - string _name; - string _name_assoc_sensor; + std::string _type; + std::string _name; + std::string _name_assoc_sensor; YAML::Node n; }; - map<string, string> _params; - string _active_name; - vector<ParamsInitSensor> _paramsSens; - vector<ParamsInitProcessor> _paramsProc; - vector<string> _files; - string _file; + std::map<std::string, std::string> _params; + std::string _active_name; + std::vector<ParamsInitSensor> _paramsSens; + std::vector<ParamsInitProcessor> _paramsProc; + std::vector<std::string> _files; + std::string _file; bool _relative_path; - string _path_root; - vector<array<string, 3>> _callbacks; + std::string _path_root; + std::vector<std::array<std::string, 3>> _callbacks; YAML::Node problem; std::string generatePath(std::string); public: parserYAML(){ - _params = map<string, string>(); + _params = std::map<std::string, std::string>(); _active_name = ""; - _paramsSens = vector<ParamsInitSensor>(); - _paramsProc = vector<ParamsInitProcessor>(); + _paramsSens = std::vector<ParamsInitSensor>(); + _paramsProc = std::vector<ParamsInitProcessor>(); _file = ""; - _files = vector<string>(); + _files = std::vector<std::string>(); _path_root = ""; _relative_path = false; - _callbacks = vector<array<string, 3>>(); + _callbacks = std::vector<std::array<std::string, 3>>(); } - parserYAML(string file){ - _params = map<string, string>(); + parserYAML(std::string file){ + _params = std::map<std::string, std::string>(); _active_name = ""; - _paramsSens = vector<ParamsInitSensor>(); - _paramsProc = vector<ParamsInitProcessor>(); - _files = vector<string>(); + _paramsSens = std::vector<ParamsInitSensor>(); + _paramsProc = std::vector<ParamsInitProcessor>(); + _files = std::vector<std::string>(); _file = file; _path_root = ""; _relative_path = false; - _callbacks = vector<array<string, 3>>(); + _callbacks = std::vector<std::array<std::string, 3>>(); } - parserYAML(string file, string path_root){ - _params = map<string, string>(); + parserYAML(std::string file, std::string path_root){ + _params = std::map<std::string, std::string>(); _active_name = ""; - _paramsSens = vector<ParamsInitSensor>(); - _paramsProc = vector<ParamsInitProcessor>(); - _files = vector<string>(); + _paramsSens = std::vector<ParamsInitSensor>(); + _paramsProc = std::vector<ParamsInitProcessor>(); + _files = std::vector<std::string>(); _file = file; - - regex r("/$"); - if(not regex_match(path_root, r)) _path_root = path_root + "/"; - else _path_root = path_root; - _relative_path = true; - _callbacks = vector<array<string, 3>>(); + if(path_root != ""){ + std::regex r("/$"); + if(not std::regex_match(path_root, r)) _path_root = path_root + "/"; + else _path_root = path_root; + _relative_path = true; + }else{ + _relative_path = false; + } + _callbacks = std::vector<std::array<std::string, 3>>(); } ~parserYAML(){ // } - void walkTree(string file); - void walkTree(string file, vector<string>& tags); - void walkTree(string file, vector<string>& tags, string hdr); - void walkTreeR(YAML::Node n, vector<string>& tags, string hdr); - void updateActiveName(string tag); - void parseFirstLevel(string file); - string tagsToString(vector<string>& tags); - vector<array<string, 2>> sensorsSerialization(); - vector<array<string, 3>> processorsSerialization(); - vector<string> getFiles(); - vector<array<string, 3>> getCallbacks(); - vector<array<string, 2>> getProblem(); - map<string,string> getParams(); + void walkTree(std::string file); + void walkTree(std::string file, std::vector<std::string>& tags); + void walkTree(std::string file, std::vector<std::string>& tags, std::string hdr); + void walkTreeR(YAML::Node n, std::vector<std::string>& tags, std::string hdr); + void updateActiveName(std::string tag); + void parseFirstLevel(std::string file); + std::string tagsToString(std::vector<std::string>& tags); + std::vector<std::array<std::string, 2>> sensorsSerialization(); + std::vector<std::array<std::string, 3>> processorsSerialization(); + std::vector<std::string> getFiles(); + std::vector<std::array<std::string, 3>> getCallbacks(); + std::vector<std::array<std::string, 2>> getProblem(); + std::map<std::string,std::string> getParams(); void parse(); - map<string, string> fetchAsMap(YAML::Node); + std::map<std::string, std::string> fetchAsMap(YAML::Node); }; std::string parserYAML::generatePath(std::string path){ - regex r("^/.*"); - if(regex_match(path, r)){ + std::regex r("^/.*"); + if(std::regex_match(path, r)){ + std::cout << "Generating " + path << std::endl; return path; }else{ + std::cout << "Generating " + _path_root + path << std::endl; return _path_root + path; } } -string parserYAML::tagsToString(vector<std::string> &tags){ - string hdr = ""; +std::string parserYAML::tagsToString(std::vector<std::string> &tags){ + std::string hdr = ""; for(auto it : tags){ hdr = hdr + "/" + it; } return hdr; } -void parserYAML::walkTree(string file){ +void parserYAML::walkTree(std::string file){ YAML::Node n; n = YAML::LoadFile(generatePath(file)); - vector<string> hdrs = vector<string>(); + std::vector<std::string> hdrs = std::vector<std::string>(); walkTreeR(n, hdrs, ""); } -void parserYAML::walkTree(string file, vector<string>& tags){ +void parserYAML::walkTree(std::string file, std::vector<std::string>& tags){ YAML::Node n; n = YAML::LoadFile(generatePath(file)); walkTreeR(n, tags, ""); } -void parserYAML::walkTree(string file, vector<string>& tags, string hdr){ +void parserYAML::walkTree(std::string file, std::vector<std::string>& tags, std::string hdr){ YAML::Node n; n = YAML::LoadFile(generatePath(file)); walkTreeR(n, tags, hdr); @@ -168,29 +173,29 @@ void parserYAML::walkTree(string file, vector<string>& tags, string hdr){ * @param tags represents the path from the root of the YAML tree to the current node * @param hdr is the name of the current YAML node */ -void parserYAML::walkTreeR(YAML::Node n, vector<string>& tags, string hdr){ +void parserYAML::walkTreeR(YAML::Node n, std::vector<std::string>& tags, std::string hdr){ switch (n.Type()) { case YAML::NodeType::Scalar : { - regex r("^@.*"); - if(regex_match(n.Scalar(), r)){ - string str = n.Scalar(); + std::regex r("^@.*"); + if(std::regex_match(n.Scalar(), r)){ + std::string str = n.Scalar(); walkTree(str.substr(1,str.size() - 1), tags, hdr); }else{ - _params.insert(pair<string,string>(hdr, n.Scalar())); + _params.insert(std::pair<std::string,std::string>(hdr, n.Scalar())); } break; } case YAML::NodeType::Sequence : { - string aux = parseSequence(n); - _params.insert(pair<string,string>(hdr, aux)); + std::string aux = parseSequence(n); + _params.insert(std::pair<std::string,std::string>(hdr, aux)); 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, //otherwise the parser recursively parses the map. - regex r("^\\$.*"); - if(not regex_match(kv.first.as<string>(), r)){ + std::regex r("^\\$.*"); + if(not std::regex_match(kv.first.as<std::string>(), r)){ /* If key=="follow" then the parser will assume that the value is a path and will parse the (expected) yaml file at the specified path. Note that this does not increase the header depth. @@ -213,21 +218,21 @@ void parserYAML::walkTreeR(YAML::Node n, vector<string>& tags, string hdr){ - $follow: "@some_path" | - var: 1.2 | */ - 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>()); + std::regex rr("follow"); + if(not std::regex_match(kv.first.as<std::string>(), rr)) { + tags.push_back(kv.first.as<std::string>()); + if(tags.size() == 2) this->updateActiveName(kv.first.as<std::string>()); + walkTreeR(kv.second, tags, hdr +"/"+ kv.first.as<std::string>()); tags.pop_back(); if(tags.size() == 1) this->updateActiveName(""); }else{ - walkTree(kv.second.as<string>(), tags, hdr); + walkTree(kv.second.as<std::string>(), tags, hdr); } }else{ - string key = kv.first.as<string>(); + std::string key = kv.first.as<std::string>(); key = key.substr(1,key.size() - 1); auto fm = fetchAsMap(kv.second); - _params.insert(pair<string,string>(hdr + "/" + key, mapToString(fm))); + _params.insert(std::pair<std::string,std::string>(hdr + "/" + key, mapToString(fm))); } } break; @@ -237,13 +242,13 @@ void parserYAML::walkTreeR(YAML::Node n, vector<string>& tags, string hdr){ break; } } -void parserYAML::updateActiveName(string tag){ +void parserYAML::updateActiveName(std::string tag){ this->_active_name = tag; } /** @Brief Parse the sensors, processors, callbacks and files elements of the YAML file. We assume that these elements are defined at the top level of the YAML file. * @param file is the path to the YAML file */ -void parserYAML::parseFirstLevel(string file){ +void parserYAML::parseFirstLevel(std::string file){ YAML::Node n; n = YAML::LoadFile(generatePath(file)); @@ -265,76 +270,77 @@ void parserYAML::parseFirstLevel(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))); } -vector<array<string, 2>> parserYAML::sensorsSerialization(){ - vector<array<string, 2>> aux = vector<array<string, 2>>(); +std::vector<std::array<std::string, 2>> parserYAML::sensorsSerialization(){ + std::vector<std::array<std::string, 2>> aux = std::vector<std::array<std::string, 2>>(); for(auto it : this->_paramsSens) aux.push_back({{it._type,it._name}}); return aux; } -vector<array<string, 3>> parserYAML::processorsSerialization(){ - vector<array<string, 3>> aux = vector<array<string, 3>>(); +std::vector<std::array<std::string, 3>> parserYAML::processorsSerialization(){ + std::vector<std::array<std::string, 3>> aux = std::vector<std::array<std::string, 3>>(); for(auto it : this->_paramsProc) aux.push_back({{it._type,it._name,it._name_assoc_sensor}}); return aux; } -vector<string> parserYAML::getFiles(){ +std::vector<std::string> parserYAML::getFiles(){ return this->_files; } -vector<array<string, 3>> parserYAML::getCallbacks(){ +std::vector<std::array<std::string, 3>> parserYAML::getCallbacks(){ return this->_callbacks; } -vector<array<string, 2>> parserYAML::getProblem(){ - return vector<array<string, 2>>(); +std::vector<std::array<std::string, 2>> parserYAML::getProblem(){ + return std::vector<std::array<std::string, 2>>(); } -map<string,string> parserYAML::getParams(){ - map<string,string> rtn = _params; +std::map<std::string,std::string> parserYAML::getParams(){ + std::map<std::string,std::string> rtn = _params; return rtn; } void parserYAML::parse(){ this->parseFirstLevel(this->_file); if(this->problem.Type() != YAML::NodeType::Undefined){ - vector<string> tags = vector<string>(); + std::vector<std::string> tags = std::vector<std::string>(); this->walkTreeR(this->problem, tags , "problem"); } for(auto it : _paramsSens){ - vector<string> tags = vector<string>(); + std::vector<std::string> tags = std::vector<std::string>(); this->walkTreeR(it.n , tags , it._name); } for(auto it : _paramsProc){ - vector<string> tags = vector<string>(); + std::vector<std::string> tags = std::vector<std::string>(); this->walkTreeR(it.n , tags , it._name); } } -map<string, string> parserYAML::fetchAsMap(YAML::Node 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 = map<string, string>(); + auto m = std::map<std::string, std::string>(); for(const auto& kv : n){ - string key = kv.first.as<string>(); + std::string key = kv.first.as<std::string>(); switch (kv.second.Type()) { case YAML::NodeType::Scalar : { - string value = kv.second.Scalar(); - m.insert(pair<string,string>(key, value)); + std::string value = kv.second.Scalar(); + m.insert(std::pair<std::string,std::string>(key, value)); break; } case YAML::NodeType::Sequence : { - string aux = parseSequence(kv.second); - m.insert(pair<string,string>(key, aux)); + std::string aux = parseSequence(kv.second); + m.insert(std::pair<std::string,std::string>(key, aux)); break; } case YAML::NodeType::Map : { m = fetchAsMap(kv.second); - auto rtn = vector<string>(); - std::transform(m.begin(), m.end(), back_inserter(rtn), [](const std::pair<string,string> v){return "{" + v.first + ":" + v.second + "}";}); - auto concat = [](string ac, string str)-> string { + 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 + ","; }; - string aux = ""; - string accumulated = std::accumulate(rtn.begin(), rtn.end(), aux, concat); + 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(pair<string,string>(key, "[" + accumulated + "]")); + m.insert(std::pair<std::string,std::string>(key, "[" + accumulated + "]")); break; } default: diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index fa78a9e38aea5c2ba26cbd4ee3f5a54531dd6685..9cd5226b7399dc598c5e5e4e351882b2c420944e 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -11,7 +11,6 @@ #include "core/sensor/sensor_factory.h" #include "core/processor/processor_factory.h" #include "core/state_block/state_block.h" -#include "core/yaml/parser_yaml.hpp" #include "core/utils/params_server.hpp" #include "core/utils/loader.hpp" @@ -20,6 +19,8 @@ // C++ includes #include <algorithm> +#include <map> +#include <vector> namespace wolf { @@ -72,36 +73,35 @@ ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim) p->setup(); return p->shared_from_this(); } -ProblemPtr Problem::autoSetup(const std::string& _yaml_file) +ProblemPtr Problem::autoSetup(paramsServer &_server) { - // string file = "/home/jcasals/catkin_ws/src/wolf_ros_wrapper/src/params.yaml"; - parserYAML parser = parserYAML(_yaml_file); - parser.parse(); - paramsServer server = paramsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization()); - std::string frame_structure = server.getParam<std::string>("problem/frame structure", "PO"); - int dim = server.getParam<int>("problem/dimension", "2"); + // parserYAML parser = parserYAML(_yaml_file, _root_path); + // parser.parse(); + // paramsServer server = paramsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization()); + std::string frame_structure = _server.getParam<std::string>("problem/frame structure", "PO"); + int dim = _server.getParam<int>("problem/dimension", "2"); auto p = Problem::create(frame_structure, dim); // cout << "PRINTING SERVER MAP" << endl; - // server.print(); + // _server.print(); // cout << "-----------------------------------" << endl; - auto loaders = vector<Loader*>(); - for(auto it : parser.getFiles()) { - cout << "LOADING " << it << endl; + auto loaders = std::vector<Loader*>(); + for(auto it : _server.getParam<std::vector<std::string>>("files")) { + std::cout << "LOADING " << it << std::endl; auto l = new LoaderRaw(it); l->load(); loaders.push_back(l); } //TODO: To be fixed. This prior should be set in here, but now it is set externally. // setPrior(Eigen::Vector3s::Zero(), 0.1*Eigen::Matrix3s::Identity(), TimeStamp(), Scalar(0.1)); - 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,p->installSensor(s._type, s._name, server))); + auto sensorMap = std::map<std::string, SensorBasePtr>(); + auto procesorMap = std::map<std::string, ProcessorBasePtr>(); + for(auto s : _server.getSensors()){ + // cout << s._name << " " << s._type << endl; + sensorMap.insert(std::pair<std::string, SensorBasePtr>(s._name,p->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,p->installProcessor(s._type, s._name, s._name_assoc_sensor, server))); + for(auto s : _server.getProcessors()){ + // cout << s._name << " " << s._type << " " << s._name_assoc_sensor << endl; + procesorMap.insert(std::pair<std::string, ProcessorBasePtr>(s._name,p->installProcessor(s._type, s._name, s._name_assoc_sensor, _server))); } return p; } diff --git a/test/gtest_converter.cpp b/test/gtest_converter.cpp index b49f30a2d3e3f519dcc9e525aa8652830c809b51..6a263e55e7bdb16bfba9a1ab9c275c136e269344 100644 --- a/test/gtest_converter.cpp +++ b/test/gtest_converter.cpp @@ -20,6 +20,13 @@ TEST(Converter, ParseToVector) ASSERT_EQ(v[8],11); vector<string> vs {"a","b","c"}; ASSERT_EQ(converter<string>::convert(vs), "[a,b,c]"); + string v2 = "[first,second,third,fourth]"; + vector<string> vv = converter<vector<string>>::convert(v2); + ASSERT_EQ(vv.size(),4); + ASSERT_EQ(vv[0],"first"); + ASSERT_EQ(vv[1],"second"); + ASSERT_EQ(vv[2],"third"); + ASSERT_EQ(vv[3],"fourth"); } TEST(Converter, ParseToEigenMatrix)