diff --git a/build.gradle b/build.gradle index 6e164f878f..e7bdac1575 100644 --- a/build.gradle +++ b/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' 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 bae31562e0..7ec8091a75 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 @@ -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 diff --git a/photon-core/src/main/java/org/photonvision/vision/opencv/CVMat.java b/photon-core/src/main/java/org/photonvision/vision/opencv/CVMat.java index 728d399158..2d254ce7f4 100644 --- a/photon-core/src/main/java/org/photonvision/vision/opencv/CVMat.java +++ b/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; diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CalculateFPSPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CalculateFPSPipe.java index 113da4d357..b840e66028 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CalculateFPSPipe.java +++ b/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 { 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; } diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle index 8c0bc49e32..6f97872b08 100644 --- a/photon-targeting/build.gradle +++ b/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") } } diff --git a/photon-targeting/src/main/java/org/photonvision/jni/CscoreExtras.java b/photon-targeting/src/main/java/org/photonvision/jni/CscoreExtras.java new file mode 100644 index 0000000000..ccce0e79b4 --- /dev/null +++ b/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 . + */ + +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. + * + *

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())); + } +} diff --git a/photon-targeting/src/main/native/jni/CscoreExtras.cpp b/photon-targeting/src/main/native/jni/CscoreExtras.cpp new file mode 100644 index 0000000000..9f6aa944d5 --- /dev/null +++ b/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 . + */ + +#include + +#include +#include + +#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(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 + +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(framePtr); + CS_Status status = 0; + + // fill frame with a copy of the latest frame from the Source + auto rv = cs::GrabSinkFrameTimeoutLastTime( + static_cast(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(framePtr); + + return reinterpret_cast(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(framePtr); + return frame->timestampSrc; +} + +} // extern "C" diff --git a/photon-targeting/src/test/java/jni/CscoreExtrasTest.java b/photon-targeting/src/test/java/jni/CscoreExtrasTest.java new file mode 100644 index 0000000000..1c5e8ea485 --- /dev/null +++ b/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 . + */ + +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(); + } +} diff --git a/photon-targeting/src/test/java/wpiutil_extras/FileLoggerTest.java b/photon-targeting/src/test/java/jni/FileLoggerTest.java similarity index 100% rename from photon-targeting/src/test/java/wpiutil_extras/FileLoggerTest.java rename to photon-targeting/src/test/java/jni/FileLoggerTest.java