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 
33 namespace 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 */
65 IMUInterface::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 */
84 IMUInterface::~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  */
95 char *
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  */
105 size_t
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  */
117 void
118 IMUInterface::set_frame(const char * new_frame)
119 {
120  data_changed |= change_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  */
129 float *
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  */
143 float
144 IMUInterface::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  */
156 size_t
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  */
168 void
169 IMUInterface::set_orientation(const float * new_orientation)
170 {
171  data_changed |= change_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  */
181 void
182 IMUInterface::set_orientation(unsigned int index, const float new_orientation)
183 {
184  data_changed |= change_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  */
192 double *
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  */
206 double
207 IMUInterface::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  */
219 size_t
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  */
231 void
232 IMUInterface::set_orientation_covariance(const double * new_orientation_covariance)
233 {
234  data_changed |= change_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  */
244 void
245 IMUInterface::set_orientation_covariance(unsigned int index, const double new_orientation_covariance)
246 {
247  data_changed |= change_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  */
255 float *
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  */
269 float
270 IMUInterface::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  */
282 size_t
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  */
294 void
295 IMUInterface::set_angular_velocity(const float * new_angular_velocity)
296 {
297  data_changed |= change_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  */
307 void
308 IMUInterface::set_angular_velocity(unsigned int index, const float new_angular_velocity)
309 {
310  data_changed |= change_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  */
318 double *
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  */
332 double
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  */
345 size_t
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  */
357 void
358 IMUInterface::set_angular_velocity_covariance(const double * new_angular_velocity_covariance)
359 {
360  data_changed |= change_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  */
370 void
371 IMUInterface::set_angular_velocity_covariance(unsigned int index, const double new_angular_velocity_covariance)
372 {
373  data_changed |= change_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  */
381 float *
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  */
395 float
396 IMUInterface::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  */
408 size_t
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  */
420 void
421 IMUInterface::set_linear_acceleration(const float * new_linear_acceleration)
422 {
423  data_changed |= change_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  */
433 void
434 IMUInterface::set_linear_acceleration(unsigned int index, const float new_linear_acceleration)
435 {
436  data_changed |= change_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  */
444 double *
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  */
458 double
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  */
471 size_t
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  */
483 void
484 IMUInterface::set_linear_acceleration_covariance(const double * new_linear_acceleration_covariance)
485 {
486  data_changed |= change_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  */
496 void
497 IMUInterface::set_linear_acceleration_covariance(unsigned int index, const double new_linear_acceleration_covariance)
498 {
499  data_changed |= change_field(data->linear_acceleration_covariance, index, new_linear_acceleration_covariance);
500 }
501 /* =========== message create =========== */
502 Message *
503 IMUInterface::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  */
513 void
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 
524 const char *
525 IMUInterface::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  */
535 bool
537 {
538  return false;
539 }
540 
541 /// @cond INTERNALS
542 EXPORT_INTERFACE(IMUInterface)
543 /// @endcond
544 
545 
546 } // end namespace fawkes
fawkes::IMUInterface::message_valid
virtual bool message_valid(const Message *message) const
Check if message is valid and can be enqueued.
Definition: IMUInterface.cpp:536
fawkes::Interface::data_ptr
void * data_ptr
Pointer to local memory storage.
Definition: interface.h:224
fawkes::IMUInterface::set_orientation_covariance
void set_orientation_covariance(unsigned int index, const double new_orientation_covariance)
Set orientation_covariance value at given index.
Definition: IMUInterface.cpp:245
fawkes::IMUInterface::set_frame
void set_frame(const char *new_frame)
Set frame value.
Definition: IMUInterface.cpp:118
fawkes::IMUInterface::maxlenof_angular_velocity
size_t maxlenof_angular_velocity() const
Get maximum length of angular_velocity value.
Definition: IMUInterface.cpp:283
fawkes::IMUInterface::linear_acceleration_covariance
double * linear_acceleration_covariance() const
Get linear_acceleration_covariance value.
Definition: IMUInterface.cpp:445
fawkes::IMUInterface::set_angular_velocity_covariance
void set_angular_velocity_covariance(unsigned int index, const double new_angular_velocity_covariance)
Set angular_velocity_covariance value at given index.
Definition: IMUInterface.cpp:371
fawkes::IMUInterface
IMUInterface Fawkes BlackBoard Interface.
Definition: IMUInterface.h:34
fawkes::Message
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:45
fawkes::IMUInterface::angular_velocity
float * angular_velocity() const
Get angular_velocity value.
Definition: IMUInterface.cpp:256
fawkes::IMUInterface::copy_values
virtual void copy_values(const Interface *other)
Copy values from other interface.
Definition: IMUInterface.cpp:514
fawkes::IMUInterface::maxlenof_linear_acceleration_covariance
size_t maxlenof_linear_acceleration_covariance() const
Get maximum length of linear_acceleration_covariance value.
Definition: IMUInterface.cpp:472
fawkes::IFT_FLOAT
@ IFT_FLOAT
float field
Definition: types.h:46
fawkes::IMUInterface::orientation_covariance
double * orientation_covariance() const
Get orientation_covariance value.
Definition: IMUInterface.cpp:193
fawkes::IMUInterface::frame
char * frame() const
Get frame value.
Definition: IMUInterface.cpp:96
fawkes::Interface::type
const char * type() const
Get type of interface.
Definition: interface.cpp:643
fawkes::Interface::add_fieldinfo
void add_fieldinfo(interface_fieldtype_t type, const char *name, size_t length, void *value, const char *enumtype=0, const interface_enum_map_t *enum_map=0)
Add an entry to the field info list.
Definition: interface.cpp:336
fawkes::Interface::data_ts
interface_data_ts_t * data_ts
Pointer to data casted to timestamp struct.
Definition: interface.h:228
fawkes::IMUInterface::maxlenof_orientation
size_t maxlenof_orientation() const
Get maximum length of orientation value.
Definition: IMUInterface.cpp:157
fawkes::IMUInterface::enum_tostring
virtual const char * enum_tostring(const char *enumtype, int val) const
Convert arbitrary enum value to string.
Definition: IMUInterface.cpp:525
fawkes::IMUInterface::create_message
virtual Message * create_message(const char *type) const
Create message based on type name.
Definition: IMUInterface.cpp:503
fawkes::TypeMismatchException
Type mismatch.
Definition: software.h:44
fawkes::Interface::data_changed
bool data_changed
Indicator if data has changed.
Definition: interface.h:226
fawkes::IMUInterface::set_linear_acceleration
void set_linear_acceleration(unsigned int index, const float new_linear_acceleration)
Set linear_acceleration value at given index.
Definition: IMUInterface.cpp:434
fawkes::IMUInterface::linear_acceleration
float * linear_acceleration() const
Get linear_acceleration value.
Definition: IMUInterface.cpp:382
fawkes::IMUInterface::orientation
float * orientation() const
Get orientation value.
Definition: IMUInterface.cpp:130
fawkes::UnknownTypeException
Unknown type.
Definition: software.h:50
fawkes
Fawkes library namespace.
fawkes::Interface::set_hash
void set_hash(unsigned char *ihash)
Set hash.
Definition: interface.cpp:316
fawkes::Interface
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
fawkes::IMUInterface::angular_velocity_covariance
double * angular_velocity_covariance() const
Get angular_velocity_covariance value.
Definition: IMUInterface.cpp:319
fawkes::IMUInterface::set_orientation
void set_orientation(unsigned int index, const float new_orientation)
Set orientation value at given index.
Definition: IMUInterface.cpp:182
fawkes::IMUInterface::maxlenof_angular_velocity_covariance
size_t maxlenof_angular_velocity_covariance() const
Get maximum length of angular_velocity_covariance value.
Definition: IMUInterface.cpp:346
fawkes::IMUInterface::maxlenof_frame
size_t maxlenof_frame() const
Get maximum length of frame value.
Definition: IMUInterface.cpp:106
fawkes::IFT_DOUBLE
@ IFT_DOUBLE
double field
Definition: types.h:47
fawkes::Interface::data_size
unsigned int data_size
Minimal data size to hold data storage.
Definition: interface.h:225
fawkes::IFT_STRING
@ IFT_STRING
string field
Definition: types.h:48
fawkes::IMUInterface::maxlenof_orientation_covariance
size_t maxlenof_orientation_covariance() const
Get maximum length of orientation_covariance value.
Definition: IMUInterface.cpp:220
fawkes::IMUInterface::maxlenof_linear_acceleration
size_t maxlenof_linear_acceleration() const
Get maximum length of linear_acceleration value.
Definition: IMUInterface.cpp:409
fawkes::IMUInterface::set_linear_acceleration_covariance
void set_linear_acceleration_covariance(unsigned int index, const double new_linear_acceleration_covariance)
Set linear_acceleration_covariance value at given index.
Definition: IMUInterface.cpp:497
fawkes::change_field
bool change_field(FieldT &field, const DataT &value)
Set a field and return whether it changed.
Definition: message.h:167
fawkes::IMUInterface::set_angular_velocity
void set_angular_velocity(unsigned int index, const float new_angular_velocity)
Set angular_velocity value at given index.
Definition: IMUInterface.cpp:308
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36