perf: unpack velodyne packets in parallel

nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/parallel_unpack.hpp

@@ -0,0 +1,34 @@
+#pragma once
+
+#include "nebula_common/point_types.hpp"
+
+#include <vector>
+
+namespace nebula
+{
+
+namespace drivers
+{
+class ParallelUnpack
+{
+protected:
+  std::vector<drivers::NebulaPointCloud> packet_clouds_;
+
+public:
+  void reset_packet_clouds(const size_t & n_packets, const int & n_pts_per_packet)
+  {
+    packet_clouds_.clear();
+    packet_clouds_.resize(n_packets);
+    for (auto & packet_cloud : packet_clouds_) {
+      packet_cloud.points.reserve(n_pts_per_packet);
+    }
+  }
+
+  void append_point(const drivers::NebulaPoint & point, const size_t & packet_idx)
+  {
+    packet_clouds_[packet_idx].points.push_back(point);
+  }
+};
+}  // namespace drivers
+
+}  // namespace nebula

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

@@ -167,7 +167,8 @@ class VelodyneScanDecoder
 
   /// @brief Virtual function for parsing and shaping VelodynePacket
   /// @param pandar_packet
-  virtual void unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) = 0;
+  virtual void unpack(
+    const velodyne_msgs::msg::VelodynePacket & velodyne_packet, const size_t & packet_index) = 0;
   /// @brief Virtual function for parsing VelodynePacket based on packet structure
   /// @param pandar_packet
   /// @return Resulting flag

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

@@ -1,5 +1,6 @@
 #pragma once
 
+#include "nebula_decoders/nebula_decoders_velodyne/decoders/parallel_unpack.hpp"
 #include "nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp"
 
 #include "velodyne_msgs/msg/velodyne_packet.hpp"
