Add filter to VLP32 and VLS128

tier4 · Feb 2, 2024 · 90e6ca1 · 90e6ca1
1 parent ff5438e
commit 90e6ca1
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 0 deletions.
diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp
@@ -340,6 +340,9 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
           current_point.return_type = static_cast<uint8_t>(return_type);
           current_point.channel = corrections.laser_ring;
           current_point.azimuth = rotation_radians_[block.rotation];
+
+          if (check_invalid_point(corrections.laser_ring, block.rotation)) continue;
+
           current_point.elevation = sin_vert_angle;
           auto point_ts = block_timestamp - scan_timestamp_ + point_time_offset;
           if (point_ts < 0) point_ts = 0;

diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp
@@ -332,6 +332,9 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p
               current_point.return_type = return_type;
               current_point.channel = corrections.laser_ring;
               current_point.azimuth = rotation_radians_[azimuth_corrected];
+
+              if (check_invalid_point(corrections.laser_ring, azimuth_corrected)) continue;
+
               current_point.elevation = sin_vert_angle;
               current_point.distance = distance;
               auto point_ts = block_timestamp - scan_timestamp_ + point_time_offset;