Return num valid columns from processer

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,14 +66,15 @@
       <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>
 
   <group ns="$(arg ouster_ns)">
     <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>
 

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>
 

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 == ''))"/>

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 == ''))"/>

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>

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>
 

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,12 +197,12 @@ class ImageProcessor {
 
    private:
     std::string frame;
-    OutputType image_msgs;
+    ProcessedData image_msgs;
     PostProcessingFn post_processing_fn;
     sensor::sensor_info info_;
 
     viz::AutoExposure nearir_ae, signal_ae, reflec_ae;
     viz::BeamUniformityCorrector nearir_buc;
 };
 
-}  // namespace ouster_ros
+}  // namespace ouster_ros

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
+}  // namespace ouster_ros

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

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