fix(velodyne_decoder): vlp16 and vlp32 overflow handling (#113)

* fix: overflow handling in VLP16 and VLP32

* add definition and initialization of last_block_timestamp_

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

Co-authored-by: Kenzo Lobos Tsunekawa <kenzo.lobos@tier4.jp>

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

Co-authored-by: Kenzo Lobos Tsunekawa <kenzo.lobos@tier4.jp>

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

Co-authored-by: Kenzo Lobos Tsunekawa <kenzo.lobos@tier4.jp>

* add missing return statements to vlp16 and vls128 decoders

* remove superfluous code

---------

Co-authored-by: Kenzo Lobos Tsunekawa <kenzo.lobos@tier4.jp>

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

@@ -6,6 +6,9 @@
 #include <velodyne_msgs/msg/velodyne_scan.hpp>
 
 #include <array>
+#include <memory>
+#include <tuple>
+#include <vector>
 
 namespace nebula
 {
@@ -52,6 +55,7 @@ class Vlp16Decoder : public VelodyneScanDecoder
   float rotation_radians_[ROTATION_MAX_UNITS];
   int phase_;
   int max_pts_;
+  double last_block_timestamp_;
   std::vector<std::vector<float>> timing_offsets_;
 };
 

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

@@ -6,6 +6,9 @@
 #include <velodyne_msgs/msg/velodyne_scan.hpp>
 
 #include <array>
+#include <memory>
+#include <tuple>
+#include <vector>
 
 namespace nebula
 {
@@ -52,6 +55,7 @@ class Vlp32Decoder : public VelodyneScanDecoder
   std::vector<std::vector<float>> timing_offsets_;
   int phase_;
   int max_pts_;
+  double last_block_timestamp_;
 };
 
 }  // namespace vlp32

nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp

@@ -85,21 +85,53 @@ int Vlp16Decoder::pointsPerPacket()
   return BLOCKS_PER_PACKET * VLP16_FIRINGS_PER_BLOCK * VLP16_SCANS_PER_FIRING;
 }
 
-void Vlp16Decoder::reset_pointcloud(size_t n_pts, [[maybe_unused]] double time_stamp)
+void Vlp16Decoder::reset_pointcloud(size_t n_pts, double time_stamp)
 {
   scan_pc_->points.clear();
   max_pts_ = n_pts * pointsPerPacket();
   scan_pc_->points.reserve(max_pts_);
   reset_overflow(time_stamp);  // transfer existing overflow points to the cleared pointcloud
-  scan_timestamp_ = -1;
 }
 
-void Vlp16Decoder::reset_overflow([[maybe_unused]] double time_stamp)
+void Vlp16Decoder::reset_overflow(double time_stamp)
 {
+  if (overflow_pc_->points.size() == 0) {
+    scan_timestamp_ = -1;
+    overflow_pc_->points.reserve(max_pts_);
+    return;
+  }
+
+  // Compute the absolute time stamp of the last point of the overflow pointcloud
+  const double last_overflow_time_stamp =
+    scan_timestamp_ + 1e-9 * overflow_pc_->points.back().time_stamp;
+
+  // Detect cases where there is an unacceptable time difference between the last overflow point and
+  // the first point of the next packet. In that case, there was probably a packet drop so it is
+  // better to ignore the overflow pointcloud
+  if (time_stamp - last_overflow_time_stamp > 0.05) {
+    scan_timestamp_ = -1;
+    overflow_pc_->points.clear();
+    overflow_pc_->points.reserve(max_pts_);
+    return;
+  }
+
   // Add the overflow buffer points
-  for (size_t i = 0; i < overflow_pc_->points.size(); i++) {
-    scan_pc_->points.emplace_back(overflow_pc_->points[i]);
+  while (overflow_pc_->points.size() > 0) {
+    auto overflow_point = overflow_pc_->points.back();
+
+    // The overflow points had the stamps from the previous pointcloud. These need to be changed to
+    // be relative to the overflow's packet timestamp
+    double new_timestamp_seconds =
+      scan_timestamp_ + 1e-9 * overflow_point.time_stamp - last_block_timestamp_;
+    overflow_point.time_stamp =
+      static_cast<uint32_t>(new_timestamp_seconds < 0.0 ? 0.0 : 1e9 * new_timestamp_seconds);
+
+    scan_pc_->points.emplace_back(overflow_point);
+    overflow_pc_->points.pop_back();
   }
+
+  // When there is overflow, the timestamp becomes the overflow packets' one
+  scan_timestamp_ = last_block_timestamp_;
   overflow_pc_->points.clear();
   overflow_pc_->points.reserve(max_pts_);
 }
@@ -230,6 +262,8 @@ 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];
 
