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>

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 @@ 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;
     }
 

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()));
+    }
+}

photon-targeting/src/main/native/jni/CscoreExtras.cpp

@@ -0,0 +1,160 @@
+/*
+ * 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/>.
+ */
+
+#include <string>
+
+#include <opencv2/core/mat.hpp>
+#include <wpi/jni_util.h>
+
+#include "cscore_raw.h"
+#include "org_photonvision_jni_CscoreExtras.h"
+
+// from wpilib, licensed under the wpilib BSD license
+using namespace wpi::java;
+static JException videoEx;
+static const JExceptionInit exceptions[] = {
+    {"edu/wpi/first/cscore/VideoException", &videoEx}};
+static void ReportError(JNIEnv* env, CS_Status status) {
+  if (status == CS_OK) {
+    return;
+  }
+  std::string_view msg;
+  std::string msgBuf;
+  switch (status) {
+    case CS_PROPERTY_WRITE_FAILED:
+      msg = "property write failed";
+      break;
+    case CS_INVALID_HANDLE:
+      msg = "invalid handle";
+      break;
+    case CS_WRONG_HANDLE_SUBTYPE:
+      msg = "wrong handle subtype";
+      break;
+    case CS_INVALID_PROPERTY:
+      msg = "invalid property";
+      break;
+    case CS_WRONG_PROPERTY_TYPE:
+      msg = "wrong property type";
+      break;
+    case CS_READ_FAILED:
+      msg = "read failed";
+      break;
+    case CS_SOURCE_IS_DISCONNECTED:
+      msg = "source is disconnected";
+      break;
+    case CS_EMPTY_VALUE:
+      msg = "empty value";
+      break;
+    case CS_BAD_URL:
+      msg = "bad URL";
+      break;
+    case CS_TELEMETRY_NOT_ENABLED:
+      msg = "telemetry not enabled";
+      break;
+    default: {
+      msgBuf = fmt::format("unknown error code={}", status);
+      msg = msgBuf;
+      break;
+    }
+  }
+  videoEx.Throw(env, msg);
+}
+static inline bool CheckStatus(JNIEnv* env, CS_Status status) {
+  if (status != CS_OK) {
+    ReportError(env, status);
+  }
+  return status == CS_OK;
+}
+
+static inline int GetCVFormat(int wpiFormat) {
+  auto format = static_cast<WPI_PixelFormat>(wpiFormat);
+
+  switch (format) {
+    case WPI_PIXFMT_YUYV:
+    case WPI_PIXFMT_RGB565:
+    case WPI_PIXFMT_Y16:
+    case WPI_PIXFMT_UYVY:
+      return CV_8UC2;
+    case WPI_PIXFMT_BGR:
+      return CV_8UC3;
+    case WPI_PIXFMT_BGRA:
+      return CV_8UC4;
+    case WPI_PIXFMT_GRAY:
+    case WPI_PIXFMT_MJPEG:
+    case WPI_PIXFMT_UNKNOWN:
+    default:
+      return CV_8UC1;
+  }
+}
+
+#include <cstdio>
+
+extern "C" {
+
+/*
+ * Class:     org_photonvision_jni_CscoreExtras
+ * Method:    grabRawSinkFrameTimeoutLastTime
+ * Signature: (IJDJ)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_photonvision_jni_CscoreExtras_grabRawSinkFrameTimeoutLastTime
+  (JNIEnv* env, jclass, jint sink, jlong framePtr, jdouble timeout,
+   jlong lastFrameTimeout)
+{
+  auto* frame = reinterpret_cast<wpi::RawFrame*>(framePtr);
+  CS_Status status = 0;
+
+  // fill frame with a copy of the latest frame from the Source
+  auto rv = cs::GrabSinkFrameTimeoutLastTime(
+      static_cast<CS_Sink>(sink), *frame, timeout, lastFrameTimeout, &status);
+  if (!CheckStatus(env, status)) {
+    return 0;
+  }
+
+  return rv;
+}
+
+/*
+ * Class:     org_photonvision_jni_CscoreExtras
+ * Method:    wrapRawFrame
+ * Signature: (J)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_photonvision_jni_CscoreExtras_wrapRawFrame
+  (JNIEnv*, jclass, jlong framePtr)
+{
+  auto* frame = reinterpret_cast<wpi::RawFrame*>(framePtr);
+
+  return reinterpret_cast<jlong>(new cv::Mat(frame->height, frame->width,
+                                             GetCVFormat(frame->pixelFormat),
+                                             frame->data, frame->stride));
+}
+
+/*
+ * Class:     org_photonvision_jni_CscoreExtras
+ * Method:    getTimestampSourceNative
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL
+Java_org_photonvision_jni_CscoreExtras_getTimestampSourceNative
+  (JNIEnv*, jclass, jlong framePtr)
+{
+  auto* frame = reinterpret_cast<wpi::RawFrame*>(framePtr);
+  return frame->timestampSrc;
+}
+
+}  // extern "C"

photon-targeting/src/test/java/jni/CscoreExtrasTest.java

@@ -0,0 +1,107 @@
+/*
+ * 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 jni;
+
+import static org.junit.jupiter.api.Assertions.fail;
+import static org.junit.jupiter.api.Assumptions.assumeTrue;
+
+import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.cscore.CameraServerJNI;
+import edu.wpi.first.cscore.CvSink;
+import edu.wpi.first.cscore.CvSource;
+import edu.wpi.first.cscore.UsbCamera;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.util.PixelFormat;
+import edu.wpi.first.util.RawFrame;
+import java.io.IOException;
+import org.junit.jupiter.api.AfterAll;
+import org.junit.jupiter.api.BeforeAll;
+import org.opencv.core.Mat;
+import org.photonvision.jni.CscoreExtras;
+import org.photonvision.jni.PhotonTargetingJniLoader;
+import org.photonvision.jni.WpilibLoader;
+
+public class CscoreExtrasTest {
+    @BeforeAll
+    public static void load_wpilib() throws UnsatisfiedLinkError, IOException {
+        if (!WpilibLoader.loadLibraries()) {
+            fail();
+        }
+        if (!PhotonTargetingJniLoader.load()) {
+            fail();
+        }
+
+        HAL.initialize(1000, 0);
+    }
+
+    @AfterAll
+    public static void teardown() {
+        HAL.shutdown();
+    }
+
+    // Skip this test for now. This doesn't work in CI anyways.
+    // @Test
+    public void testCaptureImage() {
+        assumeTrue(CameraServerJNI.enumerateUsbCameras().length > 0);
+
+        UsbCamera camera = CameraServer.startAutomaticCapture(2);
+
+        camera.setVideoMode(PixelFormat.kMJPEG, 1280, 720, 30);
+        var cameraMode = camera.getVideoMode();
+
+        CvSink cvSink = CameraServer.getVideo(camera);
+
+        CvSource outputStream = CameraServer.putVideo("Detected", 640, 480);
+
+        long lastTime = 0;
+        for (long i = 0; i < 10000000; i++) {
+            var frame = new RawFrame();
+            frame.setInfo(
+                    cameraMode.width,
+                    cameraMode.height,
+                    // hard-coded 3 channel
+                    cameraMode.width * 3,
+                    PixelFormat.kBGR);
+            final double CSCORE_DEFAULT_FRAME_TIMEOUT = 1.0 / 4.0;
+            long time =
+                    CscoreExtras.grabRawSinkFrameTimeoutLastTime(
+                            cvSink.getHandle(), frame.getNativeObj(), CSCORE_DEFAULT_FRAME_TIMEOUT, lastTime);
+
+            if (time != 0) {
+                var mat = new Mat(CscoreExtras.wrapRawFrame(frame.getNativeObj()));
+
+                System.out.println(mat);
+                System.out.println(
+                        "Mat is " + mat.cols() + " x " + mat.rows() + " (cols x rows) with type " + mat.type());
+
+                outputStream.putFrame(mat);
+
+                mat.release();
+            } else {
+                System.err.println("Sink produced an error...");
+            }
+
+            lastTime = time;
+
+            frame.close();
+        }
+
+        cvSink.close();
+        camera.close();
+    }
+}