mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-20 00:51:41 +00:00
UI Redesign (#22)
* Rework UI into a new, responsive layout * Send two streams (only one is currently downscaled)
This commit is contained in:
committed by
GitHub
parent
aed92e7132
commit
8b46ad1cab
@@ -93,6 +93,7 @@ public class PhotonConfiguration {
|
||||
public int currentPipelineIndex;
|
||||
public List<String> pipelineNicknames;
|
||||
public HashMap<Integer, HashMap<String, Object>> videoFormatList;
|
||||
public int streamPort;
|
||||
public int outputStreamPort;
|
||||
public int inputStreamPort;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -50,7 +50,7 @@ public class MJPGFrameConsumer implements FrameConsumer {
|
||||
|
||||
@Override
|
||||
public void accept(Frame frame) {
|
||||
if (!frame.image.getMat().empty()) {
|
||||
if (frame != null && !frame.image.getMat().empty()) {
|
||||
if (divisor != FrameDivisor.NONE) {
|
||||
var tempMat = new Mat();
|
||||
Imgproc.resize(
|
||||
|
||||
@@ -20,6 +20,7 @@ package org.photonvision.vision.pipeline;
|
||||
import java.util.List;
|
||||
import org.apache.commons.lang3.tuple.Pair;
|
||||
import org.opencv.core.Mat;
|
||||
import org.opencv.imgproc.Imgproc;
|
||||
import org.photonvision.common.util.math.MathUtils;
|
||||
import org.photonvision.vision.frame.Frame;
|
||||
import org.photonvision.vision.frame.FrameStaticProperties;
|
||||
@@ -92,10 +93,6 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
|
||||
new HSVPipe.HSVParams(settings.hsvHue, settings.hsvSaturation, settings.hsvValue);
|
||||
hsvPipe.setParams(hsvParams);
|
||||
|
||||
OutputMatPipe.OutputMatParams outputMatParams =
|
||||
new OutputMatPipe.OutputMatParams(settings.outputShowThresholded);
|
||||
outputMatPipe.setParams(outputMatParams);
|
||||
|
||||
FindContoursPipe.FindContoursParams findContoursParams =
|
||||
new FindContoursPipe.FindContoursParams();
|
||||
findContoursPipe.setParams(findContoursParams);
|
||||
@@ -161,6 +158,7 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
|
||||
solvePNPPipe.setParams(solvePNPParams);
|
||||
}
|
||||
|
||||
@SuppressWarnings("DuplicatedCode")
|
||||
@Override
|
||||
public CVPipelineResult process(Frame frame, ReflectivePipelineSettings settings) {
|
||||
setPipeParams(frame.frameStaticProperties, settings);
|
||||
@@ -180,12 +178,10 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
|
||||
sumPipeNanosElapsed += hsvPipeResult.nanosElapsed;
|
||||
|
||||
// mat leak fix attempt
|
||||
// the first is the raw input mat, the second is the hsvpipe result
|
||||
outputMats.first = rawInputMat;
|
||||
outputMats.second = hsvPipeResult.result;
|
||||
|
||||
CVPipeResult<Mat> outputMatResult = outputMatPipe.apply(outputMats);
|
||||
sumPipeNanosElapsed += outputMatResult.nanosElapsed;
|
||||
|
||||
CVPipeResult<List<Contour>> findContoursResult = findContoursPipe.apply(hsvPipeResult.result);
|
||||
sumPipeNanosElapsed += findContoursResult.nanosElapsed;
|
||||
|
||||
@@ -224,30 +220,54 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
|
||||
targetList = collect2dTargetsResult;
|
||||
}
|
||||
|
||||
CVPipeResult<Mat> result;
|
||||
CVPipeResult<Mat> drawOnInputResult, drawOnOutputResult;
|
||||
|
||||
CVPipeResult<Mat> draw2dCrosshairResult =
|
||||
draw2dCrosshairPipe.apply(Pair.of(outputMatResult.result, targetList.result));
|
||||
sumPipeNanosElapsed += draw2dCrosshairResult.nanosElapsed;
|
||||
// the first is the raw input mat, the second is the hsvpipe result
|
||||
// Draw on input
|
||||
|
||||
CVPipeResult<Mat> draw2dContoursResult =
|
||||
CVPipeResult<Mat> draw2dCrosshairResultOnInput =
|
||||
draw2dCrosshairPipe.apply(Pair.of(outputMats.first, targetList.result));
|
||||
sumPipeNanosElapsed += draw2dCrosshairResultOnInput.nanosElapsed;
|
||||
|
||||
CVPipeResult<Mat> draw2dContoursResultOnInput =
|
||||
draw2DTargetsPipe.apply(
|
||||
Pair.of(draw2dCrosshairResult.result, collect2dTargetsResult.result));
|
||||
sumPipeNanosElapsed += draw2dContoursResult.nanosElapsed;
|
||||
Pair.of(draw2dCrosshairResultOnInput.result, collect2dTargetsResult.result));
|
||||
sumPipeNanosElapsed += draw2dCrosshairResultOnInput.nanosElapsed;
|
||||
|
||||
if (settings.solvePNPEnabled) {
|
||||
result =
|
||||
drawOnInputResult =
|
||||
draw3dTargetsPipe.apply(
|
||||
Pair.of(draw2dCrosshairResult.result, collect2dTargetsResult.result));
|
||||
sumPipeNanosElapsed += result.nanosElapsed;
|
||||
Pair.of(draw2dContoursResultOnInput.result, collect2dTargetsResult.result));
|
||||
sumPipeNanosElapsed += drawOnInputResult.nanosElapsed;
|
||||
} else {
|
||||
result = draw2dContoursResult;
|
||||
drawOnInputResult = draw2dContoursResultOnInput;
|
||||
}
|
||||
|
||||
// Draw on output
|
||||
|
||||
Imgproc.cvtColor(outputMats.second, outputMats.second, Imgproc.COLOR_GRAY2BGR, 3);
|
||||
CVPipeResult<Mat> draw2dCrosshairResultOnOutput =
|
||||
draw2dCrosshairPipe.apply(Pair.of(outputMats.second, targetList.result));
|
||||
sumPipeNanosElapsed += draw2dCrosshairResultOnOutput.nanosElapsed;
|
||||
|
||||
CVPipeResult<Mat> draw2dContoursResultOnOutput =
|
||||
draw2DTargetsPipe.apply(
|
||||
Pair.of(draw2dCrosshairResultOnOutput.result, collect2dTargetsResult.result));
|
||||
sumPipeNanosElapsed += draw2dContoursResultOnOutput.nanosElapsed;
|
||||
|
||||
if (settings.solvePNPEnabled) {
|
||||
drawOnOutputResult =
|
||||
draw3dTargetsPipe.apply(
|
||||
Pair.of(draw2dContoursResultOnOutput.result, collect2dTargetsResult.result));
|
||||
sumPipeNanosElapsed += drawOnOutputResult.nanosElapsed;
|
||||
} else {
|
||||
drawOnOutputResult = draw2dContoursResultOnOutput;
|
||||
}
|
||||
|
||||
// TODO: Implement all the things
|
||||
return new CVPipelineResult(
|
||||
MathUtils.nanosToMillis(sumPipeNanosElapsed),
|
||||
collect2dTargetsResult.result,
|
||||
new Frame(new CVMat(result.result), frame.frameStaticProperties));
|
||||
new Frame(new CVMat(outputMats.second), frame.frameStaticProperties),
|
||||
new Frame(new CVMat(outputMats.first), frame.frameStaticProperties));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -27,12 +27,19 @@ public class CVPipelineResult implements Releasable {
|
||||
public final double processingMillis;
|
||||
public final List<TrackedTarget> targets;
|
||||
public final Frame outputFrame;
|
||||
public final Frame inputFrame;
|
||||
|
||||
public CVPipelineResult(double processingMillis, List<TrackedTarget> targets, Frame outputFrame) {
|
||||
public CVPipelineResult(
|
||||
double processingMillis, List<TrackedTarget> targets, Frame outputFrame, Frame inputFrame) {
|
||||
this.processingMillis = processingMillis;
|
||||
this.targets = targets;
|
||||
|
||||
this.outputFrame = Frame.copyFrom(outputFrame);
|
||||
this.inputFrame = inputFrame != null ? Frame.copyFrom(inputFrame) : null;
|
||||
}
|
||||
|
||||
public CVPipelineResult(double processingMillis, List<TrackedTarget> targets, Frame outputFrame) {
|
||||
this(processingMillis, targets, outputFrame, null);
|
||||
}
|
||||
|
||||
public boolean hasTargets() {
|
||||
@@ -44,6 +51,7 @@ public class CVPipelineResult implements Releasable {
|
||||
tt.release();
|
||||
}
|
||||
outputFrame.release();
|
||||
if (inputFrame != null) inputFrame.release();
|
||||
}
|
||||
|
||||
public double getLatencyMillis() {
|
||||
|
||||
@@ -60,12 +60,13 @@ public class VisionModule {
|
||||
private final LinkedList<FrameConsumer> frameConsumers = new LinkedList<>();
|
||||
private final NTDataPublisher ntConsumer;
|
||||
private final int moduleIndex;
|
||||
private final MJPGFrameConsumer uiStreamer;
|
||||
|
||||
private long lastUIResultUpdateTime = 0;
|
||||
private long lastRunTime = 0;
|
||||
private MedianFilter fpsAverager = new MedianFilter(10);
|
||||
|
||||
private MJPGFrameConsumer dashboardStreamer;
|
||||
private MJPGFrameConsumer dashboardOutputStreamer;
|
||||
private MJPGFrameConsumer dashboardInputStreamer;
|
||||
|
||||
public VisionModule(PipelineManager pipelineManager, VisionSource visionSource, int index) {
|
||||
logger =
|
||||
@@ -84,11 +85,20 @@ public class VisionModule {
|
||||
|
||||
DataChangeService.getInstance().addSubscriber(new VisionSettingChangeSubscriber());
|
||||
|
||||
dashboardStreamer =
|
||||
new MJPGFrameConsumer(visionSource.getSettables().getConfiguration().uniqueName);
|
||||
uiStreamer = new MJPGFrameConsumer(visionSource.getSettables().getConfiguration().nickname);
|
||||
addFrameConsumer(dashboardStreamer);
|
||||
addFrameConsumer(uiStreamer);
|
||||
dashboardOutputStreamer =
|
||||
new MJPGFrameConsumer(
|
||||
visionSource.getSettables().getConfiguration().uniqueName + "-output");
|
||||
dashboardInputStreamer =
|
||||
new MJPGFrameConsumer(visionSource.getSettables().getConfiguration().uniqueName + "-input");
|
||||
|
||||
addResultConsumer(
|
||||
result -> {
|
||||
dashboardInputStreamer.accept(result.inputFrame);
|
||||
});
|
||||
addResultConsumer(
|
||||
result -> {
|
||||
dashboardOutputStreamer.accept(result.outputFrame);
|
||||
});
|
||||
|
||||
ntConsumer =
|
||||
new NTDataPublisher(
|
||||
@@ -140,7 +150,7 @@ public class VisionModule {
|
||||
|
||||
setPipeline(visionSource.getSettables().getConfiguration().currentPipelineIndex);
|
||||
|
||||
dashboardStreamer.setFrameDivisor(
|
||||
dashboardOutputStreamer.setFrameDivisor(
|
||||
pipelineManager.getCurrentPipelineSettings().outputFrameDivisor);
|
||||
}
|
||||
|
||||
@@ -215,6 +225,11 @@ public class VisionModule {
|
||||
setPipeline(index);
|
||||
saveAndBroadcast();
|
||||
return;
|
||||
case "selectedOutputs":
|
||||
// 0 indicates normal, 1 indicates thresholded
|
||||
var outputs = (ArrayList<Integer>) newPropValue;
|
||||
// TODO
|
||||
return;
|
||||
}
|
||||
|
||||
// special case for camera settables
|
||||
@@ -265,7 +280,7 @@ public class VisionModule {
|
||||
|
||||
// special case for extra tasks to perform after setting PipelineSettings
|
||||
if (propName.equals("outputFrameDivisor")) {
|
||||
dashboardStreamer.setFrameDivisor(
|
||||
dashboardOutputStreamer.setFrameDivisor(
|
||||
pipelineManager.getCurrentPipelineSettings().outputFrameDivisor);
|
||||
}
|
||||
|
||||
@@ -322,10 +337,10 @@ public class VisionModule {
|
||||
visionSource.getSettables().getConfiguration().nickname = newName;
|
||||
ntConsumer.updateCameraNickname(newName);
|
||||
|
||||
frameConsumers.remove(dashboardStreamer);
|
||||
dashboardStreamer =
|
||||
frameConsumers.remove(dashboardOutputStreamer);
|
||||
dashboardOutputStreamer =
|
||||
new MJPGFrameConsumer(visionSource.getSettables().getConfiguration().nickname);
|
||||
frameConsumers.add(dashboardStreamer);
|
||||
frameConsumers.add(dashboardOutputStreamer);
|
||||
saveAndBroadcast();
|
||||
}
|
||||
|
||||
@@ -355,8 +370,9 @@ public class VisionModule {
|
||||
temp.put(k, internalMap);
|
||||
}
|
||||
ret.videoFormatList = temp;
|
||||
ret.streamPort = dashboardStreamer.getCurrentStreamPort();
|
||||
// ret.uiStreamPort = uiStreamer.getCurrentStreamPort();
|
||||
ret.outputStreamPort = dashboardOutputStreamer.getCurrentStreamPort();
|
||||
ret.inputStreamPort = dashboardInputStreamer.getCurrentStreamPort();
|
||||
// ret.uiStreamPort = uiStreamer.getCurrentStreamPort();
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
@@ -43,7 +43,7 @@ public class TrackedTarget implements Releasable {
|
||||
private double m_yaw;
|
||||
private double m_area;
|
||||
|
||||
private Pose2d m_robotRelativePose;
|
||||
private Pose2d m_robotRelativePose = new Pose2d();
|
||||
|
||||
private Mat m_cameraRelativeTvec, m_cameraRelativeRvec;
|
||||
|
||||
|
||||
Reference in New Issue
Block a user