Fawkes API  Fawkes Development Version
LaserLineInterface.cpp
1 
2 /***************************************************************************
3  * LaserLineInterface.cpp - Fawkes BlackBoard Interface - LaserLineInterface
4  *
5  * Templated created: Thu Oct 12 10:49:19 2006
6  * Copyright 2013 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/LaserLineInterface.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 LaserLineInterface <interfaces/LaserLineInterface.h>
36  * LaserLineInterface Fawkes BlackBoard Interface.
37  * Line parameterization.
38  * @ingroup FawkesInterfaces
39  */
40 
41 
42 
43 /** Constructor */
44 LaserLineInterface::LaserLineInterface() : Interface()
45 {
46  data_size = sizeof(LaserLineInterface_data_t);
47  data_ptr = malloc(data_size);
48  data = (LaserLineInterface_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_id", 32, data->frame_id);
52  add_fieldinfo(IFT_INT32, "visibility_history", 1, &data->visibility_history);
53  add_fieldinfo(IFT_FLOAT, "point_on_line", 3, &data->point_on_line);
54  add_fieldinfo(IFT_FLOAT, "line_direction", 3, &data->line_direction);
55  add_fieldinfo(IFT_FLOAT, "bearing", 1, &data->bearing);
56  add_fieldinfo(IFT_FLOAT, "end_point_1", 3, &data->end_point_1);
57  add_fieldinfo(IFT_FLOAT, "end_point_2", 3, &data->end_point_2);
58  add_fieldinfo(IFT_FLOAT, "length", 1, &data->length);
59  unsigned char tmp_hash[] = {0x80, 0xa, 0x8e, 0xab, 0x65, 0xe7, 0x47, 0x3f, 0xc3, 0x8a, 0x44, 0x7b, 0xda, 0xbd, 0xfb, 0x5f};
60  set_hash(tmp_hash);
61 }
62 
63 /** Destructor */
64 LaserLineInterface::~LaserLineInterface()
65 {
66  free(data_ptr);
67 }
68 /* Methods */
69 /** Get frame_id value.
70  *
71  Coordinate frame ID of data.
72 
73  * @return frame_id value
74  */
75 char *
76 LaserLineInterface::frame_id() const
77 {
78  return data->frame_id;
79 }
80 
81 /** Get maximum length of frame_id value.
82  * @return length of frame_id value, can be length of the array or number of
83  * maximum number of characters for a string
84  */
85 size_t
86 LaserLineInterface::maxlenof_frame_id() const
87 {
88  return 32;
89 }
90 
91 /** Set frame_id value.
92  *
93  Coordinate frame ID of data.
94 
95  * @param new_frame_id new frame_id value
96  */
97 void
98 LaserLineInterface::set_frame_id(const char * new_frame_id)
99 {
100  strncpy(data->frame_id, new_frame_id, sizeof(data->frame_id));
101  data_changed = true;
102 }
103 
104 /** Get visibility_history value.
105  *
106  The visibilitiy history indicates the number of consecutive positive or negative
107  sightings. If the history is negative, there have been as many negative sightings
108  (object not visible) as the absolute value of the history. A positive value denotes
109  as many positive sightings. 0 shall only be used during the initialization of the
110  interface or if the visibility history is not updated.
111 
112  * @return visibility_history value
113  */
114 int32_t
115 LaserLineInterface::visibility_history() const
116 {
117  return data->visibility_history;
118 }
119 
120 /** Get maximum length of visibility_history value.
121  * @return length of visibility_history value, can be length of the array or number of
122  * maximum number of characters for a string
123  */
124 size_t
125 LaserLineInterface::maxlenof_visibility_history() const
126 {
127  return 1;
128 }
129 
130 /** Set visibility_history value.
131  *
132  The visibilitiy history indicates the number of consecutive positive or negative
133  sightings. If the history is negative, there have been as many negative sightings
134  (object not visible) as the absolute value of the history. A positive value denotes
135  as many positive sightings. 0 shall only be used during the initialization of the
136  interface or if the visibility history is not updated.
137 
138  * @param new_visibility_history new visibility_history value
139  */
140 void
141 LaserLineInterface::set_visibility_history(const int32_t new_visibility_history)
142 {
143  data->visibility_history = new_visibility_history;
144  data_changed = true;
145 }
146 
147 /** Get point_on_line value.
148  *
149  Vector to some point on the line
150 
151  * @return point_on_line value
152  */
153 float *
154 LaserLineInterface::point_on_line() const
155 {
156  return data->point_on_line;
157 }
158 
159 /** Get point_on_line value at given index.
160  *
161  Vector to some point on the line
162 
163  * @param index index of value
164  * @return point_on_line value
165  * @exception Exception thrown if index is out of bounds
166  */
167 float
168 LaserLineInterface::point_on_line(unsigned int index) const
169 {
170  if (index > 3) {
171  throw Exception("Index value %u out of bounds (0..3)", index);
172  }
173  return data->point_on_line[index];
174 }
175 
176 /** Get maximum length of point_on_line value.
177  * @return length of point_on_line value, can be length of the array or number of
178  * maximum number of characters for a string
179  */
180 size_t
181 LaserLineInterface::maxlenof_point_on_line() const
182 {
183  return 3;
184 }
185 
186 /** Set point_on_line value.
187  *
188  Vector to some point on the line
189 
190  * @param new_point_on_line new point_on_line value
191  */
192 void
193 LaserLineInterface::set_point_on_line(const float * new_point_on_line)
194 {
195  memcpy(data->point_on_line, new_point_on_line, sizeof(float) * 3);
196  data_changed = true;
197 }
198 
199 /** Set point_on_line value at given index.
200  *
201  Vector to some point on the line
202 
203  * @param new_point_on_line new point_on_line value
204  * @param index index for of the value
205  */
206 void
207 LaserLineInterface::set_point_on_line(unsigned int index, const float new_point_on_line)
208 {
209  if (index > 3) {
210  throw Exception("Index value %u out of bounds (0..3)", index);
211  }
212  data->point_on_line[index] = new_point_on_line;
213  data_changed = true;
214 }
215 /** Get line_direction value.
216  *
217  Vector in the direction of the line.
218 
219  * @return line_direction value
220  */
221 float *
222 LaserLineInterface::line_direction() const
223 {
224  return data->line_direction;
225 }
226 
227 /** Get line_direction value at given index.
228  *
229  Vector in the direction of the line.
230 
231  * @param index index of value
232  * @return line_direction value
233  * @exception Exception thrown if index is out of bounds
234  */
235 float
236 LaserLineInterface::line_direction(unsigned int index) const
237 {
238  if (index > 3) {
239  throw Exception("Index value %u out of bounds (0..3)", index);
240  }
241  return data->line_direction[index];
242 }
243 
244 /** Get maximum length of line_direction value.
245  * @return length of line_direction value, can be length of the array or number of
246  * maximum number of characters for a string
247  */
248 size_t
249 LaserLineInterface::maxlenof_line_direction() const
250 {
251  return 3;
252 }
253 
254 /** Set line_direction value.
255  *
256  Vector in the direction of the line.
257 
258  * @param new_line_direction new line_direction value
259  */
260 void
261 LaserLineInterface::set_line_direction(const float * new_line_direction)
262 {
263  memcpy(data->line_direction, new_line_direction, sizeof(float) * 3);
264  data_changed = true;
265 }
266 
267 /** Set line_direction value at given index.
268  *
269  Vector in the direction of the line.
270 
271  * @param new_line_direction new line_direction value
272  * @param index index for of the value
273  */
274 void
275 LaserLineInterface::set_line_direction(unsigned int index, const float new_line_direction)
276 {
277  if (index > 3) {
278  throw Exception("Index value %u out of bounds (0..3)", index);
279  }
280  data->line_direction[index] = new_line_direction;
281  data_changed = true;
282 }
283 /** Get bearing value.
284  *
285  Direction towards the line, i.e. if the robot turns by this
286  angle the robot will stand parallel to the line.
287 
288  * @return bearing value
289  */
290 float
291 LaserLineInterface::bearing() const
292 {
293  return data->bearing;
294 }
295 
296 /** Get maximum length of bearing value.
297  * @return length of bearing value, can be length of the array or number of
298  * maximum number of characters for a string
299  */
300 size_t
301 LaserLineInterface::maxlenof_bearing() const
302 {
303  return 1;
304 }
305 
306 /** Set bearing value.
307  *
308  Direction towards the line, i.e. if the robot turns by this
309  angle the robot will stand parallel to the line.
310 
311  * @param new_bearing new bearing value
312  */
313 void
314 LaserLineInterface::set_bearing(const float new_bearing)
315 {
316  data->bearing = new_bearing;
317  data_changed = true;
318 }
319 
320 /** Get end_point_1 value.
321  *
322  3D coordinates in the reference frame of one endpoint of the
323  line. The end points are ordered arbitrarily.
324 
325  * @return end_point_1 value
326  */
327 float *
328 LaserLineInterface::end_point_1() const
329 {
330  return data->end_point_1;
331 }
332 
333 /** Get end_point_1 value at given index.
334  *
335  3D coordinates in the reference frame of one endpoint of the
336  line. The end points are ordered arbitrarily.
337 
338  * @param index index of value
339  * @return end_point_1 value
340  * @exception Exception thrown if index is out of bounds
341  */
342 float
343 LaserLineInterface::end_point_1(unsigned int index) const
344 {
345  if (index > 3) {
346  throw Exception("Index value %u out of bounds (0..3)", index);
347  }
348  return data->end_point_1[index];
349 }
350 
351 /** Get maximum length of end_point_1 value.
352  * @return length of end_point_1 value, can be length of the array or number of
353  * maximum number of characters for a string
354  */
355 size_t
356 LaserLineInterface::maxlenof_end_point_1() const
357 {
358  return 3;
359 }
360 
361 /** Set end_point_1 value.
362  *
363  3D coordinates in the reference frame of one endpoint of the
364  line. The end points are ordered arbitrarily.
365 
366  * @param new_end_point_1 new end_point_1 value
367  */
368 void
369 LaserLineInterface::set_end_point_1(const float * new_end_point_1)
370 {
371  memcpy(data->end_point_1, new_end_point_1, sizeof(float) * 3);
372  data_changed = true;
373 }
374 
375 /** Set end_point_1 value at given index.
376  *
377  3D coordinates in the reference frame of one endpoint of the
378  line. The end points are ordered arbitrarily.
379 
380  * @param new_end_point_1 new end_point_1 value
381  * @param index index for of the value
382  */
383 void
384 LaserLineInterface::set_end_point_1(unsigned int index, const float new_end_point_1)
385 {
386  if (index > 3) {
387  throw Exception("Index value %u out of bounds (0..3)", index);
388  }
389  data->end_point_1[index] = new_end_point_1;
390  data_changed = true;
391 }
392 /** Get end_point_2 value.
393  *
394  3D coordinates in the reference frame of the second endpoint of
395  the line.
396 
397  * @return end_point_2 value
398  */
399 float *
400 LaserLineInterface::end_point_2() const
401 {
402  return data->end_point_2;
403 }
404 
405 /** Get end_point_2 value at given index.
406  *
407  3D coordinates in the reference frame of the second endpoint of
408  the line.
409 
410  * @param index index of value
411  * @return end_point_2 value
412  * @exception Exception thrown if index is out of bounds
413  */
414 float
415 LaserLineInterface::end_point_2(unsigned int index) const
416 {
417  if (index > 3) {
418  throw Exception("Index value %u out of bounds (0..3)", index);
419  }
420  return data->end_point_2[index];
421 }
422 
423 /** Get maximum length of end_point_2 value.
424  * @return length of end_point_2 value, can be length of the array or number of
425  * maximum number of characters for a string
426  */
427 size_t
428 LaserLineInterface::maxlenof_end_point_2() const
429 {
430  return 3;
431 }
432 
433 /** Set end_point_2 value.
434  *
435  3D coordinates in the reference frame of the second endpoint of
436  the line.
437 
438  * @param new_end_point_2 new end_point_2 value
439  */
440 void
441 LaserLineInterface::set_end_point_2(const float * new_end_point_2)
442 {
443  memcpy(data->end_point_2, new_end_point_2, sizeof(float) * 3);
444  data_changed = true;
445 }
446 
447 /** Set end_point_2 value at given index.
448  *
449  3D coordinates in the reference frame of the second endpoint of
450  the line.
451 
452  * @param new_end_point_2 new end_point_2 value
453  * @param index index for of the value
454  */
455 void
456 LaserLineInterface::set_end_point_2(unsigned int index, const float new_end_point_2)
457 {
458  if (index > 3) {
459  throw Exception("Index value %u out of bounds (0..3)", index);
460  }
461  data->end_point_2[index] = new_end_point_2;
462  data_changed = true;
463 }
464 /** Get length value.
465  * Length of the line.
466  * @return length value
467  */
468 float
469 LaserLineInterface::length() const
470 {
471  return data->length;
472 }
473 
474 /** Get maximum length of length value.
475  * @return length of length value, can be length of the array or number of
476  * maximum number of characters for a string
477  */
478 size_t
479 LaserLineInterface::maxlenof_length() const
480 {
481  return 1;
482 }
483 
484 /** Set length value.
485  * Length of the line.
486  * @param new_length new length value
487  */
488 void
489 LaserLineInterface::set_length(const float new_length)
490 {
491  data->length = new_length;
492  data_changed = true;
493 }
494 
495 /* =========== message create =========== */
496 Message *
497 LaserLineInterface::create_message(const char *type) const
498 {
499  throw UnknownTypeException("The given type '%s' does not match any known "
500  "message type for this interface type.", type);
501 }
502 
503 
504 /** Copy values from other interface.
505  * @param other other interface to copy values from
506  */
507 void
508 LaserLineInterface::copy_values(const Interface *other)
509 {
510  const LaserLineInterface *oi = dynamic_cast<const LaserLineInterface *>(other);
511  if (oi == NULL) {
512  throw TypeMismatchException("Can only copy values from interface of same type (%s vs. %s)",
513  type(), other->type());
514  }
515  memcpy(data, oi->data, sizeof(LaserLineInterface_data_t));
516 }
517 
518 const char *
519 LaserLineInterface::enum_tostring(const char *enumtype, int val) const
520 {
521  throw UnknownTypeException("Unknown enum type %s", enumtype);
522 }
523 
524 /* =========== messages =========== */
525 /** Check if message is valid and can be enqueued.
526  * @param message Message to check
527  * @return true if the message is valid, false otherwise.
528  */
529 bool
530 LaserLineInterface::message_valid(const Message *message) const
531 {
532  return false;
533 }
534 
535 /// @cond INTERNALS
536 EXPORT_INTERFACE(LaserLineInterface)
537 /// @endcond
538 
539 
540 } // end namespace fawkes
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
Fawkes library namespace.
LaserLineInterface Fawkes BlackBoard Interface.
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
const char * type() const
Get type of interface.
Definition: interface.cpp:651
Base class for exceptions in Fawkes.
Definition: exception.h:36