diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp index 338163d603369..4afcfe98ea1b8 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp @@ -231,7 +231,7 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa const float azimuth_corrected_f = azimuth + (azimuth_diff * ((dsr * VLP16_DSR_TOFFSET) + (firing * VLP16_FIRING_TOFFSET)) / - VLP16_BLOCK_DURATION); + VLP16_BLOCK_DURATION) - corrections.rot_correction * 180.0 / M_PI * 100; const uint16_t azimuth_corrected = (static_cast(round(azimuth_corrected_f))) % 36000; @@ -247,13 +247,8 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa // Convert polar coordinates to Euclidean XYZ. const float cos_vert_angle = corrections.cos_vert_correction; const float sin_vert_angle = corrections.sin_vert_correction; - const float cos_rot_correction = corrections.cos_rot_correction; - const float sin_rot_correction = corrections.sin_rot_correction; - - 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 - - cos_rot_table_[azimuth_corrected] * sin_rot_correction; + const float cos_rot_angle = cos_rot_table_[azimuth_corrected]; + const float sin_rot_angle = sin_rot_table_[azimuth_corrected]; // Compute the distance in the xy plane (w/o accounting for rotation). const float xy_distance = distance * cos_vert_angle; diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp index c42f9b0b5624b..41ee630884e8c 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp @@ -250,7 +250,8 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p // Correct for the laser rotation as a function of timing during the firings. const float azimuth_corrected_f = - azimuth + (azimuth_diff * vls_128_laser_azimuth_cache_[firing_order]); + azimuth + (azimuth_diff * vls_128_laser_azimuth_cache_[firing_order]) + - corrections.rot_correction * 180.0 / M_PI * 100; const uint16_t azimuth_corrected = ((uint16_t)round(azimuth_corrected_f)) % 36000; if ( @@ -268,13 +269,8 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p // convert polar coordinates to Euclidean XYZ. const float cos_vert_angle = corrections.cos_vert_correction; const float sin_vert_angle = corrections.sin_vert_correction; - const float cos_rot_correction = corrections.cos_rot_correction; - const float sin_rot_correction = corrections.sin_rot_correction; - - 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 - - cos_rot_table_[azimuth_corrected] * sin_rot_correction; + const float cos_rot_angle = cos_rot_table_[azimuth_corrected]; + const float sin_rot_angle = sin_rot_table_[azimuth_corrected]; // Compute the distance in the xy plane (w/o accounting for rotation). const float xy_distance = distance * cos_vert_angle; diff --git a/nebula_tests/data/velodyne/vls128/1614315746748918706.pcd b/nebula_tests/data/velodyne/vls128/1614315746748918706.pcd index 80ed53270c6e9..91e85b9a82c60 100644 Binary files a/nebula_tests/data/velodyne/vls128/1614315746748918706.pcd and b/nebula_tests/data/velodyne/vls128/1614315746748918706.pcd differ