From fc911d1abd14bb1c2ca73b9b76dd57b8bfba45b4 Mon Sep 17 00:00:00 2001
From: Ussama Naal <606033+Samahu@users.noreply.github.com>
Date: Fri, 11 Oct 2024 03:30:42 +0300
Subject: [PATCH] ROS-350[HUMBLE/IRON/JAZZY]: 't' timestamp field content is
 not plausible (#387)

Align timestamps based on staggered/destaggered option
---
 CHANGELOG.rst                        |  4 ++++
 ouster-ros/package.xml               |  2 +-
 ouster-ros/src/point_cloud_compose.h | 25 ++++++++++++++-----------
 3 files changed, 19 insertions(+), 12 deletions(-)

diff --git a/CHANGELOG.rst b/CHANGELOG.rst
index ea55f61b..135d4bed 100644
--- a/CHANGELOG.rst
+++ b/CHANGELOG.rst
@@ -2,6 +2,10 @@
 Changelog
 =========
 
+[unreleased]
+============
+* [BUGFIX]: correctly align timestamps to the generated point cloud.
+
 ouster_ros v0.13.2
 ==================
 * [BUGFIX]: Make sure to initialize the sensor with launch file parameters.
diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml
index b00538b4..a2a1bd15 100644
--- a/ouster-ros/package.xml
+++ b/ouster-ros/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>ouster_ros</name>
-  <version>0.13.2</version>
+  <version>0.13.3</version>
   <description>Ouster ROS2 driver</description>
   <maintainer email="oss@ouster.io">ouster developers</maintainer>
   <license file="LICENSE">BSD</license>
diff --git a/ouster-ros/src/point_cloud_compose.h b/ouster-ros/src/point_cloud_compose.h
index 6b425438..2e4895cd 100644
--- a/ouster-ros/src/point_cloud_compose.h
+++ b/ouster-ros/src/point_cloud_compose.h
@@ -117,10 +117,9 @@ using Cloud = pcl::PointCloud<T>;
 // TODO[UN]: make this a functor
 template <std::size_t N, const ChanFieldTable<N>& PROFILE, typename PointT,
           typename PointS>
-void scan_to_cloud_f(ouster_ros::Cloud<PointT>& cloud,
-                     PointS& staging_point,
-                     const ouster::PointsF& points,
-                     uint64_t scan_ts, const ouster::LidarScan& ls,
+void scan_to_cloud_f(ouster_ros::Cloud<PointT>& cloud, PointS& staging_point,
+                     const ouster::PointsF& points, uint64_t scan_ts,
+                     const ouster::LidarScan& ls,
                      const std::vector<int>& pixel_shift_by_row,
                      bool organized = false, bool destagger = true,
                      int rows_step = 1) {
@@ -132,11 +131,12 @@ void scan_to_cloud_f(ouster_ros::Cloud<PointT>& cloud,
 
     for (auto u = 0; u < ls.h; u += rows_step) {
         for (auto v = 0; v < ls.w; ++v) {   // TODO[UN]: consider cols_step in future
-            const auto v_shift = destagger ?
-                (v + ls.w - pixel_shift_by_row[u]) % ls.w : v;
+            const auto v_shift =
+                destagger ? (v + ls.w - pixel_shift_by_row[u]) % ls.w : v;
             const auto src_idx = u * ls.w + v_shift;
             const auto xyz = points.row(src_idx);
-            const auto tgt_idx = organized ? (u / rows_step) * ls.w + v : cloud.size();
+            const auto tgt_idx =
+                organized ? (u / rows_step) * ls.w + v : cloud.size();
 
             if (xyz.isNaN().any()) {
                 if (organized) {
@@ -148,12 +148,15 @@ void scan_to_cloud_f(ouster_ros::Cloud<PointT>& cloud,
                 }
                 continue;
             } else {
-                if (!organized)
-                    cloud.points.emplace_back();
+                if (!organized) cloud.points.emplace_back();
             }
 
-            auto ts = timestamp[v_shift];
-            ts = ts > scan_ts ? ts - scan_ts : 0UL;
+            // as opposed to the point cloud destaggering if it is disabled
+            // then timestamps needs to be staggered.
+            auto ts_idx =
+                destagger ? v : (v + ls.w + pixel_shift_by_row[u]) % ls.w;
+            auto ts =
+                timestamp[ts_idx] > scan_ts ? timestamp[ts_idx] - scan_ts : 0UL;
 
             // if target point and staging point has matching type bind the
             // target directly and avoid performing transform_point at the end