From 7b19a951ca122eea92956f7eb00690f5fb55573d Mon Sep 17 00:00:00 2001 From: "Cameron (3539)" Date: Thu, 6 Jun 2024 21:46:46 -0400 Subject: [PATCH] Camera Lost Stream (#1341) * Fix no stream on camera unplug. * Spotless remove datarate * Make Static Frames Class * lint and format --- .../vision/frame/StaticFrames.java | 113 +++++++++++++++++ .../frame/consumer/FileSaveFrameConsumer.java | 53 ++++---- .../frame/consumer/MJPGFrameConsumer.java | 114 ++---------------- .../vision/processes/VisionRunner.java | 3 + 4 files changed, 154 insertions(+), 129 deletions(-) create mode 100644 photon-core/src/main/java/org/photonvision/vision/frame/StaticFrames.java diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/StaticFrames.java b/photon-core/src/main/java/org/photonvision/vision/frame/StaticFrames.java new file mode 100644 index 000000000..0462b8f31 --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/vision/frame/StaticFrames.java @@ -0,0 +1,113 @@ +/* + * 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.frame; + +import java.awt.Color; +import org.opencv.core.CvType; +import org.opencv.core.Mat; +import org.opencv.core.Point; +import org.opencv.core.Rect; +import org.opencv.imgproc.Imgproc; +import org.photonvision.common.util.ColorHelper; + +public class StaticFrames { + public static final Mat LOST_MAT = new Mat(60, 15 * 7, CvType.CV_8UC3); + public static final Mat EMPTY_MAT = new Mat(60, 15 * 7, CvType.CV_8UC3); + + static { + EMPTY_MAT.setTo(ColorHelper.colorToScalar(Color.BLACK)); + var col = 0; + Imgproc.rectangle( + EMPTY_MAT, + new Rect(col, 0, 15, EMPTY_MAT.height()), + ColorHelper.colorToScalar(new Color(0xa2a2a2)), + -1); + col += 15; + Imgproc.rectangle( + EMPTY_MAT, + new Rect(col, 0, 15, EMPTY_MAT.height()), + ColorHelper.colorToScalar(new Color(0xa2a300)), + -1); + col += 15; + Imgproc.rectangle( + EMPTY_MAT, + new Rect(col, 0, 15, EMPTY_MAT.height()), + ColorHelper.colorToScalar(new Color(0x00a3a2)), + -1); + col += 15; + Imgproc.rectangle( + EMPTY_MAT, + new Rect(col, 0, 15, EMPTY_MAT.height()), + ColorHelper.colorToScalar(new Color(0x00a200)), + -1); + col += 15; + Imgproc.rectangle( + EMPTY_MAT, + new Rect(col, 0, 15, EMPTY_MAT.height()), + ColorHelper.colorToScalar(new Color(0x440045)), + -1); + col += 15; + Imgproc.rectangle( + EMPTY_MAT, + new Rect(col, 0, 15, EMPTY_MAT.height()), + ColorHelper.colorToScalar(new Color(0x0000a2)), + -1); + col += 15; + Imgproc.rectangle( + EMPTY_MAT, + new Rect(col, 0, 15, EMPTY_MAT.height()), + ColorHelper.colorToScalar(new Color(0)), + -1); + Imgproc.rectangle( + EMPTY_MAT, + new Rect(0, 50, EMPTY_MAT.width(), 10), + ColorHelper.colorToScalar(new Color(0)), + -1); + Imgproc.rectangle( + EMPTY_MAT, new Rect(15, 50, 30, 10), ColorHelper.colorToScalar(Color.WHITE), -1); + + EMPTY_MAT.copyTo(LOST_MAT); + + Imgproc.putText( + EMPTY_MAT, "Stream", new Point(14, 20), 0, 0.6, ColorHelper.colorToScalar(Color.white), 2); + Imgproc.putText( + EMPTY_MAT, + "Disabled", + new Point(14, 45), + 0, + 0.6, + ColorHelper.colorToScalar(Color.white), + 2); + + Imgproc.putText( + EMPTY_MAT, "Stream", new Point(14, 20), 0, 0.6, ColorHelper.colorToScalar(Color.RED), 1); + Imgproc.putText( + EMPTY_MAT, "Disabled", new Point(14, 45), 0, 0.6, ColorHelper.colorToScalar(Color.RED), 1); + + Imgproc.putText( + LOST_MAT, "Camera", new Point(14, 20), 0, 0.6, ColorHelper.colorToScalar(Color.white), 2); + Imgproc.putText( + LOST_MAT, "Lost", new Point(14, 45), 0, 0.6, ColorHelper.colorToScalar(Color.white), 2); + Imgproc.putText( + LOST_MAT, "Camera", new Point(14, 20), 0, 0.6, ColorHelper.colorToScalar(Color.RED), 1); + Imgproc.putText( + LOST_MAT, "Lost", new Point(14, 45), 0, 0.6, ColorHelper.colorToScalar(Color.RED), 1); + } + + public StaticFrames() {} +} diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java b/photon-core/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java index a9f915182..3e2a8e545 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java @@ -29,6 +29,7 @@ import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.dataflow.networktables.NetworkTablesManager; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; +import org.photonvision.vision.frame.StaticFrames; import org.photonvision.vision.opencv.CVMat; public class FileSaveFrameConsumer implements Consumer { @@ -64,36 +65,38 @@ public class FileSaveFrameConsumer implements Consumer { } public void accept(CVMat image) { - if (image != null && image.getMat() != null && !image.getMat().empty()) { - long currentCount = saveFrameEntry.get(); + long currentCount = saveFrameEntry.get(); - // Await save request - if (currentCount == -1) return; + // Await save request + if (currentCount == -1) return; - // The requested count is greater than the actual count - if (savedImagesCount < currentCount) { - Date now = new Date(); + // The requested count is greater than the actual count + if (savedImagesCount < currentCount) { + Date now = new Date(); - String fileName = - cameraNickname + "_" + streamType + "_" + df.format(now) + "T" + tf.format(now); + String fileName = + cameraNickname + "_" + streamType + "_" + df.format(now) + "T" + tf.format(now); - // Check if the Unique Camera directory exists and create it if it doesn't - String cameraPath = FILE_PATH + File.separator + this.cameraUniqueName; - var cameraDir = new File(cameraPath); - if (!cameraDir.exists()) { - cameraDir.mkdir(); - } - - String saveFilePath = cameraPath + File.separator + fileName + FILE_EXTENSION; - - Imgcodecs.imwrite(saveFilePath, image.getMat()); - - savedImagesCount++; - logger.info("Saved new image at " + saveFilePath); - } else if (savedImagesCount > currentCount) { - // Reset local value with NT value in case of de-sync - savedImagesCount = currentCount; + // Check if the Unique Camera directory exists and create it if it doesn't + String cameraPath = FILE_PATH + File.separator + this.cameraUniqueName; + var cameraDir = new File(cameraPath); + if (!cameraDir.exists()) { + cameraDir.mkdir(); } + + String saveFilePath = cameraPath + File.separator + fileName + FILE_EXTENSION; + + if (image == null || image.getMat() == null || image.getMat().empty()) { + Imgcodecs.imwrite(saveFilePath, StaticFrames.LOST_MAT); + } else { + Imgcodecs.imwrite(saveFilePath, image.getMat()); + } + + savedImagesCount++; + logger.info("Saved new image at " + saveFilePath); + } else if (savedImagesCount > currentCount) { + // Reset local value with NT value in case of de-sync + savedImagesCount = currentCount; } } diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/consumer/MJPGFrameConsumer.java b/photon-core/src/main/java/org/photonvision/vision/frame/consumer/MJPGFrameConsumer.java index 3f3a2962d..7a168245b 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/consumer/MJPGFrameConsumer.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/consumer/MJPGFrameConsumer.java @@ -21,102 +21,22 @@ import edu.wpi.first.cscore.*; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.util.PixelFormat; -import java.awt.*; import java.util.ArrayList; -import org.opencv.core.CvType; -import org.opencv.core.Mat; -import org.opencv.core.Point; -import org.opencv.core.Rect; -import org.opencv.imgproc.Imgproc; -import org.photonvision.common.util.ColorHelper; import org.photonvision.common.util.math.MathUtils; +import org.photonvision.vision.frame.StaticFrames; import org.photonvision.vision.opencv.CVMat; public class MJPGFrameConsumer implements AutoCloseable { private static final double MAX_FRAMERATE = -1; private static final long MAX_FRAME_PERIOD_NS = Math.round(1e9 / MAX_FRAMERATE); + private long lastFrameTimeNs; - - private static final Mat EMPTY_MAT = new Mat(60, 15 * 7, CvType.CV_8UC3); - private static final double EMPTY_FRAMERATE = 2; - private static final long EMPTY_FRAME_PERIOD_NS = Math.round(1e9 / EMPTY_FRAMERATE); - private long lastEmptyTimeNs; - - static { - EMPTY_MAT.setTo(ColorHelper.colorToScalar(Color.BLACK)); - var col = 0; - Imgproc.rectangle( - EMPTY_MAT, - new Rect(col, 0, 15, EMPTY_MAT.height()), - ColorHelper.colorToScalar(new Color(0xa2a2a2)), - -1); - col += 15; - Imgproc.rectangle( - EMPTY_MAT, - new Rect(col, 0, 15, EMPTY_MAT.height()), - ColorHelper.colorToScalar(new Color(0xa2a300)), - -1); - col += 15; - Imgproc.rectangle( - EMPTY_MAT, - new Rect(col, 0, 15, EMPTY_MAT.height()), - ColorHelper.colorToScalar(new Color(0x00a3a2)), - -1); - col += 15; - Imgproc.rectangle( - EMPTY_MAT, - new Rect(col, 0, 15, EMPTY_MAT.height()), - ColorHelper.colorToScalar(new Color(0x00a200)), - -1); - col += 15; - Imgproc.rectangle( - EMPTY_MAT, - new Rect(col, 0, 15, EMPTY_MAT.height()), - ColorHelper.colorToScalar(new Color(0x440045)), - -1); - col += 15; - Imgproc.rectangle( - EMPTY_MAT, - new Rect(col, 0, 15, EMPTY_MAT.height()), - ColorHelper.colorToScalar(new Color(0x0000a2)), - -1); - col += 15; - Imgproc.rectangle( - EMPTY_MAT, - new Rect(col, 0, 15, EMPTY_MAT.height()), - ColorHelper.colorToScalar(new Color(0)), - -1); - Imgproc.rectangle( - EMPTY_MAT, - new Rect(0, 50, EMPTY_MAT.width(), 10), - ColorHelper.colorToScalar(new Color(0)), - -1); - Imgproc.rectangle( - EMPTY_MAT, new Rect(15, 50, 30, 10), ColorHelper.colorToScalar(Color.WHITE), -1); - - Imgproc.putText( - EMPTY_MAT, "Stream", new Point(14, 20), 0, 0.6, ColorHelper.colorToScalar(Color.white), 2); - Imgproc.putText( - EMPTY_MAT, - "Disabled", - new Point(14, 45), - 0, - 0.6, - ColorHelper.colorToScalar(Color.white), - 2); - Imgproc.putText( - EMPTY_MAT, "Stream", new Point(14, 20), 0, 0.6, ColorHelper.colorToScalar(Color.RED), 1); - Imgproc.putText( - EMPTY_MAT, "Disabled", new Point(14, 45), 0, 0.6, ColorHelper.colorToScalar(Color.RED), 1); - } - private CvSource cvSource; private MjpegServer mjpegServer; private VideoListener listener; private final NetworkTable table; - boolean isDisabled = false; public MJPGFrameConsumer(String sourceName, int width, int height, int port) { this.cvSource = new CvSource(sourceName, PixelFormat.kMJPEG, width, height, 30); @@ -174,29 +94,15 @@ public class MJPGFrameConsumer implements AutoCloseable { } public void accept(CVMat image) { - if (image != null && !image.getMat().empty()) { - long now = MathUtils.wpiNanoTime(); - if (now - lastFrameTimeNs > MAX_FRAME_PERIOD_NS) { - lastFrameTimeNs = now; - cvSource.putFrame(image.getMat()); - } - - // Make sure our disabled framerate limiting doesn't get confused - isDisabled = false; - lastEmptyTimeNs = 0; - } - } - - public void disabledTick() { - if (!isDisabled) { - cvSource.setVideoMode(PixelFormat.kMJPEG, EMPTY_MAT.width(), EMPTY_MAT.height(), 0); - isDisabled = true; - } - long now = MathUtils.wpiNanoTime(); - if (now - lastEmptyTimeNs > EMPTY_FRAME_PERIOD_NS) { - lastEmptyTimeNs = now; - cvSource.putFrame(EMPTY_MAT); + + if (image == null || image.getMat() == null || image.getMat().empty()) { + image.copyFrom(StaticFrames.LOST_MAT); + } + + if (now - lastFrameTimeNs > MAX_FRAME_PERIOD_NS) { + lastFrameTimeNs = now; + cvSource.putFrame(image.getMat()); } } 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 932e56b53..10cbd4fdb 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 @@ -22,6 +22,7 @@ import java.util.function.Supplier; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.vision.camera.QuirkyCamera; +import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameProvider; import org.photonvision.vision.pipe.impl.HSVPipe; import org.photonvision.vision.pipeline.AdvancedPipelineSettings; @@ -95,6 +96,8 @@ public class VisionRunner { // Frame empty -- no point in trying to do anything more? if (frame.processedImage.getMat().empty() && frame.colorImage.getMat().empty()) { // give up without increasing loop count + // Still feed with blank frames just dont run any pipelines + pipelineResultConsumer.accept(new CVPipelineResult(0l, 0, 0, null, new Frame())); continue; }