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 *
96 IMUInterface::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  */
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  strncpy(data->frame, new_frame, sizeof(data->frame)-1);
121  data->frame[sizeof(data->frame)-1] = 0;
122  data_changed = true;
123 }
124 
125 /** Get orientation value.
126  *
127  Rotation quaternion ordered as (x, y, z, w).
128 
129  * @return orientation value
130  */
131 float *
133 {
134  return data->orientation;
135 }
136 
137 /** Get orientation value at given index.
138  *
139  Rotation quaternion ordered as (x, y, z, w).
140 
141  * @param index index of value
142  * @return orientation value
143  * @exception Exception thrown if index is out of bounds
144  */
145 float
146 IMUInterface::orientation(unsigned int index) const
147 {
148  if (index > 3) {
149  throw Exception("Index value %u out of bounds (0..3)", index);
150  }
151  return data->orientation[index];
152 }
153 
154 /** Get maximum length of orientation value.
155  * @return length of orientation value, can be length of the array or number of
156  * maximum number of characters for a string
157  */
158 size_t
160 {
161  return 4;
162 }
163 
164 /** Set orientation value.
165  *
166  Rotation quaternion ordered as (x, y, z, w).
167 
168  * @param new_orientation new orientation value
169  */
170 void
171 IMUInterface::set_orientation(const float * new_orientation)
172 {
173  memcpy(data->orientation, new_orientation, sizeof(float) * 4);
174  data_changed = true;
175 }
176 
177 /** Set orientation value at given index.
178  *
179  Rotation quaternion ordered as (x, y, z, w).
180 
181  * @param new_orientation new orientation value
182  * @param index index for of the value
183  */
184 void
185 IMUInterface::set_orientation(unsigned int index, const float new_orientation)
186 {
187  if (index > 3) {
188  throw Exception("Index value %u out of bounds (0..3)", index);
189  }
190  data->orientation[index] = new_orientation;
191  data_changed = true;
192 }
193 /** Get orientation_covariance value.
194  *
195  Covariance of orientation, row major about x, y, z axes.
196 
197  * @return orientation_covariance value
198  */
199 double *
201 {
202  return data->orientation_covariance;
203 }
204 
205 /** Get orientation_covariance value at given index.
206  *
207  Covariance of orientation, row major about x, y, z axes.
208 
209  * @param index index of value
210  * @return orientation_covariance value
211  * @exception Exception thrown if index is out of bounds
212  */
213 double
214 IMUInterface::orientation_covariance(unsigned int index) const
215 {
216  if (index > 8) {
217  throw Exception("Index value %u out of bounds (0..8)", index);
218  }
219  return data->orientation_covariance[index];
220 }
221 
222 /** Get maximum length of orientation_covariance value.
223  * @return length of orientation_covariance value, can be length of the array or number of
224  * maximum number of characters for a string
225  */
226 size_t
228 {
229  return 9;
230 }
231 
232 /** Set orientation_covariance value.
233  *
234  Covariance of orientation, row major about x, y, z axes.
235 
236  * @param new_orientation_covariance new orientation_covariance value
237  */
238 void
239 IMUInterface::set_orientation_covariance(const double * new_orientation_covariance)
240 {
241  memcpy(data->orientation_covariance, new_orientation_covariance, sizeof(double) * 9);
242  data_changed = true;
243 }
244 
245 /** Set orientation_covariance value at given index.
246  *
247  Covariance of orientation, row major about x, y, z axes.
248 
249  * @param new_orientation_covariance new orientation_covariance value
250  * @param index index for of the value
251  */
252 void
253 IMUInterface::set_orientation_covariance(unsigned int index, const double new_orientation_covariance)
254 {
255  if (index > 8) {
256  throw Exception("Index value %u out of bounds (0..8)", index);
257  }
258  data->orientation_covariance[index] = new_orientation_covariance;
259  data_changed = true;
260 }
261 /** Get angular_velocity value.
262  *
263  Angular velocities ordered as (x, y, z).
264 
265  * @return angular_velocity value
266  */
267 float *
269 {
270  return data->angular_velocity;
271 }
272 
273 /** Get angular_velocity value at given index.
274  *
275  Angular velocities ordered as (x, y, z).
276 
277  * @param index index of value
278  * @return angular_velocity value
279  * @exception Exception thrown if index is out of bounds
280  */
281 float
282 IMUInterface::angular_velocity(unsigned int index) const
283 {
284  if (index > 2) {
285  throw Exception("Index value %u out of bounds (0..2)", index);
286  }
287  return data->angular_velocity[index];
288 }
289 
290 /** Get maximum length of angular_velocity value.
291  * @return length of angular_velocity value, can be length of the array or number of
292  * maximum number of characters for a string
293  */
294 size_t
296 {
297  return 3;
298 }
299 
300 /** Set angular_velocity value.
301  *
302  Angular velocities ordered as (x, y, z).
303 
304  * @param new_angular_velocity new angular_velocity value
305  */
306 void
307 IMUInterface::set_angular_velocity(const float * new_angular_velocity)
308 {
309  memcpy(data->angular_velocity, new_angular_velocity, sizeof(float) * 3);
310  data_changed = true;
311 }
312 
313 /** Set angular_velocity value at given index.
314  *
315  Angular velocities ordered as (x, y, z).
316 
317  * @param new_angular_velocity new angular_velocity value
318  * @param index index for of the value
319  */
320 void
321 IMUInterface::set_angular_velocity(unsigned int index, const float new_angular_velocity)
322 {
323  if (index > 2) {
324  throw Exception("Index value %u out of bounds (0..2)", index);
325  }
326  data->angular_velocity[index] = new_angular_velocity;
327  data_changed = true;
328 }
329 /** Get angular_velocity_covariance value.
330  *
331  Covariance of angular velocity, row major about x, y, z axes.
332 
333  * @return angular_velocity_covariance value
334  */
335 double *
337 {
338  return data->angular_velocity_covariance;
339 }
340 
341 /** Get angular_velocity_covariance value at given index.
342  *
343  Covariance of angular velocity, row major about x, y, z axes.
344 
345  * @param index index of value
346  * @return angular_velocity_covariance value
347  * @exception Exception thrown if index is out of bounds
348  */
349 double
350 IMUInterface::angular_velocity_covariance(unsigned int index) const
351 {
352  if (index > 8) {
353  throw Exception("Index value %u out of bounds (0..8)", index);
354  }
355  return data->angular_velocity_covariance[index];
356 }
357 
358 /** Get maximum length of angular_velocity_covariance value.
359  * @return length of angular_velocity_covariance value, can be length of the array or number of
360  * maximum number of characters for a string
361  */
362 size_t
364 {
365  return 9;
366 }
367 
368 /** Set angular_velocity_covariance value.
369  *
370  Covariance of angular velocity, row major about x, y, z axes.
371 
372  * @param new_angular_velocity_covariance new angular_velocity_covariance value
373  */
374 void
375 IMUInterface::set_angular_velocity_covariance(const double * new_angular_velocity_covariance)
376 {
377  memcpy(data->angular_velocity_covariance, new_angular_velocity_covariance, sizeof(double) * 9);
378  data_changed = true;
379 }
380 
381 /** Set angular_velocity_covariance value at given index.
382  *
383  Covariance of angular velocity, row major about x, y, z axes.
384 
385  * @param new_angular_velocity_covariance new angular_velocity_covariance value
386  * @param index index for of the value
387  */
388 void
389 IMUInterface::set_angular_velocity_covariance(unsigned int index, const double new_angular_velocity_covariance)
390 {
391  if (index > 8) {
392  throw Exception("Index value %u out of bounds (0..8)", index);
393  }
394  data->angular_velocity_covariance[index] = new_angular_velocity_covariance;
395  data_changed = true;
396 }
397 /** Get linear_acceleration value.
398  *
399  Linear acceleration ordered as (x, y, z).
400 
401  * @return linear_acceleration value
402  */
403 float *
405 {
406  return data->linear_acceleration;
407 }
408 
409 /** Get linear_acceleration value at given index.
410  *
411  Linear acceleration ordered as (x, y, z).
412 
413  * @param index index of value
414  * @return linear_acceleration value
415  * @exception Exception thrown if index is out of bounds
416  */
417 float
418 IMUInterface::linear_acceleration(unsigned int index) const
419 {
420  if (index > 2) {
421  throw Exception("Index value %u out of bounds (0..2)", index);
422  }
423  return data->linear_acceleration[index];
424 }
425 
426 /** Get maximum length of linear_acceleration value.
427  * @return length of linear_acceleration value, can be length of the array or number of
428  * maximum number of characters for a string
429  */
430 size_t
432 {
433  return 3;
434 }
435 
436 /** Set linear_acceleration value.
437  *
438  Linear acceleration ordered as (x, y, z).
439 
440  * @param new_linear_acceleration new linear_acceleration value
441  */
442 void
443 IMUInterface::set_linear_acceleration(const float * new_linear_acceleration)
444 {
445  memcpy(data->linear_acceleration, new_linear_acceleration, sizeof(float) * 3);
446  data_changed = true;
447 }
448 
449 /** Set linear_acceleration value at given index.
450  *
451  Linear acceleration ordered as (x, y, z).
452 
453  * @param new_linear_acceleration new linear_acceleration value
454  * @param index index for of the value
455  */
456 void
457 IMUInterface::set_linear_acceleration(unsigned int index, const float new_linear_acceleration)
458 {
459  if (index > 2) {
460  throw Exception("Index value %u out of bounds (0..2)", index);
461  }
462  data->linear_acceleration[index] = new_linear_acceleration;
463  data_changed = true;
464 }
465 /** Get linear_acceleration_covariance value.
466  *
467  Covariance of linear acceleration, row major about x, y, z axes.
468 
469  * @return linear_acceleration_covariance value
470  */
471 double *
473 {
474  return data->linear_acceleration_covariance;
475 }
476 
477 /** Get linear_acceleration_covariance value at given index.
478  *
479  Covariance of linear acceleration, row major about x, y, z axes.
480 
481  * @param index index of value
482  * @return linear_acceleration_covariance value
483  * @exception Exception thrown if index is out of bounds
484  */
485 double
486 IMUInterface::linear_acceleration_covariance(unsigned int index) const
487 {
488  if (index > 8) {
489  throw Exception("Index value %u out of bounds (0..8)", index);
490  }
491  return data->linear_acceleration_covariance[index];
492 }
493 
494 /** Get maximum length of linear_acceleration_covariance value.
495  * @return length of linear_acceleration_covariance value, can be length of the array or number of
496  * maximum number of characters for a string
497  */
498 size_t
500 {
501  return 9;
502 }
503 
504 /** Set linear_acceleration_covariance value.
505  *
506  Covariance of linear acceleration, row major about x, y, z axes.
507 
508  * @param new_linear_acceleration_covariance new linear_acceleration_covariance value
509  */
510 void
511 IMUInterface::set_linear_acceleration_covariance(const double * new_linear_acceleration_covariance)
512 {
513  memcpy(data->linear_acceleration_covariance, new_linear_acceleration_covariance, sizeof(double) * 9);
514  data_changed = true;
515 }
516 
517 /** Set linear_acceleration_covariance value at given index.
518  *
519  Covariance of linear acceleration, row major about x, y, z axes.
520 
521  * @param new_linear_acceleration_covariance new linear_acceleration_covariance value
522  * @param index index for of the value
523  */
524 void
525 IMUInterface::set_linear_acceleration_covariance(unsigned int index, const double new_linear_acceleration_covariance)
526 {
527  if (index > 8) {
528  throw Exception("Index value %u out of bounds (0..8)", index);
529  }
530  data->linear_acceleration_covariance[index] = new_linear_acceleration_covariance;
531  data_changed = true;
532 }
533 /* =========== message create =========== */
534 Message *
535 IMUInterface::create_message(const char *type) const
536 {
537  throw UnknownTypeException("The given type '%s' does not match any known "
538  "message type for this interface type.", type);
539 }
540 
541 
542 /** Copy values from other interface.
543  * @param other other interface to copy values from
544  */
545 void
547 {
548  const IMUInterface *oi = dynamic_cast<const IMUInterface *>(other);
549  if (oi == NULL) {
550  throw TypeMismatchException("Can only copy values from interface of same type (%s vs. %s)",
551  type(), other->type());
552  }
553  memcpy(data, oi->data, sizeof(IMUInterface_data_t));
554 }
555 
556 const char *
557 IMUInterface::enum_tostring(const char *enumtype, int val) const
558 {
559  throw UnknownTypeException("Unknown enum type %s", enumtype);
560 }
561 
562 /* =========== messages =========== */
563 /** Check if message is valid and can be enqueued.
564  * @param message Message to check
565  * @return true if the message is valid, false otherwise.
566  */
567 bool
568 IMUInterface::message_valid(const Message *message) const
569 {
570  return false;
571 }
572 
573 /// @cond INTERNALS
574 EXPORT_INTERFACE(IMUInterface)
575 /// @endcond
576 
577 
578 } // 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:574
fawkes::Interface::data_ptr
void * data_ptr
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:259
fawkes::IMUInterface::set_frame
void set_frame(const char *new_frame)
Set frame value.
Definition: IMUInterface.cpp:124
fawkes::IMUInterface::maxlenof_angular_velocity
size_t maxlenof_angular_velocity() const
Get maximum length of angular_velocity value.
Definition: IMUInterface.cpp:301
fawkes::IMUInterface::linear_acceleration_covariance
double * linear_acceleration_covariance() const
Get linear_acceleration_covariance value.
Definition: IMUInterface.cpp:478
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:395
fawkes::IMUInterface
Definition: IMUInterface.h:39
fawkes::Message
Definition: message.h:41
fawkes::IMUInterface::angular_velocity
float * angular_velocity() const
Get angular_velocity value.
Definition: IMUInterface.cpp:274
fawkes::IMUInterface::copy_values
virtual void copy_values(const Interface *other)
Copy values from other interface.
Definition: IMUInterface.cpp:552
fawkes::IMUInterface::maxlenof_linear_acceleration_covariance
size_t maxlenof_linear_acceleration_covariance() const
Get maximum length of linear_acceleration_covariance value.
Definition: IMUInterface.cpp:505
fawkes::IFT_FLOAT
@ IFT_FLOAT
float field
Definition: types.h:60
fawkes::IMUInterface::orientation_covariance
double * orientation_covariance() const
Get orientation_covariance value.
Definition: IMUInterface.cpp:206
fawkes::IMUInterface::frame
char * frame() const
Get frame value.
Definition: IMUInterface.cpp:102
fawkes::Interface::type
const char * type() const
Get type of interface.
Definition: interface.cpp:645
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:341
fawkes::Interface::data_ts
interface_data_ts_t * data_ts
Definition: interface.h:228
fawkes::IMUInterface::maxlenof_orientation
size_t maxlenof_orientation() const
Get maximum length of orientation value.
Definition: IMUInterface.cpp:165
fawkes::IMUInterface::enum_tostring
virtual const char * enum_tostring(const char *enumtype, int val) const
Definition: IMUInterface.cpp:563
fawkes::IMUInterface::create_message
virtual Message * create_message(const char *type) const
Definition: IMUInterface.cpp:541
fawkes::TypeMismatchException
Definition: software.h:49
fawkes::Interface::data_changed
bool data_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:463
fawkes::IMUInterface::linear_acceleration
float * linear_acceleration() const
Get linear_acceleration value.
Definition: IMUInterface.cpp:410
fawkes::IMUInterface::orientation
float * orientation() const
Get orientation value.
Definition: IMUInterface.cpp:138
fawkes::UnknownTypeException
Definition: software.h:55
fawkes
fawkes::Interface::set_hash
void set_hash(unsigned char *ihash)
Set hash.
Definition: interface.cpp:321
fawkes::Interface
Definition: interface.h:78
fawkes::IMUInterface::angular_velocity_covariance
double * angular_velocity_covariance() const
Get angular_velocity_covariance value.
Definition: IMUInterface.cpp:342
fawkes::IMUInterface::set_orientation
void set_orientation(unsigned int index, const float new_orientation)
Set orientation value at given index.
Definition: IMUInterface.cpp:191
fawkes::IMUInterface::maxlenof_angular_velocity_covariance
size_t maxlenof_angular_velocity_covariance() const
Get maximum length of angular_velocity_covariance value.
Definition: IMUInterface.cpp:369
fawkes::IMUInterface::maxlenof_frame
size_t maxlenof_frame() const
Get maximum length of frame value.
Definition: IMUInterface.cpp:112
fawkes::IFT_DOUBLE
@ IFT_DOUBLE
double field
Definition: types.h:61
fawkes::Interface::data_size
unsigned int data_size
Definition: interface.h:225
fawkes::IFT_STRING
@ IFT_STRING
string field
Definition: types.h:62
fawkes::IMUInterface::maxlenof_orientation_covariance
size_t maxlenof_orientation_covariance() const
Get maximum length of orientation_covariance value.
Definition: IMUInterface.cpp:233
fawkes::IMUInterface::maxlenof_linear_acceleration
size_t maxlenof_linear_acceleration() const
Get maximum length of linear_acceleration value.
Definition: IMUInterface.cpp:437
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:531
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:327
fawkes::Exception
Definition: exception.h:41