From 17009bc04c6bc9f309895adb8aefbe2f99ac490b Mon Sep 17 00:00:00 2001 From: Banks Troutman Date: Fri, 20 Sep 2019 21:25:54 -0400 Subject: [PATCH] Moved camera stream to separate thread --- .../chameleonvision/vision/camera/Camera.java | 17 ++++++-- .../vision/process/CameraProcess.java | 31 +++++++++----- .../vision/process/StreamProcess.java | 42 +++++++++++++++++++ 3 files changed, 77 insertions(+), 13 deletions(-) create mode 100644 Main/src/main/java/com/chameleonvision/vision/process/StreamProcess.java diff --git a/Main/src/main/java/com/chameleonvision/vision/camera/Camera.java b/Main/src/main/java/com/chameleonvision/vision/camera/Camera.java index 54922cec7..be4a00c74 100644 --- a/Main/src/main/java/com/chameleonvision/vision/camera/Camera.java +++ b/Main/src/main/java/com/chameleonvision/vision/camera/Camera.java @@ -30,6 +30,9 @@ public class Camera { private int currentPipelineIndex; private HashMap pipelines; + private final Object cvSourceLock = new Object(); + + public Camera(String cameraName) { this(cameraName, defaultFOV); } @@ -85,7 +88,9 @@ public class Camera { // update camera values camVals = new CameraValues(this); if ( prevVideoMode != null && prevVideoMode.width != newVideoMode.width && prevVideoMode.height != newVideoMode.height) { // if resolution changed - cvSource = cs.putVideo(name, newVideoMode.width, newVideoMode.height); + synchronized (cvSourceLock) { + cvSource = cs.putVideo(name, newVideoMode.width, newVideoMode.height); + } } } @@ -114,7 +119,7 @@ public class Camera { return pipelines; } - CamVideoMode getVideoMode() { + public CamVideoMode getVideoMode() { return camVideoMode; } @@ -156,7 +161,13 @@ public class Camera { return cvSink.grabFrame(image, timeout); } + public CameraValues getCamVals() { + return camVals; + } + public void putFrame(Mat image) { - cvSource.putFrame(image); + synchronized(cvSourceLock) { + cvSource.putFrame(image); + } } } diff --git a/Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java b/Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java index d46a159c6..de77ac8ce 100644 --- a/Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java +++ b/Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java @@ -6,9 +6,6 @@ import com.chameleonvision.vision.Pipeline; import com.chameleonvision.vision.camera.Camera; import com.chameleonvision.vision.camera.CameraValues; import com.chameleonvision.web.Server; -import edu.wpi.cscore.CvSink; -import edu.wpi.cscore.CvSource; -import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.networktables.*; import org.opencv.core.*; import org.opencv.imgproc.Imgproc; @@ -48,6 +45,8 @@ public class CameraProcess implements Runnable { private Scalar contourRectColor = new Scalar(255, 0, 0); private long TimeStamp = 0; + private final StreamProcess streamProcess; + private void DriverModeListener(EntryNotification entryNotification) { if (entryNotification.value.getBoolean()) { camera.setExposure(25); @@ -92,8 +91,8 @@ public class CameraProcess implements Runnable { ntPipelineEntry.setString("pipeline" + camera.getCurrentPipelineIndex()); // camera settings - camVals = new CameraValues(camera); visionProcess = new VisionProcess(camVals); + streamProcess = new StreamProcess(camera); } private void drawContour(Mat inputMat, RotatedRect contourRect) { @@ -170,6 +169,8 @@ public class CameraProcess implements Runnable { double processTimeMs; double fps = 0; + new Thread(streamProcess).start(); + while (!Thread.interrupted()) { FoundContours.clear(); FilteredContours.clear(); @@ -186,20 +187,30 @@ public class CameraProcess implements Runnable { // get vision data var pipelineResult = runVisionProcess(cameraInputMat, streamOutputMat); updateNetworkTables(pipelineResult); - if (cameraName.equals(SettingsManager.GeneralSettings.curr_camera) && pipelineResult.IsValid) { + if (cameraName.equals(SettingsManager.GeneralSettings.curr_camera)) { HashMap WebSend = new HashMap<>(); HashMap point = new HashMap<>(); List center = new ArrayList<>(); - center.add(pipelineResult.RawPoint.center.x); - center.add(pipelineResult.RawPoint.center.y); - point.put("pitch", pipelineResult.Pitch); - point.put("yaw", pipelineResult.Yaw); + if (pipelineResult.IsValid) { + center.add(pipelineResult.RawPoint.center.x); + center.add(pipelineResult.RawPoint.center.y); + point.put("pitch", pipelineResult.Pitch); + point.put("yaw", pipelineResult.Yaw); + } else { + center.add(0.0); + center.add(0.0); + point.put("pitch", 0); + point.put("yaw", 0); + } point.put("fps", fps); WebSend.put("point", point); WebSend.put("raw_point", center); Server.broadcastMessage(WebSend); } - camera.putFrame(streamOutputMat); + + //camera.putFrame(streamOutputMat); + streamProcess.updateFrame(streamOutputMat); + // calculate FPS after publishing output frame processTimeMs = (System.nanoTime() - startTime) * 1e-6; fps = 1000 / processTimeMs; diff --git a/Main/src/main/java/com/chameleonvision/vision/process/StreamProcess.java b/Main/src/main/java/com/chameleonvision/vision/process/StreamProcess.java new file mode 100644 index 000000000..ac3d7e951 --- /dev/null +++ b/Main/src/main/java/com/chameleonvision/vision/process/StreamProcess.java @@ -0,0 +1,42 @@ +package com.chameleonvision.vision.process; + +import com.chameleonvision.vision.camera.Camera; +import org.opencv.core.CvType; +import org.opencv.core.Mat; + +public class StreamProcess implements Runnable { + + private final Camera camera; + private final int maxFPS; + private Mat streamFrame; + + private final Object streamFrameLock = new Object(); + + public StreamProcess(Camera camera) { + this.camera = camera; + maxFPS = camera.getVideoMode().fps; + var camVals = camera.getCamVals(); + streamFrame = new Mat(camVals.ImageWidth, camVals.ImageHeight, CvType.CV_8UC3); + } + + void updateFrame(Mat inputFrame) { + synchronized (streamFrameLock) { + inputFrame.copyTo(streamFrame); + } + } + + @Override + public void run() { + while(!Thread.interrupted()) { + synchronized (streamFrameLock) { + camera.putFrame(streamFrame); + } + var msToWait = (long)1000/maxFPS; + try { + Thread.sleep(msToWait); + } catch (InterruptedException e) { + e.printStackTrace(); + } + } + } +}