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 
33 namespace 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 */
49 const uint32_t ObjectPositionInterface::TYPE_OTHER = 0u;
50 /** TYPE_BALL constant */
51 const uint32_t ObjectPositionInterface::TYPE_BALL = 1u;
52 /** TYPE_OPPONENT constant */
53 const uint32_t ObjectPositionInterface::TYPE_OPPONENT = 2u;
54 /** TYPE_TEAMMEMBER constant */
56 /** TYPE_LINE constant */
57 const uint32_t ObjectPositionInterface::TYPE_LINE = 4u;
58 /** TYPE_SELF constant */
59 const uint32_t ObjectPositionInterface::TYPE_SELF = 5u;
60 /** TYPE_GOAL_BLUE constant */
61 const uint32_t ObjectPositionInterface::TYPE_GOAL_BLUE = 6u;
62 /** TYPE_GOAL_YELLOW constant */
64 /** FLAG_NONE constant */
65 const uint32_t ObjectPositionInterface::FLAG_NONE = 0u;
66 /** FLAG_HAS_WORLD constant */
67 const uint32_t ObjectPositionInterface::FLAG_HAS_WORLD = 1u;
68 /** FLAG_HAS_RELATIVE_CARTESIAN constant */
70 /** FLAG_HAS_RELATIVE_POLAR constant */
72 /** FLAG_HAS_EULER_ANGLES constant */
74 /** FLAG_HAS_EXTENT constant */
75 const uint32_t ObjectPositionInterface::FLAG_HAS_EXTENT = 16u;
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 */
85 const uint32_t ObjectPositionInterface::FLAG_HAS_Z_AS_ORI = 512u;
86 /** FLAG_IS_FIXED_OBJECT constant */
88 
89 /** Constructor */
90 ObjectPositionInterface::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 */
132 ObjectPositionInterface::~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  */
143 uint32_t
144 ObjectPositionInterface::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  */
153 size_t
154 ObjectPositionInterface::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  */
165 void
166 ObjectPositionInterface::set_object_type(const uint32_t new_object_type)
167 {
168  data_changed |= change_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  */
179 uint32_t
180 ObjectPositionInterface::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  */
189 size_t
190 ObjectPositionInterface::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  */
203 void
204 ObjectPositionInterface::set_flags(const uint32_t new_flags)
205 {
206  data_changed |= change_field(data->flags, new_flags);
207 }
208 
209 /** Get visible value.
210  * True, if object is visible.
211  * @return visible value
212  */
213 bool
214 ObjectPositionInterface::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  */
223 size_t
224 ObjectPositionInterface::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  */
233 void
234 ObjectPositionInterface::set_visible(const bool new_visible)
235 {
236  data_changed |= change_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  */
249 int32_t
250 ObjectPositionInterface::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  */
259 size_t
260 ObjectPositionInterface::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  */
275 void
276 ObjectPositionInterface::set_visibility_history(const int32_t new_visibility_history)
277 {
278  data_changed |= change_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  */
287 float
288 ObjectPositionInterface::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  */
297 size_t
298 ObjectPositionInterface::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  */
309 void
310 ObjectPositionInterface::set_roll(const float new_roll)
311 {
312  data_changed |= change_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  */
321 float
322 ObjectPositionInterface::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  */
331 size_t
332 ObjectPositionInterface::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  */
343 void
344 ObjectPositionInterface::set_pitch(const float new_pitch)
345 {
346  data_changed |= change_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  */
355 float
356 ObjectPositionInterface::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  */
365 size_t
366 ObjectPositionInterface::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  */
377 void
378 ObjectPositionInterface::set_yaw(const float new_yaw)
379 {
380  data_changed |= change_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  */
390 float
391 ObjectPositionInterface::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  */
400 size_t
401 ObjectPositionInterface::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  */
413 void
414 ObjectPositionInterface::set_distance(const float new_distance)
415 {
416  data_changed |= change_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  */
426 float
427 ObjectPositionInterface::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  */
436 size_t
437 ObjectPositionInterface::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  */
449 void
450 ObjectPositionInterface::set_bearing(const float new_bearing)
451 {
452  data_changed |= change_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  */
463 float
464 ObjectPositionInterface::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  */
473 size_t
474 ObjectPositionInterface::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  */
487 void
488 ObjectPositionInterface::set_slope(const float new_slope)
489 {
490  data_changed |= change_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  */
501 float *
502 ObjectPositionInterface::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  */
517 float
518 ObjectPositionInterface::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  */
530 size_t
531 ObjectPositionInterface::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  */
544 void
545 ObjectPositionInterface::set_dbs_covariance(const float * new_dbs_covariance)
546 {
547  data_changed |= change_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  */
559 void
560 ObjectPositionInterface::set_dbs_covariance(unsigned int index, const float new_dbs_covariance)
561 {
562  data_changed |= change_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  */
572 float
573 ObjectPositionInterface::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  */
582 size_t
583 ObjectPositionInterface::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  */
596 void
597 ObjectPositionInterface::set_world_x(const float new_world_x)
598 {
599  data_changed |= change_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  */
610 float
611 ObjectPositionInterface::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  */
620 size_t
621 ObjectPositionInterface::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  */
634 void
635 ObjectPositionInterface::set_world_y(const float new_world_y)
636 {
637  data_changed |= change_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  */
648 float
649 ObjectPositionInterface::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  */
658 size_t
659 ObjectPositionInterface::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  */
672 void
673 ObjectPositionInterface::set_world_z(const float new_world_z)
674 {
675  data_changed |= change_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  */
686 float *
687 ObjectPositionInterface::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  */
702 float
703 ObjectPositionInterface::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  */
715 size_t
716 ObjectPositionInterface::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  */
729 void
730 ObjectPositionInterface::set_world_xyz_covariance(const float * new_world_xyz_covariance)
731 {
732  data_changed |= change_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  */
744 void
745 ObjectPositionInterface::set_world_xyz_covariance(unsigned int index, const float new_world_xyz_covariance)
746 {
747  data_changed |= change_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  */
755 float
756 ObjectPositionInterface::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  */
765 size_t
766 ObjectPositionInterface::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  */
777 void
778 ObjectPositionInterface::set_relative_x(const float new_relative_x)
779 {
780  data_changed |= change_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  */
789 float
790 ObjectPositionInterface::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  */
799 size_t
800 ObjectPositionInterface::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  */
811 void
812 ObjectPositionInterface::set_relative_y(const float new_relative_y)
813 {
814  data_changed |= change_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  */
823 float
824 ObjectPositionInterface::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  */
833 size_t
834 ObjectPositionInterface::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  */
845 void
846 ObjectPositionInterface::set_relative_z(const float new_relative_z)
847 {
848  data_changed |= change_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  */
859 float *
860 ObjectPositionInterface::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  */
875 float
876 ObjectPositionInterface::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  */
888 size_t
889 ObjectPositionInterface::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  */
902 void
903 ObjectPositionInterface::set_relative_xyz_covariance(const float * new_relative_xyz_covariance)
904 {
905  data_changed |= change_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  */
917 void
918 ObjectPositionInterface::set_relative_xyz_covariance(unsigned int index, const float new_relative_xyz_covariance)
919 {
920  data_changed |= change_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  */
928 float
929 ObjectPositionInterface::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  */
938 size_t
939 ObjectPositionInterface::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  */
950 void
951 ObjectPositionInterface::set_extent_x(const float new_extent_x)
952 {
953  data_changed |= change_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  */
962 float
963 ObjectPositionInterface::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  */
972 size_t
973 ObjectPositionInterface::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  */
984 void
985 ObjectPositionInterface::set_extent_y(const float new_extent_y)
986 {
987  data_changed |= change_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  */
996 float
997 ObjectPositionInterface::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  */
1006 size_t
1007 ObjectPositionInterface::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  */
1018 void
1019 ObjectPositionInterface::set_extent_z(const float new_extent_z)
1020 {
1021  data_changed |= change_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  */
1030 float
1031 ObjectPositionInterface::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  */
1040 size_t
1041 ObjectPositionInterface::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  */
1052 void
1053 ObjectPositionInterface::set_world_x_velocity(const float new_world_x_velocity)
1054 {
1055  data_changed |= change_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  */
1064 float
1065 ObjectPositionInterface::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  */
1074 size_t
1075 ObjectPositionInterface::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  */
1086 void
1087 ObjectPositionInterface::set_world_y_velocity(const float new_world_y_velocity)
1088 {
1089  data_changed |= change_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  */
1098 float
1099 ObjectPositionInterface::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  */
1108 size_t
1109 ObjectPositionInterface::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  */
1120 void
1121 ObjectPositionInterface::set_world_z_velocity(const float new_world_z_velocity)
1122 {
1123  data_changed |= change_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  */
1134 float *
1135 ObjectPositionInterface::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  */
1150 float
1151 ObjectPositionInterface::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  */
1163 size_t
1164 ObjectPositionInterface::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  */
1177 void
1178 ObjectPositionInterface::set_world_xyz_velocity_covariance(const float * new_world_xyz_velocity_covariance)
1179 {
1180  data_changed |= change_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  */
1192 void
1193 ObjectPositionInterface::set_world_xyz_velocity_covariance(unsigned int index, const float new_world_xyz_velocity_covariance)
1194 {
1195  data_changed |= change_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  */
1203 float
1204 ObjectPositionInterface::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  */
1213 size_t
1214 ObjectPositionInterface::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  */
1225 void
1226 ObjectPositionInterface::set_relative_x_velocity(const float new_relative_x_velocity)
1227 {
1228  data_changed |= change_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  */
1237 float
1238 ObjectPositionInterface::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  */
1247 size_t
1248 ObjectPositionInterface::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  */
1259 void
1260 ObjectPositionInterface::set_relative_y_velocity(const float new_relative_y_velocity)
1261 {
1262  data_changed |= change_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  */
1271 float
1272 ObjectPositionInterface::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  */
1281 size_t
1282 ObjectPositionInterface::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  */
1293 void
1294 ObjectPositionInterface::set_relative_z_velocity(const float new_relative_z_velocity)
1295 {
1296  data_changed |= change_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  */
1307 float *
1308 ObjectPositionInterface::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  */
1323 float
1324 ObjectPositionInterface::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  */
1336 size_t
1337 ObjectPositionInterface::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  */
1350 void
1351 ObjectPositionInterface::set_relative_xyz_velocity_covariance(const float * new_relative_xyz_velocity_covariance)
1352 {
1353  data_changed |= change_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  */
1365 void
1366 ObjectPositionInterface::set_relative_xyz_velocity_covariance(unsigned int index, const float new_relative_xyz_velocity_covariance)
1367 {
1368  data_changed |= change_field(data->relative_xyz_velocity_covariance, index, new_relative_xyz_velocity_covariance);
1369 }
1370 /* =========== message create =========== */
1371 Message *
1372 ObjectPositionInterface::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  */
1382 void
1383 ObjectPositionInterface::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 
1393 const char *
1394 ObjectPositionInterface::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  */
1404 bool
1405 ObjectPositionInterface::message_valid(const Message *message) const
1406 {
1407  return false;
1408 }
1409 
1410 /// @cond INTERNALS
1411 EXPORT_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:79
const char * type() const
Get type of interface.
Definition: interface.cpp:643
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:45
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.
bool change_field(FieldT &field, const DataT &value)
Set a field and return whether it changed.
Definition: message.h:167