From 2374a8fb2655f40cbda5158e04d77756ad9f6c7e Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Wed, 29 Jan 2025 12:47:42 -0800 Subject: [PATCH 1/8] Skip the lidar_scan based on a hard-coded set number of valid cols --- src/lidar_packet_handler.h | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/lidar_packet_handler.h b/src/lidar_packet_handler.h index 3e04cc09..c3d98532 100644 --- a/src/lidar_packet_handler.h +++ b/src/lidar_packet_handler.h @@ -128,6 +128,17 @@ class LidarPacketHandler { *(mutexes[ring_buffer.write_head()])); auto& lidar_scan = *lidar_scans[ring_buffer.write_head()]; result = lidar_handler(*this, pf, lidar_packet, lidar_scan); + if (result) { + // count the number of valid columns in the scan + auto status = lidar_scan.status(); + size_t valid_cols = std::count_if(status.data(), status.data() + status.size(), + [](const uint32_t s) { return (s & 0x01); }); + if (valid_cols < static_cast(0.7 * lidar_scan.status().size())) { + NODELET_WARN("valid_cols %zu, less than 70%%, SKIPPING SCAN", + valid_cols); + result = false; + } + } } if (result) { ring_buffer.write(); From 8c60906a2f9c92ab863018e77ea98e5a5178ff60 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 30 Jan 2025 16:23:43 -0800 Subject: [PATCH 2/8] Parameterize the min cols filter --- src/lidar_packet_handler.h | 23 +++++++++++++++-------- src/os_cloud_nodelet.cpp | 11 ++++++++++- src/os_driver_nodelet.cpp | 10 +++++++++- src/os_image_nodelet.cpp | 9 ++++++++- 4 files changed, 42 insertions(+), 11 deletions(-) diff --git a/src/lidar_packet_handler.h b/src/lidar_packet_handler.h index c3d98532..c4ab216b 100644 --- a/src/lidar_packet_handler.h +++ b/src/lidar_packet_handler.h @@ -69,10 +69,12 @@ class LidarPacketHandler { LidarPacketHandler(const sensor::sensor_info& info, const std::vector& handlers, const std::string& timestamp_mode, - int64_t ptp_utc_tai_offset) + int64_t ptp_utc_tai_offset, + float min_scan_valid_columns_ratio) : ring_buffer(LIDAR_SCAN_COUNT), lidar_scan_handlers{handlers}, - ptp_utc_tai_offset_(ptp_utc_tai_offset) { + ptp_utc_tai_offset_(ptp_utc_tai_offset), + min_scan_valid_columns_ratio_(min_scan_valid_columns_ratio) { // initialize lidar_scan processor and buffer scan_batcher = std::make_unique(info); @@ -133,9 +135,10 @@ class LidarPacketHandler { auto status = lidar_scan.status(); size_t valid_cols = std::count_if(status.data(), status.data() + status.size(), [](const uint32_t s) { return (s & 0x01); }); - if (valid_cols < static_cast(0.7 * lidar_scan.status().size())) { - NODELET_WARN("valid_cols %zu, less than 70%%, SKIPPING SCAN", - valid_cols); + if (valid_cols < static_cast(min_scan_valid_columns_ratio_ * lidar_scan.status().size())) { + NODELET_WARN_STREAM("number of valid columns per scan " << valid_cols + <<" which is below the ratio " << std::setprecision(4) << (100 * min_scan_valid_columns_ratio_) + << "%% SKIPPING SCAN"); result = false; } } @@ -167,9 +170,11 @@ class LidarPacketHandler { static HandlerType create( const sensor::sensor_info& info, const std::vector& handlers, - const std::string& timestamp_mode, int64_t ptp_utc_tai_offset) { + const std::string& timestamp_mode, int64_t ptp_utc_tai_offset, + float min_scan_valid_columns_ratio) { auto handler = std::make_shared( - info, handlers, timestamp_mode, ptp_utc_tai_offset); + info, handlers, timestamp_mode, ptp_utc_tai_offset, + min_scan_valid_columns_ratio); return [handler](const sensor::LidarPacket& lidar_packet) { if (handler->lidar_packet_accumlator(lidar_packet)) { handler->ring_buffer_has_elements.notify_one(); @@ -348,7 +353,7 @@ class LidarPacketHandler { private: std::unique_ptr scan_batcher; const int LIDAR_SCAN_COUNT = 10; - const double THROTTLE_PERCENT = 0.7; + const float THROTTLE_PERCENT = 0.7f; LockFreeRingBuffer ring_buffer; std::mutex ring_buffer_mutex; std::vector> lidar_scans; @@ -377,6 +382,8 @@ class LidarPacketHandler { std::condition_variable ring_buffer_has_elements; int64_t ptp_utc_tai_offset_; + + float min_scan_valid_columns_ratio_ = 0.7; }; } // namespace ouster_ros \ No newline at end of file diff --git a/src/os_cloud_nodelet.cpp b/src/os_cloud_nodelet.cpp index 14e47eb4..7d5e803d 100644 --- a/src/os_cloud_nodelet.cpp +++ b/src/os_cloud_nodelet.cpp @@ -180,6 +180,13 @@ class OusterCloud : public nodelet::Nodelet { } std::vector processors; + + auto min_scan_valid_columns_ratio = pnh.param("min_scan_valid_columns_ratio", 0.0); + if (min_scan_valid_columns_ratio < 0.0 || min_scan_valid_columns_ratio > 1.0) { + NODELET_FATAL("min_scan_valid_columns_ratio needs to be in the range [0, 1]"); + throw std::runtime_error("min_scan_valid_columns_ratio out of bounds!"); + } + if (impl::check_token(tokens, "PCL")) { auto point_type = pnh.param("point_type", std::string{"original"}); auto organized = pnh.param("organized", true); @@ -200,6 +207,7 @@ class OusterCloud : public nodelet::Nodelet { uint32_t min_range = impl::ulround(min_range_m * 1000); uint32_t max_range = impl::ulround(max_range_m * 1000); auto rows_step = pnh.param("rows_step", 1); + processors.push_back( PointCloudProcessorFactory::create_point_cloud_processor( point_type, info, tf_bcast.point_cloud_frame_id(), @@ -254,7 +262,8 @@ class OusterCloud : public nodelet::Nodelet { impl::check_token(tokens, "SCAN")) { lidar_packet_handler = LidarPacketHandler::create( info, processors, timestamp_mode, - static_cast(ptp_utc_tai_offset * 1e+9)); + static_cast(ptp_utc_tai_offset * 1e+9), + min_scan_valid_columns_ratio); } if (impl::check_token(tokens, "TLM")) { diff --git a/src/os_driver_nodelet.cpp b/src/os_driver_nodelet.cpp index 6bfe6f2d..ee4be59d 100644 --- a/src/os_driver_nodelet.cpp +++ b/src/os_driver_nodelet.cpp @@ -125,6 +125,12 @@ class OusterDriver : public OusterSensor { static_cast(ptp_utc_tai_offset * 1e+9)); } + auto min_scan_valid_columns_ratio = pnh.param("min_scan_valid_columns_ratio", 0.0); + if (min_scan_valid_columns_ratio < 0.0 || min_scan_valid_columns_ratio > 1.0) { + NODELET_FATAL("min_scan_valid_columns_ratio needs to be in the range [0, 1]"); + throw std::runtime_error("min_scan_valid_columns_ratio out of bounds!"); + } + std::vector processors; if (impl::check_token(tokens, "PCL")) { auto point_type = pnh.param("point_type", std::string{"original"}); @@ -146,6 +152,7 @@ class OusterDriver : public OusterSensor { uint32_t min_range = impl::ulround(min_range_m * 1000); uint32_t max_range = impl::ulround(max_range_m * 1000); auto rows_step = pnh.param("rows_step", 1); + processors.push_back( PointCloudProcessorFactory::create_point_cloud_processor( point_type, info, tf_bcast.point_cloud_frame_id(), @@ -205,7 +212,8 @@ class OusterDriver : public OusterSensor { impl::check_token(tokens, "IMG")) { lidar_packet_handler = LidarPacketHandler::create( info, processors, timestamp_mode, - static_cast(ptp_utc_tai_offset * 1e+9)); + static_cast(ptp_utc_tai_offset * 1e+9), + min_scan_valid_columns_ratio); } if (impl::check_token(tokens, "TLM")) { diff --git a/src/os_image_nodelet.cpp b/src/os_image_nodelet.cpp index 17fbd410..9d26f08b 100644 --- a/src/os_image_nodelet.cpp +++ b/src/os_image_nodelet.cpp @@ -99,6 +99,12 @@ class OusterImage : public nodelet::Nodelet { auto timestamp_mode = pnh.param("timestamp_mode", std::string{}); double ptp_utc_tai_offset = pnh.param("ptp_utc_tai_offset", -37.0); + auto min_scan_valid_columns_ratio = pnh.param("min_scan_valid_columns_ratio", 0.0); + if (min_scan_valid_columns_ratio < 0.0 || min_scan_valid_columns_ratio > 1.0) { + NODELET_FATAL("min_scan_valid_columns_ratio needs to be in the range [0, 1]"); + throw std::runtime_error("min_scan_valid_columns_ratio out of bounds!"); + } + std::vector processors { ImageProcessor::create( info, "os_lidar", /*TODO: tf_bcast.point_cloud_frame_id()*/ @@ -111,7 +117,8 @@ class OusterImage : public nodelet::Nodelet { lidar_packet_handler = LidarPacketHandler::create( info, processors, timestamp_mode, - static_cast(ptp_utc_tai_offset * 1e+9)); + static_cast(ptp_utc_tai_offset * 1e+9), + min_scan_valid_columns_ratio); } private: From 4bf83f97e58006a02f79a24d2f0a46894f8c068b Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 30 Jan 2025 16:32:41 -0800 Subject: [PATCH 3/8] Add the parameter to launch files --- launch/common.launch | 5 +++++ launch/driver.launch | 5 +++++ launch/record.launch | 5 +++++ launch/replay.launch | 5 +++++ launch/replay_pcap.launch | 5 +++++ launch/sensor.launch | 5 +++++ launch/sensor_mtp.launch | 5 +++++ 7 files changed, 35 insertions(+) diff --git a/launch/common.launch b/launch/common.launch index e3347b81..aea8657c 100644 --- a/launch/common.launch +++ b/launch/common.launch @@ -51,6 +51,9 @@ + + + diff --git a/launch/driver.launch b/launch/driver.launch index a54d6279..b6945832 100644 --- a/launch/driver.launch +++ b/launch/driver.launch @@ -99,6 +99,9 @@ + + @@ -139,6 +142,8 @@ + diff --git a/launch/record.launch b/launch/record.launch index 5666a366..2ff3e872 100644 --- a/launch/record.launch +++ b/launch/record.launch @@ -99,6 +99,9 @@ doc="maximum number of attempts trying to communicate with the sensor. Counter resets upon successful connection"/> + + @@ -154,6 +157,8 @@ + diff --git a/launch/replay.launch b/launch/replay.launch index a970b916..0ba7319c 100644 --- a/launch/replay.launch +++ b/launch/replay.launch @@ -77,6 +77,9 @@ + + @@ -116,6 +119,8 @@ + diff --git a/launch/replay_pcap.launch b/launch/replay_pcap.launch index 05b544c2..5402b401 100644 --- a/launch/replay_pcap.launch +++ b/launch/replay_pcap.launch @@ -72,6 +72,9 @@ + + @@ -112,6 +115,8 @@ + diff --git a/launch/sensor.launch b/launch/sensor.launch index 84075fb1..ba3d102b 100644 --- a/launch/sensor.launch +++ b/launch/sensor.launch @@ -107,6 +107,9 @@ + + @@ -162,6 +165,8 @@ + diff --git a/launch/sensor_mtp.launch b/launch/sensor_mtp.launch index a65acf67..894ba32c 100644 --- a/launch/sensor_mtp.launch +++ b/launch/sensor_mtp.launch @@ -111,6 +111,9 @@ + + @@ -168,6 +171,8 @@ + From 674c7406edcfeee46de08d4791693a933372407e Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 30 Jan 2025 16:39:56 -0800 Subject: [PATCH 4/8] Update CHANGELOG.txt --- CHANGELOG.rst | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 21b22bc1..6852002f 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -12,6 +12,8 @@ Changelog * Introduced a new topic ``/ouster/telemetry`` that publishes ``ouster_ros::Telemetry`` messages, the topic can be turned on/off by including the token ``TLM`` in the flag ``proc_mask`` launch arg. * [BUGFIX]: NEAR_IR data is not populated with data for organized point clouds that have no range. +* Add a new launch file parameter ``min_scan_valid_columns_ratio`` to allow users to set the minimum + ratio of valid columns in a scan for it to be processed. Default value is ``0.0``. ouster_ros v0.13.0 From fc45475d05f77c41f78bff3ed7e10b74ca8a71be Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Fri, 31 Jan 2025 08:55:53 -0800 Subject: [PATCH 5/8] Include scan width and correct formatting --- src/lidar_packet_handler.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/lidar_packet_handler.h b/src/lidar_packet_handler.h index c4ab216b..db14012c 100644 --- a/src/lidar_packet_handler.h +++ b/src/lidar_packet_handler.h @@ -135,10 +135,10 @@ class LidarPacketHandler { auto status = lidar_scan.status(); size_t valid_cols = std::count_if(status.data(), status.data() + status.size(), [](const uint32_t s) { return (s & 0x01); }); - if (valid_cols < static_cast(min_scan_valid_columns_ratio_ * lidar_scan.status().size())) { - NODELET_WARN_STREAM("number of valid columns per scan " << valid_cols + if (valid_cols < static_cast(min_scan_valid_columns_ratio_ * status.size())) { + NODELET_WARN_STREAM("number of valid columns per scan " << valid_cols << "/" << status.size() <<" which is below the ratio " << std::setprecision(4) << (100 * min_scan_valid_columns_ratio_) - << "%% SKIPPING SCAN"); + << "%, SKIPPING SCAN"); result = false; } } From 96f6da2a9f7ef2ccfb6c49aef8d8980765ee844c Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Mon, 3 Feb 2025 15:46:02 -0800 Subject: [PATCH 6/8] min_scan_valid_columns_ratio should be zero by default --- CHANGELOG.rst | 8 ++++---- src/lidar_packet_handler.h | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 6852002f..bde8896a 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -6,12 +6,12 @@ Changelog [unreleased] ============ * [BUGFIX]: correctly align timestamps to the generated point cloud -* Added support to enable **loop** for pcap replay + other replay config -* Added a new launch file parameter ``pub_static_tf`` that allows users to turn off the broadcast +* [BUGFIX]: NEAR_IR data is not populated with data for organized point clouds that have no range. +* Add support to enable **loop** for pcap replay + other replay config +* Add a new launch file parameter ``pub_static_tf`` that allows users to turn off the broadcast of sensor TF transforms. -* Introduced a new topic ``/ouster/telemetry`` that publishes ``ouster_ros::Telemetry`` messages, +* Introduce a new topic ``/ouster/telemetry`` that publishes ``ouster_ros::Telemetry`` messages, the topic can be turned on/off by including the token ``TLM`` in the flag ``proc_mask`` launch arg. -* [BUGFIX]: NEAR_IR data is not populated with data for organized point clouds that have no range. * Add a new launch file parameter ``min_scan_valid_columns_ratio`` to allow users to set the minimum ratio of valid columns in a scan for it to be processed. Default value is ``0.0``. diff --git a/src/lidar_packet_handler.h b/src/lidar_packet_handler.h index db14012c..dfca43a5 100644 --- a/src/lidar_packet_handler.h +++ b/src/lidar_packet_handler.h @@ -383,7 +383,7 @@ class LidarPacketHandler { int64_t ptp_utc_tai_offset_; - float min_scan_valid_columns_ratio_ = 0.7; + float min_scan_valid_columns_ratio_ = 0.0f; }; } // namespace ouster_ros \ No newline at end of file From 82a70e584a6f6ca8ffb8525e5c15dd50dcfa84ad Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Mon, 3 Feb 2025 15:58:19 -0800 Subject: [PATCH 7/8] Tidy up things --- src/os_cloud_nodelet.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/os_cloud_nodelet.cpp b/src/os_cloud_nodelet.cpp index 7d5e803d..40bc7dcb 100644 --- a/src/os_cloud_nodelet.cpp +++ b/src/os_cloud_nodelet.cpp @@ -179,14 +179,14 @@ class OusterCloud : public nodelet::Nodelet { static_cast(ptp_utc_tai_offset * 1e+9)); } - std::vector processors; - auto min_scan_valid_columns_ratio = pnh.param("min_scan_valid_columns_ratio", 0.0); if (min_scan_valid_columns_ratio < 0.0 || min_scan_valid_columns_ratio > 1.0) { NODELET_FATAL("min_scan_valid_columns_ratio needs to be in the range [0, 1]"); throw std::runtime_error("min_scan_valid_columns_ratio out of bounds!"); } + std::vector processors; + if (impl::check_token(tokens, "PCL")) { auto point_type = pnh.param("point_type", std::string{"original"}); auto organized = pnh.param("organized", true); From b324d5a7941027f2fd7081701a691683886ada3c Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Mon, 3 Feb 2025 16:05:34 -0800 Subject: [PATCH 8/8] Couple nits --- src/os_cloud_nodelet.cpp | 4 ++-- src/os_driver_nodelet.cpp | 4 ++-- src/os_image_nodelet.cpp | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/os_cloud_nodelet.cpp b/src/os_cloud_nodelet.cpp index 40bc7dcb..5c1a3437 100644 --- a/src/os_cloud_nodelet.cpp +++ b/src/os_cloud_nodelet.cpp @@ -179,8 +179,8 @@ class OusterCloud : public nodelet::Nodelet { static_cast(ptp_utc_tai_offset * 1e+9)); } - auto min_scan_valid_columns_ratio = pnh.param("min_scan_valid_columns_ratio", 0.0); - if (min_scan_valid_columns_ratio < 0.0 || min_scan_valid_columns_ratio > 1.0) { + auto min_scan_valid_columns_ratio = pnh.param("min_scan_valid_columns_ratio", 0.0f); + if (min_scan_valid_columns_ratio < 0.0f || min_scan_valid_columns_ratio > 1.0f) { NODELET_FATAL("min_scan_valid_columns_ratio needs to be in the range [0, 1]"); throw std::runtime_error("min_scan_valid_columns_ratio out of bounds!"); } diff --git a/src/os_driver_nodelet.cpp b/src/os_driver_nodelet.cpp index ee4be59d..bbab29c9 100644 --- a/src/os_driver_nodelet.cpp +++ b/src/os_driver_nodelet.cpp @@ -125,8 +125,8 @@ class OusterDriver : public OusterSensor { static_cast(ptp_utc_tai_offset * 1e+9)); } - auto min_scan_valid_columns_ratio = pnh.param("min_scan_valid_columns_ratio", 0.0); - if (min_scan_valid_columns_ratio < 0.0 || min_scan_valid_columns_ratio > 1.0) { + auto min_scan_valid_columns_ratio = pnh.param("min_scan_valid_columns_ratio", 0.0f); + if (min_scan_valid_columns_ratio < 0.0f || min_scan_valid_columns_ratio > 1.0f) { NODELET_FATAL("min_scan_valid_columns_ratio needs to be in the range [0, 1]"); throw std::runtime_error("min_scan_valid_columns_ratio out of bounds!"); } diff --git a/src/os_image_nodelet.cpp b/src/os_image_nodelet.cpp index 9d26f08b..34c9a773 100644 --- a/src/os_image_nodelet.cpp +++ b/src/os_image_nodelet.cpp @@ -99,8 +99,8 @@ class OusterImage : public nodelet::Nodelet { auto timestamp_mode = pnh.param("timestamp_mode", std::string{}); double ptp_utc_tai_offset = pnh.param("ptp_utc_tai_offset", -37.0); - auto min_scan_valid_columns_ratio = pnh.param("min_scan_valid_columns_ratio", 0.0); - if (min_scan_valid_columns_ratio < 0.0 || min_scan_valid_columns_ratio > 1.0) { + auto min_scan_valid_columns_ratio = pnh.param("min_scan_valid_columns_ratio", 0.0f); + if (min_scan_valid_columns_ratio < 0.0f || min_scan_valid_columns_ratio > 1.0f) { NODELET_FATAL("min_scan_valid_columns_ratio needs to be in the range [0, 1]"); throw std::runtime_error("min_scan_valid_columns_ratio out of bounds!"); }