mc_rtc  2.13.0
Configuration.h
Go to the documentation of this file.
1 /*
2  * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
3  */
4 
5 #pragma once
6 
8 #include <mc_rtc/deprecated.h>
9 
10 #include <mc_rbdyn/Gains.h>
11 #include <mc_rbdyn/rpy_utils.h>
12 
13 #include <SpaceVecAlg/SpaceVecAlg>
14 
15 #include <Eigen/Core>
16 #include <spdlog/fmt/fmt.h>
17 
18 #include <array>
19 #include <exception>
20 #include <map>
21 #include <memory>
22 #include <optional>
23 #include <set>
24 #include <string>
25 #include <string_view>
26 #include <unordered_set>
27 #include <variant>
28 #include <vector>
29 
30 // The code we use to convert a Configuration value to an std::variant value requires GCC >= 8.3
31 // We use a less optimal recursive implementation for the case where GCC < 8.3
32 #if defined(__GNUC__) && !defined(__llvm__) && !defined(__INTEL_COMPILER)
33 # if __GNUC__ > 8 || (__GNUC__ == 8 && __GNUC_MINOR__ > 2)
34 # define MC_RTC_USE_VARIANT_WORKAROUND 0
35 # else
36 # define MC_RTC_USE_VARIANT_WORKAROUND 1
37 # endif
38 #else
39 # define MC_RTC_USE_VARIANT_WORKAROUND 0
40 #endif
41 
42 namespace mc_rtc
43 {
44 
47 
54 template<typename T>
56 {
57  static void load(const mc_rtc::Configuration &) {}
58 
59  static void save(const T &) {}
60 };
61 
62 template<>
63 struct ConfigurationLoader<void>
64 {
65  static void load(const mc_rtc::Configuration &) {}
66  static void save() {}
67 };
68 
69 namespace internal
70 {
76 {
77  template<typename T,
78  typename std::enable_if<
79  std::is_same<decltype(ConfigurationLoader<T>::load(std::declval<const Configuration &>())), T>::value,
80  int>::type = 0>
81  static std::true_type test(T * p);
82 
83  template<typename T>
84  static std::false_type test(...);
85 };
86 
87 template<typename T>
88 inline constexpr bool has_configuration_load_object_v =
89  decltype(_has_configuration_load_object::test<T>(nullptr))::value;
90 
96 {
97  template<typename T,
98  typename = std::enable_if_t<
99  std::is_same_v<decltype(T::fromConfiguration(std::declval<const Configuration &>())), T>>>
100  static std::true_type test(T * p);
101 
102  template<typename T>
103  static std::false_type test(...);
104 };
105 
106 template<typename T>
107 inline constexpr bool has_static_fromConfiguration_v = decltype(_has_static_fromConfiguration::test<T>(nullptr))::value;
108 
114 {
115  template<typename T,
116  typename... Args,
117  typename std::enable_if<
118  std::is_same<decltype(ConfigurationLoader<T>::save(std::declval<const T &>(), std::declval<Args>()...)),
119  Configuration>::value,
120  int>::type = 0>
121  static std::true_type test(T * p);
122 
123  template<typename T, typename... Args>
124  static std::false_type test(...);
125 };
126 
127 template<typename T, typename... Args>
128 inline constexpr bool has_configuration_save_object_v =
129  decltype(_has_configuration_save_object::test<T, Args...>(nullptr))::value;
130 
136 {
137  template<
138  typename T,
139  typename... Args,
140  typename = std::enable_if_t<
141  std::is_same_v<decltype(std::declval<const T &>().toConfiguration(std::declval<Args>()...)), Configuration>>>
142  static std::true_type test(T * p);
143 
144  template<typename T, typename... Args>
145  static std::false_type test(...);
146 };
147 
148 template<typename T, typename... Args>
149 inline constexpr bool has_toConfiguration_method_v =
150  decltype(_has_toConfiguration_method::test<T, Args...>(nullptr))::value;
151 
152 #if MC_RTC_USE_VARIANT_WORKAROUND
154 template<size_t IDX, typename... Args>
155 std::variant<Args...> to_variant(const Configuration & c, size_t idx);
156 #endif
157 
158 } // namespace internal
159 
166 {
167 private:
173  struct MC_RTC_UTILS_DLLAPI Json
174  {
176  bool isArray() const noexcept;
178  size_t size() const noexcept;
183  Json operator[](size_t idx) const;
185  bool isObject() const noexcept;
187  std::vector<std::string> keys() const noexcept;
192  Json operator[](const std::string & key) const;
194  std::optional<Json> find(const std::string & key) const;
196  bool isString() const noexcept;
198  bool isNumeric() const noexcept;
200  bool isInteger() const noexcept;
202  bool isInt32() const noexcept;
204  bool isInt64() const noexcept;
206  bool isUInt32() const noexcept;
208  bool isUInt64() const noexcept;
210  bool isDouble() const noexcept;
211 
216  double asDouble() const;
221  void path(std::string & out) const noexcept;
223  void * value_;
225  std::shared_ptr<void> doc_;
226  };
227 
228 public:
236  struct MC_RTC_UTILS_DLLAPI Exception : public std::exception
237  {
244  Exception(const std::string & msg, const Json & v);
245 
252  Exception(const std::string & msg, const Configuration & c) : Exception(msg, c.v) {}
253 
254  ~Exception() noexcept;
255 
257  virtual const char * what() const noexcept override;
258 
260  void silence() const noexcept;
261 
263  const std::string & msg() const noexcept;
264 
265  private:
266  mutable std::string msg_;
267  mutable Json v_;
268  };
269 
276  MC_RTC_DEPRECATED bool isMember(const std::string & key) const;
277 
284  bool has(const std::string & key) const;
285 
290  operator bool() const;
291 
298  operator int8_t() const;
299 
307  operator uint8_t() const;
308 
315  operator int16_t() const;
316 
324  operator uint16_t() const;
325 
332  operator int32_t() const;
333 
341  operator uint32_t() const;
342 
349  operator int64_t() const;
350 
358  operator uint64_t() const;
359 
366  operator double() const;
367 
378  operator std::string() const;
379 
385  operator Eigen::Vector2d() const;
386 
392  operator Eigen::Vector3d() const;
393 
399  operator Eigen::Vector4d() const;
400 
406  operator Eigen::Vector6d() const;
407 
412  operator mc_rbdyn::Gains2d() const;
413 
418  operator mc_rbdyn::Gains3d() const;
419 
424  operator mc_rbdyn::Gains6d() const;
425 
434  operator Eigen::VectorXd() const;
435 
443  operator Eigen::Quaterniond() const;
444 
450  operator Eigen::Matrix3d() const;
451 
456  operator Eigen::Matrix6d() const;
457 
462  operator Eigen::MatrixXd() const;
463 
468  operator sva::PTransformd() const;
469 
474  operator sva::ForceVecd() const;
475 
480  operator sva::MotionVecd() const;
481 
486  operator sva::ImpedanceVecd() const;
487 
494  template<class T, class A>
495  operator std::vector<T, A>() const
496  {
497  if(v.isArray())
498  {
499  std::vector<T, A> ret;
500  for(size_t i = 0; i < v.size(); ++i) { ret.push_back(Configuration(v[i])); }
501  return ret;
502  }
503  throw Configuration::Exception("Stored Json value is not a vector", v);
504  }
505 
512  template<class T, std::size_t N>
513  operator std::array<T, N>() const
514  {
515  if(v.isArray() && v.size() == N)
516  {
517  std::array<T, N> ret;
518  for(size_t i = 0; i < N; ++i) { ret[i] = Configuration(v[i]); }
519  return ret;
520  }
521  throw Configuration::Exception("Stored Json value is not an array or its size is incorrect", v);
522  }
523 
529  template<class T1, class T2>
530  operator std::pair<T1, T2>() const
531  {
532  if(v.isArray() && v.size() == 2) { return std::make_pair<T1, T2>(Configuration(v[0]), Configuration(v[1])); }
533  throw Configuration::Exception("Stored Json value is not an array of size 2", v);
534  }
535 
541  template<typename T, class C, class A>
542  operator std::map<std::string, T, C, A>() const
543  {
544  if(v.isObject())
545  {
546  std::map<std::string, T, C, A> ret;
547  auto keys = v.keys();
548  assert(std::set<std::string>(keys.begin(), keys.end()).size() == keys.size());
549  for(const auto & k : keys)
550  {
551  T value = Configuration(v[k]);
552  ret[k] = value;
553  }
554  return ret;
555  }
556  throw Configuration::Exception("Stored Json value is not an object", v);
557  }
558 
565  template<typename T, typename C = std::less<T>, typename A = std::allocator<T>>
566  operator std::set<T, C, A>() const
567  {
568  if(v.isArray())
569  {
570  std::set<T, C, A> ret;
571  for(size_t i = 0; i < v.size(); ++i)
572  {
573  auto ins = ret.insert(Configuration(v[i]));
574  if(!ins.second) { throw Configuration::Exception("Stored Json set does not hold unique values", v); }
575  }
576  return ret;
577  }
578  throw Configuration::Exception("Stored Json value is not an array", v);
579  }
580 
587  template<typename T, typename H = std::hash<T>, typename E = std::equal_to<T>, typename A = std::allocator<T>>
588  operator std::unordered_set<T, H, E, A>() const
589  {
590  if(v.isArray())
591  {
592  std::unordered_set<T, H, E, A> ret;
593  for(size_t i = 0; i < v.size(); ++i)
594  {
595  auto ins = ret.insert(Configuration(v[i]));
596  if(!ins.second) { throw Configuration::Exception("Stored Json set does not hold unique values", v); }
597  }
598  return ret;
599  }
600  throw Configuration::Exception("Stored Json value is not an array", v);
601  }
602 
612  template<typename... Args>
613  operator std::variant<Args...>() const
614  {
615  if(!v.isArray()) { throw Configuration::Exception("Stored Json value is not an array", v); }
616  if(v.size() != 2) { throw Configuration::Exception("Stored Json value is not of size 2", v); }
617  size_t idx = Configuration(v[0]);
618  if(idx >= sizeof...(Args)) { throw Configuration::Exception("Variant index out of type index bound", v); }
619 #if MC_RTC_USE_VARIANT_WORKAROUND
620  return internal::to_variant<0, Args...>(v[1], idx);
621 #else
622  static constexpr auto table =
623  std::array{+[](const Configuration & c) { return std::variant<Args...>{c.operator Args()}; }...};
624  return table[idx](v[1]);
625 #endif
626  }
627 
632  template<typename T, typename std::enable_if<internal::is_integral_v<T>, int>::type = 0>
633  operator T() const
634  {
635  if constexpr(internal::is_like_int8_t<T>) { return static_cast<T>(this->operator int8_t()); }
636  else if constexpr(internal::is_like_int16_t<T>) { return static_cast<T>(this->operator int16_t()); }
637  else if constexpr(internal::is_like_int32_t<T>) { return static_cast<T>(this->operator int32_t()); }
638  else if constexpr(internal::is_like_int64_t<T>) { return static_cast<T>(this->operator int64_t()); }
639  else if constexpr(internal::is_like_uint8_t<T>) { return static_cast<T>(this->operator uint8_t()); }
640  else if constexpr(internal::is_like_uint16_t<T>) { return static_cast<T>(this->operator uint16_t()); }
641  else if constexpr(internal::is_like_uint32_t<T>) { return static_cast<T>(this->operator uint32_t()); }
642  else if constexpr(internal::is_like_uint64_t<T>) { return static_cast<T>(this->operator uint64_t()); }
643  else { static_assert(!std::is_same_v<T, T>, "T is integral but has an unsupported size"); }
644  }
645 
651  template<typename T,
652  typename std::enable_if<internal::has_configuration_load_object_v<T>
653  || internal::has_static_fromConfiguration_v<T>,
654  int>::type = 0>
655  operator T() const
656  {
657  if constexpr(internal::has_configuration_load_object_v<T>) { return ConfigurationLoader<T>::load(*this); }
658  else { return T::fromConfiguration(*this); }
659  }
660 
665  template<typename T>
666  explicit operator std::optional<T>() const
667  {
668  try
669  {
670  return this->convert<T>();
671  }
672  catch(Exception & exc)
673  {
674  exc.silence();
675  return std::nullopt;
676  }
677  }
678 
681 
687  Configuration(const std::string & path);
688 
694  Configuration(const char * path);
695 
701 
707  static Configuration fromData(const std::string & data);
708 
714  static Configuration fromData(const char * data);
715 
721  static Configuration fromYAMLData(const std::string & data);
722 
728  static Configuration fromYAMLData(const char * data);
729 
737  static Configuration fromMessagePack(const char * data, size_t size);
738 
748  void load(const std::string & path);
749 
761  void load(const mc_rtc::Configuration & config);
762 
770  void loadData(const std::string & data);
771 
779  void loadYAMLData(const std::string & data);
780 
790  void save(const std::string & path, bool pretty = true) const;
791 
799  std::string dump(bool pretty = false, bool yaml = false) const;
800 
808  size_t toMessagePack(std::vector<char> & data) const;
809 
817  void toMessagePack(MessagePackBuilder & builder) const;
818 
829  Configuration operator()(const std::string & key) const;
830 
837  std::optional<Configuration> find(const std::string & key) const;
838 
849  template<typename... Args>
850  std::optional<Configuration> find(const std::string & key, Args &&... others) const
851  {
852  auto out = find(key);
853  return out ? out->find(std::forward<Args>(others)...) : std::nullopt;
854  }
855 
866  template<typename T, typename... Args>
867  std::optional<T> find(const std::string & key, Args &&... others) const
868  {
869  auto maybe_cfg = find(key, std::forward<Args>(others)...);
870  if(maybe_cfg) { return maybe_cfg->operator T(); }
871  return std::nullopt;
872  }
873 
875  bool empty() const;
876 
883  size_t size() const;
884 
886  inline bool isArray() const noexcept { return v.isArray(); }
887 
889  inline bool isObject() const noexcept { return v.isObject(); }
890 
892  inline bool isString() const noexcept { return v.isString(); }
893 
895  inline bool isNumeric() const noexcept { return v.isNumeric(); }
897  inline bool isInteger() const noexcept { return v.isInteger(); }
899  inline bool isInt32() const noexcept { return v.isInt32(); }
901  inline bool isInt64() const noexcept { return v.isInt64(); }
903  inline bool isUInt32() const noexcept { return v.isUInt32(); }
905  inline bool isUInt64() const noexcept { return v.isUInt64(); }
907  inline bool isDouble() const noexcept { return v.isDouble(); }
908 
916  Configuration operator[](size_t i) const;
917 
926  template<typename T>
927  T at(size_t i, const T & v) const
928  {
929  try
930  {
931  return (*this)[i].convert<T>();
932  }
933  catch(Exception & exc)
934  {
935  exc.silence();
936  return v;
937  }
938  }
939 
952  template<typename T>
953  void operator()(const std::string & key, T & v) const
954  {
955  try
956  {
957  v = (*this)(key).convert<T>();
958  }
959  catch(Exception & exc)
960  {
961  exc.silence();
962  }
963  }
964 
975  template<typename T>
976  T operator()(const std::string & key, const T & v) const
977  {
978  try
979  {
980  return (*this)(key).convert<T>();
981  }
982  catch(Exception & exc)
983  {
984  exc.silence();
985  return v;
986  }
987  }
988 
993  bool operator==(const char * rhs) const;
994 
999  template<typename T>
1000  bool operator==(const T & rhs) const
1001  {
1002  T lhs = *this;
1003  return lhs == rhs;
1004  }
1005 
1013  void add_null(const std::string & key);
1014 
1023  void add(const std::string & key, bool value);
1024 
1029  void add(const std::string & key, int8_t value);
1030 
1035  void add(const std::string & key, uint8_t value);
1036 
1041  void add(const std::string & key, int16_t value);
1042 
1047  void add(const std::string & key, uint16_t value);
1048 
1053  void add(const std::string & key, int32_t value);
1054 
1059  void add(const std::string & key, uint32_t value);
1060 
1065  void add(const std::string & key, int64_t value);
1066 
1071  void add(const std::string & key, uint64_t value);
1072 
1077  void add(const std::string & key, double value);
1078 
1083  void add(const std::string & key, const std::string & value);
1084 
1090  void add(const std::string & key, const char * value);
1091 
1096  void add(const std::string & key, const Eigen::Vector2d & value);
1097 
1102  void add(const std::string & key, const Eigen::Vector3d & value);
1103 
1108  void add(const std::string & key, const Eigen::Vector4d & value);
1109 
1114  void add(const std::string & key, const Eigen::Vector6d & value);
1115 
1120  void add(const std::string & key, const Eigen::VectorXd & value);
1121 
1126  void add(const std::string & key, const Eigen::Quaterniond & value);
1127 
1132  void add(const std::string & key, const Eigen::Matrix3d & value);
1133 
1138  void add(const std::string & key, const Eigen::Matrix6d & value);
1139 
1144  void add(const std::string & key, const Eigen::MatrixXd & value);
1145 
1150  void add(const std::string & key, const sva::PTransformd & value);
1151 
1156  void add(const std::string & key, const sva::ForceVecd & value);
1157 
1162  void add(const std::string & key, const sva::MotionVecd & value);
1163 
1168  void add(const std::string & key, const sva::ImpedanceVecd & value);
1169 
1174  void add(const std::string & key, const Configuration & value);
1175 
1182  Configuration add(const std::string & key);
1183 
1192  Configuration array(const std::string & key, size_t size = 0);
1193 
1198  void push_null();
1199 
1206  void push(bool value);
1207 
1212  void push(int8_t value);
1213 
1218  void push(uint8_t value);
1219 
1224  void push(int16_t value);
1225 
1230  void push(uint16_t value);
1231 
1236  void push(int32_t value);
1237 
1242  void push(uint32_t value);
1243 
1248  void push(int64_t value);
1249 
1254  void push(uint64_t value);
1255 
1260  void push(double value);
1261 
1266  void push(const std::string & value);
1267 
1272  void push(const char * value);
1273 
1278  void push(const Eigen::Vector2d & value);
1279 
1284  void push(const Eigen::Vector3d & value);
1285 
1290  void push(const Eigen::Vector4d & value);
1291 
1296  void push(const Eigen::Vector6d & value);
1297 
1302  void push(const Eigen::VectorXd & value);
1303 
1308  void push(const Eigen::Quaterniond & value);
1309 
1314  void push(const Eigen::Matrix3d & value);
1315 
1320  void push(const Eigen::Matrix6d & value);
1321 
1326  void push(const Eigen::MatrixXd & value);
1327 
1332  void push(const sva::PTransformd & value);
1333 
1338  void push(const sva::ForceVecd & value);
1339 
1344  void push(const sva::MotionVecd & value);
1345 
1350  void push(const sva::ImpedanceVecd & value);
1351 
1356  void push(const Configuration & value);
1357 
1362  Configuration array(size_t reserve = 0);
1363 
1369 
1377  template<typename T,
1378  typename... Args,
1379  typename std::enable_if<internal::has_configuration_save_object_v<T, Args...>
1381  int>::type = 0>
1382  void push(const T & value, Args &&... args)
1383  {
1384  if constexpr(internal::has_configuration_save_object_v<T, Args...>)
1385  {
1386  push(mc_rtc::ConfigurationLoader<T>::save(value, std::forward<Args>(args)...));
1387  }
1388  else { push(value.toConfiguration(std::forward<Args>(args)...)); }
1389  }
1390 
1395  template<typename T, typename std::enable_if<internal::is_integral_v<T>, int>::type = 0>
1396  void push(const T & value)
1397  {
1398  if constexpr(internal::is_like_int8_t<T>) { push(static_cast<int8_t>(value)); }
1399  else if constexpr(internal::is_like_int16_t<T>) { push(static_cast<int16_t>(value)); }
1400  else if constexpr(internal::is_like_int32_t<T>) { push(static_cast<int32_t>(value)); }
1401  else if constexpr(internal::is_like_int64_t<T>) { push(static_cast<int64_t>(value)); }
1402  else if constexpr(internal::is_like_uint8_t<T>) { push(static_cast<uint8_t>(value)); }
1403  else if constexpr(internal::is_like_uint16_t<T>) { push(static_cast<uint16_t>(value)); }
1404  else if constexpr(internal::is_like_uint32_t<T>) { push(static_cast<uint32_t>(value)); }
1405  else if constexpr(internal::is_like_uint64_t<T>) { push(static_cast<uint64_t>(value)); }
1406  else { static_assert(!std::is_same_v<T, T>, "T is integral but has an unsupported size"); }
1407  }
1408 
1417  template<typename T, typename A = std::allocator<T>, typename... Args>
1418  void add(const std::string & key, const std::vector<T, A> & value, Args &&... args)
1419  {
1420  Configuration v = array(key, value.size());
1421  for(const auto & vi : value) { v.push(vi, std::forward<Args>(args)...); }
1422  }
1423 
1432  template<typename T, std::size_t N, typename... Args>
1433  void add(const std::string & key, const std::array<T, N> & value, Args &&... args)
1434  {
1435  Configuration v = array(key, N);
1436  for(const auto & vi : value) { v.push(vi, std::forward<Args>(args)...); }
1437  }
1438 
1447  template<typename T1, typename T2, typename... Args>
1448  void add(const std::string & key, const std::pair<T1, T2> & value, Args &&... args)
1449  {
1450  Configuration v = array(key, 2);
1451  v.push(value.first, std::forward<Args>(args)...);
1452  v.push(value.second, std::forward<Args>(args)...);
1453  }
1454 
1463  template<typename T,
1464  class C = std::less<std::string>,
1465  class A = std::allocator<std::pair<const std::string, T>>,
1466  typename... Args>
1467  void add(const std::string & key, const std::map<std::string, T, C, A> & value, Args &&... args)
1468  {
1469  Configuration v = add(key);
1470  for(const auto & el : value) { v.add(el.first, el.second, std::forward<Args>(args)...); }
1471  }
1472 
1481  template<typename T, typename C = std::less<T>, typename A = std::allocator<T>, typename... Args>
1482  void add(const std::string & key, const std::set<T, C, A> & value, Args &&... args)
1483  {
1484  Configuration v = array(key, value.size());
1485  for(const auto & v : value) { v.push(*v, std::forward<Args>(args)...); }
1486  }
1487 
1496  template<typename T,
1497  typename H = std::hash<T>,
1498  typename E = std::equal_to<T>,
1499  typename A = std::allocator<T>,
1500  typename... Args>
1501  void add(const std::string & key, const std::unordered_set<T, H, E, A> & value, Args &&... args)
1502  {
1503  Configuration v = array(key, value.size());
1504  for(const auto & v : value) { v.push(*v, std::forward<Args>(args)...); }
1505  }
1506 
1517  template<typename... Args>
1518  void add(const std::string & key, const std::variant<Args...> & value)
1519  {
1520  Configuration v = array(key, 2);
1521  v.push(value.index());
1522  std::visit([&v](const auto & hold) { v.push(hold); }, value);
1523  }
1524 
1531  template<typename T, typename std::enable_if<internal::is_integral_v<T>, int>::type = 0>
1532  void add(const std::string & key, const T & value)
1533  {
1534  if constexpr(internal::is_like_int8_t<T>) { add(key, static_cast<int8_t>(value)); }
1535  else if constexpr(internal::is_like_int16_t<T>) { add(key, static_cast<int16_t>(value)); }
1536  else if constexpr(internal::is_like_int32_t<T>) { add(key, static_cast<int32_t>(value)); }
1537  else if constexpr(internal::is_like_int64_t<T>) { add(key, static_cast<int64_t>(value)); }
1538  else if constexpr(internal::is_like_uint8_t<T>) { add(key, static_cast<uint8_t>(value)); }
1539  else if constexpr(internal::is_like_uint16_t<T>) { add(key, static_cast<uint16_t>(value)); }
1540  else if constexpr(internal::is_like_uint32_t<T>) { add(key, static_cast<uint32_t>(value)); }
1541  else if constexpr(internal::is_like_uint64_t<T>) { add(key, static_cast<uint64_t>(value)); }
1542  else { static_assert(!std::is_same_v<T, T>, "T is integral but has an unsupported size"); }
1543  }
1544 
1554  template<typename T,
1555  typename... Args,
1556  typename std::enable_if<internal::has_configuration_save_object_v<T, Args...>
1558  int>::type = 0>
1559  void add(const std::string & key, const T & value, Args &&... args)
1560  {
1561  if constexpr(internal::has_configuration_save_object_v<T, Args...>)
1562  {
1563  add(key, ConfigurationLoader<T>::save(value, std::forward<Args>(args)...));
1564  }
1565  else { add(key, value.toConfiguration(std::forward<Args>(args)...)); }
1566  }
1567 
1572  template<typename T, typename A = std::allocator<T>, typename... Args>
1573  void push(const std::vector<T, A> & value, Args &&... args)
1574  {
1575  Configuration v = array(value.size());
1576  for(const auto & vi : value) { v.push(vi, std::forward<Args>(args)...); }
1577  }
1578 
1583  template<typename T, std::size_t N, typename... Args>
1584  void push(const std::array<T, N> & value, Args &&... args)
1585  {
1586  Configuration v = array(N);
1587  for(const auto & vi : value) { v.push(vi, std::forward<Args>(args)...); }
1588  }
1589 
1594  template<typename T1, typename T2, typename... Args>
1595  void push(const std::pair<T1, T2> & value, Args &&... args)
1596  {
1597  Configuration v = array(2);
1598  v.push(value.first, std::forward<Args>(args)...);
1599  v.push(value.second, std::forward<Args>(args)...);
1600  }
1601 
1606  template<typename T,
1607  class C = std::less<std::string>,
1608  class A = std::allocator<std::pair<const std::string, T>>,
1609  typename... Args>
1610  void push(const std::map<std::string, T, C, A> & value, Args &&... args)
1611  {
1612  Configuration v = object();
1613  for(const auto & el : value) { v.add(el.first, el.second, std::forward<Args>(args)...); }
1614  }
1615 
1624  template<typename T, typename C = std::less<T>, typename A = std::allocator<T>, typename... Args>
1625  void push(const std::set<T, C, A> & value, Args &&... args)
1626  {
1627  Configuration v = array(value.size());
1628  for(const auto & v : value) { v.push(*v, std::forward<Args>(args)...); }
1629  }
1630 
1639  template<typename T,
1640  typename H = std::hash<T>,
1641  typename E = std::equal_to<T>,
1642  typename A = std::allocator<T>,
1643  typename... Args>
1644  void push(const std::unordered_set<T, H, E, A> & value, Args &&... args)
1645  {
1646  Configuration v = array(value.size());
1647  for(const auto & v : value) { v.push(*v, std::forward<Args>(args)...); }
1648  }
1649 
1660  template<typename... Args>
1661  void push(const std::variant<Args...> & value)
1662  {
1663  Configuration v = array(2);
1664  v.push(value.index());
1665  std::visit([&v](const auto & hold) { v.push(hold); }, value);
1666  }
1667 
1677  bool remove(const std::string & key);
1678 
1683  std::vector<std::string> keys() const;
1684 
1686 
1688 
1689 private:
1690  Json v;
1691  Configuration(const Json & v);
1692 
1693  template<typename T>
1694  T convert() const
1695  {
1696  if constexpr(std::is_same_v<T, Configuration>) { return *this; }
1697  else { return this->operator T(); }
1698  }
1699 };
1700 
1702 {
1704  bool operator!=(const ConfigurationArrayIterator & rhs) const;
1707  const Configuration operator*() const;
1708  size_t i = 0;
1710 };
1711 
1713 template<>
1714 void MC_RTC_UTILS_DLLAPI Configuration::operator()(const std::string & key, std::string & v) const;
1715 
1736 {
1738  ConfigurationFile(const std::string & path);
1739 
1741  void reload();
1742 
1744  void save() const;
1745 
1746  using Configuration::save;
1747 
1749  inline const std::string & path() const noexcept { return path_; }
1750 
1751 private:
1752  std::string path_;
1753 };
1754 
1755 #if MC_RTC_USE_VARIANT_WORKAROUND
1756 namespace internal
1757 {
1758 
1759 template<size_t IDX, typename... Args>
1760 std::variant<Args...> to_variant(const Configuration & c, size_t idx)
1761 {
1762  // Note: this never happens because we always pass idx < sizeof...(Args)
1763  if constexpr(IDX >= sizeof...(Args)) { mc_rtc::log::error_and_throw("Invalid runtime index for variant"); }
1764  else
1765  {
1766  if(idx == IDX) { return c.operator std::variant_alternative_t<IDX, std::variant<Args...>>(); }
1767  else { return to_variant<IDX + 1, Args...>(c, idx); }
1768  }
1769 }
1770 
1771 } // namespace internal
1772 #endif
1773 
1774 } // namespace mc_rtc
1775 
1777 MC_RTC_UTILS_DLLAPI std::ostream & operator<<(std::ostream & os, const mc_rtc::Configuration & c);
1778 
1779 namespace fmt
1780 {
1781 
1782 template<>
1783 struct formatter<mc_rtc::Configuration> : public formatter<string_view>
1784 {
1785  template<typename FormatContext>
1786  auto format(const mc_rtc::Configuration & c, FormatContext & ctx) -> decltype(ctx.out())
1787  {
1788  return formatter<string_view>::format(static_cast<std::string>(c), ctx);
1789  }
1790 };
1791 
1792 } // namespace fmt
MC_RTC_UTILS_DLLAPI std::ostream & operator<<(std::ostream &os, const mc_rtc::Configuration &c)
Ostream operator.
Definition: Configuration.h:1780
Definition: generic_gripper.h:15
Gains< 3 > Gains3d
Definition: Gains.h:34
Gains< 2 > Gains2d
Definition: Gains.h:33
Gains< 6 > Gains6d
Definition: Gains.h:35
constexpr bool has_static_fromConfiguration_v
Definition: Configuration.h:107
constexpr bool has_toConfiguration_method_v
Definition: Configuration.h:149
constexpr bool has_configuration_save_object_v
Definition: Configuration.h:128
constexpr bool has_configuration_load_object_v
Definition: Configuration.h:88
void error_and_throw(Args &&... args)
Definition: logging.h:47
Definition: Contact.h:88
struct MC_RTC_UTILS_DLLAPI ConfigurationArrayIterator
Definition: Configuration.h:45
struct MC_RTC_UTILS_DLLAPI Configuration
Definition: Configuration.h:46
Definition: Contact.h:67
auto format(const mc_rtc::Configuration &c, FormatContext &ctx) -> decltype(ctx.out())
Definition: Configuration.h:1786
Definition: Configuration.h:1702
ConfigurationArrayIterator(const Configuration &conf)
bool operator!=(const ConfigurationArrayIterator &rhs) const
ConfigurationArrayIterator & operator++()
Configuration conf
Definition: Configuration.h:1709
const Configuration operator*() const
Configuration object that keeps track of the file it comes from
Definition: Configuration.h:1736
ConfigurationFile(const std::string &path)
const std::string & path() const noexcept
Definition: Configuration.h:1749
static void save()
Definition: Configuration.h:66
static void load(const mc_rtc::Configuration &)
Definition: Configuration.h:65
Definition: Configuration.h:56
static void load(const mc_rtc::Configuration &)
Definition: Configuration.h:57
static void save(const T &)
Definition: Configuration.h:59
Exception thrown by this class when something bad occurs.
Definition: Configuration.h:237
void silence() const noexcept
Exception(const std::string &msg, const Configuration &c)
Constructor.
Definition: Configuration.h:252
Exception(const std::string &msg, const Json &v)
Constructor.
Simplify access to values hold within a JSON file.
Definition: Configuration.h:166
size_t size() const
If the stored value is an array, returns its size, otherwise returns 0.
void push(const Eigen::Vector4d &value)
Insert a Eigen::Vector4d element into an array.
static Configuration fromYAMLData(const char *data)
Static constructor to load from YAML data (C overload)
void push(const Configuration &value)
Push a Configuration element into an array.
void add(const std::string &key, const std::array< T, N > &value, Args &&... args)
Add an array into the JSON document.
Definition: Configuration.h:1433
void toMessagePack(MessagePackBuilder &builder) const
Append to an existing MessagePackBuilder.
void add(const std::string &key, const Eigen::Vector6d &value)
Add a Eigen::Vector6d element to the Configuration.
void loadData(const std::string &data)
Load data from a JSON string.
Configuration(const char *path)
Constructor using a file path (C-style string)
bool isUInt64() const noexcept
Definition: Configuration.h:905
void push(const Eigen::Quaterniond &value)
Insert a Eigen::Quaterniond element into an array.
void add(const std::string &key, const Configuration &value)
Add another Configuration to the Configuration.
void push(uint64_t value)
Insert a uint64_t element into an array.
void add(const std::string &key, int64_t value)
Add a int64_t element to the Configuration.
void push(const Eigen::Matrix3d &value)
Insert a Eigen::Matrix3d element into an array.
void push(int64_t value)
Insert a int64_t element int64_to an array.
void add(const std::string &key, const std::vector< T, A > &value, Args &&... args)
Add a vector into the JSON document.
Definition: Configuration.h:1418
void push(int8_t value)
Insert a int8_t element int8_to an array.
bool empty() const
Returns true if the underlying array or underlying object is empty.
static Configuration fromYAMLData(const std::string &data)
Static constructor to load from YAML data.
void push(const std::variant< Args... > &value)
Push a variant object into the JSON document.
Definition: Configuration.h:1661
void push(bool value)
Insert a bool element into an array.
void push(const std::unordered_set< T, H, E, A > &value, Args &&... args)
Push an unordered set into the JSON document.
Definition: Configuration.h:1644
Configuration add(const std::string &key)
Create an empty object in the Configuration.
void add(const std::string &key, double value)
Add a double element to the Configuration.
void add(const std::string &key, const Eigen::Matrix3d &value)
Add a Eigen::Matrix3d element to the Configuration.
void push(int16_t value)
Insert a int16_t element int16_to an array.
void add(const std::string &key, const std::map< std::string, T, C, A > &value, Args &&... args)
Add string-indexed map into the JSON document.
Definition: Configuration.h:1467
void push(const T &value)
Definition: Configuration.h:1396
void add(const std::string &key, const sva::MotionVecd &value)
Add an sva::MotionVecd element to the Configuration.
void push(int32_t value)
Insert a int32_t element int32_to an array.
void add(const std::string &key, const Eigen::Vector2d &value)
Add a Eigen::Vector2d element to the Configuration.
void push(const Eigen::VectorXd &value)
Insert a Eigen::VectorXd element into an array.
void push_null()
Insert a null value into an array.
T operator()(const std::string &key, const T &v) const
Retrieve a given value stored within the configuration with a default value.
Definition: Configuration.h:976
void add(const std::string &key, const Eigen::Quaterniond &value)
Add a Eigen::Quaterniond element to the Configuration.
void operator()(const std::string &key, T &v) const
Retrieve and store a given value stored within the configuration .
Definition: Configuration.h:953
void push(const sva::ForceVecd &value)
Insert an sva::ForceVecd element into an array.
void add(const std::string &key, const std::string &value)
Add a std::string element to the Configuration.
void add(const std::string &key, const Eigen::Matrix6d &value)
Add a Eigen::Matrix6d element to the Configuration.
void add(const std::string &key, uint8_t value)
Add a uint8_t element to the Configuration.
ConfigurationArrayIterator end() const
std::optional< Configuration > find(const std::string &key, Args &&... others) const
Return the Configuration entry at (key, others...) if it exists, std::nullopt otherwise.
Definition: Configuration.h:850
Configuration()
Creates an empty configuration.
void push(uint8_t value)
Insert a uint8_t element into an array.
void push(const T &value, Args &&... args)
User-defined conversion.
Definition: Configuration.h:1382
void push(const std::pair< T1, T2 > &value, Args &&... args)
Push a pair into the JSON document.
Definition: Configuration.h:1595
void add(const std::string &key, bool value)
Add a bool element to the Configuration.
void push(uint32_t value)
Insert a uint32_t element into an array.
std::vector< std::string > keys() const
void load(const mc_rtc::Configuration &config)
Load data from another Configuration object.
void push(const Eigen::Vector3d &value)
Insert a Eigen::Vector3d element into an array.
bool isObject() const noexcept
Returns true if the underlying value is an object.
Definition: Configuration.h:889
bool isNumeric() const noexcept
Returns true if the underlying value is numeric.
Definition: Configuration.h:895
void save(const std::string &path, bool pretty=true) const
Save the configuration to a file.
void push(const std::array< T, N > &value, Args &&... args)
Push an array into the JSON document.
Definition: Configuration.h:1584
Configuration object()
Push an empty object.
void add(const std::string &key, uint64_t value)
Add a uint64_t element to the Configuration.
bool isInt64() const noexcept
Definition: Configuration.h:901
static Configuration fromMessagePack(const char *data, size_t size)
Static constructor to load from MessagePack data.
void push(const Eigen::Vector6d &value)
Insert a Eigen::Vector6d element into an array.
bool operator==(const char *rhs) const
Non-template version for C-style strings comparison.
std::optional< T > find(const std::string &key, Args &&... others) const
Return the Configuration entry at (key, others...) if it exists, std::nullopt otherwise.
Definition: Configuration.h:867
std::optional< Configuration > find(const std::string &key) const
Returns the Configuration entry if it exists, std::nullopt otherwise.
Configuration array(const std::string &key, size_t size=0)
Create an empty array in the Configuration.
void push(const sva::ImpedanceVecd &value)
Insert an sva::ImpedanceVecd element into an array.
Configuration array(size_t reserve=0)
Push an empty array.
bool isArray() const noexcept
Returns true if the underlying value is an array.
Definition: Configuration.h:886
void add(const std::string &key, uint32_t value)
Add a uint32_t element to the Configuration.
void add(const std::string &key, const Eigen::Vector3d &value)
Add a Eigen::Vector3d element to the Configuration.
T at(size_t i, const T &v) const
Retrieve a given value from a JSON array.
Definition: Configuration.h:927
void add(const std::string &key, int32_t value)
Add a int32_t element to the Configuration.
void add(const std::string &key, const T &value, Args &&... args)
User-defined conversion.
Definition: Configuration.h:1559
void add(const std::string &key, const T &value)
Definition: Configuration.h:1532
void add_null(const std::string &key)
Add a null element to the configuration.
bool isUInt32() const noexcept
Definition: Configuration.h:903
void add(const std::string &key, const sva::ForceVecd &value)
Add an sva::ForceVecd element to the Configuration.
void add(const std::string &key, const std::variant< Args... > &value)
Add a variant object into the JSON document.
Definition: Configuration.h:1518
void push(const std::vector< T, A > &value, Args &&... args)
Push a vector into the JSON document.
Definition: Configuration.h:1573
bool isDouble() const noexcept
Definition: Configuration.h:907
void add(const std::string &key, const Eigen::VectorXd &value)
Add a Eigen::VectorXd element to the Configuration.
void push(const Eigen::Matrix6d &value)
Insert a Eigen::Matrix6d element into an array.
bool operator==(const T &rhs) const
Compare stored values with given value.
Definition: Configuration.h:1000
void loadYAMLData(const std::string &data)
Load data from a YAML string.
void add(const std::string &key, const sva::ImpedanceVecd &value)
Add an sva::ImpedanceVecd element to the Configuration.
static Configuration rootArray()
Returns a Configuration with an array as root entry.
void push(const std::set< T, C, A > &value, Args &&... args)
Push a set into the JSON document.
Definition: Configuration.h:1625
void push(const std::map< std::string, T, C, A > &value, Args &&... args)
Push a string-indexed map into the JSON document.
Definition: Configuration.h:1610
bool isString() const noexcept
Returns true if the underlying value is a string.
Definition: Configuration.h:892
void push(const sva::MotionVecd &value)
Insert an sva::MotionVecd element into an array.
void add(const std::string &key, uint16_t value)
Add a uint16_t element to the Configuration.
void add(const std::string &key, const Eigen::Vector4d &value)
Add a Eigen::Vector4d element to the Configuration.
size_t toMessagePack(std::vector< char > &data) const
Convert to MessagePack.
bool isInt32() const noexcept
Definition: Configuration.h:899
void add(const std::string &key, const std::set< T, C, A > &value, Args &&... args)
Add a set into the JSON document.
Definition: Configuration.h:1482
void push(const sva::PTransformd &value)
Insert an sva::PTransformd element into an array.
void add(const std::string &key, const char *value)
Add a const char* element to the Configuration, Behaves like std::string.
void load(const std::string &path)
Load more data into the configuration.
void push(const Eigen::Vector2d &value)
Insert a Eigen::Vector2d element into an array.
static Configuration fromData(const std::string &data)
Static constructor to load from JSON data.
void push(const Eigen::MatrixXd &value)
Insert a Eigen::MatrixXd element into an array.
Configuration operator()(const std::string &key) const
Returns a Entry value stored within the configuration.
void add(const std::string &key, int16_t value)
Add a int16_t element to the Configuration.
void add(const std::string &key, int8_t value)
Add a int64_t element to the Configuration.
void push(uint16_t value)
Insert a uint16_t element into an array.
void add(const std::string &key, const std::unordered_set< T, H, E, A > &value, Args &&... args)
Add an unordered set into the JSON document.
Definition: Configuration.h:1501
void push(const char *value)
Insert a const char * element into an array Behaves like push(std::string)
ConfigurationArrayIterator begin() const
static Configuration fromData(const char *data)
Static constructor to load from JSON data (C overload)
bool remove(const std::string &key)
Configuration(const std::string &path)
Constructor using a file path.
bool isInteger() const noexcept
Definition: Configuration.h:897
void push(const std::string &value)
Insert a std::string element into an array.
void add(const std::string &key, const sva::PTransformd &value)
Add an sva::PTransformd element to the Configuration.
Configuration operator[](size_t i) const
If the stored value is an array, return a Configuration element for the i-th element.
void add(const std::string &key, const std::pair< T1, T2 > &value, Args &&... args)
Add a pair into the JSON document.
Definition: Configuration.h:1448
void add(const std::string &key, const Eigen::MatrixXd &value)
Add an Eigen::MatrixXd element to the Configuration.
void push(double value)
Insert a double element into an array.
std::string dump(bool pretty=false, bool yaml=false) const
Dump the configuration into a string.
Definition: MessagePackBuilder.h:87
Definition: Configuration.h:136
#define MC_RTC_UTILS_DLLAPI
Definition: utils_api.h:50