diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 21b22bc1..bde8896a 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -6,12 +6,14 @@ 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``. ouster_ros v0.13.0 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 @@ + diff --git a/src/lidar_packet_handler.h b/src/lidar_packet_handler.h index 3e04cc09..dfca43a5 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); @@ -128,6 +130,18 @@ 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(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"); + result = false; + } + } } if (result) { ring_buffer.write(); @@ -156,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(); @@ -337,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; @@ -366,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.0f; }; } // 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..5c1a3437 100644 --- a/src/os_cloud_nodelet.cpp +++ b/src/os_cloud_nodelet.cpp @@ -179,7 +179,14 @@ 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.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!"); + } + 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); @@ -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..bbab29c9 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.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!"); + } + 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..34c9a773 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.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!"); + } + 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: