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