From d41c11e1fb6a67d2f9d2fbe6c1e506ed58fe3fab Mon Sep 17 00:00:00 2001
From: "Ola Chr. Vaage" <ola.christoffer.vage@scoutdi.com>
Date: Wed, 22 Nov 2023 13:38:42 +0100
Subject: [PATCH 1/3] Add min_lidar_packets_per_cloud arg in launch files

---
 launch/common.launch     | 10 ++++++++++
 launch/driver.launch     | 12 ++++++++++++
 launch/record.launch     | 12 ++++++++++++
 launch/replay.launch     | 12 ++++++++++++
 launch/sensor.launch     | 12 ++++++++++++
 launch/sensor_mtp.launch | 13 +++++++++++++
 6 files changed, 71 insertions(+)

diff --git a/launch/common.launch b/launch/common.launch
index ef3725e2..0f66a0b7 100644
--- a/launch/common.launch
+++ b/launch/common.launch
@@ -41,6 +41,15 @@
     xyzir
     }"/>
 
+  <arg name="min_lidar_packets_per_cloud" doc="Each cloud is built up of an expected
+  amount of udp packets. With this set to 0, the driver will publish all clouds regardless
+  of how many packets it received. If set to a positive integer, the driver will discard the entire
+  cloud if it does not consist of at least this many udp packets.
+  Number of expected packets is dependent on lidar_mode and udp_profile_lidar.
+  To calculate the number of expected packets, use the following formula:
+
+  num_expected_packets = info.format.columns_per_frame * info.format.columns_per_packet"
+  />
 
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_cloud_node"
@@ -60,6 +69,7 @@
       <param name="~/scan_ring" value="$(arg scan_ring)"/>
       <param name="~/ptp_utc_tai_offset" type="double" value="$(arg ptp_utc_tai_offset)"/>
       <param name="~/point_type" value="$(arg point_type)"/>
+      <param name="~/min_lidar_packets_per_cloud" value="$(arg min_lidar_packets_per_cloud)"/>
     </node>
   </group>
 
diff --git a/launch/driver.launch b/launch/driver.launch
index 0cc23f05..f34a1ae0 100644
--- a/launch/driver.launch
+++ b/launch/driver.launch
@@ -64,6 +64,17 @@
     xyzir
     }"/>
 
+  <arg name="min_lidar_packets_per_cloud" default="0"
+  doc="Each cloud is built up of an expected amount of udp packets. With this set
+  to 0, the driver will publish all clouds regardless of how many packets it
+  received. If set to a positive integer, the driver will discard the entire
+  cloud if it does not consist of at least this many udp packets.
+  Number of expected packets is dependent on lidar_mode and udp_profile_lidar.
+  To calculate the number of expected packets, use the following formula:
+
+  num_expected_packets = info.format.columns_per_frame * info.format.columns_per_packet"
+  />
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -91,6 +102,7 @@
       <param name="~/proc_mask" value="$(arg proc_mask)"/>
       <param name="~/scan_ring" value="$(arg scan_ring)"/>
       <param name="~/point_type" value="$(arg point_type)"/>
+      <param name="~/min_lidar_packets_per_cloud" value="$(arg min_lidar_packets_per_cloud)"/>
     </node>
   </group>
 
diff --git a/launch/record.launch b/launch/record.launch
index ab32e6b5..e7f4686e 100644
--- a/launch/record.launch
+++ b/launch/record.launch
@@ -67,6 +67,17 @@
     xyzir
     }"/>
 
+  <arg name="min_lidar_packets_per_cloud" default="0"
+  doc="Each cloud is built up of an expected amount of udp packets. With this set
+  to 0, the driver will publish all clouds regardless of how many packets it
+  received. If set to a positive integer, the driver will discard the entire
+  cloud if it does not consist of at least this many udp packets.
+  Number of expected packets is dependent on lidar_mode and udp_profile_lidar.
+  To calculate the number of expected packets, use the following formula:
+
+  num_expected_packets = info.format.columns_per_frame * info.format.columns_per_packet"
+  />
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -106,6 +117,7 @@
     <arg name="proc_mask" value="$(arg proc_mask)"/>
     <arg name="scan_ring" value="$(arg scan_ring)"/>
     <arg name="point_type" value="$(arg point_type)"/>
+    <arg name="min_lidar_packets_per_cloud" value="$(arg min_lidar_packets_per_cloud)"/>
   </include>
 
   <arg name="_use_bag_file_name" value="$(eval not (bag_file == ''))"/>
diff --git a/launch/replay.launch b/launch/replay.launch
index 85a85896..a22be58a 100644
--- a/launch/replay.launch
+++ b/launch/replay.launch
@@ -47,6 +47,17 @@
     xyzir
     }"/>
 
+  <arg name="min_lidar_packets_per_cloud" default="0"
+  doc="Each cloud is built up of an expected amount of udp packets. With this set
+  to 0, the driver will publish all clouds regardless of how many packets it
+  received. If set to a positive integer, the driver will discard the entire
+  cloud if it does not consist of at least this many udp packets.
+  Number of expected packets is dependent on lidar_mode and udp_profile_lidar.
+  To calculate the number of expected packets, use the following formula:
+
+  num_expected_packets = info.format.columns_per_frame * info.format.columns_per_packet"
+  />
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -81,6 +92,7 @@
     <arg name="proc_mask" value="$(arg proc_mask)"/>
     <arg name="scan_ring" value="$(arg scan_ring)"/>
     <arg name="point_type" value="$(arg point_type)"/>
