diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index ca7da8dbe679b5bf5f0b3ca8518a6cd045e0aafa..68c1d028f5a4821accd5cef45d0036660522482c 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -283,7 +283,14 @@ inline bool file_exists(const std::string& _name)
inline const Eigen::Vector3d gravity(void) {
return Eigen::Vector3d(0,0,-9.806);
}
+template<class A, class B>
+struct Wpair : std::pair<A,B>
+{
+ Wpair(A first, B second): std::pair<A,B>(first, second)
+ {
+ }
+};
} // namespace wolf
#endif /* WOLF_H_ */
diff --git a/include/core/utils/converter.h b/include/core/utils/converter.h
index a1b4a5a86ee2082b6f44bd3eb8d37e0c3a5c88d3..2f8a3ec191935570ebe37b4179fd5bcf720dcdaa 100644
--- a/include/core/utils/converter.h
+++ b/include/core/utils/converter.h
@@ -4,6 +4,7 @@
// wolf
#include "core/common/wolf.h"
#include "core/utils/converter_utils.h"
+#include "core/state_block/state_composite.h"
// Eigen
#include <eigen3/Eigen/Dense>
@@ -22,6 +23,7 @@
*/
namespace wolf{
+ //// CONVERTERS ~~~~ STRING ----> TYPE
template<typename T>
struct converter{
static T convert(std::string val){
@@ -120,6 +122,11 @@ struct converter<std::string>{
static std::string convert(std::pair<A,B> val){
return "{" + converter<std::string>::convert(val.first) + ":" + converter<std::string>::convert(val.second) + "}";
}
+ //NEW GUY
+ template<typename A, typename B>
+ static std::string convert(Wpair<A,B> val){
+ return "(" + converter<std::string>::convert(val.first) + "," + converter<std::string>::convert(val.second) + ")";
+ }
template<typename A, typename B>
static std::string convert(std::map<A,B> val){
std::string result = "";
@@ -129,6 +136,15 @@ struct converter<std::string>{
if(result.size() > 0) result = result.substr(1,result.size());
return "[" + result + "]";
}
+ template<typename A, typename B>
+ static std::string convert(std::unordered_map<A,B> val){
+ std::string result = "";
+ for(auto it : val){
+ result += "," + converter<std::string>::convert(it);
+ }
+ if(result.size() > 0) result = result.substr(1,result.size());
+ return "[" + result + "]";
+ }
template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
static std::string convert(Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> val){
std::string result = "";
@@ -143,7 +159,25 @@ struct converter<std::string>{
}
return "[" + result + "]";
}
+ //// WOLF DEFINED TYPES -----> TO STRING
+ static std::string convert(VectorComposite val){
+ //TODO
+ // throw std::runtime_error("TODO! We still haven't got around to implement convert for VectorComposite to String :(");
+ auto helper = std::unordered_map<StateStructure, Eigen::VectorXd>();
+ for(const auto& pair: val)
+ helper.insert(std::pair<StateStructure, Eigen::VectorXd>(pair.first, pair.second));
+ return converter<std::string>::convert(helper);
+ }
+ static std::string convert(MatrixComposite val){
+ //TODO
+ // throw std::runtime_error("TODO! We still haven't got around to implement convert for MatrixComposite to String :(");
+ auto helper = std::unordered_map< StateStructure, std::unordered_map<StateStructure, Eigen::MatrixXd>>();
+ for(const auto& pair: val)
+ helper.insert(std::pair<StateStructure, std::unordered_map<StateStructure, Eigen::MatrixXd>>(pair.first, pair.second));
+ return converter<std::string>::convert(helper);
+ }
};
+ //// CONVERTERS ~~~~ TYPE ----> STRING
template<typename A, typename B>
struct converter<std::pair<A,B>>{
static std::pair<A,B> convert(std::string val){
@@ -153,6 +187,17 @@ struct converter<std::pair<A,B>>{
return std::pair<A,B>(converter<A>::convert(matches[1].str()), converter<B>::convert(matches[2].str()));
} else throw std::runtime_error("Invalid string format representing a pair. Correct format is {identifier:value}. String provided: " + val);
}
+};
+ //NEW GUY
+template<typename A, typename B>
+struct converter<Wpair<A,B>>{
+ static Wpair<A,B> convert(std::string val){
+ std::regex rgxP("\\(([^\\(,]+),([^\\)]+)\\)");
+ std::smatch matches;
+ if(std::regex_match(val, matches, rgxP)) {
+ return Wpair<A,B>(converter<A>::convert(matches[1].str()), converter<B>::convert(matches[2].str()));
+ } else throw std::runtime_error("Invalid string format representing a Wpair. Correct format is (first,second). String provided: " + val);
+ }
};
// TODO: WARNING!! For some reason when trying to specialize converter to std::array
// it defaults to the generic T type, thus causing an error!
@@ -216,5 +261,67 @@ struct converter<std::map<std::string,A>>{
return map;
}
};
+template<typename A, typename B>
+struct converter<std::map<A,B>>{
+ static std::map<A,B> convert(std::string val){
+ std::regex rgxM("\\[((?:(?:\\{[^\\{:]+:[^:\\}]+\\}),?)*)\\]");
+ if(not std::regex_match(val, rgxM))
+ throw std::runtime_error("Invalid string representation of a Map. Correct format is [({id:value})?(,{id:value})*]. String provided: " + val);
+
+ auto v = utils::parseList(val);
+ auto map = std::map<A, B>();
+ for(auto it : v){
+ auto p = converter<std::pair<A,B>>::convert(it);
+ map.insert(std::pair<A, B>(p.first, p.second));
+ }
+ return map;
+ }
+};
+template<typename A, typename B>
+struct converter<std::unordered_map<A,B>>{
+ static std::unordered_map<A,B> convert(std::string val){
+ std::regex rgxM("\\[((?:(?:\\{[^\\{:]+:[^:\\}]+\\}),?)*)\\]");
+ if(not std::regex_match(val, rgxM))
+ throw std::runtime_error("Invalid string representation of an unordered Map. Correct format is [({id:value})?(,{id:value})*]. String provided: " + val);
+
+ auto v = utils::parseList(val);
+ auto map = std::unordered_map<A, B>();
+ for(auto it : v){
+ auto p = converter<std::pair<A,B>>::convert(it);
+ map.insert(std::pair<A, B>(p.first, p.second));
+ }
+ return map;
+ }
+};
+//// FROM STRING -----> WOLF DEFINED TYPES
+template<>
+struct converter<VectorComposite>{
+ static VectorComposite convert(std::string val){
+ auto unordered_map = converter<std::unordered_map<StateStructure, Eigen::VectorXd>>::convert(val);
+ // VectorComposite result = VectorComposite(unordered_map);
+ // return result;
+ auto helper = VectorComposite();
+ for(auto const& it: unordered_map)
+ {
+ helper.insert(std::pair<StateStructure, Eigen::VectorXd>(it.first, it.second));
+ }
+ return helper;
+ }
+};
+template<>
+struct converter<MatrixComposite>{
+ static MatrixComposite convert(std::string val){
+ auto unordered_map = converter<std::unordered_map<StateStructure,
+ std::unordered_map<StateStructure, Eigen::MatrixXd>>>::convert(val);
+ // VectorComposite result = VectorComposite(unordered_map);
+ // return result;
+ auto helper = MatrixComposite();
+ for(auto const& it: unordered_map)
+ {
+ helper.insert(std::pair<StateStructure, std::unordered_map<StateStructure, Eigen::MatrixXd>>(it.first, it.second));
+ }
+ return helper;
+ }
+};
}
#endif