From f903e15af0c3a62d0c98a2329535ad75ef3ff68f Mon Sep 17 00:00:00 2001
From: Ussama Naal <ussama.naal@ouster.io>
Date: Thu, 23 Nov 2023 11:55:46 -0800
Subject: [PATCH] address an issue where LaserScan appeared different on FW
 prior to 2.4

---
 CHANGELOG.rst                          |  1 +
 ouster-ros/include/ouster_ros/os_ros.h |  1 +
 ouster-ros/src/laser_scan_processor.h  | 24 +++++++++++++++++-------
 ouster-ros/src/os_ros.cpp              | 22 +++++++++++++++++++++-
 4 files changed, 40 insertions(+), 8 deletions(-)

diff --git a/CHANGELOG.rst b/CHANGELOG.rst
index 8c97112a..ffac6b65 100644
--- a/CHANGELOG.rst
+++ b/CHANGELOG.rst
@@ -5,6 +5,7 @@ Changelog
 [unreleased]
 ============
 * [BUGFIX]: LaserScan is not properly aligned with generated point cloud
+  * address an issue where LaserScan appeared different on FW prior to 2.4
 * [BUGFIX]: LaserScan does not work when using dual mode
 
 
diff --git a/ouster-ros/include/ouster_ros/os_ros.h b/ouster-ros/include/ouster_ros/os_ros.h
index 60dbf25b..10fbf66b 100644
--- a/ouster-ros/include/ouster_ros/os_ros.h
+++ b/ouster-ros/include/ouster_ros/os_ros.h
@@ -156,6 +156,7 @@ inline bool check_token(const std::set<std::string>& tokens,
     return tokens.find(token) != tokens.end();
 }
 
+ouster::util::version parse_version(const std::string& fw_rev);
 
 } // namespace impl
 
diff --git a/ouster-ros/src/laser_scan_processor.h b/ouster-ros/src/laser_scan_processor.h
index 0fe19d7a..72096517 100644
--- a/ouster-ros/src/laser_scan_processor.h
+++ b/ouster-ros/src/laser_scan_processor.h
@@ -8,7 +8,7 @@
 
 #pragma once
 
-// prevent clang-format from altering the location of "ouster_ros/ros.h", the
+// prevent clang-format from altering the location of "ouster_ros/os_ros.h", the
 // header file needs to be the first include due to PCL_NO_PRECOMPILE flag
 // clang-format off
 #include "ouster_ros/os_ros.h"
@@ -30,17 +30,27 @@ class LaserScanProcessor {
           ld_mode(info.mode),
           ring_(ring),
           pixel_shift_by_row(info.format.pixel_shift_by_row),
-          scan_msgs(get_n_returns(info)), post_processing_fn(func) {
-            for (size_t i = 0; i < scan_msgs.size(); ++i)
-                scan_msgs[i] = std::make_shared<sensor_msgs::msg::LaserScan>();
-          }
+          scan_msgs(get_n_returns(info)),
+          post_processing_fn(func) {
+        for (size_t i = 0; i < scan_msgs.size(); ++i)
+            scan_msgs[i] = std::make_shared<sensor_msgs::msg::LaserScan>();
+
+        const auto fw = impl::parse_version(info.fw_rev);
+        if (fw.major == 2 && fw.minor < 4) {
+            std::transform(pixel_shift_by_row.begin(),
+                           pixel_shift_by_row.end(),
+                           pixel_shift_by_row.begin(),
+                           [](auto c) { return c - 31; });
+        }
+    }
 
    private:
     void process(const ouster::LidarScan& lidar_scan, uint64_t,
                  const rclcpp::Time& msg_ts) {
         for (size_t i = 0; i < scan_msgs.size(); ++i) {
-            *scan_msgs[i] = lidar_scan_to_laser_scan_msg(
-                lidar_scan, msg_ts, frame, ld_mode, ring_, pixel_shift_by_row, i);
+            *scan_msgs[i] =
+                lidar_scan_to_laser_scan_msg(lidar_scan, msg_ts, frame, ld_mode,
+                                             ring_, pixel_shift_by_row, i);
         }
 
         if (post_processing_fn) post_processing_fn(scan_msgs);
diff --git a/ouster-ros/src/os_ros.cpp b/ouster-ros/src/os_ros.cpp
index 188a9acb..1d77e8e2 100644
--- a/ouster-ros/src/os_ros.cpp
+++ b/ouster-ros/src/os_ros.cpp
@@ -19,11 +19,15 @@
 #include <chrono>
 #include <string>
 #include <vector>
+#include <regex>
+
+
+namespace ouster_ros {
 
 namespace sensor = ouster::sensor;
+using namespace ouster::util;
 using ouster_sensor_msgs::msg::PacketMsg;
 
-namespace ouster_ros {
 
 bool is_legacy_lidar_profile(const sensor::sensor_info& info) {
     using sensor::UDPProfileLidar;
@@ -130,6 +134,22 @@ std::set<std::string> parse_tokens(const std::string& input, char delim) {
     return tokens;
 }
 
+version parse_version(const std::string& fw_rev) {
+    auto rgx = std::regex(R"(v(\d+).(\d+)\.(\d+))");
+    std::smatch matches;
+    std::regex_search(fw_rev, matches, rgx);
+
+    if (matches.size() < 4) return invalid_version;
+
+    try {
+        return version{static_cast<uint16_t>(stoul(matches[1])),
+                    static_cast<uint16_t>(stoul(matches[2])),
+                    static_cast<uint16_t>(stoul(matches[3]))};
+    } catch (const std::exception&) {
+        return invalid_version;
+    }
+}
+
 }  // namespace impl
 
 geometry_msgs::msg::TransformStamped transform_to_tf_msg(