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
33namespace 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 */
46Position3DInterface::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 */
63Position3DInterface::~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 */
74char *
75Position3DInterface::frame() const
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 */
84size_t
85Position3DInterface::maxlenof_frame() const
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 */
96void
97Position3DInterface::set_frame(const char * new_frame)
98{
99 set_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 */
112int32_t
113Position3DInterface::visibility_history() const
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 */
122size_t
123Position3DInterface::maxlenof_visibility_history() const
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 */
138void
139Position3DInterface::set_visibility_history(const int32_t new_visibility_history)
140{
141 set_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 */
150double *
151Position3DInterface::rotation() const
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 */
164double
165Position3DInterface::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 */
177size_t
178Position3DInterface::maxlenof_rotation() const
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 */
189void
190Position3DInterface::set_rotation(const double * new_rotation)
191{
192 set_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 */
202void
203Position3DInterface::set_rotation(unsigned int index, const double new_rotation)
204{
205 set_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 */
213double *
214Position3DInterface::translation() const
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 */
227double
228Position3DInterface::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 */
240size_t
241Position3DInterface::maxlenof_translation() const
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 */
252void
253Position3DInterface::set_translation(const double * new_translation)
254{
255 set_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 */
265void
266Position3DInterface::set_translation(unsigned int index, const double new_translation)
267{
268 set_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 */
279double *
280Position3DInterface::covariance() const
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 */
296double
297Position3DInterface::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 */
309size_t
310Position3DInterface::maxlenof_covariance() const
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 */
324void
325Position3DInterface::set_covariance(const double * new_covariance)
326{
327 set_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 */
340void
341Position3DInterface::set_covariance(unsigned int index, const double new_covariance)
342{
343 set_field(data->covariance, index, new_covariance);
344}
345/* =========== message create =========== */
346Message *
347Position3DInterface::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 */
357void
358Position3DInterface::copy_values(const Interface *other)
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
368const char *
369Position3DInterface::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 */
379bool
380Position3DInterface::message_valid(const Message *message) const
381{
382 return false;
383}
384
385/// @cond INTERNALS
386EXPORT_INTERFACE(Position3DInterface)
387/// @endcond
388
389
390} // end namespace fawkes
Base class for exceptions in Fawkes.
Definition: exception.h:36
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
Position3DInterface Fawkes BlackBoard Interface.
Fawkes library namespace.