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) {