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