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
33namespace 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 */
53TransformInterface::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 */
70TransformInterface::~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 */
82char *
83TransformInterface::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 */
92size_t
93TransformInterface::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 */
105void
106TransformInterface::set_frame(const char * new_frame)
107{
108 set_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 */
118char *
119TransformInterface::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 */
128size_t
129TransformInterface::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 */
141void
142TransformInterface::set_child_frame(const char * new_child_frame)
143{
144 set_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 */
154bool
155TransformInterface::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 */
164size_t
165TransformInterface::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 */
177void
178TransformInterface::set_static_transform(const bool new_static_transform)
179{
180 set_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 */
191double *
192TransformInterface::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 */
207double
208TransformInterface::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 */
220size_t
221TransformInterface::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 */
234void
235TransformInterface::set_translation(const double * new_translation)
236{
237 set_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 */
249void
250TransformInterface::set_translation(unsigned int index, const double new_translation)
251{
252 set_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 */
263double *
264TransformInterface::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 */
280double
281TransformInterface::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 */
293size_t
294TransformInterface::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 */
308void
309TransformInterface::set_rotation(const double * new_rotation)
310{
311 set_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 */
324void
325TransformInterface::set_rotation(unsigned int index, const double new_rotation)
326{
327 set_field(data->rotation, index, new_rotation);
328}
329/* =========== message create =========== */
330Message *
331TransformInterface::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 */
341void
342TransformInterface::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
352const char *
353TransformInterface::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 */
363bool
364TransformInterface::message_valid(const Message *message) const
365{
366 return false;
367}
368
369/// @cond INTERNALS
370EXPORT_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:80
const char * type() const
Get type of interface.
Definition: interface.cpp:652
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
TransformInterface Fawkes BlackBoard Interface.
Fawkes library namespace.