Ingest wpilib!7609 and add turbo button (#1662)

Now that https://github.com/wpilibsuite/allwpilib/pull/7572 and
https://github.com/wpilibsuite/allwpilib/pull/7609 have been merged:
- Adds a magic hidden button to enable the new frame grabber behavior by
adding a boolean topic at `/photonvision/use_new_cscore_frametime`.
Toggle this to true to maybe increase FPS at the cost of latency
variability
- Bumps WPILib to ingest
https://github.com/wpilibsuite/allwpilib/pull/7609 , but doesn't
currently provide any user feedback about the time source. I don't think
that reporting this super matters?

---------

Co-authored-by: Jade <spacey-sooty@proton.me>

photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java

@@ -20,8 +20,14 @@ package org.photonvision.vision.frame.provider;
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.cscore.CvSink;
 import edu.wpi.first.cscore.UsbCamera;
+import edu.wpi.first.networktables.BooleanSubscriber;
+import edu.wpi.first.util.PixelFormat;
+import edu.wpi.first.util.RawFrame;
+import org.opencv.core.Mat;
+import org.photonvision.common.dataflow.networktables.NetworkTablesManager;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
+import org.photonvision.jni.CscoreExtras;
 import org.photonvision.vision.opencv.CVMat;
 import org.photonvision.vision.processes.VisionSourceSettables;
 
@@ -36,6 +42,11 @@ public class USBFrameProvider extends CpuImageProcessor {
 
     private Runnable connectedCallback;
 
+    private long lastTime = 0;
+
+    // subscribers are lightweight, and I'm lazy
+    private final BooleanSubscriber useNewBehaviorSub;
+
     @SuppressWarnings("SpellCheckingInspection")
     public USBFrameProvider(
             UsbCamera camera, VisionSourceSettables visionSettables, Runnable connectedCallback) {
@@ -47,6 +58,11 @@ public class USBFrameProvider extends CpuImageProcessor {
         this.cvSink.setEnabled(true);
 
         this.settables = visionSettables;
+
+        var useNewBehaviorTopic =
+                NetworkTablesManager.getInstance().kRootTable.getBooleanTopic("use_new_cscore_frametime");
+
+        useNewBehaviorSub = useNewBehaviorTopic.subscribe(false);
         this.connectedCallback = connectedCallback;
     }
 
@@ -62,28 +78,66 @@ public class USBFrameProvider extends CpuImageProcessor {
         return connected;
     }
 
+    final double CSCORE_DEFAULT_FRAME_TIMEOUT = 1.0 / 4.0;
+
     @Override
     public CapturedFrame getInputMat() {
         if (!cameraPropertiesCached && camera.isConnected()) {
             onCameraConnected();
         }
 
-        // We allocate memory so we don't fill a Mat in use by another thread (memory
-        // model is easier)
-        var mat = new CVMat();
-        // This is from wpi::Now, or WPIUtilJNI.now(). The epoch from grabFrame is uS
-        // since
-        // Hal::initialize was called. Timeout is in seconds
-        // TODO - under the hood, this incurs an extra copy. We should avoid this, if we
-        // can.
-        long captureTimeNs = cvSink.grabFrame(mat.getMat(), 1.0) * 1000;
+        if (!useNewBehaviorSub.get()) {
+            // We allocate memory so we don't fill a Mat in use by another thread (memory model is easier)
+            var mat = new CVMat();
+            // This is from wpi::Now, or WPIUtilJNI.now(). The epoch from grabFrame is uS since
+            // Hal::initialize was called
+            // TODO - under the hood, this incurs an extra copy. We should avoid this, if we
+            // can.
+            long captureTimeNs = cvSink.grabFrame(mat.getMat(), CSCORE_DEFAULT_FRAME_TIMEOUT) * 1000;
 
-        if (captureTimeNs == 0) {
-            var error = cvSink.getError();
-            logger.error("Error grabbing image: " + error);
+            if (captureTimeNs == 0) {
+                var error = cvSink.getError();
+                logger.error("Error grabbing image: " + error);
+            }
+
+            return new CapturedFrame(mat, settables.getFrameStaticProperties(), captureTimeNs);
+        } else {
+            // We allocate memory so we don't fill a Mat in use by another thread (memory model is easier)
+            // TODO - consider a frame pool
+            // TODO - getCurrentVideoMode is a JNI call for us, but profiling indicates it's fast
+            var cameraMode = settables.getCurrentVideoMode();
+            var frame = new RawFrame();
+            frame.setInfo(
+                    cameraMode.width,
+                    cameraMode.height,
+                    // hard-coded 3 channel
+                    cameraMode.width * 3,
+                    PixelFormat.kBGR);
+
+            // This is from wpi::Now, or WPIUtilJNI.now(). The epoch from grabFrame is uS since
+            // Hal::initialize was called
+            long captureTimeUs =
+                    CscoreExtras.grabRawSinkFrameTimeoutLastTime(
+                            cvSink.getHandle(), frame.getNativeObj(), CSCORE_DEFAULT_FRAME_TIMEOUT, lastTime);
+            lastTime = captureTimeUs;
+
+            CVMat ret;
+
+            if (captureTimeUs == 0) {
+                var error = cvSink.getError();
+                logger.error("Error grabbing image: " + error);
+
+                frame.close();
+                ret = new CVMat();
+            } else {
+                // No error! yay
+                var mat = new Mat(CscoreExtras.wrapRawFrame(frame.getNativeObj()));
+
+                ret = new CVMat(mat, frame);
+            }
+
+            return new CapturedFrame(ret, settables.getFrameStaticProperties(), captureTimeUs * 1000);
         }
-
-        return new CapturedFrame(mat, settables.getFrameStaticProperties(), captureTimeNs);
     }
 
     @Override

photon-core/src/main/java/org/photonvision/vision/opencv/CVMat.java

@@ -17,6 +17,7 @@
 
 package org.photonvision.vision.opencv;
 
+import edu.wpi.first.util.RawFrame;
 import java.util.HashMap;
 import org.opencv.core.Mat;
 import org.photonvision.common.logging.LogGroup;
@@ -31,11 +32,16 @@ public class CVMat implements Releasable {
     private static boolean shouldPrint;
 
     private final Mat mat;
+    private final RawFrame backingFrame;
 
     public CVMat() {
         this(new Mat());
     }
 
+    public CVMat(Mat mat) {
+        this(mat, null);
+    }
+
     public void copyFrom(CVMat srcMat) {
         copyFrom(srcMat.getMat());
     }
@@ -56,8 +62,10 @@ public class CVMat implements Releasable {
         return traceStr;
     }
 
-    public CVMat(Mat mat) {
+    public CVMat(Mat mat, RawFrame frame) {
         this.mat = mat;
+        this.backingFrame = frame;
+
         allMatCounter++;
         allMats.put(mat, allMatCounter);
 
@@ -69,6 +77,8 @@ public class CVMat implements Releasable {
 
     @Override
     public void release() {
+        if (this.backingFrame != null) this.backingFrame.close();
+
         // If this mat is empty, all we can do is return
         if (mat.empty()) return;
 

photon-core/src/main/java/org/photonvision/vision/pipe/impl/CalculateFPSPipe.java

@@ -18,24 +18,34 @@
 package org.photonvision.vision.pipe.impl;
 
 import edu.wpi.first.math.filter.LinearFilter;
-import org.apache.commons.lang3.time.StopWatch;
+import edu.wpi.first.wpilibj.Timer;
 import org.photonvision.vision.pipe.CVPipe;
 
 public class CalculateFPSPipe
         extends CVPipe<Void, Integer, CalculateFPSPipe.CalculateFPSPipeParams> {
     private final LinearFilter fpsFilter = LinearFilter.movingAverage(20);
-    StopWatch clock = new StopWatch();
+
+    // roll my own Timer, since this is so trivial
+    double lastTime = -1;
 
     @Override
     protected Integer process(Void in) {
-        if (!clock.isStarted()) {
-            clock.reset();
-            clock.start();
+        if (lastTime < 0) {
+            lastTime = Timer.getFPGATimestamp();
         }
-        clock.stop();
-        var fps = (int) fpsFilter.calculate(1000.0 / clock.getTime());
-        clock.reset();
-        clock.start();
+
+        var now = Timer.getFPGATimestamp();
+        var dtSeconds = now - lastTime;
+        lastTime = now;
+
+        // If < 1 uS between ticks, something is probably wrong
+        int fps;
+        if (dtSeconds < 1e-6) {
+            fps = 0;
+        } else {
+            fps = (int) fpsFilter.calculate(1 / dtSeconds);
+        }
+
         return fps;
     }