merge commit 04d8d38

tier4 · Jan 10, 2024 · 1fc55d2 · 1fc55d2
2 parents abc4d39 + 04d8d38
commit 1fc55d2
Show file tree

Hide file tree

Showing 8 changed files with 126 additions and 66 deletions.
diff --git a/...oders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp b/...oders/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

diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp b/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

diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp b/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

diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp b/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_;
 };
 

diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp b/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();
@@ -85,7 +90,7 @@ 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
+  reset_overflow(time_stamp);  // transfer existing overflow points to the cleared pointcloud
   scan_timestamp_ = -1;
 }
 
@@ -271,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;
@@ -309,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);

diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp b/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,15 +78,18 @@ 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, 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;
 }
 
@@ -336,9 +342,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);