From c54b177267f4840f0a6ee0cffd5d7af3904112da Mon Sep 17 00:00:00 2001
From: Ussama Naal <ussama.naal@ouster.io>
Date: Fri, 29 Sep 2023 09:22:09 -0700
Subject: [PATCH] Port the alignment solution to ROS1

---
 include/ouster_ros/os_ros.h | 10 +++++-----
 src/laser_scan_processor.h  |  4 +++-
 src/os_ros.cpp              | 14 +++++++++-----
 3 files changed, 17 insertions(+), 11 deletions(-)

diff --git a/include/ouster_ros/os_ros.h b/include/ouster_ros/os_ros.h
index d053732b..c69bccd1 100644
--- a/include/ouster_ros/os_ros.h
+++ b/include/ouster_ros/os_ros.h
@@ -106,14 +106,14 @@ geometry_msgs::TransformStamped transform_to_tf_msg(
  * @param[in] frame the parent frame of the generated laser scan message
  * @param[in] lidar_mode lidar mode (width x frequency)
  * @param[in] ring selected ring to be published
+ * @param[in] pixel_shift_by_row pixel shifts by row
+ * @param[in] return_index index of return desired starting at 0
  * @return ROS message suitable for publishing as a LaserScan
  */
 sensor_msgs::LaserScan lidar_scan_to_laser_scan_msg(
-    const ouster::LidarScan& ls,
-    const ros::Time& timestamp,
-    const std::string &frame,
-    const ouster::sensor::lidar_mode lidar_mode,
-    const uint16_t ring,
+    const ouster::LidarScan& ls, const ros::Time& timestamp,
+    const std::string& frame, const ouster::sensor::lidar_mode lidar_mode,
+    const uint16_t ring, const std::vector<int>& pixel_shift_by_row,
     const int return_index);
 
 namespace impl {
diff --git a/src/laser_scan_processor.h b/src/laser_scan_processor.h
index ccd184e7..74960392 100644
--- a/src/laser_scan_processor.h
+++ b/src/laser_scan_processor.h
@@ -28,6 +28,7 @@ class LaserScanProcessor {
         : frame(frame_id),
           ld_mode(info.mode),
           ring_(ring),
+          pixel_shift_by_row(info.format.pixel_shift_by_row),
           scan_msgs(get_n_returns(info),
                     std::make_shared<sensor_msgs::LaserScan>()),
           post_processing_fn(func) {}
@@ -37,7 +38,7 @@ class LaserScanProcessor {
                  const ros::Time& msg_ts) {
         for (int i = 0; i < static_cast<int>(scan_msgs.size()); ++i) {
             *scan_msgs[i] = lidar_scan_to_laser_scan_msg(
-                lidar_scan, msg_ts, frame, ld_mode, ring_, i);
+                lidar_scan, msg_ts, frame, ld_mode, ring_, pixel_shift_by_row, i);
         }
 
         if (post_processing_fn) post_processing_fn(scan_msgs);
@@ -60,6 +61,7 @@ class LaserScanProcessor {
     std::string frame;
     sensor::lidar_mode ld_mode;
     uint16_t ring_;
+    std::vector<int> pixel_shift_by_row;
     OutputType scan_msgs;
     PostProcessingFn post_processing_fn;
 };
diff --git a/src/os_ros.cpp b/src/os_ros.cpp
index 0dd13f67..f2cc7ed2 100644
--- a/src/os_ros.cpp
+++ b/src/os_ros.cpp
@@ -150,7 +150,8 @@ geometry_msgs::TransformStamped transform_to_tf_msg(
 sensor_msgs::LaserScan lidar_scan_to_laser_scan_msg(
     const ouster::LidarScan& ls, const ros::Time& timestamp,
     const std::string& frame, const ouster::sensor::lidar_mode ld_mode,
-    const uint16_t ring, const int return_index) {
+    const uint16_t ring, const std::vector<int>& pixel_shift_by_row,
+    const int return_index) {
     sensor_msgs::LaserScan msg;
     msg.header.stamp = timestamp;
     msg.header.frame_id = frame;
@@ -176,10 +177,13 @@ sensor_msgs::LaserScan lidar_scan_to_laser_scan_msg(
     const auto sg = signal.data();
     msg.ranges.resize(ls.w);
     msg.intensities.resize(ls.w);
-    int idx = ls.w * ring + ls.w;
-    for (int i = 0; idx-- > ls.w * ring; ++i) {
-        msg.ranges[i] = rg[idx] * ouster::sensor::range_unit;
-        msg.intensities[i] = static_cast<float>(sg[idx]);
+
+    const auto u = ring;
+    for (auto v = 0; v < ls.w; ++v) {
+        const auto v_shift = (ls.w - 1 - (v + ls.w / 2 + pixel_shift_by_row[u])) % ls.w;
+        const auto src_idx = u * ls.w + v_shift;
+        msg.ranges[v] = rg[src_idx] * ouster::sensor::range_unit;
+        msg.intensities[v] = static_cast<float>(sg[src_idx]);
     }
 
     return msg;