fix(velodyne_decoder): overflow handling in vls128 (#111)

* fix: overflow handling in vls128

The timestamps in the vls128 were still relative to the last pointcloud, which was producing some errors
Additionally, when entire scans were dropped, the overflow was not being handled correctly (this bug was also present in the awf velodyne driver)

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* chore: fixed spelling

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* fix: relative time was in seconds

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* fix: timestamp handling and overflow order. error happen then the computed time is higher than the next measured one...

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* Update nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp

Co-authored-by: Benjamin Gilby <40575701+bgilby59@users.noreply.github.com>

* Update nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp

Co-authored-by: Benjamin Gilby <40575701+bgilby59@users.noreply.github.com>

---------

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>
Co-authored-by: Benjamin Gilby <40575701+bgilby59@users.noreply.github.com>

nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp

@@ -185,9 +185,9 @@ class VelodyneScanDecoder
   virtual std::tuple<drivers::NebulaPointCloudPtr, double> get_pointcloud() = 0;
   /// @brief Resetting point cloud buffer
   /// @param n_pts # of points
-  virtual void reset_pointcloud(size_t n_pts) = 0;
+  virtual void reset_pointcloud(size_t n_pts, double time_stamp) = 0;
   /// @brief Resetting overflowed point cloud buffer
-  virtual void reset_overflow() = 0;
+  virtual void reset_overflow(double time_stamp) = 0;
 };
 
 }  // namespace drivers

nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp

