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 <cstring>
29 #include <cstdlib>
30 
31 namespace fawkes {
32 
33 /** @class TransformInterface <interfaces/TransformInterface.h>
34  * TransformInterface Fawkes BlackBoard Interface.
35  *
36  This interface is used to publish transforms. It aims to be as
37  compatible as possible with ROS' tf library and is used
38  extensively by the Fawkes tf library.
39 
40  For this to work properly it is crucial to have correct
41  timestamp set (cf. Interface::set_timestamp()). Set this as
42  close as possible to the time of when the data, from which the
43  transform is computed, has been acquired.
44 
45  * @ingroup FawkesInterfaces
46  */
47 
48 
49 
50 /** Constructor */
51 TransformInterface::TransformInterface() : Interface()
52 {
53  data_size = sizeof(TransformInterface_data_t);
54  data_ptr = malloc(data_size);
55  data = (TransformInterface_data_t *)data_ptr;
56  data_ts = (interface_data_ts_t *)data_ptr;
57  memset(data_ptr, 0, data_size);
58  add_fieldinfo(IFT_STRING, "frame", 32, data->frame);
59  add_fieldinfo(IFT_STRING, "child_frame", 32, data->child_frame);
60  add_fieldinfo(IFT_DOUBLE, "translation", 3, &data->translation);
61  add_fieldinfo(IFT_DOUBLE, "rotation", 4, &data->rotation);
62  unsigned char tmp_hash[] = {0x97, 0xc8, 0x15, 0xe9, 0xcb, 0xd2, 0x33, 0x33, 0xf3, 0xfe, 0x49, 0x72, 0x92, 0x99, 0xd9, 0xeb};
63  set_hash(tmp_hash);
64 }
65 
66 /** Destructor */
67 TransformInterface::~TransformInterface()
68 {
69  free(data_ptr);
70 }
71 /* Methods */
72 /** Get frame value.
73  *
74  Parent frame ID. The given transform is relative to the origin
75  of this coordinate frame.
76 
77  * @return frame value
78  */
79 char *
81 {
82  return data->frame;
83 }
84 
85 /** Get maximum length of frame value.
86  * @return length of frame value, can be length of the array or number of
87  * maximum number of characters for a string
88  */
89 size_t
91 {
92  return 32;
93 }
94 
95 /** Set frame value.
96  *
97  Parent frame ID. The given transform is relative to the origin
98  of this coordinate frame.
99 
100  * @param new_frame new frame value
101  */
102 void
103 TransformInterface::set_frame(const char * new_frame)
104 {
105  strncpy(data->frame, new_frame, sizeof(data->frame));
106  data_changed = true;
107 }
108 
109 /** Get child_frame value.
110  *
111  The ID of the child frame. The child frame's origin is at the
112  given point in the parent frame denoted by the transform.
113 
114  * @return child_frame value
115  */
116 char *
118 {
119  return data->child_frame;
120 }
121 
122 /** Get maximum length of child_frame value.
123  * @return length of child_frame value, can be length of the array or number of
124  * maximum number of characters for a string
125  */
126 size_t
128 {
129  return 32;
130 }
131 
132 /** Set child_frame value.
133  *
134  The ID of the child frame. The child frame's origin is at the
135  given point in the parent frame denoted by the transform.
136 
137  * @param new_child_frame new child_frame value
138  */
139 void
140 TransformInterface::set_child_frame(const char * new_child_frame)
141 {
142  strncpy(data->child_frame, new_child_frame, sizeof(data->child_frame));
143  data_changed = true;
144 }
145 
146 /** Get translation value.
147  *
148  This array denotes the translation vector of the transform. The
149  element indexes are ordered x, y, z, i.e. translation[0] is the
150  X value of the translation vector.
151 
152  * @return translation value
153  */
154 double *
156 {
157  return data->translation;
158 }
159 
160 /** Get translation value at given index.
161  *
162  This array denotes the translation vector of the transform. The
163  element indexes are ordered x, y, z, i.e. translation[0] is the
164  X value of the translation vector.
165 
166  * @param index index of value
167  * @return translation value
168  * @exception Exception thrown if index is out of bounds
169  */
170 double
171 TransformInterface::translation(unsigned int index) const
172 {
173  if (index > 3) {
174  throw Exception("Index value %u out of bounds (0..3)", index);
175  }
176  return data->translation[index];
177 }
178 
179 /** Get maximum length of translation value.
180  * @return length of translation value, can be length of the array or number of
181  * maximum number of characters for a string
182  */
183 size_t
185 {
186  return 3;
187 }
188 
189 /** Set translation value.
190  *
191  This array denotes the translation vector of the transform. The
192  element indexes are ordered x, y, z, i.e. translation[0] is the
193  X value of the translation vector.
194 
195  * @param new_translation new translation value
196  */
197 void
198 TransformInterface::set_translation(const double * new_translation)
199 {
200  memcpy(data->translation, new_translation, sizeof(double) * 3);
201  data_changed = true;
202 }
203 
204 /** Set translation value at given index.
205  *
206  This array denotes the translation vector of the transform. The
207  element indexes are ordered x, y, z, i.e. translation[0] is the
208  X value of the translation vector.
209 
210  * @param new_translation new translation value
211  * @param index index for of the value
212  */
213 void
214 TransformInterface::set_translation(unsigned int index, const double new_translation)
215 {
216  if (index > 3) {
217  throw Exception("Index value %u out of bounds (0..3)", index);
218  }
219  data->translation[index] = new_translation;
220  data_changed = true;
221 }
222 /** Get rotation value.
223  *
224  This array denotes the rotation quaternion of the transform. The
225  element indexes are ordered x, y, z, w, i.e. translation[0] is
226  the X value of the rotation quaternion and translation[3] is the
227  W value.
228 
229  * @return rotation value
230  */
231 double *
233 {
234  return data->rotation;
235 }
236 
237 /** Get rotation value at given index.
238  *
239  This array denotes the rotation quaternion of the transform. The
240  element indexes are ordered x, y, z, w, i.e. translation[0] is
241  the X value of the rotation quaternion and translation[3] is the
242  W value.
243 
244  * @param index index of value
245  * @return rotation value
246  * @exception Exception thrown if index is out of bounds
247  */
248 double
249 TransformInterface::rotation(unsigned int index) const
250 {
251  if (index > 4) {
252  throw Exception("Index value %u out of bounds (0..4)", index);
253  }
254  return data->rotation[index];
255 }
256 
257 /** Get maximum length of rotation value.
258  * @return length of rotation value, can be length of the array or number of
259  * maximum number of characters for a string
260  */
261 size_t
263 {
264  return 4;
265 }
266 
267 /** Set rotation value.
268  *
269  This array denotes the rotation quaternion of the transform. The
270  element indexes are ordered x, y, z, w, i.e. translation[0] is
271  the X value of the rotation quaternion and translation[3] is the
272  W value.
273 
274  * @param new_rotation new rotation value
275  */
276 void
277 TransformInterface::set_rotation(const double * new_rotation)
278 {
279  memcpy(data->rotation, new_rotation, sizeof(double) * 4);
280  data_changed = true;
281 }
282 
283 /** Set rotation value at given index.
284  *
285  This array denotes the rotation quaternion of the transform. The
286  element indexes are ordered x, y, z, w, i.e. translation[0] is
287  the X value of the rotation quaternion and translation[3] is the
288  W value.
289 
290  * @param new_rotation new rotation value
291  * @param index index for of the value
292  */
293 void
294 TransformInterface::set_rotation(unsigned int index, const double new_rotation)
295 {
296  if (index > 4) {
297  throw Exception("Index value %u out of bounds (0..4)", index);
298  }
299  data->rotation[index] = new_rotation;
300  data_changed = true;
301 }
302 /* =========== message create =========== */
303 Message *
305 {
306  throw UnknownTypeException("The given type '%s' does not match any known "
307  "message type for this interface type.", type);
308 }
309 
310 
311 /** Copy values from other interface.
312  * @param other other interface to copy values from
313  */
314 void
316 {
317  const TransformInterface *oi = dynamic_cast<const TransformInterface *>(other);
318  if (oi == NULL) {
319  throw TypeMismatchException("Can only copy values from interface of same type (%s vs. %s)",
320  type(), other->type());
321  }
322  memcpy(data, oi->data, sizeof(TransformInterface_data_t));
323 }
324 
325 const char *
326 TransformInterface::enum_tostring(const char *enumtype, int val) const
327 {
328  throw UnknownTypeException("Unknown enum type %s", enumtype);
329 }
330 
331 /* =========== messages =========== */
332 /** Check if message is valid and can be enqueued.
333  * @param message Message to check
334  * @return true if the message is valid, false otherwise.
335  */
336 bool
338 {
339  return false;
340 }
341 
342 /// @cond INTERNALS
343 EXPORT_INTERFACE(TransformInterface)
344 /// @endcond
345 
346 
347 } // end namespace fawkes
void * data_ptr
Pointer to memory that contains local data.
Definition: message.h:114
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:43
void set_hash(unsigned char *ihash)
Set hash.
Definition: interface.cpp:312
double * translation() const
Get translation value.
Fawkes library namespace.
void set_child_frame(const char *new_child_frame)
Set child_frame value.
string field
Definition: types.h:45
virtual Message * create_message(const char *type) const
Create message based on type name.
TransformInterface Fawkes BlackBoard Interface.
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
size_t maxlenof_translation() const
Get maximum length of translation value.
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:123
unsigned int data_size
Size of memory needed to hold all data.
Definition: message.h:115
double * rotation() const
Get rotation value.
bool data_changed
Indicator if data has changed.
Definition: interface.h:208
void * data_ptr
Pointer to local memory storage.
Definition: interface.h:206
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * enum_tostring(const char *enumtype, int val) const
Convert arbitrary enum value to string.
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
virtual bool message_valid(const Message *message) const
Check if message is valid and can be enqueued.
size_t maxlenof_child_frame() const
Get maximum length of child_frame value.
char * frame() const
Get frame value.
void add_fieldinfo(interface_fieldtype_t type, const char *name, size_t length, void *value, const char *enumtype=0)
Add an entry to the info list.
Definition: message.cpp:435
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
void set_frame(const char *new_frame)
Set frame value.
const char * type() const
Get type of interface.
Definition: interface.cpp:635
char * child_frame() const
Get child_frame value.
size_t maxlenof_frame() const
Get maximum length of frame value.
size_t maxlenof_rotation() const
Get maximum length of rotation value.
double field
Definition: types.h:44