push VLP32 results *almost* identical to original decoder

nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_generic_decoder.hpp

@@ -261,7 +261,9 @@ class VelodyneDecoder : public VelodyneScanDecoder
         (sensor_configuration_->cloud_min_angle < sensor_configuration_->cloud_max_angle &&
          azimuth >= sensor_configuration_->cloud_min_angle * 100 &&
          azimuth <= sensor_configuration_->cloud_max_angle * 100) ||
-        (sensor_configuration_->cloud_min_angle > sensor_configuration_->cloud_max_angle)) {
+        (sensor_configuration_->cloud_min_angle > sensor_configuration_->cloud_max_angle &&
+         (azimuth <= sensor_configuration_->cloud_max_angle * 100 ||
+          azimuth >= sensor_configuration_->cloud_min_angle * 100))) {
         for (int firing_seq = 0, k = 0;
              firing_seq <
              std::max(static_cast<long>(SensorT::firing_sequences_per_block), static_cast<long>(1));
@@ -300,7 +302,8 @@ class VelodyneDecoder : public VelodyneScanDecoder
             const uint laser_number =
               channel + bank_origin;  // offset the laser in this block by which block it's in
             const uint firing_order =
-              laser_number / SensorT::num_simultaneous_firings;  // VLS-128 fires 8 lasers at a time
+              laser_number / SensorT::num_simultaneous_firings;  // VLS-128 fires 8 lasers at a
+                                                                 // time, VLP32 = 2, VLP16 = 1
 
             VelodyneLaserCorrection & corrections =
               calibration_configuration_->velodyne_calibration.laser_corrections[laser_number];
@@ -320,12 +323,12 @@ class VelodyneDecoder : public VelodyneScanDecoder
               // Condition added to avoid calculating points which are not in the interesting
               // defined area (cloud_min_angle < area < cloud_max_angle).
               if (
-                (azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100 &&
-                 azimuth_corrected <= sensor_configuration_->cloud_max_angle * 100 &&
+                (current_block.rotation >= sensor_configuration_->cloud_min_angle * 100 &&
+                 current_block.rotation <= sensor_configuration_->cloud_max_angle * 100 &&
                  sensor_configuration_->cloud_min_angle < sensor_configuration_->cloud_max_angle) ||
                 (sensor_configuration_->cloud_min_angle > sensor_configuration_->cloud_max_angle &&
-                 (azimuth_corrected <= sensor_configuration_->cloud_max_angle * 100 ||
-                  azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100))) {
+                 (current_block.rotation <= sensor_configuration_->cloud_max_angle * 100 ||
+                  current_block.rotation >= sensor_configuration_->cloud_min_angle * 100))) {
                 // convert polar coordinates to Euclidean XYZ.
                 const float cos_vert_angle = corrections.cos_vert_correction;
                 const float sin_vert_angle = corrections.sin_vert_correction;
@@ -344,6 +347,7 @@ class VelodyneDecoder : public VelodyneScanDecoder
                 const float x_coord = xy_distance * cos_rot_angle;     // velodyne y
                 const float y_coord = -(xy_distance * sin_rot_angle);  // velodyne x
                 const float z_coord = distance * sin_vert_angle;       // velodyne z
+
                 const uint8_t intensity = current_block.data[k + 2];
 
                 last_block_timestamp_ = block_timestamp;
@@ -396,6 +400,7 @@ class VelodyneDecoder : public VelodyneScanDecoder
                   default:
                     return_type = static_cast<uint8_t>(drivers::ReturnType::UNKNOWN);
                 }
+
                 drivers::NebulaPoint current_point{};
                 current_point.x = x_coord;
                 current_point.y = y_coord;

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

@@ -28,9 +28,9 @@ namespace drivers
  * Raw Velodyne packet constants and structures.
  */
 static const int SIZE_BLOCK = 100;
-static const int RAW_SCAN_SIZE = 3;   // TODO: remove
+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 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);
 
@@ -55,6 +55,12 @@ 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]
 

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

@@ -29,8 +29,6 @@ class VLP16 : public VelodyneSensor
   constexpr static int channels_per_firing_sequence = 16;
 
   constexpr static float distance_resolution_m = 0.002f;
-
-  constexpr static char sensor_model[16] = "vlp16";
 };
 }  // namespace drivers
 }  // namespace nebula

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

@@ -0,0 +1,43 @@
+#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 / 8);
+    }
+    return true;
+  }
+
+  // TODO: implement this function
+  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;  // TODO: double check this
+
+private:
+  float laser_azimuth_cache_[16];
+};
+}  // namespace drivers
+}  // namespace nebula

nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls_128.hpp

@@ -18,16 +18,15 @@ class VLS128 : public VelodyneSensor
   bool fillAzimuthCache()
   {
     for (uint8_t i = 0; i < 16; i++) {
-      vls128_laser_azimuth_cache_[i] =
-        (VLS128_CHANNEL_DURATION / VLS128_SEQ_DURATION) * (i + i / 8);
+      laser_azimuth_cache_[i] = (VLS128_CHANNEL_DURATION / VLS128_SEQ_DURATION) * (i + i / 8);
     }
     return true;
   }
 
   uint16_t getAzimuthCorrected(
     uint16_t azimuth, float azimuth_diff, int firing_sequence, int firing_order)
   {
-    float azimuth_corrected = azimuth + (azimuth_diff * vls128_laser_azimuth_cache_[firing_order]);
+    float azimuth_corrected = azimuth + (azimuth_diff * laser_azimuth_cache_[firing_order]);
 
     return static_cast<uint16_t>(round(azimuth_corrected)) % 36000;
   }
@@ -42,10 +41,8 @@ class VLS128 : public VelodyneSensor
 
   constexpr static float distance_resolution_m = 0.004f;
 
-  constexpr static char sensor_model[16] = "vls128";
-
 private:
-  float vls128_laser_azimuth_cache_[16];
+  float laser_azimuth_cache_[16];
 };
 }  // namespace drivers
 }  // namespace nebula

nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp

@@ -1,8 +1,8 @@
 #include "nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp"
 
 #include "nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_generic_decoder.hpp"
-#include "nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp"
 #include "nebula_decoders/nebula_decoders_velodyne/decoders/vlp_16.hpp"
+#include "nebula_decoders/nebula_decoders_velodyne/decoders/vlp_32.hpp"
 #include "nebula_decoders/nebula_decoders_velodyne/decoders/vls_128.hpp"
 
 namespace nebula
@@ -27,7 +27,7 @@ VelodyneDriver::VelodyneDriver(
     case SensorModel::VELODYNE_HDL64:
     case SensorModel::VELODYNE_HDL32:
       scan_decoder_.reset(
-        new drivers::vlp32::Vlp32Decoder(sensor_configuration, calibration_configuration));
+        new VelodyneDecoder<VLP32>(sensor_configuration, calibration_configuration));
       break;
     case SensorModel::VELODYNE_VLP16:
       scan_decoder_.reset(