28 #include <ArmarXCore/interface/observers/VariantBase.h>
32 #include "../EigenForwardDeclarations.h"
36 template <
class T,
class =
void>
64 throw std::logic_error{
"This function should never be called"};
71 throw std::logic_error{
"This function should never be called"};
78 throw std::logic_error{
"This function should never be called"};
85 throw std::logic_error{
"This function should never be called"};
92 throw std::logic_error{
"This function should never be called"};
99 throw std::logic_error{
"This function should never be called"};
106 throw std::logic_error{
"This function should never be called"};
113 throw std::logic_error{
"This function should never be called"};
116 static const std::vector<std::string>&
120 throw std::logic_error{
"This function should never be called"};
128 #define make_def_for_build_in_ice_type(Type) \
130 struct DataFieldsInfo<Type, void> : DataFieldsInfoBase<Type> \
132 using DataFieldsInfoBase<Type>::GetDataFieldAs; \
134 GetNumberOfFields() \
139 GetDataFieldAs(std::size_t i, const Type& field, std::string& out) \
141 ARMARX_CHECK_EQUAL(i, 0); \
142 out = to_string(field); \
145 GetDataFieldAs(std::size_t i, const Type& field, Type& out) \
147 ARMARX_CHECK_EQUAL(i, 0); \
150 static const std::type_info& \
151 GetDataFieldType(std::size_t i) \
153 return typeid(Type); \
155 static std::map<std::string, VariantBasePtr> \
156 ToVariants(const Type& value, \
157 const std::string& name, \
158 const IceUtil::Time& timestamp, \
159 const std::string& frame = "", \
160 const std::string& agent = "") \
162 ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) \
163 << "There is no framed version for build in ice types"; \
164 return {{name, {new TimedVariant(value, timestamp)}}}; \
174 #undef make_def_for_build_in_ice_type
184 static std::size_t GetNumberOfFields();
185 static void GetDataFieldAs(std::size_t i,
const Eigen::Vector3f& field,
float& out);
186 static void GetDataFieldAs(std::size_t i,
const Eigen::Vector3f& field, std::string& out);
188 static const std::vector<std::string>& GetFieldNames();
189 static std::map<std::string, VariantBasePtr> ToVariants(
const Eigen::Vector3f&
value,
190 const std::string& name,
192 const std::string& frame =
"",
193 const std::string& agent =
"");
200 #define make_DataFieldsInfo_for_eigen_vector(Type, TypeName, Num) \
202 struct DataFieldsInfo<Eigen::Vector##Num##Type, void> : \
203 DataFieldsInfoBase<Eigen::Vector##Num##Type> \
205 using DataFieldsInfoBase<Eigen::Vector##Num##Type>::GetDataFieldAs; \
207 GetNumberOfFields() \
212 GetDataFieldAs(std::size_t i, const Eigen::Vector##Num##Type& field, std::string& out); \
214 GetDataFieldAs(std::size_t i, const Eigen::Vector##Num##Type& field, Ice::TypeName& out); \
215 static const std::type_info& GetDataFieldType(std::size_t i); \
216 static const std::vector<std::string>& GetFieldNames(); \
217 static std::map<std::string, VariantBasePtr> \
218 ToVariants(const Eigen::Vector##Num##Type& value, \
219 const std::string& name, \
220 const IceUtil::Time& timestamp, \
221 const std::string& frame = "", \
222 const std::string& agent = ""); \
242 #undef make_DataFieldsInfo_for_eigen_vector
252 static std::size_t GetNumberOfFields();
253 static void GetDataFieldAs(std::size_t i,
const Eigen::Matrix4f& field,
float& out);
254 static void GetDataFieldAs(std::size_t i,
const Eigen::Matrix4f& field, std::string& out);
256 static const std::vector<std::string>& GetFieldNames();
258 const std::string& name,
260 const std::string& frame =
"",
261 const std::string& agent =
"");
272 static std::size_t GetNumberOfFields();
273 static void GetDataFieldAs(std::size_t i,
const Eigen::Quaternionf& field,
float& out);
277 static const std::vector<std::string>& GetFieldNames();
279 const std::string& name,
281 const std::string& frame =
"",
282 const std::string& agent =
"");
294 static std::size_t GetNumberOfFields();
296 GetDataFieldAs(std::size_t i,
const std::chrono::microseconds& field,
long& out);
298 GetDataFieldAs(std::size_t i,
const std::chrono::microseconds& field, std::string& out);
300 static std::map<std::string, VariantBasePtr> ToVariants(std::chrono::microseconds
value,
301 const std::string& name,
303 const std::string& frame =
"",
304 const std::string& agent =
"");
315 static std::size_t GetNumberOfFields();
316 static void GetDataFieldAs(std::size_t i,
const IceUtil::Time& field,
long& out);
317 static void GetDataFieldAs(std::size_t i,
const IceUtil::Time& field, std::string& out);
320 const std::string& name,
322 const std::string& frame =
"",
323 const std::string& agent =
"");
339 static std::size_t GetNumberOfFields();
340 static void GetDataFieldAs(std::size_t i,
const JointStatus& field, std::string& out);
341 static void GetDataFieldAs(std::size_t i,
const JointStatus& field,
Ice::Int& out);
342 static void GetDataFieldAs(std::size_t i,
const JointStatus& field,
bool& out);
344 static const std::vector<std::string>& GetFieldNames();
346 static std::map<std::string, VariantBasePtr> ToVariants(
const JointStatus&
value,
347 const std::string& name,
349 const std::string& frame,
350 const std::string& agent);
357 #define make_def_for_type_mapped_to_long(Type) \
359 struct DataFieldsInfo<Type, void> : DataFieldsInfoBase<Type> \
361 using DataFieldsInfoBase<Type>::GetDataFieldAs; \
363 GetNumberOfFields() \
368 GetDataFieldAs(std::size_t i, const Type& field, std::string& out) \
370 ARMARX_CHECK_EQUAL(i, 0); \
371 out = to_string(field); \
374 GetDataFieldAs(std::size_t i, const Type& field, Ice::Long& out) \
376 ARMARX_CHECK_EQUAL(i, 0); \
379 static const std::type_info& \
380 GetDataFieldType(std::size_t i) \
382 return typeid(Type); \
384 static std::map<std::string, VariantBasePtr> \
385 ToVariants(const Type& value, \
386 const std::string& name, \
387 const IceUtil::Time& timestamp, \
388 const std::string& frame = "", \
389 const std::string& agent = "") \
391 ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) \
392 << "There is no framed version for build in ice types"; \
393 return {{name, {new TimedVariant(value, timestamp)}}}; \
398 #undef make_def_for_type_mapped_to_long
405 template <
class T,
class =
void>
413 std::enable_if_t<(&DataFieldsInfo<T>::GetFieldNames ==
414 &DataFieldsInfoBase<T>::GetFieldNames)>> : std::true_type
421 static_assert(!DataFieldsInfoHasNoFieldNamesV<Eigen::Vector3f>);
422 static_assert(!DataFieldsInfoHasNoFieldNamesV<Eigen::Quaternionf>);
423 static_assert(DataFieldsInfoHasNoFieldNamesV<Ice::Int>);
424 static_assert(DataFieldsInfoHasNoFieldNamesV<std::uint16_t>);
426 template <
class T, std::
size_t N>
435 return N * sub_t::GetNumberOfFields();
444 const auto subN = sub_t::GetNumberOfFields();
445 sub_t::GetDataFieldAs(i % subN, field.at(i / subN), out);
448 static const std::type_info&
455 static const std::vector<std::string>&
459 static const std::vector<std::string> result = []
461 if constexpr (!DataFieldsInfoHasNoFieldNamesV<T>)
464 const auto sub_names = sub_t::GetFieldNames();
465 std::vector<std::string> r;
466 r.reserve(N * sub_names.size());
467 for (std::size_t i = 0; i < N; ++i)
470 for (
const auto& name : sub_names)
472 r.emplace_back(pre + name);
480 std::vector<std::string> r;
482 for (std::size_t i = 0; i < N; ++i)
492 static std::map<std::string, VariantBasePtr>
494 const std::string& name,
496 const std::string& frame =
"",
497 const std::string& agent =
"")
500 std::map<std::string, VariantBasePtr> result;
501 for (std::size_t i = 0; i < N; ++i)
504 for (
const auto& [k,
v] :
505 sub_t::ToVariants(
value.at(i), name, timestamp, frame, agent))