@@ -13,7 +13,7 @@ namespace drivers
 {
 namespace vlp16
 {
-  constexpr uint32_t MAX_POINTS = 300000;
+constexpr uint32_t MAX_POINTS = 300000;
 /// @brief Velodyne LiDAR decoder (VLP16)
 class Vlp16Decoder : public VelodyneScanDecoder
 {
@@ -38,9 +38,9 @@ class Vlp16Decoder : public VelodyneScanDecoder
   std::tuple<drivers::NebulaPointCloudPtr, double> get_pointcloud() override;
   /// @brief Resetting point cloud buffer
   /// @param n_pts # of points
-  void reset_pointcloud(size_t n_pts) override;
+  void reset_pointcloud(size_t n_pts, double time_stamp) override;
   /// @brief Resetting overflowed point cloud buffer
-  void reset_overflow() override;
+  void reset_overflow(double time_stamp) override;
 
 private:
   /// @brief Parsing VelodynePacket based on packet structure

nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp

@@ -37,9 +37,9 @@ class Vlp32Decoder : public VelodyneScanDecoder
   std::tuple<drivers::NebulaPointCloudPtr, double> get_pointcloud() override;
   /// @brief Resetting point cloud buffer
   /// @param n_pts # of points
-  void reset_pointcloud(size_t n_pts) override;
+  void reset_pointcloud(size_t n_pts, double time_stamp) override;
   /// @brief Resetting overflowed point cloud buffer
-  void reset_overflow() override;
+  void reset_overflow(double time_stamp) override;
 
 private:
   /// @brief Parsing VelodynePacket based on packet structure

nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp

@@ -37,9 +37,9 @@ class Vls128Decoder : public VelodyneScanDecoder
   std::tuple<drivers::NebulaPointCloudPtr, double> get_pointcloud() override;
   /// @brief Resetting point cloud buffer
   /// @param n_pts # of points
-  void reset_pointcloud(size_t n_pts) override;
+  void reset_pointcloud(size_t n_pts, double time_stamp) override;
   /// @brief Resetting overflowed point cloud buffer
-  void reset_overflow() override;
+  void reset_overflow(double time_stamp) override;
 
 private:
   /// @brief Parsing VelodynePacket based on packet structure
@@ -52,6 +52,7 @@ class Vls128Decoder : public VelodyneScanDecoder
   float vls_128_laser_azimuth_cache_[16];
   int phase_;
   int max_pts_;
+  double last_block_timestamp_;
   std::vector<std::vector<float>> timing_offsets_;
 };
 

nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp

@@ -30,43 +30,48 @@ Vlp16Decoder::Vlp16Decoder(
   }
   // timing table calculation, from velodyne user manual p.64
   timing_offsets_.resize(BLOCKS_PER_PACKET);
-  for (size_t i=0; i < timing_offsets_.size(); ++i){
+  for (size_t i = 0; i < timing_offsets_.size(); ++i) {
     timing_offsets_[i].resize(32);
   }
   double full_firing_cycle_s = 55.296 * 1e-6;
   double single_firing_s = 2.304 * 1e-6;
   double data_block_index, data_point_index;
-  bool dual_mode = sensor_configuration_->return_mode==ReturnMode::DUAL;
+  bool dual_mode = sensor_configuration_->return_mode == ReturnMode::DUAL;
   // compute timing offsets
-  for (size_t x = 0; x < timing_offsets_.size(); ++x){
-    for (size_t y = 0; y < timing_offsets_[x].size(); ++y){
-      if (dual_mode){
+  for (size_t x = 0; x < timing_offsets_.size(); ++x) {
+    for (size_t y = 0; y < timing_offsets_[x].size(); ++y) {
+      if (dual_mode) {
         data_block_index = (x - (x % 2)) + (y / 16);
-      }
-      else{
+      } else {
         data_block_index = (x * 2) + (y / 16);
       }
       data_point_index = y % 16;
-      timing_offsets_[x][y] = (full_firing_cycle_s * data_block_index) + (single_firing_s * data_point_index);
+      timing_offsets_[x][y] =
+        (full_firing_cycle_s * data_block_index) + (single_firing_s * data_point_index);
     }
   }
 
   phase_ = (uint16_t)round(sensor_configuration_->scan_phase * 100);
 }
 
-bool Vlp16Decoder::hasScanned() { return has_scanned_; }
+bool Vlp16Decoder::hasScanned()
+{
+  return has_scanned_;
+}
 
 std::tuple<drivers::NebulaPointCloudPtr, double> Vlp16Decoder::get_pointcloud()
 {
   double phase = angles::from_degrees(sensor_configuration_->scan_phase);
   if (!scan_pc_->points.empty()) {
     auto current_azimuth = scan_pc_->points.back().azimuth;
-    auto phase_diff = static_cast<size_t>(angles::to_degrees(2*M_PI + current_azimuth - phase)) % 360;
+    auto phase_diff =
+      static_cast<size_t>(angles::to_degrees(2 * M_PI + current_azimuth - phase)) % 360;
     while (phase_diff < M_PI_2 && !scan_pc_->points.empty()) {
       overflow_pc_->points.push_back(scan_pc_->points.back());
       scan_pc_->points.pop_back();
       current_azimuth = scan_pc_->points.back().azimuth;
-      phase_diff = static_cast<size_t>(angles::to_degrees(2*M_PI + current_azimuth - phase)) % 360;
+      phase_diff =
+        static_cast<size_t>(angles::to_degrees(2 * M_PI + current_azimuth - phase)) % 360;
     }
     overflow_pc_->width = overflow_pc_->points.size();
     scan_pc_->width = scan_pc_->points.size();
@@ -80,16 +85,16 @@ int Vlp16Decoder::pointsPerPacket()
   return BLOCKS_PER_PACKET * VLP16_FIRINGS_PER_BLOCK * VLP16_SCANS_PER_FIRING;
 }
 
-void Vlp16Decoder::reset_pointcloud(size_t n_pts)
+void Vlp16Decoder::reset_pointcloud(size_t n_pts, [[maybe_unused]] double time_stamp)
 {
   scan_pc_->points.clear();
   max_pts_ = n_pts * pointsPerPacket();
   scan_pc_->points.reserve(max_pts_);
-  reset_overflow();  // transfer existing overflow points to the cleared pointcloud
+  reset_overflow(time_stamp);  // transfer existing overflow points to the cleared pointcloud
   scan_timestamp_ = -1;
 }
 
-void Vlp16Decoder::reset_overflow()
+void Vlp16Decoder::reset_overflow([[maybe_unused]] double time_stamp)
 {
   // Add the overflow buffer points
   for (size_t i = 0; i < overflow_pc_->points.size(); i++) {
@@ -225,7 +230,7 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
                 const float z_coord = distance * sin_vert_angle;       // velodyne z
                 const uint8_t intensity = current_block.data[k + 2];
 
-                double point_time_offset =  timing_offsets_[block][firing * 16 + dsr];
+                double point_time_offset = timing_offsets_[block][firing * 16 + dsr];
 
                 // Determine return type.
                 uint8_t return_type;
@@ -237,9 +242,10 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
                        other_return.bytes[1] == current_return.bytes[1])) {
                       return_type = static_cast<uint8_t>(drivers::ReturnType::IDENTICAL);
                     } else {
-                      const uint8_t other_intensity = block % 2 ? raw->blocks[block - 1].data[k + 2]
-                                                              : raw->blocks[block + 1].data[k + 2];
-                      bool first = current_return.uint > other_return.uint ;
+                      const uint8_t other_intensity = block % 2
+                                                        ? raw->blocks[block - 1].data[k + 2]
+                                                        : raw->blocks[block + 1].data[k + 2];
+                      bool first = current_return.uint > other_return.uint;
                       bool strongest = intensity > other_intensity;
                       if (other_intensity == intensity) {
                         strongest = !first;
@@ -275,9 +281,8 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
                 current_point.azimuth = rotation_radians_[azimuth_corrected];
                 current_point.elevation = sin_vert_angle;
                 auto point_ts = block_timestamp - scan_timestamp_ + point_time_offset;
-                if (point_ts < 0)
-                  point_ts = 0;
-                current_point.time_stamp = static_cast<uint32_t>(point_ts*1e9);
+                if (point_ts < 0) point_ts = 0;
+                current_point.time_stamp = static_cast<uint32_t>(point_ts * 1e9);
                 current_point.intensity = intensity;
                 current_point.distance = distance;
                 scan_pc_->points.emplace_back(current_point);

nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp

@@ -31,42 +31,45 @@ Vlp32Decoder::Vlp32Decoder(
   phase_ = (uint16_t)round(sensor_configuration_->scan_phase * 100);
 
   timing_offsets_.resize(12);
-  for (size_t i=0; i < timing_offsets_.size(); ++i){
+  for (size_t i = 0; i < timing_offsets_.size(); ++i) {
     timing_offsets_[i].resize(32);
   }
   // constants
-  double full_firing_cycle = 55.296 * 1e-6; // seconds
-  double single_firing = 2.304 * 1e-6; // seconds
+  double full_firing_cycle = 55.296 * 1e-6;  // seconds
+  double single_firing = 2.304 * 1e-6;       // seconds
   double dataBlockIndex, dataPointIndex;
   bool dual_mode = sensor_configuration_->return_mode == ReturnMode::DUAL;
   // compute timing offsets
-  for (size_t x = 0; x < timing_offsets_.size(); ++x){
-    for (size_t y = 0; y < timing_offsets_[x].size(); ++y){
-      if (dual_mode){
+  for (size_t x = 0; x < timing_offsets_.size(); ++x) {
+    for (size_t y = 0; y < timing_offsets_[x].size(); ++y) {
+      if (dual_mode) {
         dataBlockIndex = x / 2;
-      }
-      else{
+      } else {
         dataBlockIndex = x;
       }
       dataPointIndex = y / 2;
-      timing_offsets_[x][y] = (full_firing_cycle * dataBlockIndex) + (single_firing * dataPointIndex);
+      timing_offsets_[x][y] =
+        (full_firing_cycle * dataBlockIndex) + (single_firing * dataPointIndex);
     }
   }
 }
 
-bool Vlp32Decoder::hasScanned() { return has_scanned_; }
+bool Vlp32Decoder::hasScanned()
+{
+  return has_scanned_;
+}
 
 std::tuple<drivers::NebulaPointCloudPtr, double> Vlp32Decoder::get_pointcloud()
 {
   double phase = angles::from_degrees(sensor_configuration_->scan_phase);
   if (!scan_pc_->points.empty()) {
     auto current_azimuth = scan_pc_->points.back().azimuth;
-    auto phase_diff = (2*M_PI + current_azimuth - phase);
+    auto phase_diff = (2 * M_PI + current_azimuth - phase);
     while (phase_diff < M_PI_2 && !scan_pc_->points.empty()) {
       overflow_pc_->points.push_back(scan_pc_->points.back());
       scan_pc_->points.pop_back();
       current_azimuth = scan_pc_->points.back().azimuth;
-      phase_diff = (2*M_PI + current_azimuth - phase);
+      phase_diff = (2 * M_PI + current_azimuth - phase);
     }
     overflow_pc_->width = overflow_pc_->points.size();
     scan_pc_->width = scan_pc_->points.size();
@@ -75,19 +78,22 @@ std::tuple<drivers::NebulaPointCloudPtr, double> Vlp32Decoder::get_pointcloud()
   return std::make_tuple(scan_pc_, scan_timestamp_);
 }
 
-int Vlp32Decoder::pointsPerPacket() { return BLOCKS_PER_PACKET * SCANS_PER_BLOCK; }
+int Vlp32Decoder::pointsPerPacket()
+{
+  return BLOCKS_PER_PACKET * SCANS_PER_BLOCK;
+}
 
-void Vlp32Decoder::reset_pointcloud(size_t n_pts)
+void Vlp32Decoder::reset_pointcloud(size_t n_pts, [[maybe_unused]] double time_stamp)
 {
   //  scan_pc_.reset(new NebulaPointCloud);
   scan_pc_->points.clear();
   max_pts_ = n_pts * pointsPerPacket();
   scan_pc_->points.reserve(max_pts_);
-  reset_overflow();  // transfer existing overflow points to the cleared pointcloud
+  reset_overflow(time_stamp);  // transfer existing overflow points to the cleared pointcloud
   scan_timestamp_ = -1;
 }
 
-void Vlp32Decoder::reset_overflow()
+void Vlp32Decoder::reset_overflow([[maybe_unused]] double time_stamp)
 {
   // Add the overflow buffer points
   for (size_t i = 0; i < overflow_pc_->points.size(); i++) {
@@ -302,9 +308,8 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
           current_point.azimuth = rotation_radians_[block.rotation];
           current_point.elevation = sin_vert_angle;
           auto point_ts = block_timestamp - scan_timestamp_ + point_time_offset;
-          if (point_ts < 0)
-            point_ts = 0;
-          current_point.time_stamp = static_cast<uint32_t>(point_ts*1e9);
+          if (point_ts < 0) point_ts = 0;
+          current_point.time_stamp = static_cast<uint32_t>(point_ts * 1e9);
           current_point.distance = distance;
           current_point.intensity = intensity;
           scan_pc_->points.emplace_back(current_point);