+    <arg name="min_lidar_packets_per_cloud" value="$(arg min_lidar_packets_per_cloud)"/>
   </include>
 
   <arg name="_use_bag_file_name" value="$(eval not (bag_file == ''))"/>
diff --git a/launch/sensor.launch b/launch/sensor.launch
index 003fed6a..74164150 100644
--- a/launch/sensor.launch
+++ b/launch/sensor.launch
@@ -72,6 +72,17 @@
     xyzir
     }"/>
 
+  <arg name="min_lidar_packets_per_cloud" default="0"
+  doc="Each cloud is built up of an expected amount of udp packets. With this set
+  to 0, the driver will publish all clouds regardless of how many packets it
+  received. If set to a positive integer, the driver will discard the entire
+  cloud if it does not consist of at least this many udp packets.
+  Number of expected packets is dependent on lidar_mode and udp_profile_lidar.
+  To calculate the number of expected packets, use the following formula:
+
+  num_expected_packets = info.format.columns_per_frame * info.format.columns_per_packet"
+  />
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -111,6 +122,7 @@
     <arg name="proc_mask" value="$(arg proc_mask)"/>
     <arg name="scan_ring" value="$(arg scan_ring)"/>
     <arg name="point_type" value="$(arg point_type)"/>
+    <arg name="min_lidar_packets_per_cloud" value="$(arg min_lidar_packets_per_cloud)"/>
   </include>
 
 </launch>
diff --git a/launch/sensor_mtp.launch b/launch/sensor_mtp.launch
index db05f5eb..a6467f79 100644
--- a/launch/sensor_mtp.launch
+++ b/launch/sensor_mtp.launch
@@ -76,6 +76,17 @@
     xyzir
     }"/>
 
+  <arg name="min_lidar_packets_per_cloud" default="0"
+  doc="Each cloud is built up of an expected amount of udp packets. With this set
+  to 0, the driver will publish all clouds regardless of how many packets it
+  received. If set to a positive integer, the driver will discard the entire
+  cloud if it does not consist of at least this many udp packets.
+  Number of expected packets is dependent on lidar_mode and udp_profile_lidar.
+  To calculate the number of expected packets, use the following formula:
+
+  num_expected_packets = info.format.columns_per_frame * info.format.columns_per_packet"
+  />
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true"
@@ -118,6 +129,8 @@
     <arg name="proc_mask" value="$(arg proc_mask)"/>
     <arg name="scan_ring" value="$(arg scan_ring)"/>
     <arg name="point_type" value="$(arg point_type)"/>
+    <arg name="min_lidar_packets_per_cloud" value="$(arg min_lidar_packets_per_cloud)"/>
+
   </include>
 
 </launch>

From 0aab35a08d14994f891022490e5d398490910899 Mon Sep 17 00:00:00 2001
From: "Ola Chr. Vaage" <ola.christoffer.vage@scoutdi.com>
Date: Thu, 23 Nov 2023 09:53:55 +0100
Subject: [PATCH 2/3] Discard clouds with less lidar packets than
 min_lidar_packets_per_cloud

No functional changes with default param settings, but this allows the user to discard incomplete
clouds that could lead to e.g. bad scan-matching results.
---
 src/lidar_packet_handler.h | 22 +++++++++++++++++++---
 src/os_cloud_nodelet.cpp   |  5 ++++-
 src/os_driver_nodelet.cpp  |  5 ++++-
 src/os_image_nodelet.cpp   |  7 +++++--
 4 files changed, 32 insertions(+), 7 deletions(-)

