Fawkes API Fawkes Development Version
LocalizationInterface.cpp
1
2/***************************************************************************
3 * LocalizationInterface.cpp - Fawkes BlackBoard Interface - LocalizationInterface
4 *
5 * Templated created: Thu Oct 12 10:49:19 2006
6 * Copyright 2015 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/LocalizationInterface.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 LocalizationInterface <interfaces/LocalizationInterface.h>
36 * LocalizationInterface Fawkes BlackBoard Interface.
37 *
38 Information and commands relevant to a self-localization
39 component. This does not contain the pose as it is provided in a
40 Position3DInterface.
41
42 * @ingroup FawkesInterfaces
43 */
44
45
46
47/** Constructor */
48LocalizationInterface::LocalizationInterface() : Interface()
49{
50 data_size = sizeof(LocalizationInterface_data_t);
51 data_ptr = malloc(data_size);
52 data = (LocalizationInterface_data_t *)data_ptr;
53 data_ts = (interface_data_ts_t *)data_ptr;
54 memset(data_ptr, 0, data_size);
55 add_fieldinfo(IFT_STRING, "map", 64, data->map);
56 add_messageinfo("SetInitialPoseMessage");
57 unsigned char tmp_hash[] = {0x7f, 0x9, 0xec, 0xd1, 00, 0x3f, 0x3, 0xb7, 0x95, 0xce, 0xe, 0x1d, 0x6f, 0x48, 0x6c, 0xad};
58 set_hash(tmp_hash);
59}
60
61/** Destructor */
62LocalizationInterface::~LocalizationInterface()
63{
64 free(data_ptr);
65}
66/* Methods */
67/** Get map value.
68 * The currently used map.
69 * @return map value
70 */
71char *
72LocalizationInterface::map() const
73{
74 return data->map;
75}
76
77/** Get maximum length of map value.
78 * @return length of map value, can be length of the array or number of
79 * maximum number of characters for a string
80 */
81size_t
82LocalizationInterface::maxlenof_map() const
83{
84 return 64;
85}
86
87/** Set map value.
88 * The currently used map.
89 * @param new_map new map value
90 */
91void
92LocalizationInterface::set_map(const char * new_map)
93{
94 set_field(data->map, new_map);
95}
96
97/* =========== message create =========== */
98Message *
99LocalizationInterface::create_message(const char *type) const
100{
101 if ( strncmp("SetInitialPoseMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
102 return new SetInitialPoseMessage();
103 } else {
104 throw UnknownTypeException("The given type '%s' does not match any known "
105 "message type for this interface type.", type);
106 }
107}
108
109
110/** Copy values from other interface.
111 * @param other other interface to copy values from
112 */
113void
114LocalizationInterface::copy_values(const Interface *other)
115{
116 const LocalizationInterface *oi = dynamic_cast<const LocalizationInterface *>(other);
117 if (oi == NULL) {
118 throw TypeMismatchException("Can only copy values from interface of same type (%s vs. %s)",
119 type(), other->type());
120 }
121 memcpy(data, oi->data, sizeof(LocalizationInterface_data_t));
122}
123
124const char *
125LocalizationInterface::enum_tostring(const char *enumtype, int val) const
126{
127 throw UnknownTypeException("Unknown enum type %s", enumtype);
128}
129
130/* =========== messages =========== */
131/** @class LocalizationInterface::SetInitialPoseMessage <interfaces/LocalizationInterface.h>
132 * SetInitialPoseMessage Fawkes BlackBoard Interface Message.
133 *
134
135 */
136
137
138/** Constructor with initial values.
139 * @param ini_frame initial value for frame
140 * @param ini_rotation initial value for rotation
141 * @param ini_translation initial value for translation
142 * @param ini_covariance initial value for covariance
143 */
144LocalizationInterface::SetInitialPoseMessage::SetInitialPoseMessage(const char * ini_frame, const double * ini_rotation, const double * ini_translation, const double * ini_covariance) : Message("SetInitialPoseMessage")
145{
146 data_size = sizeof(SetInitialPoseMessage_data_t);
147 data_ptr = malloc(data_size);
148 memset(data_ptr, 0, data_size);
149 data = (SetInitialPoseMessage_data_t *)data_ptr;
151 strncpy(data->frame, ini_frame, 32-1);
152 data->frame[32-1] = 0;
153 memcpy(data->rotation, ini_rotation, sizeof(double) * 4);
154 memcpy(data->translation, ini_translation, sizeof(double) * 3);
155 memcpy(data->covariance, ini_covariance, sizeof(double) * 36);
156 add_fieldinfo(IFT_STRING, "frame", 32, data->frame);
157 add_fieldinfo(IFT_DOUBLE, "rotation", 4, &data->rotation);
158 add_fieldinfo(IFT_DOUBLE, "translation", 3, &data->translation);
159 add_fieldinfo(IFT_DOUBLE, "covariance", 36, &data->covariance);
160}
161/** Constructor */
163{
164 data_size = sizeof(SetInitialPoseMessage_data_t);
165 data_ptr = malloc(data_size);
166 memset(data_ptr, 0, data_size);
167 data = (SetInitialPoseMessage_data_t *)data_ptr;
169 add_fieldinfo(IFT_STRING, "frame", 32, data->frame);
170 add_fieldinfo(IFT_DOUBLE, "rotation", 4, &data->rotation);
171 add_fieldinfo(IFT_DOUBLE, "translation", 3, &data->translation);
172 add_fieldinfo(IFT_DOUBLE, "covariance", 36, &data->covariance);
173}
174
175/** Destructor */
177{
178 free(data_ptr);
179}
180
181/** Copy constructor.
182 * @param m message to copy from
183 */
185{
186 data_size = m->data_size;
187 data_ptr = malloc(data_size);
188 memcpy(data_ptr, m->data_ptr, data_size);
189 data = (SetInitialPoseMessage_data_t *)data_ptr;
191}
192
193/* Methods */
194/** Get frame value.
195 *
196 Reference coordinate frame for the data.
197
198 * @return frame value
199 */
200char *
202{
203 return data->frame;
204}
205
206/** Get maximum length of frame value.
207 * @return length of frame value, can be length of the array or number of
208 * maximum number of characters for a string
209 */
210size_t
212{
213 return 32;
214}
215
216/** Set frame value.
217 *
218 Reference coordinate frame for the data.
219
220 * @param new_frame new frame value
221 */
222void
224{
225 set_field(data->frame, new_frame);
226}
227
228/** Get rotation value.
229 *
230 Rotation quaternion relative to reference frame, ordered as (x, y, z, w).
231
232 * @return rotation value
233 */
234double *
236{
237 return data->rotation;
238}
239
240/** Get rotation value at given index.
241 *
242 Rotation quaternion relative to reference frame, ordered as (x, y, z, w).
243
244 * @param index index of value
245 * @return rotation value
246 * @exception Exception thrown if index is out of bounds
247 */
248double
250{
251 if (index > 3) {
252 throw Exception("Index value %u out of bounds (0..3)", index);
253 }
254 return data->rotation[index];
255}
256
257/** Get maximum length of rotation value.
258 * @return length of rotation value, can be length of the array or number of
259 * maximum number of characters for a string
260 */
261size_t
263{
264 return 4;
265}
266
267/** Set rotation value.
268 *
269 Rotation quaternion relative to reference frame, ordered as (x, y, z, w).
270
271 * @param new_rotation new rotation value
272 */
273void
275{
276 set_field(data->rotation, new_rotation);
277}
278
279/** Set rotation value at given index.
280 *
281 Rotation quaternion relative to reference frame, ordered as (x, y, z, w).
282
283 * @param new_rotation new rotation value
284 * @param index index for of the value
285 */
286void
287LocalizationInterface::SetInitialPoseMessage::set_rotation(unsigned int index, const double new_rotation)
288{
289 set_field(data->rotation, index, new_rotation);
290}
291/** Get translation value.
292 *
293 Translation vector from the reference frame's origin, ordered as (x, y, z).
294
295 * @return translation value
296 */
297double *
299{
300 return data->translation;
301}
302
303/** Get translation value at given index.
304 *
305 Translation vector from the reference frame's origin, ordered as (x, y, z).
306
307 * @param index index of value
308 * @return translation value
309 * @exception Exception thrown if index is out of bounds
310 */
311double
313{
314 if (index > 2) {
315 throw Exception("Index value %u out of bounds (0..2)", index);
316 }
317 return data->translation[index];
318}
319
320/** Get maximum length of translation value.
321 * @return length of translation value, can be length of the array or number of
322 * maximum number of characters for a string
323 */
324size_t
326{
327 return 3;
328}
329
330/** Set translation value.
331 *
332 Translation vector from the reference frame's origin, ordered as (x, y, z).
333
334 * @param new_translation new translation value
335 */
336void
338{
339 set_field(data->translation, new_translation);
340}
341
342/** Set translation value at given index.
343 *
344 Translation vector from the reference frame's origin, ordered as (x, y, z).
345
346 * @param new_translation new translation value
347 * @param index index for of the value
348 */
349void
350LocalizationInterface::SetInitialPoseMessage::set_translation(unsigned int index, const double new_translation)
351{
352 set_field(data->translation, index, new_translation);
353}
354/** Get covariance value.
355 *
356 Row-major representation of the 6x6 covariance matrix. The
357 orientation parameters use a fixed-axis representation. In
358 order, the parameters are: (x, y, z, rotation about X axis,
359 rotation about Y axis, rotation about Z axis).
360
361 * @return covariance value
362 */
363double *
365{
366 return data->covariance;
367}
368
369/** Get covariance value at given index.
370 *
371 Row-major representation of the 6x6 covariance matrix. The
372 orientation parameters use a fixed-axis representation. In
373 order, the parameters are: (x, y, z, rotation about X axis,
374 rotation about Y axis, rotation about Z axis).
375
376 * @param index index of value
377 * @return covariance value
378 * @exception Exception thrown if index is out of bounds
379 */
380double
382{
383 if (index > 35) {
384 throw Exception("Index value %u out of bounds (0..35)", index);
385 }
386 return data->covariance[index];
387}
388
389/** Get maximum length of covariance value.
390 * @return length of covariance value, can be length of the array or number of
391 * maximum number of characters for a string
392 */
393size_t
395{
396 return 36;
397}
398
399/** Set covariance value.
400 *
401 Row-major representation of the 6x6 covariance matrix. The
402 orientation parameters use a fixed-axis representation. In
403 order, the parameters are: (x, y, z, rotation about X axis,
404 rotation about Y axis, rotation about Z axis).
405
406 * @param new_covariance new covariance value
407 */
408void
410{
411 set_field(data->covariance, new_covariance);
412}
413
414/** Set covariance value at given index.
415 *
416 Row-major representation of the 6x6 covariance matrix. The
417 orientation parameters use a fixed-axis representation. In
418 order, the parameters are: (x, y, z, rotation about X axis,
419 rotation about Y axis, rotation about Z axis).
420
421 * @param new_covariance new covariance value
422 * @param index index for of the value
423 */
424void
425LocalizationInterface::SetInitialPoseMessage::set_covariance(unsigned int index, const double new_covariance)
426{
427 set_field(data->covariance, index, new_covariance);
428}
429/** Clone this message.
430 * Produces a message of the same type as this message and copies the
431 * data to the new message.
432 * @return clone of this message
433 */
434Message *
436{
438}
439/** Check if message is valid and can be enqueued.
440 * @param message Message to check
441 * @return true if the message is valid, false otherwise.
442 */
443bool
445{
446 const SetInitialPoseMessage *m0 = dynamic_cast<const SetInitialPoseMessage *>(message);
447 if ( m0 != NULL ) {
448 return true;
449 }
450 return false;
451}
452
453/// @cond INTERNALS
454EXPORT_INTERFACE(LocalizationInterface)
455/// @endcond
456
457
458} // 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
void * data_ptr
Pointer to local memory storage.
Definition: interface.h:244
void set_field(FieldT &field, DataT &data)
Set a field, set data_changed to true and update data_changed accordingly.
Definition: interface.h:304
SetInitialPoseMessage Fawkes BlackBoard Interface Message.
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
virtual Message * clone() const
Clone this message.
size_t maxlenof_translation() const
Get maximum length of translation value.
size_t maxlenof_covariance() const
Get maximum length of covariance value.
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
size_t maxlenof_frame() const
Get maximum length of frame value.
size_t maxlenof_rotation() const
Get maximum length of rotation value.
void set_frame(const char *new_frame)
Set frame value.
void set_covariance(unsigned int index, const double new_covariance)
Set covariance value at given index.
LocalizationInterface Fawkes BlackBoard Interface.
virtual bool message_valid(const Message *message) const
Check if message is valid and can be enqueued.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
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 info list.
Definition: message.cpp:435
void * data_ptr
Pointer to memory that contains local data.
Definition: message.h:146
message_data_ts_t * data_ts
data timestamp aliasing pointer
Definition: message.h:156
unsigned int data_size
Size of memory needed to hold all data.
Definition: message.h:147
Fawkes library namespace.
@ IFT_DOUBLE
double field
Definition: types.h:47
@ IFT_STRING
string field
Definition: types.h:48
Timestamp data, must be present and first entries for each interface data structs!...
Definition: message.h:152