Fawkes API  Fawkes Development Version
Position3DInterface.cpp
1 
2 /***************************************************************************
3  * Position3DInterface.cpp - Fawkes BlackBoard Interface - Position3DInterface
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/Position3DInterface.h>
25 
26 #include <core/exceptions/software.h>
27 
28 #include <cstring>
29 #include <cstdlib>
30 
31 namespace fawkes {
32 
33 /** @class Position3DInterface <interfaces/Position3DInterface.h>
34  * Position3DInterface Fawkes BlackBoard Interface.
35  *
36  Storage for a 3D pose in Euclidean space.
37 
38  * @ingroup FawkesInterfaces
39  */
40 
41 
42 
43 /** Constructor */
44 Position3DInterface::Position3DInterface() : Interface()
45 {
46  data_size = sizeof(Position3DInterface_data_t);
47  data_ptr = malloc(data_size);
48  data = (Position3DInterface_data_t *)data_ptr;
49  data_ts = (interface_data_ts_t *)data_ptr;
50  memset(data_ptr, 0, data_size);
51  add_fieldinfo(IFT_STRING, "frame", 32, data->frame);
52  add_fieldinfo(IFT_INT32, "visibility_history", 1, &data->visibility_history);
53  add_fieldinfo(IFT_DOUBLE, "rotation", 4, &data->rotation);
54  add_fieldinfo(IFT_DOUBLE, "translation", 3, &data->translation);
55  unsigned char tmp_hash[] = {0xd, 0x82, 0xad, 0x13, 0x3c, 0xeb, 0x96, 0x82, 0x25, 0x6f, 0x2f, 0x62, 0xd7, 0x87, 0xec, 0x5a};
56  set_hash(tmp_hash);
57 }
58 
59 /** Destructor */
60 Position3DInterface::~Position3DInterface()
61 {
62  free(data_ptr);
63 }
64 /* Methods */
65 /** Get frame value.
66  *
67  Reference coordinate frame for the data.
68 
69  * @return frame value
70  */
71 char *
73 {
74  return data->frame;
75 }
76 
77 /** Get maximum length of frame value.
78  * @return length of frame value, can be length of the array or number of
79  * maximum number of characters for a string
80  */
81 size_t
83 {
84  return 32;
85 }
86 
87 /** Set frame value.
88  *
89  Reference coordinate frame for the data.
90 
91  * @param new_frame new frame value
92  */
93 void
94 Position3DInterface::set_frame(const char * new_frame)
95 {
96  strncpy(data->frame, new_frame, sizeof(data->frame));
97  data_changed = true;
98 }
99 
100 /** Get visibility_history value.
101  *
102  The visibilitiy history indicates the number of consecutive positive or negative
103  sightings. If the history is negative, there have been as many negative sightings
104  (object not visible) as the absolute value of the history. A positive value denotes
105  as many positive sightings. 0 shall only be used during the initialization of the
106  interface or if the visibility history is not updated.
107 
108  * @return visibility_history value
109  */
110 int32_t
112 {
113  return data->visibility_history;
114 }
115 
116 /** Get maximum length of visibility_history value.
117  * @return length of visibility_history value, can be length of the array or number of
118  * maximum number of characters for a string
119  */
120 size_t
122 {
123  return 1;
124 }
125 
126 /** Set visibility_history value.
127  *
128  The visibilitiy history indicates the number of consecutive positive or negative
129  sightings. If the history is negative, there have been as many negative sightings
130  (object not visible) as the absolute value of the history. A positive value denotes
131  as many positive sightings. 0 shall only be used during the initialization of the
132  interface or if the visibility history is not updated.
133 
134  * @param new_visibility_history new visibility_history value
135  */
136 void
137 Position3DInterface::set_visibility_history(const int32_t new_visibility_history)
138 {
139  data->visibility_history = new_visibility_history;
140  data_changed = true;
141 }
142 
143 /** Get rotation value.
144  *
145  Rotation quaternion relative to reference frame, ordered as (x, y, z, w).
146 
147  * @return rotation value
148  */
149 double *
151 {
152  return data->rotation;
153 }
154 
155 /** Get rotation value at given index.
156  *
157  Rotation quaternion relative to reference frame, ordered as (x, y, z, w).
158 
159  * @param index index of value
160  * @return rotation value
161  * @exception Exception thrown if index is out of bounds
162  */
163 double
164 Position3DInterface::rotation(unsigned int index) const
165 {
166  if (index > 4) {
167  throw Exception("Index value %u out of bounds (0..4)", index);
168  }
169  return data->rotation[index];
170 }
171 
172 /** Get maximum length of rotation value.
173  * @return length of rotation value, can be length of the array or number of
174  * maximum number of characters for a string
175  */
176 size_t
178 {
179  return 4;
180 }
181 
182 /** Set rotation value.
183  *
184  Rotation quaternion relative to reference frame, ordered as (x, y, z, w).
185 
186  * @param new_rotation new rotation value
187  */
188 void
189 Position3DInterface::set_rotation(const double * new_rotation)
190 {
191  memcpy(data->rotation, new_rotation, sizeof(double) * 4);
192  data_changed = true;
193 }
194 
195 /** Set rotation value at given index.
196  *
197  Rotation quaternion relative to reference frame, ordered as (x, y, z, w).
198 
199  * @param new_rotation new rotation value
200  * @param index index for of the value
201  */
202 void
203 Position3DInterface::set_rotation(unsigned int index, const double new_rotation)
204 {
205  if (index > 4) {
206  throw Exception("Index value %u out of bounds (0..4)", index);
207  }
208  data->rotation[index] = new_rotation;
209  data_changed = true;
210 }
211 /** Get translation value.
212  *
213  Translation vector from the reference frame's origin, ordered as (x, y, z).
214 
215  * @return translation value
216  */
217 double *
219 {
220  return data->translation;
221 }
222 
223 /** Get translation value at given index.
224  *
225  Translation vector from the reference frame's origin, ordered as (x, y, z).
226 
227  * @param index index of value
228  * @return translation value
229  * @exception Exception thrown if index is out of bounds
230  */
231 double
232 Position3DInterface::translation(unsigned int index) const
233 {
234  if (index > 3) {
235  throw Exception("Index value %u out of bounds (0..3)", index);
236  }
237  return data->translation[index];
238 }
239 
240 /** Get maximum length of translation value.
241  * @return length of translation value, can be length of the array or number of
242  * maximum number of characters for a string
243  */
244 size_t
246 {
247  return 3;
248 }
249 
250 /** Set translation value.
251  *
252  Translation vector from the reference frame's origin, ordered as (x, y, z).
253 
254  * @param new_translation new translation value
255  */
256 void
257 Position3DInterface::set_translation(const double * new_translation)
258 {
259  memcpy(data->translation, new_translation, sizeof(double) * 3);
260  data_changed = true;
261 }
262 
263 /** Set translation value at given index.
264  *
265  Translation vector from the reference frame's origin, ordered as (x, y, z).
266 
267  * @param new_translation new translation value
268  * @param index index for of the value
269  */
270 void
271 Position3DInterface::set_translation(unsigned int index, const double new_translation)
272 {
273  if (index > 3) {
274  throw Exception("Index value %u out of bounds (0..3)", index);
275  }
276  data->translation[index] = new_translation;
277  data_changed = true;
278 }
279 /* =========== message create =========== */
280 Message *
281 Position3DInterface::create_message(const char *type) const
282 {
283  throw UnknownTypeException("The given type '%s' does not match any known "
284  "message type for this interface type.", type);
285 }
286 
287 
288 /** Copy values from other interface.
289  * @param other other interface to copy values from
290  */
291 void
293 {
294  const Position3DInterface *oi = dynamic_cast<const Position3DInterface *>(other);
295  if (oi == NULL) {
296  throw TypeMismatchException("Can only copy values from interface of same type (%s vs. %s)",
297  type(), other->type());
298  }
299  memcpy(data, oi->data, sizeof(Position3DInterface_data_t));
300 }
301 
302 const char *
303 Position3DInterface::enum_tostring(const char *enumtype, int val) const
304 {
305  throw UnknownTypeException("Unknown enum type %s", enumtype);
306 }
307 
308 /* =========== messages =========== */
309 /** Check if message is valid and can be enqueued.
310  * @param message Message to check
311  * @return true if the message is valid, false otherwise.
312  */
313 bool
315 {
316  return false;
317 }
318 
319 /// @cond INTERNALS
320 EXPORT_INTERFACE(Position3DInterface)
321 /// @endcond
322 
323 
324 } // end namespace fawkes
void set_frame(const char *new_frame)
Set frame value.
size_t maxlenof_translation() const
Get maximum length of translation value.
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
virtual bool message_valid(const Message *message) const
Check if message is valid and can be enqueued.
Fawkes library namespace.
size_t maxlenof_frame() const
Get maximum length of frame value.
unsigned int data_size
Minimal data size to hold data storage.
Definition: interface.h:207
size_t maxlenof_rotation() const
Get maximum length of rotation value.
string field
Definition: types.h:45
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
virtual const char * enum_tostring(const char *enumtype, int val) const
Convert arbitrary enum value to string.
double * translation() const
Get translation value.
bool data_changed
Indicator if data has changed.
Definition: interface.h:208
Position3DInterface Fawkes BlackBoard Interface.
void * data_ptr
Pointer to local memory storage.
Definition: interface.h:206
void set_visibility_history(const int32_t new_visibility_history)
Set visibility_history value.
Base class for exceptions in Fawkes.
Definition: exception.h:36
size_t maxlenof_visibility_history() const
Get maximum length of visibility_history value.
double * rotation() const
Get rotation 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 field info list.
Definition: interface.cpp:332
virtual Message * create_message(const char *type) const
Create message based on type name.
int32_t visibility_history() const
Get visibility_history value.
32 bit integer field
Definition: types.h:39
interface_data_ts_t * data_ts
Pointer to data casted to timestamp struct.
Definition: interface.h:216
virtual void copy_values(const Interface *other)
Copy values from other interface.
const char * type() const
Get type of interface.
Definition: interface.cpp:635
char * frame() const
Get frame value.
double field
Definition: types.h:44