Add support for automatic scan ring selection [Humble]

ouster-ros/config/driver_params.yaml

@@ -36,14 +36,14 @@ ouster/os_driver:
     # - RNG15_RFL8_NIR8
     udp_profile_lidar: ''
     # metadata[optional]: path to save metadata file to, if left empty a file
-    # with the sensor hostname or ip will be crearted in ~/.ros folder.
+    # with the sensor hostname or ip will be created in ~/.ros folder.
     metadata: ''
     # lidar_port[optional]: port value should be in the range [0, 65535]. If you
-    # use 0 as the port value then the first avaliable port number will be
+    # use 0 as the port value then the first available port number will be
     # assigned.
     lidar_port: 0
     # imu_port[optional]: port value should be in the range [0, 65535]. If you
-    # use 0 as the port value then the first avaliable port number will be
+    # use 0 as the port value then the first available port number will be
     # assigned.
     imu_port: 0
     # sensor_frame[optional]: name to use when referring to the sensor frame.
@@ -61,8 +61,10 @@ ouster/os_driver:
     proc_mask: IMG|PCL|IMU|SCAN
     # scan_ring[optional]: use this parameter in conjunction with the SCAN flag
     # to select which beam of the LidarScan to use when producing the LaserScan
-    # message. Choose a value the range [0, sensor_beams_count).
-    scan_ring: 0
+    # message. Choose a value within the range [0, sensor_beams_count). If you
+    # use -1 as the ring value, then the ring that is closest to a zero altitude
+    # angle (middle ring) will be automatically chosen.
+    scan_ring: -1
     # use_system_default_qos[optional]: if false, data are published with sensor
     # data QoS. This is preferrable for production but default QoS is needed for
     # rosbag. See: https://github.com/ros2/rosbag2/issues/125
@@ -72,7 +74,7 @@ ouster/os_driver:
     #  - original: This uses the original point representation ouster_ros::Point
     #          of the ouster-ros driver.
     #  - native: directly maps all fields as published by the sensor to an
-    #          equivalent point cloud representation with the additon of ring
+    #          equivalent point cloud representation with the addition of ring
     #          and timestamp fields.
     #  - xyz: the simplest point type, only has {x, y, z}
     #  - xyzi: same as xyz point type but adds intensity (signal) field. this

ouster-ros/config/os_sensor_cloud_image_params.yaml

@@ -25,8 +25,10 @@ ouster/os_cloud:
     ptp_utc_tai_offset: -37.0 # UTC/TAI offset in seconds to apply when using TIME_FROM_PTP_1588
     proc_mask: IMU|PCL|SCAN # pick IMU, PCL, SCAN or any combination of the three options
     use_system_default_qos: False # needs to match the value defined for os_sensor node
-    scan_ring: 0  # Use this parameter in conjunction with the SCAN flag and choose a
-                  # value the range [0, sensor_beams_count)
+    scan_ring: -1  # Use this parameter in conjunction with the SCAN flag and choose a
+                  # value within the range [0, sensor_beams_count), use value -1 to
+                  # automatically select the ring that is closest to a zero altitude angle (middle ring).
     point_type: original # choose from: {original, native, xyz, xyzi, xyzir}
 ouster/os_image:
+  ros__parameters:
     use_system_default_qos: False # needs to match the value defined for os_sensor node

ouster-ros/launch/record.composite.launch.xml

@@ -63,9 +63,11 @@
     to disable image topics you would need to omit the os_image node
     from the launch file"/>
 
