From aac00343dba54a8fa395edbf71380fe399d35b5c Mon Sep 17 00:00:00 2001
From: Ken <ken.hasselmann@mil.be>
Date: Tue, 22 Oct 2024 22:10:48 +0200
Subject: [PATCH] Prevent negative sec and nano in TimeToROSStamp when time is
 large, replaced Time functions with standard methods.

---
 .../server/LidarOdometryServer.cpp            | 21 ++++---------------
 1 file changed, 4 insertions(+), 17 deletions(-)

diff --git a/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp b/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp
index 9ee60e4..98d192c 100644
--- a/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp
+++ b/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp
@@ -34,7 +34,6 @@
 #include "kinematic_icp_ros/utils/RosUtils.hpp"
 
 // ROS 2 headers
-#include <tf2/time.h>
 #include <tf2_ros/buffer_interface.h>
 #include <tf2_ros/transform_broadcaster.h>
 
@@ -54,19 +53,7 @@ namespace {
 using milliseconds = std::chrono::milliseconds;
 using seconds = std::chrono::duration<long double>;
 using std::chrono::duration_cast;
-
-// Convert to ros time
-auto TimeToROSStamp(const double &time) {
-    builtin_interfaces::msg::Time stamp;
-    stamp.sec = static_cast<int32_t>(std::floor(time));
-    stamp.nanosec = static_cast<uint32_t>((time - stamp.sec) * 1e9);
-    return stamp;
-};
-double ROSStampToTime(const builtin_interfaces::msg::Time stamp) {
-    double time = static_cast<double>(stamp.sec);
-    time += static_cast<double>(stamp.nanosec) * 1e-9;
-    return time;
-};
+using Time = builtin_interfaces::msg::Time;
 }  // namespace
 
 namespace kinematic_icp_ros {
@@ -199,9 +186,9 @@ void LidarOdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::Con
     const auto &[min_it, max_it] = std::minmax_element(timestamps.cbegin(), timestamps.cend());
     // Update what is the current stamp of this iteration
     const auto begin_scan_stamp =
-        min_it != timestamps.cend() ? TimeToROSStamp(*min_it) : last_stamp;
+        min_it != timestamps.cend() ? Time{rclcpp::Time(*min_it)} : last_stamp;
     const auto end_scan_stamp =
-        max_it != timestamps.cend() ? TimeToROSStamp(*max_it) : msg->header.stamp;
+        max_it != timestamps.cend() ? Time{rclcpp::Time(*max_it)} : msg->header.stamp;
     current_stamp_ = end_scan_stamp;
     // Get the initial guess from the wheel odometry
     const auto delta =
@@ -219,7 +206,7 @@ void LidarOdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::Con
     }
 
     // Compute velocities, use the elapsed time between the current msg and the last received
-    const double elapsed_time = ROSStampToTime(end_scan_stamp) - ROSStampToTime(begin_scan_stamp);
+    const double elapsed_time = rclcpp::Time(current_stamp_).nanoseconds() - rclcpp::Time(last_stamp).nanoseconds();
     const Sophus::SE3d::Tangent delta_twist = (last_pose.inverse() * kinematic_icp_->pose()).log();
     const Sophus::SE3d::Tangent velocity = delta_twist / elapsed_time;