ROS-268-min-valid-cols-lidar-scan

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

launch/common.launch

@@ -51,6 +51,9 @@
   <arg name="min_range" doc="minimum lidar range to consider (meters)"/>
   <arg name="max_range" doc="maximum lidar range to consider (meters)"/>
 
+  <arg name="min_scan_valid_columns_ratio"
+    doc="The minimum ratio of valid columns for processing the LidarScan [0, 1]"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_cloud_node"
       output="screen" required="true"
@@ -74,6 +77,8 @@
       <param name="~/destagger" value="$(arg destagger)"/>
       <param name="~/min_range" value="$(arg min_range)"/>
       <param name="~/max_range" value="$(arg max_range)"/>
+      <param name="~/min_scan_valid_columns_ratio"
+        value="$(arg min_scan_valid_columns_ratio)"/>
     </node>
   </group>
 

launch/driver.launch

@@ -99,6 +99,9 @@
   <arg name="max_range" default="10000.0"
     doc="maximum lidar range to consider (meters)"/>
 
+  <arg name="min_scan_valid_columns_ratio" default="0.0"
+    doc="The minimum ratio of valid columns for processing the LidarScan [0, 1]"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -139,6 +142,8 @@
       <param name="~/destagger" value="$(arg destagger)"/>
       <param name="~/min_range" value="$(arg min_range)"/>
       <param name="~/max_range" value="$(arg max_range)"/>
+      <param name="~/min_scan_valid_columns_ratio"
+        value="$(arg min_scan_valid_columns_ratio)"/>
     </node>
   </group>
 

launch/record.launch

@@ -99,6 +99,9 @@
     doc="maximum number of attempts trying to communicate with the sensor.
          Counter resets upon successful connection"/>
 
+  <arg name="min_scan_valid_columns_ratio" default="0.0"
+    doc="The minimum ratio of valid columns for processing the LidarScan [0, 1]"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -154,6 +157,8 @@
     <arg name="destagger" value="$(arg destagger)"/>
     <arg name="min_range" value="$(arg min_range)"/>
     <arg name="max_range" value="$(arg max_range)"/>
+    <arg name="~/min_scan_valid_columns_ratio"
+        value="$(arg min_scan_valid_columns_ratio)"/>
   </include>
 
   <arg name="_use_bag_file_name" value="$(eval not (bag_file == ''))"/>

launch/replay.launch

@@ -77,6 +77,9 @@
   <arg name="max_range" default="10000.0"
     doc="maximum lidar range to consider (meters)"/>
 
+  <arg name="min_scan_valid_columns_ratio" default="0.0"
+    doc="The minimum ratio of valid columns for processing the LidarScan [0, 1]"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -116,6 +119,8 @@
     <arg name="destagger" value="$(arg destagger)"/>
     <arg name="min_range" value="$(arg min_range)"/>
     <arg name="max_range" value="$(arg max_range)"/>
+    <arg name="~/min_scan_valid_columns_ratio"
+        value="$(arg min_scan_valid_columns_ratio)"/>
   </include>
 
   <arg name="_use_bag_file_name" value="$(eval not (bag_file == ''))"/>

launch/replay_pcap.launch

@@ -72,6 +72,9 @@
   <arg name="max_range" default="10000.0"
     doc="maximum lidar range to consider (meters)"/>
 
+  <arg name="min_scan_valid_columns_ratio" default="0.0"
+    doc="The minimum ratio of valid columns for processing the LidarScan [0, 1]"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -112,6 +115,8 @@
     <arg name="destagger" value="$(arg destagger)"/>
     <arg name="min_range" value="$(arg min_range)"/>
     <arg name="max_range" value="$(arg max_range)"/>
+    <arg name="~/min_scan_valid_columns_ratio"
+        value="$(arg min_scan_valid_columns_ratio)"/>
   </include>
 
 

launch/sensor.launch

@@ -107,6 +107,9 @@
   <arg name="max_range" default="10000.0"
     doc="maximum lidar range to consider (meters)"/>
 
+  <arg name="min_scan_valid_columns_ratio" default="0.0"
+    doc="The minimum ratio of valid columns for processing the LidarScan [0, 1]"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -162,6 +165,8 @@
     <arg name="destagger" value="$(arg destagger)"/>
     <arg name="min_range" value="$(arg min_range)"/>
     <arg name="max_range" value="$(arg max_range)"/>
+    <arg name="~/min_scan_valid_columns_ratio"
+        value="$(arg min_scan_valid_columns_ratio)"/>
   </include>
 
 </launch>

launch/sensor_mtp.launch

@@ -111,6 +111,9 @@
   <arg name="max_range" default="10000.0"
     doc="maximum lidar range to consider (meters)"/>
 
+  <arg name="min_scan_valid_columns_ratio" default="0.0"
+    doc="The minimum ratio of valid columns for processing the LidarScan [0, 1]"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -168,6 +171,8 @@
     <arg name="destagger" value="$(arg destagger)"/>
     <arg name="min_range" value="$(arg min_range)"/>
     <arg name="max_range" value="$(arg max_range)"/>
+    <arg name="~/min_scan_valid_columns_ratio"
+        value="$(arg min_scan_valid_columns_ratio)"/>
   </include>
 
 </launch>

src/lidar_packet_handler.h

@@ -69,10 +69,12 @@ class LidarPacketHandler {
     LidarPacketHandler(const sensor::sensor_info& info,
                        const std::vector<LidarScanProcessor>& 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<ouster::ScanBatcher>(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<size_t>(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;
+                        }
+                    }
                 }
                 if (result) {
                     ring_buffer.write();
@@ -156,9 +170,11 @@ class LidarPacketHandler {
     static HandlerType create(
         const sensor::sensor_info& info,
         const std::vector<LidarScanProcessor>& 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<LidarPacketHandler>(
-            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<ouster::ScanBatcher> 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<std::unique_ptr<ouster::LidarScan>> 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.7;
 };
 
 }  // namespace ouster_ros

src/os_cloud_nodelet.cpp

@@ -180,6 +180,13 @@ class OusterCloud : public nodelet::Nodelet {
         }
 
         std::vector<LidarScanProcessor> 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<int64_t>(ptp_utc_tai_offset * 1e+9));
+                static_cast<int64_t>(ptp_utc_tai_offset * 1e+9),
+                min_scan_valid_columns_ratio);
         }
 
         if (impl::check_token(tokens, "TLM")) {

src/os_driver_nodelet.cpp

@@ -125,6 +125,12 @@ class OusterDriver : public OusterSensor {
                 static_cast<int64_t>(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<LidarScanProcessor> 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<int64_t>(ptp_utc_tai_offset * 1e+9));
+                static_cast<int64_t>(ptp_utc_tai_offset * 1e+9),
+                min_scan_valid_columns_ratio);
         }
 
         if (impl::check_token(tokens, "TLM")) {

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<LidarScanProcessor> 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<int64_t>(ptp_utc_tai_offset * 1e+9));
+            static_cast<int64_t>(ptp_utc_tai_offset * 1e+9),
+            min_scan_valid_columns_ratio);
     }
 
    private: