ibeo_core.h
Go to the documentation of this file.
00001 /*
00002 * Unpublished Copyright (c) 2009-2019 AutonomouStuff, LLC, All Rights Reserved.
00003 *
00004 * This file is part of the ibeo_core ROS driver which is released under the MIT license.
00005 * See file LICENSE included with this software or go to https://opensource.org/licenses/MIT for full license details.
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;  // LUX_MESSAGE_DATA_OFFSET and LUX_HEADER_SIZE
00031 const int32_t IBEO_PAYLOAD_SIZE = 10000;  // LUX_PAYLOAD_SIZE
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  // LUX: Float2D and Point2D
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  // LUX: Resolution
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  // LUX: LuxHeader_TX_Message
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 // Start the top-level messages.
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  // LUX: LuxScandata_TX_Message
00666 {
00667 public:
00668   static const int32_t DATA_TYPE;
00669 
00670   uint16_t scan_number;
00671   uint16_t scanner_status;  // Not yet available for ScaLa.
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 }  // namespace Ibeo
00995 }  // namespace Drivers
00996 }  // namespace AS
00997 
00998 #endif  // IBEO_CORE_IBEO_CORE_H


ibeo_core
Author(s): Joshua Whitley , Daniel Stanek
autogenerated on Sat May 18 2019 02:23:51