00001
00002
00003
00004
00005
00006
00007
00008 #ifndef IBEO_CORE_IBEO_CORE_H
00009 #define IBEO_CORE_IBEO_CORE_H
00010
00011 #include <vector>
00012 #include <cstdio>
00013 #include <iostream>
00014 #include <memory>
00015 #include <cmath>
00016
00017 #include <sys/time.h>
00018
00019 #include <ibeo_core/utils.h>
00020 #include <network_interface/common.h>
00021
00022 using AS::Network::ByteOrder;
00023
00024 namespace AS
00025 {
00026 namespace Drivers
00027 {
00028 namespace Ibeo
00029 {
00030 const uint8_t IBEO_HEADER_SIZE = 24;
00031 const int32_t IBEO_PAYLOAD_SIZE = 10000;
00032
00033 enum Classification
00034 {
00035 UNCLASSIFIED = 0,
00036 UNKNOWN_SMALL,
00037 UNKNOWN_BIG,
00038 PEDESTRIAN,
00039 BIKE,
00040 CAR,
00041 TRUCK
00042 };
00043
00044 enum MirrorSide
00045 {
00046 FRONT = 0,
00047 REAR
00048 };
00049
00050 enum CoordinateSystem
00051 {
00052 SCANNER = 0,
00053 VEHICLE
00054 };
00055
00056 enum MotorRotatingDirection
00057 {
00058 CLOCKWISE = 0,
00059 COUNTER_CLOCKWISE
00060 };
00061
00062 enum PointLocation
00063 {
00064 COG = 0,
00065 TOP_FRONT_LEFT_CORNER,
00066 TOP_FRONT_RIGHT_CORNER,
00067 BOTTOM_REAR_RIGHT_CORNER,
00068 BOTTOM_REAR_LEFT_CORNER,
00069 CENTER_OF_TOP_FRONT_EDGE,
00070 CENTER_OF_RIGHT_EDGE,
00071 CENTER_OF_BOTTOM_REAR_EDGE,
00072 CENTER_OF_LEFT_EDGE,
00073 BOX_CENTER,
00074 INVALID = 255
00075 };
00076
00077 enum TrackingModel
00078 {
00079 DYNAMIC = 0,
00080 STATIC
00081 };
00082
00083 enum ObjectPhase
00084 {
00085 INITIALIZATION = 0,
00086 TRACKING
00087 };
00088
00089 enum DynamicProperty
00090 {
00091 DYNAMIC_AND_MOVING = 0,
00092 DYNAMIC_AND_STOPPED,
00093 A_PRIORI_STATIONARY = 4
00094 };
00095
00096 enum ImageFormat
00097 {
00098 JPEG = 0,
00099 MJPEG,
00100 GRAY8,
00101 YUV420,
00102 YUV422
00103 };
00104
00105 struct Point3D
00106 {
00107 double x;
00108 double y;
00109 double z;
00110 };
00111
00112 struct Point3DL :
00113 public Point3D
00114 {
00115 uint32_t label;
00116 };
00117
00118 class MountingPositionF
00119 {
00120 public:
00121 float yaw_angle;
00122 float pitch_angle;
00123 float roll_angle;
00124 float x_position;
00125 float y_position;
00126 float z_position;
00127
00128 void parse(
00129 const std::vector<uint8_t>& in,
00130 const uint16_t& offset);
00131 };
00132
00133 class Point2Df
00134 {
00135 public:
00136 float x;
00137 float y;
00138
00139 void parse(
00140 const std::vector<uint8_t>& in,
00141 const uint16_t& offset,
00142 ByteOrder bo);
00143 };
00144
00145 class Point2Di
00146 {
00147 public:
00148 int16_t x;
00149 int16_t y;
00150
00151 void parse(
00152 const std::vector<uint8_t>& in,
00153 const uint16_t& offset,
00154 ByteOrder bo);
00155 };
00156
00157 class Point2Dui
00158 {
00159 public:
00160 uint16_t x;
00161 uint16_t y;
00162
00163 void parse(
00164 const std::vector<uint8_t>& in,
00165 const uint16_t& offset,
00166 ByteOrder bo);
00167 };
00168
00169 class Sigma2D
00170 {
00171 public:
00172 uint16_t sigma_x;
00173 uint16_t sigma_y;
00174
00175 void parse(
00176 const std::vector<uint8_t>& in,
00177 const uint16_t& offset,
00178 ByteOrder bo);
00179 };
00180
00181 class Size2D
00182 {
00183 public:
00184 uint16_t size_x;
00185 uint16_t size_y;
00186
00187 void parse(
00188 const std::vector<uint8_t>& in,
00189 const uint16_t& offset,
00190 ByteOrder bo);
00191 };
00192
00193 class Size2Df
00194 {
00195 public:
00196 float size_x;
00197 float size_y;
00198 };
00199
00200 class Velocity2D
00201 {
00202 public:
00203 int16_t velocity_x;
00204 int16_t velocity_y;
00205
00206 void parse(
00207 const std::vector<uint8_t>& in,
00208 const uint16_t& offset,
00209 ByteOrder bo);
00210 };
00211
00212 class ContourPointSigma : public Point2Di
00213 {
00214 public:
00215 uint8_t x_sigma;
00216 uint8_t y_sigma;
00217
00218 void parse(
00219 const std::vector<uint8_t>& in,
00220 const uint16_t& offset,
00221 ByteOrder bo);
00222 };
00223
00224 class ResolutionInfo
00225 {
00226 public:
00227 float resolution_start_angle;
00228 float resolution;
00229
00230 void parse(
00231 const std::vector<uint8_t>& in,
00232 const uint16_t& offset);
00233 };
00234
00235 class ScannerInfo2204
00236 {
00237 public:
00238 uint8_t device_id;
00239 uint8_t scanner_type;
00240 uint16_t scan_number;
00241 float start_angle;
00242 float end_angle;
00243 float yaw_angle;
00244 float pitch_angle;
00245 float roll_angle;
00246 float offset_x;
00247 float offset_y;
00248 float offset_z;
00249
00250 void parse(
00251 const std::vector<uint8_t>& in,
00252 const uint16_t& offset);
00253 };
00254
00255 class ScannerInfo2205
00256 {
00257 public:
00258 uint8_t device_id;
00259 uint8_t scanner_type;
00260 uint16_t scan_number;
00261 float start_angle;
00262 float end_angle;
00263 NTPTime scan_start_time;
00264 NTPTime scan_end_time;
00265 NTPTime scan_start_time_from_device;
00266 NTPTime scan_end_time_from_device;
00267 float scan_frequency;
00268 float beam_tilt;
00269 uint32_t scan_flags;
00270 MountingPositionF mounting_position;
00271 ResolutionInfo resolutions[8];
00272
00273 void parse(
00274 const std::vector<uint8_t>& in,
00275 const uint16_t& offset);
00276 };
00277
00278 class UntrackedProperties
00279 {
00280 public:
00281 uint16_t relative_time_of_measurement;
00282 Point2Di position_closest_point;
00283 Point2Di object_box_size;
00284 Point2Dui object_box_size_sigma;
00285 int16_t object_box_orientation;
00286 uint16_t object_box_orientation_sigma;
00287 Point2Di tracking_point_coordinate;
00288 Point2Dui tracking_point_coordinate_sigma;
00289 uint8_t number_of_contour_points;
00290 std::vector<ContourPointSigma> contour_point_list;
00291
00292 void parse(
00293 const std::vector<uint8_t>& in,
00294 const uint16_t& offset);
00295 };
00296
00297 class TrackedProperties
00298 {
00299 public:
00300 uint16_t object_age;
00301 uint16_t hidden_status_age;
00302 ObjectPhase object_phase;
00303 DynamicProperty dynamic_property;
00304 uint16_t relative_time_of_measure;
00305 Point2Di position_closest_point;
00306 Point2Di relative_velocity;
00307 Point2Dui relative_velocity_sigma;
00308 Classification classification;
00309 uint16_t classification_age;
00310 Point2Di object_box_size;
00311 Point2Dui object_box_size_sigma;
00312 int16_t object_box_orientation;
00313 uint16_t object_box_orientation_sigma;
00314 PointLocation tracking_point_location;
00315 Point2Di tracking_point_coordinate;
00316 Point2Dui tracking_point_coordinate_sigma;
00317 Point2Di velocity;
00318 Point2Dui velocity_sigma;
00319 Point2Di acceleration;
00320 Point2Dui acceleration_sigma;
00321 int16_t yaw_rate;
00322 uint16_t yaw_rate_sigma;
00323 uint8_t number_of_contour_points;
00324 std::vector<ContourPointSigma> contour_point_list;
00325
00326 void parse(
00327 const std::vector<uint8_t>& in,
00328 const uint16_t& offset);
00329 };
00330
00331 class ScanPoint2202
00332 {
00333 public:
00334 uint8_t layer;
00335 uint8_t echo;
00336 bool transparent_point;
00337 bool clutter_atmospheric;
00338 bool ground;
00339 bool dirt;
00340 int16_t horizontal_angle;
00341 uint16_t radial_distance;
00342 uint16_t echo_pulse_width;
00343
00344 void parse(
00345 const std::vector<uint8_t>& in,
00346 const uint16_t& offset);
00347 };
00348
00349 class ScanPoint2204
00350 {
00351 public:
00352 float x_position;
00353 float y_position;
00354 float z_position;
00355 float echo_width;
00356 uint8_t device_id;
00357 uint8_t layer;
00358 uint8_t echo;
00359 uint32_t time_offset;
00360 bool ground;
00361 bool dirt;
00362 bool precipitation;
00363
00364 void parse(
00365 const std::vector<uint8_t>& in,
00366 const uint16_t& offset);
00367 };
00368
00369 class ScanPoint2205
00370 {
00371 public:
00372 float x_position;
00373 float y_position;
00374 float z_position;
00375 float echo_width;
00376 uint8_t device_id;
00377 uint8_t layer;
00378 uint8_t echo;
00379 uint32_t time_offset;
00380 bool ground;
00381 bool dirt;
00382 bool precipitation;
00383 bool transparent;
00384
00385 void parse(
00386 const std::vector<uint8_t>& in,
00387 const uint16_t& offset);
00388 };
00389
00390 class ScanPoint2208
00391 {
00392 public:
00393 uint8_t echo;
00394 uint8_t layer;
00395 bool transparent_point;
00396 bool clutter_atmospheric;
00397 bool ground;
00398 bool dirt;
00399 int16_t horizontal_angle;
00400 uint16_t radial_distance;
00401 uint16_t echo_pulse_width;
00402
00403 void parse(
00404 const std::vector<uint8_t>& in,
00405 const uint16_t& offset);
00406 };
00407
00408 struct IbeoObject
00409 {
00410 uint16_t id;
00411 uint32_t age;
00412 uint16_t prediction_age;
00413 uint16_t relative_timestamp;
00414 CoordinateSystem object_coordinate_system;
00415 PointLocation reference_point_location;
00416 Point2Df reference_point;
00417 Point2Df reference_point_sigma;
00418 Point2Df closest_point;
00419 Point2Df bounding_box_center;
00420 Size2Df bounding_box_size;
00421 Point2Df object_box_center;
00422 Size2Df object_box_size;
00423 float object_box_orientation;
00424 Point2Df absolute_velocity;
00425 Point2Df absolute_velocity_sigma;
00426 Point2Df relative_velocity;
00427 uint16_t classification;
00428 uint16_t classification_age;
00429 uint16_t classification_certainty;
00430 uint16_t number_of_contour_points;
00431 std::vector<Point3D> contour_point_list;
00432 };
00433
00434 class Object2221
00435 {
00436 public:
00437 uint16_t id;
00438 uint16_t age;
00439 uint16_t prediction_age;
00440 uint16_t relative_timestamp;
00441 Point2Di reference_point;
00442 Point2Di reference_point_sigma;
00443 Point2Di closest_point;
00444 Point2Di bounding_box_center;
00445 uint16_t bounding_box_width;
00446 uint16_t bounding_box_length;
00447 Point2Di object_box_center;
00448 Size2D object_box_size;
00449 int16_t object_box_orientation;
00450 Point2Di absolute_velocity;
00451 Size2D absolute_velocity_sigma;
00452 Point2Di relative_velocity;
00453 Classification classification;
00454 uint16_t classification_age;
00455 uint16_t classification_certainty;
00456 uint16_t number_of_contour_points;
00457 std::vector<Point2Di> contour_point_list;
00458
00459 void parse(
00460 const std::vector<uint8_t>& in,
00461 const uint16_t& offset);
00462 };
00463
00464 class Object2225
00465 {
00466 public:
00467 uint16_t id;
00468 uint32_t age;
00469 NTPTime timestamp;
00470 uint16_t hidden_status_age;
00471 Classification classification;
00472 uint8_t classification_certainty;
00473 uint32_t classification_age;
00474 Point2Df bounding_box_center;
00475 Point2Df bounding_box_size;
00476 Point2Df object_box_center;
00477 Point2Df object_box_center_sigma;
00478 Point2Df object_box_size;
00479 float yaw_angle;
00480 Point2Df relative_velocity;
00481 Point2Df relative_velocity_sigma;
00482 Point2Df absolute_velocity;
00483 Point2Df absolute_velocity_sigma;
00484 uint8_t number_of_contour_points;
00485 uint8_t closest_point_index;
00486 std::vector<Point2Df> contour_point_list;
00487
00488 void parse(
00489 const std::vector<uint8_t>& in,
00490 const uint16_t& offset);
00491 };
00492
00493 class Object2270
00494 {
00495 public:
00496 uint16_t id;
00497 uint16_t age;
00498 uint16_t prediction_age;
00499 uint16_t relative_moment_of_measurement;
00500 PointLocation reference_point_location;
00501 int16_t reference_point_position_x;
00502 int16_t reference_point_position_y;
00503 uint16_t reference_point_position_sigma_x;
00504 uint16_t reference_point_position_sigma_y;
00505 int16_t contour_points_cog_x;
00506 int16_t contour_points_cog_y;
00507 uint16_t object_box_length;
00508 uint16_t object_box_width;
00509 int16_t object_box_orientation_angle;
00510 int16_t object_box_orientation_angle_sigma;
00511 int16_t absolute_velocity_x;
00512 int16_t absolute_velocity_y;
00513 uint16_t absolute_velocity_sigma_x;
00514 uint16_t absolute_velocity_sigma_y;
00515 int16_t relative_velocity_x;
00516 int16_t relative_velocity_y;
00517 uint16_t relative_velocity_sigma_x;
00518 uint16_t relative_velocity_sigma_y;
00519 Classification classification;
00520 TrackingModel tracking_model;
00521 bool mobile_detected;
00522 bool track_valid;
00523 uint16_t classification_age;
00524 uint16_t classification_confidence;
00525 uint16_t number_of_contour_points;
00526 std::vector<Point2Di> contour_point_list;
00527
00528 void parse(
00529 const std::vector<uint8_t>& in,
00530 const uint16_t& offset);
00531 };
00532
00533 class Object2271
00534 {
00535 public:
00536 uint32_t id;
00537 bool untracked_properties_available;
00538 bool tracked_properties_available;
00539 UntrackedProperties untracked_properties;
00540 TrackedProperties tracked_properties;
00541
00542 void parse(
00543 const std::vector<uint8_t>& in,
00544 const uint16_t& offset);
00545 };
00546
00547 class Object2280
00548 {
00549 public:
00550 uint16_t id;
00551 TrackingModel tracking_model;
00552 bool mobility_of_dyn_object_detected;
00553 bool motion_model_validated;
00554 uint32_t object_age;
00555 NTPTime timestamp;
00556 uint16_t object_prediction_age;
00557 Classification classification;
00558 uint8_t classification_certainty;
00559 uint32_t classification_age;
00560 Point2Df object_box_center;
00561 Point2Df object_box_center_sigma;
00562 Point2Df object_box_size;
00563 float object_box_orientation_angle;
00564 float object_box_orientation_angle_sigma;
00565 Point2Df relative_velocity;
00566 Point2Df relative_velocity_sigma;
00567 Point2Df absolute_velocity;
00568 Point2Df absolute_velocity_sigma;
00569 uint8_t number_of_contour_points;
00570 uint8_t closest_point_index;
00571 PointLocation reference_point_location;
00572 Point2Df reference_point_coordinate;
00573 Point2Df reference_point_coordinate_sigma;
00574 float reference_point_position_correction_coefficient;
00575 uint16_t object_priority;
00576 float object_existence_measurement;
00577 std::vector<Point2Df> contour_point_list;
00578
00579 void parse(
00580 const std::vector<uint8_t>& in,
00581 const uint16_t& offset);
00582 };
00583
00584 class IbeoDataHeader
00585 {
00586 public:
00587 uint32_t previous_message_size;
00588 uint32_t message_size;
00589 uint8_t device_id;
00590 uint16_t data_type_id;
00591 NTPTime time;
00592
00593 std::vector<uint8_t> encoded_data;
00594
00595 void parse(const std::vector<uint8_t>& in);
00596 void encode();
00597 };
00598
00599
00600 class IbeoTxMessage
00601 {
00602 public:
00603 bool has_scan_points;
00604 bool has_contour_points;
00605 bool has_objects;
00606 IbeoDataHeader ibeo_header;
00607 uint16_t data_type;
00608
00609 IbeoTxMessage();
00610 IbeoTxMessage(bool scan_points, bool contour_points, bool objects);
00611
00612 static std::shared_ptr<IbeoTxMessage> make_message(const uint16_t& data_type);
00613 virtual void parse(const std::vector<uint8_t>& in) = 0;
00614 virtual std::vector<Point3DL> get_scan_points();
00615 virtual std::vector<Point3D> get_contour_points();
00616 virtual std::vector<IbeoObject> get_objects();
00617 };
00618
00619 class ErrorWarning : public IbeoTxMessage
00620 {
00621 public:
00622 static const int32_t DATA_TYPE;
00623
00624 bool err_internal_error;
00625 bool err_motor_1_fault;
00626 bool err_buffer_error_xmt_incomplete;
00627 bool err_buffer_error_overflow;
00628 bool err_apd_over_temperature;
00629 bool err_apd_under_temperature;
00630 bool err_apd_temperature_sensor_defect;
00631 bool err_motor_2_fault;
00632 bool err_motor_3_fault;
00633 bool err_motor_4_fault;
00634 bool err_motor_5_fault;
00635 bool err_int_no_scan_data;
00636 bool err_int_communication_error;
00637 bool err_int_incorrect_scan_data;
00638 bool err_config_fpga_not_configurable;
00639 bool err_config_incorrect_config_data;
00640 bool err_config_contains_incorrect_params;
00641 bool err_timeout_data_processing;
00642 bool err_timeout_env_model_computation_reset;
00643 bool wrn_int_communication_error;
00644 bool wrn_low_temperature;
00645 bool wrn_high_temperature;
00646 bool wrn_int_motor_1;
00647 bool wrn_sync_error;
00648 bool wrn_laser_1_start_pulse_missing;
00649 bool wrn_laser_2_start_pulse_missing;
00650 bool wrn_can_interface_blocked;
00651 bool wrn_eth_interface_blocked;
00652 bool wrn_incorrect_can_data_rcvd;
00653 bool wrn_int_incorrect_scan_data;
00654 bool wrn_eth_unkwn_incomplete_data;
00655 bool wrn_incorrect_or_forbidden_cmd_rcvd;
00656 bool wrn_memory_access_failure;
00657 bool wrn_int_overflow;
00658 bool wrn_ego_motion_data_missing;
00659 bool wrn_incorrect_mounting_params;
00660 bool wrn_no_obj_comp_due_to_scan_freq;
00661
00662 void parse(const std::vector<uint8_t>& in);
00663 };
00664
00665 class ScanData2202 : public IbeoTxMessage
00666 {
00667 public:
00668 static const int32_t DATA_TYPE;
00669
00670 uint16_t scan_number;
00671 uint16_t scanner_status;
00672 uint16_t sync_phase_offset;
00673 NTPTime scan_start_time;
00674 NTPTime scan_end_time;
00675 uint16_t angle_ticks_per_rotation;
00676 int16_t start_angle_ticks;
00677 int16_t end_angle_ticks;
00678 uint16_t scan_points_count;
00679 int16_t mounting_yaw_angle_ticks;
00680 int16_t mounting_pitch_angle_ticks;
00681 int16_t mounting_roll_angle_ticks;
00682 int16_t mounting_position_x;
00683 int16_t mounting_position_y;
00684 int16_t mounting_position_z;
00685 bool ground_labeled;
00686 bool dirt_labeled;
00687 bool rain_labeled;
00688 MirrorSide mirror_side;
00689 std::vector<ScanPoint2202> scan_point_list;
00690
00691 ScanData2202();
00692
00693 void parse(const std::vector<uint8_t>& in);
00694 std::vector<Point3DL> get_scan_points();
00695 };
00696
00697 class ScanData2204 : public IbeoTxMessage
00698 {
00699 public:
00700 static const int32_t DATA_TYPE;
00701
00702 NTPTime scan_start_time;
00703 uint32_t scan_end_time_offset;
00704 bool ground_labeled;
00705 bool dirt_labeled;
00706 bool rain_labeled;
00707 bool fused_scan;
00708 MirrorSide mirror_side;
00709 CoordinateSystem coordinate_system;
00710 uint16_t scan_number;
00711 uint16_t scan_points;
00712 uint16_t number_of_scanner_infos;
00713 std::vector<ScannerInfo2204> scanner_info_list;
00714 std::vector<ScanPoint2204> scan_point_list;
00715
00716 ScanData2204();
00717
00718 void parse(const std::vector<uint8_t>& in);
00719 std::vector<Point3DL> get_scan_points();
00720 };
00721
00722 class ScanData2205 : public IbeoTxMessage
00723 {
00724 public:
00725 static const int32_t DATA_TYPE;
00726
00727 NTPTime scan_start_time;
00728 uint32_t scan_end_time_offset;
00729 bool fused_scan;
00730 MirrorSide mirror_side;
00731 CoordinateSystem coordinate_system;
00732 uint16_t scan_number;
00733 uint16_t scan_points;
00734 uint8_t number_of_scanner_infos;
00735 std::vector<ScannerInfo2205> scanner_info_list;
00736 std::vector<ScanPoint2205> scan_point_list;
00737
00738 ScanData2205();
00739
00740 void parse(const std::vector<uint8_t>& in);
00741 std::vector<Point3DL> get_scan_points();
00742 };
00743
00744 class ScanData2208 : public IbeoTxMessage
00745 {
00746 public:
00747 static const int32_t DATA_TYPE;
00748
00749 uint16_t scan_number;
00750 uint16_t scanner_type;
00751 bool motor_on;
00752 bool laser_on;
00753 bool frequency_locked;
00754 MotorRotatingDirection motor_rotating_direction;
00755 uint16_t angle_ticks_per_rotation;
00756 uint32_t scan_flags;
00757 int16_t mounting_yaw_angle_ticks;
00758 int16_t mounting_pitch_angle_ticks;
00759 int16_t mounting_roll_angle_ticks;
00760 int16_t mounting_position_x;
00761 int16_t mounting_position_y;
00762 int16_t mounting_position_z;
00763 uint8_t device_id;
00764 NTPTime scan_start_time;
00765 NTPTime scan_end_time;
00766 int16_t start_angle_ticks;
00767 int16_t end_angle_ticks;
00768 uint8_t subflags;
00769 MirrorSide mirror_side;
00770 int16_t mirror_tilt;
00771 uint16_t number_of_scan_points;
00772 std::vector<ScanPoint2208> scan_point_list;
00773
00774 ScanData2208();
00775
00776 void parse(const std::vector<uint8_t>& in);
00777 std::vector<Point3DL> get_scan_points();
00778 };
00779
00780 class ObjectData2221 : public IbeoTxMessage
00781 {
00782 public:
00783 static const int32_t DATA_TYPE;
00784
00785 NTPTime scan_start_timestamp;
00786 uint16_t number_of_objects;
00787 std::vector<Object2221> object_list;
00788
00789 ObjectData2221();
00790
00791 void parse(const std::vector<uint8_t>& in);
00792 std::vector<Point3D> get_contour_points();
00793 std::vector<IbeoObject> get_objects();
00794 };
00795
00796 class ObjectData2225 : public IbeoTxMessage
00797 {
00798 public:
00799 static const int32_t DATA_TYPE;
00800
00801 NTPTime mid_scan_timestamp;
00802 uint16_t number_of_objects;
00803 std::vector<Object2225> object_list;
00804
00805 ObjectData2225();
00806
00807 void parse(const std::vector<uint8_t>& in);
00808 std::vector<Point3D> get_contour_points();
00809 std::vector<IbeoObject> get_objects();
00810 };
00811
00812 class ObjectData2270 : public IbeoTxMessage
00813 {
00814 public:
00815 static const int32_t DATA_TYPE;
00816
00817 NTPTime start_scan_timestamp;
00818 uint16_t object_list_number;
00819 uint16_t number_of_objects;
00820 std::vector<Object2270> object_list;
00821
00822 ObjectData2270();
00823
00824 void parse(const std::vector<uint8_t>& in);
00825 std::vector<Point3D> get_contour_points();
00826 std::vector<IbeoObject> get_objects();
00827 };
00828
00829 class ObjectData2271 : public IbeoTxMessage
00830 {
00831 public:
00832 static const int32_t DATA_TYPE;
00833
00834 NTPTime start_scan_timestamp;
00835 uint16_t scan_number;
00836 uint16_t number_of_objects;
00837 std::vector<Object2271> object_list;
00838
00839 ObjectData2271();
00840
00841 void parse(const std::vector<uint8_t>& in);
00842 std::vector<Point3D> get_contour_points();
00843 std::vector<IbeoObject> get_objects();
00844 };
00845
00846 class ObjectData2280 : public IbeoTxMessage
00847 {
00848 public:
00849 static const int32_t DATA_TYPE;
00850
00851 NTPTime mid_scan_timestamp;
00852 uint16_t number_of_objects;
00853 std::vector<Object2280> object_list;
00854
00855 ObjectData2280();
00856
00857 void parse(const std::vector<uint8_t>& in);
00858 std::vector<Point3D> get_contour_points();
00859 std::vector<IbeoObject> get_objects();
00860 };
00861
00862 class CameraImage : public IbeoTxMessage
00863 {
00864 public:
00865 static const int32_t DATA_TYPE;
00866
00867 ImageFormat image_format;
00868 uint32_t us_since_power_on;
00869 NTPTime timestamp;
00870 uint8_t device_id;
00871 MountingPositionF mounting_position;
00872 double horizontal_opening_angle;
00873 double vertical_opening_angle;
00874 uint16_t image_width;
00875 uint16_t image_height;
00876 uint32_t compressed_size;
00877 std::vector<uint8_t> image_buffer;
00878
00879 CameraImage();
00880
00881 void parse(const std::vector<uint8_t>& in);
00882 };
00883
00884 class HostVehicleState2805 : public IbeoTxMessage
00885 {
00886 public:
00887 static const int32_t DATA_TYPE;
00888
00889 NTPTime timestamp;
00890 uint16_t scan_number;
00891 uint16_t error_flags;
00892 int16_t longitudinal_velocity;
00893 int16_t steering_wheel_angle;
00894 int16_t front_wheel_angle;
00895 int32_t x_position;
00896 int32_t y_position;
00897 int16_t course_angle;
00898 uint16_t time_difference;
00899 int16_t x_difference;
00900 int16_t y_difference;
00901 int16_t heading_difference;
00902 int16_t current_yaw_rate;
00903
00904 HostVehicleState2805();
00905
00906 void parse(const std::vector<uint8_t>& in);
00907 };
00908
00909 class HostVehicleState2806 : public IbeoTxMessage
00910 {
00911 public:
00912 static const int32_t DATA_TYPE;
00913
00914 NTPTime timestamp;
00915 int32_t distance_x;
00916 int32_t distance_y;
00917 float course_angle;
00918 float longitudinal_velocity;
00919 float yaw_rate;
00920 float steering_wheel_angle;
00921 float cross_acceleration;
00922 float front_wheel_angle;
00923 float vehicle_width;
00924 float vehicle_front_to_front_axle;
00925 float rear_axle_to_front_axle;
00926 float rear_axle_to_vehicle_rear;
00927 float steer_ratio_poly_0;
00928 float steer_ratio_poly_1;
00929 float steer_ratio_poly_2;
00930 float steer_ratio_poly_3;
00931
00932 HostVehicleState2806();
00933
00934 void parse(const std::vector<uint8_t>& in);
00935 };
00936
00937 class HostVehicleState2807 : public HostVehicleState2806
00938 {
00939 public:
00940 static const int32_t DATA_TYPE;
00941
00942 NTPTime timestamp;
00943 int32_t distance_x;
00944 int32_t distance_y;
00945 float course_angle;
00946 float longitudinal_velocity;
00947 float yaw_rate;
00948 float steering_wheel_angle;
00949 float cross_acceleration;
00950 float front_wheel_angle;
00951 float vehicle_width;
00952 float vehicle_front_to_front_axle;
00953 float rear_axle_to_front_axle;
00954 float rear_axle_to_vehicle_rear;
00955 float steer_ratio_poly_0;
00956 float steer_ratio_poly_1;
00957 float steer_ratio_poly_2;
00958 float steer_ratio_poly_3;
00959 float longitudinal_acceleration;
00960
00961 HostVehicleState2807();
00962
00963 void parse(const std::vector<uint8_t>& in);
00964 };
00965
00966 class DeviceStatus : public IbeoTxMessage
00967 {
00968 public:
00969 static const int32_t DATA_TYPE;
00970
00971 uint8_t scanner_type;
00972 float sensor_temperature;
00973 float frequency;
00974
00975 DeviceStatus();
00976
00977 void parse(const std::vector<uint8_t>& in);
00978 };
00979
00980 class CommandSetFilter
00981 {
00982 public:
00983 IbeoDataHeader ibeo_header;
00984 uint16_t command_identifier;
00985 uint16_t version;
00986 uint16_t begin_filter_range;
00987 uint16_t end_filter_range;
00988
00989 std::vector<uint8_t> encoded_data;
00990
00991 void encode();
00992 };
00993
00994 }
00995 }
00996 }
00997
00998 #endif // IBEO_CORE_IBEO_CORE_H