diff --git a/src/lidar_packet_handler.h b/src/lidar_packet_handler.h
index 9c683fe9..f05adcbf 100644
--- a/src/lidar_packet_handler.h
+++ b/src/lidar_packet_handler.h
@@ -120,11 +120,27 @@ class LidarPacketHandler {
     static HandlerType create_handler(
         const ouster::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,
+        const int min_lidar_packets_per_cloud) {
         auto handler = std::make_shared<LidarPacketHandler>(
             info, handlers, timestamp_mode, ptp_utc_tai_offset);
-        return [handler](const uint8_t* lidar_buf) {
+        return [handler, info,
+                min_lidar_packets_per_cloud](const uint8_t* lidar_buf) {
+            thread_local int num_packets_in_cloud = 0;
+            num_packets_in_cloud++;
+
             if (handler->lidar_packet_accumlator(lidar_buf)) {
+                if (num_packets_in_cloud < min_lidar_packets_per_cloud) {
+                    ROS_WARN_STREAM_THROTTLE(
+                        0.1,
+                        "Incomplete cloud, dropping it. Got "
+                            << num_packets_in_cloud << ", expected "
+                            << min_lidar_packets_per_cloud << " lidar packets.");
+                    num_packets_in_cloud = 0;
+                    return;
+              }
+              num_packets_in_cloud = 0;
+
                 for (auto h : handler->lidar_scan_handlers) {
                     h(*handler->lidar_scan, handler->lidar_scan_estimated_ts,
                       handler->lidar_scan_estimated_msg_ts);
@@ -288,4 +304,4 @@ class LidarPacketHandler {
     LidarPacketAccumlator lidar_packet_accumlator;
 };
 
-}  // namespace ouster_ros
\ No newline at end of file
+}  // namespace ouster_ros
diff --git a/src/os_cloud_nodelet.cpp b/src/os_cloud_nodelet.cpp
index c1f7f3f8..472cff24 100644
--- a/src/os_cloud_nodelet.cpp
+++ b/src/os_cloud_nodelet.cpp
@@ -171,10 +171,13 @@ class OusterCloud : public nodelet::Nodelet {
                 }));
         }
 
+        const int min_lidar_packets_per_cloud =
+            pnh.param("min_lidar_packets_per_cloud", 0);
         if (impl::check_token(tokens, "PCL") || impl::check_token(tokens, "SCAN")) {
             lidar_packet_handler = LidarPacketHandler::create_handler(
                 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_lidar_packets_per_cloud);
             lidar_packet_sub = nh.subscribe<PacketMsg>(
                 "lidar_packets", 100, [this](const PacketMsg::ConstPtr msg) {
                     lidar_packet_handler(msg->buf.data());
diff --git a/src/os_driver_nodelet.cpp b/src/os_driver_nodelet.cpp
index bfe84927..c4479153 100644
--- a/src/os_driver_nodelet.cpp
+++ b/src/os_driver_nodelet.cpp
@@ -156,11 +156,14 @@ class OusterDriver : public OusterSensor {
                 }));
         }
 
+        const int min_lidar_packets_per_cloud =
+            pnh.param("min_lidar_packets_per_cloud", 0);
         if (impl::check_token(tokens, "PCL") || impl::check_token(tokens, "SCAN") ||
             impl::check_token(tokens, "IMG"))
             lidar_packet_handler = LidarPacketHandler::create_handler(
                 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_lidar_packets_per_cloud);
     }
 
     virtual void on_lidar_packet_msg(const uint8_t* raw_lidar_packet) override {
diff --git a/src/os_image_nodelet.cpp b/src/os_image_nodelet.cpp
index 736fae69..bbf46a73 100644
--- a/src/os_image_nodelet.cpp
+++ b/src/os_image_nodelet.cpp
@@ -50,7 +50,7 @@ class OusterImage : public nodelet::Nodelet {
     }
 
     void create_publishers_subscribers(int n_returns) {
-        // TODO: avoid having to replicate the parameters: 
+        // TODO: avoid having to replicate the parameters:
         // timestamp_mode, ptp_utc_tai_offset, use_system_default_qos in yet
         // another node.
         auto& pnh = getPrivateNodeHandle();
@@ -94,9 +94,12 @@ class OusterImage : public nodelet::Nodelet {
                 })
         };
 
+        const int min_lidar_packets_per_cloud =
+            pnh.param("min_lidar_packets_per_cloud", 0);
         lidar_packet_handler = LidarPacketHandler::create_handler(
             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_lidar_packets_per_cloud);
         lidar_packet_sub = nh.subscribe<PacketMsg>(
                 "lidar_packets", 100,
                 [this](const PacketMsg::ConstPtr msg) {

From 3f4472cb332f8253595098cb0611a37bf6bc020e Mon Sep 17 00:00:00 2001
From: "Ola Chr. Vaage" <ola.christoffer.vage@scoutdi.com>
Date: Thu, 23 Nov 2023 13:36:26 +0100
Subject: [PATCH 3/3] Return num valid columns from processer

---
 launch/common.launch        | 18 ++++++++----------
 launch/driver.launch        | 18 +++++++-----------
 launch/record.launch        | 18 +++++++-----------
 launch/replay.launch        | 18 +++++++-----------
 launch/sensor.launch        | 18 +++++++-----------
 launch/sensor_mtp.launch    | 18 +++++++-----------
 src/image_processor.h       | 17 +++++++++++++----
 src/laser_scan_processor.h  | 16 ++++++++++++----
 src/lidar_packet_handler.h  | 22 +++-------------------
 src/os_cloud_nodelet.cpp    | 34 ++++++++++++++++++++--------------
 src/os_driver_nodelet.cpp   | 34 +++++++++++++++++++++++-----------
 src/os_image_nodelet.cpp    | 15 +++++++++------
 src/point_cloud_processor.h | 16 ++++++++++++----
 13 files changed, 135 insertions(+), 127 deletions(-)

diff --git a/launch/common.launch b/launch/common.launch
index 0f66a0b7..271548af 100644
--- a/launch/common.launch
+++ b/launch/common.launch
@@ -41,15 +41,12 @@
     xyzir
     }"/>
 
-  <arg name="min_lidar_packets_per_cloud" doc="Each cloud is built up of an expected
-  amount of udp packets. With this set to 0, the driver will publish all clouds regardless
-  of how many packets it received. If set to a positive integer, the driver will discard the entire
-  cloud if it does not consist of at least this many udp packets.
-  Number of expected packets is dependent on lidar_mode and udp_profile_lidar.
-  To calculate the number of expected packets, use the following formula:
-
-  num_expected_packets = info.format.columns_per_frame * info.format.columns_per_packet"
-  />
+  <arg name="num_columns_required" doc="
+    Each pointcloud is built from a number of UDP data packets. If some packets are
+    lost, the pointcloud will be incomplete. If you set this to zero, all pointclouds
+    will be published regardless of completeness. If set to a positive integer,
+    pointclouds with fewer than the specified number of columns will not be published.
+    The number of columns is the same as the width of the pointcloud (512, 1024, etc)."/>
 
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_cloud_node"
@@ -69,7 +66,7 @@
       <param name="~/scan_ring" value="$(arg scan_ring)"/>
       <param name="~/ptp_utc_tai_offset" type="double" value="$(arg ptp_utc_tai_offset)"/>
       <param name="~/point_type" value="$(arg point_type)"/>
-      <param name="~/min_lidar_packets_per_cloud" value="$(arg min_lidar_packets_per_cloud)"/>
+      <param name="~/num_columns_required" value="$(arg num_columns_required)"/>
     </node>
   </group>
 
@@ -77,6 +74,7 @@
     <node pkg="nodelet" type="nodelet" name="img_node"
       output="screen" required="true"
       args="load ouster_ros/OusterImage os_nodelet_mgr $(arg _no_bond)">
+      <param name="~/num_columns_required" value="$(arg num_columns_required)"/>
     </node>
   </group>
 
diff --git a/launch/driver.launch b/launch/driver.launch
index f34a1ae0..51699ca0 100644
--- a/launch/driver.launch
+++ b/launch/driver.launch
@@ -64,16 +64,12 @@
     xyzir
     }"/>
 
