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  strncpy(data->frame, new_frame, sizeof(data->frame));
100  data_changed = true;
101 }
102 
103 /** Get visibility_history value.
104  *
105  The visibilitiy history indicates the number of consecutive positive or negative
106  sightings. If the history is negative, there have been as many negative sightings
107  (object not visible) as the absolute value of the history. A positive value denotes
108  as many positive sightings. 0 shall only be used during the initialization of the
109  interface or if the visibility history is not updated.
110 
111  * @return visibility_history value
112  */
113 int32_t
115 {
116  return data->visibility_history;
117 }
118 
119 /** Get maximum length of visibility_history value.
120  * @return length of visibility_history value, can be length of the array or number of
121  * maximum number of characters for a string
122  */
123 size_t
125 {
126  return 1;
127 }
128 
129 /** Set visibility_history value.
130  *
131  The visibilitiy history indicates the number of consecutive positive or negative
132  sightings. If the history is negative, there have been as many negative sightings
133  (object not visible) as the absolute value of the history. A positive value denotes
134  as many positive sightings. 0 shall only be used during the initialization of the
135  interface or if the visibility history is not updated.
136 
137  * @param new_visibility_history new visibility_history value
138  */
139 void
140 Position3DInterface::set_visibility_history(const int32_t new_visibility_history)
141 {
142  data->visibility_history = new_visibility_history;
143  data_changed = true;
144 }
145 
146 /** Get rotation value.
147  *
148  Rotation quaternion relative to reference frame, ordered as (x, y, z, w).
149 
150  * @return rotation value
151  */
152 double *
154 {
155  return data->rotation;
156 }
157 
158 /** Get rotation value at given index.
159  *
160  Rotation quaternion relative to reference frame, ordered as (x, y, z, w).
161 
162  * @param index index of value
163  * @return rotation value
164  * @exception Exception thrown if index is out of bounds
165  */
166 double
167 Position3DInterface::rotation(unsigned int index) const
168 {
169  if (index > 4) {
170  throw Exception("Index value %u out of bounds (0..4)", index);
171  }
172  return data->rotation[index];
173 }
174 
175 /** Get maximum length of rotation value.
176  * @return length of rotation value, can be length of the array or number of
177  * maximum number of characters for a string
178  */
179 size_t
181 {
182  return 4;
183 }
184 
185 /** Set rotation value.
186  *
187  Rotation quaternion relative to reference frame, ordered as (x, y, z, w).
188 
189  * @param new_rotation new rotation value
190  */
191 void
192 Position3DInterface::set_rotation(const double * new_rotation)
193 {
194  memcpy(data->rotation, new_rotation, sizeof(double) * 4);
195  data_changed = true;
196 }
197 
198 /** Set rotation value at given index.
199  *
200  Rotation quaternion relative to reference frame, ordered as (x, y, z, w).
201 
202  * @param new_rotation new rotation value
203  * @param index index for of the value
204  */
205 void
206 Position3DInterface::set_rotation(unsigned int index, const double new_rotation)
207 {
208  if (index > 4) {
209  throw Exception("Index value %u out of bounds (0..4)", index);
210  }
211  data->rotation[index] = new_rotation;
212  data_changed = true;
213 }
214 /** Get translation value.
215  *
216  Translation vector from the reference frame's origin, ordered as (x, y, z).
217 
218  * @return translation value
219  */
220 double *
222 {
223  return data->translation;
224 }
225 
226 /** Get translation value at given index.
227  *
228  Translation vector from the reference frame's origin, ordered as (x, y, z).
229 
230  * @param index index of value
231  * @return translation value
232  * @exception Exception thrown if index is out of bounds
233  */
234 double
235 Position3DInterface::translation(unsigned int index) const
236 {
237  if (index > 3) {
238  throw Exception("Index value %u out of bounds (0..3)", index);
239  }
240  return data->translation[index];
241 }
242 
243 /** Get maximum length of translation value.
244  * @return length of translation value, can be length of the array or number of
245  * maximum number of characters for a string
246  */
247 size_t
249 {
250  return 3;
251 }
252 
253 /** Set translation value.
254  *
255  Translation vector from the reference frame's origin, ordered as (x, y, z).
256 
257  * @param new_translation new translation value
258  */
259 void
260 Position3DInterface::set_translation(const double * new_translation)
261 {
262  memcpy(data->translation, new_translation, sizeof(double) * 3);
263  data_changed = true;
264 }
265 
266 /** Set translation value at given index.
267  *
268  Translation vector from the reference frame's origin, ordered as (x, y, z).
269 
270  * @param new_translation new translation value
271  * @param index index for of the value
272  */
273 void
274 Position3DInterface::set_translation(unsigned int index, const double new_translation)
275 {
276  if (index > 3) {
277  throw Exception("Index value %u out of bounds (0..3)", index);
278  }
279  data->translation[index] = new_translation;
280  data_changed = true;
281 }
282 /** Get covariance value.
283  *
284  Row-major representation of the 6x6 covariance matrix.
285  The orientation parameters use a fixed-axis representation.
286  In order, the parameters are:
287  (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
288 
289  * @return covariance value
290  */
291 double *
293 {
294  return data->covariance;
295 }
296 
297 /** Get covariance value at given index.
298  *
299  Row-major representation of the 6x6 covariance matrix.
300  The orientation parameters use a fixed-axis representation.
301  In order, the parameters are:
302  (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
303 
304  * @param index index of value
305  * @return covariance value
306  * @exception Exception thrown if index is out of bounds
307  */
308 double
309 Position3DInterface::covariance(unsigned int index) const
310 {
311  if (index > 36) {
312  throw Exception("Index value %u out of bounds (0..36)", index);
313  }
314  return data->covariance[index];
315 }
316 
317 /** Get maximum length of covariance value.
318  * @return length of covariance value, can be length of the array or number of
319  * maximum number of characters for a string
320  */
321 size_t
323 {
324  return 36;
325 }
326 
327 /** Set covariance value.
328  *
329  Row-major representation of the 6x6 covariance matrix.
330  The orientation parameters use a fixed-axis representation.
331  In order, the parameters are:
332  (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
333 
334  * @param new_covariance new covariance value
335  */
336 void
337 Position3DInterface::set_covariance(const double * new_covariance)
338 {
339  memcpy(data->covariance, new_covariance, sizeof(double) * 36);
340  data_changed = true;
341 }
342 
343 /** Set covariance value at given index.
344  *
345  Row-major representation of the 6x6 covariance matrix.
346  The orientation parameters use a fixed-axis representation.
347  In order, the parameters are:
348  (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
349 
350  * @param new_covariance new covariance value
351  * @param index index for of the value
352  */
353 void
354 Position3DInterface::set_covariance(unsigned int index, const double new_covariance)
355 {
356  if (index > 36) {
357  throw Exception("Index value %u out of bounds (0..36)", index);
358  }
359  data->covariance[index] = new_covariance;
360  data_changed = true;
361 }
362 /* =========== message create =========== */
363 Message *
365 {
366  throw UnknownTypeException("The given type '%s' does not match any known "
367  "message type for this interface type.", type);
368 }
369 
370 
371 /** Copy values from other interface.
372  * @param other other interface to copy values from
373  */
374 void
376 {
377  const Position3DInterface *oi = dynamic_cast<const Position3DInterface *>(other);
378  if (oi == NULL) {
379  throw TypeMismatchException("Can only copy values from interface of same type (%s vs. %s)",
380  type(), other->type());
381  }
382  memcpy(data, oi->data, sizeof(Position3DInterface_data_t));
383 }
384 
385 const char *
386 Position3DInterface::enum_tostring(const char *enumtype, int val) const
387 {
388  throw UnknownTypeException("Unknown enum type %s", enumtype);
389 }
390 
391 /* =========== messages =========== */
392 /** Check if message is valid and can be enqueued.
393  * @param message Message to check
394  * @return true if the message is valid, false otherwise.
395  */
396 bool
398 {
399  return false;
400 }
401 
402 /// @cond INTERNALS
403 EXPORT_INTERFACE(Position3DInterface)
404 /// @endcond
405 
406 
407 } // end namespace fawkes
void set_frame(const char *new_frame)
Set frame value.
double * rotation() const
Get rotation value.
char * frame() const
Get frame value.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
void set_hash(unsigned char *ihash)
Set hash.
Definition: interface.cpp:314
Fawkes library namespace.
unsigned int data_size
Minimal data size to hold data storage.
Definition: interface.h:221
size_t maxlenof_frame() const
Get maximum length of frame value.
string field
Definition: types.h:47
virtual Message * create_message(const char *type) const
Create message based on type name.
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
void set_rotation(unsigned int index, const double new_rotation)
Set rotation 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
const char * type() const
Get type of interface.
Definition: interface.cpp:651
Position3DInterface Fawkes BlackBoard Interface.
void * data_ptr
Pointer to local memory storage.
Definition: interface.h:220
void set_visibility_history(const int32_t new_visibility_history)
Set visibility_history value.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void set_covariance(unsigned int index, const double new_covariance)
Set covariance value at given index.
double * translation() const
Get translation value.
virtual bool message_valid(const Message *message) const
Check if message is valid and can be enqueued.
size_t maxlenof_translation() const
Get maximum length of translation value.
32 bit integer field
Definition: types.h:41
virtual const char * enum_tostring(const char *enumtype, int val) const
Convert arbitrary enum value to string.
size_t maxlenof_visibility_history() const
Get maximum length of visibility_history value.
size_t maxlenof_rotation() const
Get maximum length of rotation value.
int32_t visibility_history() const
Get visibility_history value.
interface_data_ts_t * data_ts
Pointer to data casted to timestamp struct.
Definition: interface.h:224
virtual void copy_values(const Interface *other)
Copy values from other interface.
size_t maxlenof_covariance() const
Get maximum length of covariance value.
double * covariance() const
Get covariance value.
double field
Definition: types.h:46