ROS-350[NOETIC]: 't' timestamp field content is not plausible

.github/ISSUE_TEMPLATE/bug_report.md

@@ -7,6 +7,14 @@ assignees: ''
 
 ---
 
+> **IMPORTANT**  
+> Before reporting a new bug make sure you have:
+> - [ ] Checked existing issues https://github.com/ouster-lidar/ouster-ros/issues
+> - [ ] Checked existing issues https://github.com/ouster-lidar/ouster-ros/issues
+> - [ ] Checked ouster community https://community.ouster.com/
+> If you couldn't find relevant information and you think this is rather a driver 
+> problem then proceed with bug reporting.
+
 **Describe the bug**
 A clear and concise description of what the bug is.
 

CHANGELOG.rst

@@ -2,8 +2,14 @@
 Changelog
 =========
 
-[ouster_ros v0.13.0]
-====================
+
+[unreleased]
+============
+* [BUGFIX]: correctly align timestamps to the generated point cloud
+
+
+ouster_ros v0.13.0
+==================
 
 ouster_ros(1)
 -------------

src/os_sensor_nodelet.cpp

@@ -116,9 +116,9 @@ void OusterSensor::onInit() {
     create_services();
     create_publishers();
     attempt_reconnect = getPrivateNodeHandle().param("attempt_reconnect", false);
-    dormant_period_between_reconnects = 
+    dormant_period_between_reconnects =
         getPrivateNodeHandle().param("dormant_period_between_reconnects", 1.0);
-    reconnect_attempts_available = 
+    reconnect_attempts_available =
         getPrivateNodeHandle().param("max_failed_reconnect_attempts", INT_MAX);
     attempt_start();
 }

src/point_cloud_compose.h

@@ -3,10 +3,9 @@
 #include <pcl_conversions/pcl_conversions.h>
 #include <sensor_msgs/PointCloud2.h>
 
+#include "ouster_ros/common_point_types.h"
 #include "ouster_ros/os_point.h"
 #include "ouster_ros/sensor_point_types.h"
-#include "ouster_ros/common_point_types.h"
-
 #include "point_meta_helpers.h"
 #include "point_transform.h"
 
@@ -117,10 +116,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 +130,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 +147,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