refactor(velodyne): implement generic velodyne decoder

nebula_decoders/CMakeLists.txt

@@ -38,9 +38,6 @@ ament_auto_add_library(nebula_decoders_hesai SHARED
 # Velodyne
 ament_auto_add_library(nebula_decoders_velodyne SHARED
         src/nebula_decoders_velodyne/velodyne_driver.cpp
-        src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp
-        src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp
-        src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp
         )
 
 # Robosense

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

@@ -1,7 +1,14 @@
 #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 <pcl/point_cloud.h>
@@ -10,16 +17,8 @@
 #include <cmath>
 #include <cstdint>
 #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
 {
@@ -29,8 +28,10 @@ namespace drivers
  * Raw Velodyne packet constants and structures.
  */
 static const int SIZE_BLOCK = 100;
-static const int RAW_SCAN_SIZE = 3;
-static const int SCANS_PER_BLOCK = 32;
+static const int RAW_SCAN_SIZE = 3;  // TODO: remove
+static const int RAW_CHANNEL_SIZE = 3;
+static const int SCANS_PER_BLOCK = 32;  // TODO: remove
+static const int CHANNELS_PER_BLOCK = 32;
 static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE);
 
 static const double ROTATION_RESOLUTION = 0.01;     // [deg]
@@ -54,15 +55,21 @@ static const float VLP16_BLOCK_DURATION = 110.592f;  // [µs]
 static const float VLP16_DSR_TOFFSET = 2.304f;       // [µs]
 static const float VLP16_FIRING_TOFFSET = 55.296f;   // [µs]
 
+/** Special Definitions for VLS32 support **/
+static const float VLP32_CHANNEL_DURATION =
+  2.304f;  // [µs] Channels corresponds to one laser firing
+static const float VLP32_SEQ_DURATION =
+  55.296f;  // [µs] Sequence is a set of laser firings including recharging
+
 /** Special Definitions for VLS128 support **/
 static const float VLP128_DISTANCE_RESOLUTION = 0.004f;  // [m]
 
 /** Special Definitions for VLS128 support **/
 // These are used to detect which bank of 32 lasers is in this block
-static const uint16_t VLS128_BANK_1 = 0xeeff;
-static const uint16_t VLS128_BANK_2 = 0xddff;
-static const uint16_t VLS128_BANK_3 = 0xccff;
-static const uint16_t VLS128_BANK_4 = 0xbbff;
+static const uint16_t BANK_1 = 0xeeff;
+static const uint16_t BANK_2 = 0xddff;
+static const uint16_t BANK_3 = 0xccff;
+static const uint16_t BANK_4 = 0xbbff;
 
 static const float VLS128_CHANNEL_DURATION =
   2.665f;  // [µs] Channels corresponds to one laser firing

nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_sensor.hpp

@@ -0,0 +1,21 @@
+#pragma once
+#include "nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp"
+
+#include <cstdint>
+
+namespace nebula
+{
+namespace drivers
+{
+class VelodyneSensor
+{
+public:
+  virtual int getNumPaddingBlocks(bool /* dual_return */) { return 0; }
+
+  virtual bool fillAzimuthCache() { return false; }
+
+  virtual uint16_t getAzimuthCorrected(
+    uint16_t azimuth, float azimuth_diff, int firing_sequence, int firing_order) = 0;
+};
+}  // namespace drivers
+}  // namespace nebula

nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp_16.hpp

@@ -0,0 +1,40 @@
+#pragma once
+#include "nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_sensor.hpp"
+
+namespace nebula
+{
+namespace drivers
+{
+
+class VLP16 : public VelodyneSensor
+{
+public:
+  uint16_t getAzimuthCorrected(
+    uint16_t azimuth, float azimuth_diff, int firing_sequence, int firing_order)
+  {
+    float azimuth_corrected =
+      azimuth + (azimuth_diff *
+                 ((firing_order * VLP16_DSR_TOFFSET) + (firing_sequence * VLP16_FIRING_TOFFSET)) /
+                 VLP16_BLOCK_DURATION);
+
+    return static_cast<uint16_t>(round(azimuth_corrected)) % 36000;
+  }
+
+  constexpr static int num_maintenance_periods = 0;
+
+  constexpr static int num_simultaneous_firings = 1;
+
+  constexpr static double firing_sequences_per_block = 2.0;
+
+  constexpr static int channels_per_firing_sequence = 16;
+
+  constexpr static float distance_resolution_m = 0.002f;
+
+  constexpr static double full_firing_cycle_s = 55.296 * 1e-6;
+
+  constexpr static double single_firing_s = 2.304 * 1e-6;
+
+  constexpr static double offset_packet_time = 0;
+};
+}  // namespace drivers
+}  // namespace nebula

nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp_32.hpp

@@ -0,0 +1,48 @@
+#pragma once
+
+#include "nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_sensor.hpp"
+
+namespace nebula
+{
+namespace drivers
+{
+class VLP32 : public VelodyneSensor
+{
+public:
+  bool fillAzimuthCache()
+  {
+    for (uint8_t i = 0; i < 16; i++) {
+      laser_azimuth_cache_[i] = (VLP32_CHANNEL_DURATION / VLP32_SEQ_DURATION) * (i + i / 2);
+    }
+    return true;
+  }
+
+  uint16_t getAzimuthCorrected(
+    uint16_t azimuth, float azimuth_diff, int /* firing_sequence */, int firing_order)
+  {
+    float azimuth_corrected = azimuth + (azimuth_diff * laser_azimuth_cache_[firing_order]);
+
+    return static_cast<uint16_t>(round(azimuth_corrected)) % 36000;
+  }
+
+  constexpr static int num_maintenance_periods = 0;
+
+  constexpr static int num_simultaneous_firings = 2;
+
+  constexpr static double firing_sequences_per_block = 1.0;
+
+  constexpr static int channels_per_firing_sequence = 32;
+
+  constexpr static float distance_resolution_m = 0.004f;
+
+  constexpr static double full_firing_cycle_s = 55.296 * 1e-6;
+
+  constexpr static double single_firing_s = 2.304 * 1e-6;
+
+  constexpr static double offset_packet_time = 0;
+
+private:
+  float laser_azimuth_cache_[16];
+};
+}  // namespace drivers
+}  // namespace nebula