From 6a7ca13722647a37516fdd44b5817e664c6e4da2 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Mon, 18 Nov 2024 10:16:43 +0100 Subject: [PATCH] Try to take care of absolute timestamping of points + scan stamped at the end --- ros/src/kinematic_icp_ros/utils/TimeStampHandler.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/ros/src/kinematic_icp_ros/utils/TimeStampHandler.cpp b/ros/src/kinematic_icp_ros/utils/TimeStampHandler.cpp index e76f5d9..7759c39 100644 --- a/ros/src/kinematic_icp_ros/utils/TimeStampHandler.cpp +++ b/ros/src/kinematic_icp_ros/utils/TimeStampHandler.cpp @@ -100,7 +100,6 @@ std::vector GetTimestamps(const PointCloud2::ConstSharedPtr msg) { return timestamps; } - } // namespace namespace kinematic_icp_ros::utils { @@ -117,15 +116,21 @@ std::tuple> TimeStampHandler::ProcessT const double &min_stamp_in_seconds = *min_it; // Check if stamping happen and the beginning or the end of scan const double msg_stamp_in_seconds = this->toTime(msg_stamp); - if (max_stamp_in_seconds > msg_stamp_in_seconds) { + double begin_normalization = min_stamp_in_seconds; + if (msg_stamp_in_seconds < max_stamp_in_seconds) { + // begin + absolute stamping -> add scan duration to the stamp const auto scan_duration = tf2::durationFromSec(max_stamp_in_seconds - min_stamp_in_seconds); end_stamp = StampType(rclcpp::Time(end_stamp) + scan_duration); + } else if (std::abs(msg_stamp_in_seconds - max_stamp_in_seconds) < 1e-8) { + // end-stamping + absolute stamping -> need to put negative normalized timestamps for + // deskewing + begin_normalization = msg_stamp_in_seconds; } // Normalize timestamps std::transform(timestamps.cbegin(), timestamps.cend(), timestamps.begin(), [&](const auto ×tamp) { - return (timestamp - min_stamp_in_seconds) / + return (timestamp - begin_normalization) / (max_stamp_in_seconds - min_stamp_in_seconds); }); }