+                last_block_timestamp_ = block_timestamp;
+
                 double point_time_offset = timing_offsets_[block][firing * 16 + dsr];
 
                 // Determine return type.

nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp

@@ -83,22 +83,54 @@ int Vlp32Decoder::pointsPerPacket()
   return BLOCKS_PER_PACKET * SCANS_PER_BLOCK;
 }
 
-void Vlp32Decoder::reset_pointcloud(size_t n_pts, [[maybe_unused]] double time_stamp)
+void Vlp32Decoder::reset_pointcloud(size_t n_pts, 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(time_stamp);  // transfer existing overflow points to the cleared pointcloud
-  scan_timestamp_ = -1;
 }
 
-void Vlp32Decoder::reset_overflow([[maybe_unused]] double time_stamp)
+void Vlp32Decoder::reset_overflow(double time_stamp)
 {
+  if (overflow_pc_->points.size() == 0) {
+    scan_timestamp_ = -1;
+    overflow_pc_->points.reserve(max_pts_);
+    return;
+  }
+
+  // Compute the absolute time stamp of the last point of the overflow pointcloud
+  const double last_overflow_time_stamp =
+    scan_timestamp_ + 1e-9 * overflow_pc_->points.back().time_stamp;
+
+  // Detect cases where there is an unacceptable time difference between the last overflow point and
+  // the first point of the next packet. In that case, there was probably a packet drop so it is
+  // better to ignore the overflow pointcloud
+  if (time_stamp - last_overflow_time_stamp > 0.05) {
+    scan_timestamp_ = -1;
+    overflow_pc_->points.clear();
+    overflow_pc_->points.reserve(max_pts_);
+    return;
+  }
+
   // Add the overflow buffer points
-  for (size_t i = 0; i < overflow_pc_->points.size(); i++) {
-    scan_pc_->points.emplace_back(overflow_pc_->points[i]);
+  while (overflow_pc_->points.size() > 0) {
+    auto overflow_point = overflow_pc_->points.back();
+
+    // The overflow points had the stamps from the previous pointcloud. These need to be changed to
+    // be relative to the overflow's packet timestamp
+    double new_timestamp_seconds =
+      scan_timestamp_ + 1e-9 * overflow_point.time_stamp - last_block_timestamp_;
+    overflow_point.time_stamp =
+      static_cast<uint32_t>(new_timestamp_seconds < 0.0 ? 0.0 : 1e9 * new_timestamp_seconds);
+
+    scan_pc_->points.emplace_back(overflow_point);
+    overflow_pc_->points.pop_back();
   }
+
+  // When there is overflow, the timestamp becomes the overflow packets' one
+  scan_timestamp_ = last_block_timestamp_;
   overflow_pc_->points.clear();
   overflow_pc_->points.reserve(max_pts_);
 }
@@ -250,6 +282,8 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
 
           intensity = raw->blocks[i].data[k + 2];
 
+          last_block_timestamp_ = block_timestamp;
+
           const float focal_offset = 256 * (1 - corrections.focal_distance / 13100) *
                                      (1 - corrections.focal_distance / 13100);
           const float focal_slope = corrections.focal_slope;

nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp

@@ -113,6 +113,7 @@ void Vls128Decoder::reset_overflow(double time_stamp)
     scan_timestamp_ = -1;
     overflow_pc_->points.clear();
     overflow_pc_->points.reserve(max_pts_);
+    return;
   }
 
   // Add the overflow buffer points
@@ -123,7 +124,8 @@ void Vls128Decoder::reset_overflow(double time_stamp)
     // be relative to the overflow's packet timestamp
     double new_timestamp_seconds =
       scan_timestamp_ + 1e-9 * overflow_point.time_stamp - last_block_timestamp_;
-    overflow_point.time_stamp = static_cast<uint32_t>(new_timestamp_seconds < 0.0 ? 0.0 : 1e9 * new_timestamp_seconds);
+    overflow_point.time_stamp =
+      static_cast<uint32_t>(new_timestamp_seconds < 0.0 ? 0.0 : 1e9 * new_timestamp_seconds);
 
     scan_pc_->points.emplace_back(overflow_point);
     overflow_pc_->points.pop_back();
@@ -280,11 +282,7 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p
               const float y_coord = -(xy_distance * sin_rot_angle);  // velodyne x
               const float z_coord = distance * sin_vert_angle;       // velodyne z
               const uint8_t intensity = current_block.data[k + 2];
-              auto block_timestamp = rclcpp::Time(velodyne_packet.stamp).seconds();
               last_block_timestamp_ = block_timestamp;
-              if (scan_timestamp_ < 0) {
-                scan_timestamp_ = block_timestamp;
-              }
               double point_time_offset =
                 timing_offsets_[block / 4][firing_order + laser_number / 64];