diff --git a/hello_wolf/processor_range_bearing.h b/hello_wolf/processor_range_bearing.h index be9159c30dcb72cdeb54872db4ef64f4e35fa3de..6cfdd7d8943f13b4fb757c071582d99e9b6c3ea5 100644 --- a/hello_wolf/processor_range_bearing.h +++ b/hello_wolf/processor_range_bearing.h @@ -22,19 +22,19 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsRangeBearing); struct ProcessorParamsRangeBearing : public ProcessorParamsBase { // We do not need special parameters, but in case you need they should be defined here. - ProcessorParamsRangeBearing() - { - //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE. - } - ProcessorParamsRangeBearing(std::string _unique_name, const paramsServer& _server): - ProcessorParamsBase(_unique_name, _server) - { - // - } - std::string print() - { - return "\n" + ProcessorParamsBase::print(); - } + ProcessorParamsRangeBearing() + { + //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE. + } + ProcessorParamsRangeBearing(std::string _unique_name, const paramsServer& _server) : + ProcessorParamsBase(_unique_name, _server) + { + // + } + std::string print() + { + return "\n" + ProcessorParamsBase::print(); + } }; using namespace Eigen; diff --git a/include/core/processor/processor_odom_2D.h b/include/core/processor/processor_odom_2D.h index 37d6c9ee11c6f9c518ccbcdeb01bc9fa3f2c20a5..8e6c9ee6b2a8debaa727ef8128c47e8c20b2d3c4 100644 --- a/include/core/processor/processor_odom_2D.h +++ b/include/core/processor/processor_odom_2D.h @@ -21,18 +21,19 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom2D); struct ProcessorParamsOdom2D : public ProcessorParamsMotion { - Scalar cov_det = 1.0; // 1 rad^2 - ProcessorParamsOdom2D() = default; - ProcessorParamsOdom2D(std::string _unique_name, const wolf::paramsServer & _server): - ProcessorParamsMotion(_unique_name, _server) - { - cov_det = _server.getParam<Scalar>(_unique_name + "/cov_det", "1.0"); - } - std::string print() - { - return "\n" + ProcessorParamsMotion::print() + "cov_det: " + std::to_string(cov_det) + "\n"; - } + Scalar cov_det = 1.0; // 1 rad^2 + ProcessorParamsOdom2D() = default; + ProcessorParamsOdom2D(std::string _unique_name, const wolf::paramsServer & _server) : + ProcessorParamsMotion(_unique_name, _server) + { + cov_det = _server.getParam<Scalar>(_unique_name + "/cov_det", "1.0"); + } + std::string print() + { + return "\n" + ProcessorParamsMotion::print() + "cov_det: " + std::to_string(cov_det) + "\n"; + } }; + class ProcessorOdom2D : public ProcessorMotion { public: diff --git a/include/core/utils/params_server.hpp b/include/core/utils/params_server.hpp index 5c2eee90c8802dd3a12acc8f53594fb7a0d8001e..4a58b290b31d38b29cea102431e2335c2597f1ee 100644 --- a/include/core/utils/params_server.hpp +++ b/include/core/utils/params_server.hpp @@ -2,30 +2,38 @@ #define PARAMS_SERVER_HPP #include "core/utils/converter.h" +//#include "core/yaml/parser_yaml.hpp" + #include <vector> #include <regex> #include <map> namespace wolf{ + class paramsServer{ struct ParamsInitSensor{ std::string _type; std::string _name; }; + struct ParamsInitProcessor{ std::string _type; std::string _name; std::string _name_assoc_sensor; }; + std::map<std::string, std::string> _params; std::map<std::string,ParamsInitSensor> _paramsSens; std::map<std::string,ParamsInitProcessor> _paramsProc; -public: + + public: + paramsServer(){ _params = std::map<std::string, std::string>(); _paramsSens = std::map<std::string,ParamsInitSensor>(); _paramsProc = std::map<std::string,ParamsInitProcessor>(); } + paramsServer(std::map<std::string, std::string> params, std::vector<std::array<std::string,2>> sensors, std::vector<std::array<std::string,3>> procs){ @@ -41,24 +49,37 @@ public: _paramsProc.insert(std::pair<std::string,ParamsInitProcessor>(it.at(1), pProcs)); } } + +// paramsServer(std::string _file) +// { +// parserYAML parser = parserYAML(_file); +// parser.parse(); +// paramsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization()); +// } + ~paramsServer(){ // } + void print(){ for(auto it : _params) std::cout << it.first << "~~" << it.second << std::endl; } + void addInitParamsSensor(std::string type, std::string name){ ParamsInitSensor params = {type, name}; _paramsSens.insert(std::pair<std::string, ParamsInitSensor>(type + "/" + name + "/", params)); } + void addInitParamsProcessor(std::string type, std::string name, std::string name_assoc_sensor){ ParamsInitProcessor params = {type, name, name_assoc_sensor}; _paramsProc.insert(std::pair<std::string, ParamsInitProcessor>(type + "/" + name + "/", params)); } + void addParam(std::string key, std::string value){ _params.insert(std::pair<std::string, std::string>(key, value)); } + template<typename T> T getParam(std::string key, std::string def_value) const { if(_params.find(key) != _params.end()){ @@ -67,6 +88,7 @@ public: return converter<T>::convert(def_value); } } + template<typename T> T getParam(std::string key) const { if(_params.find(key) != _params.end()){ @@ -75,16 +97,20 @@ public: throw std::runtime_error("The following key: '" + key + "' has not been found in the parameters server and no default value was provided."); } } + std::vector<ParamsInitSensor> getSensors(){ std::vector<ParamsInitSensor> rtn = std::vector<ParamsInitSensor>(); std::transform(this->_paramsSens.begin(), this->_paramsSens.end(), back_inserter(rtn), [](const std::pair<std::string,ParamsInitSensor> v){return v.second;}); return rtn; } + std::vector<ParamsInitProcessor> getProcessors(){ std::vector<ParamsInitProcessor> rtn = std::vector<ParamsInitProcessor>(); std::transform(this->_paramsProc.begin(), this->_paramsProc.end(), back_inserter(rtn), [](const std::pair<std::string,ParamsInitProcessor> v){return v.second;}); return rtn; } }; + } -#endif \ No newline at end of file + +#endif