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
33namespace 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 */
44LaserLineInterface::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 */
66LaserLineInterface::~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 */
77char *
78LaserLineInterface::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 */
87size_t
88LaserLineInterface::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 */
99void
100LaserLineInterface::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 */
115int32_t
116LaserLineInterface::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 */
125size_t
126LaserLineInterface::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 */
141void
142LaserLineInterface::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 */
153float *
154LaserLineInterface::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 */
167float
168LaserLineInterface::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 */
180size_t
181LaserLineInterface::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 */
192void
193LaserLineInterface::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 */
205void
206LaserLineInterface::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 */
216float *
217LaserLineInterface::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 */
230float
231LaserLineInterface::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 */
243size_t
244LaserLineInterface::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 */
255void
256LaserLineInterface::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 */
268void
269LaserLineInterface::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 */
280float
281LaserLineInterface::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 */
290size_t
291LaserLineInterface::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 */
303void
304LaserLineInterface::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 */
316float *
317LaserLineInterface::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 */
331float
332LaserLineInterface::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 */
344size_t
345LaserLineInterface::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 */
357void
358LaserLineInterface::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 */
371void
372LaserLineInterface::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 */
383float *
384LaserLineInterface::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 */
398float
399LaserLineInterface::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 */
411size_t
412LaserLineInterface::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 */
424void
425LaserLineInterface::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 */
438void
439LaserLineInterface::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 */
447float
448LaserLineInterface::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 */
457size_t
458LaserLineInterface::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 */
467void
468LaserLineInterface::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 */
479char *
480LaserLineInterface::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 */
489size_t
490LaserLineInterface::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 */
501void
502LaserLineInterface::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 */
513char *
514LaserLineInterface::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 */
523size_t
524LaserLineInterface::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 */
535void
536LaserLineInterface::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 =========== */
542Message *
543LaserLineInterface::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 */
553void
554LaserLineInterface::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
564const char *
565LaserLineInterface::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 */
575bool
576LaserLineInterface::message_valid(const Message *message) const
577{
578 return false;
579}
580
581/// @cond INTERNALS
582EXPORT_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.