Repository: ouster-lidar/ouster-ros (language: cpp)

Issue:
Issues with ouster/scan topic
I have some issues with ouster scan topic publishing misaligned and poorly converted 3D to 2D laser scans.  
As it can be observed in the image below 3D point clouds and the converted 2D laser scan are not properly aligned.  

Second, it will be helpful if there are some parameters available for configuring how 3D point clouds are converted into 2D laser scans through limiting the channels/beams, or options for using only selected beams for laser scans, as with the default setting, as can be seen in the screenshot below, instead of mapping the walls and shorter obstacles into 2D plane, it captures the closer ceiling furniture (like vents) (indoor), and tree crowns (outdoor) and maps those onto the ground which will make it difficult for ROS SLAM and navigation packages to properly map and generate feasible path for the robot.  

![Screenshot from 2023-10-25 15-12-59](https://github.com/ouster-lidar/ouster-ros/assets/49401497/ba5d5f5c-3775-489c-9dd2-449cddab778e)


- Ouster Sensor: OS-1-64-U13
- Ouster Firmware Version: v2.5.1
- ROS version: Noetic
- Operating System: Ubuntu 20.04.6 LST
- Machine Architecture? ARM (Jetson Orin)
- git commit hash (if not the latest): I believe I have the latest

I appreciate any help.



Discussion / hints:

Fix patch:
diff --git a/CHANGELOG.rst b/CHANGELOG.rst
index b22bb927..238038de 100644
--- a/CHANGELOG.rst
+++ b/CHANGELOG.rst
@@ -9,6 +9,10 @@ Changelog
   other common pcl point types.
 * ouster_image_nodelet can operate independently from ouster_cloud_nodelet.
 * install ouster-ros and ouster_client include directories in separate folders.
+* [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
+
 
 ouster_ros v0.10.0
 ==================
diff --git a/README.md b/README.md
index a14f1e0d..3a4de566 100644
--- a/README.md
+++ b/README.md
@@ -14,6 +14,7 @@
 
 - [Official ROS1/ROS2 drivers for Ouster sensors](#official-ros1ros2-drivers-for-ouster-sensors)
   - [Overview](#overview)
+  - [Supported Devices](#supported-devices)
   - [Requirements](#requirements)
   - [Getting Started](#getting-started)
   - [Usage](#usage)
@@ -39,6 +40,16 @@ on the topics of `/ouster/imu` and `/ouster/points`. In the case the used sensor
 return and it was configured to use this capability, then another topic will published named
 `/ouster/points2` which corresponds to the second point cloud.
 
+## Supported Devices
+The driver supports the following list of Ouster sensors:
+- [OS0](https://ouster.com/products/hardware/os0-lidar-sensor)
+- [OS1](https://ouster.com/products/hardware/os1-lidar-sensor)
+- [OS2](https://ouster.com/products/hardware/os2-lidar-sensor)
+- [OSDome](https://ouster.com/products/hardware/osdome-lidar-sensor)
+
+You can obtain detailed specs sheet about the sensors and obtain updated FW through the website
+[downloads](https://ouster.com/downloads) section.
+
 ## Requirements
 This package only supports **Melodic** and **Noetic** ROS distros. Please refer to ROS online
 documentation on how to setup ros on your machine before proceeding with the remainder of this guide.
diff --git a/config/viz.rviz b/config/viz.rviz
index 570ec59a..521506dc 100644
--- a/config/viz.rviz
+++ b/config/viz.rviz
@@ -6,8 +6,9 @@ Panels:
       Expanded:
         - /Global Options1
         - /Status1
+        - /LaserScan1
       Splitter Ratio: 0.5654885768890381
-    Tree Height: 756
+    Tree Height: 629
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -23,10 +24,9 @@ Panels:
     Name: Views
     Splitter Ratio: 0.5
   - Class: rviz/Time
-    Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: ""
+    SyncSource: PointCloud2
 Preferences:
   PromptSaveOnExit: false
 Toolbars:
@@ -40,7 +40,7 @@ Visualization Manager:
       Color: 160; 160; 164
       Enabled: false
       Line Style:
-        Line Width: 0.029999999329447746
+        Line Width: 0.01
         Value: Lines
       Name: Grid
       Normal Cell Count: 0
@@ -75,7 +75,7 @@ Visualization Manager:
       Queue Size: 10
       Selectable: true
       Size (Pixels): 2
-      Size (m): 0.029999999329447746
+      Size (m): 0.01
       Style: Flat Squares
       Topic: /ouster/points
       Unreliable: false
@@ -105,7 +105,7 @@ Visualization Manager:
       Queue Size: 10
       Selectable: true
       Size (Pixels): 3
-      Size (m): 0.029999999329447746
+      Size (m): 0.01
       Style: Flat Squares
       Topic: /ouster/points2
       Unreliable: false
@@ -198,6 +198,8 @@ Visualization Manager:
       Value: false
     - Class: rviz/TF
       Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
       Frame Timeout: 15
       Frames:
         All Enabled: false
@@ -207,6 +209,7 @@ Visualization Manager:
           Value: false
         os_sensor:
           Value: true
+      Marker Alpha: 1
       Marker Scale: 1
       Name: TF
       Show Arrows: true
@@ -220,6 +223,34 @@ Visualization Manager:
             {}
       Update Interval: 0
       Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /ouster/scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -248,7 +279,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 45.77256774902344
+      Distance: 8.687203407287598
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -264,18 +295,17 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 1.2703977823257446
+      Pitch: 1.5697963237762451
       Target Frame: <Fixed Frame>
-      Value: Orbit (rviz)
-      Yaw: 4.513584136962891
+      Yaw: 4.6935882568359375
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1080
+  Height: 2272
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000003170000032ffc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000005fb0000046000000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000005f0000000180000000000000000fb0000000800480065006c007000000002540000046e000000000000000000000001000001e30000032ffc0200000005fb000000100044006900730070006c00610079007301000000ee0000032f000000c700fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb000000100044006900730070006c0061007900730100000189000002d10000000000000000fb0000000a00560069006500770073000000036d00000206000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000780000000adfc0100000003fc00000000000007800000008000fffffffa000000000200000009fb00000018007200650066006c006500630074006900760069007400790100000000ffffffff0000001600fffffffb0000000a00720061006e006700650100000000ffffffff0000001600fffffffb0000000c007300690067006e0061006c0100000000ffffffff0000001600fffffffb0000000e006e0065006100720020006900720100000000ffffffff0000001600fffffffb0000001c007200650066006c00650063007400690076006900740079002000320100000000ffffffff0000001600fffffffb0000000e00720061006e00670065002000320100000000ffffffff0000001600fffffffb00000010007300690067006e0061006c002000320100000000ffffffff0000001600fffffffb000000120069006e00740065006e00730069007400790100000000ffffffff0000000000000000fb0000000e0061006d006200690065006e0074010000003a000000c60000000000000000fc0000000000000a000000000000fffffffa000000000200000003fb000000120069006e00740065006e00730069007400790100000000ffffffff0000000000000000fb0000000a00720061006e006700650100000000ffffffff0000000000000000fb0000000e0061006d006200690065006e0074010000003a000000a70000000000000000fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000082b00000043fc0100000001fc000000000000082b0000000000fffffffa000000010200000002fb0000000800540069006d00650000000000ffffffff0000003700fffffffb0000000800540069006d006501000006c90000003a0000000000000000000005970000032f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
@@ -284,9 +314,9 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1920
+  Width: 3840
   X: 0
-  Y: 1151
+  Y: 54
   near ir:
     collapsed: false
   range:
diff --git a/include/ouster_ros/os_ros.h b/include/ouster_ros/os_ros.h
index d053732b..631aa6ad 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 {
@@ -158,6 +158,8 @@ 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
 
 }  // namespace ouster_ros
diff --git a/package.xml b/package.xml
index ee3f8425..e0bc6977 100644
--- a/package.xml
+++ b/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>ouster_ros</name>
-  <version>0.12.0</version>
+  <version>0.12.1</version>
   <description>Ouster ROS driver</description>
   <maintainer email="oss@ouster.io">ouster developers</maintainer>
   <license file="LICENSE">BSD</license>
diff --git a/src/laser_scan_processor.h b/src/laser_scan_processor.h
index ccd184e7..e9222ae8 100644
--- a/src/laser_scan_processor.h
+++ b/src/laser_scan_processor.h
@@ -28,16 +28,28 @@ class LaserScanProcessor {
         : frame(frame_id),
           ld_mode(info.mode),
           ring_(ring),
-          scan_msgs(get_n_returns(info),
-                    std::make_shared<sensor_msgs::LaserScan>()),
-          post_processing_fn(func) {}
+          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::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 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);
+        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);
         }
 
         if (post_processing_fn) post_processing_fn(scan_msgs);
@@ -60,6 +72,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..171171b9 100644
--- a/src/os_ros.cpp
+++ b/src/os_ros.cpp
@@ -19,11 +19,14 @@
 #include <chrono>
 #include <string>
 #include <vector>
+#include <regex>
 
-namespace sensor = ouster::sensor;
 
 namespace ouster_ros {
 
+namespace sensor = ouster::sensor;
+using namespace ouster::util;
+
 bool is_legacy_lidar_profile(const sensor::sensor_info& info) {
     using sensor::UDPProfileLidar;
     return info.format.udp_profile_lidar ==
@@ -129,6 +132,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::TransformStamped transform_to_tf_msg(
@@ -150,7 +169,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;
@@ -165,21 +185,23 @@ sensor_msgs::LaserScan lidar_scan_to_laser_scan_msg(
     msg.time_increment = 1.0f / (scan_width * scan_frequency);
     msg.angle_increment = 2 * M_PI / scan_width;
 
-    auto which_range = return_index == 0 ? sensor::ChanField::RANGE
-                                         : sensor::ChanField::RANGE2;
-    ouster::img_t<uint32_t> range = ls.field<uint32_t>(which_range);
-    auto which_signal = return_index == 0 ? sensor::ChanField::SIGNAL
-                                          : sensor::ChanField::SIGNAL2;
-    ouster::img_t<uint32_t> signal =
-        impl::get_or_fill_zero<uint32_t>(which_signal, ls);
+    ouster::img_t<uint32_t> range = ls.field<uint32_t>(
+        static_cast<sensor::ChanField>(sensor::ChanField::RANGE + return_index));
+    ouster::img_t<uint32_t> signal = impl::get_or_fill_zero<uint32_t>(
+        static_cast<sensor::ChanField>(sensor::ChanField::SIGNAL + return_index),
+        ls);
     const auto rg = range.data();
     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]);
+
+    uint16_t u = ring;
+    for (auto v =  0; v < ls.w; ++v) {
+        auto v_shift = (v + ls.w - pixel_shift_by_row[u] + ls.w / 2) % ls.w;
+        auto src_idx = u * ls.w + v_shift;
+        auto tgt_idx = ls.w - 1 - v;
+        msg.ranges[tgt_idx] = rg[src_idx] * ouster::sensor::range_unit;
+        msg.intensities[tgt_idx] = static_cast<float>(sg[src_idx]);
     }
 
     return msg;


Test patch: