From f3458a70a43c210fc8476b814f78150ca4ec7318 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Fri, 27 Sep 2024 19:10:30 +0900 Subject: [PATCH 1/2] fix(velodyne): propagate sub-second part of timestamps to pointcloud timestamp Signed-off-by: Max SCHMELLER --- .../decoders/velodyne_scan_decoder.hpp | 2 +- .../nebula_decoders_velodyne/decoders/vlp16_decoder.hpp | 2 +- .../nebula_decoders_velodyne/decoders/vlp32_decoder.hpp | 2 +- .../nebula_decoders_velodyne/decoders/vls128_decoder.hpp | 2 +- .../nebula_decoders_velodyne/velodyne_driver.hpp | 2 +- .../src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp | 2 +- .../src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp | 2 +- .../src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp | 2 +- .../src/nebula_decoders_velodyne/velodyne_driver.cpp | 2 +- nebula_ros/src/velodyne/decoder_wrapper.cpp | 5 ++++- 10 files changed, 13 insertions(+), 10 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 5fab49cc9..d2391c982 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 @@ -221,7 +221,7 @@ class VelodyneScanDecoder /// @brief Virtual function for parsing and shaping VelodynePacket /// @param pandar_packet - virtual void unpack(const std::vector & packet, int32_t packet_seconds) = 0; + virtual void unpack(const std::vector & packet, double packet_seconds) = 0; /// @brief Virtual function for parsing VelodynePacket based on packet structure /// @param pandar_packet /// @return Resulting flag 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 645b7fdaf..8d95758ca 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 @@ -44,7 +44,7 @@ class Vlp16Decoder : public VelodyneScanDecoder calibration_configuration); /// @brief Parsing and shaping VelodynePacket /// @param velodyne_packet - void unpack(const std::vector & packet, int32_t packet_seconds) override; + void unpack(const std::vector & packet, double packet_seconds) override; /// @brief Calculation of points in each packet /// @return # of points int pointsPerPacket() override; 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 3d434af0c..e17e967c0 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 @@ -43,7 +43,7 @@ class Vlp32Decoder : public VelodyneScanDecoder calibration_configuration); /// @brief Parsing and shaping VelodynePacket /// @param velodyne_packet - void unpack(const std::vector & packet, int32_t packet_seconds) override; + void unpack(const std::vector & packet, double packet_seconds) override; /// @brief Calculation of points in each packet /// @return # of points int pointsPerPacket() override; 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 0dc4ba563..59335e126 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 @@ -43,7 +43,7 @@ class Vls128Decoder : public VelodyneScanDecoder calibration_configuration); /// @brief Parsing and shaping VelodynePacket /// @param velodyne_packet - void unpack(const std::vector & packet, int32_t packet_seconds) override; + void unpack(const std::vector & packet, double packet_seconds) override; /// @brief Calculation of points in each packet /// @return # of points int pointsPerPacket() override; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp index 746cec719..fc52e68dc 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp @@ -71,7 +71,7 @@ class VelodyneDriver : NebulaDriverBase /// @param velodyne_scan Message /// @return tuple of Point cloud and timestamp std::tuple ParseCloudPacket( - const std::vector & packet, int32_t packet_seconds); + const std::vector & packet, double packet_seconds); }; } // namespace drivers 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 d73eb29c4..1fa656612 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp @@ -134,7 +134,7 @@ void Vlp16Decoder::reset_overflow(double time_stamp) overflow_pc_->points.reserve(max_pts_); } -void Vlp16Decoder::unpack(const std::vector & packet, int32_t packet_seconds) +void Vlp16Decoder::unpack(const std::vector & packet, double packet_seconds) { checkAndHandleScanComplete(packet, packet_seconds, phase_); 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 3bf88e2a3..15224c0a5 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp @@ -133,7 +133,7 @@ void Vlp32Decoder::reset_overflow(double time_stamp) overflow_pc_->points.reserve(max_pts_); } -void Vlp32Decoder::unpack(const std::vector & packet, int32_t packet_seconds) +void Vlp32Decoder::unpack(const std::vector & packet, double packet_seconds) { checkAndHandleScanComplete(packet, packet_seconds, phase_); 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 9c683d48d..4d55c54cc 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp @@ -135,7 +135,7 @@ void Vls128Decoder::reset_overflow(double time_stamp) overflow_pc_->points.reserve(max_pts_); } -void Vls128Decoder::unpack(const std::vector & packet, int32_t packet_seconds) +void Vls128Decoder::unpack(const std::vector & packet, double packet_seconds) { checkAndHandleScanComplete(packet, packet_seconds, phase_); diff --git a/nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp b/nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp index c173e6a9d..5892c782e 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp @@ -50,7 +50,7 @@ Status VelodyneDriver::SetCalibrationConfiguration( } std::tuple VelodyneDriver::ParseCloudPacket( - const std::vector & packet, int32_t packet_seconds) + const std::vector & packet, double packet_seconds) { std::tuple pointcloud; diff --git a/nebula_ros/src/velodyne/decoder_wrapper.cpp b/nebula_ros/src/velodyne/decoder_wrapper.cpp index f55d7f28e..a98f74e8a 100644 --- a/nebula_ros/src/velodyne/decoder_wrapper.cpp +++ b/nebula_ros/src/velodyne/decoder_wrapper.cpp @@ -2,6 +2,8 @@ #include "nebula_ros/velodyne/decoder_wrapper.hpp" +#include + namespace nebula { namespace ros @@ -176,7 +178,8 @@ void VelodyneDecoderWrapper::ProcessCloudPacket( nebula::drivers::NebulaPointCloudPtr pointcloud = nullptr; { std::lock_guard lock(mtx_driver_ptr_); - pointcloud_ts = driver_ptr_->ParseCloudPacket(packet_msg->data, packet_msg->stamp.sec); + pointcloud_ts = + driver_ptr_->ParseCloudPacket(packet_msg->data, rclcpp::Time(packet_msg->stamp).seconds()); pointcloud = std::get<0>(pointcloud_ts); } From 3af07cfabe761a254c49fb45340418455fcb3e69 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 1 Oct 2024 15:48:27 +0900 Subject: [PATCH 2/2] fix: change signature of `checkAndHandleScanComplete` to double for timestamp Signed-off-by: Max SCHMELLER --- .../decoders/velodyne_scan_decoder.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 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 d2391c982..82cff5ac6 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 @@ -165,10 +165,10 @@ class VelodyneScanDecoder /// TODO: this has been moved from velodyne_hw_interface.cpp and is a temporary solution until /// the Velodyne decoder uses the same structure as Hesai/Robosense /// @param packet The packet buffer to extract azimuths from - /// @param packet_seconds The seconds-since-epoch part of the packet's timestamp + /// @param packet_seconds The packet's timestamp in seconds, including the sub-second part /// @param phase The sensor's scan phase used for scan cutting void checkAndHandleScanComplete( - const std::vector & packet, int32_t packet_seconds, const uint32_t phase) + const std::vector & packet, double packet_seconds, const uint32_t phase) { if (has_scanned_) { processed_packets_ = 0;