From 0509b7aed0f397d7415780d0fba5ca0eb5aa98c6 Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 22 Nov 2019 14:20:23 -0800 Subject: [PATCH] Start plumbing UI --- .../classabstraction/VisionProcess.java | 163 ++++++++++++++++-- .../pipeline/CVPipeline2d.java | 2 +- .../vision/process/VisionProcess.java | 1 - 3 files changed, 153 insertions(+), 13 deletions(-) diff --git a/Main/src/main/java/com/chameleonvision/classabstraction/VisionProcess.java b/Main/src/main/java/com/chameleonvision/classabstraction/VisionProcess.java index 7df56efeb..d5c252fd4 100644 --- a/Main/src/main/java/com/chameleonvision/classabstraction/VisionProcess.java +++ b/Main/src/main/java/com/chameleonvision/classabstraction/VisionProcess.java @@ -2,15 +2,21 @@ package com.chameleonvision.classabstraction; import com.chameleonvision.classabstraction.camera.CameraProcess; import com.chameleonvision.classabstraction.camera.CameraStreamer; -import com.chameleonvision.classabstraction.pipeline.CVPipeline; -import com.chameleonvision.classabstraction.pipeline.CVPipelineResult; -import com.chameleonvision.classabstraction.pipeline.CVPipelineSettings; -import com.chameleonvision.classabstraction.pipeline.DriverVisionPipeline; +import com.chameleonvision.classabstraction.config.ConfigManager; +import com.chameleonvision.classabstraction.pipeline.*; import com.chameleonvision.classabstraction.util.LoopingRunnable; +import com.chameleonvision.web.ServerHandler; +import com.google.gson.Gson; +import com.google.gson.GsonBuilder; import edu.wpi.cscore.VideoMode; +import edu.wpi.first.networktables.EntryListenerFlags; +import edu.wpi.first.networktables.EntryNotification; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; import org.opencv.core.Mat; import java.util.ArrayList; +import java.util.HashMap; import java.util.List; public class VisionProcess { @@ -22,6 +28,7 @@ public class VisionProcess { private final VisionProcessRunnable visionRunnable; private final CameraStreamer cameraStreamer; private CVPipeline currentPipeline; + private int currentPipelineIndex = 0; private final CVPipelineSettings driverVisionSettings = new CVPipelineSettings(); @@ -30,11 +37,24 @@ public class VisionProcess { private volatile boolean hasUnprocessedFrame = true; private volatile CVPipelineResult lastPipelineResult; + // network table stuff + public NetworkTableEntry ntPipelineEntry; + public NetworkTableEntry ntDriverModeEntry; + private int ntDriveModeListenerID; + private int ntPipelineListenerID; + private NetworkTableEntry ntYawEntry; + private NetworkTableEntry ntPitchEntry; + private NetworkTableEntry ntAuxListEntry; + private NetworkTableEntry ntDistanceEntry; + private NetworkTableEntry ntTimeStampEntry; + private NetworkTableEntry ntValidEntry; + Gson gson = new GsonBuilder().setPrettyPrinting().create(); + public VisionProcess(CameraProcess cameraProcess, String name) { this.cameraProcess = cameraProcess; pipelines.add(new DriverVisionPipeline(() -> driverVisionSettings)); - setPipeline(pipelines.get(0)); + setPipeline(0); // Thread to grab frames from the camera // TODO: fix video modes!!! @@ -69,18 +89,134 @@ public class VisionProcess { new Thread(streamRunnable).start(); } - public void setPipeline(int pipelineIndex) { - CVPipeline newPipeline = pipelines.get(pipelineIndex); - if (newPipeline != null) { - setPipeline(newPipeline); + /** + * Removes the old value change listeners + * calls {@link #initNT} + * + * @param newTable passed to {@link #initNT} + */ + public void resetNT(NetworkTable newTable) { + ntDriverModeEntry.removeListener(ntDriveModeListenerID); + ntPipelineEntry.removeListener(ntPipelineListenerID); + initNT(newTable); + } + + private void initNT(NetworkTable newTable) { + ntPipelineEntry = newTable.getEntry("pipeline"); + ntDriverModeEntry = newTable.getEntry("driver_mode"); + ntPitchEntry = newTable.getEntry("pitch"); + ntYawEntry = newTable.getEntry("yaw"); + ntDistanceEntry = newTable.getEntry("distance"); + ntTimeStampEntry = newTable.getEntry("timestamp"); + ntValidEntry = newTable.getEntry("is_valid"); + ntDriveModeListenerID = ntDriverModeEntry.addListener(this::setDriverMode, EntryListenerFlags.kUpdate); + ntPipelineListenerID = ntPipelineEntry.addListener(this::setPipeline, EntryListenerFlags.kUpdate); + ntDriverModeEntry.setBoolean(false); + ntPipelineEntry.setNumber(0); + } + + private void setDriverMode(EntryNotification ignored) { + setPipeline(0); + } + + /** + * Method called by the nt entry listener to update the next pipeline. + * @param notification the notification + */ + private void setPipeline(EntryNotification notification) { + var wantedPipelineIndex = (int) notification.value.getDouble(); + + if (wantedPipelineIndex >= pipelines.size()) { + ntPipelineEntry.setNumber(currentPipelineIndex); + } else { + currentPipelineIndex = wantedPipelineIndex; + setPipeline(wantedPipelineIndex); } } - public void setPipeline(CVPipeline pipeline) { + public void setPipeline(int pipelineIndex) { + + CVPipeline newPipeline = pipelines.get(pipelineIndex); + if (newPipeline != null) { + setPipelineInternal(newPipeline); + } + + // update the configManager + if(ConfigManager.settings.currentCamera.equals(cameraProcess.getProperties().name)) { + ConfigManager.settings.currentPipeline = pipelineIndex; + HashMap pipeChange = new HashMap<>(); + pipeChange.put("currentPipeline", pipelineIndex); + ServerHandler.broadcastMessage(pipeChange); + ServerHandler.sendFullSettings(); + } + + } + + private void setPipelineInternal(CVPipeline pipeline) { + pipelines.add(pipeline); currentPipeline = pipeline; currentPipeline.initPipeline(cameraProcess); } + private void updateUI(CVPipelineResult data) { + if(cameraProcess.getProperties().name.equals(ConfigManager.settings.currentCamera)) { + HashMap WebSend = new HashMap<>(); + HashMap point = new HashMap<>(); + HashMap calculated = new HashMap<>(); + List center = new ArrayList<>(); + if (data.hasTarget) { + + if(data instanceof CVPipeline2d.CVPipeline2dResult) { + CVPipeline2d.CVPipeline2dResult result = (CVPipeline2d.CVPipeline2dResult) data; + CVPipeline2d.Target bestTarget = result.targets.get(0); + center.add(bestTarget.rawPoint.center.x); + center.add(bestTarget.rawPoint.center.y); + calculated.put("pitch", bestTarget.pitch); + calculated.put("yaw", bestTarget.yaw); + } else { + center.add(0.0); + center.add(0.0); + calculated.put("pitch", 0); + calculated.put("yaw", 0); + } + } else { + center.add(0.0); + center.add(0.0); + calculated.put("pitch", 0); + calculated.put("yaw", 0); + } + point.put("fps", visionRunnable.fps); + point.put("calculated", calculated); + point.put("rawPoint", center); + WebSend.put("point", point); + ServerHandler.broadcastMessage(WebSend); + } + } + + private void updateNetworkTableData(CVPipelineResult data) { + ntValidEntry.setBoolean(data.hasTarget); + if(data.hasTarget && !(data instanceof DriverVisionPipeline.DriverPipelineResult)) { + if(data instanceof CVPipeline2d.CVPipeline2dResult) { + ntTimeStampEntry.setDouble(data.processTime); + + //noinspection unchecked + List targets = (List) data.targets; + ntPitchEntry.setDouble(targets.get(0).pitch); + ntYawEntry.setDouble(targets.get(0).yaw); + ntDistanceEntry.setDouble(targets.get(0).area); + ntAuxListEntry.setString(gson.toJson(targets)); + + } else if(data instanceof CVPipeline3d.CVPipeline3dResult) { + // TODO implement + } + } else { + ntPitchEntry.setDouble(0.0); + ntYawEntry.setDouble(0.0); + ntDistanceEntry.setDouble(0.0); + ntTimeStampEntry.setDouble(0.0); + } + } + public void setVideoMode(VideoMode newMode) { cameraProcess.setVideoMode(newMode); cameraStreamer.setNewVideoMode(newMode); @@ -126,7 +262,7 @@ public class VisionProcess { camData.getLeft().copyTo(lastCameraFrame); hasUnprocessedFrame = true; - } + } } public Mat getFrame(Mat dst) { @@ -145,6 +281,7 @@ public class VisionProcess { */ private class VisionProcessRunnable implements Runnable { + public Double fps = 0.0; // TODO update or average or something private CVPipelineResult result; private Mat streamBuffer = new Mat(); @@ -167,6 +304,10 @@ public class VisionProcess { if (streamBuffer.cols() > 0 && streamBuffer.rows() > 0) { result = currentPipeline.runPipeline(streamBuffer); lastPipelineResult = result; + + updateNetworkTableData(lastPipelineResult); + updateUI(lastPipelineResult); + } else { // System.err.println("Bad streambuffer mat"); } diff --git a/Main/src/main/java/com/chameleonvision/classabstraction/pipeline/CVPipeline2d.java b/Main/src/main/java/com/chameleonvision/classabstraction/pipeline/CVPipeline2d.java index 5e281290f..aed8cb8c4 100644 --- a/Main/src/main/java/com/chameleonvision/classabstraction/pipeline/CVPipeline2d.java +++ b/Main/src/main/java/com/chameleonvision/classabstraction/pipeline/CVPipeline2d.java @@ -124,7 +124,7 @@ public class CVPipeline2d extends CVPipeline { + public static class CVPipeline2dResult extends CVPipelineResult { public CVPipeline2dResult(List targets, Mat outputMat, long processTime) { super(targets, outputMat, processTime); } diff --git a/Main/src/main/java/com/chameleonvision/vision/process/VisionProcess.java b/Main/src/main/java/com/chameleonvision/vision/process/VisionProcess.java index a329b82ce..b38032e8a 100644 --- a/Main/src/main/java/com/chameleonvision/vision/process/VisionProcess.java +++ b/Main/src/main/java/com/chameleonvision/vision/process/VisionProcess.java @@ -81,7 +81,6 @@ public class VisionProcess implements Runnable { pipeChange.put("currentPipeline", ntPipelineIndex); ServerHandler.broadcastMessage(pipeChange); ServerHandler.sendFullSettings(); - } } }