Update source code to use min_valid_columns

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:
@@ -27,6 +31,7 @@ class LaserScanProcessor {
                        PostProcessingFn func)
         : frame(frame_id),
           ld_mode(info.mode),
+          columns_per_packet(info.format.columns_per_packet),
           ring_(ring),
           scan_msgs(get_n_returns(info),
                     std::make_shared<sensor_msgs::LaserScan>()),
@@ -35,12 +40,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:
@@ -59,9 +68,10 @@ class LaserScanProcessor {
    private:
     std::string frame;
     sensor::lidar_mode ld_mode;
+    uint32_t columns_per_packet;
     uint16_t ring_;
-    OutputType scan_msgs;
+    ProcessedData scan_msgs;
     PostProcessingFn post_processing_fn;
 };
 
-}  // namespace ouster_ros
+}  // namespace ouster_ros

src/os_cloud_nodelet.cpp

@@ -120,15 +120,23 @@ class OusterCloud : public nodelet::Nodelet {
             }
 
             auto point_type = pnh.param("point_type", std::string{"original"});
+            auto min_valid_columns_in_scan = pnh.param("min_valid_columns_in_scan", 0);
             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) {
-                            if (msgs[i]->header.stamp > last_msg_ts)
-                                last_msg_ts = msgs[i]->header.stamp;
-                            lidar_pubs[i].publish(*msgs[i]);
+                    [this, min_valid_columns_in_scan](PointCloudProcessor_OutputType data) {
+                        if (data.num_valid_columns < min_valid_columns_in_scan) {
+                            ROS_WARN_STREAM(
+                                "Incomplete cloud, dropping it. Got "
+                                << data.num_valid_columns << " valid columns, expected "
+                                << min_valid_columns_in_scan << ".");
+                            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]);
                     }
                 }));
         }

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);
+        auto min_valid_columns_in_scan = pnh.param("min_valid_columns_in_scan", 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, min_valid_columns_in_scan](PointCloudProcessor_OutputType data) {
+                        if (data.num_valid_columns < min_valid_columns_in_scan) {
+                            ROS_WARN_STREAM(
+                                "Incomplete cloud, dropping it. Got "
+                                << data.num_valid_columns << " valid columns, expected "
+                                << min_valid_columns_in_scan << ".");
+                            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, min_valid_columns_in_scan](LaserScanProcessor::OutputType data) {
+                    if (data.num_valid_columns < min_valid_columns_in_scan)
+                        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, min_valid_columns_in_scan](ImageProcessor::OutputType data) {
+                    if (data.num_valid_columns < min_valid_columns_in_scan)
+                        return;
+                    for (auto it = data.image_msgs.begin();
+                            it != data.image_msgs.end(); ++it) {
                         image_pubs[it->first].publish(*it->second);
                     }
                 }));

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();
@@ -84,11 +84,15 @@ class OusterImage : public nodelet::Nodelet {
                 nh.advertise<sensor_msgs::Image>(it->second, 100);
         }
 
+        auto min_valid_columns_in_scan = pnh.param("min_valid_columns_in_scan", 0);
         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, min_valid_columns_in_scan](ImageProcessor::OutputType data) {
+                    if (data.num_valid_columns < min_valid_columns_in_scan)
+                        return;
+                    for (auto it = data.image_msgs.begin();
+                        it != data.image_msgs.end(); ++it) {
                         image_pubs[it->first].publish(*it->second);
                     }
                 })

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)>;
 
 
@@ -42,6 +46,7 @@ class PointCloudProcessor {
                         PointCloudProcessor_PostProcessingFn post_processing_fn_)
         : frame(frame_id),
           pixel_shift_by_row(info.format.pixel_shift_by_row),
+          columns_per_packet(info.format.columns_per_packet),
           cloud{info.format.columns_per_frame, info.format.pixels_per_column},
           pc_msgs(get_n_returns(info)),
           scan_to_cloud_fn(scan_to_cloud_fn_),
@@ -75,6 +80,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 +93,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:
@@ -117,10 +126,11 @@ class PointCloudProcessor {
     ouster::PointsF lut_offset;
     ouster::PointsF points;
     std::vector<int> pixel_shift_by_row;
+    uint32_t columns_per_packet;
     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
+}  // namespace ouster_ros