ros2bridge: fix detection and conversion of float64 point stamps

doc/source/doxygen-docs/changelog.md

@@ -3,7 +3,9 @@
 # Version 2.13.5: UNRELEASED
 - Changes in libraries:
   - \ref mrpt_ros2bridge_grp:
-    - mrpt::ros2bridge::toROS() conversion from `PointCloud2` to mrpt::maps::CPointsMapXYZIRT: recognize timestamp field names `"t"` and `"timestamp"`, apart of `"time"`.
+    - mrpt::ros2bridge::toROS() conversion from `PointCloud2` to mrpt::maps::CPointsMapXYZIRT:
+      - Recognize timestamp field names `"t"` and `"timestamp"`, apart of `"time"`.
+      - Fixed detection of timestamp fields in `Float64` format, and convert them from absolute to relative timestamps.
 - Build system:
   - This main MRPT repository is no longer directly built as a ROS package. Please, use the wrappers for better modularity:
     - https://github.com/MRPT/mrpt_ros

libs/maps/src/obs/CObservationPointCloud.cpp

@@ -70,8 +70,8 @@ void CObservationPointCloud::getDescriptionAsText(std::ostream& o) const
     {
       float Tmin, Tmax;
       mrpt::math::minimum_maximum(*ptrTs, Tmin, Tmax);
-      o << "Timestamp channel values: min=" << Tmin << " max=" << Tmax << " (" << ptrTs->size()
-        << " entries)\n";
+      o << mrpt::format("Timestamp channel values: min=%f max=%f", Tmin, Tmax);
+      o << "(" << ptrTs->size() << " entries)\n";
     }
     if (auto* ptrRs = pointcloud->getPointsBufferRef_ring(); ptrRs && !ptrRs->empty())
     {

libs/ros2bridge/src/point_cloud2.cpp

@@ -53,12 +53,27 @@ static void get_float_from_field(
   {
     if (field->datatype == sensor_msgs::msg::PointField::FLOAT32)
       output = *(reinterpret_cast<const float*>(&data[field->offset]));
-    else
-      output = (float)(*(reinterpret_cast<const double*>(&data[field->offset])));
+    else if (field->datatype == sensor_msgs::msg::PointField::FLOAT64)
+      output = static_cast<float>(*(reinterpret_cast<const double*>(&data[field->offset])));
   }
   else
     output = 0.0;
 }
+
+static void get_double_from_field(
+    const sensor_msgs::msg::PointField* field, const unsigned char* data, double& output)
+{
+  if (field != nullptr)
+  {
+    if (field->datatype == sensor_msgs::msg::PointField::FLOAT32)
+      output = static_cast<double>(*(reinterpret_cast<const float*>(&data[field->offset])));
+    else if (field->datatype == sensor_msgs::msg::PointField::FLOAT64)
+      output = *(reinterpret_cast<const double*>(&data[field->offset]));
+  }
+  else
+    output = 0.0;
+}
+
 static void get_uint16_from_field(
     const sensor_msgs::msg::PointField* field, const unsigned char* data, uint16_t& output)
 {
@@ -190,6 +205,7 @@ bool mrpt::ros2bridge::fromROS(const sensor_msgs::msg::PointCloud2& msg, CPoints
   obj.resize_XYZIRT(num_points, !!i_field, !!r_field, !!t_field);
 
   unsigned int idx = 0;
+  std::optional<double> baseTimeStamp;
   for (unsigned int row = 0; row < msg.height; ++row)
   {
     const unsigned char* row_data = &msg.data[row * msg.row_step];
@@ -217,9 +233,22 @@ bool mrpt::ros2bridge::fromROS(const sensor_msgs::msg::PointCloud2& msg, CPoints
       }
       if (t_field)
       {
-        float t;
-        get_float_from_field(t_field, msg_data, t);
-        obj.setPointTime(idx, t);
+        double t;
+        get_double_from_field(t_field, msg_data, t);
+
+        // If the sensor uses absolute timestamp, convert them to relative
+        // since otherwise precision is lost in the double->float conversion:
+        if (std::abs(t) > 5.0)
+        {
+          // It looks like absolute timestamps, convert to relative:
+          if (!baseTimeStamp) baseTimeStamp = t;
+          obj.setPointTime(idx, static_cast<float>(t - *baseTimeStamp));
+        }
+        else
+        {
+          // It looks relative timestamps:
+          obj.setPointTime(idx, static_cast<float>(t));
+        }
       }
     }
   }