From a2d4e31dae83194d249b9b66852d7ac484f4b3de Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Thu, 7 Nov 2024 15:32:57 +0100
Subject: [PATCH] Revert "Suggestion to be discussed"

This reverts commit 77091b1e696416a376d2de3e589ca4922bf1fc0c.
---
 ros/src/kinematic_icp_ros/utils/RosUtils.cpp | 21 ++++++++++++--------
 1 file changed, 13 insertions(+), 8 deletions(-)

diff --git a/ros/src/kinematic_icp_ros/utils/RosUtils.cpp b/ros/src/kinematic_icp_ros/utils/RosUtils.cpp
index 84a08da..a8f05fa 100644
--- a/ros/src/kinematic_icp_ros/utils/RosUtils.cpp
+++ b/ros/src/kinematic_icp_ros/utils/RosUtils.cpp
@@ -22,7 +22,8 @@
 // SOFTWARE.
 #include "kinematic_icp_ros/utils/RosUtils.hpp"
 
-#include <limits>
+#include <cmath>
+#include <cstdint>
 
 namespace kinematic_icp_ros::utils {
 
@@ -56,19 +57,23 @@ std::vector<double> NormalizeTimestamps(const std::vector<double> &timestamps) {
 
 auto ExtractTimestampsFromMsg(const PointCloud2::ConstSharedPtr msg,
                               const PointField &timestamp_field) {
+    auto number_of_digits_decimal_part = [](const auto &stamp) {
+        const uint64_t number_of_seconds = static_cast<uint64_t>(std::round(stamp));
+        return number_of_seconds > 0 ? std::floor(std::log10(number_of_seconds) + 1) : 1;
+    };
     auto extract_timestamps =
-        [&msg]<typename T>(sensor_msgs::PointCloud2ConstIterator<T> &&it) -> std::vector<double> {
+        [&]<typename T>(sensor_msgs::PointCloud2ConstIterator<T> &&it) -> std::vector<double> {
         const size_t n_points = msg->height * msg->width;
         std::vector<double> timestamps;
         timestamps.reserve(n_points);
         for (size_t i = 0; i < n_points; ++i, ++it) {
-            double stamp = static_cast<double>(*it);
-            // If the stamp is larger than the max of int32, we assume the stamp is in
-            // nanoseconds instead of seconds and perform conversion
-            if (*it > std::numeric_limits<int32_t>::max()) {
-                stamp *= 1e-9;
+            double stampd = static_cast<double>(*it);
+            // If the number of digits is greater than 10,
+            // the stamp is in nanoseconds instead of seconds, perform conversion
+            if (number_of_digits_decimal_part(*it) > 10) {
+                stampd *= 1e-9;
             }
-            timestamps.emplace_back(stamp);
+            timestamps.emplace_back(stampd);
         }
         return timestamps;
     };