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 *
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
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  strncpy(data->map, new_map, sizeof(data->map));
95  data_changed = true;
96 }
97 
98 /* =========== message create =========== */
99 Message *
101 {
102  if ( strncmp("SetInitialPoseMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
103  return new SetInitialPoseMessage();
104  } else {
105  throw UnknownTypeException("The given type '%s' does not match any known "
106  "message type for this interface type.", type);
107  }
108 }
109 
110 
111 /** Copy values from other interface.
112  * @param other other interface to copy values from
113  */
114 void
116 {
117  const LocalizationInterface *oi = dynamic_cast<const LocalizationInterface *>(other);
118  if (oi == NULL) {
119  throw TypeMismatchException("Can only copy values from interface of same type (%s vs. %s)",
120  type(), other->type());
121  }
122  memcpy(data, oi->data, sizeof(LocalizationInterface_data_t));
123 }
124 
125 const char *
126 LocalizationInterface::enum_tostring(const char *enumtype, int val) const
127 {
128  throw UnknownTypeException("Unknown enum type %s", enumtype);
129 }
130 
131 /* =========== messages =========== */
132 /** @class LocalizationInterface::SetInitialPoseMessage <interfaces/LocalizationInterface.h>
133  * SetInitialPoseMessage Fawkes BlackBoard Interface Message.
134  *
135 
136  */
137 
138 
139 /** Constructor with initial values.
140  * @param ini_frame initial value for frame
141  * @param ini_rotation initial value for rotation
142  * @param ini_translation initial value for translation
143  * @param ini_covariance initial value for covariance
144  */
145 LocalizationInterface::SetInitialPoseMessage::SetInitialPoseMessage(const char * ini_frame, const double * ini_rotation, const double * ini_translation, const double * ini_covariance) : Message("SetInitialPoseMessage")
146 {
147  data_size = sizeof(SetInitialPoseMessage_data_t);
148  data_ptr = malloc(data_size);
149  memset(data_ptr, 0, data_size);
150  data = (SetInitialPoseMessage_data_t *)data_ptr;
152  strncpy(data->frame, ini_frame, 32);
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  strncpy(data->frame, new_frame, sizeof(data->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 > 4) {
252  throw Exception("Index value %u out of bounds (0..4)", 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  memcpy(data->rotation, new_rotation, sizeof(double) * 4);
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  if (index > 4) {
290  throw Exception("Index value %u out of bounds (0..4)", index);
291  }
292  data->rotation[index] = new_rotation;
293 }
294 /** Get translation value.
295  *
296  Translation vector from the reference frame's origin, ordered as (x, y, z).
297 
298  * @return translation value
299  */
300 double *
302 {
303  return data->translation;
304 }
305 
306 /** Get translation value at given index.
307  *
308  Translation vector from the reference frame's origin, ordered as (x, y, z).
309 
310  * @param index index of value
311  * @return translation value
312  * @exception Exception thrown if index is out of bounds
313  */
314 double
316 {
317  if (index > 3) {
318  throw Exception("Index value %u out of bounds (0..3)", index);
319  }
320  return data->translation[index];
321 }
322 
323 /** Get maximum length of translation value.
324  * @return length of translation value, can be length of the array or number of
325  * maximum number of characters for a string
326  */
327 size_t
329 {
330  return 3;
331 }
332 
333 /** Set translation value.
334  *
335  Translation vector from the reference frame's origin, ordered as (x, y, z).
336 
337  * @param new_translation new translation value
338  */
339 void
341 {
342  memcpy(data->translation, new_translation, sizeof(double) * 3);
343 }
344 
345 /** Set translation value at given index.
346  *
347  Translation vector from the reference frame's origin, ordered as (x, y, z).
348 
349  * @param new_translation new translation value
350  * @param index index for of the value
351  */
352 void
353 LocalizationInterface::SetInitialPoseMessage::set_translation(unsigned int index, const double new_translation)
354 {
355  if (index > 3) {
356  throw Exception("Index value %u out of bounds (0..3)", index);
357  }
358  data->translation[index] = new_translation;
359 }
360 /** Get covariance value.
361  *
362  Row-major representation of the 6x6 covariance matrix. The
363  orientation parameters use a fixed-axis representation. In
364  order, the parameters are: (x, y, z, rotation about X axis,
365  rotation about Y axis, rotation about Z axis).
366 
367  * @return covariance value
368  */
369 double *
371 {
372  return data->covariance;
373 }
374 
375 /** Get covariance value at given index.
376  *
377  Row-major representation of the 6x6 covariance matrix. The
378  orientation parameters use a fixed-axis representation. In
379  order, the parameters are: (x, y, z, rotation about X axis,
380  rotation about Y axis, rotation about Z axis).
381 
382  * @param index index of value
383  * @return covariance value
384  * @exception Exception thrown if index is out of bounds
385  */
386 double
388 {
389  if (index > 36) {
390  throw Exception("Index value %u out of bounds (0..36)", index);
391  }
392  return data->covariance[index];
393 }
394 
395 /** Get maximum length of covariance value.
396  * @return length of covariance value, can be length of the array or number of
397  * maximum number of characters for a string
398  */
399 size_t
401 {
402  return 36;
403 }
404 
405 /** Set covariance value.
406  *
407  Row-major representation of the 6x6 covariance matrix. The
408  orientation parameters use a fixed-axis representation. In
409  order, the parameters are: (x, y, z, rotation about X axis,
410  rotation about Y axis, rotation about Z axis).
411 
412  * @param new_covariance new covariance value
413  */
414 void
416 {
417  memcpy(data->covariance, new_covariance, sizeof(double) * 36);
418 }
419 
420 /** Set covariance value at given index.
421  *
422  Row-major representation of the 6x6 covariance matrix. The
423  orientation parameters use a fixed-axis representation. In
424  order, the parameters are: (x, y, z, rotation about X axis,
425  rotation about Y axis, rotation about Z axis).
426 
427  * @param new_covariance new covariance value
428  * @param index index for of the value
429  */
430 void
431 LocalizationInterface::SetInitialPoseMessage::set_covariance(unsigned int index, const double new_covariance)
432 {
433  if (index > 36) {
434  throw Exception("Index value %u out of bounds (0..36)", index);
435  }
436  data->covariance[index] = new_covariance;
437 }
438 /** Clone this message.
439  * Produces a message of the same type as this message and copies the
440  * data to the new message.
441  * @return clone of this message
442  */
443 Message *
445 {
447 }
448 /** Check if message is valid and can be enqueued.
449  * @param message Message to check
450  * @return true if the message is valid, false otherwise.
451  */
452 bool
454 {
455  const SetInitialPoseMessage *m0 = dynamic_cast<const SetInitialPoseMessage *>(message);
456  if ( m0 != NULL ) {
457  return true;
458  }
459  return false;
460 }
461 
462 /// @cond INTERNALS
463 EXPORT_INTERFACE(LocalizationInterface)
464 /// @endcond
465 
466 
467 } // end namespace fawkes
LocalizationInterface Fawkes BlackBoard Interface.
void * data_ptr
Pointer to memory that contains local data.
Definition: message.h:124
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
virtual const char * enum_tostring(const char *enumtype, int val) const
Convert arbitrary enum value to string.
void set_hash(unsigned char *ihash)
Set hash.
Definition: interface.cpp:314
Fawkes library namespace.
Timestamp data, must be present and first entries for each interface data structs! This leans on time...
Definition: message.h:129
void set_map(const char *new_map)
Set map value.
size_t maxlenof_map() const
Get maximum length of map value.
string field
Definition: types.h:47
size_t maxlenof_translation() const
Get maximum length of translation value.
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
virtual Message * create_message(const char *type) const
Create message based on type name.
message_data_ts_t * data_ts
data timestamp aliasing pointer
Definition: message.h:133
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
unsigned int data_size
Size of memory needed to hold all data.
Definition: message.h:125
void add_messageinfo(const char *name)
Add an entry to the message info list.
Definition: interface.cpp:373
bool data_changed
Indicator if data has changed.
Definition: interface.h:222
double * translation() const
Get translation value.
const char * type() const
Get type of interface.
Definition: interface.cpp:651
Base class for exceptions in Fawkes.
Definition: exception.h:36
SetInitialPoseMessage Fawkes BlackBoard Interface Message.
virtual void copy_values(const Interface *other)
Copy values from other interface.
size_t maxlenof_frame() const
Get maximum length of frame value.
size_t maxlenof_rotation() const
Get maximum length of rotation value.
char * map() const
Get map value.
size_t maxlenof_covariance() const
Get maximum length of covariance value.
void set_frame(const char *new_frame)
Set frame value.
virtual Message * clone() const
Clone this message.
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:436
const char * type() const
Get message type.
Definition: message.cpp:378
void set_covariance(unsigned int index, const double new_covariance)
Set covariance value at given index.
virtual bool message_valid(const Message *message) const
Check if message is valid and can be enqueued.
double field
Definition: types.h:46