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  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 > 4) {
149  throw Exception("Index value %u out of bounds (0..4)", 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 > 4) {
188  throw Exception("Index value %u out of bounds (0..4)", 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 > 9) {
217  throw Exception("Index value %u out of bounds (0..9)", 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 > 9) {
256  throw Exception("Index value %u out of bounds (0..9)", 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 > 3) {
285  throw Exception("Index value %u out of bounds (0..3)", 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 > 3) {
324  throw Exception("Index value %u out of bounds (0..3)", 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
351 {
352  if (index > 9) {
353  throw Exception("Index value %u out of bounds (0..9)", 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 > 9) {
392  throw Exception("Index value %u out of bounds (0..9)", 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 > 3) {
421  throw Exception("Index value %u out of bounds (0..3)", 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 > 3) {
460  throw Exception("Index value %u out of bounds (0..3)", 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
487 {
488  if (index > 9) {
489  throw Exception("Index value %u out of bounds (0..9)", 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 > 9) {
528  throw Exception("Index value %u out of bounds (0..9)", 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
569 {
570  return false;
571 }
572 
573 /// @cond INTERNALS
574 EXPORT_INTERFACE(IMUInterface)
575 /// @endcond
576 
577 
578 } // end namespace fawkes
virtual Message * create_message(const char *type) const
Create message based on type name.
double * angular_velocity_covariance() const
Get angular_velocity_covariance value.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:41
size_t maxlenof_angular_velocity_covariance() const
Get maximum length of angular_velocity_covariance value.
void set_hash(unsigned char *ihash)
Set hash.
Definition: interface.cpp:316
void set_orientation(unsigned int index, const float new_orientation)
Set orientation value at given index.
Fawkes library namespace.
size_t maxlenof_frame() const
Get maximum length of frame value.
unsigned int data_size
Minimal data size to hold data storage.
Definition: interface.h:225
string field
Definition: types.h:48
size_t maxlenof_linear_acceleration() const
Get maximum length of linear_acceleration value.
size_t maxlenof_orientation_covariance() const
Get maximum length of orientation_covariance value.
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:78
void set_linear_acceleration_covariance(unsigned int index, const double new_linear_acceleration_covariance)
Set linear_acceleration_covariance value at given index.
void set_angular_velocity(unsigned int index, const float new_angular_velocity)
Set angular_velocity value at given index.
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
bool data_changed
Indicator if data has changed.
Definition: interface.h:226
void set_frame(const char *new_frame)
Set frame value.
const char * type() const
Get type of interface.
Definition: interface.cpp:640
void * data_ptr
Pointer to local memory storage.
Definition: interface.h:224
void set_orientation_covariance(unsigned int index, const double new_orientation_covariance)
Set orientation_covariance value at given index.
Base class for exceptions in Fawkes.
Definition: exception.h:35
virtual bool message_valid(const Message *message) const
Check if message is valid and can be enqueued.
void set_angular_velocity_covariance(unsigned int index, const double new_angular_velocity_covariance)
Set angular_velocity_covariance value at given index.
float * angular_velocity() const
Get angular_velocity value.
size_t maxlenof_angular_velocity() const
Get maximum length of angular_velocity value.
size_t maxlenof_linear_acceleration_covariance() const
Get maximum length of linear_acceleration_covariance value.
float field
Definition: types.h:46
double * linear_acceleration_covariance() const
Get linear_acceleration_covariance value.
IMUInterface Fawkes BlackBoard Interface.
Definition: IMUInterface.h:33
virtual void copy_values(const Interface *other)
Copy values from other interface.
char * frame() const
Get frame value.
double * orientation_covariance() const
Get orientation_covariance value.
size_t maxlenof_orientation() const
Get maximum length of orientation value.
interface_data_ts_t * data_ts
Pointer to data casted to timestamp struct.
Definition: interface.h:228
float * linear_acceleration() const
Get linear_acceleration value.
void set_linear_acceleration(unsigned int index, const float new_linear_acceleration)
Set linear_acceleration value at given index.
float * orientation() const
Get orientation value.
virtual const char * enum_tostring(const char *enumtype, int val) const
Convert arbitrary enum value to string.
double field
Definition: types.h:47