From 15bd85dd6584160dd94c4a1dc4adcec7a537a323 Mon Sep 17 00:00:00 2001 From: Imaniac230 <44968160+Imaniac230@users.noreply.github.com> Date: Thu, 4 Apr 2024 21:52:50 +0200 Subject: [PATCH 1/2] feat: Implement an automatic zero altitude angle selection for scan_ring if set to a default value of -1. * When specifying value -1 for the scan_ring parameter, the node will automatically choose the ring that is closest to a zero altitude angle. * Updated launch options to use the automatic selection with value -1 as default. * Fixed some typos in parameters descriptions. --- ouster-ros/config/driver_params.yaml | 14 ++++---- .../config/os_sensor_cloud_image_params.yaml | 5 +-- ouster-ros/launch/record.composite.launch.xml | 8 +++-- ouster-ros/launch/replay.composite.launch.xml | 9 ++--- ouster-ros/launch/sensor.composite.launch.xml | 8 +++-- .../launch/sensor.independent.launch.xml | 8 +++-- ouster-ros/launch/sensor_mtp.launch.xml | 8 +++-- ouster-ros/src/os_cloud_node.cpp | 35 +++++++++++++----- ouster-ros/src/os_driver_node.cpp | 36 +++++++++++++------ 9 files changed, 88 insertions(+), 43 deletions(-) diff --git a/ouster-ros/config/driver_params.yaml b/ouster-ros/config/driver_params.yaml index c3882a97..358f508b 100644 --- a/ouster-ros/config/driver_params.yaml +++ b/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 diff --git a/ouster-ros/config/os_sensor_cloud_image_params.yaml b/ouster-ros/config/os_sensor_cloud_image_params.yaml index 063774f4..33c3da3f 100644 --- a/ouster-ros/config/os_sensor_cloud_image_params.yaml +++ b/ouster-ros/config/os_sensor_cloud_image_params.yaml @@ -25,8 +25,9 @@ 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: use_system_default_qos: False # needs to match the value defined for os_sensor node diff --git a/ouster-ros/launch/record.composite.launch.xml b/ouster-ros/launch/record.composite.launch.xml index 39ebac82..0523a123 100644 --- a/ouster-ros/launch/record.composite.launch.xml +++ b/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"/> - + - - + - + - + - + ( topic_for_return("scan", i), selected_qos); } + // TODO: avoid this duplication in os_cloud_node - int beams_count = static_cast(get_beams_count(info)); + const auto max_ring = static_cast(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( diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp index 6af4a235..0e60e29d 100644 --- a/ouster-ros/src/os_driver_node.cpp +++ b/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(get_beams_count(info)); + const auto max_ring = static_cast(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( From 53cdeabc0f0ff6bf8017d65932cdc8a0320df655 Mon Sep 17 00:00:00 2001 From: Imaniac230 <44968160+Imaniac230@users.noreply.github.com> Date: Wed, 8 May 2024 23:14:46 +0200 Subject: [PATCH 2/2] fix: Add missing 'ros__parameters' key in os_image params. --- ouster-ros/config/os_sensor_cloud_image_params.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/ouster-ros/config/os_sensor_cloud_image_params.yaml b/ouster-ros/config/os_sensor_cloud_image_params.yaml index 33c3da3f..0799830b 100644 --- a/ouster-ros/config/os_sensor_cloud_image_params.yaml +++ b/ouster-ros/config/os_sensor_cloud_image_params.yaml @@ -30,4 +30,5 @@ ouster/os_cloud: # 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