Apply destagger to laser scan +

Add laser to rviz

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

@@ -179,14 +179,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 {

ouster-ros/src/laser_scan_processor.h

@@ -29,6 +29,7 @@ class LaserScanProcessor {
         : frame(frame_id),
           ld_mode(info.mode),
           ring_(ring),
+          pixel_shift_by_row(info.format.pixel_shift_by_row),
           scan_msgs(get_n_returns(info),
                     std::make_shared<sensor_msgs::msg::LaserScan>()),
           post_processing_fn(func) {}
@@ -38,7 +39,7 @@ class LaserScanProcessor {
                  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);
+                lidar_scan, msg_ts, frame, ld_mode, ring_, pixel_shift_by_row, i);
         }
 
         if (post_processing_fn) post_processing_fn(scan_msgs);
@@ -61,6 +62,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

@@ -354,7 +354,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;
@@ -380,10 +381,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]);
+
+    const auto u = ring;
+    for (auto v = 0; v < ls.w; ++v) {
+        const auto v_shift = (ls.w - 1 - (v + ls.w / 2 + pixel_shift_by_row[u])) % ls.w;
+        const auto src_idx = u * 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;