Prevent negative sec and nano in TimeToROSStamp when time is large

PRBonn · Oct 22, 2024 · 02f329a · 02f329a
1 parent 6ba2e18
commit 02f329a
Showing 1 changed file with 3 additions and 2 deletions.
diff --git 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) {