From 965b4d552eb503e2d7d24496bfa1a96314a882ee Mon Sep 17 00:00:00 2001 From: Benjamin Gilby <40575701+bgilby59@users.noreply.github.com> Date: Thu, 11 Jan 2024 16:39:30 +0900 Subject: [PATCH] fix(velodyne_decoder): vlp16 and vlp32 overflow handling (#113) * fix: overflow handling in VLP16 and VLP32 * add definition and initialization of last_block_timestamp_ * Update nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp Co-authored-by: Kenzo Lobos Tsunekawa * Update nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp Co-authored-by: Kenzo Lobos Tsunekawa * Update nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp Co-authored-by: Kenzo Lobos Tsunekawa * add missing return statements to vlp16 and vls128 decoders * remove superfluous code --------- Co-authored-by: Kenzo Lobos Tsunekawa --- .../decoders/vlp16_decoder.hpp | 4 ++ .../decoders/vlp32_decoder.hpp | 4 ++ .../decoders/vlp16_decoder.cpp | 44 ++++++++++++++++--- .../decoders/vlp32_decoder.cpp | 44 ++++++++++++++++--- .../decoders/vls128_decoder.cpp | 8 ++-- 5 files changed, 89 insertions(+), 15 deletions(-) diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp index f9905373f..730f7045f 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp @@ -6,6 +6,9 @@ #include #include +#include +#include +#include namespace nebula { @@ -52,6 +55,7 @@ class Vlp16Decoder : public VelodyneScanDecoder float rotation_radians_[ROTATION_MAX_UNITS]; int phase_; int max_pts_; + double last_block_timestamp_; std::vector> timing_offsets_; }; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp index 3f9b1c27d..0ffa1e3ce 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp @@ -6,6 +6,9 @@ #include #include +#include +#include +#include namespace nebula { @@ -52,6 +55,7 @@ class Vlp32Decoder : public VelodyneScanDecoder std::vector> timing_offsets_; int phase_; int max_pts_; + double last_block_timestamp_; }; } // namespace vlp32 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 a7ed45804..e1e9062f0 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp @@ -85,21 +85,53 @@ int Vlp16Decoder::pointsPerPacket() return BLOCKS_PER_PACKET * VLP16_FIRINGS_PER_BLOCK * VLP16_SCANS_PER_FIRING; } -void Vlp16Decoder::reset_pointcloud(size_t n_pts, [[maybe_unused]] double time_stamp) +void Vlp16Decoder::reset_pointcloud(size_t n_pts, double time_stamp) { scan_pc_->points.clear(); max_pts_ = n_pts * pointsPerPacket(); scan_pc_->points.reserve(max_pts_); reset_overflow(time_stamp); // transfer existing overflow points to the cleared pointcloud - scan_timestamp_ = -1; } -void Vlp16Decoder::reset_overflow([[maybe_unused]] double time_stamp) +void Vlp16Decoder::reset_overflow(double time_stamp) { + if (overflow_pc_->points.size() == 0) { + scan_timestamp_ = -1; + overflow_pc_->points.reserve(max_pts_); + return; + } + + // Compute the absolute time stamp of the last point of the overflow pointcloud + const double last_overflow_time_stamp = + scan_timestamp_ + 1e-9 * overflow_pc_->points.back().time_stamp; + + // Detect cases where there is an unacceptable time difference between the last overflow point and + // the first point of the next packet. In that case, there was probably a packet drop so it is + // better to ignore the overflow pointcloud + if (time_stamp - last_overflow_time_stamp > 0.05) { + scan_timestamp_ = -1; + overflow_pc_->points.clear(); + overflow_pc_->points.reserve(max_pts_); + return; + } + // Add the overflow buffer points - for (size_t i = 0; i < overflow_pc_->points.size(); i++) { - scan_pc_->points.emplace_back(overflow_pc_->points[i]); + while (overflow_pc_->points.size() > 0) { + auto overflow_point = overflow_pc_->points.back(); + + // The overflow points had the stamps from the previous pointcloud. These need to be changed to + // be relative to the overflow's packet timestamp + double new_timestamp_seconds = + scan_timestamp_ + 1e-9 * overflow_point.time_stamp - last_block_timestamp_; + overflow_point.time_stamp = + static_cast(new_timestamp_seconds < 0.0 ? 0.0 : 1e9 * new_timestamp_seconds); + + scan_pc_->points.emplace_back(overflow_point); + overflow_pc_->points.pop_back(); } + + // When there is overflow, the timestamp becomes the overflow packets' one + scan_timestamp_ = last_block_timestamp_; overflow_pc_->points.clear(); overflow_pc_->points.reserve(max_pts_); } @@ -230,6 +262,8 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa const float z_coord = distance * sin_vert_angle; // velodyne z const uint8_t intensity = current_block.data[k + 2]; + last_block_timestamp_ = block_timestamp; + double point_time_offset = timing_offsets_[block][firing * 16 + dsr]; // Determine return type. diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp index 5576a6f3d..25feaf8f7 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp @@ -83,22 +83,54 @@ int Vlp32Decoder::pointsPerPacket() return BLOCKS_PER_PACKET * SCANS_PER_BLOCK; } -void Vlp32Decoder::reset_pointcloud(size_t n_pts, [[maybe_unused]] double time_stamp) +void Vlp32Decoder::reset_pointcloud(size_t n_pts, double time_stamp) { // scan_pc_.reset(new NebulaPointCloud); scan_pc_->points.clear(); max_pts_ = n_pts * pointsPerPacket(); scan_pc_->points.reserve(max_pts_); reset_overflow(time_stamp); // transfer existing overflow points to the cleared pointcloud - scan_timestamp_ = -1; } -void Vlp32Decoder::reset_overflow([[maybe_unused]] double time_stamp) +void Vlp32Decoder::reset_overflow(double time_stamp) { + if (overflow_pc_->points.size() == 0) { + scan_timestamp_ = -1; + overflow_pc_->points.reserve(max_pts_); + return; + } + + // Compute the absolute time stamp of the last point of the overflow pointcloud + const double last_overflow_time_stamp = + scan_timestamp_ + 1e-9 * overflow_pc_->points.back().time_stamp; + + // Detect cases where there is an unacceptable time difference between the last overflow point and + // the first point of the next packet. In that case, there was probably a packet drop so it is + // better to ignore the overflow pointcloud + if (time_stamp - last_overflow_time_stamp > 0.05) { + scan_timestamp_ = -1; + overflow_pc_->points.clear(); + overflow_pc_->points.reserve(max_pts_); + return; + } + // Add the overflow buffer points - for (size_t i = 0; i < overflow_pc_->points.size(); i++) { - scan_pc_->points.emplace_back(overflow_pc_->points[i]); + while (overflow_pc_->points.size() > 0) { + auto overflow_point = overflow_pc_->points.back(); + + // The overflow points had the stamps from the previous pointcloud. These need to be changed to + // be relative to the overflow's packet timestamp + double new_timestamp_seconds = + scan_timestamp_ + 1e-9 * overflow_point.time_stamp - last_block_timestamp_; + overflow_point.time_stamp = + static_cast(new_timestamp_seconds < 0.0 ? 0.0 : 1e9 * new_timestamp_seconds); + + scan_pc_->points.emplace_back(overflow_point); + overflow_pc_->points.pop_back(); } + + // When there is overflow, the timestamp becomes the overflow packets' one + scan_timestamp_ = last_block_timestamp_; overflow_pc_->points.clear(); overflow_pc_->points.reserve(max_pts_); } @@ -250,6 +282,8 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa intensity = raw->blocks[i].data[k + 2]; + last_block_timestamp_ = block_timestamp; + const float focal_offset = 256 * (1 - corrections.focal_distance / 13100) * (1 - corrections.focal_distance / 13100); const float focal_slope = corrections.focal_slope; 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 b621d5482..23442a6f3 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp @@ -113,6 +113,7 @@ void Vls128Decoder::reset_overflow(double time_stamp) scan_timestamp_ = -1; overflow_pc_->points.clear(); overflow_pc_->points.reserve(max_pts_); + return; } // Add the overflow buffer points @@ -123,7 +124,8 @@ void Vls128Decoder::reset_overflow(double time_stamp) // be relative to the overflow's packet timestamp double new_timestamp_seconds = scan_timestamp_ + 1e-9 * overflow_point.time_stamp - last_block_timestamp_; - overflow_point.time_stamp = static_cast(new_timestamp_seconds < 0.0 ? 0.0 : 1e9 * new_timestamp_seconds); + overflow_point.time_stamp = + static_cast(new_timestamp_seconds < 0.0 ? 0.0 : 1e9 * new_timestamp_seconds); scan_pc_->points.emplace_back(overflow_point); overflow_pc_->points.pop_back(); @@ -280,11 +282,7 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p const float y_coord = -(xy_distance * sin_rot_angle); // velodyne x const float z_coord = distance * sin_vert_angle; // velodyne z const uint8_t intensity = current_block.data[k + 2]; - auto block_timestamp = rclcpp::Time(velodyne_packet.stamp).seconds(); last_block_timestamp_ = block_timestamp; - if (scan_timestamp_ < 0) { - scan_timestamp_ = block_timestamp; - } double point_time_offset = timing_offsets_[block / 4][firing_order + laser_number / 64];