pass 32 test

tier4 · May 22, 2024 · 5259d6f · 5259d6f
1 parent 7395065
commit 5259d6f
Show file tree

Hide file tree

Showing 4 changed files with 18 additions and 1 deletion.
diff --git a/...rs/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_generic_decoder.hpp b/...rs/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_generic_decoder.hpp
@@ -282,7 +282,7 @@ class VelodyneDecoder : public VelodyneScanDecoder
               distance > sensor_configuration_->min_range &&
               distance < sensor_configuration_->max_range) {
               // Correct for the laser rotation as a function of timing during the firings.
-              const uint16_t azimuth_corrected =
+              uint16_t azimuth_corrected =
                 sensor_.getAzimuthCorrected(azimuth, azimuth_diff, firing_seq, firing_order);
 
               // Condition added to avoid calculating points which are not in the interesting
@@ -294,6 +294,8 @@ class VelodyneDecoder : public VelodyneScanDecoder
                 const float cos_rot_correction = corrections.cos_rot_correction;
                 const float sin_rot_correction = corrections.sin_rot_correction;
 
+                // select correct azimuth if vlp32 currenct_block.rotation, if vlp128 and vlp16 azimuth_corrected
+                azimuth_corrected = sensor_.getTrueRotation(azimuth_corrected, current_block.rotation);
                 const float cos_rot_angle = cos_rot_table_[azimuth_corrected] * cos_rot_correction +
                                             sin_rot_table_[azimuth_corrected] * sin_rot_correction;
                 const float sin_rot_angle = sin_rot_table_[azimuth_corrected] * cos_rot_correction -

diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp_16.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp_16.hpp
@@ -20,6 +20,11 @@ class VLP16 : public VelodyneSensor
     return static_cast<uint16_t>(round(azimuth_corrected)) % 36000;
   }
 
+  uint16_t getTrueRotation(uint16_t azimuth_corrected, uint16_t current_block_rotation)
+  {
+    return azimuth_corrected;
+  }
+
   constexpr static int num_maintenance_periods = 0;
 
   constexpr static int num_simultaneous_firings = 1;

diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp_32.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp_32.hpp
@@ -25,6 +25,11 @@ class VLP32 : public VelodyneSensor
     return static_cast<uint16_t>(round(azimuth_corrected)) % 36000;
   }
 
+  uint16_t getTrueRotation(uint16_t azimuth_corrected, uint16_t current_block_rotation)
+  {
+    return current_block_rotation;
+  }
+
   constexpr static int num_maintenance_periods = 0;
 
   constexpr static int num_simultaneous_firings = 2;

diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls_128.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls_128.hpp
@@ -31,6 +31,11 @@ class VLS128 : public VelodyneSensor
     return static_cast<uint16_t>(round(azimuth_corrected)) % 36000;
   }
 
+  uint16_t getTrueRotation(uint16_t azimuth_corrected, uint16_t current_block_rotation)
+  {
+    return azimuth_corrected;
+  }
+
   constexpr static int num_maintenance_periods = 1;
 
   constexpr static int num_simultaneous_firings = 8;