-  <arg name="min_lidar_packets_per_cloud" default="0"
-  doc="Each cloud is built up of an expected amount of udp packets. With this set
-  to 0, the driver will publish all clouds regardless of how many packets it
-  received. If set to a positive integer, the driver will discard the entire
-  cloud if it does not consist of at least this many udp packets.
-  Number of expected packets is dependent on lidar_mode and udp_profile_lidar.
-  To calculate the number of expected packets, use the following formula:
-
-  num_expected_packets = info.format.columns_per_frame * info.format.columns_per_packet"
-  />
+  <arg name="num_columns_required" default="0" doc=" Each pointcloud is built from a number
+    of UDP data packets. If some packets are lost, the pointcloud will be incomplete.
+    If you set this to zero, all pointclouds will be published regardless of completeness.
+    If set to a positive integer, pointclouds with fewer than the specified number of columns
+    will not be published.
+    The number of columns is the same as the width of the pointcloud (512, 1024, etc)."/>
 
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
@@ -102,7 +98,7 @@
       <param name="~/proc_mask" value="$(arg proc_mask)"/>
       <param name="~/scan_ring" value="$(arg scan_ring)"/>
       <param name="~/point_type" value="$(arg point_type)"/>
-      <param name="~/min_lidar_packets_per_cloud" value="$(arg min_lidar_packets_per_cloud)"/>
+      <param name="~/num_columns_required" value="$(arg num_columns_required)"/>
     </node>
   </group>
 
diff --git a/launch/record.launch b/launch/record.launch
index e7f4686e..b509d01e 100644
--- a/launch/record.launch
+++ b/launch/record.launch
@@ -67,16 +67,12 @@
     xyzir
     }"/>
 
-  <arg name="min_lidar_packets_per_cloud" default="0"
-  doc="Each cloud is built up of an expected amount of udp packets. With this set
-  to 0, the driver will publish all clouds regardless of how many packets it
-  received. If set to a positive integer, the driver will discard the entire
-  cloud if it does not consist of at least this many udp packets.
-  Number of expected packets is dependent on lidar_mode and udp_profile_lidar.
-  To calculate the number of expected packets, use the following formula:
-
-  num_expected_packets = info.format.columns_per_frame * info.format.columns_per_packet"
-  />
+  <arg name="num_columns_required" default="0" doc=" Each pointcloud is built from a number
+    of UDP data packets. If some packets are lost, the pointcloud will be incomplete.
+    If you set this to zero, all pointclouds will be published regardless of completeness.
+    If set to a positive integer, pointclouds with fewer than the specified number of columns
+    will not be published.
+    The number of columns is the same as the width of the pointcloud (512, 1024, etc)."/>
 
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
@@ -117,7 +113,7 @@
     <arg name="proc_mask" value="$(arg proc_mask)"/>
     <arg name="scan_ring" value="$(arg scan_ring)"/>
     <arg name="point_type" value="$(arg point_type)"/>
-    <arg name="min_lidar_packets_per_cloud" value="$(arg min_lidar_packets_per_cloud)"/>
+    <arg name="num_columns_required" value="$(arg num_columns_required)"/>
   </include>
 
   <arg name="_use_bag_file_name" value="$(eval not (bag_file == ''))"/>
diff --git a/launch/replay.launch b/launch/replay.launch
index a22be58a..f51b22b0 100644
--- a/launch/replay.launch
+++ b/launch/replay.launch
@@ -47,16 +47,12 @@
     xyzir
     }"/>
 
-  <arg name="min_lidar_packets_per_cloud" default="0"
-  doc="Each cloud is built up of an expected amount of udp packets. With this set
-  to 0, the driver will publish all clouds regardless of how many packets it
-  received. If set to a positive integer, the driver will discard the entire
-  cloud if it does not consist of at least this many udp packets.
-  Number of expected packets is dependent on lidar_mode and udp_profile_lidar.
-  To calculate the number of expected packets, use the following formula:
-
-  num_expected_packets = info.format.columns_per_frame * info.format.columns_per_packet"
-  />
+  <arg name="num_columns_required" default="0" doc=" Each pointcloud is built from a number
+    of UDP data packets. If some packets are lost, the pointcloud will be incomplete.
+    If you set this to zero, all pointclouds will be published regardless of completeness.
+    If set to a positive integer, pointclouds with fewer than the specified number of columns
+    will not be published.
+    The number of columns is the same as the width of the pointcloud (512, 1024, etc)."/>
 
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
@@ -92,7 +88,7 @@
     <arg name="proc_mask" value="$(arg proc_mask)"/>
     <arg name="scan_ring" value="$(arg scan_ring)"/>
     <arg name="point_type" value="$(arg point_type)"/>
