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)-1);
95  data->map[sizeof(data->map)-1] = 0;
96  data_changed = true;
97 }
98 
99 /* =========== message create =========== */
100 Message *
102 {
103  if ( strncmp("SetInitialPoseMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
104  return new SetInitialPoseMessage();
105  } else {
106  throw UnknownTypeException("The given type '%s' does not match any known "
107  "message type for this interface type.", type);
108  }
109 }
110 
111 
112 /** Copy values from other interface.
113  * @param other other interface to copy values from
114  */
115 void
117 {
118  const LocalizationInterface *oi = dynamic_cast<const LocalizationInterface *>(other);
119  if (oi == NULL) {
120  throw TypeMismatchException("Can only copy values from interface of same type (%s vs. %s)",
121  type(), other->type());
122  }
123  memcpy(data, oi->data, sizeof(LocalizationInterface_data_t));
124 }
125 
126 const char *
127 LocalizationInterface::enum_tostring(const char *enumtype, int val) const
128 {
129  throw UnknownTypeException("Unknown enum type %s", enumtype);
130 }
131 
132 /* =========== messages =========== */
133 /** @class LocalizationInterface::SetInitialPoseMessage <interfaces/LocalizationInterface.h>
134  * SetInitialPoseMessage Fawkes BlackBoard Interface Message.
135  *
136 
137  */
138 
139 
140 /** Constructor with initial values.
141  * @param ini_frame initial value for frame
142  * @param ini_rotation initial value for rotation
143  * @param ini_translation initial value for translation
144  * @param ini_covariance initial value for covariance
145  */
146 LocalizationInterface::SetInitialPoseMessage::SetInitialPoseMessage(const char * ini_frame, const double * ini_rotation, const double * ini_translation, const double * ini_covariance) : Message("SetInitialPoseMessage")
147 {
148  data_size = sizeof(SetInitialPoseMessage_data_t);
149  data_ptr = malloc(data_size);
150  memset(data_ptr, 0, data_size);
151  data = (SetInitialPoseMessage_data_t *)data_ptr;
153  strncpy(data->frame, ini_frame, 32-1);
154  data->frame[32-1] = 0;
155  memcpy(data->rotation, ini_rotation, sizeof(double) * 4);
156  memcpy(data->translation, ini_translation, sizeof(double) * 3);
157  memcpy(data->covariance, ini_covariance, sizeof(double) * 36);
158  add_fieldinfo(IFT_STRING, "frame", 32, data->frame);
159  add_fieldinfo(IFT_DOUBLE, "rotation", 4, &data->rotation);
160  add_fieldinfo(IFT_DOUBLE, "translation", 3, &data->translation);
161  add_fieldinfo(IFT_DOUBLE, "covariance", 36, &data->covariance);
162 }
163 /** Constructor */
165 {
166  data_size = sizeof(SetInitialPoseMessage_data_t);
167  data_ptr = malloc(data_size);
168  memset(data_ptr, 0, data_size);
169  data = (SetInitialPoseMessage_data_t *)data_ptr;
171  add_fieldinfo(IFT_STRING, "frame", 32, data->frame);
172  add_fieldinfo(IFT_DOUBLE, "rotation", 4, &data->rotation);
173  add_fieldinfo(IFT_DOUBLE, "translation", 3, &data->translation);
174  add_fieldinfo(IFT_DOUBLE, "covariance", 36, &data->covariance);
175 }
176 
177 /** Destructor */
179 {
180  free(data_ptr);
181 }
182 
183 /** Copy constructor.
184  * @param m message to copy from
185  */
187 {
188  data_size = m->data_size;
189  data_ptr = malloc(data_size);
191  data = (SetInitialPoseMessage_data_t *)data_ptr;
193 }
194 
195 /* Methods */
196 /** Get frame value.
197  *
198  Reference coordinate frame for the data.
199 
200  * @return frame value
201  */
202 char *
204 {
205  return data->frame;
206 }
207 
208 /** Get maximum length of frame value.
209  * @return length of frame value, can be length of the array or number of
210  * maximum number of characters for a string
211  */
212 size_t
214 {
215  return 32;
216 }
217 
218 /** Set frame value.
219  *
220  Reference coordinate frame for the data.
221 
222  * @param new_frame new frame value
223  */
224 void
226 {
227  strncpy(data->frame, new_frame, sizeof(data->frame)-1);
228  data->frame[sizeof(data->frame)-1] = 0;
229 }
230 
231 /** Get rotation value.
232  *
233  Rotation quaternion relative to reference frame, ordered as (x, y, z, w).
234 
235  * @return rotation value
236  */
237 double *
239 {
240  return data->rotation;
241 }
242 
243 /** Get rotation value at given index.
244  *
245  Rotation quaternion relative to reference frame, ordered as (x, y, z, w).
246 
247  * @param index index of value
248  * @return rotation value
249  * @exception Exception thrown if index is out of bounds
250  */
251 double
253 {
254  if (index > 3) {
255  throw Exception("Index value %u out of bounds (0..3)", index);
256  }
257  return data->rotation[index];
258 }
259 
260 /** Get maximum length of rotation value.
261  * @return length of rotation value, can be length of the array or number of
262  * maximum number of characters for a string
263  */
264 size_t
266 {
267  return 4;
268 }
269 
270 /** Set rotation value.
271  *
272  Rotation quaternion relative to reference frame, ordered as (x, y, z, w).
273 
274  * @param new_rotation new rotation value
275  */
276 void
278 {
279  memcpy(data->rotation, new_rotation, sizeof(double) * 4);
280 }
281 
282 /** Set rotation value at given index.
283  *
284  Rotation quaternion relative to reference frame, ordered as (x, y, z, w).
285 
286  * @param new_rotation new rotation value
287  * @param index index for of the value
288  */
289 void
290 LocalizationInterface::SetInitialPoseMessage::set_rotation(unsigned int index, const double new_rotation)
291 {
292  if (index > 3) {
293  throw Exception("Index value %u out of bounds (0..3)", index);
294  }
295  data->rotation[index] = new_rotation;
296 }
297 /** Get translation value.
298  *
299  Translation vector from the reference frame's origin, ordered as (x, y, z).
300 
301  * @return translation value
302  */
303 double *
305 {
306  return data->translation;
307 }
308 
309 /** Get translation value at given index.
310  *
311  Translation vector from the reference frame's origin, ordered as (x, y, z).
312 
313  * @param index index of value
314  * @return translation value
315  * @exception Exception thrown if index is out of bounds
316  */
317 double
319 {
320  if (index > 2) {
321  throw Exception("Index value %u out of bounds (0..2)", index);
322  }
323  return data->translation[index];
324 }
325 
326 /** Get maximum length of translation value.
327  * @return length of translation value, can be length of the array or number of
328  * maximum number of characters for a string
329  */
330 size_t
332 {
333  return 3;
334 }
335 
336 /** Set translation value.
337  *
338  Translation vector from the reference frame's origin, ordered as (x, y, z).
339 
340  * @param new_translation new translation value
341  */
342 void
344 {
345  memcpy(data->translation, new_translation, sizeof(double) * 3);
346 }
347 
348 /** Set translation value at given index.
349  *
350  Translation vector from the reference frame's origin, ordered as (x, y, z).
351 
352  * @param new_translation new translation value
353  * @param index index for of the value
354  */
355 void
356 LocalizationInterface::SetInitialPoseMessage::set_translation(unsigned int index, const double new_translation)
357 {
358  if (index > 2) {
359  throw Exception("Index value %u out of bounds (0..2)", index);
360  }
361  data->translation[index] = new_translation;
362 }
363 /** Get covariance value.
364  *
365  Row-major representation of the 6x6 covariance matrix. The
366  orientation parameters use a fixed-axis representation. In
367  order, the parameters are: (x, y, z, rotation about X axis,
368  rotation about Y axis, rotation about Z axis).
369 
370  * @return covariance value
371  */
372 double *
374 {
375  return data->covariance;
376 }
377 
378 /** Get covariance value at given index.
379  *
380  Row-major representation of the 6x6 covariance matrix. The
381  orientation parameters use a fixed-axis representation. In
382  order, the parameters are: (x, y, z, rotation about X axis,
383  rotation about Y axis, rotation about Z axis).
384 
385  * @param index index of value
386  * @return covariance value
387  * @exception Exception thrown if index is out of bounds
388  */
389 double
391 {
392  if (index > 35) {
393  throw Exception("Index value %u out of bounds (0..35)", index);
394  }
395  return data->covariance[index];
396 }
397 
398 /** Get maximum length of covariance value.
399  * @return length of covariance value, can be length of the array or number of
400  * maximum number of characters for a string
401  */
402 size_t
404 {
405  return 36;
406 }
407 
408 /** Set covariance value.
409  *
410  Row-major representation of the 6x6 covariance matrix. The
411  orientation parameters use a fixed-axis representation. In
412  order, the parameters are: (x, y, z, rotation about X axis,
413  rotation about Y axis, rotation about Z axis).
414 
415  * @param new_covariance new covariance value
416  */
417 void
419 {
420  memcpy(data->covariance, new_covariance, sizeof(double) * 36);
421 }
422 
423 /** Set covariance value at given index.
424  *
425  Row-major representation of the 6x6 covariance matrix. The
426  orientation parameters use a fixed-axis representation. In
427  order, the parameters are: (x, y, z, rotation about X axis,
428  rotation about Y axis, rotation about Z axis).
429 
430  * @param new_covariance new covariance value
431  * @param index index for of the value
432  */
433 void
434 LocalizationInterface::SetInitialPoseMessage::set_covariance(unsigned int index, const double new_covariance)
435 {
436  if (index > 35) {
437  throw Exception("Index value %u out of bounds (0..35)", index);
438  }
439  data->covariance[index] = new_covariance;
440 }
441 /** Clone this message.
442  * Produces a message of the same type as this message and copies the
443  * data to the new message.
444  * @return clone of this message
445  */
446 Message *
448 {
450 }
451 /** Check if message is valid and can be enqueued.
452  * @param message Message to check
453  * @return true if the message is valid, false otherwise.
454  */
455 bool
456 LocalizationInterface::message_valid(const Message *message) const
457 {
458  const SetInitialPoseMessage *m0 = dynamic_cast<const SetInitialPoseMessage *>(message);
459  if ( m0 != NULL ) {
460  return true;
461  }
462  return false;
463 }
464 
465 /// @cond INTERNALS
466 EXPORT_INTERFACE(LocalizationInterface)
467 /// @endcond
468 
469 
470 } // end namespace fawkes
fawkes::Interface::data_ptr
void * data_ptr
Definition: interface.h:223
fawkes::LocalizationInterface::SetInitialPoseMessage::covariance
double * covariance() const
Get covariance value.
Definition: LocalizationInterface.cpp:377
fawkes::LocalizationInterface::SetInitialPoseMessage::clone
virtual Message * clone() const
Clone this message.
Definition: LocalizationInterface.cpp:451
fawkes::Message
Definition: message.h:40
fawkes::Message::data_ptr
void * data_ptr
Definition: message.h:124
fawkes::LocalizationInterface::SetInitialPoseMessage::translation
double * translation() const
Get translation value.
Definition: LocalizationInterface.cpp:308
fawkes::LocalizationInterface::message_valid
virtual bool message_valid(const Message *message) const
Check if message is valid and can be enqueued.
Definition: LocalizationInterface.cpp:460
fawkes::Message::data_ts
message_data_ts_t * data_ts
data timestamp aliasing pointer
Definition: message.h:134
fawkes::LocalizationInterface::map
char * map() const
Get map value.
Definition: LocalizationInterface.cpp:76
fawkes::LocalizationInterface::SetInitialPoseMessage
Definition: LocalizationInterface.h:62
fawkes::Interface::type
const char * type() const
Get type of interface.
Definition: interface.cpp:643
fawkes::LocalizationInterface::SetInitialPoseMessage::set_rotation
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
Definition: LocalizationInterface.cpp:294
fawkes::LocalizationInterface::SetInitialPoseMessage::~SetInitialPoseMessage
~SetInitialPoseMessage()
Destructor.
Definition: LocalizationInterface.cpp:182
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::LocalizationInterface::maxlenof_map
size_t maxlenof_map() const
Get maximum length of map value.
Definition: LocalizationInterface.cpp:86
fawkes::Message::message_data_ts_t
Timestamp data, must be present and first entries for each interface data structs!...
Definition: message.h:129
fawkes::TypeMismatchException
Definition: software.h:47
fawkes::Interface::data_changed
bool data_changed
Definition: interface.h:225
fawkes::LocalizationInterface::copy_values
virtual void copy_values(const Interface *other)
Copy values from other interface.
Definition: LocalizationInterface.cpp:120
fawkes::LocalizationInterface::SetInitialPoseMessage::maxlenof_frame
size_t maxlenof_frame() const
Get maximum length of frame value.
Definition: LocalizationInterface.cpp:217
fawkes::LocalizationInterface::enum_tostring
virtual const char * enum_tostring(const char *enumtype, int val) const
Definition: LocalizationInterface.cpp:131
fawkes::LocalizationInterface::SetInitialPoseMessage::SetInitialPoseMessage
SetInitialPoseMessage()
Constructor.
Definition: LocalizationInterface.cpp:168
fawkes::UnknownTypeException
Definition: software.h:53
fawkes::Interface::Interface
Interface()
Constructor.
Definition: interface.cpp:236
fawkes::LocalizationInterface::SetInitialPoseMessage::set_covariance
void set_covariance(unsigned int index, const double new_covariance)
Set covariance value at given index.
Definition: LocalizationInterface.cpp:438
fawkes
fawkes::Interface::set_hash
void set_hash(unsigned char *ihash)
Set hash.
Definition: interface.cpp:319
fawkes::Message::data_size
unsigned int data_size
Definition: message.h:125
fawkes::LocalizationInterface::set_map
void set_map(const char *new_map)
Set map value.
Definition: LocalizationInterface.cpp:96
fawkes::LocalizationInterface::SetInitialPoseMessage::set_frame
void set_frame(const char *new_frame)
Set frame value.
Definition: LocalizationInterface.cpp:229
fawkes::Message::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 info list.
Definition: message.cpp:404
fawkes::LocalizationInterface::SetInitialPoseMessage::frame
char * frame() const
Get frame value.
Definition: LocalizationInterface.cpp:207
fawkes::IFT_DOUBLE
double field
Definition: types.h:58
fawkes::LocalizationInterface::SetInitialPoseMessage::rotation
double * rotation() const
Get rotation value.
Definition: LocalizationInterface.cpp:242
fawkes::Interface::data_size
unsigned int data_size
Definition: interface.h:224
fawkes::LocalizationInterface::SetInitialPoseMessage::maxlenof_translation
size_t maxlenof_translation() const
Get maximum length of translation value.
Definition: LocalizationInterface.cpp:335
fawkes::IFT_STRING
string field
Definition: types.h:59
fawkes::Interface::add_messageinfo
void add_messageinfo(const char *name)
Add an entry to the message info list.
Definition: interface.cpp:378
fawkes::LocalizationInterface::SetInitialPoseMessage::set_translation
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
Definition: LocalizationInterface.cpp:360
fawkes::LocalizationInterface::SetInitialPoseMessage::maxlenof_rotation
size_t maxlenof_rotation() const
Get maximum length of rotation value.
Definition: LocalizationInterface.cpp:269
fawkes::LocalizationInterface::SetInitialPoseMessage::maxlenof_covariance
size_t maxlenof_covariance() const
Get maximum length of covariance value.
Definition: LocalizationInterface.cpp:407
fawkes::LocalizationInterface::create_message
virtual Message * create_message(const char *type) const
Definition: LocalizationInterface.cpp:105
fawkes::Exception
Definition: exception.h:39