refactor(velodyne)!: combine Velodyne ROS wrappers into a single node

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

@@ -1,25 +1,27 @@
 #ifndef NEBULA_WS_VELODYNE_SCAN_DECODER_HPP
 #define NEBULA_WS_VELODYNE_SCAN_DECODER_HPP
+
+#include <nebula_common/point_types.hpp>
+#include <nebula_common/velodyne/velodyne_calibration_decoder.hpp>
+#include <nebula_common/velodyne/velodyne_common.hpp>
+
 #include <rclcpp/rclcpp.hpp>
 
+#include <velodyne_msgs/msg/velodyne_packet.hpp>
+#include <velodyne_msgs/msg/velodyne_scan.hpp>
+
 #include <boost/format.hpp>
 
+#include <angles/angles.h>
 #include <pcl/point_cloud.h>
 
 #include <cerrno>
 #include <cmath>
 #include <cstdint>
+#include <memory>
 #include <string>
-#include <vector>
-
-#include "nebula_common/point_types.hpp"
-#include "nebula_common/velodyne/velodyne_calibration_decoder.hpp"
-#include "nebula_common/velodyne/velodyne_common.hpp"
-
-#include <velodyne_msgs/msg/velodyne_packet.hpp>
-#include <velodyne_msgs/msg/velodyne_scan.hpp>
-
 #include <tuple>
