DataFieldsInfo.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package RobotAPI::RobotUnit
17  * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
18  * @date 2017
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include "DataFieldsInfo.h"
24 
25 #include <Eigen/Core>
26 #include <Eigen/Geometry>
27 
28 #include <ArmarXCore/observers/variant/TimestampVariant.h>
29 
30 #include <RobotAPI/interface/units/KinematicUnitInterface.h>
31 #include <RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.h>
32 #include <RobotAPI/libraries/core/FramedPose.h>
33 #include <RobotAPI/libraries/core/OrientedPoint.h>
34 #include <RobotAPI/libraries/core/Pose.h>
35 
36 //Eigen::Vector3f
37 namespace armarx::introspection
38 {
39  std::size_t
40  DataFieldsInfo<Eigen::Vector3f, void>::GetNumberOfFields()
41  {
42  return 3;
43  }
44 
45  void
46  DataFieldsInfo<Eigen::Vector3f, void>::GetDataFieldAs(std::size_t i,
47  const Eigen::Vector3f& field,
48  std::string& out)
49  {
50  ARMARX_CHECK_LESS(i, 3);
51  out = to_string(field(i));
52  }
53 
54  void
55  DataFieldsInfo<Eigen::Vector3f, void>::GetDataFieldAs(std::size_t i,
56  const Eigen::Vector3f& field,
57  float& out)
58  {
59  ARMARX_CHECK_LESS(i, 3);
60  out = field(i);
61  }
62 
63  const std::type_info&
64  DataFieldsInfo<Eigen::Vector3f, void>::GetDataFieldType(std::size_t i)
65  {
66  return typeid(float);
67  }
68 
69  const std::vector<std::string>&
70  DataFieldsInfo<Eigen::Vector3f, void>::GetFieldNames()
71  {
72  static const std::vector<std::string> result{"x", "y", "z"};
73  return result;
74  }
75 
76  std::map<std::string, VariantBasePtr>
77  DataFieldsInfo<Eigen::Vector3f, void>::ToVariants(const Eigen::Vector3f& value,
78  const std::string& name,
79  const IceUtil::Time& timestamp,
80  const std::string& frame,
81  const std::string& agent)
82  {
83  if (!frame.empty())
84  {
85  return {{name, {new TimedVariant(FramedPosition{value, frame, agent}, timestamp)}}};
86  }
87  ARMARX_CHECK_EXPRESSION(agent.empty()) << "No frame but an agent given";
88  return {{name, {new TimedVariant(Vector3{value}, timestamp)}}};
89  }
90 } // namespace armarx::introspection
91 
92 //Eigen::Vector##Num##Type
93 namespace armarx::introspection
94 {
95 #define make_DataFieldsInfo_for_eigen_vector(Type, TypeName, Num) \
96  void DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetDataFieldAs( \
97  std::size_t i, const Eigen::Vector##Num##Type& field, std::string& out) \
98  { \
99  ARMARX_CHECK_LESS(i, Num); \
100  out = to_string(field(i)); \
101  } \
102  void DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetDataFieldAs( \
103  std::size_t i, const Eigen::Vector##Num##Type& field, Ice::TypeName& out) \
104  { \
105  ARMARX_CHECK_LESS(i, Num); \
106  out = field(i); \
107  } \
108  const std::type_info& DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetDataFieldType( \
109  std::size_t i) \
110  { \
111  return typeid(Ice::TypeName); \
112  } \
113  std::map<std::string, VariantBasePtr> \
114  DataFieldsInfo<Eigen::Vector##Num##Type, void>::ToVariants( \
115  const Eigen::Vector##Num##Type& value, \
116  const std::string& name, \
117  const IceUtil::Time& timestamp, \
118  const std::string& frame, \
119  const std::string& agent) \
120  { \
121  ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) \
122  << "There is no framed version of TimestampVariant"; \
123  std::map<std::string, VariantBasePtr> result; \
124  for (int i = 0; i < Num; ++i) \
125  { \
126  result.emplace(name + "." + to_string(i), \
127  VariantBasePtr{new TimedVariant(value(i), timestamp)}); \
128  } \
129  return result; \
130  } \
131  const std::vector<std::string>& \
132  DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetFieldNames() \
133  { \
134  static const std::vector<std::string> result = [] \
135  { \
136  std::vector<std::string> r; \
137  for (int i = 0; i < Num; ++i) \
138  { \
139  r.emplace_back(to_string(i)); \
140  } \
141  return r; \
142  }(); \
143  return result; \
144  }
145 
146  make_DataFieldsInfo_for_eigen_vector(f, Float, 2) make_DataFieldsInfo_for_eigen_vector(f,
147  Float,
148  4)
149  make_DataFieldsInfo_for_eigen_vector(f, Float, 5)
150  make_DataFieldsInfo_for_eigen_vector(f, Float, 6)
151 
152  make_DataFieldsInfo_for_eigen_vector(d, Double, 2)
153  make_DataFieldsInfo_for_eigen_vector(d, Double, 3)
154  make_DataFieldsInfo_for_eigen_vector(d, Double, 4)
155  make_DataFieldsInfo_for_eigen_vector(d, Double, 5)
156  make_DataFieldsInfo_for_eigen_vector(d, Double, 6)
157 
158  make_DataFieldsInfo_for_eigen_vector(i, Int, 2)
159  make_DataFieldsInfo_for_eigen_vector(i, Int, 3)
160  make_DataFieldsInfo_for_eigen_vector(i, Int, 4)
161  make_DataFieldsInfo_for_eigen_vector(i, Int, 5)
162  make_DataFieldsInfo_for_eigen_vector(i, Int, 6)
163 #undef make_DataFieldsInfo_for_eigen_vector
164 } // namespace armarx::introspection
165 
166 //Eigen::Matrix4f
167 namespace armarx::introspection
168 {
169  std::size_t
170  DataFieldsInfo<Eigen::Matrix4f, void>::GetNumberOfFields()
171  {
172  return 16;
173  }
174 
175  void
176  DataFieldsInfo<Eigen::Matrix4f, void>::GetDataFieldAs(std::size_t i,
177  const Eigen::Matrix4f& field,
178  float& out)
179  {
180  ARMARX_CHECK_LESS(i, 16);
181  out = field(i / 4, i % 4);
182  }
183 
184  void
185  DataFieldsInfo<Eigen::Matrix4f, void>::GetDataFieldAs(std::size_t i,
186  const Eigen::Matrix4f& field,
187  std::string& out)
188  {
189  ARMARX_CHECK_LESS(i, 16);
190  out = to_string(field(i / 4, i % 4));
191  }
192 
193  const std::type_info&
194  DataFieldsInfo<Eigen::Matrix4f, void>::GetDataFieldType(std::size_t i)
195  {
196  return typeid(float);
197  }
198 
199  const std::vector<std::string>&
200  DataFieldsInfo<Eigen::Matrix4f, void>::GetFieldNames()
201  {
202  static const std::vector<std::string> result{"00",
203  "01",
204  "02",
205  "03",
206  "10",
207  "11",
208  "12",
209  "13",
210  "20",
211  "21",
212  "22",
213  "23",
214  "30",
215  "31",
216  "32",
217  "33"};
218  return result;
219  }
220 
221  std::map<std::string, VariantBasePtr>
222  DataFieldsInfo<Eigen::Matrix4f, void>::ToVariants(const Eigen::Matrix4f& value,
223  const std::string& name,
224  const IceUtil::Time& timestamp,
225  const std::string& frame,
226  const std::string& agent)
227  {
228  if (!frame.empty())
229  {
230  return {{name, {new TimedVariant(FramedPose{value, frame, agent}, timestamp)}}};
231  }
232  ARMARX_CHECK_EXPRESSION(agent.empty()) << "No frame but an agent given";
233  return {{name, {new TimedVariant(Pose{value}, timestamp)}}};
234  }
235 } // namespace armarx::introspection
236 
237 //Eigen::Quaternionf
238 namespace armarx::introspection
239 {
240  std::size_t
241  DataFieldsInfo<Eigen::Quaternionf, void>::GetNumberOfFields()
242  {
243  return 4;
244  }
245 
246  void
247  DataFieldsInfo<Eigen::Quaternionf, void>::GetDataFieldAs(std::size_t i,
248  const Eigen::Quaternionf& field,
249  float& out)
250  {
251  ARMARX_CHECK_LESS(i, 4);
252  switch (i)
253  {
254  case 0:
255  out = field.w();
256  return;
257  case 1:
258  out = field.x();
259  return;
260  case 2:
261  out = field.y();
262  return;
263  case 3:
264  out = field.z();
265  return;
266  }
267  throw std::logic_error{__FILE__ " : " + to_string(__LINE__) +
268  " : Unreachable code reached"};
269  }
270 
271  void
272  DataFieldsInfo<Eigen::Quaternionf, void>::GetDataFieldAs(std::size_t i,
273  const Eigen::Quaternionf& field,
274  std::string& out)
275  {
276  ARMARX_CHECK_LESS(i, 4);
277  switch (i)
278  {
279  case 0:
280  out = to_string(field.w());
281  return;
282  case 1:
283  out = to_string(field.x());
284  return;
285  case 2:
286  out = to_string(field.y());
287  return;
288  case 3:
289  out = to_string(field.z());
290  return;
291  }
292  throw std::logic_error{__FILE__ " : " + to_string(__LINE__) +
293  " : Unreachable code reached"};
294  }
295 
296  const std::type_info&
297  DataFieldsInfo<Eigen::Quaternionf, void>::GetDataFieldType(std::size_t i)
298  {
299  return typeid(float);
300  }
301 
302  const std::vector<std::string>&
303  DataFieldsInfo<Eigen::Quaternionf, void>::GetFieldNames()
304  {
305  static const std::vector<std::string> result{"qw", "qx", "qy", "qz"};
306  return result;
307  }
308 
309  std::map<std::string, VariantBasePtr>
310  DataFieldsInfo<Eigen::Quaternionf, void>::ToVariants(const Eigen::Quaternionf& value,
311  const std::string& name,
312  const IceUtil::Time& timestamp,
313  const std::string& frame,
314  const std::string& agent)
315  {
316  if (!frame.empty())
317  {
318  return {{name, {new TimedVariant(FramedOrientation{value, frame, agent}, timestamp)}}};
319  }
320  ARMARX_CHECK_EXPRESSION(agent.empty()) << "No frame but an agent given";
321  return {{name, {new TimedVariant(Quaternion{value}, timestamp)}}};
322  }
323 } // namespace armarx::introspection
324 
325 //std::chrono::microseconds
326 namespace armarx::introspection
327 {
328  std::size_t
329  DataFieldsInfo<std::chrono::microseconds, void>::GetNumberOfFields()
330  {
331  return 1;
332  }
333 
334  void
335  DataFieldsInfo<std::chrono::microseconds, void>::GetDataFieldAs(
336  std::size_t i,
337  const std::chrono::microseconds& field,
338  long& out)
339  {
340  ARMARX_CHECK_EQUAL(i, 0);
341  out = field.count();
342  }
343 
344  void
345  DataFieldsInfo<std::chrono::microseconds, void>::GetDataFieldAs(
346  std::size_t i,
347  const std::chrono::microseconds& field,
348  std::string& out)
349  {
350  ARMARX_CHECK_EQUAL(i, 0);
351  out = to_string(field.count());
352  }
353 
354  const std::type_info&
355  GetDataFieldType(std::size_t i)
356  {
357  return typeid(long);
358  }
359 
360  std::map<std::string, VariantBasePtr>
361  DataFieldsInfo<std::chrono::microseconds, void>::ToVariants(std::chrono::microseconds value,
362  const std::string& name,
363  const IceUtil::Time& timestamp,
364  const std::string& frame,
365  const std::string& agent)
366  {
367  ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty())
368  << "There is no framed version of TimestampVariant";
369  return {{name, {new TimedVariant(TimestampVariant{value.count()}, timestamp)}}};
370  }
371 
372 } // namespace armarx::introspection
373 
374 //IceUtil::Time
375 namespace armarx::introspection
376 {
377  std::size_t
378  DataFieldsInfo<IceUtil::Time, void>::GetNumberOfFields()
379  {
380  return 1;
381  }
382 
383  void
384  DataFieldsInfo<IceUtil::Time, void>::GetDataFieldAs(std::size_t i,
385  const IceUtil::Time& field,
386  long& out)
387  {
388  ARMARX_CHECK_EQUAL(i, 0);
389  out = field.toMicroSeconds();
390  }
391 
392  void
393  DataFieldsInfo<IceUtil::Time, void>::GetDataFieldAs(std::size_t i,
394  const IceUtil::Time& field,
395  std::string& out)
396  {
397  ARMARX_CHECK_EQUAL(i, 0);
398  out = to_string(field.toMicroSeconds());
399  }
400 
401  const std::type_info&
402  DataFieldsInfo<IceUtil::Time, void>::GetDataFieldType(std::size_t i)
403  {
404  return typeid(long);
405  }
406 
407  std::map<std::string, VariantBasePtr>
408  DataFieldsInfo<IceUtil::Time, void>::ToVariants(IceUtil::Time value,
409  const std::string& name,
410  const IceUtil::Time& timestamp,
411  const std::string& frame,
412  const std::string& agent)
413  {
414  ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty())
415  << "There is no framed version of TimestampVariant";
416  return {{name, {new TimedVariant(TimestampVariant{value.toMicroSeconds()}, timestamp)}}};
417  }
418 } // namespace armarx::introspection
419 
420 //JointStatus
421 namespace armarx::introspection
422 {
423  std::size_t
424  DataFieldsInfo<JointStatus, void>::GetNumberOfFields()
425  {
426  return 4;
427  }
428 
429  void
430  DataFieldsInfo<JointStatus, void>::GetDataFieldAs(std::size_t i,
431  const JointStatus& field,
432  std::string& out)
433  {
434  ARMARX_CHECK_LESS(i, 4);
435  switch (i)
436  {
437  case 0:
438  out = to_string(field.error);
439  return;
440  case 1:
441  out = to_string(field.operation);
442  return;
443  case 2:
444  out = to_string(field.enabled);
445  return;
446  case 3:
447  out = to_string(field.emergencyStop);
448  return;
449  }
450  throw std::logic_error{__FILE__ " : " + to_string(__LINE__) +
451  " : Unreachable code reached"};
452  }
453 
454  void
455  DataFieldsInfo<JointStatus, void>::GetDataFieldAs(std::size_t i,
456  const JointStatus& field,
457  Ice::Int& out)
458  {
459  ARMARX_CHECK_EXPRESSION(i == 0 || i == 1);
460  switch (i)
461  {
462  case 0:
463  out = field.enabled;
464  return;
465  case 1:
466  out = field.emergencyStop;
467  return;
468  }
469  throw std::logic_error{__FILE__ " : " + to_string(__LINE__) +
470  " : Unreachable code reached"};
471  }
472 
473  void
474  DataFieldsInfo<JointStatus, void>::GetDataFieldAs(std::size_t i,
475  const JointStatus& field,
476  bool& out)
477  {
478  ARMARX_CHECK_EXPRESSION(i == 2 || i == 3);
479  switch (i)
480  {
481  case 2:
482  out = static_cast<Ice::Int>(field.error);
483  return;
484  case 3:
485  out = static_cast<Ice::Int>(field.operation);
486  return;
487  }
488  throw std::logic_error{__FILE__ " : " + to_string(__LINE__) +
489  " : Unreachable code reached"};
490  }
491 
492  const std::type_info&
493  DataFieldsInfo<JointStatus, void>::GetDataFieldType(std::size_t i)
494  {
495  ARMARX_CHECK_LESS(i, 4);
496  switch (i)
497  {
498  case 0:
499  return typeid(Ice::Int);
500  case 1:
501  return typeid(Ice::Int);
502  case 2:
503  return typeid(bool);
504  case 3:
505  return typeid(bool);
506  }
507  throw std::logic_error{__FILE__ " : " + to_string(__LINE__) +
508  " : Unreachable code reached"};
509  }
510 
511  const std::vector<std::string>&
512  DataFieldsInfo<JointStatus, void>::GetFieldNames()
513  {
514  static const std::vector<std::string> result{
515  "error", "operation", "enabled", "emergencyStop"};
516  return result;
517  }
518 
519  std::map<std::string, VariantBasePtr>
520  DataFieldsInfo<JointStatus, void>::ToVariants(const JointStatus& value,
521  const std::string& name,
522  const IceUtil::Time& timestamp,
523  const std::string& frame,
524  const std::string& agent)
525  {
526  ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty())
527  << "There is no framed version for JointStatus";
528  static const auto fnames = GetFieldNames();
529  return {{name + fnames.at(0), {new TimedVariant{to_string(value.error), timestamp}}},
530  {name + fnames.at(1), {new TimedVariant{to_string(value.operation), timestamp}}},
531  {name + fnames.at(2), {new TimedVariant{value.enabled, timestamp}}},
532  {name + fnames.at(3), {new TimedVariant{value.emergencyStop, timestamp}}}};
533  }
534 } // namespace armarx::introspection
armarx::FramedPose
The FramedPose class.
Definition: FramedPose.h:258
armarx::introspection::Int
Int
Definition: DataFieldsInfo.cpp:158
Pose.h
DataFieldsInfo.h
armarx::TimestampVariant
Definition: TimestampVariant.h:54
ARMARX_CHECK_LESS
#define ARMARX_CHECK_LESS(lhs, rhs)
This macro evaluates whether lhs is less (<) than rhs and if it turns out to be false it will throw a...
Definition: ExpressionException.h:102
OrientedPoint.h
armarx::introspection::DataFieldsInfo
Definition: DataFieldsInfo.h:37
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:926
FramedPose.h
armarx::FramedPosition
The FramedPosition class.
Definition: FramedPose.h:142
TimestampVariant.h
armarx::Vector3
The Vector3 class.
Definition: Pose.h:112
armarx::FramedOrientation
The FramedOrientation class.
Definition: FramedPose.h:199
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:40
armarx::introspection::GetDataFieldType
const std::type_info & GetDataFieldType(std::size_t i)
Definition: DataFieldsInfo.cpp:355
armarx::introspection::Double
Double
Definition: DataFieldsInfo.cpp:153
armarx::Pose
The Pose class.
Definition: Pose.h:242
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
float
#define float
Definition: 16_Level.h:22
armarx::Quaternion< float, 0 >
armarx::introspection::Float
Float
Definition: DataFieldsInfo.cpp:147
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition: ExpressionException.h:130
armarx::introspection::make_DataFieldsInfo_for_eigen_vector
make_DataFieldsInfo_for_eigen_vector(f, Float, 2) make_DataFieldsInfo_for_eigen_vector(f
armarx::introspection
Definition: ClassMemberInfo.h:28
armarx::TimedVariant
Definition: TimedVariant.h:40