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 77091b1.

* Clean comments

---------

Co-authored-by: Benedikt Mersch <[email protected]>

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;
     };