+#include <vector>
 
 namespace nebula
 {
@@ -69,6 +71,10 @@ static const float VLS128_CHANNEL_DURATION =
 static const float VLS128_SEQ_DURATION =
   53.3f;  // [µs] Sequence is a set of laser firings including recharging
 
+static const size_t OFFSET_FIRST_AZIMUTH = 2;
+static const size_t OFFSET_LAST_AZIMUTH = 1102;
+static const uint32_t DEGREE_SUBDIVISIONS = 100;
+
 /** \brief Raw Velodyne data block.
  *
  *  Each block contains data from either the upper or lower laser
@@ -132,23 +138,60 @@ enum RETURN_TYPE {
 /// @brief Base class for Velodyne LiDAR decoder
 class VelodyneScanDecoder
 {
+private:
+  size_t processed_packets_{0};
+  uint32_t prev_packet_first_azm_phased_{0};
+  bool has_scanned_{false};
+
 protected:
+  /// @brief Checks if the current packet completes the ongoing scan.
+  /// TODO: this has been moved from velodyne_hw_interface.cpp and is a temporary solution until
+  /// the Velodyne decoder uses the same structure as Hesai/Robosense
+  /// @param packet The packet buffer to extract azimuths from
+  /// @param packet_seconds The seconds-since-epoch part of the packet's timestamp
+  /// @param phase The sensor's scan phase used for scan cutting
+  void checkAndHandleScanComplete(
+    const std::vector<uint8_t> & packet, int32_t packet_seconds, const uint32_t phase)
+  {
+    if (has_scanned_) {
+      processed_packets_ = 0;
+      reset_pointcloud(packet_seconds);
+    }
+
+    has_scanned_ = false;
+    processed_packets_++;
+
+    uint32_t packet_first_azm = packet[OFFSET_FIRST_AZIMUTH];  // lower word of azimuth block 0
+    packet_first_azm |= packet[OFFSET_FIRST_AZIMUTH + 1] << 8; // higher word of azimuth block 0
+
+    uint32_t packet_last_azm = packet[OFFSET_LAST_AZIMUTH];
+    packet_last_azm |= packet[OFFSET_LAST_AZIMUTH + 1] << 8;
+
+    const uint32_t max_azi = 360 * DEGREE_SUBDIVISIONS;
+
+    uint32_t packet_first_azm_phased = (max_azi + packet_first_azm - phase) % max_azi;
+    uint32_t packet_last_azm_phased = (max_azi + packet_last_azm - phase) % max_azi;
+
+    has_scanned_ =
+      processed_packets_ > 1 && (packet_last_azm_phased < packet_first_azm_phased ||
+                                 packet_first_azm_phased < prev_packet_first_azm_phased_);
+
+    prev_packet_first_azm_phased_ = packet_first_azm_phased;
+  }
+
   /// @brief Decoded point cloud
   drivers::NebulaPointCloudPtr scan_pc_;
   /// @brief Point cloud overflowing from one cycle
   drivers::NebulaPointCloudPtr overflow_pc_;
 
-  uint16_t scan_phase_{};
-  uint16_t last_phase_{};
-  bool has_scanned_ = true;
   double dual_return_distance_threshold_{};  // Velodyne does this internally, this will not be
                                              // implemented here
   double scan_timestamp_{};
 
   /// @brief SensorConfiguration for this decoder
-  std::shared_ptr<drivers::VelodyneSensorConfiguration> sensor_configuration_;
+  std::shared_ptr<const drivers::VelodyneSensorConfiguration> sensor_configuration_;
   /// @brief Calibration for this decoder
-  std::shared_ptr<drivers::VelodyneCalibrationConfiguration> calibration_configuration_;
+  std::shared_ptr<const drivers::VelodyneCalibrationConfiguration> calibration_configuration_;
 
 public:
   VelodyneScanDecoder(VelodyneScanDecoder && c) = delete;
@@ -161,15 +204,16 @@ 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 std::vector<uint8_t> & packet, int32_t packet_seconds) = 0;
   /// @brief Virtual function for parsing VelodynePacket based on packet structure
   /// @param pandar_packet
   /// @return Resulting flag
   virtual bool parsePacket(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) = 0;
 
   /// @brief Virtual function for getting the flag indicating whether one cycle is ready
   /// @return Readied
-  virtual bool hasScanned() = 0;
+  bool hasScanned() { return has_scanned_; }
+
   /// @brief Calculation of points in each packet
   /// @return # of points
   virtual int pointsPerPacket() = 0;
@@ -178,8 +222,7 @@ class VelodyneScanDecoder
   /// @return tuple of Point cloud and timestamp
   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, double time_stamp) = 0;
+  virtual void reset_pointcloud(double time_stamp) = 0;
   /// @brief Resetting overflowed point cloud buffer
   virtual void reset_overflow(double time_stamp) = 0;
 };

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

@@ -25,23 +25,20 @@ class Vlp16Decoder : public VelodyneScanDecoder
   /// @param sensor_configuration SensorConfiguration for this decoder
   /// @param calibration_configuration Calibration for this decoder
   explicit Vlp16Decoder(
-    const std::shared_ptr<drivers::VelodyneSensorConfiguration> & sensor_configuration,
-    const std::shared_ptr<drivers::VelodyneCalibrationConfiguration> & calibration_configuration);
+    const std::shared_ptr<const drivers::VelodyneSensorConfiguration> & sensor_configuration,
+    const std::shared_ptr<const drivers::VelodyneCalibrationConfiguration> &
+      calibration_configuration);
   /// @brief Parsing and shaping VelodynePacket
   /// @param velodyne_packet
-  void unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override;
-  /// @brief Get the flag indicating whether one cycle is ready
-  /// @return Readied
-  bool hasScanned() override;
+  void unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds) override;
   /// @brief Calculation of points in each packet
   /// @return # of points
   int pointsPerPacket() override;
   /// @brief Get the constructed point cloud
   /// @return tuple of Point cloud and timestamp
   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, double time_stamp) override;
+  void reset_pointcloud(double time_stamp) override;
   /// @brief Resetting overflowed point cloud buffer
   void reset_overflow(double time_stamp) override;
 

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

@@ -24,23 +24,20 @@ class Vlp32Decoder : public VelodyneScanDecoder
   /// @param sensor_configuration SensorConfiguration for this decoder
   /// @param calibration_configuration Calibration for this decoder
   explicit Vlp32Decoder(
-    const std::shared_ptr<drivers::VelodyneSensorConfiguration> & sensor_configuration,
-    const std::shared_ptr<drivers::VelodyneCalibrationConfiguration> & calibration_configuration);
+    const std::shared_ptr<const drivers::VelodyneSensorConfiguration> & sensor_configuration,
+    const std::shared_ptr<const drivers::VelodyneCalibrationConfiguration> &
+      calibration_configuration);
   /// @brief Parsing and shaping VelodynePacket
   /// @param velodyne_packet
-  void unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override;
-  /// @brief Get the flag indicating whether one cycle is ready
-  /// @return Readied
-  bool hasScanned() override;
+  void unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds) override;
   /// @brief Calculation of points in each packet
   /// @return # of points
   int pointsPerPacket() override;
   /// @brief Get the constructed point cloud
   /// @return tuple of Point cloud and timestamp
   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, double time_stamp) override;
+  void reset_pointcloud(double time_stamp) override;
   /// @brief Resetting overflowed point cloud buffer
   void reset_overflow(double time_stamp) override;
 

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

@@ -6,6 +6,9 @@
 #include <velodyne_msgs/msg/velodyne_scan.hpp>
 
 #include <array>
+#include <memory>
+#include <tuple>
+#include <vector>
 
 namespace nebula
 {
@@ -21,23 +24,20 @@ class Vls128Decoder : public VelodyneScanDecoder
   /// @param sensor_configuration SensorConfiguration for this decoder
   /// @param calibration_configuration Calibration for this decoder
   explicit Vls128Decoder(
-    const std::shared_ptr<drivers::VelodyneSensorConfiguration> & sensor_configuration,
-    const std::shared_ptr<drivers::VelodyneCalibrationConfiguration> & calibration_configuration);
+    const std::shared_ptr<const drivers::VelodyneSensorConfiguration> & sensor_configuration,
+    const std::shared_ptr<const drivers::VelodyneCalibrationConfiguration> &
+      calibration_configuration);
   /// @brief Parsing and shaping VelodynePacket
   /// @param velodyne_packet
-  void unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override;
-  /// @brief Get the flag indicating whether one cycle is ready
-  /// @return Readied
-  bool hasScanned() override;
+  void unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds) override;
   /// @brief Calculation of points in each packet
   /// @return # of points
   int pointsPerPacket() override;
   /// @brief Get the constructed point cloud
   /// @return tuple of Point cloud and timestamp
   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, double time_stamp) override;
+  void reset_pointcloud(double time_stamp) override;
   /// @brief Resetting overflowed point cloud buffer
   void reset_overflow(double time_stamp) override;
 

nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp

@@ -14,8 +14,11 @@
 #include <pcl_conversions/pcl_conversions.h>
 
 #include <iostream>
+#include <memory>
 #include <stdexcept>
 #include <string>
+#include <tuple>
+#include <vector>
 
 namespace nebula
 {
@@ -36,8 +39,9 @@ class VelodyneDriver : NebulaDriverBase
   /// @param sensor_configuration SensorConfiguration for this driver
   /// @param calibration_configuration CalibrationConfiguration for this driver
   VelodyneDriver(
-    const std::shared_ptr<drivers::VelodyneSensorConfiguration> & sensor_configuration,
-    const std::shared_ptr<drivers::VelodyneCalibrationConfiguration> & calibration_configuration);
+    const std::shared_ptr<const drivers::VelodyneSensorConfiguration> & sensor_configuration,
+    const std::shared_ptr<const drivers::VelodyneCalibrationConfiguration> &
+      calibration_configuration);
 
   /// @brief Setting CalibrationConfiguration (not used)
   /// @param calibration_configuration
@@ -52,8 +56,8 @@ class VelodyneDriver : NebulaDriverBase
   /// @brief Convert VelodyneScan message to point cloud
   /// @param velodyne_scan Message
   /// @return tuple of Point cloud and timestamp
-  std::tuple<drivers::NebulaPointCloudPtr, double> ConvertScanToPointcloud(
-    const std::shared_ptr<velodyne_msgs::msg::VelodyneScan> & velodyne_scan);
+  std::tuple<drivers::NebulaPointCloudPtr, double> ParseCloudPacket(
+    const std::vector<uint8_t> & packet, int32_t packet_seconds);
 };
 
 }  // namespace drivers

nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp

@@ -1,19 +1,20 @@
 #include "nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp"
 
+#include <angles/angles.h>
+
 #include <cmath>
 #include <utility>
 
-#include <angles/angles.h> 
-
 namespace nebula
 {
 namespace drivers
 {
 namespace vlp16
 {
 Vlp16Decoder::Vlp16Decoder(
-  const std::shared_ptr<drivers::VelodyneSensorConfiguration> & sensor_configuration,
-  const std::shared_ptr<drivers::VelodyneCalibrationConfiguration> & calibration_configuration)
+  const std::shared_ptr<const drivers::VelodyneSensorConfiguration> & sensor_configuration,
+  const std::shared_ptr<const drivers::VelodyneCalibrationConfiguration> &
+    calibration_configuration)
 {
   sensor_configuration_ = sensor_configuration;
   calibration_configuration_ = calibration_configuration;
@@ -56,11 +57,6 @@ Vlp16Decoder::Vlp16Decoder(
   phase_ = (uint16_t)round(sensor_configuration_->scan_phase * 100);
 }
 
-bool Vlp16Decoder::hasScanned()
-{
-  return has_scanned_;
-}
-
 std::tuple<drivers::NebulaPointCloudPtr, double> Vlp16Decoder::get_pointcloud()
 {
   double phase = angles::from_degrees(sensor_configuration_->scan_phase);
@@ -87,11 +83,9 @@ int Vlp16Decoder::pointsPerPacket()
   return BLOCKS_PER_PACKET * VLP16_FIRINGS_PER_BLOCK * VLP16_SCANS_PER_FIRING;
 }
 
-void Vlp16Decoder::reset_pointcloud(size_t n_pts, double time_stamp)
+void Vlp16Decoder::reset_pointcloud(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
 }
 
@@ -138,12 +132,14 @@ void Vlp16Decoder::reset_overflow(double time_stamp)
   overflow_pc_->points.reserve(max_pts_);
 }
 
-void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet)
+void Vlp16Decoder::unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds)
 {
-  const raw_packet_t * raw = (const raw_packet_t *)&velodyne_packet.data[0];
+  checkAndHandleScanComplete(packet, packet_seconds, phase_);
+
+  const raw_packet_t * raw = (const raw_packet_t *)packet.data();
   float last_azimuth_diff = 0;
   uint16_t azimuth_next;
-  const uint8_t return_mode = velodyne_packet.data[RETURN_MODE_INDEX];
+  const uint8_t return_mode = packet[RETURN_MODE_INDEX];
   const bool dual_return = (return_mode == RETURN_MODE_DUAL);
 
   for (uint block = 0; block < BLOCKS_PER_PACKET; block++) {
@@ -203,7 +199,7 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
               block % 2 ? raw->blocks[block - 1].data[k + 1] : raw->blocks[block + 1].data[k + 1];
           }
           // Apply timestamp if this is the first new packet in the scan.
-          auto block_timestamp = rclcpp::Time(velodyne_packet.stamp).seconds();
+          auto block_timestamp = packet_seconds;
           if (scan_timestamp_ < 0) {
             scan_timestamp_ = block_timestamp;
           }
@@ -216,7 +212,7 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
             continue;
           }
           {
-            VelodyneLaserCorrection & corrections =
+            const VelodyneLaserCorrection & corrections =
               calibration_configuration_->velodyne_calibration.laser_corrections[dsr];
             float distance = current_return.uint *
                              calibration_configuration_->velodyne_calibration.distance_resolution_m;