Fawkes API Fawkes Development Version
IMUInterface.cpp
1
2/***************************************************************************
3 * IMUInterface.cpp - Fawkes BlackBoard Interface - IMUInterface
4 *
5 * Templated created: Thu Oct 12 10:49:19 2006
6 * Copyright 2014 Tim Niemueller
7 *
8 ****************************************************************************/
9
10/* This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version. A runtime exception applies to
14 * this software (see LICENSE.GPL_WRE file mentioned below for details).
15 *
16 * This program is distributed in the hope that it will be useful,
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 * GNU Library General Public License for more details.
20 *
21 * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
22 */
23
24#include <interfaces/IMUInterface.h>
25
26#include <core/exceptions/software.h>
27
28#include <map>
29#include <string>
30#include <cstring>
31#include <cstdlib>
32
33namespace fawkes {
34
35/** @class IMUInterface <interfaces/IMUInterface.h>
36 * IMUInterface Fawkes BlackBoard Interface.
37 *
38 This interface provides access to data of inertial measurement
39 units. It is based on the sensor_msgs/Imu data type for
40 compatibility.
41
42 Accelerations should be in m/s^2 (not in g's), and rotational
43 velocity should be in rad/sec.
44
45 If the covariance of the measurement is known, it should be
46 filled in (if all you know is the variance of each measurement,
47 e.g. from the datasheet, just put those along the diagonal). A
48 covariance matrix of all zeros will be interpreted as
49 "covariance unknown", and to use the data a covariance will have
50 to be assumed or gotten from some other source.
51
52 If you have no estimate for one of the data elements (e.g. your
53 IMU doesn't produce an orientation # estimate), please set
54 element 0 of the associated covariance matrix to -1. If you are
55 interpreting this message, please check for a value of -1 in the
56 first element of each covariance matrix, and disregard the
57 associated estimate.
58
59 * @ingroup FawkesInterfaces
60 */
61
62
63
64/** Constructor */
65IMUInterface::IMUInterface() : Interface()
66{
67 data_size = sizeof(IMUInterface_data_t);
68 data_ptr = malloc(data_size);
69 data = (IMUInterface_data_t *)data_ptr;
70 data_ts = (interface_data_ts_t *)data_ptr;
71 memset(data_ptr, 0, data_size);
72 add_fieldinfo(IFT_STRING, "frame", 32, data->frame);
73 add_fieldinfo(IFT_FLOAT, "orientation", 4, &data->orientation);
74 add_fieldinfo(IFT_DOUBLE, "orientation_covariance", 9, &data->orientation_covariance);
75 add_fieldinfo(IFT_FLOAT, "angular_velocity", 3, &data->angular_velocity);
76 add_fieldinfo(IFT_DOUBLE, "angular_velocity_covariance", 9, &data->angular_velocity_covariance);
77 add_fieldinfo(IFT_FLOAT, "linear_acceleration", 3, &data->linear_acceleration);
78 add_fieldinfo(IFT_DOUBLE, "linear_acceleration_covariance", 9, &data->linear_acceleration_covariance);
79 unsigned char tmp_hash[] = {0x9d, 0xf6, 0xde, 0x9d, 0x32, 0xe3, 0xf, 0x11, 0xac, 0xdc, 0x5d, 0x92, 0x27, 0x89, 0x27, 0x7e};
80 set_hash(tmp_hash);
81}
82
83/** Destructor */
84IMUInterface::~IMUInterface()
85{
86 free(data_ptr);
87}
88/* Methods */
89/** Get frame value.
90 *
91 Coordinate frame in which the data is presented.
92
93 * @return frame value
94 */
95char *
96IMUInterface::frame() const
97{
98 return data->frame;
99}
100
101/** Get maximum length of frame value.
102 * @return length of frame value, can be length of the array or number of
103 * maximum number of characters for a string
104 */
105size_t
106IMUInterface::maxlenof_frame() const
107{
108 return 32;
109}
110
111/** Set frame value.
112 *
113 Coordinate frame in which the data is presented.
114
115 * @param new_frame new frame value
116 */
117void
118IMUInterface::set_frame(const char * new_frame)
119{
120 set_field(data->frame, new_frame);
121}
122
123/** Get orientation value.
124 *
125 Rotation quaternion ordered as (x, y, z, w).
126
127 * @return orientation value
128 */
129float *
130IMUInterface::orientation() const
131{
132 return data->orientation;
133}
134
135/** Get orientation value at given index.
136 *
137 Rotation quaternion ordered as (x, y, z, w).
138
139 * @param index index of value
140 * @return orientation value
141 * @exception Exception thrown if index is out of bounds
142 */
143float
144IMUInterface::orientation(unsigned int index) const
145{
146 if (index > 3) {
147 throw Exception("Index value %u out of bounds (0..3)", index);
148 }
149 return data->orientation[index];
150}
151
152/** Get maximum length of orientation value.
153 * @return length of orientation value, can be length of the array or number of
154 * maximum number of characters for a string
155 */
156size_t
157IMUInterface::maxlenof_orientation() const
158{
159 return 4;
160}
161
162/** Set orientation value.
163 *
164 Rotation quaternion ordered as (x, y, z, w).
165
166 * @param new_orientation new orientation value
167 */
168void
169IMUInterface::set_orientation(const float * new_orientation)
170{
171 set_field(data->orientation, new_orientation);
172}
173
174/** Set orientation value at given index.
175 *
176 Rotation quaternion ordered as (x, y, z, w).
177
178 * @param new_orientation new orientation value
179 * @param index index for of the value
180 */
181void
182IMUInterface::set_orientation(unsigned int index, const float new_orientation)
183{
184 set_field(data->orientation, index, new_orientation);
185}
186/** Get orientation_covariance value.
187 *
188 Covariance of orientation, row major about x, y, z axes.
189
190 * @return orientation_covariance value
191 */
192double *
193IMUInterface::orientation_covariance() const
194{
195 return data->orientation_covariance;
196}
197
198/** Get orientation_covariance value at given index.
199 *
200 Covariance of orientation, row major about x, y, z axes.
201
202 * @param index index of value
203 * @return orientation_covariance value
204 * @exception Exception thrown if index is out of bounds
205 */
206double
207IMUInterface::orientation_covariance(unsigned int index) const
208{
209 if (index > 8) {
210 throw Exception("Index value %u out of bounds (0..8)", index);
211 }
212 return data->orientation_covariance[index];
213}
214
215/** Get maximum length of orientation_covariance value.
216 * @return length of orientation_covariance value, can be length of the array or number of
217 * maximum number of characters for a string
218 */
219size_t
220IMUInterface::maxlenof_orientation_covariance() const
221{
222 return 9;
223}
224
225/** Set orientation_covariance value.
226 *
227 Covariance of orientation, row major about x, y, z axes.
228
229 * @param new_orientation_covariance new orientation_covariance value
230 */
231void
232IMUInterface::set_orientation_covariance(const double * new_orientation_covariance)
233{
234 set_field(data->orientation_covariance, new_orientation_covariance);
235}
236
237/** Set orientation_covariance value at given index.
238 *
239 Covariance of orientation, row major about x, y, z axes.
240
241 * @param new_orientation_covariance new orientation_covariance value
242 * @param index index for of the value
243 */
244void
245IMUInterface::set_orientation_covariance(unsigned int index, const double new_orientation_covariance)
246{
247 set_field(data->orientation_covariance, index, new_orientation_covariance);
248}
249/** Get angular_velocity value.
250 *
251 Angular velocities ordered as (x, y, z).
252
253 * @return angular_velocity value
254 */
255float *
256IMUInterface::angular_velocity() const
257{
258 return data->angular_velocity;
259}
260
261/** Get angular_velocity value at given index.
262 *
263 Angular velocities ordered as (x, y, z).
264
265 * @param index index of value
266 * @return angular_velocity value
267 * @exception Exception thrown if index is out of bounds
268 */
269float
270IMUInterface::angular_velocity(unsigned int index) const
271{
272 if (index > 2) {
273 throw Exception("Index value %u out of bounds (0..2)", index);
274 }
275 return data->angular_velocity[index];
276}
277
278/** Get maximum length of angular_velocity value.
279 * @return length of angular_velocity value, can be length of the array or number of
280 * maximum number of characters for a string
281 */
282size_t
283IMUInterface::maxlenof_angular_velocity() const
284{
285 return 3;
286}
287
288/** Set angular_velocity value.
289 *
290 Angular velocities ordered as (x, y, z).
291
292 * @param new_angular_velocity new angular_velocity value
293 */
294void
295IMUInterface::set_angular_velocity(const float * new_angular_velocity)
296{
297 set_field(data->angular_velocity, new_angular_velocity);
298}
299
300/** Set angular_velocity value at given index.
301 *
302 Angular velocities ordered as (x, y, z).
303
304 * @param new_angular_velocity new angular_velocity value
305 * @param index index for of the value
306 */
307void
308IMUInterface::set_angular_velocity(unsigned int index, const float new_angular_velocity)
309{
310 set_field(data->angular_velocity, index, new_angular_velocity);
311}
312/** Get angular_velocity_covariance value.
313 *
314 Covariance of angular velocity, row major about x, y, z axes.
315
316 * @return angular_velocity_covariance value
317 */
318double *
319IMUInterface::angular_velocity_covariance() const
320{
321 return data->angular_velocity_covariance;
322}
323
324/** Get angular_velocity_covariance value at given index.
325 *
326 Covariance of angular velocity, row major about x, y, z axes.
327
328 * @param index index of value
329 * @return angular_velocity_covariance value
330 * @exception Exception thrown if index is out of bounds
331 */
332double
333IMUInterface::angular_velocity_covariance(unsigned int index) const
334{
335 if (index > 8) {
336 throw Exception("Index value %u out of bounds (0..8)", index);
337 }
338 return data->angular_velocity_covariance[index];
339}
340
341/** Get maximum length of angular_velocity_covariance value.
342 * @return length of angular_velocity_covariance value, can be length of the array or number of
343 * maximum number of characters for a string
344 */
345size_t
346IMUInterface::maxlenof_angular_velocity_covariance() const
347{
348 return 9;
349}
350
351/** Set angular_velocity_covariance value.
352 *
353 Covariance of angular velocity, row major about x, y, z axes.
354
355 * @param new_angular_velocity_covariance new angular_velocity_covariance value
356 */
357void
358IMUInterface::set_angular_velocity_covariance(const double * new_angular_velocity_covariance)
359{
360 set_field(data->angular_velocity_covariance, new_angular_velocity_covariance);
361}
362
363/** Set angular_velocity_covariance value at given index.
364 *
365 Covariance of angular velocity, row major about x, y, z axes.
366
367 * @param new_angular_velocity_covariance new angular_velocity_covariance value
368 * @param index index for of the value
369 */
370void
371IMUInterface::set_angular_velocity_covariance(unsigned int index, const double new_angular_velocity_covariance)
372{
373 set_field(data->angular_velocity_covariance, index, new_angular_velocity_covariance);
374}
375/** Get linear_acceleration value.
376 *
377 Linear acceleration ordered as (x, y, z).
378
379 * @return linear_acceleration value
380 */
381float *
382IMUInterface::linear_acceleration() const
383{
384 return data->linear_acceleration;
385}
386
387/** Get linear_acceleration value at given index.
388 *
389 Linear acceleration ordered as (x, y, z).
390
391 * @param index index of value
392 * @return linear_acceleration value
393 * @exception Exception thrown if index is out of bounds
394 */
395float
396IMUInterface::linear_acceleration(unsigned int index) const
397{
398 if (index > 2) {
399 throw Exception("Index value %u out of bounds (0..2)", index);
400 }
401 return data->linear_acceleration[index];
402}
403
404/** Get maximum length of linear_acceleration value.
405 * @return length of linear_acceleration value, can be length of the array or number of
406 * maximum number of characters for a string
407 */
408size_t
409IMUInterface::maxlenof_linear_acceleration() const
410{
411 return 3;
412}
413
414/** Set linear_acceleration value.
415 *
416 Linear acceleration ordered as (x, y, z).
417
418 * @param new_linear_acceleration new linear_acceleration value
419 */
420void
421IMUInterface::set_linear_acceleration(const float * new_linear_acceleration)
422{
423 set_field(data->linear_acceleration, new_linear_acceleration);
424}
425
426/** Set linear_acceleration value at given index.
427 *
428 Linear acceleration ordered as (x, y, z).
429
430 * @param new_linear_acceleration new linear_acceleration value
431 * @param index index for of the value
432 */
433void
434IMUInterface::set_linear_acceleration(unsigned int index, const float new_linear_acceleration)
435{
436 set_field(data->linear_acceleration, index, new_linear_acceleration);
437}
438/** Get linear_acceleration_covariance value.
439 *
440 Covariance of linear acceleration, row major about x, y, z axes.
441
442 * @return linear_acceleration_covariance value
443 */
444double *
445IMUInterface::linear_acceleration_covariance() const
446{
447 return data->linear_acceleration_covariance;
448}
449
450/** Get linear_acceleration_covariance value at given index.
451 *
452 Covariance of linear acceleration, row major about x, y, z axes.
453
454 * @param index index of value
455 * @return linear_acceleration_covariance value
456 * @exception Exception thrown if index is out of bounds
457 */
458double
459IMUInterface::linear_acceleration_covariance(unsigned int index) const
460{
461 if (index > 8) {
462 throw Exception("Index value %u out of bounds (0..8)", index);
463 }
464 return data->linear_acceleration_covariance[index];
465}
466
467/** Get maximum length of linear_acceleration_covariance value.
468 * @return length of linear_acceleration_covariance value, can be length of the array or number of
469 * maximum number of characters for a string
470 */
471size_t
472IMUInterface::maxlenof_linear_acceleration_covariance() const
473{
474 return 9;
475}
476
477/** Set linear_acceleration_covariance value.
478 *
479 Covariance of linear acceleration, row major about x, y, z axes.
480
481 * @param new_linear_acceleration_covariance new linear_acceleration_covariance value
482 */
483void
484IMUInterface::set_linear_acceleration_covariance(const double * new_linear_acceleration_covariance)
485{
486 set_field(data->linear_acceleration_covariance, new_linear_acceleration_covariance);
487}
488
489/** Set linear_acceleration_covariance value at given index.
490 *
491 Covariance of linear acceleration, row major about x, y, z axes.
492
493 * @param new_linear_acceleration_covariance new linear_acceleration_covariance value
494 * @param index index for of the value
495 */
496void
497IMUInterface::set_linear_acceleration_covariance(unsigned int index, const double new_linear_acceleration_covariance)
498{
499 set_field(data->linear_acceleration_covariance, index, new_linear_acceleration_covariance);
500}
501/* =========== message create =========== */
502Message *
503IMUInterface::create_message(const char *type) const
504{
505 throw UnknownTypeException("The given type '%s' does not match any known "
506 "message type for this interface type.", type);
507}
508
509
510/** Copy values from other interface.
511 * @param other other interface to copy values from
512 */
513void
514IMUInterface::copy_values(const Interface *other)
515{
516 const IMUInterface *oi = dynamic_cast<const IMUInterface *>(other);
517 if (oi == NULL) {
518 throw TypeMismatchException("Can only copy values from interface of same type (%s vs. %s)",
519 type(), other->type());
520 }
521 memcpy(data, oi->data, sizeof(IMUInterface_data_t));
522}
523
524const char *
525IMUInterface::enum_tostring(const char *enumtype, int val) const
526{
527 throw UnknownTypeException("Unknown enum type %s", enumtype);
528}
529
530/* =========== messages =========== */
531/** Check if message is valid and can be enqueued.
532 * @param message Message to check
533 * @return true if the message is valid, false otherwise.
534 */
535bool
536IMUInterface::message_valid(const Message *message) const
537{
538 return false;
539}
540
541/// @cond INTERNALS
542EXPORT_INTERFACE(IMUInterface)
543/// @endcond
544
545
546} // end namespace fawkes
Base class for exceptions in Fawkes.
Definition: exception.h:36
IMUInterface Fawkes BlackBoard Interface.
Definition: IMUInterface.h:34
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
const char * type() const
Get type of interface.
Definition: interface.cpp:652
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
Fawkes library namespace.