From 0bc6cf38ccea6411a77f0bd371a962932bbe3c83 Mon Sep 17 00:00:00 2001 From: Benjamin Gilby Date: Tue, 9 Jan 2024 16:56:14 +0900 Subject: [PATCH 1/7] fix: overflow handling in VLP16 and VLP32 --- .../decoders/vlp16_decoder.cpp | 39 +++++++++++++++++-- .../decoders/vlp32_decoder.cpp | 39 +++++++++++++++++-- 2 files changed, 70 insertions(+), 8 deletions(-) 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 2e877e523..db14679a2 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp @@ -80,7 +80,7 @@ int Vlp16Decoder::pointsPerPacket() return BLOCKS_PER_PACKET * VLP16_FIRINGS_PER_BLOCK * VLP16_SCANS_PER_FIRING; } -void Vlp16Decoder::reset_pointcloud(size_t n_pts) +void Vlp16Decoder::reset_pointcloud(size_t n_pts, double time_stamp) { scan_pc_->points.clear(); max_pts_ = n_pts * pointsPerPacket(); @@ -89,12 +89,43 @@ void Vlp16Decoder::reset_pointcloud(size_t n_pts) scan_timestamp_ = -1; } -void Vlp16Decoder::reset_overflow() +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_); + } + // 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_); } 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 285e14525..a15bcb5c8 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp @@ -77,7 +77,7 @@ std::tuple Vlp32Decoder::get_pointcloud() int Vlp32Decoder::pointsPerPacket() { return BLOCKS_PER_PACKET * SCANS_PER_BLOCK; } -void Vlp32Decoder::reset_pointcloud(size_t n_pts) +void Vlp32Decoder::reset_pointcloud(size_t n_pts, double time_stamp) { // scan_pc_.reset(new NebulaPointCloud); scan_pc_->points.clear(); @@ -87,12 +87,43 @@ void Vlp32Decoder::reset_pointcloud(size_t n_pts) scan_timestamp_ = -1; } -void Vlp32Decoder::reset_overflow() +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_); + } + // 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_); } From abc4d39637172074cb574609b1b2e86bb965cb38 Mon Sep 17 00:00:00 2001 From: Benjamin Gilby Date: Tue, 9 Jan 2024 17:05:21 +0900 Subject: [PATCH 2/7] add definition and initialization of last_block_timestamp_ --- .../nebula_decoders_velodyne/decoders/vlp16_decoder.hpp | 1 + .../nebula_decoders_velodyne/decoders/vlp32_decoder.hpp | 1 + .../src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp | 3 +++ .../src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp | 3 +++ 4 files changed, 8 insertions(+) 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 ed8ef2e32..de60e96c6 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 @@ -52,6 +52,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 28db49afa..d1006b448 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 @@ -52,6 +52,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 db14679a2..2754ddaa4 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp @@ -256,6 +256,9 @@ 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]; + auto block_timestamp = rclcpp::Time(velodyne_packet.stamp).seconds(); + 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 a15bcb5c8..4ef0236da 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp @@ -275,6 +275,9 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa intensity = raw->blocks[i].data[k + 2]; + auto block_timestamp = rclcpp::Time(velodyne_packet.stamp).seconds(); + 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; From ffb2e79248179a53e3836d2dcb73433b57d119ed Mon Sep 17 00:00:00 2001 From: Benjamin Gilby <40575701+bgilby59@users.noreply.github.com> Date: Thu, 11 Jan 2024 11:17:02 +0900 Subject: [PATCH 3/7] Update nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp Co-authored-by: Kenzo Lobos Tsunekawa --- .../src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp | 1 - 1 file changed, 1 deletion(-) 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 f4383daba..0222c61e3 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp @@ -91,7 +91,6 @@ void Vlp16Decoder::reset_pointcloud(size_t n_pts, double time_stamp) 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(double time_stamp) From a92b2b5974f236511ee409177bf5caec10451cc6 Mon Sep 17 00:00:00 2001 From: Benjamin Gilby <40575701+bgilby59@users.noreply.github.com> Date: Thu, 11 Jan 2024 11:17:12 +0900 Subject: [PATCH 4/7] Update nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp Co-authored-by: Kenzo Lobos Tsunekawa --- .../src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp | 1 - 1 file changed, 1 deletion(-) 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 74b2be08f..589509953 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp @@ -90,7 +90,6 @@ void Vlp32Decoder::reset_pointcloud(size_t n_pts, double time_stamp) 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(double time_stamp) From d50f6b2e39460226c75ed2c7c00043582f632c12 Mon Sep 17 00:00:00 2001 From: Benjamin Gilby <40575701+bgilby59@users.noreply.github.com> Date: Thu, 11 Jan 2024 11:19:19 +0900 Subject: [PATCH 5/7] Update nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp Co-authored-by: Kenzo Lobos Tsunekawa --- .../src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp | 1 + 1 file changed, 1 insertion(+) 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 589509953..58a685251 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp @@ -111,6 +111,7 @@ void Vlp32Decoder::reset_overflow(double time_stamp) scan_timestamp_ = -1; overflow_pc_->points.clear(); overflow_pc_->points.reserve(max_pts_); + return; } // Add the overflow buffer points From 638c532f6838975ff87846d1f11751a6233a252b Mon Sep 17 00:00:00 2001 From: Benjamin Gilby Date: Thu, 11 Jan 2024 11:27:11 +0900 Subject: [PATCH 6/7] add missing return statements to vlp16 and vls128 decoders --- .../src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp | 6 ++++-- .../nebula_decoders_velodyne/decoders/vls128_decoder.cpp | 4 +++- 2 files changed, 7 insertions(+), 3 deletions(-) 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 0222c61e3..18b55c77a 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp @@ -112,6 +112,7 @@ void Vlp16Decoder::reset_overflow(double time_stamp) scan_timestamp_ = -1; overflow_pc_->points.clear(); overflow_pc_->points.reserve(max_pts_); + return; } // Add the overflow buffer points @@ -122,7 +123,8 @@ void Vlp16Decoder::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(); @@ -263,7 +265,7 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa auto block_timestamp = rclcpp::Time(velodyne_packet.stamp).seconds(); last_block_timestamp_ = block_timestamp; - double point_time_offset = timing_offsets_[block][firing * 16 + dsr]; + double point_time_offset = timing_offsets_[block][firing * 16 + dsr]; // Determine return type. uint8_t return_type; 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..09352e4c6 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(); From 12077a81dd3969d7210baaf1a92440ee132bc096 Mon Sep 17 00:00:00 2001 From: Benjamin Gilby Date: Thu, 11 Jan 2024 11:44:18 +0900 Subject: [PATCH 7/7] remove superfluous code --- .../src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp | 1 - .../src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp | 4 ++-- .../src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp | 4 ---- 3 files changed, 2 insertions(+), 7 deletions(-) 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 18b55c77a..e1e9062f0 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp @@ -262,7 +262,6 @@ 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]; - auto block_timestamp = rclcpp::Time(velodyne_packet.stamp).seconds(); last_block_timestamp_ = block_timestamp; double point_time_offset = timing_offsets_[block][firing * 16 + dsr]; 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 58a685251..25feaf8f7 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp @@ -122,7 +122,8 @@ void Vlp32Decoder::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(); @@ -281,7 +282,6 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa intensity = raw->blocks[i].data[k + 2]; - auto block_timestamp = rclcpp::Time(velodyne_packet.stamp).seconds(); last_block_timestamp_ = block_timestamp; const float focal_offset = 256 * (1 - corrections.focal_distance / 13100) * 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 09352e4c6..23442a6f3 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp @@ -282,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];