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)-1);
109  data->frame[sizeof(data->frame)-1] = 0;
110  data_changed = true;
111 }
112 
113 /** Get child_frame value.
114  *
115  The ID of the child frame. The child frame's origin is at the
116  given point in the parent frame denoted by the transform.
117 
118  * @return child_frame value
119  */
120 char *
122 {
123  return data->child_frame;
124 }
125 
126 /** Get maximum length of child_frame value.
127  * @return length of child_frame value, can be length of the array or number of
128  * maximum number of characters for a string
129  */
130 size_t
132 {
133  return 64;
134 }
135 
136 /** Set child_frame value.
137  *
138  The ID of the child frame. The child frame's origin is at the
139  given point in the parent frame denoted by the transform.
140 
141  * @param new_child_frame new child_frame value
142  */
143 void
144 TransformInterface::set_child_frame(const char * new_child_frame)
145 {
146  strncpy(data->child_frame, new_child_frame, sizeof(data->child_frame)-1);
147  data->child_frame[sizeof(data->child_frame)-1] = 0;
148  data_changed = true;
149 }
150 
151 /** Get static_transform value.
152  *
153  True if the transform is static, i.e. it will never change
154  during its lifetime, false otherwise.
155 
156  * @return static_transform value
157  */
158 bool
160 {
161  return data->static_transform;
162 }
163 
164 /** Get maximum length of static_transform value.
165  * @return length of static_transform value, can be length of the array or number of
166  * maximum number of characters for a string
167  */
168 size_t
170 {
171  return 1;
172 }
173 
174 /** Set static_transform value.
175  *
176  True if the transform is static, i.e. it will never change
177  during its lifetime, false otherwise.
178 
179  * @param new_static_transform new static_transform value
180  */
181 void
182 TransformInterface::set_static_transform(const bool new_static_transform)
183 {
184  data->static_transform = new_static_transform;
185  data_changed = true;
186 }
187 
188 /** Get translation value.
189  *
190  This array denotes the translation vector of the transform. The
191  element indexes are ordered x, y, z, i.e. translation[0] is the
192  X value of the translation vector.
193 
194  * @return translation value
195  */
196 double *
198 {
199  return data->translation;
200 }
201 
202 /** Get translation value at given index.
203  *
204  This array denotes the translation vector of the transform. The
205  element indexes are ordered x, y, z, i.e. translation[0] is the
206  X value of the translation vector.
207 
208  * @param index index of value
209  * @return translation value
210  * @exception Exception thrown if index is out of bounds
211  */
212 double
213 TransformInterface::translation(unsigned int index) const
214 {
215  if (index > 3) {
216  throw Exception("Index value %u out of bounds (0..3)", index);
217  }
218  return data->translation[index];
219 }
220 
221 /** Get maximum length of translation value.
222  * @return length of translation value, can be length of the array or number of
223  * maximum number of characters for a string
224  */
225 size_t
227 {
228  return 3;
229 }
230 
231 /** Set translation value.
232  *
233  This array denotes the translation vector of the transform. The
234  element indexes are ordered x, y, z, i.e. translation[0] is the
235  X value of the translation vector.
236 
237  * @param new_translation new translation value
238  */
239 void
240 TransformInterface::set_translation(const double * new_translation)
241 {
242  memcpy(data->translation, new_translation, sizeof(double) * 3);
243  data_changed = true;
244 }
245 
246 /** Set translation value at given index.
247  *
248  This array denotes the translation vector of the transform. The
249  element indexes are ordered x, y, z, i.e. translation[0] is the
250  X value of the translation vector.
251 
252  * @param new_translation new translation value
253  * @param index index for of the value
254  */
255 void
256 TransformInterface::set_translation(unsigned int index, const double new_translation)
257 {
258  if (index > 3) {
259  throw Exception("Index value %u out of bounds (0..3)", index);
260  }
261  data->translation[index] = new_translation;
262  data_changed = true;
263 }
264 /** Get rotation value.
265  *
266  This array denotes the rotation quaternion of the transform. The
267  element indexes are ordered x, y, z, w, i.e. translation[0] is
268  the X value of the rotation quaternion and translation[3] is the
269  W value.
270 
271  * @return rotation value
272  */
273 double *
275 {
276  return data->rotation;
277 }
278 
279 /** Get rotation value at given index.
280  *
281  This array denotes the rotation quaternion of the transform. The
282  element indexes are ordered x, y, z, w, i.e. translation[0] is
283  the X value of the rotation quaternion and translation[3] is the
284  W value.
285 
286  * @param index index of value
287  * @return rotation value
288  * @exception Exception thrown if index is out of bounds
289  */
290 double
291 TransformInterface::rotation(unsigned int index) const
292 {
293  if (index > 4) {
294  throw Exception("Index value %u out of bounds (0..4)", index);
295  }
296  return data->rotation[index];
297 }
298 
299 /** Get maximum length of rotation value.
300  * @return length of rotation value, can be length of the array or number of
301  * maximum number of characters for a string
302  */
303 size_t
305 {
306  return 4;
307 }
308 
309 /** Set rotation value.
310  *
311  This array denotes the rotation quaternion of the transform. The
312  element indexes are ordered x, y, z, w, i.e. translation[0] is
313  the X value of the rotation quaternion and translation[3] is the
314  W value.
315 
316  * @param new_rotation new rotation value
317  */
318 void
319 TransformInterface::set_rotation(const double * new_rotation)
320 {
321  memcpy(data->rotation, new_rotation, sizeof(double) * 4);
322  data_changed = true;
323 }
324 
325 /** Set rotation value at given index.
326  *
327  This array denotes the rotation quaternion of the transform. The
328  element indexes are ordered x, y, z, w, i.e. translation[0] is
329  the X value of the rotation quaternion and translation[3] is the
330  W value.
331 
332  * @param new_rotation new rotation value
333  * @param index index for of the value
334  */
335 void
336 TransformInterface::set_rotation(unsigned int index, const double new_rotation)
337 {
338  if (index > 4) {
339  throw Exception("Index value %u out of bounds (0..4)", index);
340  }
341  data->rotation[index] = new_rotation;
342  data_changed = true;
343 }
344 /* =========== message create =========== */
345 Message *
347 {
348  throw UnknownTypeException("The given type '%s' does not match any known "
349  "message type for this interface type.", type);
350 }
351 
352 
353 /** Copy values from other interface.
354  * @param other other interface to copy values from
355  */
356 void
358 {
359  const TransformInterface *oi = dynamic_cast<const TransformInterface *>(other);
360  if (oi == NULL) {
361  throw TypeMismatchException("Can only copy values from interface of same type (%s vs. %s)",
362  type(), other->type());
363  }
364  memcpy(data, oi->data, sizeof(TransformInterface_data_t));
365 }
366 
367 const char *
368 TransformInterface::enum_tostring(const char *enumtype, int val) const
369 {
370  throw UnknownTypeException("Unknown enum type %s", enumtype);
371 }
372 
373 /* =========== messages =========== */
374 /** Check if message is valid and can be enqueued.
375  * @param message Message to check
376  * @return true if the message is valid, false otherwise.
377  */
378 bool
380 {
381  return false;
382 }
383 
384 /// @cond INTERNALS
385 EXPORT_INTERFACE(TransformInterface)
386 /// @endcond
387 
388 
389 } // end namespace fawkes
void * data_ptr
Pointer to memory that contains local data.
Definition: message.h:125
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:41
void set_hash(unsigned char *ihash)
Set hash.
Definition: interface.cpp:316
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:48
TransformInterface Fawkes BlackBoard Interface.
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:78
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:135
unsigned int data_size
Size of memory needed to hold all data.
Definition: message.h:126
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:226
const char * type() const
Get type of interface.
Definition: interface.cpp:640
void * data_ptr
Pointer to local memory storage.
Definition: interface.h:224
char * child_frame() const
Get child_frame value.
Base class for exceptions in Fawkes.
Definition: exception.h:35
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:410
boolean field
Definition: types.h:37
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.
size_t maxlenof_rotation() const
Get maximum length of rotation value.
double field
Definition: types.h:47