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

…timestamp Signed-off-by: Max SCHMELLER <[email protected]>
tier4 · Sep 30, 2024 · f3458a7 · f3458a7
1 parent 9f706e3
commit f3458a7
Show file tree

Hide file tree

Showing 10 changed files with 13 additions and 10 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
@@ -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);
   }