Skip to content

Commit 83c124f

Browse files
mcm001spacey-sooty
mcm001
and
spacey-sooty
authored
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]>
1 parent 05348f3 commit 83c124f

File tree

9 files changed

+425
-36
lines changed

9 files changed

+425
-36
lines changed

‎build.gradle

Lines changed: 0 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -90,16 +90,6 @@ spotless {
9090
trimTrailingWhitespace()
9191
endWithNewline()
9292
}
93-
format 'xml', {
94-
target fileTree('.') {
95-
include '**/*.xml'
96-
exclude '**/build/**', '**/build-*/**', "**/.idea/**"
97-
}
98-
eclipseWtp('xml')
99-
trimTrailingWhitespace()
100-
indentWithSpaces(2)
101-
endWithNewline()
102-
}
10393
format 'misc', {
10494
target fileTree('.') {
10595
include '**/*.md', '**/.gitignore'

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

Lines changed: 69 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -20,8 +20,14 @@
2020
import edu.wpi.first.cameraserver.CameraServer;
2121
import edu.wpi.first.cscore.CvSink;
2222
import edu.wpi.first.cscore.UsbCamera;
23+
import edu.wpi.first.networktables.BooleanSubscriber;
24+
import edu.wpi.first.util.PixelFormat;
25+
import edu.wpi.first.util.RawFrame;
26+
import org.opencv.core.Mat;
27+
import org.photonvision.common.dataflow.networktables.NetworkTablesManager;
2328
import org.photonvision.common.logging.LogGroup;
2429
import org.photonvision.common.logging.Logger;
30+
import org.photonvision.jni.CscoreExtras;
2531
import org.photonvision.vision.opencv.CVMat;
2632
import org.photonvision.vision.processes.VisionSourceSettables;
2733

@@ -36,6 +42,11 @@ public class USBFrameProvider extends CpuImageProcessor {
3642

3743
private Runnable connectedCallback;
3844

45+
private long lastTime = 0;
46+
47+
// subscribers are lightweight, and I'm lazy
48+
private final BooleanSubscriber useNewBehaviorSub;
49+
3950
@SuppressWarnings("SpellCheckingInspection")
4051
public USBFrameProvider(
4152
UsbCamera camera, VisionSourceSettables visionSettables, Runnable connectedCallback) {
@@ -47,6 +58,11 @@ public USBFrameProvider(
4758
this.cvSink.setEnabled(true);
4859

4960
this.settables = visionSettables;
61+
62+
var useNewBehaviorTopic =
63+
NetworkTablesManager.getInstance().kRootTable.getBooleanTopic("use_new_cscore_frametime");
64+
65+
useNewBehaviorSub = useNewBehaviorTopic.subscribe(false);
5066
this.connectedCallback = connectedCallback;
5167
}
5268

@@ -62,28 +78,66 @@ public boolean checkCameraConnected() {
6278
return connected;
6379
}
6480

81+
final double CSCORE_DEFAULT_FRAME_TIMEOUT = 1.0 / 4.0;
82+
6583
@Override
6684
public CapturedFrame getInputMat() {
6785
if (!cameraPropertiesCached && camera.isConnected()) {
6886
onCameraConnected();
6987
}
7088

71-
// We allocate memory so we don't fill a Mat in use by another thread (memory
72-
// model is easier)
73-
var mat = new CVMat();
74-
// This is from wpi::Now, or WPIUtilJNI.now(). The epoch from grabFrame is uS
75-
// since
76-
// Hal::initialize was called. Timeout is in seconds
77-
// TODO - under the hood, this incurs an extra copy. We should avoid this, if we
78-
// can.
79-
long captureTimeNs = cvSink.grabFrame(mat.getMat(), 1.0) * 1000;
80-
81-
if (captureTimeNs == 0) {
82-
var error = cvSink.getError();
83-
logger.error("Error grabbing image: " + error);
89+
if (!useNewBehaviorSub.get()) {
90+
// We allocate memory so we don't fill a Mat in use by another thread (memory model is easier)
91+
var mat = new CVMat();
92+
// This is from wpi::Now, or WPIUtilJNI.now(). The epoch from grabFrame is uS since
93+
// Hal::initialize was called
94+
// TODO - under the hood, this incurs an extra copy. We should avoid this, if we
95+
// can.
96+
long captureTimeNs = cvSink.grabFrame(mat.getMat(), CSCORE_DEFAULT_FRAME_TIMEOUT) * 1000;
97+
98+
if (captureTimeNs == 0) {
99+
var error = cvSink.getError();
100+
logger.error("Error grabbing image: " + error);
101+
}
102+
103+
return new CapturedFrame(mat, settables.getFrameStaticProperties(), captureTimeNs);
104+
} else {
105+
// We allocate memory so we don't fill a Mat in use by another thread (memory model is easier)
106+
// TODO - consider a frame pool
107+
// TODO - getCurrentVideoMode is a JNI call for us, but profiling indicates it's fast
108+
var cameraMode = settables.getCurrentVideoMode();
109+
var frame = new RawFrame();
110+
frame.setInfo(
111+
cameraMode.width,
112+
cameraMode.height,
113+
// hard-coded 3 channel
114+
cameraMode.width * 3,
115+
PixelFormat.kBGR);
116+
117+
// This is from wpi::Now, or WPIUtilJNI.now(). The epoch from grabFrame is uS since
118+
// Hal::initialize was called
119+
long captureTimeUs =
120+
CscoreExtras.grabRawSinkFrameTimeoutLastTime(
121+
cvSink.getHandle(), frame.getNativeObj(), CSCORE_DEFAULT_FRAME_TIMEOUT, lastTime);
122+
lastTime = captureTimeUs;
123+
124+
CVMat ret;
125+
126+
if (captureTimeUs == 0) {
127+
var error = cvSink.getError();
128+
logger.error("Error grabbing image: " + error);
129+
130+
frame.close();
131+
ret = new CVMat();
132+
} else {
133+
// No error! yay
134+
var mat = new Mat(CscoreExtras.wrapRawFrame(frame.getNativeObj()));
135+
136+
ret = new CVMat(mat, frame);
137+
}
138+
139+
return new CapturedFrame(ret, settables.getFrameStaticProperties(), captureTimeUs * 1000);
84140
}
85-
86-
return new CapturedFrame(mat, settables.getFrameStaticProperties(), captureTimeNs);
87141
}
88142

89143
@Override

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

Lines changed: 11 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717

1818
package org.photonvision.vision.opencv;
1919

20+
import edu.wpi.first.util.RawFrame;
2021
import java.util.HashMap;
2122
import org.opencv.core.Mat;
2223
import org.photonvision.common.logging.LogGroup;
@@ -31,11 +32,16 @@ public class CVMat implements Releasable {
3132
private static boolean shouldPrint;
3233

3334
private final Mat mat;
35+
private final RawFrame backingFrame;
3436

3537
public CVMat() {
3638
this(new Mat());
3739
}
3840

41+
public CVMat(Mat mat) {
42+
this(mat, null);
43+
}
44+
3945
public void copyFrom(CVMat srcMat) {
4046
copyFrom(srcMat.getMat());
4147
}
@@ -56,8 +62,10 @@ private StringBuilder getStackTraceBuilder() {
5662
return traceStr;
5763
}
5864

59-
public CVMat(Mat mat) {
65+
public CVMat(Mat mat, RawFrame frame) {
6066
this.mat = mat;
67+
this.backingFrame = frame;
68+
6169
allMatCounter++;
6270
allMats.put(mat, allMatCounter);
6371

@@ -69,6 +77,8 @@ public CVMat(Mat mat) {
6977

7078
@Override
7179
public void release() {
80+
if (this.backingFrame != null) this.backingFrame.close();
81+
7282
// If this mat is empty, all we can do is return
7383
if (mat.empty()) return;
7484

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

Lines changed: 19 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -18,24 +18,34 @@
1818
package org.photonvision.vision.pipe.impl;
1919

2020
import edu.wpi.first.math.filter.LinearFilter;
21-
import org.apache.commons.lang3.time.StopWatch;
21+
import edu.wpi.first.wpilibj.Timer;
2222
import org.photonvision.vision.pipe.CVPipe;
2323

2424
public class CalculateFPSPipe
2525
extends CVPipe<Void, Integer, CalculateFPSPipe.CalculateFPSPipeParams> {
2626
private final LinearFilter fpsFilter = LinearFilter.movingAverage(20);
27-
StopWatch clock = new StopWatch();
27+
28+
// roll my own Timer, since this is so trivial
29+
double lastTime = -1;
2830

2931
@Override
3032
protected Integer process(Void in) {
31-
if (!clock.isStarted()) {
32-
clock.reset();
33-
clock.start();
33+
if (lastTime < 0) {
34+
lastTime = Timer.getFPGATimestamp();
3435
}
35-
clock.stop();
36-
var fps = (int) fpsFilter.calculate(1000.0 / clock.getTime());
37-
clock.reset();
38-
clock.start();
36+
37+
var now = Timer.getFPGATimestamp();
38+
var dtSeconds = now - lastTime;
39+
lastTime = now;
40+
41+
// If < 1 uS between ticks, something is probably wrong
42+
int fps;
43+
if (dtSeconds < 1e-6) {
44+
fps = 0;
45+
} else {
46+
fps = (int) fpsFilter.calculate(1 / dtSeconds);
47+
}
48+
3949
return fps;
4050
}
4151

‎photon-targeting/build.gradle

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -88,9 +88,11 @@ model {
8888
}
8989

9090
nativeUtils.useRequiredLibrary(it, "wpiutil_shared")
91+
nativeUtils.useRequiredLibrary(it, "wpimath_shared")
9192
nativeUtils.useRequiredLibrary(it, "wpinet_shared")
9293
nativeUtils.useRequiredLibrary(it, "ntcore_shared")
93-
nativeUtils.useRequiredLibrary(it, "wpimath_shared")
94+
nativeUtils.useRequiredLibrary(it, "cscore_shared")
95+
nativeUtils.useRequiredLibrary(it, "opencv_shared")
9496
}
9597

9698
all {
@@ -153,6 +155,8 @@ model {
153155
nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared")
154156
nativeUtils.useRequiredLibrary(it, "googletest_static")
155157
nativeUtils.useRequiredLibrary(it, "apriltag_shared")
158+
nativeUtils.useRequiredLibrary(it, "cscore_shared")
159+
nativeUtils.useRequiredLibrary(it, "opencv_shared")
156160
}
157161
}
158162

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

Lines changed: 54 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,54 @@
1+
/*
2+
* Copyright (C) Photon Vision.
3+
*
4+
* This program is free software: you can redistribute it and/or modify
5+
* it under the terms of the GNU General Public License as published by
6+
* the Free Software Foundation, either version 3 of the License, or
7+
* (at your option) any later version.
8+
*
9+
* This program is distributed in the hope that it will be useful,
10+
* but WITHOUT ANY WARRANTY; without even the implied warranty of
11+
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12+
* GNU General Public License for more details.
13+
*
14+
* You should have received a copy of the GNU General Public License
15+
* along with this program. If not, see <https://www.gnu.org/licenses/>.
16+
*/
17+
18+
package org.photonvision.jni;
19+
20+
import edu.wpi.first.util.RawFrame;
21+
import edu.wpi.first.util.TimestampSource;
22+
23+
public class CscoreExtras {
24+
/**
25+
* Fill {@param framePtr} with the latest image from the source this sink is connected to.
26+
*
27+
* <p>If lastFrameTime is provided and non-zero, the sink will fill image with the first frame
28+
* from the source that is not equal to lastFrameTime. If lastFrameTime is zero, the time of the
29+
* current frame owned by the CvSource is used, and this function will block until the connected
30+
* CvSource provides a new frame.
31+
*
32+
* @param sink Sink handle.
33+
* @param framePtr Pointer to a wpi::RawFrame.
34+
* @param timeout Timeout in seconds.
35+
* @param lastFrameTime Timestamp of the last frame - used to compare new frames against.
36+
* @return Frame time, in uS, of the incoming frame.
37+
*/
38+
public static native long grabRawSinkFrameTimeoutLastTime(
39+
int sink, long framePtr, double timeout, long lastFrameTime);
40+
41+
/**
42+
* Wrap the data owned by a RawFrame in a cv::Mat
43+
*
44+
* @param rawFramePtr
45+
* @return pointer to a cv::Mat
46+
*/
47+
public static native long wrapRawFrame(long rawFramePtr);
48+
49+
private static native int getTimestampSourceNative(long rawFramePtr);
50+
51+
public static TimestampSource getTimestampSource(RawFrame frame) {
52+
return TimestampSource.getFromInt(getTimestampSourceNative(frame.getNativeObj()));
53+
}
54+
}

0 commit comments

Comments
 (0)