From 204f6f82533f4e0ca2f00fe456935f6fa2a95b76 Mon Sep 17 00:00:00 2001 From: "Cameron (3539)" Date: Mon, 7 Oct 2024 21:54:09 -0400 Subject: [PATCH 1/4] MESSYYY --- .../vision/processes/VisionModule.java | 238 +++++++++++++++++- .../vision/processes/VisionModuleChange.java | 55 ++++ .../VisionModuleChangeSubscriber.java | 223 +--------------- .../vision/processes/VisionRunner.java | 7 +- 4 files changed, 303 insertions(+), 220 deletions(-) create mode 100644 photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChange.java diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java index e5adc484c4..d82353b028 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java @@ -25,8 +25,11 @@ import java.util.HashMap; import java.util.LinkedList; import java.util.List; +import java.util.Map; import java.util.function.BiConsumer; import java.util.stream.Collectors; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.Point; import org.opencv.core.Size; import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.configuration.ConfigManager; @@ -41,6 +44,9 @@ import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.util.SerializationUtils; +import org.photonvision.common.util.file.JacksonUtils; +import org.photonvision.common.util.numbers.DoubleCouple; +import org.photonvision.common.util.numbers.IntegerCouple; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.camera.CameraQuirk; import org.photonvision.vision.camera.CameraType; @@ -51,9 +57,11 @@ import org.photonvision.vision.frame.consumer.MJPGFrameConsumer; import org.photonvision.vision.pipeline.AdvancedPipelineSettings; import org.photonvision.vision.pipeline.OutputStreamPipeline; +import org.photonvision.vision.pipeline.PipelineType; import org.photonvision.vision.pipeline.ReflectivePipelineSettings; import org.photonvision.vision.pipeline.UICalibrationData; import org.photonvision.vision.pipeline.result.CVPipelineResult; +import org.photonvision.vision.target.RobotOffsetPointOperation; import org.photonvision.vision.target.TargetModel; import org.photonvision.vision.target.TrackedTarget; @@ -70,7 +78,8 @@ public class VisionModule { private final VisionRunner visionRunner; private final StreamRunnable streamRunnable; private final LinkedList resultConsumers = new LinkedList<>(); - // Raw result consumers run before any drawing has been done by the OutputStreamPipeline + // Raw result consumers run before any drawing has been done by the + // OutputStreamPipeline private final LinkedList>> streamResultConsumers = new LinkedList<>(); private final NTDataPublisher ntConsumer; @@ -90,6 +99,8 @@ public class VisionModule { MJPGFrameConsumer inputVideoStreamer; MJPGFrameConsumer outputVideoStreamer; + private List> settingChanges = new ArrayList<>(); + public VisionModule(PipelineManager pipelineManager, VisionSource visionSource, int index) { logger = new Logger( @@ -102,7 +113,8 @@ public VisionModule(PipelineManager pipelineManager, VisionSource visionSource, if (visionSource.getCameraConfiguration().cameraQuirks == null) visionSource.getCameraConfiguration().cameraQuirks = QuirkyCamera.DefaultCamera; - // We don't show gain if the config says it's -1. So check here to make sure it's non-negative + // We don't show gain if the config says it's -1. So check here to make sure + // it's non-negative // if it _is_ supported if (cameraQuirks.hasQuirk(CameraQuirk.Gain)) { pipelineManager.userPipelineSettings.forEach( @@ -125,7 +137,8 @@ public VisionModule(PipelineManager pipelineManager, VisionSource visionSource, this.visionSource.getFrameProvider(), this.pipelineManager::getCurrentPipeline, this::consumeResult, - this.cameraQuirks); + this.cameraQuirks, + this); this.streamRunnable = new StreamRunnable(new OutputStreamPipeline()); this.moduleIndex = index; @@ -294,6 +307,219 @@ public void run() { } } + public List> getSettingChanges() { + return settingChanges; + } + + public void processSettingChanges() { + // special case for non-PipelineSetting changes + synchronized (getSettingChanges()) { + for (var change : settingChanges) { + var propName = change.getPropName(); + var newPropValue = change.getNewPropValue(); + var currentSettings = change.getCurrentSettings(); + var originContext = change.getOriginContext(); + switch (propName) { + // case "cameraNickname": // rename camera + // var newNickname = (String) newPropValue; + // logger.info("Changing nickname to " + newNickname); + // setCameraNickname(newNickname); + // return; + case "pipelineName": // rename current pipeline + logger.info("Changing nick to " + newPropValue); + pipelineManager.getCurrentPipelineSettings().pipelineNickname = (String) newPropValue; + saveAndBroadcastAll(); + continue; + case "newPipelineInfo": // add new pipeline + var typeName = (Pair) newPropValue; + var type = typeName.getRight(); + var name = typeName.getLeft(); + + logger.info("Adding a " + type + " pipeline with name " + name); + + var addedSettings = pipelineManager.addPipeline(type); + addedSettings.pipelineNickname = name; + saveAndBroadcastAll(); + continue; + case "deleteCurrPipeline": + var indexToDelete = pipelineManager.getRequestedIndex(); + logger.info("Deleting current pipe at index " + indexToDelete); + int newIndex = pipelineManager.removePipeline(indexToDelete); + setPipeline(newIndex); + saveAndBroadcastAll(); + continue; + case "changePipeline": // change active pipeline + var index = (Integer) newPropValue; + if (index == pipelineManager.getRequestedIndex()) { + logger.debug("Skipping pipeline change, index " + index + " already active"); + continue; + } + setPipeline(index); + saveAndBroadcastAll(); + continue; + case "startCalibration": + try { + var data = + JacksonUtils.deserialize( + (Map) newPropValue, UICalibrationData.class); + startCalibration(data); + saveAndBroadcastAll(); + } catch (Exception e) { + logger.error("Error deserailizing start-cal request", e); + } + continue; + case "saveInputSnapshot": + saveInputSnapshot(); + continue; + case "saveOutputSnapshot": + saveOutputSnapshot(); + continue; + case "takeCalSnapshot": + takeCalibrationSnapshot(); + continue; + case "duplicatePipeline": + int idx = pipelineManager.duplicatePipeline((Integer) newPropValue); + setPipeline(idx); + saveAndBroadcastAll(); + continue; + case "calibrationUploaded": + if (newPropValue instanceof CameraCalibrationCoefficients) + addCalibrationToConfig((CameraCalibrationCoefficients) newPropValue); + continue; + case "robotOffsetPoint": + if (currentSettings instanceof AdvancedPipelineSettings) { + var curAdvSettings = (AdvancedPipelineSettings) currentSettings; + var offsetOperation = RobotOffsetPointOperation.fromIndex((int) newPropValue); + var latestTarget = lastPipelineResultBestTarget; + + if (latestTarget != null) { + var newPoint = latestTarget.getTargetOffsetPoint(); + + switch (curAdvSettings.offsetRobotOffsetMode) { + case Single: + if (offsetOperation == RobotOffsetPointOperation.ROPO_CLEAR) { + curAdvSettings.offsetSinglePoint = new Point(); + } else if (offsetOperation == RobotOffsetPointOperation.ROPO_TAKESINGLE) { + curAdvSettings.offsetSinglePoint = newPoint; + } + break; + case Dual: + if (offsetOperation == RobotOffsetPointOperation.ROPO_CLEAR) { + curAdvSettings.offsetDualPointA = new Point(); + curAdvSettings.offsetDualPointAArea = 0; + curAdvSettings.offsetDualPointB = new Point(); + curAdvSettings.offsetDualPointBArea = 0; + } else { + // update point and area + switch (offsetOperation) { + case ROPO_TAKEFIRSTDUAL: + curAdvSettings.offsetDualPointA = newPoint; + curAdvSettings.offsetDualPointAArea = latestTarget.getArea(); + break; + case ROPO_TAKESECONDDUAL: + curAdvSettings.offsetDualPointB = newPoint; + curAdvSettings.offsetDualPointBArea = latestTarget.getArea(); + break; + default: + break; + } + } + break; + default: + break; + } + } + } + continue; + case "changePipelineType": + changePipelineType((Integer) newPropValue); + saveAndBroadcastAll(); + continue; + case "isDriverMode": + setDriverMode((Boolean) newPropValue); + continue; + } + + // special case for camera settables + if (propName.startsWith("camera")) { + var propMethodName = "set" + propName.replace("camera", ""); + var methods = visionSource.getSettables().getClass().getMethods(); + for (var method : methods) { + if (method.getName().equalsIgnoreCase(propMethodName)) { + try { + method.invoke(visionSource.getSettables(), newPropValue); + } catch (Exception e) { + logger.error("Failed to invoke camera settable method: " + method.getName(), e); + } + } + } + } + + try { + setProperty(currentSettings, propName, newPropValue); + logger.trace("Set prop " + propName + " to value " + newPropValue); + } catch (NoSuchFieldException | IllegalAccessException e) { + logger.error( + "Could not set prop " + + propName + + " with value " + + newPropValue + + " on " + + currentSettings + + " | " + + e.getClass().getSimpleName()); + } catch (Exception e) { + logger.error("Unknown exception when setting PSC prop!", e); + } + + saveAndBroadcastSelective(originContext, propName, newPropValue); + } + settingChanges.clear(); + } + } + + /** + * Sets the value of a property in the given object using reflection. This method should not be + * used generally and is only known to be correct in the context of `onDataChangeEvent`. + * + * @param currentSettings The object whose property needs to be set. + * @param propName The name of the property to be set. + * @param newPropValue The new value to be assigned to the property. + * @throws IllegalAccessException If the field cannot be accessed. + * @throws NoSuchFieldException If the field does not exist. + * @throws Exception If an some other unknown exception occurs while setting the property. + */ + protected static void setProperty(Object currentSettings, String propName, Object newPropValue) + throws IllegalAccessException, NoSuchFieldException, Exception { + var propField = currentSettings.getClass().getField(propName); + var propType = propField.getType(); + + if (propType.isEnum()) { + var actual = propType.getEnumConstants()[(int) newPropValue]; + propField.set(currentSettings, actual); + } else if (propType.isAssignableFrom(DoubleCouple.class)) { + var orig = (ArrayList) newPropValue; + var actual = new DoubleCouple(orig.get(0), orig.get(1)); + propField.set(currentSettings, actual); + } else if (propType.isAssignableFrom(IntegerCouple.class)) { + var orig = (ArrayList) newPropValue; + var actual = new IntegerCouple(orig.get(0).intValue(), orig.get(1).intValue()); + propField.set(currentSettings, actual); + } else if (propType.equals(Double.TYPE)) { + propField.setDouble(currentSettings, ((Number) newPropValue).doubleValue()); + } else if (propType.equals(Integer.TYPE)) { + propField.setInt(currentSettings, (Integer) newPropValue); + } else if (propType.equals(Boolean.TYPE)) { + if (newPropValue instanceof Integer) { + propField.setBoolean(currentSettings, (Integer) newPropValue != 0); + } else { + propField.setBoolean(currentSettings, (Boolean) newPropValue); + } + } else { + propField.set(currentSettings, newPropValue); + } + } + public void start() { visionRunner.startProcess(); streamRunnable.start(); @@ -449,9 +675,11 @@ boolean setPipeline(int index) { } private boolean camShouldControlLEDs() { - // Heuristic - if the camera has a known FOV or is a piCam, assume it's in use for + // Heuristic - if the camera has a known FOV or is a piCam, assume it's in use + // for // vision processing, and should command stuff to the LED's. - // TODO: Make LED control a property of the camera itself and controllable in the UI. + // TODO: Make LED control a property of the camera itself and controllable in + // the UI. return isVendorCamera(); } diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChange.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChange.java new file mode 100644 index 0000000000..81b675952e --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChange.java @@ -0,0 +1,55 @@ +/* + * 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.vision.processes; + +import io.javalin.websocket.WsContext; +import org.photonvision.vision.pipeline.CVPipelineSettings; + +public class VisionModuleChange { + private final String propName; + private final T newPropValue; + private final CVPipelineSettings currentSettings; + private final WsContext originContext; + + public VisionModuleChange( + String propName, + T newPropValue, + CVPipelineSettings currentSettings, + WsContext originContext) { + this.propName = propName; + this.newPropValue = newPropValue; + this.currentSettings = currentSettings; + this.originContext = originContext; + } + + public String getPropName() { + return propName; + } + + public T getNewPropValue() { + return newPropValue; + } + + public CVPipelineSettings getCurrentSettings() { + return currentSettings; + } + + public WsContext getOriginContext() { + return originContext; + } +} diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java index 510665337b..db8e949b9d 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java @@ -17,23 +17,11 @@ package org.photonvision.vision.processes; -import java.util.ArrayList; -import java.util.Map; -import org.apache.commons.lang3.tuple.Pair; -import org.opencv.core.Point; import org.photonvision.common.dataflow.DataChangeSubscriber; import org.photonvision.common.dataflow.events.DataChangeEvent; import org.photonvision.common.dataflow.events.IncomingWebSocketEvent; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; -import org.photonvision.common.util.file.JacksonUtils; -import org.photonvision.common.util.numbers.DoubleCouple; -import org.photonvision.common.util.numbers.IntegerCouple; -import org.photonvision.vision.calibration.CameraCalibrationCoefficients; -import org.photonvision.vision.pipeline.AdvancedPipelineSettings; -import org.photonvision.vision.pipeline.PipelineType; -import org.photonvision.vision.pipeline.UICalibrationData; -import org.photonvision.vision.target.RobotOffsetPointOperation; @SuppressWarnings("unchecked") public class VisionModuleChangeSubscriber extends DataChangeSubscriber { @@ -58,210 +46,17 @@ public void onDataChangeEvent(DataChangeEvent event) { if (wsEvent.cameraIndex != null && (wsEvent.cameraIndex == parentModule.moduleIndex || wsEvent.cameraIndex == -1)) { logger.trace("Got PSC event - propName: " + wsEvent.propertyName); - - var propName = wsEvent.propertyName; - var newPropValue = wsEvent.data; - var currentSettings = parentModule.pipelineManager.getCurrentPipeline().getSettings(); - - // special case for non-PipelineSetting changes - switch (propName) { - // case "cameraNickname": // rename camera - // var newNickname = (String) newPropValue; - // logger.info("Changing nickname to " + newNickname); - // parentModule.setCameraNickname(newNickname); - // return; - case "pipelineName": // rename current pipeline - logger.info("Changing nick to " + newPropValue); - parentModule.pipelineManager.getCurrentPipelineSettings().pipelineNickname = - (String) newPropValue; - parentModule.saveAndBroadcastAll(); - return; - case "newPipelineInfo": // add new pipeline - var typeName = (Pair) newPropValue; - var type = typeName.getRight(); - var name = typeName.getLeft(); - - logger.info("Adding a " + type + " pipeline with name " + name); - - var addedSettings = parentModule.pipelineManager.addPipeline(type); - addedSettings.pipelineNickname = name; - parentModule.saveAndBroadcastAll(); - return; - case "deleteCurrPipeline": - var indexToDelete = parentModule.pipelineManager.getRequestedIndex(); - logger.info("Deleting current pipe at index " + indexToDelete); - int newIndex = parentModule.pipelineManager.removePipeline(indexToDelete); - parentModule.setPipeline(newIndex); - parentModule.saveAndBroadcastAll(); - return; - case "changePipeline": // change active pipeline - var index = (Integer) newPropValue; - if (index == parentModule.pipelineManager.getRequestedIndex()) { - logger.debug("Skipping pipeline change, index " + index + " already active"); - return; - } - parentModule.setPipeline(index); - parentModule.saveAndBroadcastAll(); - return; - case "startCalibration": - try { - var data = - JacksonUtils.deserialize( - (Map) newPropValue, UICalibrationData.class); - parentModule.startCalibration(data); - parentModule.saveAndBroadcastAll(); - } catch (Exception e) { - logger.error("Error deserailizing start-cal request", e); - } - return; - case "saveInputSnapshot": - parentModule.saveInputSnapshot(); - return; - case "saveOutputSnapshot": - parentModule.saveOutputSnapshot(); - return; - case "takeCalSnapshot": - parentModule.takeCalibrationSnapshot(); - return; - case "duplicatePipeline": - int idx = parentModule.pipelineManager.duplicatePipeline((Integer) newPropValue); - parentModule.setPipeline(idx); - parentModule.saveAndBroadcastAll(); - return; - case "calibrationUploaded": - if (newPropValue instanceof CameraCalibrationCoefficients) - parentModule.addCalibrationToConfig((CameraCalibrationCoefficients) newPropValue); - return; - case "robotOffsetPoint": - if (currentSettings instanceof AdvancedPipelineSettings) { - var curAdvSettings = (AdvancedPipelineSettings) currentSettings; - var offsetOperation = RobotOffsetPointOperation.fromIndex((int) newPropValue); - var latestTarget = parentModule.lastPipelineResultBestTarget; - - if (latestTarget != null) { - var newPoint = latestTarget.getTargetOffsetPoint(); - - switch (curAdvSettings.offsetRobotOffsetMode) { - case Single: - if (offsetOperation == RobotOffsetPointOperation.ROPO_CLEAR) { - curAdvSettings.offsetSinglePoint = new Point(); - } else if (offsetOperation == RobotOffsetPointOperation.ROPO_TAKESINGLE) { - curAdvSettings.offsetSinglePoint = newPoint; - } - break; - case Dual: - if (offsetOperation == RobotOffsetPointOperation.ROPO_CLEAR) { - curAdvSettings.offsetDualPointA = new Point(); - curAdvSettings.offsetDualPointAArea = 0; - curAdvSettings.offsetDualPointB = new Point(); - curAdvSettings.offsetDualPointBArea = 0; - } else { - // update point and area - switch (offsetOperation) { - case ROPO_TAKEFIRSTDUAL: - curAdvSettings.offsetDualPointA = newPoint; - curAdvSettings.offsetDualPointAArea = latestTarget.getArea(); - break; - case ROPO_TAKESECONDDUAL: - curAdvSettings.offsetDualPointB = newPoint; - curAdvSettings.offsetDualPointBArea = latestTarget.getArea(); - break; - default: - break; - } - } - break; - default: - break; - } - } - } - return; - case "changePipelineType": - parentModule.changePipelineType((Integer) newPropValue); - parentModule.saveAndBroadcastAll(); - return; - case "isDriverMode": - parentModule.setDriverMode((Boolean) newPropValue); - return; - } - - // special case for camera settables - if (propName.startsWith("camera")) { - var propMethodName = "set" + propName.replace("camera", ""); - var methods = parentModule.visionSource.getSettables().getClass().getMethods(); - for (var method : methods) { - if (method.getName().equalsIgnoreCase(propMethodName)) { - try { - method.invoke(parentModule.visionSource.getSettables(), newPropValue); - } catch (Exception e) { - logger.error("Failed to invoke camera settable method: " + method.getName(), e); - } - } - } + synchronized (parentModule.getSettingChanges()) { + parentModule + .getSettingChanges() + .add( + new VisionModuleChange( + wsEvent.propertyName, + wsEvent.data, + parentModule.pipelineManager.getCurrentPipeline().getSettings(), + wsEvent.originContext)); } - - try { - setProperty(currentSettings, propName, newPropValue); - logger.trace("Set prop " + propName + " to value " + newPropValue); - } catch (NoSuchFieldException | IllegalAccessException e) { - logger.error( - "Could not set prop " - + propName - + " with value " - + newPropValue - + " on " - + currentSettings - + " | " - + e.getClass().getSimpleName()); - } catch (Exception e) { - logger.error("Unknown exception when setting PSC prop!", e); - } - - parentModule.saveAndBroadcastSelective(wsEvent.originContext, propName, newPropValue); - } - } - } - - /** - * Sets the value of a property in the given object using reflection. This method should not be - * used generally and is only known to be correct in the context of `onDataChangeEvent`. - * - * @param currentSettings The object whose property needs to be set. - * @param propName The name of the property to be set. - * @param newPropValue The new value to be assigned to the property. - * @throws IllegalAccessException If the field cannot be accessed. - * @throws NoSuchFieldException If the field does not exist. - * @throws Exception If an some other unknown exception occurs while setting the property. - */ - protected static void setProperty(Object currentSettings, String propName, Object newPropValue) - throws IllegalAccessException, NoSuchFieldException, Exception { - var propField = currentSettings.getClass().getField(propName); - var propType = propField.getType(); - - if (propType.isEnum()) { - var actual = propType.getEnumConstants()[(int) newPropValue]; - propField.set(currentSettings, actual); - } else if (propType.isAssignableFrom(DoubleCouple.class)) { - var orig = (ArrayList) newPropValue; - var actual = new DoubleCouple(orig.get(0), orig.get(1)); - propField.set(currentSettings, actual); - } else if (propType.isAssignableFrom(IntegerCouple.class)) { - var orig = (ArrayList) newPropValue; - var actual = new IntegerCouple(orig.get(0).intValue(), orig.get(1).intValue()); - propField.set(currentSettings, actual); - } else if (propType.equals(Double.TYPE)) { - propField.setDouble(currentSettings, ((Number) newPropValue).doubleValue()); - } else if (propType.equals(Integer.TYPE)) { - propField.setInt(currentSettings, (Integer) newPropValue); - } else if (propType.equals(Boolean.TYPE)) { - if (newPropValue instanceof Integer) { - propField.setBoolean(currentSettings, (Integer) newPropValue != 0); - } else { - propField.setBoolean(currentSettings, (Boolean) newPropValue); } - } else { - propField.set(currentSettings, newPropValue); } } } diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java index 0dc211f2c4..786883c1a9 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java @@ -38,6 +38,7 @@ public class VisionRunner { private final Supplier pipelineSupplier; private final Consumer pipelineResultConsumer; private final QuirkyCamera cameraQuirks; + private final VisionModule parentModule; private long loopCount; @@ -53,15 +54,18 @@ public VisionRunner( FrameProvider frameSupplier, Supplier pipelineSupplier, Consumer pipelineResultConsumer, - QuirkyCamera cameraQuirks) { + QuirkyCamera cameraQuirks, + VisionModule parentModule) { this.frameSupplier = frameSupplier; this.pipelineSupplier = pipelineSupplier; this.pipelineResultConsumer = pipelineResultConsumer; this.cameraQuirks = cameraQuirks; + this.parentModule = parentModule; visionProcessThread = new Thread(this::update); visionProcessThread.setName("VisionRunner - " + frameSupplier.getName()); logger = new Logger(VisionRunner.class, frameSupplier.getName(), LogGroup.VisionModule); + parentModule.processSettingChanges(); } public void startProcess() { @@ -70,6 +74,7 @@ public void startProcess() { private void update() { while (!Thread.interrupted()) { + parentModule.processSettingChanges(); var pipeline = pipelineSupplier.get(); // Tell our camera implementation here what kind of pre-processing we need it to be doing From cf7070a55a6358ec7ad227fc044e52e916414952 Mon Sep 17 00:00:00 2001 From: "Cameron (3539)" Date: Tue, 8 Oct 2024 22:14:14 -0400 Subject: [PATCH 2/4] Less messy. --- .../vision/processes/VisionModule.java | 232 +---------------- .../VisionModuleChangeSubscriber.java | 237 +++++++++++++++++- .../vision/processes/VisionRunner.java | 10 +- 3 files changed, 246 insertions(+), 233 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java index d82353b028..a02b96f95f 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java @@ -25,11 +25,8 @@ import java.util.HashMap; import java.util.LinkedList; import java.util.List; -import java.util.Map; import java.util.function.BiConsumer; import java.util.stream.Collectors; -import org.apache.commons.lang3.tuple.Pair; -import org.opencv.core.Point; import org.opencv.core.Size; import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.configuration.ConfigManager; @@ -44,9 +41,6 @@ import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.util.SerializationUtils; -import org.photonvision.common.util.file.JacksonUtils; -import org.photonvision.common.util.numbers.DoubleCouple; -import org.photonvision.common.util.numbers.IntegerCouple; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.camera.CameraQuirk; import org.photonvision.vision.camera.CameraType; @@ -57,11 +51,9 @@ import org.photonvision.vision.frame.consumer.MJPGFrameConsumer; import org.photonvision.vision.pipeline.AdvancedPipelineSettings; import org.photonvision.vision.pipeline.OutputStreamPipeline; -import org.photonvision.vision.pipeline.PipelineType; import org.photonvision.vision.pipeline.ReflectivePipelineSettings; import org.photonvision.vision.pipeline.UICalibrationData; import org.photonvision.vision.pipeline.result.CVPipelineResult; -import org.photonvision.vision.target.RobotOffsetPointOperation; import org.photonvision.vision.target.TargetModel; import org.photonvision.vision.target.TrackedTarget; @@ -99,7 +91,7 @@ public class VisionModule { MJPGFrameConsumer inputVideoStreamer; MJPGFrameConsumer outputVideoStreamer; - private List> settingChanges = new ArrayList<>(); + private VisionModuleChangeSubscriber changeSubscriber; public VisionModule(PipelineManager pipelineManager, VisionSource visionSource, int index) { logger = @@ -132,17 +124,18 @@ public VisionModule(PipelineManager pipelineManager, VisionSource visionSource, this.pipelineManager = pipelineManager; this.visionSource = visionSource; + changeSubscriber = new VisionModuleChangeSubscriber(this); this.visionRunner = new VisionRunner( this.visionSource.getFrameProvider(), this.pipelineManager::getCurrentPipeline, this::consumeResult, this.cameraQuirks, - this); + this::getChangeSubscriber); this.streamRunnable = new StreamRunnable(new OutputStreamPipeline()); this.moduleIndex = index; - DataChangeService.getInstance().addSubscriber(new VisionModuleChangeSubscriber(this)); + DataChangeService.getInstance().addSubscriber(changeSubscriber); createStreams(); @@ -307,219 +300,6 @@ public void run() { } } - public List> getSettingChanges() { - return settingChanges; - } - - public void processSettingChanges() { - // special case for non-PipelineSetting changes - synchronized (getSettingChanges()) { - for (var change : settingChanges) { - var propName = change.getPropName(); - var newPropValue = change.getNewPropValue(); - var currentSettings = change.getCurrentSettings(); - var originContext = change.getOriginContext(); - switch (propName) { - // case "cameraNickname": // rename camera - // var newNickname = (String) newPropValue; - // logger.info("Changing nickname to " + newNickname); - // setCameraNickname(newNickname); - // return; - case "pipelineName": // rename current pipeline - logger.info("Changing nick to " + newPropValue); - pipelineManager.getCurrentPipelineSettings().pipelineNickname = (String) newPropValue; - saveAndBroadcastAll(); - continue; - case "newPipelineInfo": // add new pipeline - var typeName = (Pair) newPropValue; - var type = typeName.getRight(); - var name = typeName.getLeft(); - - logger.info("Adding a " + type + " pipeline with name " + name); - - var addedSettings = pipelineManager.addPipeline(type); - addedSettings.pipelineNickname = name; - saveAndBroadcastAll(); - continue; - case "deleteCurrPipeline": - var indexToDelete = pipelineManager.getRequestedIndex(); - logger.info("Deleting current pipe at index " + indexToDelete); - int newIndex = pipelineManager.removePipeline(indexToDelete); - setPipeline(newIndex); - saveAndBroadcastAll(); - continue; - case "changePipeline": // change active pipeline - var index = (Integer) newPropValue; - if (index == pipelineManager.getRequestedIndex()) { - logger.debug("Skipping pipeline change, index " + index + " already active"); - continue; - } - setPipeline(index); - saveAndBroadcastAll(); - continue; - case "startCalibration": - try { - var data = - JacksonUtils.deserialize( - (Map) newPropValue, UICalibrationData.class); - startCalibration(data); - saveAndBroadcastAll(); - } catch (Exception e) { - logger.error("Error deserailizing start-cal request", e); - } - continue; - case "saveInputSnapshot": - saveInputSnapshot(); - continue; - case "saveOutputSnapshot": - saveOutputSnapshot(); - continue; - case "takeCalSnapshot": - takeCalibrationSnapshot(); - continue; - case "duplicatePipeline": - int idx = pipelineManager.duplicatePipeline((Integer) newPropValue); - setPipeline(idx); - saveAndBroadcastAll(); - continue; - case "calibrationUploaded": - if (newPropValue instanceof CameraCalibrationCoefficients) - addCalibrationToConfig((CameraCalibrationCoefficients) newPropValue); - continue; - case "robotOffsetPoint": - if (currentSettings instanceof AdvancedPipelineSettings) { - var curAdvSettings = (AdvancedPipelineSettings) currentSettings; - var offsetOperation = RobotOffsetPointOperation.fromIndex((int) newPropValue); - var latestTarget = lastPipelineResultBestTarget; - - if (latestTarget != null) { - var newPoint = latestTarget.getTargetOffsetPoint(); - - switch (curAdvSettings.offsetRobotOffsetMode) { - case Single: - if (offsetOperation == RobotOffsetPointOperation.ROPO_CLEAR) { - curAdvSettings.offsetSinglePoint = new Point(); - } else if (offsetOperation == RobotOffsetPointOperation.ROPO_TAKESINGLE) { - curAdvSettings.offsetSinglePoint = newPoint; - } - break; - case Dual: - if (offsetOperation == RobotOffsetPointOperation.ROPO_CLEAR) { - curAdvSettings.offsetDualPointA = new Point(); - curAdvSettings.offsetDualPointAArea = 0; - curAdvSettings.offsetDualPointB = new Point(); - curAdvSettings.offsetDualPointBArea = 0; - } else { - // update point and area - switch (offsetOperation) { - case ROPO_TAKEFIRSTDUAL: - curAdvSettings.offsetDualPointA = newPoint; - curAdvSettings.offsetDualPointAArea = latestTarget.getArea(); - break; - case ROPO_TAKESECONDDUAL: - curAdvSettings.offsetDualPointB = newPoint; - curAdvSettings.offsetDualPointBArea = latestTarget.getArea(); - break; - default: - break; - } - } - break; - default: - break; - } - } - } - continue; - case "changePipelineType": - changePipelineType((Integer) newPropValue); - saveAndBroadcastAll(); - continue; - case "isDriverMode": - setDriverMode((Boolean) newPropValue); - continue; - } - - // special case for camera settables - if (propName.startsWith("camera")) { - var propMethodName = "set" + propName.replace("camera", ""); - var methods = visionSource.getSettables().getClass().getMethods(); - for (var method : methods) { - if (method.getName().equalsIgnoreCase(propMethodName)) { - try { - method.invoke(visionSource.getSettables(), newPropValue); - } catch (Exception e) { - logger.error("Failed to invoke camera settable method: " + method.getName(), e); - } - } - } - } - - try { - setProperty(currentSettings, propName, newPropValue); - logger.trace("Set prop " + propName + " to value " + newPropValue); - } catch (NoSuchFieldException | IllegalAccessException e) { - logger.error( - "Could not set prop " - + propName - + " with value " - + newPropValue - + " on " - + currentSettings - + " | " - + e.getClass().getSimpleName()); - } catch (Exception e) { - logger.error("Unknown exception when setting PSC prop!", e); - } - - saveAndBroadcastSelective(originContext, propName, newPropValue); - } - settingChanges.clear(); - } - } - - /** - * Sets the value of a property in the given object using reflection. This method should not be - * used generally and is only known to be correct in the context of `onDataChangeEvent`. - * - * @param currentSettings The object whose property needs to be set. - * @param propName The name of the property to be set. - * @param newPropValue The new value to be assigned to the property. - * @throws IllegalAccessException If the field cannot be accessed. - * @throws NoSuchFieldException If the field does not exist. - * @throws Exception If an some other unknown exception occurs while setting the property. - */ - protected static void setProperty(Object currentSettings, String propName, Object newPropValue) - throws IllegalAccessException, NoSuchFieldException, Exception { - var propField = currentSettings.getClass().getField(propName); - var propType = propField.getType(); - - if (propType.isEnum()) { - var actual = propType.getEnumConstants()[(int) newPropValue]; - propField.set(currentSettings, actual); - } else if (propType.isAssignableFrom(DoubleCouple.class)) { - var orig = (ArrayList) newPropValue; - var actual = new DoubleCouple(orig.get(0), orig.get(1)); - propField.set(currentSettings, actual); - } else if (propType.isAssignableFrom(IntegerCouple.class)) { - var orig = (ArrayList) newPropValue; - var actual = new IntegerCouple(orig.get(0).intValue(), orig.get(1).intValue()); - propField.set(currentSettings, actual); - } else if (propType.equals(Double.TYPE)) { - propField.setDouble(currentSettings, ((Number) newPropValue).doubleValue()); - } else if (propType.equals(Integer.TYPE)) { - propField.setInt(currentSettings, (Integer) newPropValue); - } else if (propType.equals(Boolean.TYPE)) { - if (newPropValue instanceof Integer) { - propField.setBoolean(currentSettings, (Integer) newPropValue != 0); - } else { - propField.setBoolean(currentSettings, (Boolean) newPropValue); - } - } else { - propField.set(currentSettings, newPropValue); - } - } - public void start() { visionRunner.startProcess(); streamRunnable.start(); @@ -541,6 +321,10 @@ private boolean isVendorCamera() { return visionSource.isVendorCamera(); } + public VisionModuleChangeSubscriber getChangeSubscriber() { + return changeSubscriber; + } + void changePipelineType(int newType) { pipelineManager.changePipelineType(newType); setPipeline(pipelineManager.getRequestedIndex()); diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java index db8e949b9d..ebdd309654 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java @@ -17,16 +17,31 @@ package org.photonvision.vision.processes; +import java.util.ArrayList; +import java.util.List; +import java.util.Map; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.Point; import org.photonvision.common.dataflow.DataChangeSubscriber; import org.photonvision.common.dataflow.events.DataChangeEvent; import org.photonvision.common.dataflow.events.IncomingWebSocketEvent; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; +import org.photonvision.common.util.file.JacksonUtils; +import org.photonvision.common.util.numbers.DoubleCouple; +import org.photonvision.common.util.numbers.IntegerCouple; +import org.photonvision.vision.calibration.CameraCalibrationCoefficients; +import org.photonvision.vision.pipeline.AdvancedPipelineSettings; +import org.photonvision.vision.pipeline.PipelineType; +import org.photonvision.vision.pipeline.UICalibrationData; +import org.photonvision.vision.target.RobotOffsetPointOperation; @SuppressWarnings("unchecked") public class VisionModuleChangeSubscriber extends DataChangeSubscriber { private final VisionModule parentModule; private final Logger logger; + private List> settingChanges = new ArrayList<>(); + private final Object m_mutex = new Object(); public VisionModuleChangeSubscriber(VisionModule parentModule) { this.parentModule = parentModule; @@ -42,13 +57,13 @@ public void onDataChangeEvent(DataChangeEvent event) { if (event instanceof IncomingWebSocketEvent) { var wsEvent = (IncomingWebSocketEvent) event; - // Camera index -1 means a "multicast event" (i.e. the event is received by all cameras) + // Camera index -1 means a "multicast event" (i.e. the event is received by all + // cameras) if (wsEvent.cameraIndex != null && (wsEvent.cameraIndex == parentModule.moduleIndex || wsEvent.cameraIndex == -1)) { logger.trace("Got PSC event - propName: " + wsEvent.propertyName); - synchronized (parentModule.getSettingChanges()) { - parentModule - .getSettingChanges() + synchronized (m_mutex) { + getSettingChanges() .add( new VisionModuleChange( wsEvent.propertyName, @@ -59,4 +74,218 @@ public void onDataChangeEvent(DataChangeEvent event) { } } } + + public List> getSettingChanges() { + return settingChanges; + } + + public void processSettingChanges() { + // special case for non-PipelineSetting changes + synchronized (m_mutex) { + for (var change : settingChanges) { + var propName = change.getPropName(); + var newPropValue = change.getNewPropValue(); + var currentSettings = change.getCurrentSettings(); + var originContext = change.getOriginContext(); + switch (propName) { + // case "cameraNickname": // rename camera + // var newNickname = (String) newPropValue; + // logger.info("Changing nickname to " + newNickname); + // setCameraNickname(newNickname); + // return; + case "pipelineName": // rename current pipeline + logger.info("Changing nick to " + newPropValue); + parentModule.pipelineManager.getCurrentPipelineSettings().pipelineNickname = + (String) newPropValue; + parentModule.saveAndBroadcastAll(); + continue; + case "newPipelineInfo": // add new pipeline + var typeName = (Pair) newPropValue; + var type = typeName.getRight(); + var name = typeName.getLeft(); + + logger.info("Adding a " + type + " pipeline with name " + name); + + var addedSettings = parentModule.pipelineManager.addPipeline(type); + addedSettings.pipelineNickname = name; + parentModule.saveAndBroadcastAll(); + continue; + case "deleteCurrPipeline": + var indexToDelete = parentModule.pipelineManager.getRequestedIndex(); + logger.info("Deleting current pipe at index " + indexToDelete); + int newIndex = parentModule.pipelineManager.removePipeline(indexToDelete); + parentModule.setPipeline(newIndex); + parentModule.saveAndBroadcastAll(); + continue; + case "changePipeline": // change active pipeline + var index = (Integer) newPropValue; + if (index == parentModule.pipelineManager.getRequestedIndex()) { + logger.debug("Skipping pipeline change, index " + index + " already active"); + continue; + } + parentModule.setPipeline(index); + parentModule.saveAndBroadcastAll(); + continue; + case "startCalibration": + try { + var data = + JacksonUtils.deserialize( + (Map) newPropValue, UICalibrationData.class); + parentModule.startCalibration(data); + parentModule.saveAndBroadcastAll(); + } catch (Exception e) { + logger.error("Error deserailizing start-cal request", e); + } + continue; + case "saveInputSnapshot": + parentModule.saveInputSnapshot(); + continue; + case "saveOutputSnapshot": + parentModule.saveOutputSnapshot(); + continue; + case "takeCalSnapshot": + parentModule.takeCalibrationSnapshot(); + continue; + case "duplicatePipeline": + int idx = parentModule.pipelineManager.duplicatePipeline((Integer) newPropValue); + parentModule.setPipeline(idx); + parentModule.saveAndBroadcastAll(); + continue; + case "calibrationUploaded": + if (newPropValue instanceof CameraCalibrationCoefficients) + parentModule.addCalibrationToConfig((CameraCalibrationCoefficients) newPropValue); + continue; + case "robotOffsetPoint": + if (currentSettings instanceof AdvancedPipelineSettings) { + var curAdvSettings = (AdvancedPipelineSettings) currentSettings; + var offsetOperation = RobotOffsetPointOperation.fromIndex((int) newPropValue); + var latestTarget = parentModule.lastPipelineResultBestTarget; + + if (latestTarget != null) { + var newPoint = latestTarget.getTargetOffsetPoint(); + + switch (curAdvSettings.offsetRobotOffsetMode) { + case Single: + if (offsetOperation == RobotOffsetPointOperation.ROPO_CLEAR) { + curAdvSettings.offsetSinglePoint = new Point(); + } else if (offsetOperation == RobotOffsetPointOperation.ROPO_TAKESINGLE) { + curAdvSettings.offsetSinglePoint = newPoint; + } + break; + case Dual: + if (offsetOperation == RobotOffsetPointOperation.ROPO_CLEAR) { + curAdvSettings.offsetDualPointA = new Point(); + curAdvSettings.offsetDualPointAArea = 0; + curAdvSettings.offsetDualPointB = new Point(); + curAdvSettings.offsetDualPointBArea = 0; + } else { + // update point and area + switch (offsetOperation) { + case ROPO_TAKEFIRSTDUAL: + curAdvSettings.offsetDualPointA = newPoint; + curAdvSettings.offsetDualPointAArea = latestTarget.getArea(); + break; + case ROPO_TAKESECONDDUAL: + curAdvSettings.offsetDualPointB = newPoint; + curAdvSettings.offsetDualPointBArea = latestTarget.getArea(); + break; + default: + break; + } + } + break; + default: + break; + } + } + } + continue; + case "changePipelineType": + parentModule.changePipelineType((Integer) newPropValue); + parentModule.saveAndBroadcastAll(); + continue; + case "isDriverMode": + parentModule.setDriverMode((Boolean) newPropValue); + continue; + } + + // special case for camera settables + if (propName.startsWith("camera")) { + var propMethodName = "set" + propName.replace("camera", ""); + var methods = parentModule.visionSource.getSettables().getClass().getMethods(); + for (var method : methods) { + if (method.getName().equalsIgnoreCase(propMethodName)) { + try { + method.invoke(parentModule.visionSource.getSettables(), newPropValue); + } catch (Exception e) { + logger.error("Failed to invoke camera settable method: " + method.getName(), e); + } + } + } + } + + try { + setProperty(currentSettings, propName, newPropValue); + logger.trace("Set prop " + propName + " to value " + newPropValue); + } catch (NoSuchFieldException | IllegalAccessException e) { + logger.error( + "Could not set prop " + + propName + + " with value " + + newPropValue + + " on " + + currentSettings + + " | " + + e.getClass().getSimpleName()); + } catch (Exception e) { + logger.error("Unknown exception when setting PSC prop!", e); + } + + parentModule.saveAndBroadcastSelective(originContext, propName, newPropValue); + } + getSettingChanges().clear(); + } + } + + /** + * Sets the value of a property in the given object using reflection. This method should not be + * used generally and is only known to be correct in the context of `onDataChangeEvent`. + * + * @param currentSettings The object whose property needs to be set. + * @param propName The name of the property to be set. + * @param newPropValue The new value to be assigned to the property. + * @throws IllegalAccessException If the field cannot be accessed. + * @throws NoSuchFieldException If the field does not exist. + * @throws Exception If an some other unknown exception occurs while setting the property. + */ + protected static void setProperty(Object currentSettings, String propName, Object newPropValue) + throws IllegalAccessException, NoSuchFieldException, Exception { + var propField = currentSettings.getClass().getField(propName); + var propType = propField.getType(); + + if (propType.isEnum()) { + var actual = propType.getEnumConstants()[(int) newPropValue]; + propField.set(currentSettings, actual); + } else if (propType.isAssignableFrom(DoubleCouple.class)) { + var orig = (ArrayList) newPropValue; + var actual = new DoubleCouple(orig.get(0), orig.get(1)); + propField.set(currentSettings, actual); + } else if (propType.isAssignableFrom(IntegerCouple.class)) { + var orig = (ArrayList) newPropValue; + var actual = new IntegerCouple(orig.get(0).intValue(), orig.get(1).intValue()); + propField.set(currentSettings, actual); + } else if (propType.equals(Double.TYPE)) { + propField.setDouble(currentSettings, ((Number) newPropValue).doubleValue()); + } else if (propType.equals(Integer.TYPE)) { + propField.setInt(currentSettings, (Integer) newPropValue); + } else if (propType.equals(Boolean.TYPE)) { + if (newPropValue instanceof Integer) { + propField.setBoolean(currentSettings, (Integer) newPropValue != 0); + } else { + propField.setBoolean(currentSettings, (Boolean) newPropValue); + } + } else { + propField.set(currentSettings, newPropValue); + } + } } diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java index 786883c1a9..e9ad4f7b8b 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java @@ -37,8 +37,8 @@ public class VisionRunner { private final FrameProvider frameSupplier; private final Supplier pipelineSupplier; private final Consumer pipelineResultConsumer; + private final Supplier changeSubSupplier; private final QuirkyCamera cameraQuirks; - private final VisionModule parentModule; private long loopCount; @@ -55,17 +55,17 @@ public VisionRunner( Supplier pipelineSupplier, Consumer pipelineResultConsumer, QuirkyCamera cameraQuirks, - VisionModule parentModule) { + Supplier changeSubSupplier) { this.frameSupplier = frameSupplier; this.pipelineSupplier = pipelineSupplier; this.pipelineResultConsumer = pipelineResultConsumer; this.cameraQuirks = cameraQuirks; - this.parentModule = parentModule; + this.changeSubSupplier = changeSubSupplier; visionProcessThread = new Thread(this::update); visionProcessThread.setName("VisionRunner - " + frameSupplier.getName()); logger = new Logger(VisionRunner.class, frameSupplier.getName(), LogGroup.VisionModule); - parentModule.processSettingChanges(); + changeSubSupplier.get().processSettingChanges(); } public void startProcess() { @@ -74,7 +74,7 @@ public void startProcess() { private void update() { while (!Thread.interrupted()) { - parentModule.processSettingChanges(); + changeSubSupplier.get().processSettingChanges(); var pipeline = pipelineSupplier.get(); // Tell our camera implementation here what kind of pre-processing we need it to be doing From 4fbffff7d0b3720a0ff099e6737030af2325e76a Mon Sep 17 00:00:00 2001 From: "Cameron (3539)" Date: Tue, 8 Oct 2024 22:50:35 -0400 Subject: [PATCH 3/4] small changes --- .../pipeline/ArucoPipelineSettings.java | 5 +- .../vision/processes/VisionModule.java | 14 ++--- .../VisionModuleChangeSubscriber.java | 52 ++++++++++--------- .../vision/processes/VisionRunner.java | 11 ++-- 4 files changed, 45 insertions(+), 37 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipelineSettings.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipelineSettings.java index f33ae44624..42987e3fa8 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipelineSettings.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipelineSettings.java @@ -17,11 +17,12 @@ package org.photonvision.vision.pipeline; -import com.fasterxml.jackson.annotation.JsonTypeName; import org.photonvision.common.util.numbers.IntegerCouple; import org.photonvision.vision.apriltag.AprilTagFamily; import org.photonvision.vision.target.TargetModel; +import com.fasterxml.jackson.annotation.JsonTypeName; + @JsonTypeName("ArucoPipelineSettings") public class ArucoPipelineSettings extends AdvancedPipelineSettings { public AprilTagFamily tagFamily = AprilTagFamily.kTag16h5; @@ -47,7 +48,7 @@ public ArucoPipelineSettings() { pipelineType = PipelineType.Aruco; outputShowMultipleTargets = true; targetModel = TargetModel.kAprilTag6p5in_36h11; - cameraExposureRaw = -1; + cameraExposureRaw = 20; cameraAutoExposure = true; ledMode = false; } diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java index a02b96f95f..9dd0a9523d 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java @@ -17,16 +17,13 @@ package org.photonvision.vision.processes; -import edu.wpi.first.cscore.CameraServerJNI; -import edu.wpi.first.cscore.VideoException; -import edu.wpi.first.math.util.Units; -import io.javalin.websocket.WsContext; import java.util.ArrayList; import java.util.HashMap; import java.util.LinkedList; import java.util.List; import java.util.function.BiConsumer; import java.util.stream.Collectors; + import org.opencv.core.Size; import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.configuration.ConfigManager; @@ -57,6 +54,11 @@ import org.photonvision.vision.target.TargetModel; import org.photonvision.vision.target.TrackedTarget; +import edu.wpi.first.cscore.CameraServerJNI; +import edu.wpi.first.cscore.VideoException; +import edu.wpi.first.math.util.Units; +import io.javalin.websocket.WsContext; + /** * This is the God Class * @@ -69,6 +71,7 @@ public class VisionModule { protected final VisionSource visionSource; private final VisionRunner visionRunner; private final StreamRunnable streamRunnable; + private final VisionModuleChangeSubscriber changeSubscriber; private final LinkedList resultConsumers = new LinkedList<>(); // Raw result consumers run before any drawing has been done by the // OutputStreamPipeline @@ -91,7 +94,6 @@ public class VisionModule { MJPGFrameConsumer inputVideoStreamer; MJPGFrameConsumer outputVideoStreamer; - private VisionModuleChangeSubscriber changeSubscriber; public VisionModule(PipelineManager pipelineManager, VisionSource visionSource, int index) { logger = @@ -131,7 +133,7 @@ public VisionModule(PipelineManager pipelineManager, VisionSource visionSource, this.pipelineManager::getCurrentPipeline, this::consumeResult, this.cameraQuirks, - this::getChangeSubscriber); + getChangeSubscriber()); this.streamRunnable = new StreamRunnable(new OutputStreamPipeline()); this.moduleIndex = index; diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java index ebdd309654..17af68f689 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java @@ -20,6 +20,8 @@ import java.util.ArrayList; import java.util.List; import java.util.Map; +import java.util.concurrent.locks.ReentrantLock; + import org.apache.commons.lang3.tuple.Pair; import org.opencv.core.Point; import org.photonvision.common.dataflow.DataChangeSubscriber; @@ -41,15 +43,14 @@ public class VisionModuleChangeSubscriber extends DataChangeSubscriber { private final VisionModule parentModule; private final Logger logger; private List> settingChanges = new ArrayList<>(); - private final Object m_mutex = new Object(); + private final ReentrantLock changeListLock = new ReentrantLock(); public VisionModuleChangeSubscriber(VisionModule parentModule) { this.parentModule = parentModule; - logger = - new Logger( - VisionModuleChangeSubscriber.class, - parentModule.visionSource.getSettables().getConfiguration().nickname, - LogGroup.VisionModule); + logger = new Logger( + VisionModuleChangeSubscriber.class, + parentModule.visionSource.getSettables().getConfiguration().nickname, + LogGroup.VisionModule); } @Override @@ -62,7 +63,8 @@ public void onDataChangeEvent(DataChangeEvent event) { if (wsEvent.cameraIndex != null && (wsEvent.cameraIndex == parentModule.moduleIndex || wsEvent.cameraIndex == -1)) { logger.trace("Got PSC event - propName: " + wsEvent.propertyName); - synchronized (m_mutex) { + changeListLock.lock(); + try { getSettingChanges() .add( new VisionModuleChange( @@ -70,6 +72,8 @@ public void onDataChangeEvent(DataChangeEvent event) { wsEvent.data, parentModule.pipelineManager.getCurrentPipeline().getSettings(), wsEvent.originContext)); + } finally { + changeListLock.unlock(); } } } @@ -81,22 +85,18 @@ public List> getSettingChanges() { public void processSettingChanges() { // special case for non-PipelineSetting changes - synchronized (m_mutex) { + changeListLock.lock(); + try { for (var change : settingChanges) { var propName = change.getPropName(); var newPropValue = change.getNewPropValue(); var currentSettings = change.getCurrentSettings(); var originContext = change.getOriginContext(); switch (propName) { - // case "cameraNickname": // rename camera - // var newNickname = (String) newPropValue; - // logger.info("Changing nickname to " + newNickname); - // setCameraNickname(newNickname); - // return; case "pipelineName": // rename current pipeline logger.info("Changing nick to " + newPropValue); - parentModule.pipelineManager.getCurrentPipelineSettings().pipelineNickname = - (String) newPropValue; + parentModule.pipelineManager + .getCurrentPipelineSettings().pipelineNickname = (String) newPropValue; parentModule.saveAndBroadcastAll(); continue; case "newPipelineInfo": // add new pipeline @@ -128,9 +128,8 @@ public void processSettingChanges() { continue; case "startCalibration": try { - var data = - JacksonUtils.deserialize( - (Map) newPropValue, UICalibrationData.class); + var data = JacksonUtils.deserialize( + (Map) newPropValue, UICalibrationData.class); parentModule.startCalibration(data); parentModule.saveAndBroadcastAll(); } catch (Exception e) { @@ -244,19 +243,24 @@ public void processSettingChanges() { parentModule.saveAndBroadcastSelective(originContext, propName, newPropValue); } getSettingChanges().clear(); + } finally { + changeListLock.unlock(); } } /** - * Sets the value of a property in the given object using reflection. This method should not be - * used generally and is only known to be correct in the context of `onDataChangeEvent`. + * Sets the value of a property in the given object using reflection. This + * method should not be + * used generally and is only known to be correct in the context of + * `onDataChangeEvent`. * * @param currentSettings The object whose property needs to be set. - * @param propName The name of the property to be set. - * @param newPropValue The new value to be assigned to the property. + * @param propName The name of the property to be set. + * @param newPropValue The new value to be assigned to the property. * @throws IllegalAccessException If the field cannot be accessed. - * @throws NoSuchFieldException If the field does not exist. - * @throws Exception If an some other unknown exception occurs while setting the property. + * @throws NoSuchFieldException If the field does not exist. + * @throws Exception If an some other unknown exception occurs + * while setting the property. */ protected static void setProperty(Object currentSettings, String propName, Object newPropValue) throws IllegalAccessException, NoSuchFieldException, Exception { diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java index e9ad4f7b8b..dbcb4a6447 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java @@ -19,6 +19,7 @@ import java.util.function.Consumer; import java.util.function.Supplier; + import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.vision.camera.QuirkyCamera; @@ -37,7 +38,7 @@ public class VisionRunner { private final FrameProvider frameSupplier; private final Supplier pipelineSupplier; private final Consumer pipelineResultConsumer; - private final Supplier changeSubSupplier; + private final VisionModuleChangeSubscriber changeSubscriber; private final QuirkyCamera cameraQuirks; private long loopCount; @@ -55,17 +56,17 @@ public VisionRunner( Supplier pipelineSupplier, Consumer pipelineResultConsumer, QuirkyCamera cameraQuirks, - Supplier changeSubSupplier) { + VisionModuleChangeSubscriber changeSubscriber) { this.frameSupplier = frameSupplier; this.pipelineSupplier = pipelineSupplier; this.pipelineResultConsumer = pipelineResultConsumer; this.cameraQuirks = cameraQuirks; - this.changeSubSupplier = changeSubSupplier; + this.changeSubscriber = changeSubscriber; visionProcessThread = new Thread(this::update); visionProcessThread.setName("VisionRunner - " + frameSupplier.getName()); logger = new Logger(VisionRunner.class, frameSupplier.getName(), LogGroup.VisionModule); - changeSubSupplier.get().processSettingChanges(); + changeSubscriber.processSettingChanges(); } public void startProcess() { @@ -74,7 +75,7 @@ public void startProcess() { private void update() { while (!Thread.interrupted()) { - changeSubSupplier.get().processSettingChanges(); + changeSubscriber.processSettingChanges(); var pipeline = pipelineSupplier.get(); // Tell our camera implementation here what kind of pre-processing we need it to be doing From dcaec8a127fd426b68199140b927ba3927ebd803 Mon Sep 17 00:00:00 2001 From: "Cameron (3539)" Date: Tue, 8 Oct 2024 22:54:12 -0400 Subject: [PATCH 4/4] lint and formatting --- .../pipeline/ArucoPipelineSettings.java | 3 +- .../vision/processes/VisionModule.java | 11 +++--- .../VisionModuleChangeSubscriber.java | 34 +++++++++---------- .../vision/processes/VisionRunner.java | 1 - 4 files changed, 21 insertions(+), 28 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipelineSettings.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipelineSettings.java index 42987e3fa8..54fdc584e0 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipelineSettings.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipelineSettings.java @@ -17,12 +17,11 @@ package org.photonvision.vision.pipeline; +import com.fasterxml.jackson.annotation.JsonTypeName; import org.photonvision.common.util.numbers.IntegerCouple; import org.photonvision.vision.apriltag.AprilTagFamily; import org.photonvision.vision.target.TargetModel; -import com.fasterxml.jackson.annotation.JsonTypeName; - @JsonTypeName("ArucoPipelineSettings") public class ArucoPipelineSettings extends AdvancedPipelineSettings { public AprilTagFamily tagFamily = AprilTagFamily.kTag16h5; diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java index 9dd0a9523d..6fd00c363b 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java @@ -17,13 +17,16 @@ package org.photonvision.vision.processes; +import edu.wpi.first.cscore.CameraServerJNI; +import edu.wpi.first.cscore.VideoException; +import edu.wpi.first.math.util.Units; +import io.javalin.websocket.WsContext; import java.util.ArrayList; import java.util.HashMap; import java.util.LinkedList; import java.util.List; import java.util.function.BiConsumer; import java.util.stream.Collectors; - import org.opencv.core.Size; import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.configuration.ConfigManager; @@ -54,11 +57,6 @@ import org.photonvision.vision.target.TargetModel; import org.photonvision.vision.target.TrackedTarget; -import edu.wpi.first.cscore.CameraServerJNI; -import edu.wpi.first.cscore.VideoException; -import edu.wpi.first.math.util.Units; -import io.javalin.websocket.WsContext; - /** * This is the God Class * @@ -94,7 +92,6 @@ public class VisionModule { MJPGFrameConsumer inputVideoStreamer; MJPGFrameConsumer outputVideoStreamer; - public VisionModule(PipelineManager pipelineManager, VisionSource visionSource, int index) { logger = new Logger( diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java index 17af68f689..3737f611a7 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java @@ -21,7 +21,6 @@ import java.util.List; import java.util.Map; import java.util.concurrent.locks.ReentrantLock; - import org.apache.commons.lang3.tuple.Pair; import org.opencv.core.Point; import org.photonvision.common.dataflow.DataChangeSubscriber; @@ -47,10 +46,11 @@ public class VisionModuleChangeSubscriber extends DataChangeSubscriber { public VisionModuleChangeSubscriber(VisionModule parentModule) { this.parentModule = parentModule; - logger = new Logger( - VisionModuleChangeSubscriber.class, - parentModule.visionSource.getSettables().getConfiguration().nickname, - LogGroup.VisionModule); + logger = + new Logger( + VisionModuleChangeSubscriber.class, + parentModule.visionSource.getSettables().getConfiguration().nickname, + LogGroup.VisionModule); } @Override @@ -95,8 +95,8 @@ public void processSettingChanges() { switch (propName) { case "pipelineName": // rename current pipeline logger.info("Changing nick to " + newPropValue); - parentModule.pipelineManager - .getCurrentPipelineSettings().pipelineNickname = (String) newPropValue; + parentModule.pipelineManager.getCurrentPipelineSettings().pipelineNickname = + (String) newPropValue; parentModule.saveAndBroadcastAll(); continue; case "newPipelineInfo": // add new pipeline @@ -128,8 +128,9 @@ public void processSettingChanges() { continue; case "startCalibration": try { - var data = JacksonUtils.deserialize( - (Map) newPropValue, UICalibrationData.class); + var data = + JacksonUtils.deserialize( + (Map) newPropValue, UICalibrationData.class); parentModule.startCalibration(data); parentModule.saveAndBroadcastAll(); } catch (Exception e) { @@ -249,18 +250,15 @@ public void processSettingChanges() { } /** - * Sets the value of a property in the given object using reflection. This - * method should not be - * used generally and is only known to be correct in the context of - * `onDataChangeEvent`. + * Sets the value of a property in the given object using reflection. This method should not be + * used generally and is only known to be correct in the context of `onDataChangeEvent`. * * @param currentSettings The object whose property needs to be set. - * @param propName The name of the property to be set. - * @param newPropValue The new value to be assigned to the property. + * @param propName The name of the property to be set. + * @param newPropValue The new value to be assigned to the property. * @throws IllegalAccessException If the field cannot be accessed. - * @throws NoSuchFieldException If the field does not exist. - * @throws Exception If an some other unknown exception occurs - * while setting the property. + * @throws NoSuchFieldException If the field does not exist. + * @throws Exception If an some other unknown exception occurs while setting the property. */ protected static void setProperty(Object currentSettings, String propName, Object newPropValue) throws IllegalAccessException, NoSuchFieldException, Exception { diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java index dbcb4a6447..a1299bd547 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java @@ -19,7 +19,6 @@ import java.util.function.Consumer; import java.util.function.Supplier; - import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.vision.camera.QuirkyCamera;