From 768964c8fcebf77bd0e864681d09deb9e8ca7bff Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 17 Jul 2020 20:05:03 -0700 Subject: [PATCH] Misc bugfixes (#39) * Selectively send pipeline changes * Make input and output both rotated * Notify UI of driver mode change over NT * Fix "show multiple" * Rename extent to fullness, fix area filtering This is a breaking change to docs (make sure we note area is out of 100 and is percentage) * Apply stream divisor to both streams Co-authored-by: Banks T --- photon-client/src/store/index.js | 4 +- photon-client/src/store/modules/pipeline.js | 2 +- .../src/views/PipelineViews/ContoursTab.vue | 14 +-- .../src/views/PipelineViews/InputTab.vue | 12 +- .../events/IncomingWebSocketEvent.java | 11 +- .../dataflow/events/OutgoingUIEvent.java | 14 ++- .../dataflow/websocket/UIDataPublisher.java | 81 +++++++++++++ .../photonvision/common/logging/Logger.java | 18 +-- .../photonvision/server/SocketHandler.java | 27 +++-- .../server/UIInboundSubscriber.java | 4 +- .../vision/pipe/impl/FilterContoursPipe.java | 25 ++-- .../vision/pipe/impl/SortContoursPipe.java | 2 +- .../pipeline/AdvancedPipelineSettings.java | 6 +- .../vision/pipeline/CVPipelineSettings.java | 9 +- .../vision/pipeline/ColoredShapePipeline.java | 5 +- .../vision/pipeline/DriverModePipeline.java | 16 +-- .../vision/pipeline/ReflectivePipeline.java | 13 +- .../vision/processes/VisionModule.java | 111 ++++++++---------- .../vision/target/TrackedTarget.java | 2 +- 19 files changed, 231 insertions(+), 145 deletions(-) create mode 100644 photon-server/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java diff --git a/photon-client/src/store/index.js b/photon-client/src/store/index.js index cba859fb9..0f62dfadf 100644 --- a/photon-client/src/store/index.js +++ b/photon-client/src/store/index.js @@ -53,7 +53,7 @@ export default new Vuex.Store({ cameraGain: 3, inputImageRotationMode: 0, cameraVideoModeIndex: 0, - outputFrameDivisor: 0, + streamingFrameDivisor: 0, // Settings that apply to reflective hsvHue: [0, 15], @@ -63,7 +63,7 @@ export default new Vuex.Store({ dilate: false, contourArea: [0, 12], contourRatio: [0, 12], - contourExtent: [0, 12], + contourFullness: [0, 12], contourSpecklePercentage: 5, contourGroupingMode: 0, contourIntersection: 0, diff --git a/photon-client/src/store/modules/pipeline.js b/photon-client/src/store/modules/pipeline.js index 6abc86cf2..3788a50cf 100644 --- a/photon-client/src/store/modules/pipeline.js +++ b/photon-client/src/store/modules/pipeline.js @@ -13,7 +13,7 @@ export default { dilate: false, area: [0, 12], ratio: [0, 12], - extent: [0, 12], + fullness: [0, 12], speckle: 5, targetGrouping: 0, targetIntersection: 0, diff --git a/photon-client/src/views/PipelineViews/ContoursTab.vue b/photon-client/src/views/PipelineViews/ContoursTab.vue index ec0597198..5190eaeab 100644 --- a/photon-client/src/views/PipelineViews/ContoursTab.vue +++ b/photon-client/src/views/PipelineViews/ContoursTab.vue @@ -19,12 +19,12 @@ @rollback="e=> rollback('contourRatio',e)" /> @@ -119,12 +119,12 @@ this.$store.commit("mutatePipeline", {"cameraVideoModeIndex": val}); } }, - outputFrameDivisor: { + streamingFrameDivisor: { get() { - return this.$store.getters.currentPipelineSettings.outputFrameDivisor + return this.$store.getters.currentPipelineSettings.streamingFrameDivisor }, set(val) { - this.$store.commit("mutatePipeline", {"outputFrameDivisor": val}); + this.$store.commit("mutatePipeline", {"streamingFrameDivisor": val}); } }, diff --git a/photon-server/src/main/java/org/photonvision/common/dataflow/events/IncomingWebSocketEvent.java b/photon-server/src/main/java/org/photonvision/common/dataflow/events/IncomingWebSocketEvent.java index 3131c2bf3..e378d0b4b 100644 --- a/photon-server/src/main/java/org/photonvision/common/dataflow/events/IncomingWebSocketEvent.java +++ b/photon-server/src/main/java/org/photonvision/common/dataflow/events/IncomingWebSocketEvent.java @@ -17,6 +17,7 @@ package org.photonvision.common.dataflow.events; +import io.javalin.websocket.WsContext; import java.util.HashMap; import org.photonvision.common.dataflow.DataChangeDestination; import org.photonvision.common.dataflow.DataChangeSource; @@ -24,15 +25,21 @@ import org.photonvision.common.dataflow.DataChangeSource; public class IncomingWebSocketEvent extends DataChangeEvent { public final Integer cameraIndex; + public final WsContext originContext; public IncomingWebSocketEvent(DataChangeDestination destType, String propertyName, T newValue) { - this(destType, propertyName, newValue, null); + this(destType, propertyName, newValue, null, null); } public IncomingWebSocketEvent( - DataChangeDestination destType, String propertyName, T newValue, Integer cameraIndex) { + DataChangeDestination destType, + String propertyName, + T newValue, + Integer cameraIndex, + WsContext originContext) { super(DataChangeSource.DCS_WEBSOCKET, destType, propertyName, newValue); this.cameraIndex = cameraIndex; + this.originContext = originContext; } @SuppressWarnings("unchecked") diff --git a/photon-server/src/main/java/org/photonvision/common/dataflow/events/OutgoingUIEvent.java b/photon-server/src/main/java/org/photonvision/common/dataflow/events/OutgoingUIEvent.java index cdc7bb8c2..71e325d9f 100644 --- a/photon-server/src/main/java/org/photonvision/common/dataflow/events/OutgoingUIEvent.java +++ b/photon-server/src/main/java/org/photonvision/common/dataflow/events/OutgoingUIEvent.java @@ -17,6 +17,7 @@ package org.photonvision.common.dataflow.events; +import io.javalin.websocket.WsContext; import java.util.HashMap; import org.photonvision.common.dataflow.DataChangeDestination; import org.photonvision.common.dataflow.DataChangeSource; @@ -24,17 +25,24 @@ import org.photonvision.server.UIUpdateType; public class OutgoingUIEvent extends DataChangeEvent { public final UIUpdateType updateType; + public final WsContext originContext; - public OutgoingUIEvent(UIUpdateType updateType, String propertyName, T newValue) { + public OutgoingUIEvent( + UIUpdateType updateType, String propertyName, T newValue, WsContext originContext) { super(DataChangeSource.DCS_WEBSOCKET, DataChangeDestination.DCD_UI, propertyName, newValue); this.updateType = updateType; + this.originContext = originContext; } public static OutgoingUIEvent> wrappedOf( - UIUpdateType uiUpdateType, String commandName, String propertyName, Object value) { + UIUpdateType uiUpdateType, + String commandName, + String propertyName, + Object value, + WsContext originContext) { HashMap data = new HashMap<>(); data.put(propertyName, value); - return new OutgoingUIEvent<>(uiUpdateType, commandName, data); + return new OutgoingUIEvent<>(uiUpdateType, commandName, data, originContext); } } diff --git a/photon-server/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java b/photon-server/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java new file mode 100644 index 000000000..f41de278c --- /dev/null +++ b/photon-server/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java @@ -0,0 +1,81 @@ +/* + * Copyright (C) 2020 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.common.dataflow.websocket; + +import com.fasterxml.jackson.core.JsonProcessingException; +import edu.wpi.first.wpilibj.MedianFilter; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.HashMap; +import org.photonvision.common.dataflow.CVPipelineResultConsumer; +import org.photonvision.common.logging.LogGroup; +import org.photonvision.common.logging.Logger; +import org.photonvision.server.SocketHandler; +import org.photonvision.vision.pipeline.result.CVPipelineResult; + +public class UIDataPublisher implements CVPipelineResultConsumer { + private static final Logger logger = new Logger(UIDataPublisher.class, LogGroup.VisionModule); + + // TODO check if this is the right spot to do FPS calculation + private final MedianFilter fpsAverager = new MedianFilter(10); + private final int index; + private long lastRunTime = 0; + private long lastUIResultUpdateTime = 0; + + public UIDataPublisher(int index) { + this.index = index; + } + + @Override + public void accept(CVPipelineResult result) { + var now = System.currentTimeMillis(); + + var fps = fpsAverager.calculate(1000.0 / (now - lastRunTime)); + lastRunTime = now; + + // only update the UI at 15hz + if (lastUIResultUpdateTime + 1000.0 / 15.0 > now) return; + + var uiMap = new HashMap>(); + var dataMap = new HashMap(); + + dataMap.put("fps", fps); + dataMap.put("latency", result.getLatencyMillis()); + + var targets = result.targets; + + var uiTargets = new ArrayList>(); + for (var t : targets) { + uiTargets.add(t.toHashMap()); + } + dataMap.put("targets", uiTargets); + + uiMap.put(index, dataMap); + var retMap = new HashMap(); + retMap.put("updatePipelineResult", uiMap); + + try { + SocketHandler.getInstance().broadcastMessage(retMap, null); + } catch (JsonProcessingException e) { + logger.error(e.getMessage()); + logger.error(Arrays.toString(e.getStackTrace())); + } + + lastUIResultUpdateTime = now; + } +} diff --git a/photon-server/src/main/java/org/photonvision/common/logging/Logger.java b/photon-server/src/main/java/org/photonvision/common/logging/Logger.java index 19a5033ed..bf13d1485 100644 --- a/photon-server/src/main/java/org/photonvision/common/logging/Logger.java +++ b/photon-server/src/main/java/org/photonvision/common/logging/Logger.java @@ -30,6 +30,7 @@ import java.util.List; import java.util.function.Supplier; import org.photonvision.common.dataflow.DataChangeService; import org.photonvision.common.dataflow.events.OutgoingUIEvent; +import org.photonvision.server.SocketHandler; import org.photonvision.server.UIUpdateType; public class Logger { @@ -122,7 +123,7 @@ public class Logger { for (var a : currentAppenders) { var shouldColor = a instanceof ConsoleLogAppender; var formattedMessage = format(message, level, group, clazz, shouldColor); - a.log(formattedMessage); + a.log(formattedMessage, level); } } @@ -183,23 +184,24 @@ public class Logger { } private interface LogAppender { - void log(String message); + void log(String message, LogLevel level); } private static class ConsoleLogAppender implements LogAppender { @Override - public void log(String message) { + public void log(String message, LogLevel level) { System.out.println(message); } } private static class UILogAppender implements LogAppender { @Override - public void log(String message) { - var message_ = new HashMap<>(); - message_.put("logMessage", message); + public void log(String message, LogLevel level) { + var messageMap = new SocketHandler.UIMap(); + messageMap.put("logMessage", message); + messageMap.put("logLevel", level.code); DataChangeService.getInstance() - .publishEvent(new OutgoingUIEvent<>(UIUpdateType.BROADCAST, "log", message_)); + .publishEvent(new OutgoingUIEvent<>(UIUpdateType.BROADCAST, "log", messageMap, null)); } } @@ -211,7 +213,7 @@ public class Logger { } @Override - public void log(String message) { + public void log(String message, LogLevel level) { try (AsynchronousFileChannel asyncFile = AsynchronousFileChannel.open( filePath, StandardOpenOption.WRITE, StandardOpenOption.CREATE)) { diff --git a/photon-server/src/main/java/org/photonvision/server/SocketHandler.java b/photon-server/src/main/java/org/photonvision/server/SocketHandler.java index 47f4a9a86..f5007c0c8 100644 --- a/photon-server/src/main/java/org/photonvision/server/SocketHandler.java +++ b/photon-server/src/main/java/org/photonvision/server/SocketHandler.java @@ -139,7 +139,8 @@ public class SocketHandler { DataChangeDestination.DCD_ACTIVEMODULE, "cameraNickname", (String) entryValue, - cameraIndex); + cameraIndex, + context); dcService.publishEvent(ccnEvent); break; } @@ -150,7 +151,8 @@ public class SocketHandler { DataChangeDestination.DCD_ACTIVEMODULE, "pipelineName", (String) entryValue, - cameraIndex); + cameraIndex, + context); dcService.publishEvent(cpnEvent); break; } @@ -169,7 +171,8 @@ public class SocketHandler { DataChangeDestination.DCD_ACTIVEMODULE, "newPipelineInfo", Pair.of(name, type), - cameraIndex); + cameraIndex, + context); dcService.publishEvent(newPipelineEvent); break; } @@ -184,7 +187,8 @@ public class SocketHandler { DataChangeDestination.DCD_ACTIVEMODULE, "deleteCurrPipeline", 0, - cameraIndex); + cameraIndex, + context); dcService.publishEvent(deleteCurrentPipelineEvent); break; } @@ -213,7 +217,8 @@ public class SocketHandler { DataChangeDestination.DCD_ACTIVEMODULE, "changePipeline", (Integer) entryValue, - cameraIndex); + cameraIndex, + context); dcService.publishEvent(changePipelineEvent); break; } @@ -224,7 +229,8 @@ public class SocketHandler { DataChangeDestination.DCD_ACTIVEMODULE, "changePipeline", PipelineManager.CAL_3D_INDEX, - cameraIndex); + cameraIndex, + context); dcService.publishEvent(changePipelineEvent); break; } @@ -232,7 +238,11 @@ public class SocketHandler { { var takeCalSnapshotEvent = new IncomingWebSocketEvent<>( - DataChangeDestination.DCD_ACTIVEMODULE, "takeCalSnapshot", 0, cameraIndex); + DataChangeDestination.DCD_ACTIVEMODULE, + "takeCalSnapshot", + 0, + cameraIndex, + context); dcService.publishEvent(takeCalSnapshotEvent); break; } @@ -251,7 +261,8 @@ public class SocketHandler { DataChangeDestination.DCD_ACTIVEPIPELINESETTINGS, dataEntry.getKey(), dataEntry.getValue(), - cameraIndex2); + cameraIndex2, + context); dcService.publishEvent(pipelineSettingChangeEvent); } } else { diff --git a/photon-server/src/main/java/org/photonvision/server/UIInboundSubscriber.java b/photon-server/src/main/java/org/photonvision/server/UIInboundSubscriber.java index cee2d4a1f..a35083158 100644 --- a/photon-server/src/main/java/org/photonvision/server/UIInboundSubscriber.java +++ b/photon-server/src/main/java/org/photonvision/server/UIInboundSubscriber.java @@ -43,7 +43,9 @@ public class UIInboundSubscriber extends DataChangeSubscriber { || incomingWSEvent.propertyName.equals("sendFullSettings")) { // Send full settings var settings = ConfigManager.getInstance().getConfig().toHashMap(); - var message = new OutgoingUIEvent<>(UIUpdateType.BROADCAST, "fullsettings", settings); + var message = + new OutgoingUIEvent<>( + UIUpdateType.BROADCAST, "fullsettings", settings, incomingWSEvent.originContext); DataChangeService.getInstance().publishEvent(message); } } diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/FilterContoursPipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/FilterContoursPipe.java index 11bf49175..2dbf1819d 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/FilterContoursPipe.java +++ b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/FilterContoursPipe.java @@ -21,7 +21,6 @@ import java.util.ArrayList; import java.util.List; import org.opencv.core.Rect; import org.opencv.core.RotatedRect; -import org.photonvision.common.util.math.MathUtils; import org.photonvision.common.util.numbers.DoubleCouple; import org.photonvision.vision.frame.FrameStaticProperties; import org.photonvision.vision.opencv.Contour; @@ -42,18 +41,20 @@ public class FilterContoursPipe } private void filterContour(Contour contour) { - // Area Filtering. - double contourArea = contour.getArea(); - double areaRatio = (contourArea / params.getFrameStaticProperties().imageArea); - double minArea = MathUtils.sigmoid(params.getArea().getFirst()); - double maxArea = MathUtils.sigmoid(params.getArea().getSecond()); - if (areaRatio < minArea || areaRatio > maxArea) return; - - // Extent Filtering. RotatedRect minAreaRect = contour.getMinAreaRect(); - double minExtent = params.getFullness().getFirst() * minAreaRect.size.area() / 100; - double maxExtent = params.getFullness().getSecond() * minAreaRect.size.area() / 100; - if (contourArea <= minExtent || contourArea >= maxExtent) return; + + // Area Filtering. + double areaPercentage = + minAreaRect.size.area() / params.getFrameStaticProperties().imageArea * 100.0; + double minAreaPercentage = params.getArea().getFirst(); + double maxAreaPercentage = params.getArea().getSecond(); + if (areaPercentage < minAreaPercentage || areaPercentage > maxAreaPercentage) return; + + // Fullness Filtering. + double contourArea = contour.getArea(); + double minFullness = params.getFullness().getFirst() * minAreaRect.size.area() / 100; + double maxFullness = params.getFullness().getSecond() * minAreaRect.size.area() / 100; + if (contourArea <= minFullness || contourArea >= maxFullness) return; // Aspect Ratio Filtering. Rect boundingRect = contour.getBoundingRect(); diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/SortContoursPipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/SortContoursPipe.java index 470ae5700..50af77b77 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/SortContoursPipe.java +++ b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/SortContoursPipe.java @@ -49,7 +49,7 @@ public class SortContoursPipe } return new ArrayList<>( - m_sortedContours.subList(0, Math.min(in.size(), params.getMaxTargets() - 1))); + m_sortedContours.subList(0, Math.min(in.size(), params.getMaxTargets()))); } private double calcSquareCenterDistance(PotentialTarget rect) { diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/AdvancedPipelineSettings.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/AdvancedPipelineSettings.java index 1d3100a31..105e2ad0b 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipeline/AdvancedPipelineSettings.java +++ b/photon-server/src/main/java/org/photonvision/vision/pipeline/AdvancedPipelineSettings.java @@ -43,7 +43,7 @@ public class AdvancedPipelineSettings extends CVPipelineSettings { public DoubleCouple contourArea = new DoubleCouple(0.0, 100.0); public DoubleCouple contourRatio = new DoubleCouple(0.0, 20.0); - public DoubleCouple contourExtent = new DoubleCouple(0.0, 100.0); + public DoubleCouple contourFullness = new DoubleCouple(0.0, 100.0); public int contourSpecklePercentage = 5; // the order in which to sort contours to find the most desirable @@ -86,7 +86,7 @@ public class AdvancedPipelineSettings extends CVPipelineSettings { && hsvValue.equals(that.hsvValue) && contourArea.equals(that.contourArea) && contourRatio.equals(that.contourRatio) - && contourExtent.equals(that.contourExtent) + && contourFullness.equals(that.contourFullness) && contourSortMode == that.contourSortMode && contourTargetOffsetPointEdge == that.contourTargetOffsetPointEdge && contourTargetOrientation == that.contourTargetOrientation @@ -107,7 +107,7 @@ public class AdvancedPipelineSettings extends CVPipelineSettings { dilate, contourArea, contourRatio, - contourExtent, + contourFullness, contourSpecklePercentage, contourSortMode, contourTargetOffsetPointEdge, diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java index b8c1629c9..1b0339543 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java +++ b/photon-server/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java @@ -43,8 +43,7 @@ public class CVPipelineSettings { public int cameraBrightness = 50; public int cameraGain = 50; public int cameraVideoModeIndex = 0; - public FrameDivisor inputFrameDivisor = FrameDivisor.NONE; - public FrameDivisor outputFrameDivisor = FrameDivisor.NONE; + public FrameDivisor streamingFrameDivisor = FrameDivisor.NONE; public boolean ledMode = false; @Override @@ -62,8 +61,7 @@ public class CVPipelineSettings { && inputImageFlipMode == that.inputImageFlipMode && inputImageRotationMode == that.inputImageRotationMode && pipelineNickname.equals(that.pipelineNickname) - && inputFrameDivisor == that.inputFrameDivisor - && outputFrameDivisor == that.outputFrameDivisor; + && streamingFrameDivisor == that.streamingFrameDivisor; } @Override @@ -78,8 +76,7 @@ public class CVPipelineSettings { cameraBrightness, cameraGain, cameraVideoModeIndex, - inputFrameDivisor, - outputFrameDivisor, + streamingFrameDivisor, ledMode); } } diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java index eaad2668a..27889d259 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java +++ b/photon-server/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java @@ -127,7 +127,10 @@ public class ColoredShapePipeline groupContoursPipe.setParams(groupContoursParams); SortContoursPipe.SortContoursParams sortContoursParams = - new SortContoursPipe.SortContoursParams(settings.contourSortMode, frameStaticProperties, 5); + new SortContoursPipe.SortContoursParams( + settings.contourSortMode, + frameStaticProperties, + settings.outputShowMultipleTargets ? 5 : 1); // TODO don't hardcode? sortContoursPipe.setParams(sortContoursParams); Collect2dTargetsPipe.Collect2dTargetsParams collect2dTargetsParams = diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java index cb553d456..789c7b7ce 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java +++ b/photon-server/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java @@ -24,7 +24,6 @@ import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameStaticProperties; import org.photonvision.vision.opencv.CVMat; import org.photonvision.vision.pipe.impl.Draw2dCrosshairPipe; -import org.photonvision.vision.pipe.impl.ResizeImagePipe; import org.photonvision.vision.pipe.impl.RotateImagePipe; import org.photonvision.vision.pipeline.result.DriverModePipelineResult; @@ -32,9 +31,6 @@ public class DriverModePipeline extends CVPipeline { private final RotateImagePipe rotateImagePipe = new RotateImagePipe(); - - private final ResizeImagePipe resizeImagePipe = new ResizeImagePipe(); - private final Draw2dCrosshairPipe draw2dCrosshairPipe = new Draw2dCrosshairPipe(); public DriverModePipeline() { @@ -48,10 +44,6 @@ public class DriverModePipeline new RotateImagePipe.RotateImageParams(settings.inputImageRotationMode); rotateImagePipe.setParams(rotateImageParams); - ResizeImagePipe.ResizeImageParams resizeImageParams = - new ResizeImagePipe.ResizeImageParams(settings.inputFrameDivisor); - resizeImagePipe.setParams(resizeImageParams); - Draw2dCrosshairPipe.Draw2dCrosshairParams draw2dCrosshairParams = new Draw2dCrosshairPipe.Draw2dCrosshairParams( settings.offsetPointMode, settings.offsetPoint); @@ -62,15 +54,11 @@ public class DriverModePipeline public DriverModePipelineResult process(Frame frame, DriverModePipelineSettings settings) { // apply pipes var rotateImageResult = rotateImagePipe.apply(frame.image.getMat()); - var resizeImageResult = resizeImagePipe.apply(rotateImageResult.result); var draw2dCrosshairResult = - draw2dCrosshairPipe.apply(Pair.of(resizeImageResult.result, List.of())); + draw2dCrosshairPipe.apply(Pair.of(rotateImageResult.result, List.of())); // calculate elapsed nanoseconds - long totalNanos = - rotateImageResult.nanosElapsed - + resizeImageResult.nanosElapsed - + draw2dCrosshairResult.nanosElapsed; + long totalNanos = rotateImageResult.nanosElapsed + draw2dCrosshairResult.nanosElapsed; return new DriverModePipelineResult( MathUtils.nanosToMillis(totalNanos), diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java index 3631788f5..302fdc289 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java +++ b/photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java @@ -105,7 +105,7 @@ public class ReflectivePipeline extends CVPipeline rotateImageResult = rotateImagePipe.apply(frame.image.getMat()); sumPipeNanosElapsed += rotateImageResult.nanosElapsed; + rawInputMat.release(); + frame.image.getMat().copyTo(rawInputMat); + CVPipeResult erodeDilateResult = erodeDilatePipe.apply(rotateImageResult.result); sumPipeNanosElapsed += erodeDilateResult.nanosElapsed; diff --git a/photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java index 76d44911b..6842af61f 100644 --- a/photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java +++ b/photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java @@ -17,8 +17,7 @@ package org.photonvision.vision.processes; -import com.fasterxml.jackson.core.JsonProcessingException; -import edu.wpi.first.wpilibj.MedianFilter; +import io.javalin.websocket.WsContext; import java.util.*; import org.apache.commons.lang3.tuple.Pair; import org.photonvision.common.configuration.CameraConfiguration; @@ -31,12 +30,12 @@ import org.photonvision.common.dataflow.events.DataChangeEvent; import org.photonvision.common.dataflow.events.IncomingWebSocketEvent; import org.photonvision.common.dataflow.events.OutgoingUIEvent; import org.photonvision.common.dataflow.networktables.NTDataPublisher; +import org.photonvision.common.dataflow.websocket.UIDataPublisher; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.util.SerializationUtils; import org.photonvision.common.util.numbers.DoubleCouple; import org.photonvision.common.util.numbers.IntegerCouple; -import org.photonvision.server.SocketHandler; import org.photonvision.server.UIUpdateType; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameConsumer; @@ -59,13 +58,12 @@ public class VisionModule { private final LinkedList resultConsumers = new LinkedList<>(); private final LinkedList frameConsumers = new LinkedList<>(); private final NTDataPublisher ntConsumer; + private final UIDataPublisher uiDataConsumer; private final int moduleIndex; - private long lastUIResultUpdateTime = 0; - private long lastRunTime = 0; - private final MedianFilter fpsAverager = new MedianFilter(10); + private long lastSettingChangeTimestamp = 0; - private final MJPGFrameConsumer dashboardInputStreamer; + private MJPGFrameConsumer dashboardInputStreamer; private MJPGFrameConsumer dashboardOutputStreamer; public VisionModule(PipelineManager pipelineManager, VisionSource visionSource, int index) { @@ -100,50 +98,22 @@ public class VisionModule { pipelineManager::getCurrentPipelineIndex, pipelineManager::setIndex, pipelineManager::getDriverMode, - pipelineManager::setDriverMode); + this::setDriverMode); + uiDataConsumer = new UIDataPublisher(index); addResultConsumer(ntConsumer); - addResultConsumer( - result -> { - var now = System.currentTimeMillis(); - - var fps = fpsAverager.calculate(1000.0 / (now - lastRunTime)); - lastRunTime = now; - - // only update the UI at 15hz - if (lastUIResultUpdateTime + 1000.0 / 15.0 > now) return; - - var uiMap = new HashMap>(); - var dataMap = new HashMap(); - - dataMap.put("fps", fps); - dataMap.put("latency", result.getLatencyMillis()); - - var targets = result.targets; - - var uiTargets = new ArrayList>(); - for (var t : targets) { - uiTargets.add(t.toHashMap()); - } - dataMap.put("targets", uiTargets); - - uiMap.put(index, dataMap); - var retMap = new HashMap(); - retMap.put("updatePipelineResult", uiMap); - - try { - SocketHandler.getInstance().broadcastMessage(retMap, null); - } catch (JsonProcessingException e) { - logger.error(e.getMessage()); - logger.error(Arrays.toString(e.getStackTrace())); - } - - lastUIResultUpdateTime = now; - }); + addResultConsumer(uiDataConsumer); setPipeline(visionSource.getSettables().getConfiguration().currentPipelineIndex); + dashboardInputStreamer.setFrameDivisor( + pipelineManager.getCurrentPipelineSettings().streamingFrameDivisor); dashboardOutputStreamer.setFrameDivisor( - pipelineManager.getCurrentPipelineSettings().outputFrameDivisor); + pipelineManager.getCurrentPipelineSettings().streamingFrameDivisor); + } + + private void setDriverMode(boolean isDriverMode) { + pipelineManager.setDriverMode(isDriverMode); + saveAndBroadcastAll(); } public void start() { @@ -182,13 +152,12 @@ public class VisionModule { var newNickname = (String) newPropValue; logger.info("Changing nickname to " + newNickname); setCameraNickname(newNickname); - saveAndBroadcast(); + saveAndBroadcastAll(); return; case "pipelineName": // rename current pipeline logger.info("Changing nick to " + newPropValue); pipelineManager.getCurrentPipelineSettings().pipelineNickname = (String) newPropValue; - // TODO rename config file - saveAndBroadcast(); + saveAndBroadcastAll(); return; case "newPipelineInfo": // add new pipeline var typeName = (Pair) newPropValue; @@ -199,13 +168,13 @@ public class VisionModule { var addedSettings = pipelineManager.addPipeline(type); addedSettings.pipelineNickname = name; - saveAndBroadcast(); + saveAndBroadcastAll(); return; case "deleteCurrPipeline": var indexToDelete = pipelineManager.getCurrentPipelineIndex(); logger.info("Deleting current pipe at index " + indexToDelete); pipelineManager.removePipeline(indexToDelete); - saveAndBroadcast(); + saveAndBroadcastAll(); return; case "changePipeline": // change active pipeline var index = (Integer) newPropValue; @@ -215,7 +184,7 @@ public class VisionModule { } logger.debug("Setting pipeline index to " + index); setPipeline(index); - saveAndBroadcast(); + saveAndBroadcastAll(); return; } @@ -266,9 +235,11 @@ public class VisionModule { logger.trace("Set prop " + propName + " to value " + newPropValue); // special case for extra tasks to perform after setting PipelineSettings - if (propName.equals("outputFrameDivisor")) { + if (propName.equals("streamingFrameDivisor")) { + dashboardInputStreamer.setFrameDivisor( + pipelineManager.getCurrentPipelineSettings().streamingFrameDivisor); dashboardOutputStreamer.setFrameDivisor( - pipelineManager.getCurrentPipelineSettings().outputFrameDivisor); + pipelineManager.getCurrentPipelineSettings().streamingFrameDivisor); } } catch (NoSuchFieldException | IllegalAccessException e) { @@ -284,7 +255,10 @@ public class VisionModule { logger.error("Unknown exception when setting PSC prop!"); e.printStackTrace(); } - saveAndBroadcast(propName, newPropValue); + + saveModule(); + + VisionModule.this.lastSettingChangeTimestamp = System.currentTimeMillis(); } } } @@ -308,38 +282,47 @@ public class VisionModule { pipelineManager.getCurrentPipelineIndex(); } - private void saveAndBroadcast() { + private void saveModule() { ConfigManager.getInstance() .saveModule( getStateAsCameraConfig(), visionSource.getSettables().getConfiguration().uniqueName); + } + + private void saveAndBroadcastAll() { + saveModule(); DataChangeService.getInstance() .publishEvent( new OutgoingUIEvent<>( UIUpdateType.BROADCAST, "fullsettings", - ConfigManager.getInstance().getConfig().toHashMap())); + ConfigManager.getInstance().getConfig().toHashMap(), + null)); } - private void saveAndBroadcast(String propertyName, Object value) { + private void saveAndBroadcastSelective( + WsContext originContext, String propertyName, Object value) { logger.trace("Broadcasting PSC mutation - " + propertyName + ": " + value); - ConfigManager.getInstance() - .saveModule( - getStateAsCameraConfig(), visionSource.getSettables().getConfiguration().uniqueName); + saveModule(); DataChangeService.getInstance() .publishEvent( OutgoingUIEvent.wrappedOf( - UIUpdateType.BROADCAST, "mutatePipeline", propertyName, value)); + UIUpdateType.BROADCAST, "mutatePipeline", propertyName, value, originContext)); } private void setCameraNickname(String newName) { visionSource.getSettables().getConfiguration().nickname = newName; ntConsumer.updateCameraNickname(newName); + // rename streams frameConsumers.remove(dashboardOutputStreamer); + frameConsumers.remove(dashboardInputStreamer); dashboardOutputStreamer = - new MJPGFrameConsumer(visionSource.getSettables().getConfiguration().nickname); + new MJPGFrameConsumer( + visionSource.getSettables().getConfiguration().uniqueName + "-output"); + dashboardInputStreamer = + new MJPGFrameConsumer(visionSource.getSettables().getConfiguration().uniqueName + "-input"); frameConsumers.add(dashboardOutputStreamer); - saveAndBroadcast(); + frameConsumers.add(dashboardInputStreamer); } public PhotonConfiguration.UICameraConfiguration toUICameraConfig() { diff --git a/photon-server/src/main/java/org/photonvision/vision/target/TrackedTarget.java b/photon-server/src/main/java/org/photonvision/vision/target/TrackedTarget.java index d1a3cc19a..56235113e 100644 --- a/photon-server/src/main/java/org/photonvision/vision/target/TrackedTarget.java +++ b/photon-server/src/main/java/org/photonvision/vision/target/TrackedTarget.java @@ -115,7 +115,7 @@ public class TrackedTarget implements Releasable { m_yaw = TargetCalculations.calculateYaw( m_targetOffsetPoint.x, m_robotOffsetPoint.x, params.horizontalFocalLength); - m_area = m_mainContour.getMinAreaRect().size.area() / params.imageArea; + m_area = m_mainContour.getMinAreaRect().size.area() / params.imageArea * 100; m_skew = TargetCalculations.calculateSkew(params.isLandscape, getMinAreaRect()); }