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
106 IMUInterface::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  */
117 void
118 IMUInterface::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  */
129 float *
130 IMUInterface::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  */
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
157 IMUInterface::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  */
168 void
169 IMUInterface::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  */
181 void
182 IMUInterface::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  */
192 double *
193 IMUInterface::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  */
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
220 IMUInterface::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  */
231 void
232 IMUInterface::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  */
244 void
245 IMUInterface::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  */
255 float *
256 IMUInterface::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  */
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
283 IMUInterface::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  */
294 void
295 IMUInterface::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  */
307 void
308 IMUInterface::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  */
318 double *
319 IMUInterface::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  */
332 double
333 IMUInterface::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  */
345 size_t
346 IMUInterface::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  */
357 void
358 IMUInterface::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  */
370 void
371 IMUInterface::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  */
381 float *
382 IMUInterface::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  */
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
409 IMUInterface::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  */
420 void
421 IMUInterface::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  */
433 void
434 IMUInterface::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  */
444 double *
445 IMUInterface::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  */
458 double
459 IMUInterface::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  */
471 size_t
472 IMUInterface::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  */
483 void
484 IMUInterface::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  */
496 void
497 IMUInterface::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 =========== */
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
514 IMUInterface::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 
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
536 IMUInterface::message_valid(const Message *message) const
537 {
538  return false;
539 }
540 
541 /// @cond INTERNALS
542 EXPORT_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.