diff --git a/photon-server/src/main/java/org/photonvision/Main.java b/photon-server/src/main/java/org/photonvision/Main.java index a08deecee..5d9b2eeb3 100644 --- a/photon-server/src/main/java/org/photonvision/Main.java +++ b/photon-server/src/main/java/org/photonvision/Main.java @@ -22,7 +22,7 @@ import java.util.HashMap; import java.util.List; import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.configuration.ConfigManager; -import org.photonvision.common.datatransfer.networktables.NetworkTablesManager; +import org.photonvision.common.dataflow.networktables.NetworkTablesManager; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.LogLevel; import org.photonvision.common.logging.Logger; diff --git a/photon-server/src/main/java/org/photonvision/common/dataflow/DataProvider.java b/photon-server/src/main/java/org/photonvision/common/dataflow/CVPipelineResultConsumer.java similarity index 80% rename from photon-server/src/main/java/org/photonvision/common/dataflow/DataProvider.java rename to photon-server/src/main/java/org/photonvision/common/dataflow/CVPipelineResultConsumer.java index 3bfdb96ee..f588b73db 100644 --- a/photon-server/src/main/java/org/photonvision/common/dataflow/DataProvider.java +++ b/photon-server/src/main/java/org/photonvision/common/dataflow/CVPipelineResultConsumer.java @@ -17,4 +17,7 @@ package org.photonvision.common.dataflow; -public interface DataProvider {} +import java.util.function.Consumer; +import org.photonvision.vision.pipeline.result.CVPipelineResult; + +public interface CVPipelineResultConsumer extends Consumer {} diff --git a/photon-server/src/main/java/org/photonvision/common/dataflow/DataChangeService.java b/photon-server/src/main/java/org/photonvision/common/dataflow/DataChangeService.java index eaf200cd0..189a75ee5 100644 --- a/photon-server/src/main/java/org/photonvision/common/dataflow/DataChangeService.java +++ b/photon-server/src/main/java/org/photonvision/common/dataflow/DataChangeService.java @@ -48,7 +48,7 @@ public class DataChangeService { private DataChangeService() { subscribers = new CopyOnWriteArrayList<>(); dispatchThread = new Thread(this::dispatchFromQueue); - dispatchThread.setName("EventDispatchThread"); + dispatchThread.setName("DataChangeEventDispatchThread"); dispatchThread.start(); } diff --git a/photon-server/src/main/java/org/photonvision/common/dataflow/camera/IncomingCameraCommandSubscriber.java b/photon-server/src/main/java/org/photonvision/common/dataflow/camera/IncomingCameraCommandSubscriber.java deleted file mode 100644 index 4a0ed09f2..000000000 --- a/photon-server/src/main/java/org/photonvision/common/dataflow/camera/IncomingCameraCommandSubscriber.java +++ /dev/null @@ -1,42 +0,0 @@ -/* - * 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.camera; - -import org.photonvision.common.dataflow.DataChangeSubscriber; -import org.photonvision.common.dataflow.events.DataChangeEvent; -import org.photonvision.common.logging.LogGroup; -import org.photonvision.common.logging.Logger; -import org.photonvision.vision.processes.VisionModuleManager; - -public class IncomingCameraCommandSubscriber extends DataChangeSubscriber { - private static final Logger logger = - new Logger(IncomingCameraCommandSubscriber.class, LogGroup.Camera); - - private final VisionModuleManager vmm; - - public IncomingCameraCommandSubscriber(VisionModuleManager instance) { - this.vmm = instance; - } - - @Override - public void onDataChangeEvent(DataChangeEvent event) { - // logger.trace("Got event from [" + event.sourceType + "] and dest [" + event.destType - // + "] with property name [" + event.propertyName - // + "] and value [" + event.data + "]"); - } -} diff --git a/photon-server/src/main/java/org/photonvision/common/datatransfer/DataConsumer.java b/photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NTDataChangeListener.java similarity index 51% rename from photon-server/src/main/java/org/photonvision/common/datatransfer/DataConsumer.java rename to photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NTDataChangeListener.java index 6018e92bf..54acebe7d 100644 --- a/photon-server/src/main/java/org/photonvision/common/datatransfer/DataConsumer.java +++ b/photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NTDataChangeListener.java @@ -15,9 +15,25 @@ * along with this program. If not, see . */ -package org.photonvision.common.datatransfer; +package org.photonvision.common.dataflow.networktables; +import edu.wpi.first.networktables.EntryListenerFlags; +import edu.wpi.first.networktables.EntryNotification; +import edu.wpi.first.networktables.NetworkTableEntry; import java.util.function.Consumer; -import org.photonvision.vision.processes.Data; -public interface DataConsumer extends Consumer {} +public class NTDataChangeListener { + + private final NetworkTableEntry watchedEntry; + private final int listenerID; + + public NTDataChangeListener( + NetworkTableEntry watchedEntry, Consumer dataChangeConsumer) { + this.watchedEntry = watchedEntry; + listenerID = watchedEntry.addListener(dataChangeConsumer, EntryListenerFlags.kUpdate); + } + + public void remove() { + watchedEntry.removeListener(listenerID); + } +} diff --git a/photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NTDataConsumer.java b/photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NTDataConsumer.java deleted file mode 100644 index 73bf8a47c..000000000 --- a/photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NTDataConsumer.java +++ /dev/null @@ -1,50 +0,0 @@ -/* - * 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.networktables; - -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import org.photonvision.common.datatransfer.DataConsumer; -import org.photonvision.vision.pipeline.result.SimplePipelineResult; -import org.photonvision.vision.processes.Data; - -public class NTDataConsumer implements DataConsumer { - - private final NetworkTable rootTable; - NetworkTable subTable; - NetworkTableEntry rawData; - - public NTDataConsumer(NetworkTable root, String camName) { - this.rootTable = root; - this.subTable = root.getSubTable(camName); - rawData = subTable.getEntry("rawData"); - } - - public void setCameraName(String camName) { - this.subTable = rootTable.getSubTable(camName); - rawData = subTable.getEntry("rawData"); - } - - @Override - public void accept(Data data) { - var simplified = new SimplePipelineResult(data.result); - var bytes = simplified.toByteArray(); - rawData.setRaw(bytes); - rootTable.getInstance().flush(); - } -} diff --git a/photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java new file mode 100644 index 000000000..cfe4b60e2 --- /dev/null +++ b/photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java @@ -0,0 +1,164 @@ +/* + * 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.networktables; + +import edu.wpi.first.networktables.EntryNotification; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import java.util.function.BooleanSupplier; +import java.util.function.Consumer; +import java.util.function.Supplier; +import org.photonvision.common.dataflow.CVPipelineResultConsumer; +import org.photonvision.vision.pipeline.result.CVPipelineResult; +import org.photonvision.vision.pipeline.result.SimplePipelineResult; + +public class NTDataPublisher implements CVPipelineResultConsumer { + + private final NetworkTable rootTable = NetworkTablesManager.kRootTable; + private NetworkTable subTable; + private NetworkTableEntry rawBytesEntry; + + private NetworkTableEntry pipelineIndexEntry; + private final Consumer pipelineIndexConsumer; + private NTDataChangeListener pipelineIndexListener; + private NetworkTableEntry driverModeEntry; + private final Consumer driverModeConsumer; + private NTDataChangeListener driverModeListener; + + private NetworkTableEntry latencyMillisEntry; + private NetworkTableEntry hasTargetEntry; + private NetworkTableEntry targetPitchEntry; + private NetworkTableEntry targetYawEntry; + private NetworkTableEntry targetAreaEntry; + private NetworkTableEntry targetPoseEntry; + + private final Supplier pipelineIndexSupplier; + private final BooleanSupplier driverModeSupplier; + + private String currentCameraNickname; + + public NTDataPublisher( + String cameraNickname, + Supplier pipelineIndexSupplier, + Consumer pipelineIndexConsumer, + BooleanSupplier driverModeSupplier, + Consumer driverModeConsumer) { + this.pipelineIndexSupplier = pipelineIndexSupplier; + this.pipelineIndexConsumer = pipelineIndexConsumer; + this.driverModeSupplier = driverModeSupplier; + this.driverModeConsumer = driverModeConsumer; + + currentCameraNickname = cameraNickname; + updateCameraNickname(cameraNickname); + updateEntries(); + } + + private void onPipelineIndexChange(EntryNotification entryNotification) { + var newIndex = (int) entryNotification.value.getDouble(); + var originalIndex = pipelineIndexSupplier.get(); + + if (newIndex == originalIndex) { + // TODO: Log + return; + } + + pipelineIndexConsumer.accept(newIndex); + var setIndex = pipelineIndexSupplier.get(); + if (newIndex != setIndex) { // set failed + pipelineIndexEntry.forceSetNumber(setIndex); + // TODO: Log + } + // TODO: Log + } + + private void onDriverModeChange(EntryNotification entryNotification) { + var newDriverMode = entryNotification.value.getBoolean(); + var originalDriverMode = driverModeSupplier.getAsBoolean(); + + if (newDriverMode == originalDriverMode) { + // TODO: Log + return; + } + + driverModeConsumer.accept(newDriverMode); + // TODO: Log + } + + private void updateEntries() { + rawBytesEntry = subTable.getEntry("rawBytes"); + + if (pipelineIndexListener != null) { + pipelineIndexListener.remove(); + } + pipelineIndexEntry = subTable.getEntry("pipelineIndex"); + pipelineIndexListener = + new NTDataChangeListener(pipelineIndexEntry, this::onPipelineIndexChange); + + if (driverModeListener != null) { + driverModeListener.remove(); + } + driverModeEntry = subTable.getEntry("driverMode"); + driverModeListener = new NTDataChangeListener(driverModeEntry, this::onDriverModeChange); + + latencyMillisEntry = subTable.getEntry("latencyMillis"); + hasTargetEntry = subTable.getEntry("hasTarget"); + + targetPitchEntry = subTable.getEntry("targetPitch"); + targetAreaEntry = subTable.getEntry("targetArea"); + targetYawEntry = subTable.getEntry("targetYaw"); + targetPoseEntry = subTable.getEntry("targetPose"); + } + + public void updateCameraNickname(String newCameraNickname) { + rootTable.delete(currentCameraNickname); // TODO: make this actually work (if possible) + subTable = rootTable.getSubTable(newCameraNickname); + updateEntries(); + currentCameraNickname = newCameraNickname; + } + + @Override + public void accept(CVPipelineResult result) { + var simplified = new SimplePipelineResult(result); + var bytes = simplified.toByteArray(); + rawBytesEntry.forceSetRaw(bytes); + + pipelineIndexEntry.forceSetNumber(pipelineIndexSupplier.get()); + driverModeEntry.forceSetBoolean(driverModeSupplier.getAsBoolean()); + latencyMillisEntry.forceSetDouble(result.getLatencyMillis()); + hasTargetEntry.forceSetBoolean(result.hasTargets()); + + if (result.hasTargets()) { + var bestTarget = result.targets.get(0); + + targetPitchEntry.forceSetDouble(bestTarget.getPitch()); + targetYawEntry.forceSetDouble(bestTarget.getYaw()); + targetAreaEntry.forceSetDouble(bestTarget.getArea()); + + var poseX = bestTarget.getRobotRelativePose().getTranslation().getX(); + var poseY = bestTarget.getRobotRelativePose().getTranslation().getY(); + var poseRot = bestTarget.getRobotRelativePose().getRotation().getDegrees(); + targetPoseEntry.forceSetDoubleArray(new double[] {poseX, poseY, poseRot}); + } else { + targetPitchEntry.forceSetDouble(0); + targetYawEntry.forceSetDouble(0); + targetAreaEntry.forceSetDouble(0); + targetPoseEntry.forceSetDoubleArray(new double[] {0, 0, 0}); + } + rootTable.getInstance().flush(); + } +} diff --git a/photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java b/photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java index 7ae415370..0fe991176 100644 --- a/photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java +++ b/photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java @@ -21,6 +21,7 @@ import edu.wpi.first.networktables.LogMessage; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import java.util.function.Consumer; +import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.scripting.ScriptEventType; @@ -34,30 +35,31 @@ public class NetworkTablesManager { private static final NetworkTableInstance ntInstance = NetworkTableInstance.getDefault(); - public static final String kRootTableName = "/chameleon-vision"; + public static final String kRootTableName = "/photonvision"; public static final NetworkTable kRootTable = NetworkTableInstance.getDefault().getTable(kRootTableName); public static boolean isServer = false; private static int getTeamNumber() { - // TODO: FIX - return 0; - // return ConfigManager.settings.teamNumber; + return ConfigManager.getInstance().getConfig().getNetworkConfig().teamNumber; } private static class NTLogger implements Consumer { private boolean hasReportedConnectionFailure = false; + private long lastConnectMessageMillis = 0; @Override public void accept(LogMessage logMessage) { if (!hasReportedConnectionFailure && logMessage.message.contains("timed out")) { logger.error("NT Connection has failed! Will retry in background."); hasReportedConnectionFailure = true; - } else if (logMessage.message.contains("connected")) { + } else if (logMessage.message.contains("connected") + && System.currentTimeMillis() - lastConnectMessageMillis > 125) { logger.info("NT Connected!"); hasReportedConnectionFailure = false; + lastConnectMessageMillis = System.currentTimeMillis(); ScriptManager.queueEvent(ScriptEventType.kNTConnected); } } diff --git a/photon-server/src/main/java/org/photonvision/common/datatransfer/DataProvider.java b/photon-server/src/main/java/org/photonvision/common/datatransfer/DataProvider.java deleted file mode 100644 index 7b5b997ae..000000000 --- a/photon-server/src/main/java/org/photonvision/common/datatransfer/DataProvider.java +++ /dev/null @@ -1,20 +0,0 @@ -/* - * 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.datatransfer; - -public interface DataProvider {} diff --git a/photon-server/src/main/java/org/photonvision/common/datatransfer/networktables/NetworkTablesManager.java b/photon-server/src/main/java/org/photonvision/common/datatransfer/networktables/NetworkTablesManager.java deleted file mode 100644 index ffd34ebae..000000000 --- a/photon-server/src/main/java/org/photonvision/common/datatransfer/networktables/NetworkTablesManager.java +++ /dev/null @@ -1,96 +0,0 @@ -/* - * 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.datatransfer.networktables; - -import edu.wpi.first.networktables.LogMessage; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import java.util.function.Consumer; -import org.photonvision.common.configuration.ConfigManager; -import org.photonvision.common.logging.LogGroup; -import org.photonvision.common.logging.Logger; -import org.photonvision.common.scripting.ScriptEventType; -import org.photonvision.common.scripting.ScriptManager; - -public class NetworkTablesManager { - - private NetworkTablesManager() {} - - private static final Logger logger = new Logger(NetworkTablesManager.class, LogGroup.General); - - private static final NetworkTableInstance ntInstance = NetworkTableInstance.getDefault(); - - public static final String kRootTableName = "/photonvision"; - public static final NetworkTable kRootTable = - NetworkTableInstance.getDefault().getTable(kRootTableName); - - public static boolean isServer = false; - - private static int getTeamNumber() { - return ConfigManager.getInstance().getConfig().getNetworkConfig().teamNumber; - } - - private static class NTLogger implements Consumer { - - private boolean hasReportedConnectionFailure = false; - - @Override - public void accept(LogMessage logMessage) { - if (!hasReportedConnectionFailure && logMessage.message.contains("timed out")) { - logger.error("NT Connection has failed! Will retry in background."); - hasReportedConnectionFailure = true; - } else if (logMessage.message.contains("connected")) { - logger.info("NT Connected!"); - hasReportedConnectionFailure = false; - ScriptManager.queueEvent(ScriptEventType.kNTConnected); - } - } - } - - static { - NetworkTableInstance.getDefault().addLogger(new NTLogger(), 0, 255); // to hide error messages - } - - public static void setClientMode(String host) { - isServer = false; - logger.info("Starting NT Client"); - ntInstance.stopServer(); - if (host != null) { - ntInstance.startClient(host); - } else { - ntInstance.startClientTeam(getTeamNumber()); - if (ntInstance.isConnected()) { - logger.info("[NetworkTablesManager] Connected to the robot!"); - } else { - logger.info( - "[NetworkTablesManager] Could NOT to the robot! Will retry in the background..."); - } - } - } - - public static void setTeamClientMode() { - setClientMode(null); - } - - public static void setServerMode() { - isServer = true; - logger.info("Starting NT Server"); - ntInstance.stopClient(); - ntInstance.startServer(); - } -} 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 bcdb0f4df..47f4a9a86 100644 --- a/photon-server/src/main/java/org/photonvision/server/SocketHandler.java +++ b/photon-server/src/main/java/org/photonvision/server/SocketHandler.java @@ -28,13 +28,11 @@ import org.apache.commons.lang3.tuple.Pair; import org.msgpack.jackson.dataformat.MessagePackFactory; import org.photonvision.common.dataflow.DataChangeDestination; import org.photonvision.common.dataflow.DataChangeService; -import org.photonvision.common.dataflow.camera.IncomingCameraCommandSubscriber; import org.photonvision.common.dataflow.events.IncomingWebSocketEvent; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.vision.pipeline.PipelineType; import org.photonvision.vision.processes.PipelineManager; -import org.photonvision.vision.processes.VisionModuleManager; @SuppressWarnings("rawtypes") public class SocketHandler { @@ -47,9 +45,6 @@ public class SocketHandler { @SuppressWarnings("FieldCanBeLocal") private final UIOutboundSubscriber uiOutboundSubscriber = new UIOutboundSubscriber(this); - private final IncomingCameraCommandSubscriber cameraChangeSubscriber = - new IncomingCameraCommandSubscriber(VisionModuleManager.getInstance()); - public static class UIMap extends HashMap {} abstract static class SelectiveBroadcastPair extends Pair {} @@ -65,7 +60,6 @@ public class SocketHandler { private SocketHandler() { dcService.addSubscribers( uiOutboundSubscriber, - cameraChangeSubscriber, new UIInboundSubscriber()); // Subscribe outgoing messages to the data change service } diff --git a/photon-server/src/main/java/org/photonvision/vision/processes/PipelineManager.java b/photon-server/src/main/java/org/photonvision/vision/processes/PipelineManager.java index c81719b67..96318aa31 100644 --- a/photon-server/src/main/java/org/photonvision/vision/processes/PipelineManager.java +++ b/photon-server/src/main/java/org/photonvision/vision/processes/PipelineManager.java @@ -156,6 +156,11 @@ public class PipelineManager { lastPipelineIndex = currentPipelineIndex; } + if (userPipelineSettings.size() - 1 < index) { + logger.warn("User attempted to set index to non-existent pipeline!"); + return; + } + currentPipelineIndex = index; } 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 2f40f73d4..478ba91ee 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 @@ -18,19 +18,18 @@ package org.photonvision.vision.processes; import com.fasterxml.jackson.core.JsonProcessingException; -import edu.wpi.first.networktables.NetworkTableInstance; import java.util.*; import org.apache.commons.lang3.tuple.Pair; import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.configuration.PhotonConfiguration; +import org.photonvision.common.dataflow.CVPipelineResultConsumer; import org.photonvision.common.dataflow.DataChangeService; import org.photonvision.common.dataflow.DataChangeSubscriber; 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.NTDataConsumer; -import org.photonvision.common.datatransfer.DataConsumer; +import org.photonvision.common.dataflow.networktables.NTDataPublisher; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.util.SerializationUtils; @@ -56,9 +55,9 @@ public class VisionModule { private final PipelineManager pipelineManager; private final VisionSource visionSource; private final VisionRunner visionRunner; - private final LinkedList dataConsumers = new LinkedList<>(); + private final LinkedList resultConsumers = new LinkedList<>(); private final LinkedList frameConsumers = new LinkedList<>(); - private final NTDataConsumer ntConsumer; + private final NTDataPublisher ntConsumer; private final int moduleIndex; private final MJPGFrameConsumer uiStreamer; private long lastUpdateTimestamp = -1; @@ -89,19 +88,24 @@ public class VisionModule { addFrameConsumer(uiStreamer); ntConsumer = - new NTDataConsumer( - NetworkTableInstance.getDefault().getTable("photonvision"), - visionSource.getSettables().getConfiguration().nickname); - addDataConsumer(ntConsumer); - addDataConsumer( - data -> { + new NTDataPublisher( + visionSource.getSettables().getConfiguration().nickname, + pipelineManager::getCurrentPipelineIndex, + pipelineManager::setIndex, + () -> pipelineManager.getCurrentPipelineIndex() == -1, + (driverMode) -> { + /* TODO: switch to driver mode */ + }); + addResultConsumer(ntConsumer); + addResultConsumer( + result -> { var now = System.currentTimeMillis(); if (lastUpdateTimestamp + 1000.0 / 15.0 > now) return; var uiMap = new HashMap>(); var dataMap = new HashMap(); - dataMap.put("fps", 1000.0 / data.result.getLatencyMillis()); - var targets = data.result.targets; + dataMap.put("fps", 1000.0 / result.getLatencyMillis()); + var targets = result.targets; var uiTargets = new ArrayList>(); for (var t : targets) { uiTargets.add(t.toHashMap()); @@ -302,15 +306,9 @@ public class VisionModule { ConfigManager.getInstance().getConfig().toHashMap())); } - private void save() { - ConfigManager.getInstance() - .saveModule( - getStateAsCameraConfig(), visionSource.getSettables().getConfiguration().uniqueName); - } - private void setCameraNickname(String newName) { visionSource.getSettables().getConfiguration().nickname = newName; - ntConsumer.setCameraName(newName); + ntConsumer.updateCameraNickname(newName); frameConsumers.remove(dashboardStreamer); dashboardStreamer = @@ -360,8 +358,8 @@ public class VisionModule { return config; } - public void addDataConsumer(DataConsumer dataConsumer) { - dataConsumers.add(dataConsumer); + public void addResultConsumer(CVPipelineResultConsumer dataConsumer) { + resultConsumers.add(dataConsumer); } void addFrameConsumer(FrameConsumer consumer) { @@ -369,20 +367,17 @@ public class VisionModule { } void consumeResult(CVPipelineResult result) { - // TODO: put result in to Data (not this way!) - var data = new Data(); - data.result = result; - consumeData(data); + consumePipelineResult(result); var frame = result.outputFrame; consumeFrame(frame); - data.release(); + result.release(); } - void consumeData(Data data) { - for (var dataConsumer : dataConsumers) { - dataConsumer.accept(data); + void consumePipelineResult(CVPipelineResult result) { + for (var dataConsumer : resultConsumers) { + dataConsumer.accept(result); } } diff --git a/photon-server/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java b/photon-server/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java index 06243498a..7015b35ad 100644 --- a/photon-server/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java +++ b/photon-server/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java @@ -22,7 +22,7 @@ import java.util.HashMap; import java.util.List; import org.junit.jupiter.api.*; import org.photonvision.common.configuration.CameraConfiguration; -import org.photonvision.common.datatransfer.DataConsumer; +import org.photonvision.common.dataflow.CVPipelineResultConsumer; import org.photonvision.common.util.TestUtils; import org.photonvision.vision.frame.FrameProvider; import org.photonvision.vision.frame.FrameStaticProperties; @@ -105,16 +105,12 @@ public class VisionModuleManagerTest { } } - private static class TestDataConsumer implements DataConsumer { - private Data data; + private static class TestDataConsumer implements CVPipelineResultConsumer { + private CVPipelineResult result; @Override - public void accept(Data data) { - this.data = data; - } - - public Data getData() { - return data; + public void accept(CVPipelineResult result) { + this.result = result; } } @@ -131,15 +127,14 @@ public class VisionModuleManagerTest { VisionModuleManager.getInstance().addSources(sources); var module0DataConsumer = new TestDataConsumer(); - VisionModuleManager.getInstance().visionModules.get(0).addDataConsumer(module0DataConsumer); + VisionModuleManager.getInstance().visionModules.get(0).addResultConsumer(module0DataConsumer); VisionModuleManager.getInstance().startModules(); sleep(500); - Assertions.assertNotNull(module0DataConsumer.data); - Assertions.assertNotNull(module0DataConsumer.data.result); - printTestResults(module0DataConsumer.data.result); + Assertions.assertNotNull(module0DataConsumer.result); + printTestResults(module0DataConsumer.result); } private static void printTestResults(CVPipelineResult pipelineResult) {