Add filter to VLP32 and VLS128

tier4 · Sep 1, 2023 · 7c0c855 · 7c0c855
1 parent cb04f79
commit 7c0c855
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
@@ -300,6 +300,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)

diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp
@@ -294,6 +294,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;