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

Now that wpilibsuite/allwpilib#7572 and
wpilibsuite/allwpilib#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
wpilibsuite/allwpilib#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 <[email protected]>

build.gradle

@@ -90,16 +90,6 @@ spotless {
         trimTrailingWhitespace()
         endWithNewline()
     }
-    format 'xml', {
-        target fileTree('.') {
-            include '**/*.xml'
-            exclude '**/build/**', '**/build-*/**', "**/.idea/**"
-        }
-        eclipseWtp('xml')
-        trimTrailingWhitespace()
-        indentWithSpaces(2)
-        endWithNewline()
-    }
     format 'misc', {
         target fileTree('.') {
             include '**/*.md', '**/.gitignore'

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

@@ -20,8 +20,14 @@
 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 USBFrameProvider(
         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 boolean checkCameraConnected() {
         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 (captureTimeNs == 0) {
-            var error = cvSink.getError();
-            logger.error("Error grabbing image: " + error);
+        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);
+            }
+
+            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 @@ private StringBuilder getStackTraceBuilder() {
         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 CVMat(Mat mat) {
 
     @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;
     }
 

photon-targeting/build.gradle

@@ -88,9 +88,11 @@ model {
             }
 
             nativeUtils.useRequiredLibrary(it, "wpiutil_shared")
+            nativeUtils.useRequiredLibrary(it, "wpimath_shared")
             nativeUtils.useRequiredLibrary(it, "wpinet_shared")
             nativeUtils.useRequiredLibrary(it, "ntcore_shared")
-            nativeUtils.useRequiredLibrary(it, "wpimath_shared")
+            nativeUtils.useRequiredLibrary(it, "cscore_shared")
+            nativeUtils.useRequiredLibrary(it, "opencv_shared")
         }
 
         all {
@@ -153,6 +155,8 @@ model {
             nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared")
             nativeUtils.useRequiredLibrary(it, "googletest_static")
             nativeUtils.useRequiredLibrary(it, "apriltag_shared")
+            nativeUtils.useRequiredLibrary(it, "cscore_shared")
+            nativeUtils.useRequiredLibrary(it, "opencv_shared")
         }
     }
 

photon-targeting/src/main/java/org/photonvision/jni/CscoreExtras.java

@@ -0,0 +1,54 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.jni;
+
+import edu.wpi.first.util.RawFrame;
+import edu.wpi.first.util.TimestampSource;
+
+public class CscoreExtras {
+    /**
+     * Fill {@param framePtr} with the latest image from the source this sink is connected to.
+     *
+     * <p>If lastFrameTime is provided and non-zero, the sink will fill image with the first frame
+     * from the source that is not equal to lastFrameTime. If lastFrameTime is zero, the time of the
+     * current frame owned by the CvSource is used, and this function will block until the connected
+     * CvSource provides a new frame.
+     *
+     * @param sink Sink handle.
+     * @param framePtr Pointer to a wpi::RawFrame.
+     * @param timeout Timeout in seconds.
+     * @param lastFrameTime Timestamp of the last frame - used to compare new frames against.
+     * @return Frame time, in uS, of the incoming frame.
+     */
+    public static native long grabRawSinkFrameTimeoutLastTime(
+            int sink, long framePtr, double timeout, long lastFrameTime);
+
+    /**
+     * Wrap the data owned by a RawFrame in a cv::Mat
+     *
+     * @param rawFramePtr
+     * @return pointer to a cv::Mat
+     */
+    public static native long wrapRawFrame(long rawFramePtr);
+
+    private static native int getTimestampSourceNative(long rawFramePtr);
+
+    public static TimestampSource getTimestampSource(RawFrame frame) {
+        return TimestampSource.getFromInt(getTimestampSourceNative(frame.getNativeObj()));
+    }
+}