Merge branch 'main' into hesai_ptp_params

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

@@ -6,14 +6,17 @@
 #include <velodyne_msgs/msg/velodyne_scan.hpp>
 
 #include <array>
+#include <memory>
+#include <tuple>
+#include <vector>
 
 namespace nebula
 {
 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 +41,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
@@ -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
 {
@@ -37,9 +40,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
@@ -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/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,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)
+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();  // transfer existing overflow points to the cleared pointcloud
-  scan_timestamp_ = -1;
+  reset_overflow(time_stamp);  // transfer existing overflow points to the cleared pointcloud
 }
 
-void Vlp16Decoder::reset_overflow()
+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_);
 }
@@ -225,7 +262,9 @@ 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];
+                last_block_timestamp_ = block_timestamp;
+
+                double point_time_offset = timing_offsets_[block][firing * 16 + dsr];
 
                 // Determine return type.
                 uint8_t return_type;
@@ -237,9 +276,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 +315,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);