From 5259d6f7a871e24baca0ef39cfb441eebcd6ac79 Mon Sep 17 00:00:00 2001 From: ike-kazu Date: Wed, 22 May 2024 16:45:14 +0900 Subject: [PATCH] pass 32 test --- .../decoders/velodyne_generic_decoder.hpp | 4 +++- .../nebula_decoders_velodyne/decoders/vlp_16.hpp | 5 +++++ .../nebula_decoders_velodyne/decoders/vlp_32.hpp | 5 +++++ .../nebula_decoders_velodyne/decoders/vls_128.hpp | 5 +++++ 4 files changed, 18 insertions(+), 1 deletion(-) diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_generic_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_generic_decoder.hpp index de18b79de..8e632db8a 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_generic_decoder.hpp +++ b/nebula_decoders/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 index f82376488..f84bf928d 100644 --- 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(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 index b607b36ae..236c29251 100644 --- 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(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 index 5779502f6..aeb265513 100644 --- 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(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;