-  <arg name="scan_ring" default="0" description="
-    use this parameter in conjunction with the SCAN flag
-    and choose a value the range [0, sensor_beams_count)"/>
+  <arg name="scan_ring" default="-1" description="
+    Use this parameter in conjunction with the SCAN flag
+    and choose a value within the range [0, sensor_beams_count),
+    use value -1 to automatically select the ring that is closest
+    to a zero altitude angle (middle ring)."/>
 
   <arg name="point_type" default="original" description="point type for the generated point cloud;
    available options: {

ouster-ros/launch/replay.composite.launch.xml

@@ -42,10 +42,11 @@
     to disable image topics you would need to omit the os_image node
     from the launch file"/>
 
-
-  <arg name="scan_ring" default="0" description="
-    use this parameter in conjunction with the SCAN flag
-    and choose a value the range [0, sensor_beams_count)"/>
+  <arg name="scan_ring" default="-1" description="
+    Use this parameter in conjunction with the SCAN flag
+    and choose a value within the range [0, sensor_beams_count),
+    use value -1 to automatically select the ring that is closest
+    to a zero altitude angle (middle ring)."/>
 
   <arg name="point_type" default="original" description="point type for the generated point cloud;
    available options: {

ouster-ros/launch/sensor.composite.launch.xml

@@ -59,9 +59,11 @@
   <arg name="proc_mask" default="IMG|PCL|IMU|SCAN" description="
     use any combination of the 4 flags to enable or disable specific processors"/>
 
-  <arg name="scan_ring" default="0" description="
-    use this parameter in conjunction with the SCAN flag
-    and choose a value the range [0, sensor_beams_count)"/>
+  <arg name="scan_ring" default="-1" description="
+    Use this parameter in conjunction with the SCAN flag
+    and choose a value within the range [0, sensor_beams_count),
+    use value -1 to automatically select the ring that is closest
+    to a zero altitude angle (middle ring)."/>
 
   <arg name="point_type" default="original" description="point type for the generated point cloud;
    available options: {

ouster-ros/launch/sensor.independent.launch.xml

@@ -61,9 +61,11 @@
     to disable image topics you would need to omit the os_image node
     from the launch file"/>
 
-  <arg name="scan_ring" default="0" description="
-    use this parameter in conjunction with the SCAN flag
-    and choose a value the range [0, sensor_beams_count)"/>
+  <arg name="scan_ring" default="-1" description="
+    Use this parameter in conjunction with the SCAN flag
+    and choose a value within the range [0, sensor_beams_count),
+    use value -1 to automatically select the ring that is closest
+    to a zero altitude angle (middle ring)."/>
 
   <arg name="point_type" default="original" description="point type for the generated point cloud;
    available options: {

ouster-ros/launch/sensor_mtp.launch.xml

@@ -67,9 +67,11 @@
   <arg name="proc_mask" default="IMG|PCL|IMU|SCAN"
     description="Use any combination of the 4 flags to enable or disable specific processors"/>
 
-  <arg name="scan_ring" default="0" description="
-    use this parameter in conjunction with the SCAN flag
-    and choose a value the range [0, sensor_beams_count)"/>
+  <arg name="scan_ring" default="-1" description="
+    Use this parameter in conjunction with the SCAN flag
+    and choose a value within the range [0, sensor_beams_count),
+    use value -1 to automatically select the ring that is closest
+    to a zero altitude angle (middle ring)."/>
 
   <arg name="point_type" default="original" description="point type for the generated point cloud;
    available options: {

ouster-ros/src/os_cloud_node.cpp

@@ -58,7 +58,7 @@ class OusterCloud : public OusterProcessingNodeBase {
         declare_parameter("ptp_utc_tai_offset", -37.0);
         declare_parameter("proc_mask", "IMU|PCL|SCAN");
         declare_parameter("use_system_default_qos", false);
-        declare_parameter("scan_ring", 0);
+        declare_parameter("scan_ring", -1);
         declare_parameter("point_type", "original");
     }
 
@@ -134,15 +134,34 @@ class OusterCloud : public OusterProcessingNodeBase {
                 scan_pubs[i] = create_publisher<sensor_msgs::msg::LaserScan>(
                     topic_for_return("scan", i), selected_qos);
             }
+
             // TODO: avoid this duplication in os_cloud_node
-            int beams_count = static_cast<int>(get_beams_count(info));
+            const auto max_ring = static_cast<int>(get_beams_count(info) - 1);
             int scan_ring = get_parameter("scan_ring").as_int();
-            scan_ring = std::min(std::max(scan_ring, 0), beams_count - 1);
-            if (scan_ring != get_parameter("scan_ring").as_int()) {
-                RCLCPP_WARN_STREAM(get_logger(),
-                    "scan ring is set to a value that exceeds available range"
-                    "please choose a value between [0, " << beams_count << "], "
-                    "ring value clamped to: " << scan_ring);
+            if (scan_ring == -1) {
+                double zero_angle = 9999.0;
+                scan_ring = 0;
+                for (int i = 0; i <= max_ring; ++i) {
+                    const double beam_angle = fabs(info.beam_altitude_angles[i]);
+                    if (beam_angle < zero_angle) {
+                        scan_ring = i;
+                        zero_angle = beam_angle;
+                    }
+                }
+                RCLCPP_INFO_STREAM(
+                    get_logger(),
+                    "Scan ring was not specified. Automatically selected the ring"
+                    " that is closest to a zero altitude angle: "
+                        << scan_ring << ".");
+            } else {
+                scan_ring = std::min(std::max(scan_ring, 0), max_ring);
+                if (scan_ring != get_parameter("scan_ring").as_int()) {
+                    RCLCPP_WARN_STREAM(
+                        get_logger(),
+                        "Scan ring is set to a value that exceeds available range,"
+                        " please choose a value between 0 and " << max_ring
+                            << ". Ring value clamped to: " << scan_ring << ".");
+                }
             }
 
             processors.push_back(LaserScanProcessor::create(

ouster-ros/src/os_driver_node.cpp

@@ -35,7 +35,7 @@ class OusterDriver : public OusterSensor {
         tf_bcast.declare_parameters();
         tf_bcast.parse_parameters();
         declare_parameter("proc_mask", "IMU|IMG|PCL|SCAN");
-        declare_parameter("scan_ring", 0);
+        declare_parameter("scan_ring", -1);
         declare_parameter("ptp_utc_tai_offset", -37.0);
         declare_parameter("point_type", "original");
     }
@@ -109,18 +109,32 @@ class OusterDriver : public OusterSensor {
             }
 
             // TODO: avoid duplication in os_cloud_node
-            int beams_count = static_cast<int>(get_beams_count(info));
+            const auto max_ring = static_cast<int>(get_beams_count(info) - 1);
             int scan_ring = get_parameter("scan_ring").as_int();
-            scan_ring = std::min(std::max(scan_ring, 0), beams_count - 1);
-            if (scan_ring != get_parameter("scan_ring").as_int()) {
-                RCLCPP_WARN_STREAM(
+            if (scan_ring == -1) {
+                double zero_angle = 9999.0;
+                scan_ring = 0;
+                for (int i = 0; i <= max_ring; ++i) {
+                    const double beam_angle = fabs(info.beam_altitude_angles[i]);
+                    if (beam_angle < zero_angle) {
+                        scan_ring = i;
+                        zero_angle = beam_angle;
+                    }
+                }
+                RCLCPP_INFO_STREAM(
                     get_logger(),
-                    "scan ring is set to a value that exceeds available range"
-                    "please choose a value between [0, "
-                        << beams_count
-                        << "], "
-                           "ring value clamped to: "
-                        << scan_ring);
+                    "Scan ring was not specified. Automatically selected the ring"
+                    " that is closest to a zero altitude angle: "
+                        << scan_ring << ".");
+            } else {
+                scan_ring = std::min(std::max(scan_ring, 0), max_ring);
+                if (scan_ring != get_parameter("scan_ring").as_int()) {
+                    RCLCPP_WARN_STREAM(
+                        get_logger(),
+                        "Scan ring is set to a value that exceeds available range,"
+                        " please choose a value between 0 and " << max_ring
+                            << ". Ring value clamped to: " << scan_ring << ".");
+                  }
             }
 
             processors.push_back(LaserScanProcessor::create(