From 04d8d3832ae0e9571a54785ea939cdacc8bd059d Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Tue, 9 Jan 2024 18:15:30 +0900 Subject: [PATCH] fix(velodyne_decoder): overflow handling in vls128 (#111) * fix: overflow handling in vls128 The timestamps in the vls128 were still relative to the last pointcloud, which was producing some errors Additionally, when entire scans were dropped, the overflow was not being handled correctly (this bug was also present in the awf velodyne driver) Signed-off-by: Kenzo Lobos-Tsunekawa * chore: fixed spelling Signed-off-by: Kenzo Lobos-Tsunekawa * fix: relative time was in seconds Signed-off-by: Kenzo Lobos-Tsunekawa * fix: timestamp handling and overflow order. error happen then the computed time is higher than the next measured one... Signed-off-by: Kenzo Lobos-Tsunekawa * Update nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp Co-authored-by: Benjamin Gilby <40575701+bgilby59@users.noreply.github.com> * Update nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp Co-authored-by: Benjamin Gilby <40575701+bgilby59@users.noreply.github.com> --------- Signed-off-by: Kenzo Lobos-Tsunekawa Co-authored-by: Benjamin Gilby <40575701+bgilby59@users.noreply.github.com> --- .../decoders/velodyne_scan_decoder.hpp | 4 +- .../decoders/vlp16_decoder.hpp | 6 +- .../decoders/vlp32_decoder.hpp | 4 +- .../decoders/vls128_decoder.hpp | 5 +- .../decoders/vlp16_decoder.cpp | 47 ++++++----- .../decoders/vlp32_decoder.cpp | 43 +++++----- .../decoders/vls128_decoder.cpp | 79 ++++++++++++++----- .../velodyne_driver.cpp | 8 +- 8 files changed, 125 insertions(+), 71 deletions(-) diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp index f6a5b5fb2..50937e66f 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp @@ -185,9 +185,9 @@ class VelodyneScanDecoder virtual std::tuple get_pointcloud() = 0; /// @brief Resetting point cloud buffer /// @param n_pts # of points - virtual void reset_pointcloud(size_t n_pts) = 0; + virtual void reset_pointcloud(size_t n_pts, double time_stamp) = 0; /// @brief Resetting overflowed point cloud buffer - virtual void reset_overflow() = 0; + virtual void reset_overflow(double time_stamp) = 0; }; } // namespace drivers 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..f9905373f 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 @@ -13,7 +13,7 @@ namespace drivers { namespace vlp16 { - constexpr uint32_t MAX_POINTS = 300000; +constexpr uint32_t MAX_POINTS = 300000; /// @brief Velodyne LiDAR decoder (VLP16) class Vlp16Decoder : public VelodyneScanDecoder { @@ -38,9 +38,9 @@ class Vlp16Decoder : public VelodyneScanDecoder std::tuple get_pointcloud() override; /// @brief Resetting point cloud buffer /// @param n_pts # of points - void reset_pointcloud(size_t n_pts) override; + void reset_pointcloud(size_t n_pts, double time_stamp) override; /// @brief Resetting overflowed point cloud buffer - void reset_overflow() override; + void reset_overflow(double time_stamp) override; private: /// @brief Parsing VelodynePacket based on packet structure 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..3f9b1c27d 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 @@ -37,9 +37,9 @@ class Vlp32Decoder : public VelodyneScanDecoder std::tuple get_pointcloud() override; /// @brief Resetting point cloud buffer /// @param n_pts # of points - void reset_pointcloud(size_t n_pts) override; + void reset_pointcloud(size_t n_pts, double time_stamp) override; /// @brief Resetting overflowed point cloud buffer - void reset_overflow() override; + void reset_overflow(double time_stamp) override; private: /// @brief Parsing VelodynePacket based on packet structure diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp index 627b838a5..e440096e9 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp @@ -37,9 +37,9 @@ class Vls128Decoder : public VelodyneScanDecoder std::tuple get_pointcloud() override; /// @brief Resetting point cloud buffer /// @param n_pts # of points - void reset_pointcloud(size_t n_pts) override; + void reset_pointcloud(size_t n_pts, double time_stamp) override; /// @brief Resetting overflowed point cloud buffer - void reset_overflow() override; + void reset_overflow(double time_stamp) override; private: /// @brief Parsing VelodynePacket based on packet structure @@ -52,6 +52,7 @@ class Vls128Decoder : public VelodyneScanDecoder float vls_128_laser_azimuth_cache_[16]; int phase_; int max_pts_; + double last_block_timestamp_; std::vector> timing_offsets_; }; 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..a7ed45804 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp @@ -30,43 +30,48 @@ Vlp16Decoder::Vlp16Decoder( } // timing table calculation, from velodyne user manual p.64 timing_offsets_.resize(BLOCKS_PER_PACKET); - for (size_t i=0; i < timing_offsets_.size(); ++i){ + for (size_t i = 0; i < timing_offsets_.size(); ++i) { timing_offsets_[i].resize(32); } double full_firing_cycle_s = 55.296 * 1e-6; double single_firing_s = 2.304 * 1e-6; double data_block_index, data_point_index; - bool dual_mode = sensor_configuration_->return_mode==ReturnMode::DUAL; + bool dual_mode = sensor_configuration_->return_mode == ReturnMode::DUAL; // compute timing offsets - for (size_t x = 0; x < timing_offsets_.size(); ++x){ - for (size_t y = 0; y < timing_offsets_[x].size(); ++y){ - if (dual_mode){ + for (size_t x = 0; x < timing_offsets_.size(); ++x) { + for (size_t y = 0; y < timing_offsets_[x].size(); ++y) { + if (dual_mode) { data_block_index = (x - (x % 2)) + (y / 16); - } - else{ + } else { data_block_index = (x * 2) + (y / 16); } data_point_index = y % 16; - timing_offsets_[x][y] = (full_firing_cycle_s * data_block_index) + (single_firing_s * data_point_index); + timing_offsets_[x][y] = + (full_firing_cycle_s * data_block_index) + (single_firing_s * data_point_index); } } phase_ = (uint16_t)round(sensor_configuration_->scan_phase * 100); } -bool Vlp16Decoder::hasScanned() { return has_scanned_; } +bool Vlp16Decoder::hasScanned() +{ + return has_scanned_; +} std::tuple Vlp16Decoder::get_pointcloud() { double phase = angles::from_degrees(sensor_configuration_->scan_phase); if (!scan_pc_->points.empty()) { auto current_azimuth = scan_pc_->points.back().azimuth; - auto phase_diff = static_cast(angles::to_degrees(2*M_PI + current_azimuth - phase)) % 360; + auto phase_diff = + static_cast(angles::to_degrees(2 * M_PI + current_azimuth - phase)) % 360; while (phase_diff < M_PI_2 && !scan_pc_->points.empty()) { overflow_pc_->points.push_back(scan_pc_->points.back()); scan_pc_->points.pop_back(); current_azimuth = scan_pc_->points.back().azimuth; - phase_diff = static_cast(angles::to_degrees(2*M_PI + current_azimuth - phase)) % 360; + phase_diff = + static_cast(angles::to_degrees(2 * M_PI + current_azimuth - phase)) % 360; } overflow_pc_->width = overflow_pc_->points.size(); scan_pc_->width = scan_pc_->points.size(); @@ -80,16 +85,16 @@ 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, [[maybe_unused]] double time_stamp) { scan_pc_->points.clear(); max_pts_ = n_pts * pointsPerPacket(); scan_pc_->points.reserve(max_pts_); - reset_overflow(); // transfer existing overflow points to the cleared pointcloud + reset_overflow(time_stamp); // transfer existing overflow points to the cleared pointcloud scan_timestamp_ = -1; } -void Vlp16Decoder::reset_overflow() +void Vlp16Decoder::reset_overflow([[maybe_unused]] double time_stamp) { // Add the overflow buffer points for (size_t i = 0; i < overflow_pc_->points.size(); i++) { @@ -225,7 +230,7 @@ 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]; - 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; @@ -237,9 +242,10 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa other_return.bytes[1] == current_return.bytes[1])) { return_type = static_cast(drivers::ReturnType::IDENTICAL); } else { - const uint8_t other_intensity = block % 2 ? raw->blocks[block - 1].data[k + 2] - : raw->blocks[block + 1].data[k + 2]; - bool first = current_return.uint > other_return.uint ; + const uint8_t other_intensity = block % 2 + ? raw->blocks[block - 1].data[k + 2] + : raw->blocks[block + 1].data[k + 2]; + bool first = current_return.uint > other_return.uint; bool strongest = intensity > other_intensity; if (other_intensity == intensity) { strongest = !first; @@ -275,9 +281,8 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa current_point.azimuth = rotation_radians_[azimuth_corrected]; current_point.elevation = sin_vert_angle; auto point_ts = block_timestamp - scan_timestamp_ + point_time_offset; - if (point_ts < 0) - point_ts = 0; - current_point.time_stamp = static_cast(point_ts*1e9); + if (point_ts < 0) point_ts = 0; + current_point.time_stamp = static_cast(point_ts * 1e9); current_point.intensity = intensity; current_point.distance = distance; scan_pc_->points.emplace_back(current_point); 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..5576a6f3d 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp @@ -31,42 +31,45 @@ Vlp32Decoder::Vlp32Decoder( phase_ = (uint16_t)round(sensor_configuration_->scan_phase * 100); timing_offsets_.resize(12); - for (size_t i=0; i < timing_offsets_.size(); ++i){ + for (size_t i = 0; i < timing_offsets_.size(); ++i) { timing_offsets_[i].resize(32); } // constants - double full_firing_cycle = 55.296 * 1e-6; // seconds - double single_firing = 2.304 * 1e-6; // seconds + double full_firing_cycle = 55.296 * 1e-6; // seconds + double single_firing = 2.304 * 1e-6; // seconds double dataBlockIndex, dataPointIndex; bool dual_mode = sensor_configuration_->return_mode == ReturnMode::DUAL; // compute timing offsets - for (size_t x = 0; x < timing_offsets_.size(); ++x){ - for (size_t y = 0; y < timing_offsets_[x].size(); ++y){ - if (dual_mode){ + for (size_t x = 0; x < timing_offsets_.size(); ++x) { + for (size_t y = 0; y < timing_offsets_[x].size(); ++y) { + if (dual_mode) { dataBlockIndex = x / 2; - } - else{ + } else { dataBlockIndex = x; } dataPointIndex = y / 2; - timing_offsets_[x][y] = (full_firing_cycle * dataBlockIndex) + (single_firing * dataPointIndex); + timing_offsets_[x][y] = + (full_firing_cycle * dataBlockIndex) + (single_firing * dataPointIndex); } } } -bool Vlp32Decoder::hasScanned() { return has_scanned_; } +bool Vlp32Decoder::hasScanned() +{ + return has_scanned_; +} std::tuple Vlp32Decoder::get_pointcloud() { double phase = angles::from_degrees(sensor_configuration_->scan_phase); if (!scan_pc_->points.empty()) { auto current_azimuth = scan_pc_->points.back().azimuth; - auto phase_diff = (2*M_PI + current_azimuth - phase); + auto phase_diff = (2 * M_PI + current_azimuth - phase); while (phase_diff < M_PI_2 && !scan_pc_->points.empty()) { overflow_pc_->points.push_back(scan_pc_->points.back()); scan_pc_->points.pop_back(); current_azimuth = scan_pc_->points.back().azimuth; - phase_diff = (2*M_PI + current_azimuth - phase); + phase_diff = (2 * M_PI + current_azimuth - phase); } overflow_pc_->width = overflow_pc_->points.size(); scan_pc_->width = scan_pc_->points.size(); @@ -75,19 +78,22 @@ std::tuple Vlp32Decoder::get_pointcloud() return std::make_tuple(scan_pc_, scan_timestamp_); } -int Vlp32Decoder::pointsPerPacket() { return BLOCKS_PER_PACKET * SCANS_PER_BLOCK; } +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, [[maybe_unused]] 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(); // transfer existing overflow points to the cleared pointcloud + reset_overflow(time_stamp); // transfer existing overflow points to the cleared pointcloud scan_timestamp_ = -1; } -void Vlp32Decoder::reset_overflow() +void Vlp32Decoder::reset_overflow([[maybe_unused]] double time_stamp) { // Add the overflow buffer points for (size_t i = 0; i < overflow_pc_->points.size(); i++) { @@ -302,9 +308,8 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa current_point.azimuth = rotation_radians_[block.rotation]; current_point.elevation = sin_vert_angle; auto point_ts = block_timestamp - scan_timestamp_ + point_time_offset; - if (point_ts < 0) - point_ts = 0; - current_point.time_stamp = static_cast(point_ts*1e9); + if (point_ts < 0) point_ts = 0; + current_point.time_stamp = static_cast(point_ts * 1e9); current_point.distance = distance; current_point.intensity = intensity; scan_pc_->points.emplace_back(current_point); 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 19480c7bf..b621d5482 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp @@ -36,37 +36,42 @@ Vls128Decoder::Vls128Decoder( } // timing table calculation, from velodyne user manual p.64 timing_offsets_.resize(3); - for(size_t i=0; i < timing_offsets_.size(); ++i) - { - timing_offsets_[i].resize(17); // 17 (+1 for the maintenance time after firing group 8) + for (size_t i = 0; i < timing_offsets_.size(); ++i) { + timing_offsets_[i].resize(17); // 17 (+1 for the maintenance time after firing group 8) } double full_firing_cycle_s = 53.3 * 1e-6; double single_firing_s = 2.665 * 1e-6; double offset_packet_time = 8.7 * 1e-6; // compute timing offsets - for (size_t x = 0; x < timing_offsets_.size(); ++x){ - for (size_t y = 0; y < timing_offsets_[x].size(); ++y){ + for (size_t x = 0; x < timing_offsets_.size(); ++x) { + for (size_t y = 0; y < timing_offsets_[x].size(); ++y) { double sequence_index, firing_group_index; sequence_index = x; firing_group_index = y; - timing_offsets_[x][y] = (full_firing_cycle_s * sequence_index) + (single_firing_s * firing_group_index) - offset_packet_time; + timing_offsets_[x][y] = (full_firing_cycle_s * sequence_index) + + (single_firing_s * firing_group_index) - offset_packet_time; } } } -bool Vls128Decoder::hasScanned() { return has_scanned_; } +bool Vls128Decoder::hasScanned() +{ + return has_scanned_; +} std::tuple Vls128Decoder::get_pointcloud() { double phase = angles::from_degrees(sensor_configuration_->scan_phase); if (!scan_pc_->points.empty()) { auto current_azimuth = scan_pc_->points.back().azimuth; - auto phase_diff = static_cast(angles::to_degrees(2*M_PI + current_azimuth - phase)) % 360; + auto phase_diff = + static_cast(angles::to_degrees(2 * M_PI + current_azimuth - phase)) % 360; while (phase_diff < M_PI_2 && !scan_pc_->points.empty()) { overflow_pc_->points.push_back(scan_pc_->points.back()); scan_pc_->points.pop_back(); current_azimuth = scan_pc_->points.back().azimuth; - phase_diff = static_cast(angles::to_degrees(2*M_PI + current_azimuth - phase)) % 360; + phase_diff = + static_cast(angles::to_degrees(2 * M_PI + current_azimuth - phase)) % 360; } overflow_pc_->width = overflow_pc_->points.size(); scan_pc_->width = scan_pc_->points.size(); @@ -75,24 +80,57 @@ std::tuple Vls128Decoder::get_pointcloud() return std::make_tuple(scan_pc_, scan_timestamp_); } -int Vls128Decoder::pointsPerPacket() { return BLOCKS_PER_PACKET * SCANS_PER_BLOCK; } +int Vls128Decoder::pointsPerPacket() +{ + return BLOCKS_PER_PACKET * SCANS_PER_BLOCK; +} -void Vls128Decoder::reset_pointcloud(size_t n_pts) +void Vls128Decoder::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(); // transfer existing overflow points to the cleared pointcloud - scan_timestamp_ = -1; + reset_overflow(time_stamp); // transfer existing overflow points to the cleared pointcloud } -void Vls128Decoder::reset_overflow() +void Vls128Decoder::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_); } @@ -243,10 +281,12 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p 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]; + double point_time_offset = + timing_offsets_[block / 4][firing_order + laser_number / 64]; // Determine return type. uint8_t return_type; @@ -297,9 +337,8 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p current_point.elevation = sin_vert_angle; current_point.distance = distance; auto point_ts = block_timestamp - scan_timestamp_ + point_time_offset; - if (point_ts < 0) - point_ts = 0; - current_point.time_stamp = static_cast(point_ts*1e9); + if (point_ts < 0) point_ts = 0; + current_point.time_stamp = static_cast(point_ts * 1e9); current_point.intensity = intensity; scan_pc_->points.emplace_back(current_point); } // 2nd scan area condition diff --git a/nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp b/nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp index 7b12e026b..78383d569 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp @@ -53,7 +53,8 @@ std::tuple VelodyneDriver::ConvertScanToPo { std::tuple pointcloud; if (driver_status_ == nebula::Status::OK) { - scan_decoder_->reset_pointcloud(velodyne_scan->packets.size()); + scan_decoder_->reset_pointcloud( + velodyne_scan->packets.size(), rclcpp::Time(velodyne_scan->packets.front().stamp).seconds()); for (auto & packet : velodyne_scan->packets) { scan_decoder_->unpack(packet); } @@ -63,7 +64,10 @@ std::tuple VelodyneDriver::ConvertScanToPo } return pointcloud; } -Status VelodyneDriver::GetStatus() { return driver_status_; } +Status VelodyneDriver::GetStatus() +{ + return driver_status_; +} } // namespace drivers } // namespace nebula