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