ROS-196: laser scan from ros driver is not properly aligned with poin…

…t cloud [humble] (ouster-lidar#203) * Apply destagger to laser scan + Add laser to RVIZ * Align LaserScan with the PointCloud * Apply proper pixel shift * Resolve the issue of zeroed laserscan on dual mode * Address an issue where LaserScan appeared different on FW prior to 2.4 * Fix the issue for odd numbers * List selected sensors on the main page + Update RVIZ config to highlight the 2D LaserScan.
AchmadFathoni · Feb 16, 2024 · ce6ed0e · ce6ed0e
1 parent 92bd054
commit ce6ed0e
Show file tree

Hide file tree

Showing 10 changed files with 165 additions and 84 deletions.
diff --git a/CHANGELOG.rst b/CHANGELOG.rst
@@ -2,6 +2,13 @@
 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
 ==================
 * [BREAKING]: updated ouster_client to the release of ``20231031`` [v0.10.0]; changes listed below.

diff --git a/README.md b/README.md
@@ -14,6 +14,7 @@
 
 - [Official ROS driver for Ouster sensors](#official-ros-driver-for-ouster-sensors)
   - [Overview](#overview)
+  - [Supported Devices](#supported-devices)
   - [Requirements](#requirements)
     - [Linux](#linux)
     - [Windows](#windows)
@@ -43,6 +44,17 @@ messages on the topics of `/ouster/imu` and `/ouster/points`. In the case the us
 dual return and it was configured to use this capability, then another topic will published under the
 name `/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 branch is only intended for use with **Rolling**, **Humble** and **Iron** ROS 2 distros. Please
 refer to ROS 2 online documentation on how to setup ROS on your machine before proceeding with the

diff --git a/ouster-ros/config/viz.rviz b/ouster-ros/config/viz.rviz
@@ -8,12 +8,15 @@ Panels:
         - /Status1
         - /PointCloud21
         - /TF1/Tree1
-      Splitter Ratio: 0.5
-<<<<<<< Updated upstream
-    Tree Height: 624
-=======
-    Tree Height: 574
->>>>>>> Stashed changes
+        - /range1
+        - /range1/Topic1
+        - /nearir1/Topic1
+        - /reflec1
+        - /signal1
+        - /LaserScan1
+        - /LaserScan1/Topic1
+      Splitter Ratio: 0.626074492931366
+    Tree Height: 1185
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -92,13 +95,6 @@ Visualization Manager:
       Frame Timeout: 15
       Frames:
         All Enabled: true
-<<<<<<< Updated upstream
-=======
-        base_link:
-          Value: true
-        odom_enu:
-          Value: true
->>>>>>> Stashed changes
         os_imu:
           Value: true
         os_lidar:
@@ -111,7 +107,6 @@ Visualization Manager:
       Show Axes: true
       Show Names: false
       Tree:
-<<<<<<< Updated upstream
         os_sensor:
           os_imu:
             {}
@@ -174,27 +169,79 @@ Visualization Manager:
         History Policy: Keep Last
         Reliability Policy: Best Effort
         Value: /ouster/signal_image
-      Value: false
-  Enabled: true
-  Global Options:
-    Background Color: 48; 48; 48
-    Fixed Frame: os_sensor
-=======
-        base_link:
-          os_sensor:
-            os_imu:
-              {}
-            os_lidar:
-              {}
-        odom_enu:
-          {}
-      Update Interval: 0
+      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.02
+      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
+    - 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.02
+      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: odom_enu
->>>>>>> Stashed changes
+    Fixed Frame: os_sensor
     Frame Rate: 30
   Name: root
   Tools:
@@ -237,11 +284,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-<<<<<<< Updated upstream
-      Distance: 0.6095240116119385
-=======
-      Distance: 42.680667877197266
->>>>>>> Stashed changes
+      Distance: 4.583338260650635
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -256,32 +299,18 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-<<<<<<< Updated upstream
-      Pitch: 0.8797973990440369
+      Pitch: 0.9003978371620178
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 2.4603934288024902
-=======
-      Pitch: 0.624796986579895
-      Target Frame: <Fixed Frame>
-      Value: Orbit (rviz)
-      Yaw: 5.1504034996032715
->>>>>>> Stashed changes
+      Yaw: 4.277213096618652
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-<<<<<<< Updated upstream
-  Height: 1051
-  Hide Left Dock: false
-  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001f700000348fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000050000003480000010e00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f00000348fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000006e000007de000000d800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000075800000236fc0100000003fb0000000c007300690067006e0061006c0000000000000007580000006100fffffffc00000000000007580000000000fffffffaffffffff0200000003fb0000000c007200650066006c006500630000000000ffffffff0000003000fffffffb0000000c006e006500610072006900720000000000ffffffff0000003000fffffffb0000000a00720061006e00670065000000006e000002390000003000fffffffb0000000a00560069006500770073030000004e00000080000002e100000197000000030000075800000058fc0100000002fb0000000800540069006d0065010000000000000758000002c500fffffffb0000000800540069006d006501000000000000045000000000000000000000055d0000034800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-=======
-  Height: 995
+  Height: 2272
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 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
->>>>>>> Stashed changes
+  QMainWindow State: 000000ff00000000fd0000000400000000000001f700000599fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000002b3000005990000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f000007defc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000006e000007de0000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000e7000000239fc0100000002fc0000000000000e700000008d00fffffffa000000010200000004fb0000000c007200650066006c006500630000000000ffffffff0000004a00fffffffb0000000c007300690067006e0061006c0100000000ffffffff0000004a00fffffffb0000000c006e006500610072006900720000000000ffffffff0000004a00fffffffb0000000a00720061006e00670065000000006e000002390000004a00fffffffb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e700000005afc0100000002fb0000000800540069006d0065010000000000000e700000057100fffffffb0000000800540069006d0065010000000000000450000000000000000000000c6d0000059900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -290,10 +319,9 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-<<<<<<< Updated upstream
-  Width: 1880
-  X: 1960
-  Y: 0
+  Width: 3696
+  X: 144
+  Y: 54
   nearir:
     collapsed: false
   range:
@@ -302,8 +330,3 @@ Window Geometry:
     collapsed: false
   signal:
     collapsed: false
-=======
-  Width: 1755
-  X: 66
-  Y: 0
->>>>>>> Stashed changes
diff --git a/ouster-ros/include/ouster_ros/os_ros.h b/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
 

diff --git a/ouster-ros/package.xml b/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>

diff --git 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"
@@ -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;
 };

diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp
@@ -148,8 +148,7 @@ class OusterCloud : public OusterProcessingNodeBase {
             processors.push_back(LaserScanProcessor::create(
                 info, tf_bcast.lidar_frame_id(), scan_ring,
                 [this](LaserScanProcessor::OutputType msgs) {
-                    for (size_t i = 0; i < msgs.size(); ++i)
-                        scan_pubs[i]->publish(*msgs[i]);
+                    for (size_t i = 0; i < msgs.size(); ++i) scan_pubs[i]->publish(*msgs[i]);
                 }));
         }
 

diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp
@@ -41,7 +41,7 @@ class OusterDriver : public OusterSensor {
     }
 
     ~OusterDriver() override {
-        RCLCPP_INFO(get_logger(), "OusterDriver::~OusterDriver() called");
+        RCLCPP_DEBUG(get_logger(), "OusterDriver::~OusterDriver() called");
         halt();
     }
 
@@ -126,8 +126,7 @@ class OusterDriver : public OusterSensor {
             processors.push_back(LaserScanProcessor::create(
                 info, tf_bcast.lidar_frame_id(), scan_ring,
                 [this](LaserScanProcessor::OutputType msgs) {
-                    for (size_t i = 0; i < msgs.size(); ++i)
-                        scan_pubs[i]->publish(*msgs[i]);
+                    for (size_t i = 0; i < msgs.size(); ++i) scan_pubs[i]->publish(*msgs[i]);
                 }));
         }