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  add_fieldinfo(IFT_STRING, "end_point_frame_1", 32, data->end_point_frame_1);
60  add_fieldinfo(IFT_STRING, "end_point_frame_2", 32, data->end_point_frame_2);
61  unsigned char tmp_hash[] = {0x5f, 0x66, 0x25, 0x68, 0xe4, 0xe3, 0x5b, 0x51, 0x1f, 0x4, 0x79, 0x7a, 0x1, 0x96, 0xe2, 0xe8};
62  set_hash(tmp_hash);
63 }
64 
65 /** Destructor */
66 LaserLineInterface::~LaserLineInterface()
67 {
68  free(data_ptr);
69 }
70 /* Methods */
71 /** Get frame_id value.
72  *
73  Coordinate frame ID of data.
74 
75  * @return frame_id value
76  */
77 char *
78 LaserLineInterface::frame_id() const
79 {
80  return data->frame_id;
81 }
82 
83 /** Get maximum length of frame_id value.
84  * @return length of frame_id value, can be length of the array or number of
85  * maximum number of characters for a string
86  */
87 size_t
88 LaserLineInterface::maxlenof_frame_id() const
89 {
90  return 32;
91 }
92 
93 /** Set frame_id value.
94  *
95  Coordinate frame ID of data.
96 
97  * @param new_frame_id new frame_id value
98  */
99 void
100 LaserLineInterface::set_frame_id(const char * new_frame_id)
101 {
102  set_field(data->frame_id, new_frame_id);
103 }
104 
105 /** Get visibility_history value.
106  *
107  The visibilitiy history indicates the number of consecutive positive or negative
108  sightings. If the history is negative, there have been as many negative sightings
109  (object not visible) as the absolute value of the history. A positive value denotes
110  as many positive sightings. 0 shall only be used during the initialization of the
111  interface or if the visibility history is not updated.
112 
113  * @return visibility_history value
114  */
115 int32_t
116 LaserLineInterface::visibility_history() const
117 {
118  return data->visibility_history;
119 }
120 
121 /** Get maximum length of visibility_history value.
122  * @return length of visibility_history value, can be length of the array or number of
123  * maximum number of characters for a string
124  */
125 size_t
126 LaserLineInterface::maxlenof_visibility_history() const
127 {
128  return 1;
129 }
130 
131 /** Set visibility_history value.
132  *
133  The visibilitiy history indicates the number of consecutive positive or negative
134  sightings. If the history is negative, there have been as many negative sightings
135  (object not visible) as the absolute value of the history. A positive value denotes
136  as many positive sightings. 0 shall only be used during the initialization of the
137  interface or if the visibility history is not updated.
138 
139  * @param new_visibility_history new visibility_history value
140  */
141 void
142 LaserLineInterface::set_visibility_history(const int32_t new_visibility_history)
143 {
144  set_field(data->visibility_history, new_visibility_history);
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 > 2) {
171  throw Exception("Index value %u out of bounds (0..2)", 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  set_field(data->point_on_line, new_point_on_line);
196 }
197 
198 /** Set point_on_line value at given index.
199  *
200  Vector to some point on the line
201 
202  * @param new_point_on_line new point_on_line value
203  * @param index index for of the value
204  */
205 void
206 LaserLineInterface::set_point_on_line(unsigned int index, const float new_point_on_line)
207 {
208  set_field(data->point_on_line, index, new_point_on_line);
209 }
210 /** Get line_direction value.
211  *
212  Vector in the direction of the line.
213 
214  * @return line_direction value
215  */
216 float *
217 LaserLineInterface::line_direction() const
218 {
219  return data->line_direction;
220 }
221 
222 /** Get line_direction value at given index.
223  *
224  Vector in the direction of the line.
225 
226  * @param index index of value
227  * @return line_direction value
228  * @exception Exception thrown if index is out of bounds
229  */
230 float
231 LaserLineInterface::line_direction(unsigned int index) const
232 {
233  if (index > 2) {
234  throw Exception("Index value %u out of bounds (0..2)", index);
235  }
236  return data->line_direction[index];
237 }
238 
239 /** Get maximum length of line_direction value.
240  * @return length of line_direction value, can be length of the array or number of
241  * maximum number of characters for a string
242  */
243 size_t
244 LaserLineInterface::maxlenof_line_direction() const
245 {
246  return 3;
247 }
248 
249 /** Set line_direction value.
250  *
251  Vector in the direction of the line.
252 
253  * @param new_line_direction new line_direction value
254  */
255 void
256 LaserLineInterface::set_line_direction(const float * new_line_direction)
257 {
258  set_field(data->line_direction, new_line_direction);
259 }
260 
261 /** Set line_direction value at given index.
262  *
263  Vector in the direction of the line.
264 
265  * @param new_line_direction new line_direction value
266  * @param index index for of the value
267  */
268 void
269 LaserLineInterface::set_line_direction(unsigned int index, const float new_line_direction)
270 {
271  set_field(data->line_direction, index, new_line_direction);
272 }
273 /** Get bearing value.
274  *
275  Direction towards the line, i.e. if the robot turns by this
276  angle the robot will stand parallel to the line.
277 
278  * @return bearing value
279  */
280 float
281 LaserLineInterface::bearing() const
282 {
283  return data->bearing;
284 }
285 
286 /** Get maximum length of bearing value.
287  * @return length of bearing value, can be length of the array or number of
288  * maximum number of characters for a string
289  */
290 size_t
291 LaserLineInterface::maxlenof_bearing() const
292 {
293  return 1;
294 }
295 
296 /** Set bearing value.
297  *
298  Direction towards the line, i.e. if the robot turns by this
299  angle the robot will stand parallel to the line.
300 
301  * @param new_bearing new bearing value
302  */
303 void
304 LaserLineInterface::set_bearing(const float new_bearing)
305 {
306  set_field(data->bearing, new_bearing);
307 }
308 
309 /** Get end_point_1 value.
310  *
311  3D coordinates in the reference frame of one endpoint of the
312  line. The end points are ordered arbitrarily.
313 
314  * @return end_point_1 value
315  */
316 float *
317 LaserLineInterface::end_point_1() const
318 {
319  return data->end_point_1;
320 }
321 
322 /** Get end_point_1 value at given index.
323  *
324  3D coordinates in the reference frame of one endpoint of the
325  line. The end points are ordered arbitrarily.
326 
327  * @param index index of value
328  * @return end_point_1 value
329  * @exception Exception thrown if index is out of bounds
330  */
331 float
332 LaserLineInterface::end_point_1(unsigned int index) const
333 {
334  if (index > 2) {
335  throw Exception("Index value %u out of bounds (0..2)", index);
336  }
337  return data->end_point_1[index];
338 }
339 
340 /** Get maximum length of end_point_1 value.
341  * @return length of end_point_1 value, can be length of the array or number of
342  * maximum number of characters for a string
343  */
344 size_t
345 LaserLineInterface::maxlenof_end_point_1() const
346 {
347  return 3;
348 }
349 
350 /** Set end_point_1 value.
351  *
352  3D coordinates in the reference frame of one endpoint of the
353  line. The end points are ordered arbitrarily.
354 
355  * @param new_end_point_1 new end_point_1 value
356  */
357 void
358 LaserLineInterface::set_end_point_1(const float * new_end_point_1)
359 {
360  set_field(data->end_point_1, new_end_point_1);
361 }
362 
363 /** Set end_point_1 value at given index.
364  *
365  3D coordinates in the reference frame of one endpoint of the
366  line. The end points are ordered arbitrarily.
367 
368  * @param new_end_point_1 new end_point_1 value
369  * @param index index for of the value
370  */
371 void
372 LaserLineInterface::set_end_point_1(unsigned int index, const float new_end_point_1)
373 {
374  set_field(data->end_point_1, index, new_end_point_1);
375 }
376 /** Get end_point_2 value.
377  *
378  3D coordinates in the reference frame of the second endpoint of
379  the line.
380 
381  * @return end_point_2 value
382  */
383 float *
384 LaserLineInterface::end_point_2() const
385 {
386  return data->end_point_2;
387 }
388 
389 /** Get end_point_2 value at given index.
390  *
391  3D coordinates in the reference frame of the second endpoint of
392  the line.
393 
394  * @param index index of value
395  * @return end_point_2 value
396  * @exception Exception thrown if index is out of bounds
397  */
398 float
399 LaserLineInterface::end_point_2(unsigned int index) const
400 {
401  if (index > 2) {
402  throw Exception("Index value %u out of bounds (0..2)", index);
403  }
404  return data->end_point_2[index];
405 }
406 
407 /** Get maximum length of end_point_2 value.
408  * @return length of end_point_2 value, can be length of the array or number of
409  * maximum number of characters for a string
410  */
411 size_t
412 LaserLineInterface::maxlenof_end_point_2() const
413 {
414  return 3;
415 }
416 
417 /** Set end_point_2 value.
418  *
419  3D coordinates in the reference frame of the second endpoint of
420  the line.
421 
422  * @param new_end_point_2 new end_point_2 value
423  */
424 void
425 LaserLineInterface::set_end_point_2(const float * new_end_point_2)
426 {
427  set_field(data->end_point_2, new_end_point_2);
428 }
429 
430 /** Set end_point_2 value at given index.
431  *
432  3D coordinates in the reference frame of the second endpoint of
433  the line.
434 
435  * @param new_end_point_2 new end_point_2 value
436  * @param index index for of the value
437  */
438 void
439 LaserLineInterface::set_end_point_2(unsigned int index, const float new_end_point_2)
440 {
441  set_field(data->end_point_2, index, new_end_point_2);
442 }
443 /** Get length value.
444  * Length of the line.
445  * @return length value
446  */
447 float
448 LaserLineInterface::length() const
449 {
450  return data->length;
451 }
452 
453 /** Get maximum length of length value.
454  * @return length of length value, can be length of the array or number of
455  * maximum number of characters for a string
456  */
457 size_t
458 LaserLineInterface::maxlenof_length() const
459 {
460  return 1;
461 }
462 
463 /** Set length value.
464  * Length of the line.
465  * @param new_length new length value
466  */
467 void
468 LaserLineInterface::set_length(const float new_length)
469 {
470  set_field(data->length, new_length);
471 }
472 
473 /** Get end_point_frame_1 value.
474  *
475  Coordinate frame ID representing first endpoint's identity
476 
477  * @return end_point_frame_1 value
478  */
479 char *
480 LaserLineInterface::end_point_frame_1() const
481 {
482  return data->end_point_frame_1;
483 }
484 
485 /** Get maximum length of end_point_frame_1 value.
486  * @return length of end_point_frame_1 value, can be length of the array or number of
487  * maximum number of characters for a string
488  */
489 size_t
490 LaserLineInterface::maxlenof_end_point_frame_1() const
491 {
492  return 32;
493 }
494 
495 /** Set end_point_frame_1 value.
496  *
497  Coordinate frame ID representing first endpoint's identity
498 
499  * @param new_end_point_frame_1 new end_point_frame_1 value
500  */
501 void
502 LaserLineInterface::set_end_point_frame_1(const char * new_end_point_frame_1)
503 {
504  set_field(data->end_point_frame_1, new_end_point_frame_1);
505 }
506 
507 /** Get end_point_frame_2 value.
508  *
509  Coordinate frame ID representing second endpoint's identity
510 
511  * @return end_point_frame_2 value
512  */
513 char *
514 LaserLineInterface::end_point_frame_2() const
515 {
516  return data->end_point_frame_2;
517 }
518 
519 /** Get maximum length of end_point_frame_2 value.
520  * @return length of end_point_frame_2 value, can be length of the array or number of
521  * maximum number of characters for a string
522  */
523 size_t
524 LaserLineInterface::maxlenof_end_point_frame_2() const
525 {
526  return 32;
527 }
528 
529 /** Set end_point_frame_2 value.
530  *
531  Coordinate frame ID representing second endpoint's identity
532 
533  * @param new_end_point_frame_2 new end_point_frame_2 value
534  */
535 void
536 LaserLineInterface::set_end_point_frame_2(const char * new_end_point_frame_2)
537 {
538  set_field(data->end_point_frame_2, new_end_point_frame_2);
539 }
540 
541 /* =========== message create =========== */
542 Message *
543 LaserLineInterface::create_message(const char *type) const
544 {
545  throw UnknownTypeException("The given type '%s' does not match any known "
546  "message type for this interface type.", type);
547 }
548 
549 
550 /** Copy values from other interface.
551  * @param other other interface to copy values from
552  */
553 void
554 LaserLineInterface::copy_values(const Interface *other)
555 {
556  const LaserLineInterface *oi = dynamic_cast<const LaserLineInterface *>(other);
557  if (oi == NULL) {
558  throw TypeMismatchException("Can only copy values from interface of same type (%s vs. %s)",
559  type(), other->type());
560  }
561  memcpy(data, oi->data, sizeof(LaserLineInterface_data_t));
562 }
563 
564 const char *
565 LaserLineInterface::enum_tostring(const char *enumtype, int val) const
566 {
567  throw UnknownTypeException("Unknown enum type %s", enumtype);
568 }
569 
570 /* =========== messages =========== */
571 /** Check if message is valid and can be enqueued.
572  * @param message Message to check
573  * @return true if the message is valid, false otherwise.
574  */
575 bool
576 LaserLineInterface::message_valid(const Message *message) const
577 {
578  return false;
579 }
580 
581 /// @cond INTERNALS
582 EXPORT_INTERFACE(LaserLineInterface)
583 /// @endcond
584 
585 
586 } // 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
LaserLineInterface Fawkes BlackBoard Interface.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
Fawkes library namespace.