Fawkes API Fawkes Development Version
ObjectPositionInterface.cpp
1
2/***************************************************************************
3 * ObjectPositionInterface.cpp - Fawkes BlackBoard Interface - ObjectPositionInterface
4 *
5 * Templated created: Thu Oct 12 10:49:19 2006
6 * Copyright 2007-2008 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/ObjectPositionInterface.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 ObjectPositionInterface <interfaces/ObjectPositionInterface.h>
36 * ObjectPositionInterface Fawkes BlackBoard Interface.
37 *
38 This interface provides access to arbitrary object positions and velocities. You
39 can use it to store the position of any object in the RoboCup domain. There is a type
40 indicator for the RoboCup soccer domain to easily distinguish several well
41 known objects. You may choose not to use this for other application in which case
42 the value should be other (which is also the default).
43
44 * @ingroup FawkesInterfaces
45 */
46
47
48/** TYPE_OTHER constant */
49const uint32_t ObjectPositionInterface::TYPE_OTHER = 0u;
50/** TYPE_BALL constant */
51const uint32_t ObjectPositionInterface::TYPE_BALL = 1u;
52/** TYPE_OPPONENT constant */
54/** TYPE_TEAMMEMBER constant */
56/** TYPE_LINE constant */
57const uint32_t ObjectPositionInterface::TYPE_LINE = 4u;
58/** TYPE_SELF constant */
59const uint32_t ObjectPositionInterface::TYPE_SELF = 5u;
60/** TYPE_GOAL_BLUE constant */
62/** TYPE_GOAL_YELLOW constant */
64/** FLAG_NONE constant */
65const uint32_t ObjectPositionInterface::FLAG_NONE = 0u;
66/** FLAG_HAS_WORLD constant */
68/** FLAG_HAS_RELATIVE_CARTESIAN constant */
70/** FLAG_HAS_RELATIVE_POLAR constant */
72/** FLAG_HAS_EULER_ANGLES constant */
74/** FLAG_HAS_EXTENT constant */
76/** FLAG_HAS_VOLUME_EXTENT constant */
78/** FLAG_HAS_CIRCULAR_EXTENT constant */
80/** FLAG_HAS_COVARIANCES constant */
82/** FLAG_HAS_WORLD_VELOCITY constant */
84/** FLAG_HAS_Z_AS_ORI constant */
86/** FLAG_IS_FIXED_OBJECT constant */
88
89/** Constructor */
90ObjectPositionInterface::ObjectPositionInterface() : Interface()
91{
92 data_size = sizeof(ObjectPositionInterface_data_t);
93 data_ptr = malloc(data_size);
94 data = (ObjectPositionInterface_data_t *)data_ptr;
95 data_ts = (interface_data_ts_t *)data_ptr;
96 memset(data_ptr, 0, data_size);
97 add_fieldinfo(IFT_UINT32, "object_type", 1, &data->object_type);
98 add_fieldinfo(IFT_UINT32, "flags", 1, &data->flags);
99 add_fieldinfo(IFT_BOOL, "visible", 1, &data->visible);
100 add_fieldinfo(IFT_INT32, "visibility_history", 1, &data->visibility_history);
101 add_fieldinfo(IFT_FLOAT, "roll", 1, &data->roll);
102 add_fieldinfo(IFT_FLOAT, "pitch", 1, &data->pitch);
103 add_fieldinfo(IFT_FLOAT, "yaw", 1, &data->yaw);
104 add_fieldinfo(IFT_FLOAT, "distance", 1, &data->distance);
105 add_fieldinfo(IFT_FLOAT, "bearing", 1, &data->bearing);
106 add_fieldinfo(IFT_FLOAT, "slope", 1, &data->slope);
107 add_fieldinfo(IFT_FLOAT, "dbs_covariance", 9, &data->dbs_covariance);
108 add_fieldinfo(IFT_FLOAT, "world_x", 1, &data->world_x);
109 add_fieldinfo(IFT_FLOAT, "world_y", 1, &data->world_y);
110 add_fieldinfo(IFT_FLOAT, "world_z", 1, &data->world_z);
111 add_fieldinfo(IFT_FLOAT, "world_xyz_covariance", 9, &data->world_xyz_covariance);
112 add_fieldinfo(IFT_FLOAT, "relative_x", 1, &data->relative_x);
113 add_fieldinfo(IFT_FLOAT, "relative_y", 1, &data->relative_y);
114 add_fieldinfo(IFT_FLOAT, "relative_z", 1, &data->relative_z);
115 add_fieldinfo(IFT_FLOAT, "relative_xyz_covariance", 9, &data->relative_xyz_covariance);
116 add_fieldinfo(IFT_FLOAT, "extent_x", 1, &data->extent_x);
117 add_fieldinfo(IFT_FLOAT, "extent_y", 1, &data->extent_y);
118 add_fieldinfo(IFT_FLOAT, "extent_z", 1, &data->extent_z);
119 add_fieldinfo(IFT_FLOAT, "world_x_velocity", 1, &data->world_x_velocity);
120 add_fieldinfo(IFT_FLOAT, "world_y_velocity", 1, &data->world_y_velocity);
121 add_fieldinfo(IFT_FLOAT, "world_z_velocity", 1, &data->world_z_velocity);
122 add_fieldinfo(IFT_FLOAT, "world_xyz_velocity_covariance", 9, &data->world_xyz_velocity_covariance);
123 add_fieldinfo(IFT_FLOAT, "relative_x_velocity", 1, &data->relative_x_velocity);
124 add_fieldinfo(IFT_FLOAT, "relative_y_velocity", 1, &data->relative_y_velocity);
125 add_fieldinfo(IFT_FLOAT, "relative_z_velocity", 1, &data->relative_z_velocity);
126 add_fieldinfo(IFT_FLOAT, "relative_xyz_velocity_covariance", 9, &data->relative_xyz_velocity_covariance);
127 unsigned char tmp_hash[] = {0x65, 0xac, 0x82, 0xde, 0xcd, 0x8b, 0x12, 0x81, 0x7b, 0xe6, 0x61, 0xdd, 0x30, 0x55, 0xd, 0x38};
128 set_hash(tmp_hash);
129}
130
131/** Destructor */
132ObjectPositionInterface::~ObjectPositionInterface()
133{
134 free(data_ptr);
135}
136/* Methods */
137/** Get object_type value.
138 *
139 Object type, use constants to define
140
141 * @return object_type value
142 */
143uint32_t
144ObjectPositionInterface::object_type() const
145{
146 return data->object_type;
147}
148
149/** Get maximum length of object_type value.
150 * @return length of object_type value, can be length of the array or number of
151 * maximum number of characters for a string
152 */
153size_t
154ObjectPositionInterface::maxlenof_object_type() const
155{
156 return 1;
157}
158
159/** Set object_type value.
160 *
161 Object type, use constants to define
162
163 * @param new_object_type new object_type value
164 */
165void
166ObjectPositionInterface::set_object_type(const uint32_t new_object_type)
167{
168 set_field(data->object_type, new_object_type);
169}
170
171/** Get flags value.
172 *
173 Bit-wise concatenated fields of FLAG_* constants. Denotes features that the
174 writer of this interfaces provides. Use a bit-wise OR to concatenate multiple
175 flags, use a bit-wise AND to check if a flag has been set.
176
177 * @return flags value
178 */
179uint32_t
180ObjectPositionInterface::flags() const
181{
182 return data->flags;
183}
184
185/** Get maximum length of flags value.
186 * @return length of flags value, can be length of the array or number of
187 * maximum number of characters for a string
188 */
189size_t
190ObjectPositionInterface::maxlenof_flags() const
191{
192 return 1;
193}
194
195/** Set flags value.
196 *
197 Bit-wise concatenated fields of FLAG_* constants. Denotes features that the
198 writer of this interfaces provides. Use a bit-wise OR to concatenate multiple
199 flags, use a bit-wise AND to check if a flag has been set.
200
201 * @param new_flags new flags value
202 */
203void
204ObjectPositionInterface::set_flags(const uint32_t new_flags)
205{
206 set_field(data->flags, new_flags);
207}
208
209/** Get visible value.
210 * True, if object is visible.
211 * @return visible value
212 */
213bool
214ObjectPositionInterface::is_visible() const
215{
216 return data->visible;
217}
218
219/** Get maximum length of visible value.
220 * @return length of visible value, can be length of the array or number of
221 * maximum number of characters for a string
222 */
223size_t
224ObjectPositionInterface::maxlenof_visible() const
225{
226 return 1;
227}
228
229/** Set visible value.
230 * True, if object is visible.
231 * @param new_visible new visible value
232 */
233void
234ObjectPositionInterface::set_visible(const bool new_visible)
235{
236 set_field(data->visible, new_visible);
237}
238
239/** Get visibility_history value.
240 *
241 The visibilitiy history indicates the number of consecutive positive or negative
242 sightings. If the history is negative, there have been as many negative sightings
243 (object not visible) as the absolute value of the history. A positive value denotes
244 as many positive sightings. 0 shall only be used during the initialisation of the
245 interface or if the visibility history is not filled.
246
247 * @return visibility_history value
248 */
249int32_t
250ObjectPositionInterface::visibility_history() const
251{
252 return data->visibility_history;
253}
254
255/** Get maximum length of visibility_history value.
256 * @return length of visibility_history value, can be length of the array or number of
257 * maximum number of characters for a string
258 */
259size_t
260ObjectPositionInterface::maxlenof_visibility_history() const
261{
262 return 1;
263}
264
265/** Set visibility_history value.
266 *
267 The visibilitiy history indicates the number of consecutive positive or negative
268 sightings. If the history is negative, there have been as many negative sightings
269 (object not visible) as the absolute value of the history. A positive value denotes
270 as many positive sightings. 0 shall only be used during the initialisation of the
271 interface or if the visibility history is not filled.
272
273 * @param new_visibility_history new visibility_history value
274 */
275void
276ObjectPositionInterface::set_visibility_history(const int32_t new_visibility_history)
277{
278 set_field(data->visibility_history, new_visibility_history);
279}
280
281/** Get roll value.
282 *
283 Roll value for the orientation of the object in space.
284
285 * @return roll value
286 */
287float
288ObjectPositionInterface::roll() const
289{
290 return data->roll;
291}
292
293/** Get maximum length of roll value.
294 * @return length of roll value, can be length of the array or number of
295 * maximum number of characters for a string
296 */
297size_t
298ObjectPositionInterface::maxlenof_roll() const
299{
300 return 1;
301}
302
303/** Set roll value.
304 *
305 Roll value for the orientation of the object in space.
306
307 * @param new_roll new roll value
308 */
309void
310ObjectPositionInterface::set_roll(const float new_roll)
311{
312 set_field(data->roll, new_roll);
313}
314
315/** Get pitch value.
316 *
317 Pitch value for the orientation of the object in space.
318
319 * @return pitch value
320 */
321float
322ObjectPositionInterface::pitch() const
323{
324 return data->pitch;
325}
326
327/** Get maximum length of pitch value.
328 * @return length of pitch value, can be length of the array or number of
329 * maximum number of characters for a string
330 */
331size_t
332ObjectPositionInterface::maxlenof_pitch() const
333{
334 return 1;
335}
336
337/** Set pitch value.
338 *
339 Pitch value for the orientation of the object in space.
340
341 * @param new_pitch new pitch value
342 */
343void
344ObjectPositionInterface::set_pitch(const float new_pitch)
345{
346 set_field(data->pitch, new_pitch);
347}
348
349/** Get yaw value.
350 *
351 Yaw value for the orientation of the object in space.
352
353 * @return yaw value
354 */
355float
356ObjectPositionInterface::yaw() const
357{
358 return data->yaw;
359}
360
361/** Get maximum length of yaw value.
362 * @return length of yaw value, can be length of the array or number of
363 * maximum number of characters for a string
364 */
365size_t
366ObjectPositionInterface::maxlenof_yaw() const
367{
368 return 1;
369}
370
371/** Set yaw value.
372 *
373 Yaw value for the orientation of the object in space.
374
375 * @param new_yaw new yaw value
376 */
377void
378ObjectPositionInterface::set_yaw(const float new_yaw)
379{
380 set_field(data->yaw, new_yaw);
381}
382
383/** Get distance value.
384 *
385 Distance from the robot to the object on the ground plane. The distance is given
386 in meters.
387
388 * @return distance value
389 */
390float
391ObjectPositionInterface::distance() const
392{
393 return data->distance;
394}
395
396/** Get maximum length of distance value.
397 * @return length of distance value, can be length of the array or number of
398 * maximum number of characters for a string
399 */
400size_t
401ObjectPositionInterface::maxlenof_distance() const
402{
403 return 1;
404}
405
406/** Set distance value.
407 *
408 Distance from the robot to the object on the ground plane. The distance is given
409 in meters.
410
411 * @param new_distance new distance value
412 */
413void
414ObjectPositionInterface::set_distance(const float new_distance)
415{
416 set_field(data->distance, new_distance);
417}
418
419/** Get bearing value.
420 *
421 Angle between the robot's forward direction and the object on the ground plane.
422 This angle is in a local 3D coordinate system to the robot and given in radians.
423
424 * @return bearing value
425 */
426float
427ObjectPositionInterface::bearing() const
428{
429 return data->bearing;
430}
431
432/** Get maximum length of bearing value.
433 * @return length of bearing value, can be length of the array or number of
434 * maximum number of characters for a string
435 */
436size_t
437ObjectPositionInterface::maxlenof_bearing() const
438{
439 return 1;
440}
441
442/** Set bearing value.
443 *
444 Angle between the robot's forward direction and the object on the ground plane.
445 This angle is in a local 3D coordinate system to the robot and given in radians.
446
447 * @param new_bearing new bearing value
448 */
449void
450ObjectPositionInterface::set_bearing(const float new_bearing)
451{
452 set_field(data->bearing, new_bearing);
453}
454
455/** Get slope value.
456 *
457 Angle between the robot's center position on the ground plane and the middle point
458 of the object (e.g. this denotes the height of the object combined with the distance.
459 The angle is given in radians.
460
461 * @return slope value
462 */
463float
464ObjectPositionInterface::slope() const
465{
466 return data->slope;
467}
468
469/** Get maximum length of slope value.
470 * @return length of slope value, can be length of the array or number of
471 * maximum number of characters for a string
472 */
473size_t
474ObjectPositionInterface::maxlenof_slope() const
475{
476 return 1;
477}
478
479/** Set slope value.
480 *
481 Angle between the robot's center position on the ground plane and the middle point
482 of the object (e.g. this denotes the height of the object combined with the distance.
483 The angle is given in radians.
484
485 * @param new_slope new slope value
486 */
487void
488ObjectPositionInterface::set_slope(const float new_slope)
489{
490 set_field(data->slope, new_slope);
491}
492
493/** Get dbs_covariance value.
494 *
495 Covariance of Distance/Yaw/Pitch values. This is a 3x3 matrix ordered line by line,
496 first three values represent row, next tree values second row and last three values
497 last row from left to right each.
498
499 * @return dbs_covariance value
500 */
501float *
502ObjectPositionInterface::dbs_covariance() const
503{
504 return data->dbs_covariance;
505}
506
507/** Get dbs_covariance value at given index.
508 *
509 Covariance of Distance/Yaw/Pitch values. This is a 3x3 matrix ordered line by line,
510 first three values represent row, next tree values second row and last three values
511 last row from left to right each.
512
513 * @param index index of value
514 * @return dbs_covariance value
515 * @exception Exception thrown if index is out of bounds
516 */
517float
518ObjectPositionInterface::dbs_covariance(unsigned int index) const
519{
520 if (index > 8) {
521 throw Exception("Index value %u out of bounds (0..8)", index);
522 }
523 return data->dbs_covariance[index];
524}
525
526/** Get maximum length of dbs_covariance value.
527 * @return length of dbs_covariance value, can be length of the array or number of
528 * maximum number of characters for a string
529 */
530size_t
531ObjectPositionInterface::maxlenof_dbs_covariance() const
532{
533 return 9;
534}
535
536/** Set dbs_covariance value.
537 *
538 Covariance of Distance/Yaw/Pitch values. This is a 3x3 matrix ordered line by line,
539 first three values represent row, next tree values second row and last three values
540 last row from left to right each.
541
542 * @param new_dbs_covariance new dbs_covariance value
543 */
544void
545ObjectPositionInterface::set_dbs_covariance(const float * new_dbs_covariance)
546{
547 set_field(data->dbs_covariance, new_dbs_covariance);
548}
549
550/** Set dbs_covariance value at given index.
551 *
552 Covariance of Distance/Yaw/Pitch values. This is a 3x3 matrix ordered line by line,
553 first three values represent row, next tree values second row and last three values
554 last row from left to right each.
555
556 * @param new_dbs_covariance new dbs_covariance value
557 * @param index index for of the value
558 */
559void
560ObjectPositionInterface::set_dbs_covariance(unsigned int index, const float new_dbs_covariance)
561{
562 set_field(data->dbs_covariance, index, new_dbs_covariance);
563}
564/** Get world_x value.
565 *
566 This is the X coordinate in the cartesian right-handed world coordinate system.
567 This coordinate system has its origin in the center of the field, Y pointing to
568 the opponent's goal and X pointing to the right.
569
570 * @return world_x value
571 */
572float
573ObjectPositionInterface::world_x() const
574{
575 return data->world_x;
576}
577
578/** Get maximum length of world_x value.
579 * @return length of world_x value, can be length of the array or number of
580 * maximum number of characters for a string
581 */
582size_t
583ObjectPositionInterface::maxlenof_world_x() const
584{
585 return 1;
586}
587
588/** Set world_x value.
589 *
590 This is the X coordinate in the cartesian right-handed world coordinate system.
591 This coordinate system has its origin in the center of the field, Y pointing to
592 the opponent's goal and X pointing to the right.
593
594 * @param new_world_x new world_x value
595 */
596void
597ObjectPositionInterface::set_world_x(const float new_world_x)
598{
599 set_field(data->world_x, new_world_x);
600}
601
602/** Get world_y value.
603 *
604 This is the Y coordinate in the cartesian right-handed world coordinate system.
605 This coordinate system has its origin in the center of the field, Y pointing to
606 the opponent's goal and X pointing to the right and Z pointing downwards.
607
608 * @return world_y value
609 */
610float
611ObjectPositionInterface::world_y() const
612{
613 return data->world_y;
614}
615
616/** Get maximum length of world_y value.
617 * @return length of world_y value, can be length of the array or number of
618 * maximum number of characters for a string
619 */
620size_t
621ObjectPositionInterface::maxlenof_world_y() const
622{
623 return 1;
624}
625
626/** Set world_y value.
627 *
628 This is the Y coordinate in the cartesian right-handed world coordinate system.
629 This coordinate system has its origin in the center of the field, Y pointing to
630 the opponent's goal and X pointing to the right and Z pointing downwards.
631
632 * @param new_world_y new world_y value
633 */
634void
635ObjectPositionInterface::set_world_y(const float new_world_y)
636{
637 set_field(data->world_y, new_world_y);
638}
639
640/** Get world_z value.
641 *
642 This is the Z coordinate in the cartesian right-handed world coordinate system.
643 This coordinate system has its origin in the center of the field, Y pointing to
644 the opponent's goal and X pointing to the right.
645
646 * @return world_z value
647 */
648float
649ObjectPositionInterface::world_z() const
650{
651 return data->world_z;
652}
653
654/** Get maximum length of world_z value.
655 * @return length of world_z value, can be length of the array or number of
656 * maximum number of characters for a string
657 */
658size_t
659ObjectPositionInterface::maxlenof_world_z() const
660{
661 return 1;
662}
663
664/** Set world_z value.
665 *
666 This is the Z coordinate in the cartesian right-handed world coordinate system.
667 This coordinate system has its origin in the center of the field, Y pointing to
668 the opponent's goal and X pointing to the right.
669
670 * @param new_world_z new world_z value
671 */
672void
673ObjectPositionInterface::set_world_z(const float new_world_z)
674{
675 set_field(data->world_z, new_world_z);
676}
677
678/** Get world_xyz_covariance value.
679 *
680 Covariance of WorldX/WorldY/WorldZ values. This is a 3x3 matrix ordered line by line,
681 first three values represent row, next tree values second row and last three values
682 last row from left to right each.
683
684 * @return world_xyz_covariance value
685 */
686float *
687ObjectPositionInterface::world_xyz_covariance() const
688{
689 return data->world_xyz_covariance;
690}
691
692/** Get world_xyz_covariance value at given index.
693 *
694 Covariance of WorldX/WorldY/WorldZ values. This is a 3x3 matrix ordered line by line,
695 first three values represent row, next tree values second row and last three values
696 last row from left to right each.
697
698 * @param index index of value
699 * @return world_xyz_covariance value
700 * @exception Exception thrown if index is out of bounds
701 */
702float
703ObjectPositionInterface::world_xyz_covariance(unsigned int index) const
704{
705 if (index > 8) {
706 throw Exception("Index value %u out of bounds (0..8)", index);
707 }
708 return data->world_xyz_covariance[index];
709}
710
711/** Get maximum length of world_xyz_covariance value.
712 * @return length of world_xyz_covariance value, can be length of the array or number of
713 * maximum number of characters for a string
714 */
715size_t
716ObjectPositionInterface::maxlenof_world_xyz_covariance() const
717{
718 return 9;
719}
720
721/** Set world_xyz_covariance value.
722 *
723 Covariance of WorldX/WorldY/WorldZ values. This is a 3x3 matrix ordered line by line,
724 first three values represent row, next tree values second row and last three values
725 last row from left to right each.
726
727 * @param new_world_xyz_covariance new world_xyz_covariance value
728 */
729void
730ObjectPositionInterface::set_world_xyz_covariance(const float * new_world_xyz_covariance)
731{
732 set_field(data->world_xyz_covariance, new_world_xyz_covariance);
733}
734
735/** Set world_xyz_covariance value at given index.
736 *
737 Covariance of WorldX/WorldY/WorldZ values. This is a 3x3 matrix ordered line by line,
738 first three values represent row, next tree values second row and last three values
739 last row from left to right each.
740
741 * @param new_world_xyz_covariance new world_xyz_covariance value
742 * @param index index for of the value
743 */
744void
745ObjectPositionInterface::set_world_xyz_covariance(unsigned int index, const float new_world_xyz_covariance)
746{
747 set_field(data->world_xyz_covariance, index, new_world_xyz_covariance);
748}
749/** Get relative_x value.
750 *
751 This is the X coordinate in the cartesian right-handed robot coordinate system.
752
753 * @return relative_x value
754 */
755float
756ObjectPositionInterface::relative_x() const
757{
758 return data->relative_x;
759}
760
761/** Get maximum length of relative_x value.
762 * @return length of relative_x value, can be length of the array or number of
763 * maximum number of characters for a string
764 */
765size_t
766ObjectPositionInterface::maxlenof_relative_x() const
767{
768 return 1;
769}
770
771/** Set relative_x value.
772 *
773 This is the X coordinate in the cartesian right-handed robot coordinate system.
774
775 * @param new_relative_x new relative_x value
776 */
777void
778ObjectPositionInterface::set_relative_x(const float new_relative_x)
779{
780 set_field(data->relative_x, new_relative_x);
781}
782
783/** Get relative_y value.
784 *
785 This is the Y coordinate in the cartesian right-handed robot coordinate system.
786
787 * @return relative_y value
788 */
789float
790ObjectPositionInterface::relative_y() const
791{
792 return data->relative_y;
793}
794
795/** Get maximum length of relative_y value.
796 * @return length of relative_y value, can be length of the array or number of
797 * maximum number of characters for a string
798 */
799size_t
800ObjectPositionInterface::maxlenof_relative_y() const
801{
802 return 1;
803}
804
805/** Set relative_y value.
806 *
807 This is the Y coordinate in the cartesian right-handed robot coordinate system.
808
809 * @param new_relative_y new relative_y value
810 */
811void
812ObjectPositionInterface::set_relative_y(const float new_relative_y)
813{
814 set_field(data->relative_y, new_relative_y);
815}
816
817/** Get relative_z value.
818 *
819 This is the Z coordinate in the cartesian right-handed robot coordinate system.
820
821 * @return relative_z value
822 */
823float
824ObjectPositionInterface::relative_z() const
825{
826 return data->relative_z;
827}
828
829/** Get maximum length of relative_z value.
830 * @return length of relative_z value, can be length of the array or number of
831 * maximum number of characters for a string
832 */
833size_t
834ObjectPositionInterface::maxlenof_relative_z() const
835{
836 return 1;
837}
838
839/** Set relative_z value.
840 *
841 This is the Z coordinate in the cartesian right-handed robot coordinate system.
842
843 * @param new_relative_z new relative_z value
844 */
845void
846ObjectPositionInterface::set_relative_z(const float new_relative_z)
847{
848 set_field(data->relative_z, new_relative_z);
849}
850
851/** Get relative_xyz_covariance value.
852 *
853 Covariance of relative x/y/z values. This is a 3x3 matrix ordered line by line,
854 first three values represent row, next tree values second row and last three values
855 last row from left to right each.
856
857 * @return relative_xyz_covariance value
858 */
859float *
860ObjectPositionInterface::relative_xyz_covariance() const
861{
862 return data->relative_xyz_covariance;
863}
864
865/** Get relative_xyz_covariance value at given index.
866 *
867 Covariance of relative x/y/z values. This is a 3x3 matrix ordered line by line,
868 first three values represent row, next tree values second row and last three values
869 last row from left to right each.
870
871 * @param index index of value
872 * @return relative_xyz_covariance value
873 * @exception Exception thrown if index is out of bounds
874 */
875float
876ObjectPositionInterface::relative_xyz_covariance(unsigned int index) const
877{
878 if (index > 8) {
879 throw Exception("Index value %u out of bounds (0..8)", index);
880 }
881 return data->relative_xyz_covariance[index];
882}
883
884/** Get maximum length of relative_xyz_covariance value.
885 * @return length of relative_xyz_covariance value, can be length of the array or number of
886 * maximum number of characters for a string
887 */
888size_t
889ObjectPositionInterface::maxlenof_relative_xyz_covariance() const
890{
891 return 9;
892}
893
894/** Set relative_xyz_covariance value.
895 *
896 Covariance of relative x/y/z values. This is a 3x3 matrix ordered line by line,
897 first three values represent row, next tree values second row and last three values
898 last row from left to right each.
899
900 * @param new_relative_xyz_covariance new relative_xyz_covariance value
901 */
902void
903ObjectPositionInterface::set_relative_xyz_covariance(const float * new_relative_xyz_covariance)
904{
905 set_field(data->relative_xyz_covariance, new_relative_xyz_covariance);
906}
907
908/** Set relative_xyz_covariance value at given index.
909 *
910 Covariance of relative x/y/z values. This is a 3x3 matrix ordered line by line,
911 first three values represent row, next tree values second row and last three values
912 last row from left to right each.
913
914 * @param new_relative_xyz_covariance new relative_xyz_covariance value
915 * @param index index for of the value
916 */
917void
918ObjectPositionInterface::set_relative_xyz_covariance(unsigned int index, const float new_relative_xyz_covariance)
919{
920 set_field(data->relative_xyz_covariance, index, new_relative_xyz_covariance);
921}
922/** Get extent_x value.
923 *
924 Extent of the seen object given in the relative x cartesian coordinate in m.
925
926 * @return extent_x value
927 */
928float
929ObjectPositionInterface::extent_x() const
930{
931 return data->extent_x;
932}
933
934/** Get maximum length of extent_x value.
935 * @return length of extent_x value, can be length of the array or number of
936 * maximum number of characters for a string
937 */
938size_t
939ObjectPositionInterface::maxlenof_extent_x() const
940{
941 return 1;
942}
943
944/** Set extent_x value.
945 *
946 Extent of the seen object given in the relative x cartesian coordinate in m.
947
948 * @param new_extent_x new extent_x value
949 */
950void
951ObjectPositionInterface::set_extent_x(const float new_extent_x)
952{
953 set_field(data->extent_x, new_extent_x);
954}
955
956/** Get extent_y value.
957 *
958 Extent of the seen object given in the relative y cartesian coordinate in m.
959
960 * @return extent_y value
961 */
962float
963ObjectPositionInterface::extent_y() const
964{
965 return data->extent_y;
966}
967
968/** Get maximum length of extent_y value.
969 * @return length of extent_y value, can be length of the array or number of
970 * maximum number of characters for a string
971 */
972size_t
973ObjectPositionInterface::maxlenof_extent_y() const
974{
975 return 1;
976}
977
978/** Set extent_y value.
979 *
980 Extent of the seen object given in the relative y cartesian coordinate in m.
981
982 * @param new_extent_y new extent_y value
983 */
984void
985ObjectPositionInterface::set_extent_y(const float new_extent_y)
986{
987 set_field(data->extent_y, new_extent_y);
988}
989
990/** Get extent_z value.
991 *
992 Extent of the seen object given in the relative z cartesian coordinate in m.
993
994 * @return extent_z value
995 */
996float
997ObjectPositionInterface::extent_z() const
998{
999 return data->extent_z;
1000}
1001
1002/** Get maximum length of extent_z value.
1003 * @return length of extent_z value, can be length of the array or number of
1004 * maximum number of characters for a string
1005 */
1006size_t
1007ObjectPositionInterface::maxlenof_extent_z() const
1008{
1009 return 1;
1010}
1011
1012/** Set extent_z value.
1013 *
1014 Extent of the seen object given in the relative z cartesian coordinate in m.
1015
1016 * @param new_extent_z new extent_z value
1017 */
1018void
1019ObjectPositionInterface::set_extent_z(const float new_extent_z)
1020{
1021 set_field(data->extent_z, new_extent_z);
1022}
1023
1024/** Get world_x_velocity value.
1025 *
1026 Velocity of object in the world coordinate system in X-direction in meter per second.
1027
1028 * @return world_x_velocity value
1029 */
1030float
1031ObjectPositionInterface::world_x_velocity() const
1032{
1033 return data->world_x_velocity;
1034}
1035
1036/** Get maximum length of world_x_velocity value.
1037 * @return length of world_x_velocity value, can be length of the array or number of
1038 * maximum number of characters for a string
1039 */
1040size_t
1041ObjectPositionInterface::maxlenof_world_x_velocity() const
1042{
1043 return 1;
1044}
1045
1046/** Set world_x_velocity value.
1047 *
1048 Velocity of object in the world coordinate system in X-direction in meter per second.
1049
1050 * @param new_world_x_velocity new world_x_velocity value
1051 */
1052void
1053ObjectPositionInterface::set_world_x_velocity(const float new_world_x_velocity)
1054{
1055 set_field(data->world_x_velocity, new_world_x_velocity);
1056}
1057
1058/** Get world_y_velocity value.
1059 *
1060 Velocity of object in the world coordinate system in Y-direction in meter per second.
1061
1062 * @return world_y_velocity value
1063 */
1064float
1065ObjectPositionInterface::world_y_velocity() const
1066{
1067 return data->world_y_velocity;
1068}
1069
1070/** Get maximum length of world_y_velocity value.
1071 * @return length of world_y_velocity value, can be length of the array or number of
1072 * maximum number of characters for a string
1073 */
1074size_t
1075ObjectPositionInterface::maxlenof_world_y_velocity() const
1076{
1077 return 1;
1078}
1079
1080/** Set world_y_velocity value.
1081 *
1082 Velocity of object in the world coordinate system in Y-direction in meter per second.
1083
1084 * @param new_world_y_velocity new world_y_velocity value
1085 */
1086void
1087ObjectPositionInterface::set_world_y_velocity(const float new_world_y_velocity)
1088{
1089 set_field(data->world_y_velocity, new_world_y_velocity);
1090}
1091
1092/** Get world_z_velocity value.
1093 *
1094 Velocity of object in the world coordinate system in Z-direction in meter per second.
1095
1096 * @return world_z_velocity value
1097 */
1098float
1099ObjectPositionInterface::world_z_velocity() const
1100{
1101 return data->world_z_velocity;
1102}
1103
1104/** Get maximum length of world_z_velocity value.
1105 * @return length of world_z_velocity value, can be length of the array or number of
1106 * maximum number of characters for a string
1107 */
1108size_t
1109ObjectPositionInterface::maxlenof_world_z_velocity() const
1110{
1111 return 1;
1112}
1113
1114/** Set world_z_velocity value.
1115 *
1116 Velocity of object in the world coordinate system in Z-direction in meter per second.
1117
1118 * @param new_world_z_velocity new world_z_velocity value
1119 */
1120void
1121ObjectPositionInterface::set_world_z_velocity(const float new_world_z_velocity)
1122{
1123 set_field(data->world_z_velocity, new_world_z_velocity);
1124}
1125
1126/** Get world_xyz_velocity_covariance value.
1127 *
1128 Covariance of WorldX/WorldY/WorldZ velocity values. This is a 3x3 matrix ordered line
1129 by line, first three values represent row, next tree values second row and last three
1130 values last row from left to right each.
1131
1132 * @return world_xyz_velocity_covariance value
1133 */
1134float *
1135ObjectPositionInterface::world_xyz_velocity_covariance() const
1136{
1137 return data->world_xyz_velocity_covariance;
1138}
1139
1140/** Get world_xyz_velocity_covariance value at given index.
1141 *
1142 Covariance of WorldX/WorldY/WorldZ velocity values. This is a 3x3 matrix ordered line
1143 by line, first three values represent row, next tree values second row and last three
1144 values last row from left to right each.
1145
1146 * @param index index of value
1147 * @return world_xyz_velocity_covariance value
1148 * @exception Exception thrown if index is out of bounds
1149 */
1150float
1151ObjectPositionInterface::world_xyz_velocity_covariance(unsigned int index) const
1152{
1153 if (index > 8) {
1154 throw Exception("Index value %u out of bounds (0..8)", index);
1155 }
1156 return data->world_xyz_velocity_covariance[index];
1157}
1158
1159/** Get maximum length of world_xyz_velocity_covariance value.
1160 * @return length of world_xyz_velocity_covariance value, can be length of the array or number of
1161 * maximum number of characters for a string
1162 */
1163size_t
1164ObjectPositionInterface::maxlenof_world_xyz_velocity_covariance() const
1165{
1166 return 9;
1167}
1168
1169/** Set world_xyz_velocity_covariance value.
1170 *
1171 Covariance of WorldX/WorldY/WorldZ velocity values. This is a 3x3 matrix ordered line
1172 by line, first three values represent row, next tree values second row and last three
1173 values last row from left to right each.
1174
1175 * @param new_world_xyz_velocity_covariance new world_xyz_velocity_covariance value
1176 */
1177void
1178ObjectPositionInterface::set_world_xyz_velocity_covariance(const float * new_world_xyz_velocity_covariance)
1179{
1180 set_field(data->world_xyz_velocity_covariance, new_world_xyz_velocity_covariance);
1181}
1182
1183/** Set world_xyz_velocity_covariance value at given index.
1184 *
1185 Covariance of WorldX/WorldY/WorldZ velocity values. This is a 3x3 matrix ordered line
1186 by line, first three values represent row, next tree values second row and last three
1187 values last row from left to right each.
1188
1189 * @param new_world_xyz_velocity_covariance new world_xyz_velocity_covariance value
1190 * @param index index for of the value
1191 */
1192void
1193ObjectPositionInterface::set_world_xyz_velocity_covariance(unsigned int index, const float new_world_xyz_velocity_covariance)
1194{
1195 set_field(data->world_xyz_velocity_covariance, index, new_world_xyz_velocity_covariance);
1196}
1197/** Get relative_x_velocity value.
1198 *
1199 Velocity of object in the world coordinate system in X-direction in meter per second.
1200
1201 * @return relative_x_velocity value
1202 */
1203float
1204ObjectPositionInterface::relative_x_velocity() const
1205{
1206 return data->relative_x_velocity;
1207}
1208
1209/** Get maximum length of relative_x_velocity value.
1210 * @return length of relative_x_velocity value, can be length of the array or number of
1211 * maximum number of characters for a string
1212 */
1213size_t
1214ObjectPositionInterface::maxlenof_relative_x_velocity() const
1215{
1216 return 1;
1217}
1218
1219/** Set relative_x_velocity value.
1220 *
1221 Velocity of object in the world coordinate system in X-direction in meter per second.
1222
1223 * @param new_relative_x_velocity new relative_x_velocity value
1224 */
1225void
1226ObjectPositionInterface::set_relative_x_velocity(const float new_relative_x_velocity)
1227{
1228 set_field(data->relative_x_velocity, new_relative_x_velocity);
1229}
1230
1231/** Get relative_y_velocity value.
1232 *
1233 Velocity of object in the world coordinate system in Y-direction in meter per second.
1234
1235 * @return relative_y_velocity value
1236 */
1237float
1238ObjectPositionInterface::relative_y_velocity() const
1239{
1240 return data->relative_y_velocity;
1241}
1242
1243/** Get maximum length of relative_y_velocity value.
1244 * @return length of relative_y_velocity value, can be length of the array or number of
1245 * maximum number of characters for a string
1246 */
1247size_t
1248ObjectPositionInterface::maxlenof_relative_y_velocity() const
1249{
1250 return 1;
1251}
1252
1253/** Set relative_y_velocity value.
1254 *
1255 Velocity of object in the world coordinate system in Y-direction in meter per second.
1256
1257 * @param new_relative_y_velocity new relative_y_velocity value
1258 */
1259void
1260ObjectPositionInterface::set_relative_y_velocity(const float new_relative_y_velocity)
1261{
1262 set_field(data->relative_y_velocity, new_relative_y_velocity);
1263}
1264
1265/** Get relative_z_velocity value.
1266 *
1267 Velocity of object in the world coordinate system in Z-direction in meter per second.
1268
1269 * @return relative_z_velocity value
1270 */
1271float
1272ObjectPositionInterface::relative_z_velocity() const
1273{
1274 return data->relative_z_velocity;
1275}
1276
1277/** Get maximum length of relative_z_velocity value.
1278 * @return length of relative_z_velocity value, can be length of the array or number of
1279 * maximum number of characters for a string
1280 */
1281size_t
1282ObjectPositionInterface::maxlenof_relative_z_velocity() const
1283{
1284 return 1;
1285}
1286
1287/** Set relative_z_velocity value.
1288 *
1289 Velocity of object in the world coordinate system in Z-direction in meter per second.
1290
1291 * @param new_relative_z_velocity new relative_z_velocity value
1292 */
1293void
1294ObjectPositionInterface::set_relative_z_velocity(const float new_relative_z_velocity)
1295{
1296 set_field(data->relative_z_velocity, new_relative_z_velocity);
1297}
1298
1299/** Get relative_xyz_velocity_covariance value.
1300 *
1301 Covariance of relative x/y/z velocity values. This is a 3x3 matrix ordered line
1302 by line, first three values represent row, next tree values second row and last three
1303 values last row from left to right each.
1304
1305 * @return relative_xyz_velocity_covariance value
1306 */
1307float *
1308ObjectPositionInterface::relative_xyz_velocity_covariance() const
1309{
1310 return data->relative_xyz_velocity_covariance;
1311}
1312
1313/** Get relative_xyz_velocity_covariance value at given index.
1314 *
1315 Covariance of relative x/y/z velocity values. This is a 3x3 matrix ordered line
1316 by line, first three values represent row, next tree values second row and last three
1317 values last row from left to right each.
1318
1319 * @param index index of value
1320 * @return relative_xyz_velocity_covariance value
1321 * @exception Exception thrown if index is out of bounds
1322 */
1323float
1324ObjectPositionInterface::relative_xyz_velocity_covariance(unsigned int index) const
1325{
1326 if (index > 8) {
1327 throw Exception("Index value %u out of bounds (0..8)", index);
1328 }
1329 return data->relative_xyz_velocity_covariance[index];
1330}
1331
1332/** Get maximum length of relative_xyz_velocity_covariance value.
1333 * @return length of relative_xyz_velocity_covariance value, can be length of the array or number of
1334 * maximum number of characters for a string
1335 */
1336size_t
1337ObjectPositionInterface::maxlenof_relative_xyz_velocity_covariance() const
1338{
1339 return 9;
1340}
1341
1342/** Set relative_xyz_velocity_covariance value.
1343 *
1344 Covariance of relative x/y/z velocity values. This is a 3x3 matrix ordered line
1345 by line, first three values represent row, next tree values second row and last three
1346 values last row from left to right each.
1347
1348 * @param new_relative_xyz_velocity_covariance new relative_xyz_velocity_covariance value
1349 */
1350void
1351ObjectPositionInterface::set_relative_xyz_velocity_covariance(const float * new_relative_xyz_velocity_covariance)
1352{
1353 set_field(data->relative_xyz_velocity_covariance, new_relative_xyz_velocity_covariance);
1354}
1355
1356/** Set relative_xyz_velocity_covariance value at given index.
1357 *
1358 Covariance of relative x/y/z velocity values. This is a 3x3 matrix ordered line
1359 by line, first three values represent row, next tree values second row and last three
1360 values last row from left to right each.
1361
1362 * @param new_relative_xyz_velocity_covariance new relative_xyz_velocity_covariance value
1363 * @param index index for of the value
1364 */
1365void
1366ObjectPositionInterface::set_relative_xyz_velocity_covariance(unsigned int index, const float new_relative_xyz_velocity_covariance)
1367{
1368 set_field(data->relative_xyz_velocity_covariance, index, new_relative_xyz_velocity_covariance);
1369}
1370/* =========== message create =========== */
1371Message *
1372ObjectPositionInterface::create_message(const char *type) const
1373{
1374 throw UnknownTypeException("The given type '%s' does not match any known "
1375 "message type for this interface type.", type);
1376}
1377
1378
1379/** Copy values from other interface.
1380 * @param other other interface to copy values from
1381 */
1382void
1383ObjectPositionInterface::copy_values(const Interface *other)
1384{
1385 const ObjectPositionInterface *oi = dynamic_cast<const ObjectPositionInterface *>(other);
1386 if (oi == NULL) {
1387 throw TypeMismatchException("Can only copy values from interface of same type (%s vs. %s)",
1388 type(), other->type());
1389 }
1390 memcpy(data, oi->data, sizeof(ObjectPositionInterface_data_t));
1391}
1392
1393const char *
1394ObjectPositionInterface::enum_tostring(const char *enumtype, int val) const
1395{
1396 throw UnknownTypeException("Unknown enum type %s", enumtype);
1397}
1398
1399/* =========== messages =========== */
1400/** Check if message is valid and can be enqueued.
1401 * @param message Message to check
1402 * @return true if the message is valid, false otherwise.
1403 */
1404bool
1405ObjectPositionInterface::message_valid(const Message *message) const
1406{
1407 return false;
1408}
1409
1410/// @cond INTERNALS
1411EXPORT_INTERFACE(ObjectPositionInterface)
1412/// @endcond
1413
1414
1415} // 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
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
ObjectPositionInterface Fawkes BlackBoard Interface.
static const uint32_t FLAG_HAS_WORLD_VELOCITY
FLAG_HAS_WORLD_VELOCITY constant.
static const uint32_t FLAG_HAS_EULER_ANGLES
FLAG_HAS_EULER_ANGLES constant.
static const uint32_t TYPE_GOAL_BLUE
TYPE_GOAL_BLUE constant.
static const uint32_t TYPE_SELF
TYPE_SELF constant.
static const uint32_t TYPE_LINE
TYPE_LINE constant.
static const uint32_t TYPE_GOAL_YELLOW
TYPE_GOAL_YELLOW constant.
static const uint32_t FLAG_HAS_WORLD
FLAG_HAS_WORLD constant.
static const uint32_t TYPE_OTHER
TYPE_OTHER constant.
static const uint32_t FLAG_HAS_RELATIVE_CARTESIAN
FLAG_HAS_RELATIVE_CARTESIAN constant.
static const uint32_t TYPE_BALL
TYPE_BALL constant.
static const uint32_t FLAG_HAS_Z_AS_ORI
FLAG_HAS_Z_AS_ORI constant.
static const uint32_t TYPE_TEAMMEMBER
TYPE_TEAMMEMBER constant.
static const uint32_t FLAG_HAS_EXTENT
FLAG_HAS_EXTENT constant.
static const uint32_t FLAG_HAS_COVARIANCES
FLAG_HAS_COVARIANCES constant.
static const uint32_t TYPE_OPPONENT
TYPE_OPPONENT constant.
static const uint32_t FLAG_HAS_VOLUME_EXTENT
FLAG_HAS_VOLUME_EXTENT constant.
static const uint32_t FLAG_IS_FIXED_OBJECT
FLAG_IS_FIXED_OBJECT constant.
static const uint32_t FLAG_NONE
FLAG_NONE constant.
static const uint32_t FLAG_HAS_RELATIVE_POLAR
FLAG_HAS_RELATIVE_POLAR constant.
static const uint32_t FLAG_HAS_CIRCULAR_EXTENT
FLAG_HAS_CIRCULAR_EXTENT constant.
Fawkes library namespace.