diff --git a/photon-server/src/main/java/org/photonvision/vision/camera/FileVisionSource.java b/photon-server/src/main/java/org/photonvision/vision/camera/FileVisionSource.java new file mode 100644 index 000000000..6bf9fbc98 --- /dev/null +++ b/photon-server/src/main/java/org/photonvision/vision/camera/FileVisionSource.java @@ -0,0 +1,94 @@ +/* + * 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.camera; + +import edu.wpi.cscore.VideoMode; +import edu.wpi.cscore.VideoMode.PixelFormat; +import java.util.HashMap; +import org.photonvision.common.configuration.CameraConfiguration; +import org.photonvision.vision.frame.FrameProvider; +import org.photonvision.vision.frame.FrameStaticProperties; +import org.photonvision.vision.frame.provider.FileFrameProvider; +import org.photonvision.vision.processes.VisionSource; +import org.photonvision.vision.processes.VisionSourceSettables; + +public class FileVisionSource implements VisionSource { + + private final CameraConfiguration cameraConfiguration; + private final FileFrameProvider frameProvider; + private final FileSourceSettables settables; + + public FileVisionSource(String name, String imagePath, double fov) { + cameraConfiguration = new CameraConfiguration(name, imagePath); + frameProvider = new FileFrameProvider(imagePath, fov); + settables = + new FileSourceSettables(cameraConfiguration, frameProvider.get().frameStaticProperties); + } + + @Override + public FrameProvider getFrameProvider() { + return frameProvider; + } + + @Override + public VisionSourceSettables getSettables() { + return settables; + } + + private static class FileSourceSettables extends VisionSourceSettables { + + private final VideoMode videoMode; + + private final HashMap videoModes = new HashMap<>(); + + FileSourceSettables( + CameraConfiguration cameraConfiguration, FrameStaticProperties frameStaticProperties) { + super(cameraConfiguration); + this.frameStaticProperties = frameStaticProperties; + videoMode = + new VideoMode( + PixelFormat.kMJPEG, + frameStaticProperties.imageWidth, + frameStaticProperties.imageHeight, + 30); + videoModes.put(0, videoMode); + } + + @Override + public void setExposure(int exposure) {} + + @Override + public void setBrightness(int brightness) {} + + @Override + public void setGain(int gain) {} + + @Override + public VideoMode getCurrentVideoMode() { + return videoMode; + } + + @Override + public void setCurrentVideoMode(VideoMode videoMode) {} + + @Override + public HashMap getAllVideoModes() { + return videoModes; + } + } +} diff --git a/photon-server/src/main/java/org/photonvision/vision/frame/Frame.java b/photon-server/src/main/java/org/photonvision/vision/frame/Frame.java index a001c34f9..29899c5a8 100644 --- a/photon-server/src/main/java/org/photonvision/vision/frame/Frame.java +++ b/photon-server/src/main/java/org/photonvision/vision/frame/Frame.java @@ -36,11 +36,11 @@ public class Frame implements Releasable { this(image, System.nanoTime(), frameStaticProperties); } - public void copyTo(Mat destMat) { - image.getMat().copyTo(destMat); + public void copyTo(Frame destFrame) { + image.getMat().copyTo(destFrame.image.getMat()); } - public static Frame copyFrom(Frame frame) { + public static Frame copyFromAndRelease(Frame frame) { Mat newMat = new Mat(); frame.image.getMat().copyTo(newMat); frame.release(); diff --git a/photon-server/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java b/photon-server/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java index bf2434aeb..5af806d04 100644 --- a/photon-server/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java +++ b/photon-server/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java @@ -32,28 +32,41 @@ import org.photonvision.vision.opencv.CVMat; * path}. */ public class FileFrameProvider implements FrameProvider { + private static final int MAX_FPS = 120; private static int count = 0; - private Frame m_frame; - private final Path m_path; + private final int thisIndex = count++; + private final Path path; + private final int millisDelay; + private final Frame originalFrame; + private final Frame outputFrame; - private final double m_fov; - - private boolean m_reloadImage; + private long lastGetMillis = System.currentTimeMillis(); /** * Instantiates a new FileFrameProvider. * * @param path The path of the image to read from. * @param fov The fov of the image. + * @param maxFPS The max framerate to provide the image at. */ - public FileFrameProvider(Path path, double fov) { + public FileFrameProvider(Path path, double fov, int maxFPS) { if (!Files.exists(path)) throw new RuntimeException("Invalid path for image: " + path.toAbsolutePath().toString()); - m_path = path; - m_fov = fov; + this.path = path; + this.millisDelay = 1000 / maxFPS; - loadImage(); + Mat rawImage = Imgcodecs.imread(path.toString()); + if (rawImage.cols() > 0 && rawImage.rows() > 0) { + FrameStaticProperties m_properties = + new FrameStaticProperties(rawImage.width(), rawImage.height(), fov); + Mat originalImage = new Mat(); + rawImage.copyTo(originalImage); + originalFrame = new Frame(new CVMat(rawImage), m_properties); + outputFrame = new Frame(new CVMat(originalImage), m_properties); + } else { + throw new RuntimeException("Image loading failed!"); + } } /** @@ -63,53 +76,40 @@ public class FileFrameProvider implements FrameProvider { * @param fov The fov of the image. */ public FileFrameProvider(String pathAsString, double fov) { - this(Paths.get(pathAsString), fov); - } - - private void loadImage() { - Mat image = Imgcodecs.imread(m_path.toString()); - - if (image.cols() > 0 && image.rows() > 0) { - FrameStaticProperties m_properties = - new FrameStaticProperties(image.width(), image.height(), m_fov); - m_frame = new Frame(new CVMat(image), m_properties); - } else { - throw new RuntimeException("Image loading failed!"); - } + this(Paths.get(pathAsString), fov, MAX_FPS); } /** - * Set image reloading. If true this will reload the image from the path set in the constructor - * every time {@link FileFrameProvider#get()} is called. + * Instantiates a new File frame provider. * - * @param reloadImage True to enable image reloading. + * @param path The path of the image to read from. + * @param fov The fov of the image. */ - public void setImageReloading(boolean reloadImage) { - m_reloadImage = reloadImage; - } - - /** - * Returns if image reloading is enabled. - * - * @return True if image reloading is enabled. - */ - public boolean isImageReloading() { - return m_reloadImage; + public FileFrameProvider(Path path, double fov) { + this(path, fov, MAX_FPS); } @Override public Frame get() { - if (m_reloadImage) { - if (m_frame != null) m_frame.release(); - m_frame = null; - loadImage(); + if (outputFrame.image.getMat().empty()) { + originalFrame.copyTo(outputFrame); } - return m_frame; + // block to keep FPS at a defined rate + if (System.currentTimeMillis() - lastGetMillis < millisDelay) { + try { + Thread.sleep(millisDelay); + } catch (InterruptedException e) { + e.printStackTrace(); + } + } + + lastGetMillis = System.currentTimeMillis(); + return outputFrame; } @Override public String getName() { - return "FileFrameProvider" + count++ + " - " + m_path.getFileName(); + return "FileFrameProvider" + thisIndex + " - " + path.getFileName(); } } diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java index 238cb5eec..2312726ca 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java +++ b/photon-server/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java @@ -34,8 +34,8 @@ public class CVPipelineResult implements Releasable { this.processingMillis = processingMillis; this.targets = targets; - this.outputFrame = Frame.copyFrom(outputFrame); - this.inputFrame = inputFrame != null ? Frame.copyFrom(inputFrame) : null; + this.outputFrame = Frame.copyFromAndRelease(outputFrame); + this.inputFrame = inputFrame != null ? Frame.copyFromAndRelease(inputFrame) : null; } public CVPipelineResult(double processingMillis, List targets, Frame outputFrame) { diff --git a/photon-server/src/test/java/org/photonvision/common/BenchmarkTest.java b/photon-server/src/test/java/org/photonvision/common/BenchmarkTest.java index 38e24dac3..9a756729a 100644 --- a/photon-server/src/test/java/org/photonvision/common/BenchmarkTest.java +++ b/photon-server/src/test/java/org/photonvision/common/BenchmarkTest.java @@ -59,8 +59,6 @@ public class BenchmarkTest { TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoSideStraightDark72in), TestUtils.WPI2019Image.FOV); - frameProvider.setImageReloading(true); - benchmarkPipeline(frameProvider, pipeline, 5); } @@ -78,8 +76,6 @@ public class BenchmarkTest { TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_084in_Center), TestUtils.WPI2020Image.FOV); - frameProvider.setImageReloading(true); - benchmarkPipeline(frameProvider, pipeline, 5); } @@ -97,8 +93,6 @@ public class BenchmarkTest { TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_084in_Center_720p), TestUtils.WPI2020Image.FOV); - frameProvider.setImageReloading(true); - benchmarkPipeline(frameProvider, pipeline, 5); } @@ -119,8 +113,6 @@ public class BenchmarkTest { TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes), TestUtils.WPI2019Image.FOV); - frameProvider.setImageReloading(true); - benchmarkPipeline(frameProvider, pipeline, 5); } diff --git a/photon-server/src/test/java/org/photonvision/common/ShapeBenchmarkTest.java b/photon-server/src/test/java/org/photonvision/common/ShapeBenchmarkTest.java index 6e80b5c2c..f0c3bb304 100644 --- a/photon-server/src/test/java/org/photonvision/common/ShapeBenchmarkTest.java +++ b/photon-server/src/test/java/org/photonvision/common/ShapeBenchmarkTest.java @@ -75,8 +75,6 @@ public class ShapeBenchmarkTest { TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoSideStraightDark72in), TestUtils.WPI2019Image.FOV); - frameProvider.setImageReloading(true); - benchmarkPipeline(frameProvider, pipeline, 5); } @@ -99,8 +97,6 @@ public class ShapeBenchmarkTest { TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_084in_Center), TestUtils.WPI2020Image.FOV); - frameProvider.setImageReloading(true); - benchmarkPipeline(frameProvider, pipeline, 5); } @@ -123,8 +119,6 @@ public class ShapeBenchmarkTest { TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_084in_Center_720p), TestUtils.WPI2020Image.FOV); - frameProvider.setImageReloading(true); - benchmarkPipeline(frameProvider, pipeline, 5); } @@ -147,8 +141,6 @@ public class ShapeBenchmarkTest { TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes), TestUtils.WPI2019Image.FOV); - frameProvider.setImageReloading(true); - benchmarkPipeline(frameProvider, pipeline, 5); }