From 02f329abba50ec92a86636592b776f98e4979e2b 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

---
 ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp b/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp
index 9ee60e4..c6099ab 100644
--- a/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp
+++ b/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp
@@ -58,8 +58,9 @@ 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);
+    const auto time_int = static_cast<uint64_t>(time);
+    stamp.sec = static_cast<int32_t>(time_int / 1'000'000'000);
+    stamp.nanosec = static_cast<uint32_t>(time_int % 1'000'000'000);
     return stamp;
 };
 double ROSStampToTime(const builtin_interfaces::msg::Time stamp) {