Param min packets in cloud

launch/common.launch

@@ -41,6 +41,12 @@
     xyzir
     }"/>
 
+  <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"
@@ -60,13 +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="~/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,6 +64,13 @@
     xyzir
     }"/>
 
+  <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"
       output="screen" required="true" args="manager"/>
@@ -91,6 +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="~/num_columns_required" value="$(arg num_columns_required)"/>
     </node>
   </group>
 

launch/record.launch

@@ -67,6 +67,13 @@
     xyzir
     }"/>
 
+  <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"
       output="screen" required="true" args="manager"/>
@@ -106,6 +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="num_columns_required" value="$(arg num_columns_required)"/>
   </include>
 
   <arg name="_use_bag_file_name" value="$(eval not (bag_file == ''))"/>

launch/replay.launch

@@ -47,6 +47,13 @@
     xyzir
     }"/>
 
+  <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"
       output="screen" required="true" args="manager"/>
@@ -81,6 +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="num_columns_required" value="$(arg num_columns_required)"/>
   </include>
 
   <arg name="_use_bag_file_name" value="$(eval not (bag_file == ''))"/>

launch/sensor.launch

@@ -72,6 +72,13 @@
     xyzir
     }"/>
 
+  <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"
       output="screen" required="true" args="manager"/>
@@ -111,6 +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="num_columns_required" value="$(arg num_columns_required)"/>
   </include>
 
 </launch>

launch/sensor_mtp.launch

@@ -76,6 +76,13 @@
     xyzir
     }"/>
 
+  <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"
       output="screen" required="true"
@@ -118,6 +125,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="num_columns_required" value="$(arg num_columns_required)"/>
+
   </include>
 
 </launch>

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/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,11 +170,11 @@ 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]);
                     }
                 }));
         }
@@ -197,6 +205,7 @@ class OusterCloud : public nodelet::Nodelet {
 
     ros::Timer timer_;
     ros::Time last_msg_ts;
+    int num_columns_required_;
 };
 
 }  // namespace ouster_ros

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,8 +160,11 @@ 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);
                     }
                 }));
@@ -177,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;
 

src/os_image_nodelet.cpp

@@ -50,12 +50,14 @@ 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();
         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,8 +89,11 @@ 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);
                     }
                 })
@@ -107,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;