From 2d8b1ec66ddda05fed4bb81b06187586cb21a714 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Thu, 21 Mar 2024 23:23:56 -0400
Subject: [PATCH] Properly handle empty frames from cscore (#1296)

---
 .../vision/frame/provider/USBFrameProvider.java       | 11 +++++++----
 .../photonvision/vision/processes/VisionRunner.java   |  6 ++++++
 2 files changed, 13 insertions(+), 4 deletions(-)

diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java
index 03459c197..5cc5e3cba 100644
--- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java
+++ b/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java
@@ -18,11 +18,14 @@
 package org.photonvision.vision.frame.provider;
 
 import edu.wpi.first.cscore.CvSink;
-import org.photonvision.common.util.math.MathUtils;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
 import org.photonvision.vision.opencv.CVMat;
 import org.photonvision.vision.processes.VisionSourceSettables;
 
 public class USBFrameProvider extends CpuImageProcessor {
+    private static final Logger logger = new Logger(USBFrameProvider.class, LogGroup.Camera);
+
     private final CvSink cvSink;
 
     @SuppressWarnings("SpellCheckingInspection")
@@ -43,9 +46,9 @@ public class USBFrameProvider extends CpuImageProcessor {
                 cvSink.grabFrame(mat.getMat())
                         * 1000; // Units are microseconds, epoch is the same as the Unix epoch
 
-        // Sometimes CSCore gives us a zero frametime.
-        if (time <= 1e-6) {
-            time = MathUtils.wpiNanoTime();
+        if (time == 0) {
+            var error = cvSink.getError();
+            logger.error("Error grabbing image: " + error);
         }
 
         return new CapturedFrame(mat, settables.getFrameStaticProperties(), time);
diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java
index 180135458..932e56b53 100644
--- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java
+++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java
@@ -92,6 +92,12 @@ public class VisionRunner {
             // Grab the new camera frame
             var frame = frameSupplier.get();
 
+            // Frame empty -- no point in trying to do anything more?
+            if (frame.processedImage.getMat().empty() && frame.colorImage.getMat().empty()) {
+                // give up without increasing loop count
+                continue;
+            }
+
             // There's no guarantee the processing type change will occur this tick, so pipelines should
             // check themselves
             try {