From 8c8f801ad339b570540fe8bddac8907e5e5c8ff1 Mon Sep 17 00:00:00 2001
From: Tiziano Guadagnino <37455909+tizianoGuadagnino@users.noreply.github.com>
Date: Thu, 7 Nov 2024 19:00:51 +0100
Subject: [PATCH] Fix for at least two types of timestamps in PointCloud2
 messages (#13)

* Try with the count digits

* small thing

* Suggestion to be discussed

* Update comment

* Revert "Suggestion to be discussed"

This reverts commit 77091b1e696416a376d2de3e589ca4922bf1fc0c.

* Clean comments

---------

Co-authored-by: Benedikt Mersch <benedikt.mersch@gmail.com>
---
 ros/src/kinematic_icp_ros/utils/RosUtils.cpp | 18 ++++++++++++++++--
 1 file changed, 16 insertions(+), 2 deletions(-)

diff --git a/ros/src/kinematic_icp_ros/utils/RosUtils.cpp b/ros/src/kinematic_icp_ros/utils/RosUtils.cpp
index 198a1bd..3f840e6 100644
--- a/ros/src/kinematic_icp_ros/utils/RosUtils.cpp
+++ b/ros/src/kinematic_icp_ros/utils/RosUtils.cpp
@@ -22,6 +22,9 @@
 // SOFTWARE.
 #include "kinematic_icp_ros/utils/RosUtils.hpp"
 
+#include <cmath>
+#include <cstdint>
+
 namespace kinematic_icp_ros::utils {
 
 std::optional<PointField> GetTimestampField(const PointCloud2::ConstSharedPtr msg) {
@@ -54,13 +57,24 @@ 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) {
-            timestamps.emplace_back(static_cast<double>(*it));
+            double stampd = static_cast<double>(*it);
+            // If the number of digits is greater than 10 (which is the maximum number of digits
+            // that can be represented with a 32 bits integer), the stamp is in nanoseconds instead
+            // of seconds, perform conversion
+            if (number_of_digits_decimal_part(stampd) > 10) {
+                stampd *= 1e-9;
+            }
+            timestamps.emplace_back(stampd);
         }
         return timestamps;
     };