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

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
 ==================

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:
@@ -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: 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
+  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:

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

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>

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;
 };

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,22 @@ 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]);
+
+    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;