@@ -15,7 +16,7 @@ namespace vlp16
 {
   constexpr uint32_t MAX_POINTS = 300000;
 /// @brief Velodyne LiDAR decoder (VLP16)
-class Vlp16Decoder : public VelodyneScanDecoder
+class Vlp16Decoder : public VelodyneScanDecoder, public ParallelUnpack
 {
 public:
   /// @brief Constructor
@@ -26,7 +27,9 @@ class Vlp16Decoder : public VelodyneScanDecoder
     const std::shared_ptr<drivers::VelodyneCalibrationConfiguration> & calibration_configuration);
   /// @brief Parsing and shaping VelodynePacket
   /// @param velodyne_packet
-  void unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override;
+  void unpack(
+    const velodyne_msgs::msg::VelodynePacket & velodyne_packet,
+    const size_t & packet_index) override;
   /// @brief Get the flag indicating whether one cycle is ready
   /// @return Readied
   bool hasScanned() override;

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

@@ -1,5 +1,6 @@
 #pragma once
 
+#include "nebula_decoders/nebula_decoders_velodyne/decoders/parallel_unpack.hpp"
 #include "nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp"
 
 #include "velodyne_msgs/msg/velodyne_packet.hpp"
@@ -14,7 +15,7 @@ namespace drivers
 namespace vlp32
 {
 /// @brief Velodyne LiDAR decoder (VLP32)
-class Vlp32Decoder : public VelodyneScanDecoder
+class Vlp32Decoder : public VelodyneScanDecoder, public ParallelUnpack
 {
 public:
   /// @brief Constructor
@@ -25,7 +26,9 @@ class Vlp32Decoder : public VelodyneScanDecoder
     const std::shared_ptr<drivers::VelodyneCalibrationConfiguration> & calibration_configuration);
   /// @brief Parsing and shaping VelodynePacket
   /// @param velodyne_packet
-  void unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override;
+  void unpack(
+    const velodyne_msgs::msg::VelodynePacket & velodyne_packet,
+    const size_t & packet_index) override;
   /// @brief Get the flag indicating whether one cycle is ready
   /// @return Readied
   bool hasScanned() override;

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

@@ -1,6 +1,7 @@
 #pragma once
 
 #include "nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp"
+#include "nebula_decoders/nebula_decoders_velodyne/decoders/parallel_unpack.hpp"
 
 #include "velodyne_msgs/msg/velodyne_packet.hpp"
 #include "velodyne_msgs/msg/velodyne_scan.hpp"
@@ -14,7 +15,7 @@ namespace drivers
 namespace vls128
 {
 /// @brief Velodyne LiDAR decoder (VLS128)
-class Vls128Decoder : public VelodyneScanDecoder
+class Vls128Decoder : public VelodyneScanDecoder, public ParallelUnpack
 {
 public:
   /// @brief Constructor
@@ -25,7 +26,9 @@ class Vls128Decoder : public VelodyneScanDecoder
     const std::shared_ptr<drivers::VelodyneCalibrationConfiguration> & calibration_configuration);
   /// @brief Parsing and shaping VelodynePacket
   /// @param velodyne_packet
-  void unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override;
+  void unpack(
+    const velodyne_msgs::msg::VelodynePacket & velodyne_packet,
+    const size_t & packet_index) override;
   /// @brief Get the flag indicating whether one cycle is ready
   /// @return Readied
   bool hasScanned() override;

nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp

@@ -13,6 +13,8 @@
 
 #include <pcl_conversions/pcl_conversions.h>
 
+#include <algorithm>
+#include <execution>
 #include <iostream>
 #include <stdexcept>
 #include <string>

nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp

@@ -58,6 +58,10 @@ bool Vlp16Decoder::hasScanned() { return has_scanned_; }
 
 std::tuple<drivers::NebulaPointCloudPtr, double> Vlp16Decoder::get_pointcloud()
 {
+  for (const auto & cloud : packet_clouds_) {
+    *scan_pc_ += cloud;
+  }
+
   double phase = angles::from_degrees(sensor_configuration_->scan_phase);
   if (!scan_pc_->points.empty()) {
     auto current_azimuth = scan_pc_->points.back().azimuth;
@@ -87,6 +91,8 @@ void Vlp16Decoder::reset_pointcloud(size_t n_pts)
   scan_pc_->points.reserve(max_pts_);
   reset_overflow();  // transfer existing overflow points to the cleared pointcloud
   scan_timestamp_ = -1;
+
+  reset_packet_clouds(n_pts, pointsPerPacket());
 }
 
 void Vlp16Decoder::reset_overflow()
@@ -99,7 +105,8 @@ void Vlp16Decoder::reset_overflow()
   overflow_pc_->points.reserve(max_pts_);
 }
 
-void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet)
+void Vlp16Decoder::unpack(
+  const velodyne_msgs::msg::VelodynePacket & velodyne_packet, const size_t & packet_index)
 {
   const raw_packet_t * raw = (const raw_packet_t *)&velodyne_packet.data[0];
   float last_azimuth_diff = 0;
@@ -280,7 +287,8 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
                 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);
+
+                append_point(current_point, packet_index);
               }
             }
           }

nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp

@@ -58,6 +58,10 @@ bool Vlp32Decoder::hasScanned() { return has_scanned_; }
 
 std::tuple<drivers::NebulaPointCloudPtr, double> Vlp32Decoder::get_pointcloud()
 {
+  for (const auto & cloud : packet_clouds_) {
+    *scan_pc_ += cloud;
+  }
+
   double phase = angles::from_degrees(sensor_configuration_->scan_phase);
   if (!scan_pc_->points.empty()) {
     auto current_azimuth = scan_pc_->points.back().azimuth;
@@ -85,6 +89,8 @@ void Vlp32Decoder::reset_pointcloud(size_t n_pts)
   scan_pc_->points.reserve(max_pts_);
   reset_overflow();  // transfer existing overflow points to the cleared pointcloud
   scan_timestamp_ = -1;
+
+  reset_packet_clouds(n_pts, pointsPerPacket());
 }
 
 void Vlp32Decoder::reset_overflow()
@@ -97,7 +103,8 @@ void Vlp32Decoder::reset_overflow()
   overflow_pc_->points.reserve(max_pts_);
 }
 
