Camera Lost Stream (#1341)

* Fix no stream on camera unplug. * Spotless remove datarate * Make Static Frames Class * lint and format
2026-06-25 01:41:40 +00:00 · 2024-06-06 21:46:46 -04:00
parent db531f1b6a
commit 7b19a951ca
4 changed files with 154 additions and 129 deletions
--- a/photon-core/src/main/java/org/photonvision/vision/frame/StaticFrames.java
+++ 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 <https://www.gnu.org/licenses/>.
+ */
+
+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() {}
+}
--- 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<CVMat> {
@@ -64,36 +65,38 @@ public class FileSaveFrameConsumer implements Consumer<CVMat> {
    }

    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;
        }
    }

--- 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());
        }
    }

--- 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;
            }