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..15fd5590b4 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 @@ -31,6 +31,7 @@ import org.photonvision.common.util.numbers.IntegerCouple; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.pipeline.AdvancedPipelineSettings; +import org.photonvision.vision.pipeline.CVPipelineSettings; import org.photonvision.vision.pipeline.PipelineType; import org.photonvision.vision.pipeline.UICalibrationData; import org.photonvision.vision.target.RobotOffsetPointOperation; @@ -51,139 +52,45 @@ public VisionModuleChangeSubscriber(VisionModule parentModule) { @Override public void onDataChangeEvent(DataChangeEvent event) { - if (event instanceof IncomingWebSocketEvent) { - var wsEvent = (IncomingWebSocketEvent) event; - + if (event instanceof IncomingWebSocketEvent wsEvent) { // 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); - var propName = wsEvent.propertyName; - var newPropValue = wsEvent.data; - var currentSettings = parentModule.pipelineManager.getCurrentPipeline().getSettings(); + String propName = wsEvent.propertyName; + Object newPropValue = wsEvent.data; + CVPipelineSettings 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; + case "pipelineName" -> newPipelineNickname((String) newPropValue); + case "newPipelineInfo" -> newPipelineInfo((Pair) newPropValue); + case "deleteCurrPipeline" -> deleteCurrPipeline(); + case "changePipeline" -> changePipeline((Integer) newPropValue); + case "startCalibration" -> startCalibration((Map) newPropValue); + case "saveInputSnapshot" -> parentModule.saveInputSnapshot(); + case "saveOutputSnapshot" -> parentModule.saveOutputSnapshot(); + case "takeCalSnapshot" -> parentModule.takeCalibrationSnapshot(); + case "duplicatePipeline" -> duplicatePipeline((Integer) newPropValue); + case "calibrationUploaded" -> { + if (newPropValue instanceof CameraCalibrationCoefficients newCal) { + parentModule.addCalibrationToConfig(newCal); + } else { + logger.warn("Received invalid calibration data"); } - 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; - } - } + } + case "robotOffsetPoint" -> { + if (currentSettings instanceof AdvancedPipelineSettings curAdvSettings) { + robotOffsetPoint(curAdvSettings, (Integer) newPropValue); } - return; - case "changePipelineType": + } + case "changePipelineType" -> { parentModule.changePipelineType((Integer) newPropValue); parentModule.saveAndBroadcastAll(); - return; - case "isDriverMode": - parentModule.setDriverMode((Boolean) newPropValue); - return; + } + case "isDriverMode" -> parentModule.setDriverMode((Boolean) newPropValue); } // special case for camera settables @@ -223,6 +130,104 @@ public void onDataChangeEvent(DataChangeEvent event) { } } + public void newPipelineNickname(String newNickname) { + logger.info("Changing pipeline nickname to " + newNickname); + parentModule.pipelineManager.getCurrentPipelineSettings().pipelineNickname = newNickname; + parentModule.saveAndBroadcastAll(); + } + + public void newPipelineInfo(Pair typeName) { + 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(); + } + + public void 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(); + } + + public void changePipeline(int index) { + if (index == parentModule.pipelineManager.getRequestedIndex()) { + logger.debug("Skipping pipeline change, index " + index + " already active"); + return; + } + parentModule.setPipeline(index); + parentModule.saveAndBroadcastAll(); + } + + public void startCalibration(Map data) { + try { + var deserialized = JacksonUtils.deserialize(data, UICalibrationData.class); + parentModule.startCalibration(deserialized); + parentModule.saveAndBroadcastAll(); + } catch (Exception e) { + logger.error("Error deserailizing start-calibration request", e); + } + } + + public void duplicatePipeline(int index) { + var newIndex = parentModule.pipelineManager.duplicatePipeline(index); + parentModule.setPipeline(newIndex); + parentModule.saveAndBroadcastAll(); + } + + public void robotOffsetPoint(AdvancedPipelineSettings curAdvSettings, int offsetIndex) { + RobotOffsetPointOperation offsetOperation = RobotOffsetPointOperation.fromIndex(offsetIndex); + + var latestTarget = parentModule.lastPipelineResultBestTarget; + if (latestTarget == null) { + return; + } + + var newPoint = latestTarget.getTargetOffsetPoint(); + switch (curAdvSettings.offsetRobotOffsetMode) { + case Single -> { + switch (offsetOperation) { + case CLEAR -> curAdvSettings.offsetSinglePoint = new Point(); + case TAKE_SINGLE -> curAdvSettings.offsetSinglePoint = newPoint; + case TAKE_FIRST_DUAL, TAKE_SECOND_DUAL -> { + logger.warn("Dual point operation in single point mode"); + } + } + } + case Dual -> { + switch (offsetOperation) { + case CLEAR -> { + curAdvSettings.offsetDualPointA = new Point(); + curAdvSettings.offsetDualPointAArea = 0; + curAdvSettings.offsetDualPointB = new Point(); + curAdvSettings.offsetDualPointBArea = 0; + } + case TAKE_FIRST_DUAL -> { + // update point and area + curAdvSettings.offsetDualPointA = newPoint; + curAdvSettings.offsetDualPointAArea = latestTarget.getArea(); + } + case TAKE_SECOND_DUAL -> { + // update point and area + curAdvSettings.offsetDualPointB = newPoint; + curAdvSettings.offsetDualPointBArea = latestTarget.getArea(); + } + case TAKE_SINGLE -> { + logger.warn("Single point operation in dual point mode"); + } + } + } + case None -> { + logger.warn("Robot offset point operation requested, but no offset mode set"); + } + } + } + /** * 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`. @@ -255,8 +260,8 @@ protected static void setProperty(Object currentSettings, String propName, Objec } 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); + if (newPropValue instanceof Integer intValue) { + propField.setBoolean(currentSettings, intValue != 0); } else { propField.setBoolean(currentSettings, (Boolean) newPropValue); } diff --git a/photon-core/src/main/java/org/photonvision/vision/target/RobotOffsetPointOperation.java b/photon-core/src/main/java/org/photonvision/vision/target/RobotOffsetPointOperation.java index 0e69fe9d8f..d5aefd4328 100644 --- a/photon-core/src/main/java/org/photonvision/vision/target/RobotOffsetPointOperation.java +++ b/photon-core/src/main/java/org/photonvision/vision/target/RobotOffsetPointOperation.java @@ -17,10 +17,10 @@ package org.photonvision.vision.target; public enum RobotOffsetPointOperation { - ROPO_CLEAR(0), - ROPO_TAKESINGLE(1), - ROPO_TAKEFIRSTDUAL(2), - ROPO_TAKESECONDDUAL(3); + CLEAR(0), + TAKE_SINGLE(1), + TAKE_FIRST_DUAL(2), + TAKE_SECOND_DUAL(3); public final int index; @@ -29,17 +29,12 @@ public enum RobotOffsetPointOperation { } public static RobotOffsetPointOperation fromIndex(int index) { - switch (index) { - case 0: - return ROPO_CLEAR; - case 1: - return ROPO_TAKESINGLE; - case 2: - return ROPO_TAKEFIRSTDUAL; - case 3: - return ROPO_TAKESECONDDUAL; - default: - return ROPO_CLEAR; - } + return switch (index) { + case 0 -> CLEAR; + case 1 -> TAKE_SINGLE; + case 2 -> TAKE_FIRST_DUAL; + case 3 -> TAKE_SECOND_DUAL; + default -> CLEAR; + }; } }