fix(velodyne): propagate sub-second part of timestamps to pointcloud …

…timestamp (#201) * fix(velodyne): propagate sub-second part of timestamps to pointcloud timestamp Signed-off-by: Max SCHMELLER <[email protected]> * fix: change signature of `checkAndHandleScanComplete` to double for timestamp Signed-off-by: Max SCHMELLER <[email protected]> --------- Signed-off-by: Max SCHMELLER <[email protected]>
tier4 · Oct 1, 2024 · 6677df3 · 6677df3
1 parent 9f706e3
commit 6677df3
Show file tree

Hide file tree

Showing 10 changed files with 15 additions and 12 deletions.
diff --git a/...oders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp b/...oders/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<uint8_t> & packet, int32_t packet_seconds, const uint32_t phase)
+    const std::vector<uint8_t> & packet, double packet_seconds, const uint32_t phase)
   {
     if (has_scanned_) {
       processed_packets_ = 0;
@@ -221,7 +221,7 @@ class VelodyneScanDecoder
 
   /// @brief Virtual function for parsing and shaping VelodynePacket
   /// @param pandar_packet
-  virtual void unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds) = 0;
+  virtual void unpack(const std::vector<uint8_t> & 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
@@ -44,7 +44,7 @@ class Vlp16Decoder : public VelodyneScanDecoder
       calibration_configuration);
   /// @brief Parsing and shaping VelodynePacket
   /// @param velodyne_packet
-  void unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds) override;
+  void unpack(const std::vector<uint8_t> & 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
@@ -43,7 +43,7 @@ class Vlp32Decoder : public VelodyneScanDecoder
       calibration_configuration);
   /// @brief Parsing and shaping VelodynePacket
   /// @param velodyne_packet
-  void unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds) override;
+  void unpack(const std::vector<uint8_t> & 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
@@ -43,7 +43,7 @@ class Vls128Decoder : public VelodyneScanDecoder
       calibration_configuration);
   /// @brief Parsing and shaping VelodynePacket
   /// @param velodyne_packet
-  void unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds) override;
+  void unpack(const std::vector<uint8_t> & 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
@@ -71,7 +71,7 @@ class VelodyneDriver : NebulaDriverBase
   /// @param velodyne_scan Message
   /// @return tuple of Point cloud and timestamp
   std::tuple<drivers::NebulaPointCloudPtr, double> ParseCloudPacket(
-    const std::vector<uint8_t> & packet, int32_t packet_seconds);
+    const std::vector<uint8_t> & 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
@@ -134,7 +134,7 @@ void Vlp16Decoder::reset_overflow(double time_stamp)
   overflow_pc_->points.reserve(max_pts_);
 }
 
-void Vlp16Decoder::unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds)
+void Vlp16Decoder::unpack(const std::vector<uint8_t> & 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
@@ -133,7 +133,7 @@ void Vlp32Decoder::reset_overflow(double time_stamp)
   overflow_pc_->points.reserve(max_pts_);
 }
 
-void Vlp32Decoder::unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds)
+void Vlp32Decoder::unpack(const std::vector<uint8_t> & 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
@@ -135,7 +135,7 @@ void Vls128Decoder::reset_overflow(double time_stamp)
   overflow_pc_->points.reserve(max_pts_);
 }
 
-void Vls128Decoder::unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds)
+void Vls128Decoder::unpack(const std::vector<uint8_t> & 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
@@ -50,7 +50,7 @@ Status VelodyneDriver::SetCalibrationConfiguration(
 }
 
 std::tuple<drivers::NebulaPointCloudPtr, double> VelodyneDriver::ParseCloudPacket(
-  const std::vector<uint8_t> & packet, int32_t packet_seconds)
+  const std::vector<uint8_t> & packet, double packet_seconds)
 {
   std::tuple<drivers::NebulaPointCloudPtr, double> pointcloud;
 

diff --git 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 <rclcpp/time.hpp>
+
 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);
   }