Fawkes API  Fawkes Development Version
TransformInterface.cpp
1 
2 /***************************************************************************
3  * TransformInterface.cpp - Fawkes BlackBoard Interface - TransformInterface
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/TransformInterface.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 TransformInterface <interfaces/TransformInterface.h>
36  * TransformInterface Fawkes BlackBoard Interface.
37  *
38  This interface is used to publish transforms. It aims to be as
39  compatible as possible with ROS' tf library and is used
40  extensively by the Fawkes tf library.
41 
42  For this to work properly it is crucial to have correct
43  timestamp set (cf. Interface::set_timestamp()). Set this as
44  close as possible to the time of when the data, from which the
45  transform is computed, has been acquired.
46 
47  * @ingroup FawkesInterfaces
48  */
49 
50 
51 
52 /** Constructor */
53 TransformInterface::TransformInterface() : Interface()
54 {
55  data_size = sizeof(TransformInterface_data_t);
56  data_ptr = malloc(data_size);
57  data = (TransformInterface_data_t *)data_ptr;
58  data_ts = (interface_data_ts_t *)data_ptr;
59  memset(data_ptr, 0, data_size);
60  add_fieldinfo(IFT_STRING, "frame", 64, data->frame);
61  add_fieldinfo(IFT_STRING, "child_frame", 64, data->child_frame);
62  add_fieldinfo(IFT_BOOL, "static_transform", 1, &data->static_transform);
63  add_fieldinfo(IFT_DOUBLE, "translation", 3, &data->translation);
64  add_fieldinfo(IFT_DOUBLE, "rotation", 4, &data->rotation);
65  unsigned char tmp_hash[] = {0xb6, 0xb0, 0xd3, 0x96, 0xda, 0x61, 0xdd, 0xd3, 0x6, 0x9e, 0x66, 0x4d, 0x14, 0x54, 0x5e, 0xfb};
66  set_hash(tmp_hash);
67 }
68 
69 /** Destructor */
70 TransformInterface::~TransformInterface()
71 {
72  free(data_ptr);
73 }
74 /* Methods */
75 /** Get frame value.
76  *
77  Parent frame ID. The given transform is relative to the origin
78  of this coordinate frame.
79 
80  * @return frame value
81  */
82 char *
84 {
85  return data->frame;
86 }
87 
88 /** Get maximum length of frame value.
89  * @return length of frame value, can be length of the array or number of
90  * maximum number of characters for a string
91  */
92 size_t
94 {
95  return 64;
96 }
97 
98 /** Set frame value.
99  *
100  Parent frame ID. The given transform is relative to the origin
101  of this coordinate frame.
102 
103  * @param new_frame new frame value
104  */
105 void
106 TransformInterface::set_frame(const char * new_frame)
107 {
108  strncpy(data->frame, new_frame, sizeof(data->frame));
109  data_changed = true;
110 }
111 
112 /** Get child_frame value.
113  *
114  The ID of the child frame. The child frame's origin is at the
115  given point in the parent frame denoted by the transform.
116 
117  * @return child_frame value
118  */
119 char *
121 {
122  return data->child_frame;
123 }
124 
125 /** Get maximum length of child_frame value.
126  * @return length of child_frame value, can be length of the array or number of
127  * maximum number of characters for a string
128  */
129 size_t
131 {
132  return 64;
133 }
134 
135 /** Set child_frame value.
136  *
137  The ID of the child frame. The child frame's origin is at the
138  given point in the parent frame denoted by the transform.
139 
140  * @param new_child_frame new child_frame value
141  */
142 void
143 TransformInterface::set_child_frame(const char * new_child_frame)
144 {
145  strncpy(data->child_frame, new_child_frame, sizeof(data->child_frame));
146  data_changed = true;
147 }
148 
149 /** Get static_transform value.
150  *
151  True if the transform is static, i.e. it will never change
152  during its lifetime, false otherwise.
153 
154  * @return static_transform value
155  */
156 bool
158 {
159  return data->static_transform;
160 }
161 
162 /** Get maximum length of static_transform value.
163  * @return length of static_transform value, can be length of the array or number of
164  * maximum number of characters for a string
165  */
166 size_t
168 {
169  return 1;
170 }
171 
172 /** Set static_transform value.
173  *
174  True if the transform is static, i.e. it will never change
175  during its lifetime, false otherwise.
176 
177  * @param new_static_transform new static_transform value
178  */
179 void
180 TransformInterface::set_static_transform(const bool new_static_transform)
181 {
182  data->static_transform = new_static_transform;
183  data_changed = true;
184 }
185 
186 /** Get translation value.
187  *
188  This array denotes the translation vector of the transform. The
189  element indexes are ordered x, y, z, i.e. translation[0] is the
190  X value of the translation vector.
191 
192  * @return translation value
193  */
194 double *
196 {
197  return data->translation;
198 }
199 
200 /** Get translation value at given index.
201  *
202  This array denotes the translation vector of the transform. The
203  element indexes are ordered x, y, z, i.e. translation[0] is the
204  X value of the translation vector.
205 
206  * @param index index of value
207  * @return translation value
208  * @exception Exception thrown if index is out of bounds
209  */
210 double
211 TransformInterface::translation(unsigned int index) const
212 {
213  if (index > 3) {
214  throw Exception("Index value %u out of bounds (0..3)", index);
215  }
216  return data->translation[index];
217 }
218 
219 /** Get maximum length of translation value.
220  * @return length of translation value, can be length of the array or number of
221  * maximum number of characters for a string
222  */
223 size_t
225 {
226  return 3;
227 }
228 
229 /** Set translation value.
230  *
231  This array denotes the translation vector of the transform. The
232  element indexes are ordered x, y, z, i.e. translation[0] is the
233  X value of the translation vector.
234 
235  * @param new_translation new translation value
236  */
237 void
238 TransformInterface::set_translation(const double * new_translation)
239 {
240  memcpy(data->translation, new_translation, sizeof(double) * 3);
241  data_changed = true;
242 }
243 
244 /** Set translation value at given index.
245  *
246  This array denotes the translation vector of the transform. The
247  element indexes are ordered x, y, z, i.e. translation[0] is the
248  X value of the translation vector.
249 
250  * @param new_translation new translation value
251  * @param index index for of the value
252  */
253 void
254 TransformInterface::set_translation(unsigned int index, const double new_translation)
255 {
256  if (index > 3) {
257  throw Exception("Index value %u out of bounds (0..3)", index);
258  }
259  data->translation[index] = new_translation;
260  data_changed = true;
261 }
262 /** Get rotation value.
263  *
264  This array denotes the rotation quaternion of the transform. The
265  element indexes are ordered x, y, z, w, i.e. translation[0] is
266  the X value of the rotation quaternion and translation[3] is the
267  W value.
268 
269  * @return rotation value
270  */
271 double *
273 {
274  return data->rotation;
275 }
276 
277 /** Get rotation value at given index.
278  *
279  This array denotes the rotation quaternion of the transform. The
280  element indexes are ordered x, y, z, w, i.e. translation[0] is
281  the X value of the rotation quaternion and translation[3] is the
282  W value.
283 
284  * @param index index of value
285  * @return rotation value
286  * @exception Exception thrown if index is out of bounds
287  */
288 double
289 TransformInterface::rotation(unsigned int index) const
290 {
291  if (index > 4) {
292  throw Exception("Index value %u out of bounds (0..4)", index);
293  }
294  return data->rotation[index];
295 }
296 
297 /** Get maximum length of rotation value.
298  * @return length of rotation value, can be length of the array or number of
299  * maximum number of characters for a string
300  */
301 size_t
303 {
304  return 4;
305 }
306 
307 /** Set rotation value.
308  *
309  This array denotes the rotation quaternion of the transform. The
310  element indexes are ordered x, y, z, w, i.e. translation[0] is
311  the X value of the rotation quaternion and translation[3] is the
312  W value.
313 
314  * @param new_rotation new rotation value
315  */
316 void
317 TransformInterface::set_rotation(const double * new_rotation)
318 {
319  memcpy(data->rotation, new_rotation, sizeof(double) * 4);
320  data_changed = true;
321 }
322 
323 /** Set rotation value at given index.
324  *
325  This array denotes the rotation quaternion of the transform. The
326  element indexes are ordered x, y, z, w, i.e. translation[0] is
327  the X value of the rotation quaternion and translation[3] is the
328  W value.
329 
330  * @param new_rotation new rotation value
331  * @param index index for of the value
332  */
333 void
334 TransformInterface::set_rotation(unsigned int index, const double new_rotation)
335 {
336  if (index > 4) {
337  throw Exception("Index value %u out of bounds (0..4)", index);
338  }
339  data->rotation[index] = new_rotation;
340  data_changed = true;
341 }
342 /* =========== message create =========== */
343 Message *
345 {
346  throw UnknownTypeException("The given type '%s' does not match any known "
347  "message type for this interface type.", type);
348 }
349 
350 
351 /** Copy values from other interface.
352  * @param other other interface to copy values from
353  */
354 void
356 {
357  const TransformInterface *oi = dynamic_cast<const TransformInterface *>(other);
358  if (oi == NULL) {
359  throw TypeMismatchException("Can only copy values from interface of same type (%s vs. %s)",
360  type(), other->type());
361  }
362  memcpy(data, oi->data, sizeof(TransformInterface_data_t));
363 }
364 
365 const char *
366 TransformInterface::enum_tostring(const char *enumtype, int val) const
367 {
368  throw UnknownTypeException("Unknown enum type %s", enumtype);
369 }
370 
371 /* =========== messages =========== */
372 /** Check if message is valid and can be enqueued.
373  * @param message Message to check
374  * @return true if the message is valid, false otherwise.
375  */
376 bool
378 {
379  return false;
380 }
381 
382 /// @cond INTERNALS
383 EXPORT_INTERFACE(TransformInterface)
384 /// @endcond
385 
386 
387 } // end namespace fawkes
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_hash(unsigned char *ihash)
Set hash.
Definition: interface.cpp:314
Fawkes library namespace.
size_t maxlenof_child_frame() const
Get maximum length of child_frame value.
void set_child_frame(const char *new_child_frame)
Set child_frame value.
virtual bool message_valid(const Message *message) const
Check if message is valid and can be enqueued.
string field
Definition: types.h:47
TransformInterface Fawkes BlackBoard Interface.
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
virtual void copy_values(const Interface *other)
Copy values from other interface.
message_data_ts_t * data_ts
data timestamp aliasing pointer
Definition: message.h:133
unsigned int data_size
Size of memory needed to hold all data.
Definition: message.h:125
size_t maxlenof_frame() const
Get maximum length of frame value.
virtual const char * enum_tostring(const char *enumtype, int val) const
Convert arbitrary enum value to string.
bool data_changed
Indicator if data has changed.
Definition: interface.h:222
const char * type() const
Get type of interface.
Definition: interface.cpp:651
char * child_frame() const
Get child_frame value.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void set_static_transform(const bool new_static_transform)
Set static_transform value.
bool is_static_transform() const
Get static_transform value.
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
double * translation() const
Get translation value.
size_t maxlenof_static_transform() const
Get maximum length of static_transform value.
double * rotation() const
Get rotation value.
virtual Message * create_message(const char *type) const
Create message based on type name.
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
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
boolean field
Definition: types.h:36
size_t maxlenof_translation() const
Get maximum length of translation value.
void set_frame(const char *new_frame)
Set frame value.
char * frame() const
Get frame value.
const char * type() const
Get message type.
Definition: message.cpp:378
size_t maxlenof_rotation() const
Get maximum length of rotation value.
double field
Definition: types.h:46