-void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet)
+void Vlp32Decoder::unpack(
+  const velodyne_msgs::msg::VelodynePacket & velodyne_packet, const size_t & packet_index)
 {
   const raw_packet_t * raw = (const raw_packet_t *)&velodyne_packet.data[0];
   uint8_t return_mode = velodyne_packet.data[RETURN_MODE_INDEX];
@@ -307,7 +314,8 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
           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);
+
+          append_point(current_point, packet_index);
         }
       }
     }

nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp

@@ -36,7 +36,7 @@ Vls128Decoder::Vls128Decoder(
   }
   // timing table calculation, from velodyne user manual p.64
   timing_offsets_.resize(3);
-  for(size_t i=0; i < timing_offsets_.size(); ++i)
+  for (size_t i=0; i < timing_offsets_.size(); ++i)
   {
     timing_offsets_[i].resize(17); // 17 (+1 for the maintenance time after firing group 8)
   }
@@ -58,6 +58,10 @@ bool Vls128Decoder::hasScanned() { return has_scanned_; }
 
 std::tuple<drivers::NebulaPointCloudPtr, double> Vls128Decoder::get_pointcloud()
 {
+  for (const auto & cloud : packet_clouds_) {
+    *scan_pc_ += cloud;
+  }
+
   double phase = angles::from_degrees(sensor_configuration_->scan_phase);
   if (!scan_pc_->points.empty()) {
     auto current_azimuth = scan_pc_->points.back().azimuth;
@@ -85,6 +89,8 @@ void Vls128Decoder::reset_pointcloud(size_t n_pts)
   scan_pc_->points.reserve(max_pts_);
   reset_overflow();  // transfer existing overflow points to the cleared pointcloud
   scan_timestamp_ = -1;
+
+  reset_packet_clouds(n_pts, pointsPerPacket());
 }
 
 void Vls128Decoder::reset_overflow()
@@ -97,7 +103,8 @@ void Vls128Decoder::reset_overflow()
   overflow_pc_->points.reserve(max_pts_);
 }
 
-void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet)
+void Vls128Decoder::unpack(
+  const velodyne_msgs::msg::VelodynePacket & velodyne_packet, const size_t & packet_index)
 {
   const raw_packet_t * raw = (const raw_packet_t *)&velodyne_packet.data[0];
   float last_azimuth_diff = 0;
@@ -301,7 +308,9 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p
                 point_ts = 0;
               current_point.time_stamp = static_cast<uint32_t>(point_ts*1e9);
               current_point.intensity = intensity;
-              scan_pc_->points.emplace_back(current_point);
+
+              append_point(current_point, packet_index);
+
             }  // 2nd scan area condition
           }    // distance condition
         }      // empty "else"

nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp

@@ -54,9 +54,15 @@ std::tuple<drivers::NebulaPointCloudPtr, double> VelodyneDriver::ConvertScanToPo
   std::tuple<drivers::NebulaPointCloudPtr, double> pointcloud;
   if (driver_status_ == nebula::Status::OK) {
     scan_decoder_->reset_pointcloud(velodyne_scan->packets.size());
-    for (auto & packet : velodyne_scan->packets) {
-      scan_decoder_->unpack(packet);
-    }
+
+    std::vector<size_t> indices(velodyne_scan->packets.size());
+    std::iota(indices.begin(), indices.end(), 0);
+
+    std::for_each(
+      std::execution::par, indices.begin(), indices.end(), [this, velodyne_scan](const auto index) {
+        scan_decoder_->unpack(velodyne_scan->packets.at(index), index);
+      });
+
     pointcloud = scan_decoder_->get_pointcloud();
   } else {
     std::cout << "not ok driver_status_ = " << driver_status_ << std::endl;