Fawkes API  Fawkes Development Version
Position3DInterface.cpp
1 
2 /***************************************************************************
3  * Position3DInterface.cpp - Fawkes BlackBoard Interface - Position3DInterface
4  *
5  * Templated created: Thu Oct 12 10:49:19 2006
6  * Copyright 2011 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/Position3DInterface.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 Position3DInterface <interfaces/Position3DInterface.h>
36  * Position3DInterface Fawkes BlackBoard Interface.
37  *
38  Storage for a 3D pose in Euclidean space.
39 
40  * @ingroup FawkesInterfaces
41  */
42 
43 
44 
45 /** Constructor */
46 Position3DInterface::Position3DInterface() : Interface()
47 {
48  data_size = sizeof(Position3DInterface_data_t);
49  data_ptr = malloc(data_size);
50  data = (Position3DInterface_data_t *)data_ptr;
51  data_ts = (interface_data_ts_t *)data_ptr;
52  memset(data_ptr, 0, data_size);
53  add_fieldinfo(IFT_STRING, "frame", 32, data->frame);
54  add_fieldinfo(IFT_INT32, "visibility_history", 1, &data->visibility_history);
55  add_fieldinfo(IFT_DOUBLE, "rotation", 4, &data->rotation);
56  add_fieldinfo(IFT_DOUBLE, "translation", 3, &data->translation);
57  add_fieldinfo(IFT_DOUBLE, "covariance", 36, &data->covariance);
58  unsigned char tmp_hash[] = {0xd6, 0x19, 0x3f, 0x58, 0x62, 0xbc, 0x72, 0xd6, 0x22, 0x36, 0xd3, 0x7, 0x55, 0xb5, 0x3a, 0x48};
59  set_hash(tmp_hash);
60 }
61 
62 /** Destructor */
63 Position3DInterface::~Position3DInterface()
64 {
65  free(data_ptr);
66 }
67 /* Methods */
68 /** Get frame value.
69  *
70  Reference coordinate frame for the data.
71 
72  * @return frame value
73  */
74 char *
76 {
77  return data->frame;
78 }
79 
80 /** Get maximum length of frame value.
81  * @return length of frame value, can be length of the array or number of
82  * maximum number of characters for a string
83  */
84 size_t
86 {
87  return 32;
88 }
89 
90 /** Set frame value.
91  *
92  Reference coordinate frame for the data.
93 
94  * @param new_frame new frame value
95  */
96 void
97 Position3DInterface::set_frame(const char * new_frame)
98 {
99  data_changed |= change_field(data->frame, new_frame);
100 }
101 
102 /** Get visibility_history value.
103  *
104  The visibilitiy history indicates the number of consecutive positive or negative
105  sightings. If the history is negative, there have been as many negative sightings
106  (object not visible) as the absolute value of the history. A positive value denotes
107  as many positive sightings. 0 shall only be used during the initialization of the
108  interface or if the visibility history is not updated.
109 
110  * @return visibility_history value
111  */
112 int32_t
114 {
115  return data->visibility_history;
116 }
117 
118 /** Get maximum length of visibility_history value.
119  * @return length of visibility_history value, can be length of the array or number of
120  * maximum number of characters for a string
121  */
122 size_t
124 {
125  return 1;
126 }
127 
128 /** Set visibility_history value.
129  *
130  The visibilitiy history indicates the number of consecutive positive or negative
131  sightings. If the history is negative, there have been as many negative sightings
132  (object not visible) as the absolute value of the history. A positive value denotes
133  as many positive sightings. 0 shall only be used during the initialization of the
134  interface or if the visibility history is not updated.
135 
136  * @param new_visibility_history new visibility_history value
137  */
138 void
139 Position3DInterface::set_visibility_history(const int32_t new_visibility_history)
140 {
141  data_changed |= change_field(data->visibility_history, new_visibility_history);
142 }
143 
144 /** Get rotation value.
145  *
146  Rotation quaternion relative to reference frame, ordered as (x, y, z, w).
147 
148  * @return rotation value
149  */
150 double *
152 {
153  return data->rotation;
154 }
155 
156 /** Get rotation value at given index.
157  *
158  Rotation quaternion relative to reference frame, ordered as (x, y, z, w).
159 
160  * @param index index of value
161  * @return rotation value
162  * @exception Exception thrown if index is out of bounds
163  */
164 double
165 Position3DInterface::rotation(unsigned int index) const
166 {
167  if (index > 3) {
168  throw Exception("Index value %u out of bounds (0..3)", index);
169  }
170  return data->rotation[index];
171 }
172 
173 /** Get maximum length of rotation value.
174  * @return length of rotation value, can be length of the array or number of
175  * maximum number of characters for a string
176  */
177 size_t
179 {
180  return 4;
181 }
182 
183 /** Set rotation value.
184  *
185  Rotation quaternion relative to reference frame, ordered as (x, y, z, w).
186 
187  * @param new_rotation new rotation value
188  */
189 void
190 Position3DInterface::set_rotation(const double * new_rotation)
191 {
192  data_changed |= change_field(data->rotation, new_rotation);
193 }
194 
195 /** Set rotation value at given index.
196  *
197  Rotation quaternion relative to reference frame, ordered as (x, y, z, w).
198 
199  * @param new_rotation new rotation value
200  * @param index index for of the value
201  */
202 void
203 Position3DInterface::set_rotation(unsigned int index, const double new_rotation)
204 {
205  data_changed |= change_field(data->rotation, index, new_rotation);
206 }
207 /** Get translation value.
208  *
209  Translation vector from the reference frame's origin, ordered as (x, y, z).
210 
211  * @return translation value
212  */
213 double *
215 {
216  return data->translation;
217 }
218 
219 /** Get translation value at given index.
220  *
221  Translation vector from the reference frame's origin, ordered as (x, y, z).
222 
223  * @param index index of value
224  * @return translation value
225  * @exception Exception thrown if index is out of bounds
226  */
227 double
228 Position3DInterface::translation(unsigned int index) const
229 {
230  if (index > 2) {
231  throw Exception("Index value %u out of bounds (0..2)", index);
232  }
233  return data->translation[index];
234 }
235 
236 /** Get maximum length of translation value.
237  * @return length of translation value, can be length of the array or number of
238  * maximum number of characters for a string
239  */
240 size_t
242 {
243  return 3;
244 }
245 
246 /** Set translation value.
247  *
248  Translation vector from the reference frame's origin, ordered as (x, y, z).
249 
250  * @param new_translation new translation value
251  */
252 void
253 Position3DInterface::set_translation(const double * new_translation)
254 {
255  data_changed |= change_field(data->translation, new_translation);
256 }
257 
258 /** Set translation value at given index.
259  *
260  Translation vector from the reference frame's origin, ordered as (x, y, z).
261 
262  * @param new_translation new translation value
263  * @param index index for of the value
264  */
265 void
266 Position3DInterface::set_translation(unsigned int index, const double new_translation)
267 {
268  data_changed |= change_field(data->translation, index, new_translation);
269 }
270 /** Get covariance value.
271  *
272  Row-major representation of the 6x6 covariance matrix.
273  The orientation parameters use a fixed-axis representation.
274  In order, the parameters are:
275  (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
276 
277  * @return covariance value
278  */
279 double *
281 {
282  return data->covariance;
283 }
284 
285 /** Get covariance value at given index.
286  *
287  Row-major representation of the 6x6 covariance matrix.
288  The orientation parameters use a fixed-axis representation.
289  In order, the parameters are:
290  (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
291 
292  * @param index index of value
293  * @return covariance value
294  * @exception Exception thrown if index is out of bounds
295  */
296 double
297 Position3DInterface::covariance(unsigned int index) const
298 {
299  if (index > 35) {
300  throw Exception("Index value %u out of bounds (0..35)", index);
301  }
302  return data->covariance[index];
303 }
304 
305 /** Get maximum length of covariance value.
306  * @return length of covariance value, can be length of the array or number of
307  * maximum number of characters for a string
308  */
309 size_t
311 {
312  return 36;
313 }
314 
315 /** Set covariance value.
316  *
317  Row-major representation of the 6x6 covariance matrix.
318  The orientation parameters use a fixed-axis representation.
319  In order, the parameters are:
320  (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
321 
322  * @param new_covariance new covariance value
323  */
324 void
325 Position3DInterface::set_covariance(const double * new_covariance)
326 {
327  data_changed |= change_field(data->covariance, new_covariance);
328 }
329 
330 /** Set covariance value at given index.
331  *
332  Row-major representation of the 6x6 covariance matrix.
333  The orientation parameters use a fixed-axis representation.
334  In order, the parameters are:
335  (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
336 
337  * @param new_covariance new covariance value
338  * @param index index for of the value
339  */
340 void
341 Position3DInterface::set_covariance(unsigned int index, const double new_covariance)
342 {
343  data_changed |= change_field(data->covariance, index, new_covariance);
344 }
345 /* =========== message create =========== */
346 Message *
347 Position3DInterface::create_message(const char *type) const
348 {
349  throw UnknownTypeException("The given type '%s' does not match any known "
350  "message type for this interface type.", type);
351 }
352 
353 
354 /** Copy values from other interface.
355  * @param other other interface to copy values from
356  */
357 void
359 {
360  const Position3DInterface *oi = dynamic_cast<const Position3DInterface *>(other);
361  if (oi == NULL) {
362  throw TypeMismatchException("Can only copy values from interface of same type (%s vs. %s)",
363  type(), other->type());
364  }
365  memcpy(data, oi->data, sizeof(Position3DInterface_data_t));
366 }
367 
368 const char *
369 Position3DInterface::enum_tostring(const char *enumtype, int val) const
370 {
371  throw UnknownTypeException("Unknown enum type %s", enumtype);
372 }
373 
374 /* =========== messages =========== */
375 /** Check if message is valid and can be enqueued.
376  * @param message Message to check
377  * @return true if the message is valid, false otherwise.
378  */
379 bool
381 {
382  return false;
383 }
384 
385 /// @cond INTERNALS
386 EXPORT_INTERFACE(Position3DInterface)
387 /// @endcond
388 
389 
390 } // end namespace fawkes
fawkes::Position3DInterface::maxlenof_frame
size_t maxlenof_frame() const
Get maximum length of frame value.
Definition: Position3DInterface.cpp:85
fawkes::Interface::data_ptr
void * data_ptr
Pointer to local memory storage.
Definition: interface.h:224
fawkes::Position3DInterface::maxlenof_visibility_history
size_t maxlenof_visibility_history() const
Get maximum length of visibility_history value.
Definition: Position3DInterface.cpp:123
fawkes::Position3DInterface::copy_values
virtual void copy_values(const Interface *other)
Copy values from other interface.
Definition: Position3DInterface.cpp:358
fawkes::Position3DInterface::maxlenof_covariance
size_t maxlenof_covariance() const
Get maximum length of covariance value.
Definition: Position3DInterface.cpp:310
fawkes::Position3DInterface::visibility_history
int32_t visibility_history() const
Get visibility_history value.
Definition: Position3DInterface.cpp:113
fawkes::Position3DInterface::create_message
virtual Message * create_message(const char *type) const
Create message based on type name.
Definition: Position3DInterface.cpp:347
fawkes::Position3DInterface::set_rotation
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
Definition: Position3DInterface.cpp:203
fawkes::Message
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:45
fawkes::Position3DInterface::set_frame
void set_frame(const char *new_frame)
Set frame value.
Definition: Position3DInterface.cpp:97
fawkes::Position3DInterface::frame
char * frame() const
Get frame value.
Definition: Position3DInterface.cpp:75
fawkes::Position3DInterface::rotation
double * rotation() const
Get rotation value.
Definition: Position3DInterface.cpp:151
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::Position3DInterface::translation
double * translation() const
Get translation value.
Definition: Position3DInterface.cpp:214
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::Position3DInterface::set_covariance
void set_covariance(unsigned int index, const double new_covariance)
Set covariance value at given index.
Definition: Position3DInterface.cpp:341
fawkes::IFT_INT32
@ IFT_INT32
32 bit integer field
Definition: types.h:42
fawkes::UnknownTypeException
Unknown type.
Definition: software.h:50
fawkes::Position3DInterface::enum_tostring
virtual const char * enum_tostring(const char *enumtype, int val) const
Convert arbitrary enum value to string.
Definition: Position3DInterface.cpp:369
fawkes::Position3DInterface::set_translation
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
Definition: Position3DInterface.cpp:266
fawkes
Fawkes library namespace.
fawkes::Interface::set_hash
void set_hash(unsigned char *ihash)
Set hash.
Definition: interface.cpp:316
fawkes::Position3DInterface::maxlenof_translation
size_t maxlenof_translation() const
Get maximum length of translation value.
Definition: Position3DInterface.cpp:241
fawkes::Interface
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
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::Position3DInterface::set_visibility_history
void set_visibility_history(const int32_t new_visibility_history)
Set visibility_history value.
Definition: Position3DInterface.cpp:139
fawkes::Position3DInterface
Position3DInterface Fawkes BlackBoard Interface.
Definition: Position3DInterface.h:34
fawkes::Position3DInterface::covariance
double * covariance() const
Get covariance value.
Definition: Position3DInterface.cpp:280
fawkes::IFT_STRING
@ IFT_STRING
string field
Definition: types.h:48
fawkes::Position3DInterface::message_valid
virtual bool message_valid(const Message *message) const
Check if message is valid and can be enqueued.
Definition: Position3DInterface.cpp:380
fawkes::change_field
bool change_field(FieldT &field, const DataT &value)
Set a field and return whether it changed.
Definition: message.h:167
fawkes::Position3DInterface::maxlenof_rotation
size_t maxlenof_rotation() const
Get maximum length of rotation value.
Definition: Position3DInterface.cpp:178
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36