ibeo_core.cpp
Go to the documentation of this file.
00001 /*
00002  *   ibeo_core.cpp - Core parsing and encoding library for Ibeo drivers.
00003  *   Copyright (C) 2017 AutonomouStuff, Co.
00004  *
00005  *   This library is free software; you can redistribute it and/or
00006  *   modify it under the terms of the GNU Lesser General Public
00007  *   License as published by the Free Software Foundation; either
00008  *   version 2.1 of the License, or (at your option) any later version.
00009  *
00010  *   This library is distributed in the hope that it will be useful,
00011  *   but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00013  *   Lesser General Public License for more details.
00014  *
00015  *   You should have received a copy of the GNU Lesser General Public
00016  *   License along with this library; if not, write to the Free Software
00017  *   Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301
00018  *   USA
00019  */
00020 
00021 #include <vector>
00022 #include <ibeo_core/ibeo_core.h>
00023 #include <network_interface/network_utils.h>
00024 
00025 using namespace AS::Network;  // NOLINT
00026 using namespace AS::Drivers::Ibeo;  // NOLINT
00027 
00028 const int32_t AS::Drivers::Ibeo::ErrorWarning::DATA_TYPE = 0x2030;
00029 const int32_t AS::Drivers::Ibeo::ScanData2202::DATA_TYPE = 0x2202;
00030 const int32_t AS::Drivers::Ibeo::ScanData2204::DATA_TYPE = 0x2204;
00031 const int32_t AS::Drivers::Ibeo::ScanData2205::DATA_TYPE = 0x2205;
00032 const int32_t AS::Drivers::Ibeo::ScanData2208::DATA_TYPE = 0x2208;
00033 const int32_t AS::Drivers::Ibeo::ObjectData2221::DATA_TYPE = 0x2221;
00034 const int32_t AS::Drivers::Ibeo::ObjectData2225::DATA_TYPE = 0x2225;
00035 const int32_t AS::Drivers::Ibeo::ObjectData2270::DATA_TYPE = 0x2270;
00036 const int32_t AS::Drivers::Ibeo::ObjectData2271::DATA_TYPE = 0x2271;
00037 const int32_t AS::Drivers::Ibeo::ObjectData2280::DATA_TYPE = 0x2280;
00038 const int32_t AS::Drivers::Ibeo::CameraImage::DATA_TYPE = 0x2403;
00039 const int32_t AS::Drivers::Ibeo::HostVehicleState2805::DATA_TYPE = 0x2805;
00040 const int32_t AS::Drivers::Ibeo::HostVehicleState2806::DATA_TYPE = 0x2806;
00041 const int32_t AS::Drivers::Ibeo::HostVehicleState2807::DATA_TYPE = 0x2807;
00042 const int32_t AS::Drivers::Ibeo::DeviceStatus::DATA_TYPE = 0x6301;
00043 
00044 // Primitive parsing functions
00045 void MountingPositionF::parse(const std::vector<uint8_t>& in, const uint16_t& offset)
00046 {
00047   yaw_angle = read_be<float>(in, offset);
00048   pitch_angle = read_be<float>(in, offset + 4);
00049   roll_angle = read_be<float>(in, offset + 8);
00050   x_position = read_be<float>(in, offset + 12);
00051   y_position = read_be<float>(in, offset + 16);
00052   z_position = read_be<float>(in, offset + 20);
00053 }
00054 
00055 void ContourPointSigma::parse(
00056   const std::vector<uint8_t>& in,
00057   const uint16_t& offset,
00058   ByteOrder bo)
00059 {
00060   parse_tuple<int16_t>(in, &x, &y, bo, offset);
00061   parse_tuple<uint8_t>(in, &x_sigma, &y_sigma, bo, offset);
00062 }
00063 
00064 void Point2Df::parse(
00065   const std::vector<uint8_t>& in,
00066   const uint16_t& offset,
00067   ByteOrder bo)
00068 {
00069   parse_tuple<float>(in, &x, &y, bo, offset);
00070 }
00071 
00072 void Point2Di::parse(
00073   const std::vector<uint8_t>& in,
00074   const uint16_t& offset,
00075   ByteOrder bo)
00076 {
00077   parse_tuple<int16_t>(in, &x, &y, bo, offset);
00078 }
00079 
00080 void Point2Dui::parse(
00081   const std::vector<uint8_t>& in,
00082   const uint16_t& offset,
00083   ByteOrder bo)
00084 {
00085   parse_tuple<uint16_t>(in, &x, &y, bo, offset);
00086 }
00087 
00088 void Sigma2D::parse(
00089   const std::vector<uint8_t>& in,
00090   const uint16_t& offset,
00091   ByteOrder bo)
00092 {
00093   parse_tuple<uint16_t>(in, &sigma_x, &sigma_y, bo, offset);
00094 }
00095 
00096 void Size2D::parse(
00097   const std::vector<uint8_t>& in,
00098   const uint16_t& offset,
00099   ByteOrder bo)
00100 {
00101   parse_tuple<uint16_t>(in, &size_x, &size_y, bo, offset);
00102 }
00103 
00104 void Velocity2D::parse(
00105   const std::vector<uint8_t>& in,
00106   const uint16_t& offset,
00107   ByteOrder bo)
00108 {
00109   parse_tuple<int16_t>(in, &velocity_x, &velocity_y, bo, offset);
00110 }
00111 
00112 void ResolutionInfo::parse(const std::vector<uint8_t>& in, const uint16_t& offset)
00113 {
00114   resolution_start_angle = read_be<float>(in, offset);
00115   resolution = read_be<float>(in, offset + 4);
00116 }
00117 
00118 void ScannerInfo2204::parse(const std::vector<uint8_t>& in, const uint16_t& offset)
00119 {
00120   device_id = read_be<uint8_t>(in, offset);
00121   scanner_type = read_be<uint8_t>(in, offset + 1);
00122   scan_number = read_be<uint16_t>(in, offset + 2);
00123   start_angle = read_be<float>(in, offset + 8);
00124   end_angle = read_be<float>(in, offset + 12);
00125   yaw_angle = read_be<float>(in, offset + 16);
00126   pitch_angle = read_be<float>(in, offset + 20);
00127   roll_angle = read_be<float>(in, offset + 24);
00128   offset_x = read_be<float>(in, offset + 28);
00129   offset_y = read_be<float>(in, offset + 32);
00130   offset_z = read_be<float>(in, offset + 36);
00131 }
00132 
00133 void ScannerInfo2205::parse(const std::vector<uint8_t>& in, const uint16_t& offset)
00134 {
00135   device_id = read_be<uint8_t>(in, offset);
00136   scanner_type = read_be<uint8_t>(in, offset + 1);
00137   scan_number = read_be<uint16_t>(in, offset + 2);
00138   start_angle = read_be<float>(in, offset + 8);
00139   end_angle = read_be<float>(in, offset + 12);
00140   scan_start_time = read_be<NTPTime>(in, offset + 16);
00141   scan_end_time = read_be<NTPTime>(in, offset + 24);
00142   scan_start_time_from_device = read_be<NTPTime>(in, offset + 32);
00143   scan_end_time_from_device = read_be<NTPTime>(in, offset + 40);
00144   scan_frequency = read_be<float>(in, offset + 48);
00145   beam_tilt = read_be<float>(in, offset + 52);
00146   scan_flags = read_be<float>(in, offset + 56);
00147   mounting_position.parse(in, offset + 60);
00148   resolutions[0].parse(in, offset + 84);
00149   resolutions[1].parse(in, offset + 92);
00150   resolutions[2].parse(in, offset + 100);
00151   resolutions[3].parse(in, offset + 108);
00152   resolutions[4].parse(in, offset + 116);
00153   resolutions[5].parse(in, offset + 124);
00154   resolutions[6].parse(in, offset + 132);
00155   resolutions[7].parse(in, offset + 140);
00156 }
00157 
00158 // Object parsing functions
00159 void UntrackedProperties::parse(const std::vector<uint8_t>& in, const uint16_t& offset)
00160 {
00161   relative_time_of_measurement = read_be<uint16_t>(in, offset + 1);
00162   position_closest_point.parse(in, offset + 3, ByteOrder::BE);
00163   object_box_size.parse(in, offset + 9, ByteOrder::BE);
00164   object_box_size_sigma.parse(in, offset + 13, ByteOrder::BE);
00165   object_box_orientation = read_be<int16_t>(in, offset + 17);
00166   object_box_orientation_sigma = read_be<uint16_t>(in, offset + 19);
00167   tracking_point_coordinate.parse(in, offset + 23, ByteOrder::BE);
00168   tracking_point_coordinate_sigma.parse(in, offset + 27, ByteOrder::BE);
00169   number_of_contour_points = read_be<uint8_t>(in, offset + 34);
00170 
00171   if (number_of_contour_points == 255)
00172     number_of_contour_points = 0;
00173 
00174   for (uint8_t i = 0; i < number_of_contour_points; i++)
00175   {
00176     ContourPointSigma new_contour_point;
00177     new_contour_point.parse(in, offset + 35 + (i * 8), ByteOrder::BE);
00178     contour_point_list.push_back(new_contour_point);
00179   }
00180 }
00181 
00182 void TrackedProperties::parse(const std::vector<uint8_t>& in, const uint16_t& offset)
00183 {
00184   object_age = read_be<uint16_t>(in, offset + 1);
00185   hidden_status_age = read_be<uint16_t>(in, offset + 3);
00186   uint8_t dynamic_flags = read_be<uint8_t>(in, offset + 5);
00187   object_phase = ((dynamic_flags & 0x01) > 0) ? TRACKING : INITIALIZATION;
00188   dynamic_property = static_cast<DynamicProperty>((dynamic_flags & 0x70) > 0);
00189   relative_time_of_measure = read_be<uint16_t>(in, offset + 6);
00190   position_closest_point.parse(in, offset + 8, ByteOrder::BE);
00191   relative_velocity.parse(in, offset + 12, ByteOrder::BE);
00192   relative_velocity_sigma.parse(in, offset + 16, ByteOrder::BE);
00193   classification = static_cast<Classification>(read_be<uint8_t>(in, offset + 20));
00194   classification_age = read_be<uint16_t>(in, offset + 22);
00195   object_box_size.parse(in, offset + 26, ByteOrder::BE);
00196   object_box_size_sigma.parse(in, offset + 30, ByteOrder::BE);
00197   object_box_orientation = read_be<int16_t>(in, offset + 34);
00198   object_box_orientation_sigma = read_be<uint16_t>(in, offset + 36);
00199   tracking_point_location = static_cast<PointLocation>(read_be<uint8_t>(in, offset + 39));
00200   tracking_point_coordinate.parse(in, offset + 40, ByteOrder::BE);
00201   tracking_point_coordinate_sigma.parse(in, offset + 44, ByteOrder::BE);
00202   velocity.parse(in, offset + 51, ByteOrder::BE);
00203   velocity_sigma.parse(in, offset + 55, ByteOrder::BE);
00204   acceleration.parse(in, offset + 61, ByteOrder::BE);
00205   acceleration_sigma.parse(in, offset + 65, ByteOrder::BE);
00206   yaw_rate = read_be<int16_t>(in, offset + 71);
00207   yaw_rate_sigma = read_be<uint16_t>(in, offset + 73);
00208   number_of_contour_points = read_be<uint8_t>(in, offset + 75);
00209 
00210   if (number_of_contour_points == 255)
00211     number_of_contour_points = 0;
00212 
00213   for (uint8_t i = 0; i < number_of_contour_points; i++)
00214   {
00215     ContourPointSigma new_contour_point;
00216     new_contour_point.parse(in, offset + 76 + (i * 8), ByteOrder::BE);
00217     contour_point_list.push_back(new_contour_point);
00218   }
00219 }
00220 
00221 void ScanPoint2202::parse(const std::vector<uint8_t>& in, const uint16_t& offset)
00222 {
00223   uint8_t layer_and_offset = read_le<uint8_t>(in, offset);
00224   std::cout << std::hex;
00225   layer = (layer_and_offset & 0x0F);
00226   echo = ((layer_and_offset & 0xF0) >> 4);
00227   uint8_t flags = read_le<uint8_t>(in, offset + 1);
00228   transparent_point = ((flags & 0x01) > 0);
00229   clutter_atmospheric = ((flags & 0x02) > 0);
00230   ground = ((flags & 0x04) > 0);
00231   dirt = ((flags & 0x08) > 0);
00232   horizontal_angle = read_le<int16_t>(in, offset + 2);
00233   radial_distance = read_le<uint16_t>(in, offset + 4);
00234   echo_pulse_width = read_le<uint16_t>(in, offset + 6);
00235 }
00236 
00237 void ScanPoint2204::parse(const std::vector<uint8_t>& in, const uint16_t& offset)
00238 {
00239   x_position = read_be<float>(in, offset + 0);
00240   y_position = read_be<float>(in, offset + 4);
00241   z_position = read_be<float>(in, offset + 8);
00242   echo_width = read_be<float>(in, offset + 12);
00243   device_id = read_be<uint8_t>(in, offset + 16);
00244   layer = read_be<uint8_t>(in, offset + 17);
00245   echo = read_be<uint8_t>(in, offset + 18);
00246   time_offset = read_be<uint32_t>(in, offset + 20);
00247 
00248   uint16_t flags = read_be<uint16_t>(in, offset + 24);
00249 
00250   ground = ((flags & 0x0001) > 0);
00251   dirt = ((flags & 0x0002) > 0);
00252   precipitation = ((flags & 0x0004) > 0);
00253 }
00254 
00255 void ScanPoint2205::parse(const std::vector<uint8_t>& in, const uint16_t& offset)
00256 {
00257   x_position = read_be<float>(in, offset);
00258   y_position = read_be<float>(in, offset + 4);
00259   z_position = read_be<float>(in, offset + 8);
00260   echo_width = read_be<float>(in, offset + 12);
00261   device_id = read_be<uint8_t>(in, offset + 16);
00262   layer = read_be<uint8_t>(in, offset + 17);
00263   echo = read_be<uint8_t>(in, offset + 18);
00264   time_offset = read_be<uint8_t>(in, offset + 20);
00265   uint16_t flags = read_be<uint8_t>(in, offset + 24);
00266   ground = ((flags & 0x0001) > 0);
00267   dirt = ((flags & 0x0002) > 0);
00268   precipitation = ((flags & 0x0004) > 2);
00269   transparent = ((flags & 0x1000) > 12);
00270 }
00271 
00272 void ScanPoint2208::parse(const std::vector<uint8_t>& in, const uint16_t& offset)
00273 {
00274   echo = (read_be<uint8_t>(in, offset) & 0x0C);
00275   layer = read_be<uint8_t>(in, offset + 1);  // & 0x03; < should be whole byte, but this gives unexpected results.
00276   uint16_t flags = read_be<uint16_t>(in, offset + 2);
00277   transparent_point = ((flags & 0x0001) > 0);
00278   clutter_atmospheric = ((flags & 0x0002) > 0);
00279   ground = ((flags & 0x0004) > 0);
00280   dirt = ((flags & 0x0008) > 0);
00281   horizontal_angle = read_be<int16_t>(in, offset + 4);
00282   radial_distance = read_be<uint16_t>(in, offset + 6);
00283   echo_pulse_width = read_be<uint16_t>(in, offset + 8);
00284 }
00285 
00286 void Object2221::parse(const std::vector<uint8_t>& in, const uint16_t& offset)
00287 {
00288   id = read_le<uint16_t>(in, offset);
00289   age = read_le<uint16_t>(in, offset + 2);
00290   prediction_age = read_le<uint16_t>(in, offset + 4);
00291   relative_timestamp = read_le<uint16_t>(in, offset + 6);
00292   reference_point.x = read_le<int16_t>(in, offset + 8);
00293   reference_point.y = read_le<int16_t>(in, offset + 10);
00294   reference_point_sigma.x = read_le<int16_t>(in, offset + 12);
00295   reference_point_sigma.y = read_le<int16_t>(in, offset + 14);
00296   closest_point.x = read_le<int16_t>(in, offset + 16);
00297   closest_point.y = read_le<int16_t>(in, offset + 18);
00298   bounding_box_center.x = read_le<int16_t>(in, offset + 20);
00299   bounding_box_center.y = read_le<int16_t>(in, offset + 22);
00300   bounding_box_width = read_le<uint16_t>(in, offset + 24);
00301   bounding_box_length = read_le<uint16_t>(in, offset + 26);
00302   object_box_center.x = read_le<int16_t>(in, offset + 28);
00303   object_box_center.y = read_le<int16_t>(in, offset + 30);
00304   object_box_size.size_x = read_le<uint16_t>(in, offset + 32);
00305   object_box_size.size_y = read_le<uint16_t>(in, offset + 34);
00306   object_box_orientation = read_le<int16_t>(in, offset + 36);
00307   absolute_velocity.x = read_le<int16_t>(in, offset + 38);
00308   absolute_velocity.y = read_le<int16_t>(in, offset + 40);
00309   absolute_velocity_sigma.size_x = read_le<uint16_t>(in, offset + 42);
00310   absolute_velocity_sigma.size_y = read_le<uint16_t>(in, offset + 44);
00311   relative_velocity.x = read_le<int16_t>(in, offset + 46);
00312   relative_velocity.y = read_le<int16_t>(in, offset + 48);
00313   classification = static_cast<Classification>(read_le<uint8_t>(in, offset + 50));
00314   classification_age = read_le<uint16_t>(in, offset + 52);
00315   classification_certainty = read_le<uint16_t>(in, offset + 54);
00316   number_of_contour_points = read_le<uint16_t>(in, offset + 56);
00317 
00318   if (number_of_contour_points == 65535)
00319     number_of_contour_points = 0;
00320 
00321   for (uint16_t i = 0; i < number_of_contour_points; i++)
00322   {
00323     Point2Di contour_point;
00324     contour_point.parse(in, offset + 58 + (i * 4), ByteOrder::LE);
00325     contour_point_list.push_back(contour_point);
00326   }
00327 }
00328 
00329 void Object2225::parse(const std::vector<uint8_t>& in, const uint16_t& offset)
00330 {
00331   id = read_be<uint16_t>(in, offset);
00332   age = read_be<uint32_t>(in, offset + 4);
00333   timestamp = read_be<NTPTime>(in, offset + 8);
00334   hidden_status_age = read_be<uint16_t>(in, offset + 16);
00335   classification = static_cast<Classification>(read_be<uint8_t>(in, offset + 18));
00336   classification_certainty = read_be<uint8_t>(in, offset + 19);
00337   classification_age = read_be<uint32_t>(in, offset + 20);
00338   bounding_box_center.parse(in, offset + 24, ByteOrder::BE);
00339   bounding_box_size.parse(in, offset + 32, ByteOrder::BE);
00340   object_box_center.parse(in, offset + 40, ByteOrder::BE);
00341   object_box_center_sigma.parse(in, offset + 48, ByteOrder::BE);
00342   object_box_size.parse(in, offset + 56, ByteOrder::BE);
00343   yaw_angle = read_be<float>(in, offset + 72);
00344   relative_velocity.parse(in, offset + 80, ByteOrder::BE);
00345   relative_velocity_sigma.parse(in, offset + 88, ByteOrder::BE);
00346   absolute_velocity.parse(in, offset + 96, ByteOrder::BE);
00347   absolute_velocity_sigma.parse(in, offset + 104, ByteOrder::BE);
00348   number_of_contour_points = read_be<uint8_t>(in, offset + 130);
00349   closest_point_index = read_be<uint8_t>(in, offset + 131);
00350 
00351   if (number_of_contour_points == 255)
00352     number_of_contour_points = 0;
00353 
00354   for (uint8_t i = 0; i < number_of_contour_points; i++)
00355   {
00356     Point2Df contour_point;
00357     contour_point.parse(in, offset + 132 + (i * 8), ByteOrder::BE);
00358     contour_point_list.push_back(contour_point);
00359   }
00360 }
00361 
00362 void Object2270::parse(const std::vector<uint8_t>& in, const uint16_t& offset)
00363 {
00364   id = read_le<uint16_t>(in, offset);
00365   age = read_le<uint16_t>(in, offset + 2);
00366   prediction_age = read_le<uint16_t>(in, offset + 4);
00367   relative_moment_of_measurement = read_le<uint16_t>(in, offset + 6);
00368   reference_point_location = static_cast<PointLocation>(read_le<uint8_t>(in, offset + 9));
00369   reference_point_position_x = read_le<int16_t>(in, offset + 10);
00370   reference_point_position_y = read_le<int16_t>(in, offset + 12);
00371   reference_point_position_sigma_x = read_le<uint16_t>(in, offset + 14);
00372   reference_point_position_sigma_y = read_le<uint16_t>(in, offset + 16);
00373   contour_points_cog_x = read_le<int16_t>(in, offset + 36);
00374   contour_points_cog_y = read_le<int16_t>(in, offset + 38);
00375   object_box_length = read_le<uint16_t>(in, offset + 40);
00376   object_box_width = read_le<uint16_t>(in, offset + 42);
00377   object_box_orientation_angle = read_le<int16_t>(in, offset + 44);
00378   object_box_orientation_angle_sigma = read_le<int16_t>(in, offset + 50);
00379   absolute_velocity_x = read_le<int16_t>(in, offset + 52);
00380   absolute_velocity_y = read_le<int16_t>(in, offset + 54);
00381   absolute_velocity_sigma_x = read_le<uint16_t>(in, offset + 56);
00382   absolute_velocity_sigma_y = read_le<uint16_t>(in, offset + 58);
00383   relative_velocity_x = read_le<int16_t>(in, offset + 60);
00384   relative_velocity_y = read_le<int16_t>(in, offset + 62);
00385   relative_velocity_sigma_x = read_le<uint16_t>(in, offset + 64);
00386   relative_velocity_sigma_y = read_le<uint16_t>(in, offset + 66);
00387   classification = static_cast<Classification>(read_le<uint8_t>(in, offset + 68));
00388   uint8_t flags = read_le<uint8_t>(in, offset + 69);
00389   tracking_model = ((flags & 0x01) > 0) ? STATIC : DYNAMIC;
00390   mobile_detected = ((flags & 0x02) > 0);
00391   track_valid = ((flags & 0x04) > 0);
00392   classification_age = read_le<uint16_t>(in, offset + 70);
00393   classification_confidence = read_le<uint16_t>(in, offset + 72);
00394   number_of_contour_points = read_le<uint16_t>(in, offset + 74);
00395 
00396   if (number_of_contour_points == 65535)
00397     number_of_contour_points = 0;
00398 
00399   for (uint16_t i = 0; i < number_of_contour_points; i++)
00400   {
00401     Point2Di contour_point;
00402     contour_point.parse(in, offset + 76 + (i * 4), ByteOrder::LE);
00403     contour_point_list.push_back(contour_point);
00404   }
00405 }
00406 
00407 void Object2271::parse(const std::vector<uint8_t>& in, const uint16_t& offset)
00408 {
00409   id = read_be<uint32_t>(in, offset);
00410   uint8_t properties_available = read_be<uint8_t>(in, offset + 6);
00411   untracked_properties_available = ((properties_available & 0x02) > 0);
00412   tracked_properties_available = ((properties_available & 0x08) > 0);
00413 
00414   if (untracked_properties_available)
00415   {
00416     untracked_properties.parse(in, offset + 7);
00417   }
00418   else
00419   {
00420     untracked_properties.number_of_contour_points = 0;
00421   }
00422 
00423   if (tracked_properties_available)
00424   {
00425     tracked_properties.parse(in, offset + 42 + (untracked_properties.number_of_contour_points * 8));
00426   }
00427   else
00428   {
00429     tracked_properties.number_of_contour_points = 0;
00430   }
00431 }
00432 
00433 void Object2280::parse(const std::vector<uint8_t>& in, const uint16_t& offset)
00434 {
00435   id = read_be<uint16_t>(in, offset);
00436 
00437   uint16_t flags = read_be<uint16_t>(in, offset + 2);
00438   tracking_model = ((flags & 0x40) > 0) ? STATIC : DYNAMIC;
00439   mobility_of_dyn_object_detected = ((flags & 0x80) > 0);
00440   motion_model_validated = ((flags & 0x100) > 0);
00441   object_age = read_be<uint32_t>(in, offset + 4);
00442   timestamp = read_be<NTPTime>(in, offset + 8);
00443   object_prediction_age = read_be<uint16_t>(in, offset + 16);
00444   classification = static_cast<Classification>(read_be<uint8_t>(in, offset + 18));
00445   classification_certainty = read_be<uint8_t>(in, offset + 19);
00446   classification_age = read_be<uint32_t>(in, offset + 20);
00447   object_box_center.parse(in, offset + 40, ByteOrder::BE);
00448   object_box_center_sigma.parse(in, offset + 48, ByteOrder::BE);
00449   object_box_size.parse(in, offset + 56, ByteOrder::BE);
00450   object_box_orientation_angle = read_be<float>(in, offset + 72);
00451   object_box_orientation_angle_sigma = read_be<float>(in, offset + 76);
00452   relative_velocity.parse(in, offset + 80, ByteOrder::BE);
00453   relative_velocity_sigma.parse(in, offset + 88, ByteOrder::BE);
00454   absolute_velocity.parse(in, offset + 96, ByteOrder::BE);
00455   absolute_velocity_sigma.parse(in, offset + 104, ByteOrder::BE);
00456   number_of_contour_points = read_be<uint8_t>(in, offset + 130);
00457   closest_point_index = read_be<uint8_t>(in, offset + 131);
00458   reference_point_location = static_cast<PointLocation>(read_be<uint16_t>(in, offset + 132));
00459   reference_point_coordinate.parse(in, offset + 134, ByteOrder::BE);
00460   reference_point_coordinate_sigma.parse(in, offset + 142, ByteOrder::BE);
00461   reference_point_position_correction_coefficient = read_be<float>(in, offset + 150);
00462   object_priority = read_be<uint16_t>(in, offset + 162);
00463   object_existence_measurement = read_be<float>(in, offset + 164);
00464 
00465   if (number_of_contour_points == 255)
00466     number_of_contour_points = 0;
00467 
00468   for (uint8_t i = 0; i < number_of_contour_points; i++)
00469   {
00470     Point2Df new_contour_point;
00471     new_contour_point.parse(in, offset + 168 + (i * 8), ByteOrder::BE);
00472     contour_point_list.push_back(new_contour_point);
00473   }
00474 }
00475 
00476 // High-level message parsers.
00477 void IbeoDataHeader::parse(const std::vector<uint8_t>& in)
00478 {
00479   previous_message_size = read_be<uint32_t>(in, 4);
00480   message_size = read_be<uint32_t>(in, 8);
00481   device_id = read_be<uint8_t>(in, 13);
00482   data_type_id = read_be<uint16_t>(in, 14);
00483   time = read_be<NTPTime>(in, 16);
00484 }
00485 
00486 void IbeoDataHeader::encode()
00487 {
00488   encoded_data.clear();
00489 
00490   // Magic Word
00491   encoded_data.push_back(0xAF);
00492   encoded_data.push_back(0xFE);
00493   encoded_data.push_back(0xC0);
00494   encoded_data.push_back(0xC2);
00495   // size_of_previous_message (unused in live data)
00496   encoded_data.push_back(0x00);
00497   encoded_data.push_back(0x00);
00498   encoded_data.push_back(0x00);
00499   encoded_data.push_back(0x00);
00500 
00501   // message_size
00502   std::vector<uint8_t> encoded_message_size = write_be<uint32_t>(&message_size);
00503   encoded_data.insert(encoded_data.end(), encoded_message_size.begin(), encoded_message_size.end());
00504   // Reserved
00505   encoded_data.push_back(0x00);
00506   // device_id
00507   encoded_data.push_back(0x00);
00508   // data_type_id
00509   std::vector<uint8_t> encoded_data_type_id = write_be<uint16_t>(&data_type_id);
00510   encoded_data.insert(encoded_data.end(), encoded_data_type_id.begin(), encoded_data_type_id.end());
00511   // time
00512   std::vector<uint8_t> encoded_time = write_be<NTPTime>(&time);
00513   encoded_data.insert(encoded_data.end(), encoded_time.begin(), encoded_time.end());
00514 }
00515 
00516 IbeoTxMessage::IbeoTxMessage() :
00517   has_scan_points(false),
00518   has_contour_points(false),
00519   has_objects(false)
00520 {}
00521 
00522 IbeoTxMessage::IbeoTxMessage(bool scan_points, bool contour_points, bool objects) :
00523   has_scan_points(scan_points),
00524   has_contour_points(contour_points),
00525   has_objects(objects)
00526 {}
00527 
00528 std::shared_ptr<IbeoTxMessage> IbeoTxMessage::make_message(const uint16_t& data_type)
00529 {
00530   switch (data_type)
00531   {
00532   case ErrorWarning::DATA_TYPE:
00533     return std::shared_ptr<IbeoTxMessage>(new ErrorWarning);
00534     break;
00535   case ScanData2202::DATA_TYPE:
00536     return std::shared_ptr<IbeoTxMessage>(new ScanData2202);
00537     break;
00538   case ScanData2205::DATA_TYPE:
00539     return std::shared_ptr<IbeoTxMessage>(new ScanData2205);
00540     break;
00541   case ScanData2208::DATA_TYPE:
00542     return std::shared_ptr<IbeoTxMessage>(new ScanData2208);
00543     break;
00544   case ObjectData2221::DATA_TYPE:
00545     return std::shared_ptr<IbeoTxMessage>(new ObjectData2221);
00546     break;
00547   case ObjectData2225::DATA_TYPE:
00548     return std::shared_ptr<IbeoTxMessage>(new ObjectData2225);
00549     break;
00550   case ObjectData2270::DATA_TYPE:
00551     return std::shared_ptr<IbeoTxMessage>(new ObjectData2270);
00552     break;
00553   case ObjectData2271::DATA_TYPE:
00554     return std::shared_ptr<IbeoTxMessage>(new ObjectData2271);
00555     break;
00556   case ObjectData2280::DATA_TYPE:
00557     return std::shared_ptr<IbeoTxMessage>(new ObjectData2280);
00558     break;
00559   case CameraImage::DATA_TYPE:
00560     return std::shared_ptr<IbeoTxMessage>(new CameraImage);
00561     break;
00562   case HostVehicleState2805::DATA_TYPE:
00563     return std::shared_ptr<IbeoTxMessage>(new HostVehicleState2805);
00564     break;
00565   case HostVehicleState2806::DATA_TYPE:
00566     return std::shared_ptr<IbeoTxMessage>(new HostVehicleState2806);
00567     break;
00568   case HostVehicleState2807::DATA_TYPE:
00569     return std::shared_ptr<IbeoTxMessage>(new HostVehicleState2807);
00570     break;
00571   case DeviceStatus::DATA_TYPE:
00572     return std::shared_ptr<IbeoTxMessage>(new DeviceStatus);
00573     break;
00574   default:
00575     return NULL;
00576   }
00577 }
00578 
00579 std::vector<Point3DL> IbeoTxMessage::get_scan_points()  // <-- virtual
00580 {
00581   return std::vector<Point3DL>();
00582 }
00583 
00584 std::vector<Point3D> IbeoTxMessage::get_contour_points()
00585 {
00586   return std::vector<Point3D>();
00587 }
00588 
00589 std::vector<IbeoObject> IbeoTxMessage::get_objects()
00590 {
00591   return std::vector<IbeoObject>();
00592 }
00593 
00594 void ErrorWarning::parse(const std::vector<uint8_t>& in)
00595 {
00596   ibeo_header.parse(in);
00597 
00598   uint16_t hdr = IBEO_HEADER_SIZE;
00599 
00600   uint16_t err_reg_1 = read_le<uint16_t>(in, hdr);
00601   uint16_t err_reg_2 = read_le<uint16_t>(in, hdr + 2);
00602   uint16_t wrn_reg_1 = read_le<uint16_t>(in, hdr + 4);
00603   uint16_t wrn_reg_2 = read_le<uint16_t>(in, hdr + 6);
00604 
00605   err_internal_error = ((err_reg_1 & 0x0001) > 0);
00606   err_motor_1_fault = ((err_reg_1 & 0x0002) > 0);
00607   err_buffer_error_xmt_incomplete = ((err_reg_1 & 0x0004) > 0);
00608   err_buffer_error_overflow = ((err_reg_1 & 0x0008) > 0);
00609   err_apd_over_temperature = ((err_reg_1 & 0x0100) > 0);
00610   err_apd_under_temperature = ((err_reg_1 & 0x0200) > 0);
00611   err_apd_temperature_sensor_defect = (err_apd_over_temperature && err_apd_under_temperature);
00612   err_motor_2_fault = ((err_reg_1 & 0x0400) > 0);
00613   err_motor_3_fault = ((err_reg_1 & 0x0800) > 0);
00614   err_motor_4_fault = ((err_reg_1 & 0x1000) > 0);
00615   err_motor_5_fault = ((err_reg_1 & 0x2000) > 0);
00616 
00617   err_int_no_scan_data = ((err_reg_2 & 0x0001) > 0);
00618   err_int_communication_error = ((err_reg_2 & 0x0002) > 0);
00619   err_int_incorrect_scan_data = ((err_reg_2 & 0x0004) > 0);
00620   err_config_fpga_not_configurable = ((err_reg_2 & 0x0008) > 0);
00621   err_config_incorrect_config_data = ((err_reg_2 & 0x0010) > 0);
00622   err_config_contains_incorrect_params = ((err_reg_2 & 0x0020) > 0);
00623   err_timeout_data_processing = ((err_reg_2 & 0x0040) > 0);
00624   err_timeout_env_model_computation_reset = ((err_reg_2 & 0x0080) > 0);
00625 
00626   wrn_int_communication_error = ((wrn_reg_1 & 0x0001) > 0);
00627   wrn_low_temperature = ((wrn_reg_1 & 0x0008) > 0);
00628   wrn_high_temperature = ((wrn_reg_1 & 0x0010) > 0);
00629   wrn_int_motor_1 = ((wrn_reg_1 & 0x0020) > 0);
00630   wrn_sync_error = ((wrn_reg_1 & 0x0080) > 0);
00631   wrn_laser_1_start_pulse_missing = ((wrn_reg_1 & 0x1000) > 0);
00632   wrn_laser_2_start_pulse_missing = ((wrn_reg_1 & 0x2000) > 0);
00633 
00634   wrn_can_interface_blocked = ((wrn_reg_2 & 0x0001) > 0);
00635   wrn_eth_interface_blocked = ((wrn_reg_2 & 0x0002) > 0);
00636   wrn_incorrect_can_data_rcvd = ((wrn_reg_2 & 0x0004) > 0);
00637   wrn_int_incorrect_scan_data = ((wrn_reg_2 & 0x0008) > 0);
00638   wrn_eth_unkwn_incomplete_data = ((wrn_reg_2 & 0x0010) > 0);
00639   wrn_incorrect_or_forbidden_cmd_rcvd = ((wrn_reg_2 & 0x0020) > 0);
00640   wrn_memory_access_failure = ((wrn_reg_2 & 0x0040) > 0);
00641   wrn_int_overflow = ((wrn_reg_2 & 0x0080) > 0);
00642   wrn_ego_motion_data_missing = ((wrn_reg_2 & 0x0100) > 0);
00643   wrn_incorrect_mounting_params = ((wrn_reg_2 & 0x0200) > 0);
00644   wrn_no_obj_comp_due_to_scan_freq = ((wrn_reg_2 & 0x0400) > 0);
00645 }
00646 
00647 ScanData2202::ScanData2202() :
00648   IbeoTxMessage(true, false, false)
00649 {}
00650 
00651 void ScanData2202::parse(const std::vector<uint8_t>& in)
00652 {
00653   ibeo_header.parse(in);
00654 
00655   uint16_t hdr = IBEO_HEADER_SIZE;
00656 
00657   scan_number = read_le<uint16_t>(in, hdr);
00658   scanner_status = read_le<uint16_t>(in, hdr + 2);
00659   sync_phase_offset  = read_le<uint16_t>(in, hdr + 4);
00660   scan_start_time = read_le<NTPTime>(in, hdr + 6);
00661   scan_end_time = read_le<NTPTime>(in, hdr + 14);
00662   angle_ticks_per_rotation = read_le<uint16_t>(in, hdr + 22);
00663   start_angle_ticks = read_le<int16_t>(in, hdr + 24);
00664   end_angle_ticks = read_le<int16_t>(in, hdr + 26);
00665   scan_points_count = read_le<uint16_t>(in, hdr + 28);
00666   mounting_yaw_angle_ticks = read_le<int16_t>(in, hdr + 30);
00667   mounting_pitch_angle_ticks = read_le<int16_t>(in, hdr + 32);
00668   mounting_roll_angle_ticks = read_le<int16_t>(in, hdr + 34);
00669   mounting_position_x = read_le<int16_t>(in, hdr + 36);
00670   mounting_position_y = read_le<int16_t>(in, hdr + 38);
00671   mounting_position_z = read_le<int16_t>(in, hdr + 40);
00672 
00673   uint16_t flags = read_le<uint16_t>(in, hdr + 42);
00674   ground_labeled = ((flags & 0x0001) > 0);
00675   dirt_labeled = ((flags & 0x0002) > 0);
00676   rain_labeled = ((flags & 0x0004) > 0);
00677   mirror_side = (((flags & 0x0400) > 0) ? REAR : FRONT);
00678 
00679   for (uint16_t i = 0; i < scan_points_count; i++)
00680   {
00681     ScanPoint2202 new_scan_point;
00682     new_scan_point.parse(in, hdr + 44 + (i * 10));
00683     scan_point_list.push_back(new_scan_point);
00684   }
00685 }
00686 
00687 std::vector<Point3DL> ScanData2202::get_scan_points()
00688 {
00689   std::vector<Point3DL> v;
00690   double cm_to_m = 100.0;
00691 
00692   for (ScanPoint2202 sp : scan_point_list)
00693   {
00694     if (sp.echo == 0 && sp.layer < 4 && !sp.transparent_point && !sp.clutter_atmospheric && !sp.ground && !sp.dirt)
00695     {
00696       // Spherical coordinates to cartesian
00697       // This uses the following conventions (with right-hand rule applied):
00698       // phi = rotation about the Z axis (angle in radians from X = 0)
00699       // theta = angle away from Z+ (radians)
00700       // rho = radial distance (m)
00701       Point3DL p3d;
00702       double phi = ticks_to_angle(sp.horizontal_angle, angle_ticks_per_rotation);
00703       double theta;
00704       double rho = sp.radial_distance / cm_to_m;
00705 
00706       switch (sp.layer)
00707       {
00708       case 0:
00709         theta = (88.4 * (M_PI / 180.0));
00710         break;
00711       case 1:
00712         theta = (89.2 * (M_PI / 180.0));
00713         break;
00714       case 2:
00715         theta = (90.8 * (M_PI / 180.0));
00716         break;
00717       case 3:
00718         theta = (91.6 * (M_PI / 180.0));
00719         break;
00720       }
00721 
00722       p3d.x = rho * sin(theta) * cos(phi);
00723       p3d.y = rho * sin(theta) * sin(phi);
00724       p3d.z = rho * cos(theta);
00725 
00726       p3d.label = sp.layer;
00727 
00728       v.push_back(p3d);
00729     }
00730   }
00731 
00732   return v;
00733 }
00734 
00735 ScanData2204::ScanData2204() :
00736   IbeoTxMessage(true, false, false)
00737 {}
00738 
00739 void ScanData2204::parse(const std::vector<uint8_t>& in)
00740 {
00741   ibeo_header.parse(in);
00742 
00743   uint16_t hdr = IBEO_HEADER_SIZE;
00744 
00745   scan_start_time = read_be<NTPTime>(in, hdr);
00746   scan_end_time_offset = read_be<uint32_t>(in, hdr + 8);
00747 
00748   uint32_t flags = read_be<uint32_t>(in, hdr + 12);
00749   ground_labeled = ((flags & 0x00000001) > 0);
00750   dirt_labeled = ((flags & 0x00000002) > 0);
00751   rain_labeled = ((flags & 0x00000004) > 0);
00752   fused_scan = ((flags & 0x00000100) > 0);
00753   mirror_side = ((flags & 0x00000200) > 0) ? REAR : FRONT;
00754   coordinate_system = ((flags & 0x00000400) > 0) ? VEHICLE : SCANNER;
00755 
00756   scan_number = read_be<uint16_t>(in, hdr + 16);
00757   scan_points = read_be<uint16_t>(in, hdr + 18);
00758   number_of_scanner_infos = read_be<uint8_t>(in, hdr + 20);
00759 
00760   for (uint8_t i = 0; i < number_of_scanner_infos; i++)
00761   {
00762     ScannerInfo2204 scanner_info;
00763     scanner_info.parse(in, hdr + 24 + (i * 40));
00764     scanner_info_list.push_back(scanner_info);
00765   }
00766 
00767   for (uint16_t i = 0; i < scan_points; i++)
00768   {
00769     ScanPoint2204 scan_point;
00770     scan_point.parse(in, hdr + 24 + (number_of_scanner_infos * 40) + (i * 28));
00771     scan_point_list.push_back(scan_point);
00772   }
00773 }
00774 
00775 std::vector<Point3DL> ScanData2204::get_scan_points()
00776 {
00777   std::vector<Point3DL> v;
00778 
00779   for (ScanPoint2204 sp : scan_point_list)
00780   {
00781     if (sp.echo == 0 && sp.layer < 4 && !sp.ground && !sp.dirt && !sp.precipitation)
00782     {
00783       Point3DL p3d;
00784 
00785       p3d.x = sp.x_position;
00786       p3d.y = sp.y_position;
00787       p3d.z = sp.z_position;
00788       p3d.label = sp.layer;
00789       v.push_back(p3d);
00790     }
00791   }
00792 
00793   return v;
00794 }
00795 
00796 ScanData2205::ScanData2205() :
00797   IbeoTxMessage(true, false, false)
00798 {}
00799 
00800 void ScanData2205::parse(const std::vector<uint8_t>& in)
00801 {
00802   ibeo_header.parse(in);
00803 
00804   uint16_t hdr = IBEO_HEADER_SIZE;
00805 
00806   scan_start_time = read_be<NTPTime>(in, hdr);
00807   scan_end_time_offset = read_be<uint32_t>(in, hdr + 8);
00808 
00809   uint32_t flags = read_be<uint32_t>(in, hdr + 12);
00810   fused_scan = ((flags & 0x00000100) > 0);
00811   mirror_side = ((flags & 0x00000200) > 0) ? REAR : FRONT;
00812   coordinate_system = ((flags & 0x00000400) > 0) ? VEHICLE : SCANNER;
00813 
00814   scan_number = read_be<uint16_t>(in, hdr + 16);
00815   scan_points = read_be<uint16_t>(in, hdr + 18);
00816   number_of_scanner_infos = read_be<uint8_t>(in, hdr + 20);
00817 
00818   // printf("%d SCAN POINTS REPORTED\n", scan_points);
00819 
00820   for (uint8_t i = 0; i < number_of_scanner_infos; i++)
00821   {
00822     ScannerInfo2205 new_scanner_info;
00823     new_scanner_info.parse(in, hdr + 24 + (i * 148));
00824     scanner_info_list.push_back(new_scanner_info);
00825   }
00826 
00827   for (uint16_t i = 0; i < scan_points; i++)
00828   {
00829     ScanPoint2205 new_scan_point;
00830     new_scan_point.parse(in, hdr + 24 + (number_of_scanner_infos * 148) + (i * 28));
00831     scan_point_list.push_back(new_scan_point);
00832   }
00833 }
00834 
00835 std::vector<Point3DL> ScanData2205::get_scan_points()
00836 {
00837   std::vector<Point3DL> v;
00838 
00839   for (ScanPoint2205 sp : scan_point_list)
00840   {
00841     if (sp.echo == 0 && sp.layer < 4 && !sp.transparent && !sp.ground && !sp.dirt && !sp.precipitation)
00842     {
00843       Point3DL p3d;
00844 
00845       p3d.x = sp.x_position;
00846       p3d.y = sp.y_position;
00847       p3d.z = sp.z_position;
00848       p3d.label = sp.layer;
00849       v.push_back(p3d);
00850     }
00851   }
00852 
00853   return v;
00854 }
00855 
00856 ScanData2208::ScanData2208() :
00857   IbeoTxMessage(true, false, false)
00858 {}
00859 
00860 void ScanData2208::parse(const std::vector<uint8_t>& in)
00861 {
00862   ibeo_header.parse(in);
00863 
00864   uint16_t hdr = IBEO_HEADER_SIZE;
00865 
00866   scan_number = read_be<uint16_t>(in, hdr);
00867   scanner_type = read_be<uint16_t>(in, hdr + 2);
00868   uint16_t scanner_status = read_be<uint16_t>(in, hdr + 4);
00869   motor_on = ((scanner_status & 0x0001) > 0);
00870   laser_on = ((scanner_status & 0x0002) > 0);
00871   frequency_locked = ((scanner_status & 0x0004) > 0);
00872   motor_rotating_direction = ((scanner_status & 0x0100) > 0) ? COUNTER_CLOCKWISE : CLOCKWISE;
00873   angle_ticks_per_rotation = read_be<uint16_t>(in, hdr + 6);
00874   scan_flags = read_be<uint32_t>(in, hdr + 8);
00875   mounting_yaw_angle_ticks = read_be<int16_t>(in, hdr + 12);
00876   mounting_pitch_angle_ticks = read_be<int16_t>(in, hdr + 14);
00877   mounting_roll_angle_ticks = read_be<int16_t>(in, hdr + 16);
00878   mounting_position_x = read_be<int16_t>(in, hdr + 18);
00879   mounting_position_y = read_be<int16_t>(in, hdr + 20);
00880   mounting_position_z = read_be<int16_t>(in, hdr + 22);
00881   device_id = read_be<uint8_t>(in, hdr + 50);
00882   scan_start_time = read_be<NTPTime>(in, hdr + 52);
00883   scan_end_time = read_be<NTPTime>(in, hdr + 60);
00884   start_angle_ticks = read_be<int16_t>(in, hdr + 68);
00885   end_angle_ticks = read_be<int16_t>(in, hdr + 70);
00886   subflags = read_be<uint8_t>(in, hdr + 72);
00887   mirror_side = ((read_be<uint8_t>(in, hdr + 73) & 0x01) > 0) ? REAR : FRONT;
00888   mirror_tilt = read_be<int16_t>(in, hdr + 78);
00889   number_of_scan_points = read_be<uint16_t>(in, hdr + 86);
00890 
00891   for (uint16_t i = 0; i < number_of_scan_points; i++)
00892   {
00893     ScanPoint2208 new_scan_point;
00894     new_scan_point.parse(in, hdr + 88 + (i * 11));
00895     scan_point_list.push_back(new_scan_point);
00896   }
00897 }
00898 
00899 std::vector<Point3DL> ScanData2208::get_scan_points()
00900 {
00901   std::vector<Point3DL> v;
00902   double cm_to_m = 100.0;
00903 
00904   for (ScanPoint2208 sp : scan_point_list)
00905   {
00906     if (sp.echo == 0 && sp.layer < 4 && !sp.transparent_point && !sp.clutter_atmospheric && !sp.ground && !sp.dirt)
00907     {
00908       // Spherical coordinates to cartesian
00909       // This uses the following conventions (with right-hand rule applied):
00910       // phi = rotation about the Z axis (angle in radians from X = 0)
00911       // theta = angle away from Z+ (radians)
00912       // rho = radial distance (m)
00913       Point3DL p3d;
00914       double phi = ticks_to_angle(sp.horizontal_angle, angle_ticks_per_rotation);
00915       double theta;
00916       double rho = sp.radial_distance / cm_to_m;
00917 
00918       switch (sp.layer)
00919       {
00920       case 0:
00921         theta = (88.4 * (M_PI / 180.0));
00922         break;
00923       case 1:
00924         theta = (89.2 * (M_PI / 180.0));
00925         break;
00926       case 2:
00927         theta = (90.8 * (M_PI / 180.0));
00928         break;
00929       case 3:
00930         theta = (91.6 * (M_PI / 180.0));
00931         break;
00932       }
00933 
00934       p3d.x = rho * sin(theta) * cos(phi);
00935       p3d.y = rho * sin(theta) * sin(phi);
00936       p3d.z = rho * cos(theta);
00937 
00938       p3d.label = sp.layer;
00939 
00940       p3d.label = sp.layer;
00941 
00942       v.push_back(p3d);
00943     }
00944   }
00945 
00946   return v;
00947 }
00948 
00949 ObjectData2221::ObjectData2221() :
00950   IbeoTxMessage(false, true, true)
00951 {}
00952 
00953 void ObjectData2221::parse(const std::vector<uint8_t>& in)
00954 {
00955   ibeo_header.parse(in);
00956 
00957   uint16_t hdr = IBEO_HEADER_SIZE;
00958 
00959   scan_start_timestamp = read_le<NTPTime>(in, hdr);
00960   number_of_objects = read_le<uint16_t>(in, hdr + 8);
00961 
00962   uint32_t offset = hdr + 10;
00963 
00964   for (uint16_t i = 0; i < number_of_objects; i++)
00965   {
00966     Object2221 new_object;
00967     new_object.parse(in, offset);
00968     object_list.push_back(new_object);
00969     offset += 58;  // Size of object without contour points.
00970     offset += new_object.number_of_contour_points * 4;  // Size of object's contour points.
00971   }
00972 }
00973 
00974 std::vector<Point3D> ObjectData2221::get_contour_points()
00975 {
00976   std::vector<Point3D> contour_points;
00977 
00978   for (Object2221 o : object_list)
00979   {
00980     for (Point2Di p2d : o.contour_point_list)
00981     {
00982       Point3D p3d;
00983       p3d.x = static_cast<double>(p2d.x) / 100.0;
00984       p3d.y = static_cast<double>(p2d.y) / 100.0;
00985       p3d.z = 0.0;
00986       contour_points.push_back(p3d);
00987     }
00988   }
00989 
00990   return contour_points;
00991 }
00992 
00993 std::vector<IbeoObject> ObjectData2221::get_objects()
00994 {
00995   std::vector<IbeoObject> io_list;
00996 
00997   for (Object2221 o : object_list)
00998   {
00999     IbeoObject io;
01000 
01001     io.id = o.id;
01002     io.age = o.age;
01003     io.prediction_age = o.prediction_age;
01004     io.relative_timestamp = o.relative_timestamp;
01005     io.reference_point.x = o.reference_point.x / 100.0;  // In cm
01006     io.reference_point.y = o.reference_point.y / 100.0;  // In cm
01007     io.reference_point_sigma.x = o.reference_point_sigma.x / 100.0;  // In cm
01008     io.reference_point_sigma.y = o.reference_point_sigma.y / 100.0;  // In cm
01009     io.closest_point.x = o.closest_point.x / 100.0;  // In cm
01010     io.closest_point.y = o.closest_point.y / 100.0;  // In cm
01011 
01012     io.bounding_box_center.x = o.bounding_box_center.x / 100.0;  // In cm
01013     io.bounding_box_center.y = o.bounding_box_center.y / 100.0;  // In cm
01014     io.bounding_box_size.size_x = o.bounding_box_width / 100.0;  // In cm
01015     io.bounding_box_size.size_y = o.bounding_box_length / 100.0;  // In cm
01016 
01017     io.object_box_center.x = o.object_box_center.x / 100.0;  // In cm
01018     io.object_box_center.y = o.object_box_center.y / 100.0;  // In cm
01019     io.object_box_size.size_x = o.object_box_size.size_x / 100.0;  // In cm
01020     io.object_box_size.size_y = o.object_box_size.size_y / 100.0;  // In cm
01021     io.object_box_orientation = o.object_box_orientation / 100.0 * (M_PI / 180.0);  // In 1/100 degrees.
01022 
01023     io.absolute_velocity.x = o.absolute_velocity.x / 100.0;  // In cm/s
01024     io.absolute_velocity.y = o.absolute_velocity.y / 100.0;  // In cm/s
01025     io.absolute_velocity_sigma.x = o.absolute_velocity_sigma.size_x / 100.0;  // In cm/s
01026     io.absolute_velocity_sigma.y = o.absolute_velocity_sigma.size_y / 100.0;  // In cm/s
01027     io.relative_velocity.x = o.relative_velocity.x / 100.0;  // In cm/s
01028     io.relative_velocity.y = o.relative_velocity.y / 100.0;  // In cm/s
01029 
01030     io.classification = static_cast<uint16_t>(o.classification);
01031     io.classification_age = o.classification_age;
01032     io.classification_certainty = o.classification_certainty;
01033 
01034     io.number_of_contour_points = o.number_of_contour_points;
01035 
01036     io.contour_point_list = get_contour_points();
01037 
01038     io_list.push_back(io);
01039   }
01040 
01041   return io_list;
01042 }
01043 
01044 ObjectData2225::ObjectData2225() :
01045   IbeoTxMessage(false, true, true)
01046 {}
01047 
01048 void ObjectData2225::parse(const std::vector<uint8_t>& in)
01049 {
01050   ibeo_header.parse(in);
01051 
01052   uint16_t hdr = IBEO_HEADER_SIZE;
01053 
01054   mid_scan_timestamp = read_be<NTPTime>(in, hdr);
01055   number_of_objects = read_be<uint16_t>(in, hdr + 8);
01056 
01057   uint32_t offset = hdr + 10;
01058 
01059   for (uint16_t i = 0; i < number_of_objects; i++)
01060   {
01061     Object2225 new_object;
01062     new_object.parse(in, offset);
01063     object_list.push_back(new_object);
01064     offset += 132;                                      // Add size of single object without contour points.
01065     offset += new_object.number_of_contour_points * 8;  // Add size of just-parsed object's contour points.
01066   }
01067 }
01068 
01069 std::vector<Point3D> ObjectData2225::get_contour_points()
01070 {
01071   std::vector<Point3D> contour_points;
01072 
01073   for (Object2225 o : object_list)
01074   {
01075     for (Point2Df p2d : o.contour_point_list)
01076     {
01077       Point3D p3d;
01078       p3d.x = static_cast<double>(p2d.x) / 100.0;
01079       p3d.y = static_cast<double>(p2d.y) / 100.0;
01080       p3d.z = 0.0;
01081       contour_points.push_back(p3d);
01082     }
01083   }
01084 
01085   return contour_points;
01086 }
01087 
01088 std::vector<IbeoObject> ObjectData2225::get_objects()
01089 {
01090   std::vector<IbeoObject> io_list;
01091 
01092   for (Object2225 o : object_list)
01093   {
01094     IbeoObject io;
01095     io.id = o.id;
01096     io.age = o.age;
01097     io.prediction_age = 0;
01098     io.relative_timestamp = 0;
01099     io.object_coordinate_system = VEHICLE;
01100 
01101     io.classification = o.classification;
01102     io.classification_age = o.classification_age;
01103     io.classification_certainty = o.classification_certainty;
01104 
01105     io.bounding_box_center.x = o.bounding_box_center.x;
01106     io.bounding_box_center.y = o.bounding_box_center.y;
01107     io.bounding_box_size.size_x = o.bounding_box_size.x;
01108     io.bounding_box_size.size_y = o.bounding_box_size.y;
01109 
01110     io.object_box_center.x = o.object_box_center.x;
01111     io.object_box_center.y = o.object_box_center.y;
01112     io.object_box_size.size_x = o.bounding_box_size.x;
01113     io.object_box_size.size_y = o.bounding_box_size.y;
01114     io.object_box_orientation = o.yaw_angle;
01115 
01116     io.absolute_velocity.x = o.absolute_velocity.x;
01117     io.absolute_velocity.y = o.absolute_velocity.y;
01118     io.absolute_velocity_sigma.x = o.absolute_velocity_sigma.x;
01119     io.absolute_velocity_sigma.y = o.absolute_velocity_sigma.y;
01120     io.relative_velocity.x = o.relative_velocity.x;
01121     io.relative_velocity.y = o.relative_velocity.y;
01122 
01123     io.contour_point_list = get_contour_points();
01124 
01125     // In theory, this should still hold.
01126     io.closest_point.x = io.contour_point_list[o.closest_point_index].x;
01127     io.closest_point.y = io.contour_point_list[o.closest_point_index].y;
01128 
01129     io.number_of_contour_points = (uint16_t) io.contour_point_list.size();
01130     io_list.push_back(io);
01131   }
01132 
01133   return io_list;
01134 }
01135 
01136 ObjectData2270::ObjectData2270() :
01137   IbeoTxMessage(false, true, true)
01138 {}
01139 
01140 void ObjectData2270::parse(const std::vector<uint8_t>& in)
01141 {
01142   ibeo_header.parse(in);
01143 
01144   uint16_t hdr = IBEO_HEADER_SIZE;
01145 
01146   start_scan_timestamp = read_le<NTPTime>(in, hdr);
01147   object_list_number = read_le<uint16_t>(in, hdr + 8);
01148   number_of_objects = read_le<uint16_t>(in, hdr + 10);
01149 
01150   uint32_t offset = hdr + 12;
01151 
01152   for (uint16_t i = 0; i < number_of_objects; i++)
01153   {
01154     Object2270 new_object;
01155     new_object.parse(in, offset);
01156     object_list.push_back(new_object);
01157 
01158     offset += 76;                                       // Add size of single object without contour points.
01159     offset += new_object.number_of_contour_points * 4;  // Add size of just-parsed object's contour points.
01160   }
01161 }
01162 
01163 std::vector<Point3D> ObjectData2270::get_contour_points()
01164 {
01165   std::vector<Point3D> contour_points;
01166 
01167   for (Object2270 o : object_list)
01168   {
01169     for (Point2Di p2d : o.contour_point_list)
01170     {
01171       Point3D p3d;
01172       p3d.x = static_cast<double>(p2d.x) / 100.0;
01173       p3d.y = static_cast<double>(p2d.y) / 100.0;
01174       p3d.z = 0.0;
01175       contour_points.push_back(p3d);
01176     }
01177   }
01178 
01179   return contour_points;
01180 }
01181 
01182 std::vector<IbeoObject> ObjectData2270::get_objects()
01183 {
01184   std::vector<IbeoObject> io_list;
01185 
01186   for (Object2270 o : object_list)
01187   {
01188     IbeoObject io;
01189     io.id = o.id;
01190     io.age = o.age;
01191     io.prediction_age = o.prediction_age;
01192     io.relative_timestamp = 0;
01193 
01194     io.object_coordinate_system = VEHICLE;
01195 
01196     io.reference_point_location = o.reference_point_location;
01197     io.reference_point.x = o.reference_point_position_x / 100.0;  // In cm
01198     io.reference_point.y = o.reference_point_position_y / 100.0;  // In cm
01199     io.reference_point_sigma.x = o.reference_point_position_sigma_x / 100.0;  // In cm
01200     io.reference_point_sigma.y = o.reference_point_position_sigma_y / 100.0;  // In cm
01201 
01202     io.object_box_size.size_x = o.object_box_length / 100.0;  // In cm
01203     io.object_box_size.size_y = o.object_box_width / 100.0;  // In cm
01204     io.object_box_orientation = o.object_box_orientation_angle / 100.0 * (M_PI / 180.0);  // In 1/100 of a degree
01205 
01206     float half_box_x = io.object_box_size.size_x / 2.0;
01207     float half_box_y = io.object_box_size.size_y / 2.0;
01208 
01209     switch (o.reference_point_location)
01210     {
01211     case COG:
01212       io.object_box_center.x = o.contour_points_cog_x / 100.0;  // In cm
01213       io.object_box_center.y = o.contour_points_cog_y / 100.0;  // In cm
01214       break;
01215     case TOP_FRONT_LEFT_CORNER:
01216       io.object_box_center.x = io.reference_point.x - half_box_x;
01217       io.object_box_center.y = io.reference_point.y - half_box_y;
01218       break;
01219     case TOP_FRONT_RIGHT_CORNER:
01220       io.object_box_center.x = io.reference_point.x - half_box_x;
01221       io.object_box_center.y = io.reference_point.y + half_box_y;
01222       break;
01223     case BOTTOM_REAR_RIGHT_CORNER:
01224       io.object_box_center.x = io.reference_point.x + half_box_x;
01225       io.object_box_center.y = io.reference_point.y + half_box_y;
01226       break;
01227     case BOTTOM_REAR_LEFT_CORNER:
01228       io.object_box_center.x = io.reference_point.x + half_box_x;
01229       io.object_box_center.y = io.reference_point.y - half_box_y;
01230       break;
01231     case CENTER_OF_TOP_FRONT_EDGE:
01232       io.object_box_center.x = io.reference_point.x - half_box_x;
01233       io.object_box_center.y = io.reference_point.y;
01234       break;
01235     case CENTER_OF_RIGHT_EDGE:
01236       io.object_box_center.x = io.reference_point.x;
01237       io.object_box_center.y = io.reference_point.y + half_box_y;
01238       break;
01239     case CENTER_OF_BOTTOM_REAR_EDGE:
01240       io.object_box_center.x = io.reference_point.x + half_box_x;
01241       io.object_box_center.y = io.reference_point.y;
01242       break;
01243     case CENTER_OF_LEFT_EDGE:
01244       io.object_box_center.x = io.reference_point.x;
01245       io.object_box_center.y = io.reference_point.y - half_box_y;
01246       break;
01247     case BOX_CENTER:
01248     case INVALID:
01249       io.object_box_center.x = io.reference_point.x;
01250       io.object_box_center.y = io.reference_point.y;
01251       break;
01252     }
01253 
01254     io.absolute_velocity.x = o.absolute_velocity_x / 100.0;  // In cm/s
01255     io.absolute_velocity.y = o.absolute_velocity_y / 100.0;  // In cm/s
01256     io.absolute_velocity_sigma.x = o.absolute_velocity_sigma_x / 100.0;  // In cm/s
01257     io.absolute_velocity_sigma.y = o.absolute_velocity_sigma_y / 100.0;  // In cm/s
01258     io.relative_velocity.x = o.relative_velocity_x / 100.0;  // In cm/s
01259     io.relative_velocity.y = o.relative_velocity_y / 100.0;  // In cm/s
01260 
01261     io.contour_point_list = get_contour_points();
01262     io.number_of_contour_points = (uint16_t) io.contour_point_list.size();
01263 
01264     io_list.push_back(io);
01265   }
01266 
01267   return io_list;
01268 }
01269 
01270 ObjectData2271::ObjectData2271() :
01271   IbeoTxMessage(false, true, true)
01272 {}
01273 
01274 void ObjectData2271::parse(const std::vector<uint8_t>& in)
01275 {
01276   ibeo_header.parse(in);
01277 
01278   uint16_t hdr = IBEO_HEADER_SIZE;
01279 
01280   start_scan_timestamp = read_be<NTPTime>(in, hdr);
01281   scan_number = read_be<uint16_t>(in, hdr + 8);
01282   number_of_objects = read_be<uint16_t>(in, hdr + 18);
01283 
01284   uint32_t offset = hdr + 20;
01285 
01286   for (uint16_t i = 0; i < number_of_objects; i++)
01287   {
01288     Object2271 new_object;
01289     new_object.parse(in, offset);
01290     object_list.push_back(new_object);
01291 
01292     // calculate offset for the next object (i+1)
01293     offset += (118 + 4);  // Add size of single object without contour points.
01294     // Add size of just-parsed object's untracked contour points.
01295     offset += new_object.untracked_properties.number_of_contour_points * 8;
01296     // Add size of just-parsed object's tracked contour points.
01297     offset += new_object.tracked_properties.number_of_contour_points * 8;
01298   }
01299 }
01300 
01301 std::vector<Point3D> ObjectData2271::get_contour_points()
01302 {
01303   std::vector<Point3D> points;
01304 
01305   double m_to_cm = 100.0;
01306   int i = 0;
01307 
01308   for (Object2271 o : object_list)
01309   {
01310     if (o.untracked_properties_available)
01311     {
01312       for (ContourPointSigma cp : o.untracked_properties.contour_point_list)
01313       {
01314         Point3D p;
01315         p.x = cp.x / m_to_cm;
01316         p.y = cp.y / m_to_cm;
01317         points.push_back(p);
01318       }
01319     }
01320 
01321     if (o.tracked_properties_available)
01322     {
01323       for (ContourPointSigma cp : o.tracked_properties.contour_point_list)
01324       {
01325         Point3D p;
01326         p.x = cp.x / m_to_cm;
01327         p.y = cp.y / m_to_cm;
01328         p.z = 0;
01329         points.push_back(p);
01330       }
01331     }
01332 
01333     i++;
01334   }
01335 
01336   return points;
01337 }
01338 
01339 std::vector<IbeoObject> ObjectData2271::get_objects()
01340 {
01341   std::vector<IbeoObject> io_list;
01342 
01343   for (Object2271 o : object_list)
01344   {
01345     IbeoObject io;
01346     io.id = o.id;
01347     io.age = 0;
01348     io.prediction_age = 0;
01349     io.relative_timestamp = 0;
01350     io.bounding_box_size.size_x = 0;
01351     io.bounding_box_size.size_y = 0;
01352     io.object_box_orientation = 0;
01353     io.classification = UNCLASSIFIED;
01354     io.classification_age = 0;
01355     io.classification_certainty = 0;
01356 
01357     if (o.tracked_properties_available)
01358     {
01359       io.age = o.tracked_properties.object_age;
01360       io.relative_timestamp = o.tracked_properties.relative_time_of_measure;
01361 
01362       io.closest_point.x = o.tracked_properties.position_closest_point.x / 100.0;  // In cm
01363       io.closest_point.y = o.tracked_properties.position_closest_point.y / 100.0;  // In cm
01364 
01365       io.object_box_size.size_x = o.tracked_properties.object_box_size.x / 100.0;  // In cm
01366       io.object_box_size.size_y = o.tracked_properties.object_box_size.y / 100.0;  // In cm
01367       io.object_box_orientation = o.tracked_properties.object_box_orientation /
01368                                   100.0 * (M_PI / 180.0);  // In 1/100 of a degree
01369 
01370       float half_box_x = io.object_box_size.size_x / 2.0;
01371       float half_box_y = io.object_box_size.size_y / 2.0;
01372 
01373       switch (o.tracked_properties.tracking_point_location)
01374       {
01375       case TOP_FRONT_LEFT_CORNER:
01376         io.object_box_center.x = io.reference_point.x - half_box_x;
01377         io.object_box_center.y = io.reference_point.y - half_box_y;
01378         break;
01379       case TOP_FRONT_RIGHT_CORNER:
01380         io.object_box_center.x = io.reference_point.x - half_box_x;
01381         io.object_box_center.y = io.reference_point.y + half_box_y;
01382         break;
01383       case BOTTOM_REAR_RIGHT_CORNER:
01384         io.object_box_center.x = io.reference_point.x + half_box_x;
01385         io.object_box_center.y = io.reference_point.y + half_box_y;
01386         break;
01387       case BOTTOM_REAR_LEFT_CORNER:
01388         io.object_box_center.x = io.reference_point.x + half_box_x;
01389         io.object_box_center.y = io.reference_point.y - half_box_y;
01390         break;
01391       case CENTER_OF_TOP_FRONT_EDGE:
01392         io.object_box_center.x = io.reference_point.x - half_box_x;
01393         io.object_box_center.y = io.reference_point.y;
01394         break;
01395       case CENTER_OF_RIGHT_EDGE:
01396         io.object_box_center.x = io.reference_point.x;
01397         io.object_box_center.y = io.reference_point.y + half_box_y;
01398         break;
01399       case CENTER_OF_BOTTOM_REAR_EDGE:
01400         io.object_box_center.x = io.reference_point.x + half_box_x;
01401         io.object_box_center.y = io.reference_point.y;
01402         break;
01403       case CENTER_OF_LEFT_EDGE:
01404         io.object_box_center.x = io.reference_point.x;
01405         io.object_box_center.y = io.reference_point.y - half_box_y;
01406         break;
01407       case COG:
01408       case BOX_CENTER:
01409       case INVALID:
01410         io.object_box_center.x = io.reference_point.x;
01411         io.object_box_center.y = io.reference_point.y;
01412         break;
01413       }
01414 
01415       io.absolute_velocity.x = o.tracked_properties.velocity.x / 100.0;  // In cm/s
01416       io.absolute_velocity.y = o.tracked_properties.velocity.y / 100.0;  // In cm/s
01417       io.absolute_velocity_sigma.x = o.tracked_properties.velocity_sigma.x / 100.0;  // In cm/s
01418       io.absolute_velocity_sigma.y = o.tracked_properties.velocity_sigma.y / 100.0;  // In cm/s
01419       io.relative_velocity.x = o.tracked_properties.relative_velocity.x / 100.0;  // In cm/s
01420       io.relative_velocity.y = o.tracked_properties.relative_velocity.y / 100.0;  // In cm/s
01421 
01422       io.classification = o.tracked_properties.classification;
01423       io.classification_age = o.tracked_properties.classification_age;
01424     }
01425 
01426     if (o.untracked_properties_available)
01427     {
01428       io.relative_timestamp = o.untracked_properties.relative_time_of_measurement;
01429 
01430       io.closest_point.x = o.untracked_properties.position_closest_point.x / 100.0;  // In cm
01431       io.closest_point.y = o.untracked_properties.position_closest_point.y / 100.0;  // In cm
01432 
01433       io.object_box_size.size_x = o.untracked_properties.object_box_size.x / 100.0;  // In cm
01434       io.object_box_size.size_y = o.untracked_properties.object_box_size.y / 100.0;  // In cm
01435       io.object_box_orientation = o.untracked_properties.object_box_orientation /
01436                                   100.0 * (M_PI / 180.0);  // In 1/100 of a degree
01437 
01438       io.object_box_center.x = o.untracked_properties.tracking_point_coordinate.x / 100.0;  // In cm
01439       io.object_box_center.y = o.untracked_properties.tracking_point_coordinate.y / 100.0;  // In cm
01440 
01441       io.reference_point.x = o.untracked_properties.tracking_point_coordinate.x / 100.0;  // In cm
01442       io.reference_point.y = o.untracked_properties.tracking_point_coordinate.y / 100.0;  // In cm
01443 
01444       io.reference_point_sigma.x = o.untracked_properties.tracking_point_coordinate_sigma.x / 100.0;  // In cm
01445       io.reference_point_sigma.y = o.untracked_properties.tracking_point_coordinate_sigma.y / 100.0;  // In cm
01446     }
01447 
01448     io.contour_point_list = get_contour_points();
01449 
01450     io.number_of_contour_points = (uint16_t) io.contour_point_list.size();
01451     io_list.push_back(io);
01452   }
01453 
01454   return io_list;
01455 }
01456 
01457 ObjectData2280::ObjectData2280() :
01458   IbeoTxMessage(false, true, true)
01459 {}
01460 
01461 void ObjectData2280::parse(const std::vector<uint8_t>& in)
01462 {
01463   ibeo_header.parse(in);
01464 
01465   uint16_t hdr = IBEO_HEADER_SIZE;
01466 
01467   mid_scan_timestamp = read_be<NTPTime>(in, hdr);
01468   number_of_objects = read_be<uint16_t>(in, hdr + 8);
01469 
01470   uint32_t offset = hdr + 10;
01471 
01472   for (uint16_t i = 0; i < number_of_objects; i++)
01473   {
01474     Object2280 new_object;
01475     new_object.parse(in, offset);
01476 
01477     object_list.push_back(new_object);
01478 
01479     offset += 168;                                      // Add size of single object without contour points.
01480     offset += new_object.number_of_contour_points * 8;  // Add size of just-parsed object's contour points.
01481   }
01482 }
01483 
01484 std::vector<Point3D> ObjectData2280::get_contour_points()
01485 {
01486   std::vector<Point3D> contour_points;
01487 
01488   for (Object2280 o : object_list)
01489   {
01490     for (Point2Df p2d : o.contour_point_list)
01491     {
01492       if (!std::isnan(p2d.x) && !std::isnan(p2d.y) && fabs(p2d.x) < 300.0 && fabs(p2d.y) < 300.0)
01493       {
01494         Point3D p3d;
01495         p3d.x = static_cast<double>(p2d.x);
01496         p3d.y = static_cast<double>(p2d.y);
01497         p3d.z = 0.0;
01498         contour_points.push_back(p3d);
01499       }
01500     }
01501   }
01502 
01503   return contour_points;
01504 }
01505 
01506 std::vector<IbeoObject> ObjectData2280::get_objects()
01507 {
01508   std::vector<IbeoObject> io_list;
01509 
01510   for (Object2280 o : object_list)
01511   {
01512     IbeoObject io;
01513     io.id = o.id;
01514     io.age = o.object_age;
01515     io.prediction_age = o.object_prediction_age;
01516     io.classification = o.classification;
01517     io.classification_age = o.classification_age;
01518     io.classification_certainty = o.classification_certainty;
01519 
01520     io.relative_timestamp = 0;
01521     io.bounding_box_center.x = o.object_box_center.x;
01522     io.bounding_box_center.y = o.object_box_center.y;
01523     io.bounding_box_size.size_x = o.object_box_size.x;
01524     io.bounding_box_size.size_y = o.object_box_size.y;
01525 
01526     io.object_box_center.x = o.object_box_center.x;
01527     io.object_box_center.y = o.object_box_center.y;
01528     io.object_box_size.size_x = o.object_box_size.x;
01529     io.object_box_size.size_y = o.object_box_size.y;
01530     io.object_box_orientation = o.object_box_orientation_angle;
01531 
01532     io.absolute_velocity.x = o.absolute_velocity.x;
01533     io.absolute_velocity.y = o.absolute_velocity.y;
01534 
01535     io.absolute_velocity_sigma.x = o.absolute_velocity_sigma.x;
01536     io.absolute_velocity_sigma.y = o.absolute_velocity_sigma.y;
01537 
01538     io.relative_velocity.x = o.relative_velocity.x;
01539     io.relative_velocity.y = o.relative_velocity.y;
01540 
01541     io.contour_point_list = get_contour_points();
01542     io.number_of_contour_points = o.number_of_contour_points;
01543 
01544     io.closest_point.x = io.contour_point_list[o.closest_point_index].x;
01545     io.closest_point.y = io.contour_point_list[o.closest_point_index].y;
01546 
01547     io_list.push_back(io);
01548   }
01549 
01550   return io_list;
01551 }
01552 
01553 CameraImage::CameraImage() :
01554   IbeoTxMessage()
01555 {}
01556 
01557 void CameraImage::parse(const std::vector<uint8_t>& in)
01558 {
01559   ibeo_header.parse(in);
01560 
01561   uint16_t hdr = IBEO_HEADER_SIZE;
01562 
01563   image_format = static_cast<ImageFormat>(read_be<uint16_t>(in, hdr));
01564   us_since_power_on = read_be<uint32_t>(in, hdr + 2);
01565   timestamp = read_be<NTPTime>(in, hdr + 6);
01566   device_id = read_be<uint8_t>(in, hdr + 14);
01567   mounting_position.parse(in, hdr + 15);
01568   horizontal_opening_angle = read_be<double>(in, hdr + 39);
01569   vertical_opening_angle = read_be<double>(in, hdr + 47);
01570   image_width = read_be<uint16_t>(in, hdr + 55);
01571   image_height = read_be<uint16_t>(in, hdr + 57);
01572   compressed_size = read_be<uint32_t>(in, hdr + 59);
01573 
01574   for (uint32_t i = 0; i < compressed_size; i++)
01575   {
01576     image_buffer.push_back(in[hdr + 63 + i]);
01577   }
01578 }
01579 
01580 HostVehicleState2805::HostVehicleState2805() :
01581   IbeoTxMessage()
01582 {}
01583 
01584 void HostVehicleState2805::parse(const std::vector<uint8_t>& in)
01585 {
01586   ibeo_header.parse(in);
01587 
01588   uint16_t hdr = IBEO_HEADER_SIZE;
01589 
01590   timestamp = read_le<NTPTime>(in, hdr);
01591   scan_number = read_le<uint16_t>(in, hdr + 8);
01592   error_flags = read_le<uint16_t>(in, hdr + 10);
01593   longitudinal_velocity = read_le<int16_t>(in, hdr + 12);
01594   steering_wheel_angle = read_le<int16_t>(in, hdr + 14);
01595   front_wheel_angle = read_le<int16_t>(in, hdr + 16);
01596   x_position = read_le<int32_t>(in, hdr + 20);
01597   y_position = read_le<int32_t>(in, hdr + 24);
01598   course_angle = read_le<int16_t>(in, hdr + 28);
01599   time_difference = read_le<uint16_t>(in, hdr + 30);
01600   x_difference = read_le<int16_t>(in, hdr + 32);
01601   y_difference = read_le<int16_t>(in, hdr + 34);
01602   heading_difference = read_le<int16_t>(in, hdr + 36);
01603   current_yaw_rate = read_le<int16_t>(in, hdr + 40);
01604 }
01605 
01606 HostVehicleState2806::HostVehicleState2806() :
01607   IbeoTxMessage()
01608 {}
01609 
01610 void HostVehicleState2806::parse(const std::vector<uint8_t>& in)
01611 {
01612   ibeo_header.parse(in);
01613 
01614   uint16_t hdr = IBEO_HEADER_SIZE;
01615 
01616   timestamp = read_le<NTPTime>(in, hdr + 4);
01617   distance_x = read_le<int32_t>(in, hdr + 12);
01618   distance_y = read_le<int32_t>(in, hdr + 16);
01619   course_angle = read_le<float>(in, hdr + 24);
01620   longitudinal_velocity = read_le<float>(in, hdr + 28);
01621   yaw_rate = read_le<float>(in, hdr + 24);
01622   steering_wheel_angle = read_le<float>(in, hdr + 32);
01623   cross_acceleration = read_le<float>(in, hdr + 36);
01624   front_wheel_angle = read_le<float>(in, hdr + 40);
01625   vehicle_width = read_le<float>(in, hdr + 46);
01626   vehicle_front_to_front_axle = read_le<float>(in, hdr + 54);
01627   rear_axle_to_front_axle = read_le<float>(in, hdr + 58);
01628   rear_axle_to_vehicle_rear = read_le<float>(in, hdr + 62);
01629   steer_ratio_poly_0 = read_le<float>(in, hdr + 70);
01630   steer_ratio_poly_1 = read_le<float>(in, hdr + 74);
01631   steer_ratio_poly_2 = read_le<float>(in, hdr + 78);
01632   steer_ratio_poly_3 = read_le<float>(in, hdr + 82);
01633 }
01634 
01635 HostVehicleState2807::HostVehicleState2807() :
01636   HostVehicleState2806()
01637 {}
01638 
01639 void HostVehicleState2807::parse(const std::vector<uint8_t>& in)
01640 {
01641   ibeo_header.parse(in);
01642 
01643   uint16_t hdr = IBEO_HEADER_SIZE;
01644 
01645   timestamp = read_le<NTPTime>(in, hdr + 4);
01646   distance_x = read_le<int32_t>(in, hdr + 12);
01647   distance_y = read_le<int32_t>(in, hdr + 16);
01648   course_angle = read_le<float>(in, hdr + 24);
01649   longitudinal_velocity = read_le<float>(in, hdr + 28);
01650   yaw_rate = read_le<float>(in, hdr + 24);
01651   steering_wheel_angle = read_le<float>(in, hdr + 32);
01652   cross_acceleration = read_le<float>(in, hdr + 36);
01653   front_wheel_angle = read_le<float>(in, hdr + 40);
01654   vehicle_width = read_le<float>(in, hdr + 46);
01655   vehicle_front_to_front_axle = read_le<float>(in, hdr + 54);
01656   rear_axle_to_front_axle = read_le<float>(in, hdr + 58);
01657   rear_axle_to_vehicle_rear = read_le<float>(in, hdr + 62);
01658   steer_ratio_poly_0 = read_le<float>(in, hdr + 70);
01659   steer_ratio_poly_1 = read_le<float>(in, hdr + 74);
01660   steer_ratio_poly_2 = read_le<float>(in, hdr + 78);
01661   steer_ratio_poly_3 = read_le<float>(in, hdr + 82);
01662 
01663   longitudinal_acceleration = read_le<float>(in, hdr + 86);
01664 }
01665 
01666 DeviceStatus::DeviceStatus() :
01667   IbeoTxMessage()
01668 {}
01669 
01670 void DeviceStatus::parse(const std::vector<uint8_t>& in)
01671 {
01672   ibeo_header.parse(in);
01673 
01674   uint16_t hdr = IBEO_HEADER_SIZE;
01675 
01676   scanner_type = read_le<uint8_t>(in, hdr + 6);
01677   sensor_temperature = read_le<float>(in, hdr + 36);
01678   frequency = read_le<float>(in, hdr + 40);
01679 }
01680 
01681 void CommandSetFilter::encode()
01682 {
01683   ibeo_header.message_size = 8;
01684   ibeo_header.data_type_id = 0x2010;
01685 
01686   struct timeval tv;
01687   struct tm *tm;
01688   gettimeofday(&tv, NULL);
01689   tm = localtime(&tv.tv_sec);  // NOLINT
01690   ibeo_header.time = unix_time_to_ntp(tm, &tv);
01691   ibeo_header.encode();
01692 
01693   encoded_data.insert(encoded_data.end(), ibeo_header.encoded_data.begin(), ibeo_header.encoded_data.end());
01694 
01695   command_identifier = 0x0005;
01696   version = 0x0002;
01697   begin_filter_range = 0x0000;
01698   end_filter_range = 0xFFFF;
01699 
01700   std::vector<uint8_t> encoded_command_identifier = write_be<uint16_t>(&command_identifier);
01701   std::vector<uint8_t> encoded_version = write_be<uint16_t>(&version);
01702   std::vector<uint8_t> encoded_begin_filter_range = write_be<uint16_t>(&begin_filter_range);
01703   std::vector<uint8_t> encoded_end_filter_range = write_be<uint16_t>(&end_filter_range);
01704 
01705   encoded_data.insert(encoded_data.end(), encoded_command_identifier.begin(), encoded_command_identifier.end());
01706   encoded_data.insert(encoded_data.end(), encoded_version.begin(), encoded_version.end());
01707   encoded_data.insert(encoded_data.end(), encoded_begin_filter_range.begin(), encoded_begin_filter_range.end());
01708   encoded_data.insert(encoded_data.end(), encoded_end_filter_range.begin(), encoded_end_filter_range.end());
01709 }


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