-    <arg name="min_lidar_packets_per_cloud" value="$(arg min_lidar_packets_per_cloud)"/>
+    <arg name="num_columns_required" value="$(arg num_columns_required)"/>
   </include>
 
   <arg name="_use_bag_file_name" value="$(eval not (bag_file == ''))"/>
diff --git a/launch/sensor.launch b/launch/sensor.launch
index 74164150..de282c43 100644
--- a/launch/sensor.launch
+++ b/launch/sensor.launch
@@ -72,16 +72,12 @@
     xyzir
     }"/>
 
-  <arg name="min_lidar_packets_per_cloud" default="0"
-  doc="Each cloud is built up of an expected amount of udp packets. With this set
-  to 0, the driver will publish all clouds regardless of how many packets it
-  received. If set to a positive integer, the driver will discard the entire
-  cloud if it does not consist of at least this many udp packets.
-  Number of expected packets is dependent on lidar_mode and udp_profile_lidar.
-  To calculate the number of expected packets, use the following formula:
-
-  num_expected_packets = info.format.columns_per_frame * info.format.columns_per_packet"
-  />
+  <arg name="num_columns_required" default="0" doc=" Each pointcloud is built from a number
+    of UDP data packets. If some packets are lost, the pointcloud will be incomplete.
+    If you set this to zero, all pointclouds will be published regardless of completeness.
+    If set to a positive integer, pointclouds with fewer than the specified number of columns
+    will not be published.
+    The number of columns is the same as the width of the pointcloud (512, 1024, etc)."/>
 
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
@@ -122,7 +118,7 @@
     <arg name="proc_mask" value="$(arg proc_mask)"/>
     <arg name="scan_ring" value="$(arg scan_ring)"/>
     <arg name="point_type" value="$(arg point_type)"/>
-    <arg name="min_lidar_packets_per_cloud" value="$(arg min_lidar_packets_per_cloud)"/>
+    <arg name="num_columns_required" value="$(arg num_columns_required)"/>
   </include>
 
 </launch>
diff --git a/launch/sensor_mtp.launch b/launch/sensor_mtp.launch
index a6467f79..ee9cbb49 100644
--- a/launch/sensor_mtp.launch
+++ b/launch/sensor_mtp.launch
@@ -76,16 +76,12 @@
     xyzir
     }"/>
 
-  <arg name="min_lidar_packets_per_cloud" default="0"
-  doc="Each cloud is built up of an expected amount of udp packets. With this set
-  to 0, the driver will publish all clouds regardless of how many packets it
-  received. If set to a positive integer, the driver will discard the entire
-  cloud if it does not consist of at least this many udp packets.
-  Number of expected packets is dependent on lidar_mode and udp_profile_lidar.
-  To calculate the number of expected packets, use the following formula:
-
-  num_expected_packets = info.format.columns_per_frame * info.format.columns_per_packet"
-  />
+  <arg name="num_columns_required" default="0" doc=" Each pointcloud is built from a number
+    of UDP data packets. If some packets are lost, the pointcloud will be incomplete.
+    If you set this to zero, all pointclouds will be published regardless of completeness.
+    If set to a positive integer, pointclouds with fewer than the specified number of columns
+    will not be published.
+    The number of columns is the same as the width of the pointcloud (512, 1024, etc)."/>
 
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
@@ -129,7 +125,7 @@
     <arg name="proc_mask" value="$(arg proc_mask)"/>
     <arg name="scan_ring" value="$(arg scan_ring)"/>
     <arg name="point_type" value="$(arg point_type)"/>
-    <arg name="min_lidar_packets_per_cloud" value="$(arg min_lidar_packets_per_cloud)"/>
+    <arg name="num_columns_required" value="$(arg num_columns_required)"/>
 
   </include>
 
