diff --git a/CMakeLists.txt b/CMakeLists.txt
index fb83ccf5399baef48a2267f0657cf68d8bbd60db..7dafd62f1dcd36919049428014466507f73cb70e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -185,10 +185,10 @@ SET(HDRS_UTILS
include/core/utils/converter.h
include/core/utils/eigen_assert.h
include/core/utils/eigen_predicates.h
- include/core/utils/loader.hpp
+ include/core/utils/loader.h
include/core/utils/logging.h
include/core/utils/make_unique.h
- include/core/utils/params_server.hpp
+ include/core/utils/params_server.h
include/core/utils/singleton.h
include/core/utils/utils_gtest.h
include/core/utils/converter_utils.h
@@ -287,7 +287,7 @@ SET(HDRS_SOLVER
)
SET(HDRS_YAML
- include/core/yaml/parser_yaml.hpp
+ include/core/yaml/parser_yaml.h
include/core/yaml/yaml_conversion.h
)
#SOURCES
@@ -321,6 +321,8 @@ SET(SRCS_MATH
)
SET(SRCS_UTILS
src/utils/converter_utils.cpp
+ src/utils/params_server.cpp
+ src/utils/loader.cpp
)
SET(SRCS_CAPTURE
@@ -369,6 +371,7 @@ SET(SRCS_SOLVER
src/solver/solver_manager.cpp
)
SET(SRCS_YAML
+ src/yaml/parser_yaml.cpp
src/yaml/processor_odom_3d_yaml.cpp
src/yaml/sensor_odom_2d_yaml.cpp
src/yaml/sensor_odom_3d_yaml.cpp
diff --git a/hello_wolf/hello_wolf_autoconf.cpp b/hello_wolf/hello_wolf_autoconf.cpp
index a97d8d2f9c5c7fb8c7f1f37c12330038beb3ba98..a387adfe9656523c37d025bb3a74f0f198281b3d 100644
--- a/hello_wolf/hello_wolf_autoconf.cpp
+++ b/hello_wolf/hello_wolf_autoconf.cpp
@@ -9,7 +9,7 @@
// wolf core includes
#include "core/common/wolf.h"
#include "core/capture/capture_odom_2d.h"
-#include "core/yaml/parser_yaml.hpp"
+#include "core/yaml/parser_yaml.h"
#include "core/ceres_wrapper/ceres_manager.h"
// hello wolf local includes
diff --git a/hello_wolf/sensor_range_bearing.h b/hello_wolf/sensor_range_bearing.h
index e62bfc1a5cd4ff7115cac06f9738204c71017f2b..491efff4f533edc993051482de7d0e84562a790a 100644
--- a/hello_wolf/sensor_range_bearing.h
+++ b/hello_wolf/sensor_range_bearing.h
@@ -9,7 +9,7 @@
#define HELLO_WOLF_SENSOR_RANGE_BEARING_H_
#include "core/sensor/sensor_base.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
namespace wolf
{
diff --git a/include/core/ceres_wrapper/ceres_manager.h b/include/core/ceres_wrapper/ceres_manager.h
index 914002ef1d180aa7e5b85716a84334f5ce944e8a..802ab7b49dabf93455f8f93fc8c30745415fe33e 100644
--- a/include/core/ceres_wrapper/ceres_manager.h
+++ b/include/core/ceres_wrapper/ceres_manager.h
@@ -8,7 +8,7 @@
//wolf includes
#include "core/solver/solver_manager.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
#include "core/ceres_wrapper/cost_function_wrapper.h"
#include "core/ceres_wrapper/local_parametrization_wrapper.h"
#include "core/ceres_wrapper/create_numeric_diff_cost_function.h"
diff --git a/include/core/common/params_base.h b/include/core/common/params_base.h
index 5295bc0ac17f941503a8f28e462962290a9ee82b..80f6516415e28ddce47ab272415604014ffb0a66 100644
--- a/include/core/common/params_base.h
+++ b/include/core/common/params_base.h
@@ -1,7 +1,7 @@
#ifndef PARAMS_BASE_H_
#define PARAMS_BASE_H_
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
namespace wolf {
struct ParamsBase
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 36f3ce94e88681f32529f28b1bd02c93ee5316ab..f2b19ef05c22f7a4c6777f889031dbda45d80a5b 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -18,7 +18,7 @@ struct ProcessorParamsBase;
#include "core/common/wolf.h"
#include "core/frame/frame_base.h"
#include "core/state_block/state_block.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
#include "core/sensor/sensor_factory.h"
#include "core/processor/processor_factory.h"
#include "core/processor/is_motion.h"
@@ -349,7 +349,7 @@ class Problem : public std::enable_shared_from_this<Problem>
bool constr_by = false, //
bool metric = true, //
bool state_blocks = false) const;
- bool check(int verbose_level) const;
+ bool check(int verbose_level = 0) const;
bool check(bool verbose, std::ostream& stream) const;
};
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index ddaa1af0fe1c586fc72d53c7f8fec5a0a0988d97..81a9184ad4798ccbaac1ebd61a194f6e287331b4 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -13,7 +13,7 @@
#include "core/processor/processor_base.h"
#include "core/processor/is_motion.h"
#include "core/common/time_stamp.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
// std
#include <iomanip>
diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
index 6d46ab61a32c1b9573f386c97f1ae671086478c6..b03416837ec1b0a6a86a67ff0cf40fdf820b1370 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -12,7 +12,7 @@
#include "core/capture/capture_odom_2d.h"
#include "core/factor/factor_odom_2d.h"
#include "core/math/rotations.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
namespace wolf {
diff --git a/include/core/sensor/sensor_factory.h b/include/core/sensor/sensor_factory.h
index 3930176443a22cebb8c9591253af7f58b0a40a2c..daf01499ac759d799517cdd73ab830fb6ccaf4d5 100644
--- a/include/core/sensor/sensor_factory.h
+++ b/include/core/sensor/sensor_factory.h
@@ -16,7 +16,7 @@ struct ParamsSensorBase;
// wolf
#include "core/common/factory.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
namespace wolf
{
diff --git a/include/core/sensor/sensor_odom_2d.h b/include/core/sensor/sensor_odom_2d.h
index d6a431d3102b0a82a218ee71247552066c1ef8eb..45499f5c56fb342ed8a82b8521e6c2e156d06031 100644
--- a/include/core/sensor/sensor_odom_2d.h
+++ b/include/core/sensor/sensor_odom_2d.h
@@ -3,7 +3,7 @@
//wolf includes
#include "core/sensor/sensor_base.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
namespace wolf {
diff --git a/include/core/sensor/sensor_odom_3d.h b/include/core/sensor/sensor_odom_3d.h
index 67efe1e9af27f8456401c400766dcbf32f44057b..3d08375b4fc30e43f07f201a5653d5e83a258381 100644
--- a/include/core/sensor/sensor_odom_3d.h
+++ b/include/core/sensor/sensor_odom_3d.h
@@ -10,7 +10,7 @@
//wolf includes
#include "core/sensor/sensor_base.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
namespace wolf {
diff --git a/include/core/utils/loader.h b/include/core/utils/loader.h
new file mode 100644
index 0000000000000000000000000000000000000000..6f1a4cc68c2d0dcb20a6ef18f53ec259507fc176
--- /dev/null
+++ b/include/core/utils/loader.h
@@ -0,0 +1,32 @@
+#ifndef LOADER_H
+#define LOADER_H
+
+#include <string>
+
+class Loader{
+protected:
+ std::string path_;
+public:
+ Loader(std::string _file);
+ virtual void load() = 0;
+ virtual void close() = 0;
+};
+
+class LoaderRaw: public Loader{
+ void* resource_;
+public:
+ LoaderRaw(std::string _file);
+ ~LoaderRaw();
+ void load();
+ void close();
+};
+// class LoaderPlugin: public Loader{
+// ClassLoader* resource_;
+// void load(){
+// this->resource_ = new ClassLoader(this->path_);
+// }
+// void close(){
+// delete this->resource_;
+// }
+// };
+#endif
diff --git a/include/core/utils/loader.hpp b/include/core/utils/loader.hpp
deleted file mode 100644
index 3d867d799b36bfd53fb42e93577554caf1b050c1..0000000000000000000000000000000000000000
--- a/include/core/utils/loader.hpp
+++ /dev/null
@@ -1,45 +0,0 @@
-#ifndef LOADER_HPP
-#define LOADER_HPP
-#include <dlfcn.h>
-#include <string>
-#include <stdexcept>
-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_ + "\n" + "Error info: " + dlerror());
- }
- 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/include/core/utils/params_server.h b/include/core/utils/params_server.h
new file mode 100644
index 0000000000000000000000000000000000000000..e8b473ca26703f17ea932cca4d1e913717108026
--- /dev/null
+++ b/include/core/utils/params_server.h
@@ -0,0 +1,55 @@
+#ifndef PARAMS_SERVER_H
+#define PARAMS_SERVER_H
+
+#include "core/utils/converter.h"
+
+#include <map>
+#include <exception>
+
+namespace wolf{
+
+class MissingValueException : public std::runtime_error
+{
+public:
+ MissingValueException(std::string _msg) : std::runtime_error(_msg) {}
+};
+
+class ParamsServer{
+ std::map<std::string, std::string> params_;
+public:
+ ParamsServer();
+ ParamsServer(std::map<std::string, std::string> _params);
+ ~ParamsServer(){
+ //
+ }
+
+ void print();
+
+
+ void addParam(std::string _key, std::string _value);
+
+ void addParams(std::map<std::string, std::string> _params);
+
+ // template<typename T>
+ // T getParam(std::string key, std::string def_value) const {
+ // if(params_.find(key) != params_.end()){
+ // return converter<T>::convert(params_.find(key)->second);
+ // }else{
+ // return converter<T>::convert(def_value);
+ // }
+ // }
+
+ template<typename T>
+ T getParam(std::string _key) const {
+ if(params_.find(_key) != params_.end()){
+ return converter<T>::convert(params_.find(_key)->second);
+ }else{
+ throw MissingValueException("The following key: '" + _key + "' has not been found in the parameters server.");
+ }
+ }
+
+};
+
+}
+
+#endif
diff --git a/include/core/utils/params_server.hpp b/include/core/utils/params_server.hpp
deleted file mode 100644
index a750eab79f4ee90b39e4e21d1926129897c29d92..0000000000000000000000000000000000000000
--- a/include/core/utils/params_server.hpp
+++ /dev/null
@@ -1,70 +0,0 @@
-#ifndef PARAMS_SERVER_HPP
-#define PARAMS_SERVER_HPP
-
-#include "core/utils/converter.h"
-//#include "core/yaml/parser_yaml.hpp"
-
-#include <vector>
-#include <regex>
-#include <map>
-#include <exception>
-
-namespace wolf{
-
-class MissingValueException : public std::runtime_error
-{
-public:
- MissingValueException(std::string msg) : std::runtime_error(msg) {}
-};
-
-class ParamsServer{
- std::map<std::string, std::string> _params;
-public:
- ParamsServer(){
- _params = std::map<std::string, std::string>();
- }
- ParamsServer(std::map<std::string, std::string> params){
- _params = params;
- }
- ~ParamsServer(){
- //
- }
-
- void print(){
- for(auto it : _params)
- std::cout << it.first << "~~" << it.second << std::endl;
- }
-
-
- void addParam(std::string key, std::string value){
- _params.insert(std::pair<std::string, std::string>(key, value));
- }
-
- void addParams(std::map<std::string, std::string> params)
- {
- _params.insert(params.begin(), params.end());
- }
-
- // template<typename T>
- // T getParam(std::string key, std::string def_value) const {
- // if(_params.find(key) != _params.end()){
- // return converter<T>::convert(_params.find(key)->second);
- // }else{
- // return converter<T>::convert(def_value);
- // }
- // }
-
- template<typename T>
- T getParam(std::string key) const {
- if(_params.find(key) != _params.end()){
- return converter<T>::convert(_params.find(key)->second);
- }else{
- throw MissingValueException("The following key: '" + key + "' has not been found in the parameters server.");
- }
- }
-
-};
-
-}
-
-#endif
diff --git a/include/core/yaml/parser_yaml.h b/include/core/yaml/parser_yaml.h
new file mode 100644
index 0000000000000000000000000000000000000000..1b6f659ebd609dffef4f3c472800cf32714f22e1
--- /dev/null
+++ b/include/core/yaml/parser_yaml.h
@@ -0,0 +1,80 @@
+#ifndef PARSER_YAML_H
+#define PARSER_YAML_H
+
+#include "core/utils/converter.h"
+#include "core/common/wolf.h"
+
+#include "yaml-cpp/yaml.h"
+
+namespace wolf
+{
+class ParserYAML {
+ struct ParamsInitSensor{
+ std::string type_;
+ std::string name_;
+ std::string plugin_;
+ YAML::Node n_;
+ };
+ struct ParamsInitProcessor{
+ std::string type_;
+ std::string name_;
+ std::string name_assoc_sensor_;
+ std::string plugin_;
+ YAML::Node n_;
+ };
+ struct SubscriberManager{
+ std::string package_;
+ std::string subscriber_;
+ std::string topic_;
+ std::string sensor_name_;
+ YAML::Node n_;
+ };
+ struct PublisherManager{
+ std::string subscriber_;
+ std::string topic_;
+ std::string period_;
+ YAML::Node n_;
+ };
+ std::map<std::string, std::string> params_;
+ std::string active_name_;
+ std::vector<ParamsInitSensor> paramsSens_;
+ std::vector<ParamsInitProcessor> paramsProc_;
+ std::stack<std::string> parsing_file_;
+ std::string file_;
+ std::string path_root_;
+ std::vector<SubscriberManager> subscriber_managers_;
+ std::vector<PublisherManager> publisher_managers_;
+ YAML::Node problem;
+ std::string generatePath(std::string);
+ YAML::Node loadYAML(std::string);
+ void insert_register(std::string, std::string);
+public:
+ ParserYAML(std::string _file, std::string _path_root = "",
+ bool _freely_parse = false);
+ ~ParserYAML()
+ {
+ //
+ }
+ void parse_freely();
+ std::map<std::string, std::string> getParams();
+
+ private:
+ 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);
+/** @Brief Recursively walks the YAML tree while filling a map with the values parsed from the file
+ * @param YAML node to be parsed
+ * @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 walkTreeR(YAML::Node _n, std::vector<std::string>& _tags, std::string _hdr);
+ void updateActiveName(std::string _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 parseFirstLevel(std::string _file);
+ std::string tagsToString(std::vector<std::string>& _tags);
+ void parse();
+};
+}
+#endif
diff --git a/include/core/yaml/parser_yaml.hpp b/include/core/yaml/parser_yaml.hpp
deleted file mode 100644
index 22f584d76bab2aa2c3a6bd6ca30edbeb4873a4ea..0000000000000000000000000000000000000000
--- a/include/core/yaml/parser_yaml.hpp
+++ /dev/null
@@ -1,532 +0,0 @@
-#ifndef PARSER_YAML_HPP
-#define PARSER_YAML_HPP
-
-#include "core/utils/converter.h"
-#include "core/common/wolf.h"
-
-#include "yaml-cpp/yaml.h"
-
-#include <string>
-#include <vector>
-#include <list>
-#include <stack>
-#include <regex>
-#include <map>
-#include <iostream>
-#include <algorithm>
-#include <numeric>
-
-namespace {
- //====== START OF FORWARD DECLARATION ========
- std::string parseAtomicNode(YAML::Node);
- std::string fetchMapEntry(YAML::Node);
- std::string mapToString(std::map<std::string,std::string>);
- //====== END OF FORWARD DECLARATION ========
-
-/** @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> 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>();
- for(const auto& kv : n){
- std::string key = kv.first.as<std::string>();
- switch (kv.second.Type()) {
- case YAML::NodeType::Scalar : {
- std::string value = kv.second.Scalar();
- m.insert(std::pair<std::string,std::string>(key, value));
- break;
- }
- case YAML::NodeType::Sequence : {
- std::string aux = parseAtomicNode(kv.second);
- m.insert(std::pair<std::string,std::string>(key, aux));
- break;
- }
- case YAML::NodeType::Map : {
- std::string value = fetchMapEntry(kv.second);
- std::regex r("^\\$.*");
- if (std::regex_match(key, r)) key = key.substr(1,key.size()-1);
- m.insert(std::pair<std::string,std::string>(key, value));
- break;
- }
- default:
- assert(1 == 0 && "Unsupported node Type at fetchAsMap");
- break;
- }
- }
- return m;
-}
- std::string fetchMapEntry(YAML::Node n){
- switch (n.Type()) {
- case YAML::NodeType::Scalar: {
- return n.Scalar();
- break;
- }
- case YAML::NodeType::Sequence: {
- return parseAtomicNode(n);
- break;
- }
- case YAML::NodeType::Map: {
- return mapToString(fetchAsMap(n));
- break;
- }
- default: {
- assert(1 == 0 && "Unsupported node Type at fetchMapEntry");
- return "";
- break;
- }
- }
- }
- /** @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<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 + ",";
- };
- 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 + "]";
- }
- /** @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 parseAtomicNode(YAML::Node n){
- std::string aux = "";
- bool first = true;
- std::string separator = "";
- switch(n.Type()){
- case YAML::NodeType::Scalar:
- return n.Scalar();
- break;
- case YAML::NodeType::Sequence:
- for(auto it : n){
- aux += separator + parseAtomicNode(it);
- if(first){
- separator = ",";
- first = false;
- }
- }
- return "[" + aux + "]";
- break;
- case YAML::NodeType::Map:
- return mapToString(fetchAsMap(n));
- break;
- default:
- return "";
- break;
- }
- }
-
- /** @Brief checks if a node of the YAML tree is atomic. Only works if the nodes are of type
- * Scalar, Sequence or Map.
- * @param key is the key associated to the node n if n.Type() == YAML::NodeType::Map
- * @param n node to be test for atomicity
- */
- bool isAtomic(std::string key, YAML::Node n){
- assert(n.Type() != YAML::NodeType::Undefined && n.Type() != YAML::NodeType::Null && "Cannot determine atomicity of Undefined/Null node");
- std::regex r("^\\$.*");
- bool is_atomic = true;
- switch(n.Type()){
- case YAML::NodeType::Scalar:
- return true;
- break;
- case YAML::NodeType::Sequence:
- for(auto it : n) {
- switch(it.Type()){
- case YAML::NodeType::Map:
- for(const auto& kv : it){
- is_atomic = is_atomic and isAtomic(kv.first.as<std::string>(), it);
- }
- break;
- default:
- is_atomic = is_atomic and isAtomic("", it);
- break;
- }
- }
- return is_atomic;
- break;
- case YAML::NodeType::Map:
- is_atomic = std::regex_match(key, r);
- return is_atomic;
- break;
- default:
- throw std::runtime_error("Cannot determine atomicity of node type " + std::to_string(n.Type()));
- return false;
- break;
- }
- return false;
- }
-}
-class ParserYAML {
- struct ParamsInitSensor{
- std::string _type;
- std::string _name;
- std::string _plugin;
- YAML::Node n;
- };
- struct ParamsInitProcessor{
- std::string _type;
- std::string _name;
- std::string _name_assoc_sensor;
- std::string _plugin;
- YAML::Node n;
- };
- struct SubscriberManager{
- std::string _package;
- std::string _subscriber;
- std::string _topic;
- std::string _sensor_name;
- YAML::Node n;
- };
- struct PublisherManager{
- std::string _subscriber;
- std::string _topic;
- std::string _period;
- YAML::Node n;
- };
- std::map<std::string, std::string> _params;
- std::string _active_name;
- std::vector<ParamsInitSensor> _paramsSens;
- std::vector<ParamsInitProcessor> _paramsProc;
- std::stack<std::string> _parsing_file;
- std::string _file;
- std::string _path_root;
- std::vector<SubscriberManager> _subscriber_managers;
- std::vector<PublisherManager> _publisher_managers;
- YAML::Node problem;
- std::string generatePath(std::string);
- YAML::Node loadYAML(std::string);
- void insert_register(std::string, std::string);
-public:
- ParserYAML(std::string file, std::string path_root = "",
- bool freely_parse = false) {
- _params = std::map<std::string, std::string>();
- _active_name = "";
- _paramsSens = std::vector<ParamsInitSensor>();
- _paramsProc = std::vector<ParamsInitProcessor>();
- _subscriber_managers = std::vector<SubscriberManager>();
- _publisher_managers = std::vector<PublisherManager>();
- _parsing_file = std::stack<std::string>();
- _file = file;
- if (path_root != "") {
- std::regex r(".*/ *$");
- if (not std::regex_match(path_root, r))
- _path_root = path_root + "/";
- else
- _path_root = path_root;
- }
- if(not freely_parse) this->parse();
- else this->parse_freely();
- }
- ~ParserYAML()
- {
- //
- }
- void parse_freely();
- std::map<std::string, std::string> getParams();
-
- private:
- 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);
- void parse();
-};
-std::string ParserYAML::generatePath(std::string file){
- std::regex r("^/.*");
- if(std::regex_match(file, r)){
- return file;
- }else{
- return _path_root + file;
- }
-}
-YAML::Node ParserYAML::loadYAML(std::string file){
- try{
- // std::cout << "Parsing " << generatePath(file) << std::endl;
- WOLF_INFO("Parsing file: ", generatePath(file));
- return YAML::LoadFile(generatePath(file));
- }catch (YAML::BadFile& e){
- throw std::runtime_error("Couldn't load file " + generatePath(file) + ". Tried to open it from " + this->_parsing_file.top());
- }
-}
-std::string ParserYAML::tagsToString(std::vector<std::string> &tags){
- std::string hdr = "";
- for(auto it : tags){
- hdr = hdr + "/" + it;
- }
- return hdr;
-}
-void ParserYAML::walkTree(std::string file){
- YAML::Node n;
- n = loadYAML(generatePath(file));
- this->_parsing_file.push(generatePath(file));
- std::vector<std::string> hdrs = std::vector<std::string>();
- walkTreeR(n, hdrs, "");
- this->_parsing_file.pop();
-}
-void ParserYAML::walkTree(std::string file, std::vector<std::string>& tags){
- YAML::Node n;
- n = loadYAML(generatePath(file));
- this->_parsing_file.push(generatePath(file));
- walkTreeR(n, tags, "");
- this->_parsing_file.pop();
-}
-void ParserYAML::walkTree(std::string file, std::vector<std::string>& tags, std::string hdr){
- YAML::Node n;
- n = loadYAML(generatePath(file));
- this->_parsing_file.push(generatePath(file));
- walkTreeR(n, tags, hdr);
- this->_parsing_file.pop();
-}
-/** @Brief Recursively walks the YAML tree while filling a map with the values parsed from the file
-* @param YAML node to be parsed
-* @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, std::vector<std::string>& tags, std::string hdr){
- switch (n.Type()) {
- case YAML::NodeType::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{
- insert_register(hdr, n.Scalar());
- }
- break;
- }
- case YAML::NodeType::Sequence : {
- if(isAtomic("", n)){
- insert_register(hdr, parseAtomicNode(n));
- }else{
- for(const auto& kv : n){
- walkTreeR(kv, tags, hdr);
- }
- }
- break;
- }
- case YAML::NodeType::Map : {
- for(const auto& kv : n){
- if(isAtomic(kv.first.as<std::string>(), n)){
- std::string key = kv.first.as<std::string>();
- //WOLF_DEBUG("KEY IN MAP ATOMIC ", hdr + "/" + key);
- key = key.substr(1,key.size() - 1);
- insert_register(hdr + "/" + key, parseAtomicNode(kv.second));
- }else{
- /*
- 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.
- The following example shows how the header remains unafected:
- @my_main_config | @some_path
- - cov_det: 1 | - my_value : 23
- - follow: "@some_path" |
- - var: 1.2 |
- Resulting map:
- cov_det -> 1
- my_value-> 23
- var: 1.2
- Instead of:
- cov_det -> 1
- follow/my_value-> 23
- var: 1.2
- Which would result from the following yaml files
- @my_main_config | @some_path
- - cov_det: 1 | - my_value : 23
- - $follow: "@some_path" |
- - var: 1.2 |
- */
- std::string key = kv.first.as<std::string>();
- //WOLF_DEBUG("KEY IN MAP NON ATOMIC ", key);
- 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<std::string>(), tags, hdr);
- }
- }
- }
- break;
- }
- case YAML::NodeType::Null :
- break;
- default:
- assert(1 == 0 && "Unsupported node Type at walkTreeR.");
- break;
- }
-}
-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(std::string file){
- YAML::Node n;
- n = loadYAML(generatePath(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");
- if(n_config.Type() != YAML::NodeType::Map) throw std::runtime_error("Could not find config node. Please make sure that your YAML file " + generatePath(file) + " starts with 'config:'");
- if(n_config["problem"].Type() != YAML::NodeType::Map) throw std::runtime_error("Could not find problem node. Please make sure that the 'config' node in YAML file " + generatePath(file) + " has a 'problem' entry");
- this->problem = n_config["problem"];
- std::vector<std::map<std::string, std::string>> map_container;
- try{
- for(const auto& kv : n_config["sensors"]){
- ParamsInitSensor pSensor = {kv["type"].Scalar(), kv["name"].Scalar(), kv["plugin"].Scalar(), kv};
- _paramsSens.push_back(pSensor);
- map_container.push_back(std::map<std::string, std::string>({
- {"type", kv["type"].Scalar()},
- {"name", kv["name"].Scalar()},
- {"plugin", kv["plugin"].Scalar()}
- }));
- }
- insert_register("sensors", wolf::converter<std::string>::convert(map_container));
- map_container.clear();
- } catch(YAML::InvalidNode& e){
- throw std::runtime_error("Error parsing sensors @" + generatePath(file) + ". Please make sure that each sensor entry has 'type', 'name' and 'plugin' entries.");
- }
-
- try{
- for(const auto& kv : n_config["processors"]){
- ParamsInitProcessor pProc = {kv["type"].Scalar(), kv["name"].Scalar(), kv["sensor_name"].Scalar(), kv["plugin"].Scalar(), kv};
- _paramsProc.push_back(pProc);
- map_container.push_back(std::map<std::string, std::string>({
- {"type", kv["type"].Scalar()},
- {"name", kv["name"].Scalar()},
- {"plugin", kv["plugin"].Scalar()},
- {"sensor_name", kv["sensor_name"].Scalar()}}));
- }
- insert_register("processors",
- wolf::converter<std::string>::convert(map_container));
- map_container.clear();
- } catch(YAML::InvalidNode& e){
- throw std::runtime_error("Error parsing processors @" + generatePath(file) + ". Please make sure that each processor has 'type', 'name', 'plugin' and 'sensor_name' entries.");
- }
- try {
- for (const auto &kv : n_config["ROS subscriber"]) {
- SubscriberManager pSubscriberManager = {kv["package"].Scalar(), kv["type"].Scalar(), kv["topic"].Scalar(), kv["sensor_name"].Scalar(), kv};
- _subscriber_managers.push_back(pSubscriberManager);
- map_container.push_back(std::map<std::string, std::string>({
- {"package", kv["package"].Scalar()},
- {"type", kv["type"].Scalar()},
- {"topic", kv["topic"].Scalar()},
- {"sensor_name", kv["sensor_name"].Scalar()}}));
- }
- insert_register("ROS subscriber", wolf::converter<std::string>::convert(map_container));
- map_container.clear();
- } catch (YAML::InvalidNode &e) {
- throw std::runtime_error("Error parsing subscriber @" + generatePath(file) + ". Please make sure that each manager has 'package', 'type', 'topic' and 'sensor_name' entries.");
- }
-
- try {
- for (const auto &kv : n_config["ROS publisher"]) {
- WOLF_DEBUG("WHAT");
- PublisherManager pPublisherManager = {kv["type"].Scalar(), kv["topic"].Scalar(), kv["period"].Scalar(), kv};
- _publisher_managers.push_back(pPublisherManager);
- map_container.push_back(std::map<std::string, std::string>({
- {"type", kv["type"].Scalar()},
- {"topic", kv["topic"].Scalar()},
- {"period", kv["period"].Scalar()}}));
- }
- insert_register("ROS publisher", wolf::converter<std::string>::convert(map_container));
- map_container.clear();
- } catch (YAML::InvalidNode &e) {
- throw std::runtime_error("Error parsing publisher @" + generatePath(file) + ". Please make sure that each manager has 'type', 'topic' and 'period' entries.");
- }
-}
-std::map<std::string,std::string> ParserYAML::getParams(){
- std::map<std::string,std::string> rtn = _params;
- return rtn;
-}
-void ParserYAML::parse(){
- this->_parsing_file.push(generatePath(this->_file));
- this->parseFirstLevel(this->_file);
-
- if(this->problem.Type() != YAML::NodeType::Undefined){
- std::vector<std::string> tags = std::vector<std::string>();
- this->walkTreeR(this->problem, tags , "problem");
- }
- for(auto it : _paramsSens){
- std::vector<std::string> tags = std::vector<std::string>();
- tags.push_back("sensor");
- this->walkTreeR(it.n , tags , "sensor/" + it._name);
- }
- for(auto it : _paramsProc){
- std::vector<std::string> tags = std::vector<std::string>();
- tags.push_back("processor");
- this->walkTreeR(it.n , tags , "processor/" + it._name);
- }
- std::list<std::string> plugins, packages;
- for (const auto& it : this->_paramsSens)
- plugins.push_back(it._plugin);
- for (const auto& it : this->_paramsProc)
- plugins.push_back(it._plugin);
- for (const auto& it : this->_subscriber_managers)
- packages.push_back(it._package);
- plugins.sort();
- plugins.unique();
- packages.sort();
- packages.unique();
- insert_register("plugins", wolf::converter<std::string>::convert(plugins));
- insert_register("packages", wolf::converter<std::string>::convert(packages));
-
- YAML::Node n;
- n = loadYAML(generatePath(this->_file));
-
- if (n.Type() == YAML::NodeType::Map)
- {
- for (auto it : n)
- {
- auto node = it.second;
- // WOLF_INFO("WUT ", it.first);
- std::vector<std::string> tags = std::vector<std::string>();
- if(it.first.as<std::string>() != "config")
- this->walkTreeR(node, tags, it.first.as<std::string>());
- else
- {
- for (auto itt : node)
- {
- std::string node_key = itt.first.as<std::string>();
- if (node_key != "problem" and node_key != "sensors" and node_key != "processors" and
- node_key != "ROS subscriber" and node_key != "ROS publisher")
- {
- this->walkTreeR(itt.second, tags, node_key);
- }
- }
- }
- }
- }else
- {
- std::vector<std::string> tags = std::vector<std::string>();
- this->walkTreeR(n, tags, "");
- }
- this->_parsing_file.pop();
- }
-void ParserYAML::parse_freely(){
- this->_parsing_file.push(generatePath(this->_file));
- std::vector<std::string> tags = std::vector<std::string>();
- this->walkTreeR(loadYAML(this->_file), tags, "");
- this->_parsing_file.pop();
-}
-void ParserYAML::insert_register(std::string key, std::string value){
- if(key.substr(0,1) == "/") key = key.substr(1,key.size()-1);
- auto inserted_it = _params.insert(std::pair<std::string, std::string>(key, value));
- if(not inserted_it.second) WOLF_WARN("Skipping key '", key, "' with value '", value, "'. There already exists the register: (", inserted_it.first->first, ",", inserted_it.first->second, ")");
-}
-#endif
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index b81f9c8752bb047ae702a092c8f20458a29f3153..e79de9572f545e85142450497f3d4780621adc7b 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -12,8 +12,8 @@
#include "core/processor/processor_factory.h"
#include "core/state_block/state_block.h"
#include "core/utils/logging.h"
-#include "core/utils/params_server.hpp"
-#include "core/utils/loader.hpp"
+#include "core/utils/params_server.h"
+#include "core/utils/loader.h"
#include "core/utils/check_log.hpp"
@@ -1877,9 +1877,12 @@ void Problem::perturb(double amplitude)
// Frames and Captures
for (auto& F : getTrajectory()->getFrameList())
{
- F->perturb(amplitude);
- for (auto& C : F->getCaptureList())
- C->perturb(amplitude);
+ if (F->isKeyOrAux())
+ {
+ F->perturb(amplitude);
+ for (auto& C : F->getCaptureList())
+ C->perturb(amplitude);
+ }
}
// Landmarks
diff --git a/src/utils/loader.cpp b/src/utils/loader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9fdf6984b6a3de57fa8a8ae062232a685897209f
--- /dev/null
+++ b/src/utils/loader.cpp
@@ -0,0 +1,37 @@
+#include "core/utils/loader.h"
+
+#include <dlfcn.h>
+#include <stdexcept>
+
+Loader::Loader(std::string _file)
+{
+ path_ = _file;
+}
+LoaderRaw::LoaderRaw(std::string _file) : Loader(_file)
+{
+ //
+}
+LoaderRaw::~LoaderRaw()
+{
+ close();
+}
+void LoaderRaw::load()
+{
+ resource_ = dlopen(path_.c_str(), RTLD_LAZY);
+ if (not resource_)
+ throw std::runtime_error("Couldn't load resource with path " + path_ + "\n" + "Error info: " + dlerror());
+}
+void LoaderRaw::close()
+{
+ if (resource_)
+ dlclose(resource_);
+}
+// class LoaderPlugin: public Loader{
+// ClassLoader* resource_;
+// void load(){
+// resource_ = new ClassLoader(path_);
+// }
+// void close(){
+// delete resource_;
+// }
+// };
diff --git a/src/utils/params_server.cpp b/src/utils/params_server.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6344466792706c9e499695721f95e781089ec07e
--- /dev/null
+++ b/src/utils/params_server.cpp
@@ -0,0 +1,28 @@
+#include "core/utils/params_server.h"
+
+using namespace wolf;
+
+ParamsServer::ParamsServer()
+{
+ params_ = std::map<std::string, std::string>();
+}
+ParamsServer::ParamsServer(std::map<std::string, std::string> _params)
+{
+ params_ = _params;
+}
+
+void ParamsServer::print()
+{
+ for (auto it : params_)
+ std::cout << it.first << "~~" << it.second << std::endl;
+}
+
+void ParamsServer::addParam(std::string _key, std::string _value)
+{
+ params_.insert(std::pair<std::string, std::string>(_key, _value));
+}
+
+void ParamsServer::addParams(std::map<std::string, std::string> _params)
+{
+ params_.insert(_params.begin(), _params.end());
+}
diff --git a/src/yaml/parser_yaml.cpp b/src/yaml/parser_yaml.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..267d0c9da2024abd59ed5b529d13de7680d14472
--- /dev/null
+++ b/src/yaml/parser_yaml.cpp
@@ -0,0 +1,547 @@
+#include "core/yaml/parser_yaml.h"
+
+#include <string>
+#include <vector>
+#include <list>
+#include <stack>
+#include <regex>
+#include <map>
+#include <iostream>
+#include <algorithm>
+#include <numeric>
+
+using namespace wolf;
+namespace {
+ //====== START OF FORWARD DECLARATION ========
+ std::string parseAtomicNode(YAML::Node);
+ std::string fetchMapEntry(YAML::Node);
+ std::string mapToString(std::map<std::string,std::string>);
+ //====== END OF FORWARD DECLARATION ========
+
+/** @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> 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>();
+ for(const auto& kv : _n){
+ std::string key = kv.first.as<std::string>();
+ switch (kv.second.Type()) {
+ case YAML::NodeType::Scalar : {
+ std::string value = kv.second.Scalar();
+ m.insert(std::pair<std::string,std::string>(key, value));
+ break;
+ }
+ case YAML::NodeType::Sequence : {
+ std::string aux = parseAtomicNode(kv.second);
+ m.insert(std::pair<std::string,std::string>(key, aux));
+ break;
+ }
+ case YAML::NodeType::Map : {
+ std::string value = fetchMapEntry(kv.second);
+ std::regex r("^\\$.*");
+ if (std::regex_match(key, r)) key = key.substr(1,key.size()-1);
+ m.insert(std::pair<std::string,std::string>(key, value));
+ break;
+ }
+ default:
+ assert(1 == 0 && "Unsupported node Type at fetchAsMap");
+ break;
+ }
+ }
+ return m;
+}
+ std::string fetchMapEntry(YAML::Node _n){
+ switch (_n.Type()) {
+ case YAML::NodeType::Scalar: {
+ return _n.Scalar();
+ break;
+ }
+ case YAML::NodeType::Sequence: {
+ return parseAtomicNode(_n);
+ break;
+ }
+ case YAML::NodeType::Map: {
+ return mapToString(fetchAsMap(_n));
+ break;
+ }
+ default: {
+ assert(1 == 0 && "Unsupported node Type at fetchMapEntry");
+ return "";
+ break;
+ }
+ }
+ }
+ /** @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<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 + ",";
+ };
+ 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 + "]";
+ }
+ /** @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 parseAtomicNode(YAML::Node _n){
+ std::string aux = "";
+ bool first = true;
+ std::string separator = "";
+ switch(_n.Type()){
+ case YAML::NodeType::Scalar:
+ return _n.Scalar();
+ break;
+ case YAML::NodeType::Sequence:
+ for(auto it : _n){
+ aux += separator + parseAtomicNode(it);
+ if(first){
+ separator = ",";
+ first = false;
+ }
+ }
+ return "[" + aux + "]";
+ break;
+ case YAML::NodeType::Map:
+ return mapToString(fetchAsMap(_n));
+ break;
+ default:
+ return "";
+ break;
+ }
+ }
+
+ /** @Brief checks if a node of the YAML tree is atomic. Only works if the nodes are of type
+ * Scalar, Sequence or Map.
+ * @param key is the key associated to the node n if n.Type() == YAML::NodeType::Map
+ * @param n node to be test for atomicity
+ */
+ bool isAtomic(std::string _key, YAML::Node _n){
+ assert(_n.Type() != YAML::NodeType::Undefined && _n.Type() != YAML::NodeType::Null && "Cannot determine atomicity of Undefined/Null node");
+ std::regex r("^\\$.*");
+ bool is_atomic = true;
+ switch(_n.Type()){
+ case YAML::NodeType::Scalar:
+ return true;
+ break;
+ case YAML::NodeType::Sequence:
+ for(auto it : _n) {
+ switch(it.Type()){
+ case YAML::NodeType::Map:
+ for(const auto& kv : it){
+ is_atomic = is_atomic and isAtomic(kv.first.as<std::string>(), it);
+ }
+ break;
+ default:
+ is_atomic = is_atomic and isAtomic("", it);
+ break;
+ }
+ }
+ return is_atomic;
+ break;
+ case YAML::NodeType::Map:
+ is_atomic = std::regex_match(_key, r);
+ return is_atomic;
+ break;
+ default:
+ throw std::runtime_error("Cannot determine atomicity of node type " + std::to_string(_n.Type()));
+ return false;
+ break;
+ }
+ return false;
+ }
+}
+ParserYAML::ParserYAML(std::string _file, std::string path_root, bool freely_parse)
+{
+ params_ = std::map<std::string, std::string>();
+ active_name_ = "";
+ paramsSens_ = std::vector<ParamsInitSensor>();
+ paramsProc_ = std::vector<ParamsInitProcessor>();
+ subscriber_managers_ = std::vector<SubscriberManager>();
+ publisher_managers_ = std::vector<PublisherManager>();
+ parsing_file_ = std::stack<std::string>();
+ file_ = _file;
+ if (path_root != "")
+ {
+ std::regex r(".*/ *$");
+ if (not std::regex_match(path_root, r))
+ path_root_ = path_root + "/";
+ else
+ path_root_ = path_root;
+ }
+ if (not freely_parse)
+ parse();
+ else
+ parse_freely();
+}
+
+std::string ParserYAML::generatePath(std::string _file)
+{
+ std::regex r("^/.*");
+ if (std::regex_match(_file, r))
+ {
+ return _file;
+ }
+ else
+ {
+ return path_root_ + _file;
+ }
+}
+YAML::Node ParserYAML::loadYAML(std::string _file)
+{
+ try
+ {
+ // std::cout << "Parsing " << generatePath(_file) << std::endl;
+ WOLF_INFO("Parsing file: ", generatePath(_file));
+ return YAML::LoadFile(generatePath(_file));
+ }
+ catch (YAML::BadFile& e)
+ {
+ throw std::runtime_error("Couldn't load file " + generatePath(_file) + ". Tried to open it from " +
+ parsing_file_.top());
+ }
+}
+std::string ParserYAML::tagsToString(std::vector<std::string>& _tags)
+{
+ std::string hdr = "";
+ for (auto it : _tags)
+ {
+ hdr = hdr + "/" + it;
+ }
+ return hdr;
+}
+void ParserYAML::walkTree(std::string _file)
+{
+ YAML::Node n;
+ n = loadYAML(generatePath(_file));
+ parsing_file_.push(generatePath(_file));
+ std::vector<std::string> hdrs = std::vector<std::string>();
+ walkTreeR(n, hdrs, "");
+ parsing_file_.pop();
+}
+void ParserYAML::walkTree(std::string _file, std::vector<std::string>& _tags)
+{
+ YAML::Node n;
+ n = loadYAML(generatePath(_file));
+ parsing_file_.push(generatePath(_file));
+ walkTreeR(n, _tags, "");
+ parsing_file_.pop();
+}
+void ParserYAML::walkTree(std::string _file, std::vector<std::string>& _tags, std::string hdr)
+{
+ YAML::Node n;
+ n = loadYAML(generatePath(_file));
+ parsing_file_.push(generatePath(_file));
+ walkTreeR(n, _tags, hdr);
+ parsing_file_.pop();
+}
+void ParserYAML::walkTreeR(YAML::Node _n, std::vector<std::string>& _tags, std::string hdr)
+{
+ switch (_n.Type())
+ {
+ case YAML::NodeType::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
+ {
+ insert_register(hdr, _n.Scalar());
+ }
+ break;
+ }
+ case YAML::NodeType::Sequence:
+ {
+ if (isAtomic("", _n))
+ {
+ insert_register(hdr, parseAtomicNode(_n));
+ }
+ else
+ {
+ for (const auto& kv : _n)
+ {
+ walkTreeR(kv, _tags, hdr);
+ }
+ }
+ break;
+ }
+ case YAML::NodeType::Map:
+ {
+ for (const auto& kv : _n)
+ {
+ if (isAtomic(kv.first.as<std::string>(), _n))
+ {
+ std::string key = kv.first.as<std::string>();
+ // WOLF_DEBUG("KEY IN MAP ATOMIC ", hdr + "/" + key);
+ key = key.substr(1, key.size() - 1);
+ insert_register(hdr + "/" + key, parseAtomicNode(kv.second));
+ }
+ else
+ {
+ /*
+ 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.
+ The following example shows how the header remains unafected:
+ @my_main_config | @some_path
+ - cov_det: 1 | - my_value : 23
+ - follow: "@some_path" |
+ - var: 1.2 |
+ Resulting map:
+ cov_det -> 1
+ my_value-> 23
+ var: 1.2
+ Instead of:
+ cov_det -> 1
+ follow/my_value-> 23
+ var: 1.2
+ Which would result from the following yaml files
+ @my_main_config | @some_path
+ - cov_det: 1 | - my_value : 23
+ - $follow: "@some_path" |
+ - var: 1.2 |
+ */
+ std::string key = kv.first.as<std::string>();
+ // WOLF_DEBUG("KEY IN MAP NON ATOMIC ", key);
+ 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)
+ updateActiveName(kv.first.as<std::string>());
+ walkTreeR(kv.second, _tags, hdr + "/" + kv.first.as<std::string>());
+ _tags.pop_back();
+ if (_tags.size() == 1)
+ updateActiveName("");
+ }
+ else
+ {
+ walkTree(kv.second.as<std::string>(), _tags, hdr);
+ }
+ }
+ }
+ break;
+ }
+ case YAML::NodeType::Null:
+ break;
+ default:
+ assert(1 == 0 && "Unsupported node Type at walkTreeR.");
+ break;
+ }
+}
+void ParserYAML::updateActiveName(std::string _tag)
+{
+ active_name_ = _tag;
+}
+void ParserYAML::parseFirstLevel(std::string _file)
+{
+ YAML::Node n;
+ n = loadYAML(generatePath(_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");
+ if (n_config.Type() != YAML::NodeType::Map)
+ throw std::runtime_error("Could not find config node. Please make sure that your YAML file " +
+ generatePath(_file) + " starts with 'config:'");
+ if (n_config["problem"].Type() != YAML::NodeType::Map)
+ throw std::runtime_error("Could not find problem node. Please make sure that the 'config' node in YAML file " +
+ generatePath(_file) + " has a 'problem' entry");
+ problem = n_config["problem"];
+ std::vector<std::map<std::string, std::string>> map_container;
+ try
+ {
+ for (const auto& kv : n_config["sensors"])
+ {
+ ParamsInitSensor pSensor = { kv["type"].Scalar(), kv["name"].Scalar(), kv["plugin"].Scalar(), kv };
+ paramsSens_.push_back(pSensor);
+ map_container.push_back(std::map<std::string, std::string>({ { "type", kv["type"].Scalar() },
+ { "name", kv["name"].Scalar() },
+ { "plugin", kv["plugin"].Scalar() } }));
+ }
+ insert_register("sensors", wolf::converter<std::string>::convert(map_container));
+ map_container.clear();
+ }
+ catch (YAML::InvalidNode& e)
+ {
+ throw std::runtime_error("Error parsing sensors @" + generatePath(_file) +
+ ". Please make sure that each sensor entry has 'type', 'name' and 'plugin' entries.");
+ }
+
+ try
+ {
+ for (const auto& kv : n_config["processors"])
+ {
+ ParamsInitProcessor pProc = {
+ kv["type"].Scalar(), kv["name"].Scalar(), kv["sensor_name"].Scalar(), kv["plugin"].Scalar(), kv
+ };
+ paramsProc_.push_back(pProc);
+ map_container.push_back(
+ std::map<std::string, std::string>({ { "type", kv["type"].Scalar() },
+ { "name", kv["name"].Scalar() },
+ { "plugin", kv["plugin"].Scalar() },
+ { "sensor_name", kv["sensor_name"].Scalar() } }));
+ }
+ insert_register("processors", wolf::converter<std::string>::convert(map_container));
+ map_container.clear();
+ }
+ catch (YAML::InvalidNode& e)
+ {
+ throw std::runtime_error("Error parsing processors @" + generatePath(_file) +
+ ". Please make sure that each processor has 'type', 'name', 'plugin' and "
+ "'sensor_name' entries.");
+ }
+ try
+ {
+ for (const auto& kv : n_config["ROS subscriber"])
+ {
+ SubscriberManager pSubscriberManager = {
+ kv["package"].Scalar(), kv["type"].Scalar(), kv["topic"].Scalar(), kv["sensor_name"].Scalar(), kv
+ };
+ subscriber_managers_.push_back(pSubscriberManager);
+ map_container.push_back(
+ std::map<std::string, std::string>({ { "package", kv["package"].Scalar() },
+ { "type", kv["type"].Scalar() },
+ { "topic", kv["topic"].Scalar() },
+ { "sensor_name", kv["sensor_name"].Scalar() } }));
+ }
+ insert_register("ROS subscriber", wolf::converter<std::string>::convert(map_container));
+ map_container.clear();
+ }
+ catch (YAML::InvalidNode& e)
+ {
+ throw std::runtime_error("Error parsing subscriber @" + generatePath(_file) +
+ ". Please make sure that each manager has 'package', 'type', 'topic' and "
+ "'sensor_name' entries.");
+ }
+
+ try
+ {
+ for (const auto& kv : n_config["ROS publisher"])
+ {
+ WOLF_DEBUG("WHAT");
+ PublisherManager pPublisherManager = {
+ kv["type"].Scalar(), kv["topic"].Scalar(), kv["period"].Scalar(), kv
+ };
+ publisher_managers_.push_back(pPublisherManager);
+ map_container.push_back(std::map<std::string, std::string>({ { "type", kv["type"].Scalar() },
+ { "topic", kv["topic"].Scalar() },
+ { "period", kv["period"].Scalar() } }));
+ }
+ insert_register("ROS publisher", wolf::converter<std::string>::convert(map_container));
+ map_container.clear();
+ }
+ catch (YAML::InvalidNode& e)
+ {
+ throw std::runtime_error("Error parsing publisher @" + generatePath(_file) +
+ ". Please make sure that each manager has 'type', 'topic' and 'period' entries.");
+ }
+}
+std::map<std::string, std::string> ParserYAML::getParams()
+{
+ std::map<std::string, std::string> rtn = params_;
+ return rtn;
+}
+void ParserYAML::parse()
+{
+ parsing_file_.push(generatePath(file_));
+ parseFirstLevel(file_);
+
+ if (problem.Type() != YAML::NodeType::Undefined)
+ {
+ std::vector<std::string> tags = std::vector<std::string>();
+ walkTreeR(problem, tags, "problem");
+ }
+ for (auto it : paramsSens_)
+ {
+ std::vector<std::string> tags = std::vector<std::string>();
+ tags.push_back("sensor");
+ walkTreeR(it.n_, tags, "sensor/" + it.name_);
+ }
+ for (auto it : paramsProc_)
+ {
+ std::vector<std::string> tags = std::vector<std::string>();
+ tags.push_back("processor");
+ walkTreeR(it.n_, tags, "processor/" + it.name_);
+ }
+ std::list<std::string> plugins, packages;
+ for (const auto& it : paramsSens_)
+ plugins.push_back(it.plugin_);
+ for (const auto& it : paramsProc_)
+ plugins.push_back(it.plugin_);
+ for (const auto& it : subscriber_managers_)
+ packages.push_back(it.package_);
+ plugins.sort();
+ plugins.unique();
+ packages.sort();
+ packages.unique();
+ insert_register("plugins", wolf::converter<std::string>::convert(plugins));
+ insert_register("packages", wolf::converter<std::string>::convert(packages));
+
+ YAML::Node n;
+ n = loadYAML(generatePath(file_));
+
+ if (n.Type() == YAML::NodeType::Map)
+ {
+ for (auto it : n)
+ {
+ auto node = it.second;
+ // WOLF_INFO("WUT ", it.first);
+ std::vector<std::string> tags = std::vector<std::string>();
+ if (it.first.as<std::string>() != "config")
+ walkTreeR(node, tags, it.first.as<std::string>());
+ else
+ {
+ for (auto itt : node)
+ {
+ std::string node_key = itt.first.as<std::string>();
+ if (node_key != "problem" and node_key != "sensors" and node_key != "processors" and
+ node_key != "ROS subscriber" and node_key != "ROS publisher")
+ {
+ walkTreeR(itt.second, tags, node_key);
+ }
+ }
+ }
+ }
+ }
+ else
+ {
+ std::vector<std::string> tags = std::vector<std::string>();
+ walkTreeR(n, tags, "");
+ }
+ parsing_file_.pop();
+}
+void ParserYAML::parse_freely()
+{
+ parsing_file_.push(generatePath(file_));
+ std::vector<std::string> tags = std::vector<std::string>();
+ walkTreeR(loadYAML(file_), tags, "");
+ parsing_file_.pop();
+}
+void ParserYAML::insert_register(std::string _key, std::string _value)
+{
+ if (_key.substr(0, 1) == "/")
+ _key = _key.substr(1, _key.size() - 1);
+ auto inserted_it = params_.insert(std::pair<std::string, std::string>(_key, _value));
+ if (not inserted_it.second)
+ WOLF_WARN("Skipping key '",
+ _key,
+ "' with value '",
+ _value,
+ "'. There already exists the register: (",
+ inserted_it.first->first,
+ ",",
+ inserted_it.first->second,
+ ")");
+}
diff --git a/test/gtest_param_server.cpp b/test/gtest_param_server.cpp
index 18f6b1eded0955faf7c6b823e25bbde62894203d..1f1f468360e1aa5073606f8ea32e7d82cafbc438 100644
--- a/test/gtest_param_server.cpp
+++ b/test/gtest_param_server.cpp
@@ -1,8 +1,8 @@
#include "core/utils/utils_gtest.h"
#include "core/utils/converter.h"
#include "core/common/wolf.h"
-#include "core/yaml/parser_yaml.hpp"
-#include "core/utils/params_server.hpp"
+#include "core/yaml/parser_yaml.h"
+#include "core/utils/params_server.h"
using namespace std;
using namespace wolf;
diff --git a/test/gtest_parser_yaml.cpp b/test/gtest_parser_yaml.cpp
index c8702619bc5150eb688bcb7dccfa11f71ab909e5..de5164ec6a9db61ad8a0344e8e6ee6d6e52dec16 100644
--- a/test/gtest_parser_yaml.cpp
+++ b/test/gtest_parser_yaml.cpp
@@ -1,7 +1,7 @@
#include "core/utils/utils_gtest.h"
#include "core/utils/converter.h"
#include "core/common/wolf.h"
-#include "core/yaml/parser_yaml.hpp"
+#include "core/yaml/parser_yaml.h"
using namespace std;
using namespace wolf;