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 *
83 TransformInterface::frame() const
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
93 TransformInterface::maxlenof_frame() const
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 *
119 TransformInterface::child_frame() const
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
129 TransformInterface::maxlenof_child_frame() const
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
155 TransformInterface::is_static_transform() const
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
165 TransformInterface::maxlenof_static_transform() const
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 *
192 TransformInterface::translation() const
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
221 TransformInterface::maxlenof_translation() const
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 *
264 TransformInterface::rotation() const
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
294 TransformInterface::maxlenof_rotation() const
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 *
331 TransformInterface::create_message(const char *type) const
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
342 TransformInterface::copy_values(const Interface *other)
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
364 TransformInterface::message_valid(const Message *message) const
365 {
366  return false;
367 }
368 
369 /// @cond INTERNALS
370 EXPORT_INTERFACE(TransformInterface)
371 /// @endcond
372 
373 
374 } // end namespace fawkes
Base class for exceptions in Fawkes.
Definition: exception.h:36
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
const char * type() const
Get type of interface.
Definition: interface.cpp:643
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:45
TransformInterface Fawkes BlackBoard Interface.
Fawkes library namespace.
bool change_field(FieldT &field, const DataT &value)
Set a field and return whether it changed.
Definition: message.h:167