ROS-196: laser scan from ros driver is not properly aligned with point cloud [foxy]

CHANGELOG.rst

@@ -2,6 +2,12 @@
 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
+
 ouster_ros v0.12.0
 ==================
 

ouster-ros/config/viz.rviz

@@ -13,7 +13,9 @@ Panels:
         - /nearir1/Topic1
         - /reflec1
         - /signal1
-      Splitter Ratio: 0.5
+        - /LaserScan1
+        - /LaserScan1/Topic1
+      Splitter Ratio: 0.626074492931366
     Tree Height: 1185
   - Class: rviz_common/Selection
     Name: Selection
@@ -87,23 +89,23 @@ Visualization Manager:
       Enabled: true
       Frame Timeout: 15
       Frames:
-        " os_imu":
+        All Enabled: true
+        os_imu:
           Value: true
-        " os_lidar":
+        os_lidar:
           Value: true
-        " os_sensor":
+        os_sensor:
           Value: true
-        All Enabled: true
       Marker Scale: 1
       Name: TF
       Show Arrows: true
       Show Axes: true
       Show Names: false
       Tree:
-        " os_sensor":
-          " os_imu":
+        os_sensor:
+          os_imu:
             {}
-          " os_lidar":
+          os_lidar:
             {}
       Update Interval: 0
       Value: true
@@ -163,10 +165,44 @@ Visualization Manager:
         Reliability Policy: Best Effort
         Value: /ouster/signal_image
       Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: false
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 1000
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Best Effort
+        Value: /ouster/scan
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
-    Fixed Frame: "os_sensor"
+    Fixed Frame: os_sensor
     Frame Rate: 30
   Name: root
   Tools:

ouster-ros/include/ouster_ros/os_ros.h

@@ -108,14 +108,16 @@ geometry_msgs::msg::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::msg::LaserScan lidar_scan_to_laser_scan_msg(
     const ouster::LidarScan& ls,
     const rclcpp::Time& timestamp,
     const std::string &frame,
     const ouster::sensor::lidar_mode lidar_mode,
-    const uint16_t ring,
+    const uint16_t ring, const std::vector<int>& pixel_shift_by_row,
     const int return_index);
 
 namespace impl {
@@ -154,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
 

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.12.0</version>
+  <version>0.12.1</version>
   <description>Ouster ROS2 driver</description>
   <maintainer email="[email protected]">ouster developers</maintainer>
   <license file="LICENSE">BSD</license>

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"
@@ -29,16 +29,28 @@ class LaserScanProcessor {
         : frame(frame_id),
           ld_mode(info.mode),
           ring_(ring),
-          scan_msgs(get_n_returns(info),
-                    std::make_shared<sensor_msgs::msg::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::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 (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);
@@ -61,6 +73,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;
 };

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(
@@ -151,7 +171,8 @@ geometry_msgs::msg::TransformStamped transform_to_tf_msg(
 sensor_msgs::msg::LaserScan lidar_scan_to_laser_scan_msg(
     const ouster::LidarScan& ls, const rclcpp::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::msg::LaserScan msg;
     msg.header.stamp = timestamp;
     msg.header.frame_id = frame;
@@ -177,10 +198,13 @@ sensor_msgs::msg::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]);
+
+    for (auto v = 0; v < ls.w; ++v) {
+        auto sign = v < ls.w / 2 ? -1 : +1;
+        auto v_shift = (ls.w / 2 - 1 - v + sign * pixel_shift_by_row[ring]) % ls.w;
+        auto src_idx = ring * 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;