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 > 2) {
216  throw Exception("Index value %u out of bounds (0..2)", 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 > 2) {
259  throw Exception("Index value %u out of bounds (0..2)", 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 > 3) {
294  throw Exception("Index value %u out of bounds (0..3)", 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 > 3) {
339  throw Exception("Index value %u out of bounds (0..3)", index);
340  }
341  data->rotation[index] = new_rotation;
342  data_changed = true;
343 }
344 /* =========== message create =========== */
345 Message *
346 TransformInterface::create_message(const char *type) const
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
379 TransformInterface::message_valid(const Message *message) const
380 {
381  return false;
382 }
383 
384 /// @cond INTERNALS
385 EXPORT_INTERFACE(TransformInterface)
386 /// @endcond
387 
388 
389 } // end namespace fawkes
fawkes::TransformInterface::enum_tostring
virtual const char * enum_tostring(const char *enumtype, int val) const
Definition: TransformInterface.cpp:374
fawkes::Interface::data_ptr
void * data_ptr
Definition: interface.h:224
fawkes::TransformInterface::set_static_transform
void set_static_transform(const bool new_static_transform)
Set static_transform value.
Definition: TransformInterface.cpp:188
fawkes::TransformInterface::child_frame
char * child_frame() const
Get child_frame value.
Definition: TransformInterface.cpp:127
fawkes::TransformInterface::translation
double * translation() const
Get translation value.
Definition: TransformInterface.cpp:203
fawkes::TransformInterface::maxlenof_static_transform
size_t maxlenof_static_transform() const
Get maximum length of static_transform value.
Definition: TransformInterface.cpp:175
fawkes::TransformInterface::is_static_transform
bool is_static_transform() const
Get static_transform value.
Definition: TransformInterface.cpp:165
fawkes::TransformInterface::set_rotation
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
Definition: TransformInterface.cpp:342
fawkes::Message
Definition: message.h:41
fawkes::Message::data_ptr
void * data_ptr
Definition: message.h:125
fawkes::IFT_BOOL
@ IFT_BOOL
boolean field
Definition: types.h:51
fawkes::Message::data_ts
message_data_ts_t * data_ts
data timestamp aliasing pointer
Definition: message.h:135
fawkes::TransformInterface::rotation
double * rotation() const
Get rotation value.
Definition: TransformInterface.cpp:280
fawkes::Interface::type
const char * type() const
Get type of interface.
Definition: interface.cpp:645
fawkes::TransformInterface::set_translation
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
Definition: TransformInterface.cpp:262
fawkes::TransformInterface::frame
char * frame() const
Get frame value.
Definition: TransformInterface.cpp:89
fawkes::TransformInterface::create_message
virtual Message * create_message(const char *type) const
Definition: TransformInterface.cpp:352
fawkes::TransformInterface::maxlenof_rotation
size_t maxlenof_rotation() const
Get maximum length of rotation value.
Definition: TransformInterface.cpp:310
fawkes::TransformInterface::set_frame
void set_frame(const char *new_frame)
Set frame value.
Definition: TransformInterface.cpp:112
fawkes::TypeMismatchException
Definition: software.h:49
fawkes::Interface::data_changed
bool data_changed
Definition: interface.h:226
fawkes::TransformInterface::maxlenof_translation
size_t maxlenof_translation() const
Get maximum length of translation value.
Definition: TransformInterface.cpp:232
fawkes::UnknownTypeException
Definition: software.h:55
fawkes
fawkes::Interface::set_hash
void set_hash(unsigned char *ihash)
Set hash.
Definition: interface.cpp:321
fawkes::Message::data_size
unsigned int data_size
Definition: message.h:126
fawkes::Interface
Definition: interface.h:78
fawkes::TransformInterface::set_child_frame
void set_child_frame(const char *new_child_frame)
Set child_frame value.
Definition: TransformInterface.cpp:150
fawkes::TransformInterface::maxlenof_child_frame
size_t maxlenof_child_frame() const
Get maximum length of child_frame value.
Definition: TransformInterface.cpp:137
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:406
fawkes::IFT_DOUBLE
@ IFT_DOUBLE
double field
Definition: types.h:61
fawkes::TransformInterface::message_valid
virtual bool message_valid(const Message *message) const
Check if message is valid and can be enqueued.
Definition: TransformInterface.cpp:385
fawkes::TransformInterface
Definition: TransformInterface.h:39
fawkes::IFT_STRING
@ IFT_STRING
string field
Definition: types.h:62
fawkes::TransformInterface::maxlenof_frame
size_t maxlenof_frame() const
Get maximum length of frame value.
Definition: TransformInterface.cpp:99
fawkes::TransformInterface::copy_values
virtual void copy_values(const Interface *other)
Copy values from other interface.
Definition: TransformInterface.cpp:363
fawkes::Exception
Definition: exception.h:41