diff --git a/src/image_processor.h b/src/image_processor.h
index 68a2d263..4c003030 100644
--- a/src/image_processor.h
+++ b/src/image_processor.h
@@ -25,8 +25,12 @@ namespace viz = ouster::viz;
 
 class ImageProcessor {
    public:
-    using OutputType =
+    using ProcessedData =
         std::map<sensor::ChanField, std::shared_ptr<sensor_msgs::Image>>;
+    struct OutputType {
+        int num_valid_columns;
+        ProcessedData image_msgs;
+    };
     using PostProcessingFn = std::function<void(OutputType)>;
 
    public:
@@ -76,12 +80,17 @@ class ImageProcessor {
    private:
     void process(const ouster::LidarScan& lidar_scan, uint64_t,
                  const ros::Time& msg_ts) {
+        OutputType out;
         process_return(lidar_scan, 0);
         if (get_n_returns(info_) == 2) process_return(lidar_scan, 1);
         for (auto it = image_msgs.begin(); it != image_msgs.end(); ++it) {
             it->second->header.stamp = msg_ts;
         }
-        if (post_processing_fn) post_processing_fn(image_msgs);
+        out.image_msgs = image_msgs;
+        out.num_valid_columns =
+            (lidar_scan.measurement_id().array() != 0).count() + 1;
+
+        if (post_processing_fn) post_processing_fn(out);
     }
 
     void process_return(const ouster::LidarScan& lidar_scan, int return_index) {
@@ -188,7 +197,7 @@ class ImageProcessor {
 
    private:
     std::string frame;
-    OutputType image_msgs;
+    ProcessedData image_msgs;
     PostProcessingFn post_processing_fn;
     sensor::sensor_info info_;
 
@@ -196,4 +205,4 @@ class ImageProcessor {
     viz::BeamUniformityCorrector nearir_buc;
 };
 
-}  // namespace ouster_ros
\ No newline at end of file
+}  // namespace ouster_ros
diff --git a/src/laser_scan_processor.h b/src/laser_scan_processor.h
index ccd184e7..7ec0ce46 100644
--- a/src/laser_scan_processor.h
+++ b/src/laser_scan_processor.h
@@ -18,7 +18,11 @@ namespace ouster_ros {
 
 class LaserScanProcessor {
    public:
-    using OutputType = std::vector<std::shared_ptr<sensor_msgs::LaserScan>>;
+    using ProcessedData = std::vector<std::shared_ptr<sensor_msgs::LaserScan>>;
+   struct OutputType {
+        int num_valid_columns;
+        ProcessedData scan_msgs;
+    };
     using PostProcessingFn = std::function<void(OutputType)>;
 
    public:
@@ -35,12 +39,16 @@ class LaserScanProcessor {
    private:
     void process(const ouster::LidarScan& lidar_scan, uint64_t,
                  const ros::Time& msg_ts) {
+        OutputType out;
         for (int i = 0; i < static_cast<int>(scan_msgs.size()); ++i) {
             *scan_msgs[i] = lidar_scan_to_laser_scan_msg(
                 lidar_scan, msg_ts, frame, ld_mode, ring_, i);
         }
+        out.scan_msgs = scan_msgs;
+        out.num_valid_columns =
+            (lidar_scan.measurement_id().array() != 0).count() + 1;
 
-        if (post_processing_fn) post_processing_fn(scan_msgs);
+        if (post_processing_fn) post_processing_fn(out);
     }
 
    public:
@@ -60,8 +68,8 @@ class LaserScanProcessor {
     std::string frame;
     sensor::lidar_mode ld_mode;
     uint16_t ring_;
-    OutputType scan_msgs;
+    ProcessedData scan_msgs;
     PostProcessingFn post_processing_fn;
 };
 
-}  // namespace ouster_ros
\ No newline at end of file
+}  // namespace ouster_ros
diff --git a/src/lidar_packet_handler.h b/src/lidar_packet_handler.h
index f05adcbf..9c683fe9 100644
--- a/src/lidar_packet_handler.h
+++ b/src/lidar_packet_handler.h
@@ -120,27 +120,11 @@ class LidarPacketHandler {
     static HandlerType create_handler(
         const ouster::sensor::sensor_info& info,
         const std::vector<LidarScanProcessor>& handlers,
-        const std::string& timestamp_mode, int64_t ptp_utc_tai_offset,
-        const int min_lidar_packets_per_cloud) {
+        const std::string& timestamp_mode, int64_t ptp_utc_tai_offset) {
         auto handler = std::make_shared<LidarPacketHandler>(
             info, handlers, timestamp_mode, ptp_utc_tai_offset);
-        return [handler, info,
-                min_lidar_packets_per_cloud](const uint8_t* lidar_buf) {
-            thread_local int num_packets_in_cloud = 0;
-            num_packets_in_cloud++;
-
+        return [handler](const uint8_t* lidar_buf) {
             if (handler->lidar_packet_accumlator(lidar_buf)) {
-                if (num_packets_in_cloud < min_lidar_packets_per_cloud) {
-                    ROS_WARN_STREAM_THROTTLE(
-                        0.1,
-                        "Incomplete cloud, dropping it. Got "
-                            << num_packets_in_cloud << ", expected "
-                            << min_lidar_packets_per_cloud << " lidar packets.");
-                    num_packets_in_cloud = 0;
-                    return;
-              }
-              num_packets_in_cloud = 0;
-
                 for (auto h : handler->lidar_scan_handlers) {
                     h(*handler->lidar_scan, handler->lidar_scan_estimated_ts,
                       handler->lidar_scan_estimated_msg_ts);
@@ -304,4 +288,4 @@ class LidarPacketHandler {
     LidarPacketAccumlator lidar_packet_accumlator;
 };
 
-}  // namespace ouster_ros
+}  // namespace ouster_ros
\ No newline at end of file
diff --git a/src/os_cloud_nodelet.cpp b/src/os_cloud_nodelet.cpp
index 472cff24..c673ca18 100644
--- a/src/os_cloud_nodelet.cpp
+++ b/src/os_cloud_nodelet.cpp
@@ -92,6 +92,7 @@ class OusterCloud : 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);
+        num_columns_required_ = pnh.param("num_columns_required", 0);
 
         auto& nh = getNodeHandle();
 
@@ -124,11 +125,18 @@ class OusterCloud : public nodelet::Nodelet {
                 PointCloudProcessorFactory::create_point_cloud_processor(point_type,
                     info, tf_bcast.point_cloud_frame_id(),
                     tf_bcast.apply_lidar_to_sensor_transform(),
-                    [this](PointCloudProcessor_OutputType msgs) {
-                        for (size_t i = 0; i < msgs.size(); ++i) {
-                            if (msgs[i]->header.stamp > last_msg_ts)
-                                last_msg_ts = msgs[i]->header.stamp;
-                            lidar_pubs[i].publish(*msgs[i]);
+                    [this](PointCloudProcessor_OutputType data) {
+                        if (data.num_valid_columns < num_columns_required_) {
+                            ROS_WARN_STREAM(
+                                "Incomplete cloud, not publishing. Got "
+                                << data.num_valid_columns << " columns, expected "
+                                << num_columns_required_ << ".");
+                            return;
+                        }
+                        for (size_t i = 0; i < data.pc_msgs.size(); ++i) {
+                            if (data.pc_msgs[i]->header.stamp > last_msg_ts)
+                                last_msg_ts = data.pc_msgs[i]->header.stamp;
+                            lidar_pubs[i].publish(*data.pc_msgs[i]);
                         }
                     }
                 )
@@ -162,22 +170,19 @@ class OusterCloud : public nodelet::Nodelet {
 
             processors.push_back(LaserScanProcessor::create(
                 info, tf_bcast.lidar_frame_id(), scan_ring,
-                [this](LaserScanProcessor::OutputType msgs) {
-                    for (size_t i = 0; i < msgs.size(); ++i) {
-                        if (msgs[i]->header.stamp > last_msg_ts)
-                            last_msg_ts = msgs[i]->header.stamp;
-                        scan_pubs[i].publish(*msgs[i]);
+                [this](LaserScanProcessor::OutputType data) {
+                    for (size_t i = 0; i < data.scan_msgs.size(); ++i) {
+                        if (data.scan_msgs[i]->header.stamp > last_msg_ts)
+                            last_msg_ts = data.scan_msgs[i]->header.stamp;
+                        scan_pubs[i].publish(*data.scan_msgs[i]);
                     }
                 }));
         }
 
-        const int min_lidar_packets_per_cloud =
-            pnh.param("min_lidar_packets_per_cloud", 0);
         if (impl::check_token(tokens, "PCL") || impl::check_token(tokens, "SCAN")) {
             lidar_packet_handler = LidarPacketHandler::create_handler(
                 info, processors, timestamp_mode,
-                static_cast<int64_t>(ptp_utc_tai_offset * 1e+9),
-                min_lidar_packets_per_cloud);
+                static_cast<int64_t>(ptp_utc_tai_offset * 1e+9));
             lidar_packet_sub = nh.subscribe<PacketMsg>(
                 "lidar_packets", 100, [this](const PacketMsg::ConstPtr msg) {
                     lidar_packet_handler(msg->buf.data());
@@ -200,6 +205,7 @@ class OusterCloud : public nodelet::Nodelet {
 
     ros::Timer timer_;
     ros::Time last_msg_ts;
+    int num_columns_required_;
 };
 
 }  // namespace ouster_ros
diff --git a/src/os_driver_nodelet.cpp b/src/os_driver_nodelet.cpp
index c4479153..8bc1e0ae 100644
--- a/src/os_driver_nodelet.cpp
+++ b/src/os_driver_nodelet.cpp
@@ -57,6 +57,7 @@ class OusterDriver : public OusterSensor {
 
         auto timestamp_mode = pnh.param("timestamp_mode", std::string{});
         double ptp_utc_tai_offset = pnh.param("ptp_utc_tai_offset", -37.0);
+        num_columns_required_ = pnh.param("num_columns_required", 0);
 
         auto& nh = getNodeHandle();
 
@@ -81,8 +82,16 @@ class OusterDriver : public OusterSensor {
             processors.push_back(
                 PointCloudProcessorFactory::create_point_cloud_processor(point_type, info,
                     tf_bcast.point_cloud_frame_id(), tf_bcast.apply_lidar_to_sensor_transform(),
-                    [this](PointCloudProcessor_OutputType msgs) {
-                        for (size_t i = 0; i < msgs.size(); ++i) lidar_pubs[i].publish(*msgs[i]);
+                    [this](PointCloudProcessor_OutputType data) {
+                        if (data.num_valid_columns < num_columns_required_) {
+                            ROS_WARN_STREAM(
+                                "Incomplete cloud, not publishing. Got "
+                                << data.num_valid_columns << " columns, expected "
+                                << num_columns_required_ << ".");
+                            return;
+                        }
+                        for (size_t i = 0; i < data.pc_msgs.size(); ++i)
+                            lidar_pubs[i].publish(*data.pc_msgs[i]);
                     }
                 )
             );
@@ -115,9 +124,11 @@ class OusterDriver : public OusterSensor {
 
             processors.push_back(LaserScanProcessor::create(
                 info, tf_bcast.lidar_frame_id(), scan_ring,
-                [this](LaserScanProcessor::OutputType msgs) {
-                    for (size_t i = 0; i < msgs.size(); ++i) {
-                        scan_pubs[i].publish(*msgs[i]);
+                [this](LaserScanProcessor::OutputType data) {
+                    if (data.num_valid_columns < num_columns_required_)
+                        return;
+                    for (size_t i = 0; i < data.scan_msgs.size(); ++i) {
+                        scan_pubs[i].publish(*data.scan_msgs[i]);
                     }
                 }));
         }
@@ -149,21 +160,21 @@ class OusterDriver : public OusterSensor {
 
             processors.push_back(ImageProcessor::create(
                 info, tf_bcast.point_cloud_frame_id(),
-                [this](ImageProcessor::OutputType msgs) {
-                    for (auto it = msgs.begin(); it != msgs.end(); ++it) {
+                [this](ImageProcessor::OutputType data) {
+                    if (data.num_valid_columns < num_columns_required_)
+                        return;
+                    for (auto it = data.image_msgs.begin();
+                            it != data.image_msgs.end(); ++it) {
                         image_pubs[it->first].publish(*it->second);
                     }
                 }));
         }
 
-        const int min_lidar_packets_per_cloud =
-            pnh.param("min_lidar_packets_per_cloud", 0);
         if (impl::check_token(tokens, "PCL") || impl::check_token(tokens, "SCAN") ||
             impl::check_token(tokens, "IMG"))
             lidar_packet_handler = LidarPacketHandler::create_handler(
                 info, processors, timestamp_mode,
-                static_cast<int64_t>(ptp_utc_tai_offset * 1e+9),
-                min_lidar_packets_per_cloud);
+                static_cast<int64_t>(ptp_utc_tai_offset * 1e+9));
     }
 
     virtual void on_lidar_packet_msg(const uint8_t* raw_lidar_packet) override {
@@ -180,6 +191,7 @@ class OusterDriver : public OusterSensor {
     std::vector<ros::Publisher> lidar_pubs;
     std::vector<ros::Publisher> scan_pubs;
     std::map<sensor::ChanField, ros::Publisher> image_pubs;
+    int num_columns_required_;
 
     OusterTransformsBroadcaster tf_bcast;
 
diff --git a/src/os_image_nodelet.cpp b/src/os_image_nodelet.cpp
index bbf46a73..dde76bb4 100644
--- a/src/os_image_nodelet.cpp
+++ b/src/os_image_nodelet.cpp
@@ -56,6 +56,8 @@ class OusterImage : public nodelet::Nodelet {
         auto& pnh = getPrivateNodeHandle();
         auto timestamp_mode = pnh.param("timestamp_mode", std::string{});
         double ptp_utc_tai_offset = pnh.param("ptp_utc_tai_offset", -37.0);
+        num_columns_required_ = pnh.param("num_columns_required", 0);
+
 
         const std::map<sensor::ChanField, std::string>
             channel_field_topic_map_1 {
@@ -87,19 +89,19 @@ class OusterImage : public nodelet::Nodelet {
         std::vector<LidarScanProcessor> processors {
             ImageProcessor::create(
                 info, "os_lidar", /*TODO: tf_bcast.point_cloud_frame_id()*/
-                [this](ImageProcessor::OutputType msgs) {
-                    for (auto it = msgs.begin(); it != msgs.end(); ++it) {
+                [this](ImageProcessor::OutputType data) {
+                    if (data.num_valid_columns < num_columns_required_)
+                        return;
+                    for (auto it = data.image_msgs.begin();
+                        it != data.image_msgs.end(); ++it) {
                         image_pubs[it->first].publish(*it->second);
                     }
                 })
         };
 
-        const int min_lidar_packets_per_cloud =
-            pnh.param("min_lidar_packets_per_cloud", 0);
         lidar_packet_handler = LidarPacketHandler::create_handler(
             info, processors, timestamp_mode,
-            static_cast<int64_t>(ptp_utc_tai_offset * 1e+9),
-            min_lidar_packets_per_cloud);
+            static_cast<int64_t>(ptp_utc_tai_offset * 1e+9));
         lidar_packet_sub = nh.subscribe<PacketMsg>(
                 "lidar_packets", 100,
                 [this](const PacketMsg::ConstPtr msg) {
@@ -110,6 +112,7 @@ class OusterImage : public nodelet::Nodelet {
    private:
     ros::Subscriber metadata_sub;
     sensor::sensor_info info;
+    int num_columns_required_;
 
     ros::Subscriber lidar_packet_sub;
     std::map<sensor::ChanField, ros::Publisher> image_pubs;
diff --git a/src/point_cloud_processor.h b/src/point_cloud_processor.h
index 593e8d7d..57688626 100644
--- a/src/point_cloud_processor.h
+++ b/src/point_cloud_processor.h
@@ -20,8 +20,12 @@
 namespace ouster_ros {
 
 // Moved out of PointCloudProcessor to avoid type templatization
-using PointCloudProcessor_OutputType =
+using ProcessedData =
     std::vector<std::shared_ptr<sensor_msgs::PointCloud2>>;
+struct PointCloudProcessor_OutputType {
+    int num_valid_columns;
+    ProcessedData pc_msgs;
+};
 using PointCloudProcessor_PostProcessingFn = std::function<void(PointCloudProcessor_OutputType)>;
 
 
@@ -75,6 +79,7 @@ class PointCloudProcessor {
 
     void process(const ouster::LidarScan& lidar_scan, uint64_t scan_ts,
                  const ros::Time& msg_ts) {
+        PointCloudProcessor_OutputType out;
         for (int i = 0; i < static_cast<int>(pc_msgs.size()); ++i) {
             auto range_channel = static_cast<sensor::ChanField>(sensor::ChanField::RANGE + i);
             auto range = lidar_scan.field<uint32_t>(range_channel);
@@ -87,8 +92,11 @@ class PointCloudProcessor {
             pc_msgs[i]->header.stamp = msg_ts;
             pc_msgs[i]->header.frame_id = frame;
         }
+        out.pc_msgs = pc_msgs;
+        out.num_valid_columns =
+            (lidar_scan.measurement_id().array() != 0).count() + 1;
 
-        if (post_processing_fn) post_processing_fn(pc_msgs);
+        if (post_processing_fn) post_processing_fn(out);
     }
 
    public:
@@ -118,9 +126,9 @@ class PointCloudProcessor {
     ouster::PointsF points;
     std::vector<int> pixel_shift_by_row;
     ouster_ros::Cloud<PointT> cloud;
-    PointCloudProcessor_OutputType pc_msgs;
+    ProcessedData pc_msgs;
     ScanToCloudFn scan_to_cloud_fn;
     PointCloudProcessor_PostProcessingFn post_processing_fn;
 };
 
-}  // namespace ouster_ros
\ No newline at end of file
+}  // namespace ouster_ros