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