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
37namespace armarx::introspection
38{
39 std::size_t
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
93namespace 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
167namespace armarx::introspection
168{
169 std::size_t
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
238namespace armarx::introspection
239{
240 std::size_t
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
326namespace armarx::introspection
327{
328 std::size_t
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
375namespace armarx::introspection
376{
377 std::size_t
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
421namespace armarx::introspection
422{
423 std::size_t
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
float
#define float
Definition 16_Level.h:22
timestamp
std::string timestamp()
Definition CartographerAdapter.cpp:86
make_DataFieldsInfo_for_eigen_vector
#define make_DataFieldsInfo_for_eigen_vector(Type, TypeName, Num)
Definition DataFieldsInfo.cpp:95
armarx::FramedOrientation
The FramedOrientation class.
Definition FramedPose.h:216
armarx::FramedPose
The FramedPose class.
Definition FramedPose.h:281
armarx::FramedPosition
The FramedPosition class.
Definition FramedPose.h:158
armarx::Pose
The Pose class.
Definition Pose.h:243
armarx::Quaternion
The Quaternion class.
Definition Pose.h:174
armarx::TimestampVariant
Implements a Variant type for timestamps.
Definition TimestampVariant.h:55
armarx::Vector3
The Vector3 class.
Definition Pose.h:113
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
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
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
Eigen::Quaternionf
Quaternion< float, 0 > Quaternionf
Definition EigenForwardDeclarations.h:61
armarx::introspection::make_DataFieldsInfo_for_eigen_vector
make_DataFieldsInfo_for_eigen_vector(f, Float, 2) make_DataFieldsInfo_for_eigen_vector(f
armarx::introspection::GetDataFieldType
const std::type_info & GetDataFieldType(std::size_t i)
Definition DataFieldsInfo.cpp:355
armarx::to_string
const std::string & to_string(const std::string &s)
Definition StringHelpers.h:41
armarx::introspection::DataFieldsInfo< JointStatus, void >::GetFieldNames
static const std::vector< std::string > & GetFieldNames()
Definition DataFieldsInfo.cpp:512