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  strncpy(data->frame_id, new_frame_id, sizeof(data->frame_id)-1);
103  data->frame_id[sizeof(data->frame_id)-1] = 0;
104  data_changed = true;
105 }
106 
107 /** Get visibility_history value.
108  *
109  The visibilitiy history indicates the number of consecutive positive or negative
110  sightings. If the history is negative, there have been as many negative sightings
111  (object not visible) as the absolute value of the history. A positive value denotes
112  as many positive sightings. 0 shall only be used during the initialization of the
113  interface or if the visibility history is not updated.
114 
115  * @return visibility_history value
116  */
117 int32_t
118 LaserLineInterface::visibility_history() const
119 {
120  return data->visibility_history;
121 }
122 
123 /** Get maximum length of visibility_history value.
124  * @return length of visibility_history value, can be length of the array or number of
125  * maximum number of characters for a string
126  */
127 size_t
128 LaserLineInterface::maxlenof_visibility_history() const
129 {
130  return 1;
131 }
132 
133 /** Set visibility_history value.
134  *
135  The visibilitiy history indicates the number of consecutive positive or negative
136  sightings. If the history is negative, there have been as many negative sightings
137  (object not visible) as the absolute value of the history. A positive value denotes
138  as many positive sightings. 0 shall only be used during the initialization of the
139  interface or if the visibility history is not updated.
140 
141  * @param new_visibility_history new visibility_history value
142  */
143 void
144 LaserLineInterface::set_visibility_history(const int32_t new_visibility_history)
145 {
146  data->visibility_history = new_visibility_history;
147  data_changed = true;
148 }
149 
150 /** Get point_on_line value.
151  *
152  Vector to some point on the line
153 
154  * @return point_on_line value
155  */
156 float *
157 LaserLineInterface::point_on_line() const
158 {
159  return data->point_on_line;
160 }
161 
162 /** Get point_on_line value at given index.
163  *
164  Vector to some point on the line
165 
166  * @param index index of value
167  * @return point_on_line value
168  * @exception Exception thrown if index is out of bounds
169  */
170 float
171 LaserLineInterface::point_on_line(unsigned int index) const
172 {
173  if (index > 2) {
174  throw Exception("Index value %u out of bounds (0..2)", index);
175  }
176  return data->point_on_line[index];
177 }
178 
179 /** Get maximum length of point_on_line value.
180  * @return length of point_on_line value, can be length of the array or number of
181  * maximum number of characters for a string
182  */
183 size_t
184 LaserLineInterface::maxlenof_point_on_line() const
185 {
186  return 3;
187 }
188 
189 /** Set point_on_line value.
190  *
191  Vector to some point on the line
192 
193  * @param new_point_on_line new point_on_line value
194  */
195 void
196 LaserLineInterface::set_point_on_line(const float * new_point_on_line)
197 {
198  memcpy(data->point_on_line, new_point_on_line, sizeof(float) * 3);
199  data_changed = true;
200 }
201 
202 /** Set point_on_line value at given index.
203  *
204  Vector to some point on the line
205 
206  * @param new_point_on_line new point_on_line value
207  * @param index index for of the value
208  */
209 void
210 LaserLineInterface::set_point_on_line(unsigned int index, const float new_point_on_line)
211 {
212  if (index > 2) {
213  throw Exception("Index value %u out of bounds (0..2)", index);
214  }
215  data->point_on_line[index] = new_point_on_line;
216  data_changed = true;
217 }
218 /** Get line_direction value.
219  *
220  Vector in the direction of the line.
221 
222  * @return line_direction value
223  */
224 float *
225 LaserLineInterface::line_direction() const
226 {
227  return data->line_direction;
228 }
229 
230 /** Get line_direction value at given index.
231  *
232  Vector in the direction of the line.
233 
234  * @param index index of value
235  * @return line_direction value
236  * @exception Exception thrown if index is out of bounds
237  */
238 float
239 LaserLineInterface::line_direction(unsigned int index) const
240 {
241  if (index > 2) {
242  throw Exception("Index value %u out of bounds (0..2)", index);
243  }
244  return data->line_direction[index];
245 }
246 
247 /** Get maximum length of line_direction value.
248  * @return length of line_direction value, can be length of the array or number of
249  * maximum number of characters for a string
250  */
251 size_t
252 LaserLineInterface::maxlenof_line_direction() const
253 {
254  return 3;
255 }
256 
257 /** Set line_direction value.
258  *
259  Vector in the direction of the line.
260 
261  * @param new_line_direction new line_direction value
262  */
263 void
264 LaserLineInterface::set_line_direction(const float * new_line_direction)
265 {
266  memcpy(data->line_direction, new_line_direction, sizeof(float) * 3);
267  data_changed = true;
268 }
269 
270 /** Set line_direction value at given index.
271  *
272  Vector in the direction of the line.
273 
274  * @param new_line_direction new line_direction value
275  * @param index index for of the value
276  */
277 void
278 LaserLineInterface::set_line_direction(unsigned int index, const float new_line_direction)
279 {
280  if (index > 2) {
281  throw Exception("Index value %u out of bounds (0..2)", index);
282  }
283  data->line_direction[index] = new_line_direction;
284  data_changed = true;
285 }
286 /** Get bearing value.
287  *
288  Direction towards the line, i.e. if the robot turns by this
289  angle the robot will stand parallel to the line.
290 
291  * @return bearing value
292  */
293 float
294 LaserLineInterface::bearing() const
295 {
296  return data->bearing;
297 }
298 
299 /** Get maximum length of bearing value.
300  * @return length of bearing value, can be length of the array or number of
301  * maximum number of characters for a string
302  */
303 size_t
304 LaserLineInterface::maxlenof_bearing() const
305 {
306  return 1;
307 }
308 
309 /** Set bearing value.
310  *
311  Direction towards the line, i.e. if the robot turns by this
312  angle the robot will stand parallel to the line.
313 
314  * @param new_bearing new bearing value
315  */
316 void
317 LaserLineInterface::set_bearing(const float new_bearing)
318 {
319  data->bearing = new_bearing;
320  data_changed = true;
321 }
322 
323 /** Get end_point_1 value.
324  *
325  3D coordinates in the reference frame of one endpoint of the
326  line. The end points are ordered arbitrarily.
327 
328  * @return end_point_1 value
329  */
330 float *
331 LaserLineInterface::end_point_1() const
332 {
333  return data->end_point_1;
334 }
335 
336 /** Get end_point_1 value at given index.
337  *
338  3D coordinates in the reference frame of one endpoint of the
339  line. The end points are ordered arbitrarily.
340 
341  * @param index index of value
342  * @return end_point_1 value
343  * @exception Exception thrown if index is out of bounds
344  */
345 float
346 LaserLineInterface::end_point_1(unsigned int index) const
347 {
348  if (index > 2) {
349  throw Exception("Index value %u out of bounds (0..2)", index);
350  }
351  return data->end_point_1[index];
352 }
353 
354 /** Get maximum length of end_point_1 value.
355  * @return length of end_point_1 value, can be length of the array or number of
356  * maximum number of characters for a string
357  */
358 size_t
359 LaserLineInterface::maxlenof_end_point_1() const
360 {
361  return 3;
362 }
363 
364 /** Set end_point_1 value.
365  *
366  3D coordinates in the reference frame of one endpoint of the
367  line. The end points are ordered arbitrarily.
368 
369  * @param new_end_point_1 new end_point_1 value
370  */
371 void
372 LaserLineInterface::set_end_point_1(const float * new_end_point_1)
373 {
374  memcpy(data->end_point_1, new_end_point_1, sizeof(float) * 3);
375  data_changed = true;
376 }
377 
378 /** Set end_point_1 value at given index.
379  *
380  3D coordinates in the reference frame of one endpoint of the
381  line. The end points are ordered arbitrarily.
382 
383  * @param new_end_point_1 new end_point_1 value
384  * @param index index for of the value
385  */
386 void
387 LaserLineInterface::set_end_point_1(unsigned int index, const float new_end_point_1)
388 {
389  if (index > 2) {
390  throw Exception("Index value %u out of bounds (0..2)", index);
391  }
392  data->end_point_1[index] = new_end_point_1;
393  data_changed = true;
394 }
395 /** Get end_point_2 value.
396  *
397  3D coordinates in the reference frame of the second endpoint of
398  the line.
399 
400  * @return end_point_2 value
401  */
402 float *
403 LaserLineInterface::end_point_2() const
404 {
405  return data->end_point_2;
406 }
407 
408 /** Get end_point_2 value at given index.
409  *
410  3D coordinates in the reference frame of the second endpoint of
411  the line.
412 
413  * @param index index of value
414  * @return end_point_2 value
415  * @exception Exception thrown if index is out of bounds
416  */
417 float
418 LaserLineInterface::end_point_2(unsigned int index) const
419 {
420  if (index > 2) {
421  throw Exception("Index value %u out of bounds (0..2)", index);
422  }
423  return data->end_point_2[index];
424 }
425 
426 /** Get maximum length of end_point_2 value.
427  * @return length of end_point_2 value, can be length of the array or number of
428  * maximum number of characters for a string
429  */
430 size_t
431 LaserLineInterface::maxlenof_end_point_2() const
432 {
433  return 3;
434 }
435 
436 /** Set end_point_2 value.
437  *
438  3D coordinates in the reference frame of the second endpoint of
439  the line.
440 
441  * @param new_end_point_2 new end_point_2 value
442  */
443 void
444 LaserLineInterface::set_end_point_2(const float * new_end_point_2)
445 {
446  memcpy(data->end_point_2, new_end_point_2, sizeof(float) * 3);
447  data_changed = true;
448 }
449 
450 /** Set end_point_2 value at given index.
451  *
452  3D coordinates in the reference frame of the second endpoint of
453  the line.
454 
455  * @param new_end_point_2 new end_point_2 value
456  * @param index index for of the value
457  */
458 void
459 LaserLineInterface::set_end_point_2(unsigned int index, const float new_end_point_2)
460 {
461  if (index > 2) {
462  throw Exception("Index value %u out of bounds (0..2)", index);
463  }
464  data->end_point_2[index] = new_end_point_2;
465  data_changed = true;
466 }
467 /** Get length value.
468  * Length of the line.
469  * @return length value
470  */
471 float
472 LaserLineInterface::length() const
473 {
474  return data->length;
475 }
476 
477 /** Get maximum length of length value.
478  * @return length of length value, can be length of the array or number of
479  * maximum number of characters for a string
480  */
481 size_t
482 LaserLineInterface::maxlenof_length() const
483 {
484  return 1;
485 }
486 
487 /** Set length value.
488  * Length of the line.
489  * @param new_length new length value
490  */
491 void
492 LaserLineInterface::set_length(const float new_length)
493 {
494  data->length = new_length;
495  data_changed = true;
496 }
497 
498 /** Get end_point_frame_1 value.
499  *
500  Coordinate frame ID representing first endpoint's identity
501 
502  * @return end_point_frame_1 value
503  */
504 char *
505 LaserLineInterface::end_point_frame_1() const
506 {
507  return data->end_point_frame_1;
508 }
509 
510 /** Get maximum length of end_point_frame_1 value.
511  * @return length of end_point_frame_1 value, can be length of the array or number of
512  * maximum number of characters for a string
513  */
514 size_t
515 LaserLineInterface::maxlenof_end_point_frame_1() const
516 {
517  return 32;
518 }
519 
520 /** Set end_point_frame_1 value.
521  *
522  Coordinate frame ID representing first endpoint's identity
523 
524  * @param new_end_point_frame_1 new end_point_frame_1 value
525  */
526 void
527 LaserLineInterface::set_end_point_frame_1(const char * new_end_point_frame_1)
528 {
529  strncpy(data->end_point_frame_1, new_end_point_frame_1, sizeof(data->end_point_frame_1)-1);
530  data->end_point_frame_1[sizeof(data->end_point_frame_1)-1] = 0;
531  data_changed = true;
532 }
533 
534 /** Get end_point_frame_2 value.
535  *
536  Coordinate frame ID representing second endpoint's identity
537 
538  * @return end_point_frame_2 value
539  */
540 char *
541 LaserLineInterface::end_point_frame_2() const
542 {
543  return data->end_point_frame_2;
544 }
545 
546 /** Get maximum length of end_point_frame_2 value.
547  * @return length of end_point_frame_2 value, can be length of the array or number of
548  * maximum number of characters for a string
549  */
550 size_t
551 LaserLineInterface::maxlenof_end_point_frame_2() const
552 {
553  return 32;
554 }
555 
556 /** Set end_point_frame_2 value.
557  *
558  Coordinate frame ID representing second endpoint's identity
559 
560  * @param new_end_point_frame_2 new end_point_frame_2 value
561  */
562 void
563 LaserLineInterface::set_end_point_frame_2(const char * new_end_point_frame_2)
564 {
565  strncpy(data->end_point_frame_2, new_end_point_frame_2, sizeof(data->end_point_frame_2)-1);
566  data->end_point_frame_2[sizeof(data->end_point_frame_2)-1] = 0;
567  data_changed = true;
568 }
569 
570 /* =========== message create =========== */
571 Message *
572 LaserLineInterface::create_message(const char *type) const
573 {
574  throw UnknownTypeException("The given type '%s' does not match any known "
575  "message type for this interface type.", type);
576 }
577 
578 
579 /** Copy values from other interface.
580  * @param other other interface to copy values from
581  */
582 void
583 LaserLineInterface::copy_values(const Interface *other)
584 {
585  const LaserLineInterface *oi = dynamic_cast<const LaserLineInterface *>(other);
586  if (oi == NULL) {
587  throw TypeMismatchException("Can only copy values from interface of same type (%s vs. %s)",
588  type(), other->type());
589  }
590  memcpy(data, oi->data, sizeof(LaserLineInterface_data_t));
591 }
592 
593 const char *
594 LaserLineInterface::enum_tostring(const char *enumtype, int val) const
595 {
596  throw UnknownTypeException("Unknown enum type %s", enumtype);
597 }
598 
599 /* =========== messages =========== */
600 /** Check if message is valid and can be enqueued.
601  * @param message Message to check
602  * @return true if the message is valid, false otherwise.
603  */
604 bool
605 LaserLineInterface::message_valid(const Message *message) const
606 {
607  return false;
608 }
609 
610 /// @cond INTERNALS
611 EXPORT_INTERFACE(LaserLineInterface)
612 /// @endcond
613 
614 
615 } // end namespace fawkes
fawkes::Message
Definition: message.h:41
fawkes::Interface::type
const char * type() const
Get type of interface.
Definition: interface.cpp:645
fawkes::TypeMismatchException
Definition: software.h:49
fawkes::UnknownTypeException
Definition: software.h:55
fawkes
fawkes::Interface
Definition: interface.h:78
fawkes::LaserLineInterface
Definition: LaserLineInterface.h:39
fawkes::Exception
Definition: exception.h:41