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