diff --git a/photon-server/src/main/java/org/photonvision/common/configuration/ConfigManager.java b/photon-server/src/main/java/org/photonvision/common/configuration/ConfigManager.java index 13852908b..cd56da46d 100644 --- a/photon-server/src/main/java/org/photonvision/common/configuration/ConfigManager.java +++ b/photon-server/src/main/java/org/photonvision/common/configuration/ConfigManager.java @@ -358,6 +358,12 @@ public class ConfigManager { return logFile.toPath(); } + public Path getImageSavePath() { + var imgFilePath = Path.of(configDirectoryFile.toString(), "imgSaves").toFile(); + if (!imgFilePath.exists()) imgFilePath.mkdirs(); + return imgFilePath.toPath(); + } + public void requestSave() { logger.trace("Requesting save..."); saveRequestTimestamp = System.currentTimeMillis(); diff --git a/photon-server/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java b/photon-server/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java new file mode 100644 index 000000000..bf449a4ff --- /dev/null +++ b/photon-server/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java @@ -0,0 +1,114 @@ +/* + * 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.vision.frame.consumer; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import java.io.File; +import java.text.DateFormat; +import java.text.SimpleDateFormat; +import java.util.Date; +import java.util.concurrent.locks.ReentrantLock; +import org.opencv.imgcodecs.Imgcodecs; +import org.photonvision.common.configuration.ConfigManager; +import org.photonvision.common.logging.LogGroup; +import org.photonvision.common.logging.Logger; +import org.photonvision.common.util.TimedTaskManager; +import org.photonvision.vision.frame.Frame; + +public class FileSaveFrameConsumer { + + // Formatters to generate unique, timestamped file names + private static String FILE_PATH = ConfigManager.getInstance().getImageSavePath().toString(); + private static String FILE_EXTENSION = ".jpg"; + DateFormat df = new SimpleDateFormat("yyyy-MM-dd"); + DateFormat tf = new SimpleDateFormat("hhmmssSS"); + private final String NT_SUFFIX = "SaveImgCmd"; + private final String ntEntryName; + private NetworkTable subTable; + private final NetworkTable rootTable; + private final Logger logger; + private boolean prevCommand = false; + private String camNickname; + private String fnamePrefix; + private final long CMD_RESET_TIME_MS = 500; + // Helps prevent race conditions between user set & auto-reset logic + private ReentrantLock lock; + + public FileSaveFrameConsumer(String camNickname, String streamPrefix) { + this.lock = new ReentrantLock(); + this.fnamePrefix = camNickname + "_" + streamPrefix; + this.ntEntryName = streamPrefix + NT_SUFFIX; + this.rootTable = NetworkTableInstance.getDefault().getTable("/photonvision"); + updateCameraNickname(camNickname); + this.logger = new Logger(FileSaveFrameConsumer.class, this.camNickname, LogGroup.General); + } + + public void accept(Frame frame) { + if (frame != null && !frame.image.getMat().empty()) { + + if (lock.tryLock()) { + boolean curCommand = subTable.getEntry(ntEntryName).getBoolean(false); + + if (curCommand == true && prevCommand == false) { + Date now = new Date(); + String savefile = + FILE_PATH + + File.separator + + fnamePrefix + + "_" + + df.format(now) + + "T" + + tf.format(now) + + FILE_EXTENSION; + + Imgcodecs.imwrite(savefile.toString(), frame.image.getMat()); + + // Help the user a bit - set the NT entry back to false after 500ms + TimedTaskManager.getInstance().addOneShotTask(() -> resetCommand(), CMD_RESET_TIME_MS); + + logger.info("Saved new image at " + savefile); + } + + prevCommand = curCommand; + lock.unlock(); + } + } + } + + private void resetCommand() { + lock.lock(); + this.subTable.getEntry(ntEntryName).setBoolean(false); + lock.unlock(); + } + + private void removeEntries() { + if (this.subTable != null) { + if (this.subTable.containsKey(ntEntryName)) { + this.subTable.delete(ntEntryName); + } + } + } + + public void updateCameraNickname(String newCameraNickname) { + removeEntries(); + this.camNickname = newCameraNickname; + this.subTable = rootTable.getSubTable(this.camNickname); + resetCommand(); + } +} 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 aef04e03f..5bf5b5941 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 @@ -37,6 +37,7 @@ import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.camera.CameraQuirk; import org.photonvision.vision.camera.QuirkyCamera; import org.photonvision.vision.camera.USBCameraSource; +import org.photonvision.vision.frame.consumer.FileSaveFrameConsumer; import org.photonvision.vision.frame.consumer.MJPGFrameConsumer; import org.photonvision.vision.pipeline.ReflectivePipelineSettings; import org.photonvision.vision.pipeline.UICalibrationData; @@ -71,6 +72,9 @@ public class VisionModule { MJPGFrameConsumer dashboardInputStreamer; MJPGFrameConsumer dashboardOutputStreamer; + FileSaveFrameConsumer inputFrameSaver; + FileSaveFrameConsumer outputFrameSaver; + public VisionModule(PipelineManager pipelineManager, VisionSource visionSource, int index) { logger = new Logger( @@ -98,6 +102,8 @@ public class VisionModule { createStreams(); fpsLimitedResultConsumers.add(result -> dashboardInputStreamer.accept(result.inputFrame)); fpsLimitedResultConsumers.add(result -> dashboardOutputStreamer.accept(result.outputFrame)); + fpsLimitedResultConsumers.add(result -> inputFrameSaver.accept(result.inputFrame)); + fpsLimitedResultConsumers.add(result -> outputFrameSaver.accept(result.outputFrame)); ntConsumer = new NTDataPublisher( @@ -151,7 +157,13 @@ public class VisionModule { visionSource.getSettables().getConfiguration().nickname + "-output", outputStreamPort); dashboardInputStreamer = new MJPGFrameConsumer( - visionSource.getSettables().getConfiguration().nickname + "-input", inputStreamPort); + visionSource.getSettables().getConfiguration().uniqueName + "-input", inputStreamPort); + + inputFrameSaver = + new FileSaveFrameConsumer(visionSource.getSettables().getConfiguration().nickname, "input"); + outputFrameSaver = + new FileSaveFrameConsumer( + visionSource.getSettables().getConfiguration().nickname, "output"); } void setDriverMode(boolean isDriverMode) { @@ -287,6 +299,8 @@ public class VisionModule { public void setCameraNickname(String newName) { visionSource.getSettables().getConfiguration().nickname = newName; ntConsumer.updateCameraNickname(newName); + inputFrameSaver.updateCameraNickname(newName); + outputFrameSaver.updateCameraNickname(newName); // rename streams fpsLimitedResultConsumers.clear(); @@ -297,6 +311,8 @@ public class VisionModule { fpsLimitedResultConsumers.add(result -> dashboardInputStreamer.accept(result.inputFrame)); fpsLimitedResultConsumers.add(result -> dashboardOutputStreamer.accept(result.outputFrame)); + fpsLimitedResultConsumers.add(result -> inputFrameSaver.accept(result.inputFrame)); + fpsLimitedResultConsumers.add(result -> outputFrameSaver.accept(result.outputFrame)); // Push new data to the UI saveAndBroadcastAll();