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 
33 namespace 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 */
48 LocalizationInterface::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 */
62 LocalizationInterface::~LocalizationInterface()
63 {
64  free(data_ptr);
65 }
66 /* Methods */
67 /** Get map value.
68  * The currently used map.
69  * @return map value
70  */
71 char *
72 LocalizationInterface::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  */
81 size_t
82 LocalizationInterface::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  */
91 void
92 LocalizationInterface::set_map(const char * new_map)
93 {
94  data_changed |= change_field(data->map, new_map);
95 }
96 
97 /* =========== message create =========== */
98 Message *
99 LocalizationInterface::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  */
113 void
114 LocalizationInterface::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 
124 const char *
125 LocalizationInterface::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  */
144 LocalizationInterface::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  */
200 char *
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  */
210 size_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  */
222 void
224 {
225  change_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  */
234 double *
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  */
248 double
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  */
261 size_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  */
273 void
275 {
276  change_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  */
286 void
287 LocalizationInterface::SetInitialPoseMessage::set_rotation(unsigned int index, const double new_rotation)
288 {
289  change_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  */
297 double *
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  */
311 double
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  */
324 size_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  */
336 void
338 {
339  change_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  */
349 void
350 LocalizationInterface::SetInitialPoseMessage::set_translation(unsigned int index, const double new_translation)
351 {
352  change_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  */
363 double *
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  */
380 double
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  */
393 size_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  */
408 void
410 {
411  change_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  */
424 void
425 LocalizationInterface::SetInitialPoseMessage::set_covariance(unsigned int index, const double new_covariance)
426 {
427  change_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  */
434 Message *
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  */
443 bool
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
454 EXPORT_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:79
const char * type() const
Get type of interface.
Definition: interface.cpp:643
void * data_ptr
Pointer to local memory storage.
Definition: interface.h:224
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:45
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:400
void * data_ptr
Pointer to memory that contains local data.
Definition: message.h:128
message_data_ts_t * data_ts
data timestamp aliasing pointer
Definition: message.h:138
unsigned int data_size
Size of memory needed to hold all data.
Definition: message.h:129
Fawkes library namespace.
@ IFT_DOUBLE
double field
Definition: types.h:47
@ IFT_STRING
string field
Definition: types.h:48
bool change_field(FieldT &field, const DataT &value)
Set a field and return whether it changed.
Definition: message.h:167
Timestamp data, must be present and first entries for each interface data structs!...
Definition: message.h:134