Merge remote-tracking branch 'origin/Java' into Java

Main/src/main/java/com/chameleonvision/vision/CameraValues.java

@@ -19,6 +19,10 @@ public class CameraValues {
    public final double HorizontalFocalLength;
    public final double VerticalFocalLength;
 
+   public CameraValues(Camera camera) {
+       this(camera.camVideoMode.width, camera.camVideoMode.height, camera.FOV);
+   }
+
     public CameraValues(int imageWidth, int imageHeight, double fov) {
         ImageWidth = imageWidth;
         ImageHeight = imageHeight;

Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java

@@ -4,31 +4,51 @@ import com.chameleonvision.MemoryManager;
 import com.chameleonvision.settings.SettingsManager;
 import com.chameleonvision.vision.CameraValues;
 import com.chameleonvision.vision.Pipeline;
-import com.chameleonvision.web.Server;
+import edu.wpi.cscore.CvSink;
+import edu.wpi.cscore.CvSource;
 import edu.wpi.first.networktables.*;
 import edu.wpi.first.cameraserver.CameraServer;
-import org.apache.commons.math3.stat.descriptive.moment.Mean;
-import org.apache.commons.math3.stat.descriptive.moment.Variance;
 import org.opencv.core.*;
 import org.opencv.imgproc.Imgproc;
 
 import java.util.ArrayList;
 import java.util.List;
-import java.util.Set;
 
 public class CameraProcess implements Runnable {
     private String CameraName;
 
-    private CameraServer cs = CameraServer.getInstance();
+    // NetworkTables
-    private NetworkTableEntry ntPipelineEntry, ntDriverModeEntry, ntYawEntry, ntPitchEntry, ntDistanceEntry, ntTimeStampEntry, ntValidEntry;
+    private NetworkTableEntry ntPipelineEntry;
+    private NetworkTableEntry ntDriverModeEntry;
+    private NetworkTableEntry ntYawEntry;
+    private NetworkTableEntry ntPitchEntry;
+    private NetworkTableEntry ntDistanceEntry;
+    private NetworkTableEntry ntTimeStampEntry;
+    private NetworkTableEntry ntValidEntry;
 
     private MemoryManager memManager = new MemoryManager(125);
 
-    private int imgWidth, imgHeight;
+    // chameleon specific
+    private Pipeline currentPipeline;
+    private VisionProcess visionProcess;
+    private CameraValues camVals;
 
-    private void ChangeCameraValues(int Exposure, int Brightness) {
+    // cscore
-        SettingsManager.getInstance().UsbCameras.get(CameraName).setBrightness(Brightness);
+    private CvSink cvSink;
-        SettingsManager.getInstance().UsbCameras.get(CameraName).setExposureManual(Exposure);
+    private CvSource cvPublish;
+
+    // pipeline process items
+    private List<MatOfPoint> FoundContours = new ArrayList<>();
+    private List<MatOfPoint> FilteredContours = new ArrayList<>();
+    private List<RotatedRect> GroupedContours = new ArrayList<>();
+    private Mat cameraInputMat = new Mat();
+    private Mat hsvThreshMat = new Mat();
+    private Mat streamOutputMat = new Mat();
+    private Scalar contourRectColor = new Scalar(255, 0, 0);
+
+    private void ChangeCameraValues(int exposure, int brightness) {
+        SettingsManager.UsbCameras.get(CameraName).setBrightness(brightness);
+        SettingsManager.UsbCameras.get(CameraName).setExposureManual(exposure);
     }
 
     private void DriverModeListener(EntryNotification entryNotification) {
@@ -51,11 +71,11 @@ public class CameraProcess implements Runnable {
         }
     }
 
-    public CameraProcess(String CameraName) {
+    public CameraProcess(String cameraName) {
-        this.CameraName = CameraName;
+        CameraName = cameraName;
 
         // NetworkTables
-        NetworkTable ntTable = NetworkTableInstance.getDefault().getTable("/chameleon-vision/" + CameraName);
+        NetworkTable ntTable = NetworkTableInstance.getDefault().getTable("/chameleon-vision/" + cameraName);
         ntPipelineEntry = ntTable.getEntry("Pipeline");
         ntDriverModeEntry = ntTable.getEntry("Driver_Mode");
         ntPitchEntry = ntTable.getEntry("Pitch");
@@ -66,37 +86,77 @@ public class CameraProcess implements Runnable {
         ntDriverModeEntry.addListener(this::DriverModeListener, EntryListenerFlags.kUpdate);
         ntPipelineEntry.addListener(this::PipelineListener, EntryListenerFlags.kUpdate);
         ntDriverModeEntry.setBoolean(false);
-        ntPipelineEntry.setString(SettingsManager.CamerasCurrentPipeline.get(CameraName));
+        ntPipelineEntry.setString(SettingsManager.CamerasCurrentPipeline.get(cameraName));
-        imgWidth = SettingsManager.Cameras.get(CameraName).camVideoMode.width;
+
-        imgHeight = SettingsManager.Cameras.get(CameraName).camVideoMode.height;
+        // camera settings
+        camVals = new CameraValues(SettingsManager.Cameras.get(cameraName));
+        visionProcess = new VisionProcess(camVals);
+
+        // cscore setup
+        CameraServer cs = CameraServer.getInstance();
+        cvSink = cs.getVideo(SettingsManager.UsbCameras.get(cameraName));
+        cvPublish = cs.putVideo(cameraName, camVals.ImageWidth, camVals.ImageHeight);
+
+    }
+
+    private void drawContour(Mat inputMat, RotatedRect contourRect) {
+        if (contourRect == null) return;
+        List<MatOfPoint> drawnContour = new ArrayList<>();
+        Point[] vertices = new Point[4];
+        contourRect.points(vertices);
+        drawnContour.add(new MatOfPoint(vertices));
+        Imgproc.drawContours(inputMat, drawnContour, 0, contourRectColor, 3);
+        Imgproc.circle(inputMat, contourRect.center, 3, contourRectColor);
+    }
+
+    // TODO: Separate video output, contour drawing, data output to separate function, maybe even second thread
+    private PipelineResult runVisionProcess(Mat inputImage, Mat outputImage) {
+        var pipelineResult = new PipelineResult();
+
+        Scalar hsvLower = new Scalar(currentPipeline.hue.get(0), currentPipeline.saturation.get(0), currentPipeline.value.get(0));
+        Scalar hsvUpper = new Scalar(currentPipeline.hue.get(1), currentPipeline.saturation.get(1), currentPipeline.value.get(1));
+
+        visionProcess.HSVThreshold(inputImage, hsvThreshMat, hsvLower, hsvUpper, currentPipeline.erode, currentPipeline.dilate);
+
+        if (currentPipeline.is_binary == 1) {
+            Imgproc.cvtColor(hsvThreshMat, outputImage, Imgproc.COLOR_GRAY2BGR, 3);
+        } else {
+            inputImage.copyTo(outputImage);
+        }
+
+        FoundContours = visionProcess.FindContours(hsvThreshMat);
+        if (FoundContours.size() > 0) {
+            FilteredContours = visionProcess.FilterContours(FoundContours, currentPipeline.area, currentPipeline.ratio, currentPipeline.extent, currentPipeline.sort_mode, currentPipeline.target_intersection, currentPipeline.target_group);
+            if (FilteredContours.size() > 0) {
+                GroupedContours = visionProcess.GroupTargets(FilteredContours, currentPipeline.target_intersection, currentPipeline.target_group);
+                if (GroupedContours.size() > 0) {
+                    var finalRect = visionProcess.SortTargetsToOne(GroupedContours, currentPipeline.sort_mode);
+                    pipelineResult.IsValid = true;
+                    if (!currentPipeline.is_calibrated) {
+                        pipelineResult.CalibratedX = camVals.CenterX;
+                        pipelineResult.CalibratedY = camVals.CenterY;
+                    } else {
+                        pipelineResult.CalibratedX = (finalRect.center.y - currentPipeline.B) / currentPipeline.M;
+                        pipelineResult.CalibratedY = finalRect.center.x * currentPipeline.M + currentPipeline.B;
+                        pipelineResult.Pitch = camVals.CalculatePitch(finalRect.center.y, pipelineResult.CalibratedY);
+                        pipelineResult.Yaw = camVals.CalculateYaw(finalRect.center.x, pipelineResult.CalibratedX);
+                    }
+                    // Send calc using networktables
+                    // TODO Send pitch yaw distance and Raw Point using websockets to client for calib calc
+                    drawContour(outputImage, finalRect);
+                }
+            }
+        }
+
+        return pipelineResult;
     }
 
     @Override
     public void run() {
-        // camera values
-        var cv_sink = cs.getVideo(SettingsManager.UsbCameras.get(CameraName));
-        var cv_publish = cs.putVideo(CameraName, imgWidth, imgHeight);
-        double fov = SettingsManager.Cameras.get(CameraName).FOV;
-        CameraValues camVals = new CameraValues(imgWidth, imgHeight, fov);
-        VisionProcess visionProcess = new VisionProcess(camVals);
-        Pipeline currentPipeline;
-
-        // actual OpenCV objects
-        List<MatOfPoint> FoundContours = new ArrayList<>();
-        List<MatOfPoint> FilteredContours = new ArrayList<>();
-        List<RotatedRect> GroupedContours = new ArrayList<>();
-        Mat inputMat = new Mat();
-        Mat hsvThreshMat = new Mat();
-        Mat outputMat = new Mat();
-        Scalar contourColor = new Scalar(255, 0, 0);
-
         // processing time tracking
         long startTime;
         double processTimeMs;
         double fps;
-        //camera results
-        double CalibratedX, CalibratedY, Pitch, Yaw;
-        boolean isValid;
 
         while (!Thread.interrupted()) {
             FoundContours.clear();
@@ -108,63 +168,24 @@ public class CameraProcess implements Runnable {
 //            System.out.println(SettingsManager.CamerasCurrentPipeline.get(CameraName));
             // start fps counter right before grabbing input frame
             startTime = System.nanoTime();
-            cv_sink.grabFrame(inputMat);
+            cvSink.grabFrame(cameraInputMat);
-            if (inputMat.cols() == 0 && inputMat.rows() == 0) {
+            if (cameraInputMat.cols() == 0 && cameraInputMat.rows() == 0) {
                 continue;
             }
 
-            Scalar hsvLower = new Scalar(currentPipeline.hue.get(0), currentPipeline.saturation.get(0), currentPipeline.value.get(0));
+            // get vision data
-            Scalar hsvUpper = new Scalar(currentPipeline.hue.get(1), currentPipeline.saturation.get(1), currentPipeline.value.get(1));
+            var pipelineResult = runVisionProcess(cameraInputMat, streamOutputMat);
 
-            visionProcess.HSVThreshold(inputMat, hsvThreshMat, hsvLower, hsvUpper, currentPipeline.erode, currentPipeline.dilate);
+            cvPublish.putFrame(streamOutputMat);
-
-            if (currentPipeline.is_binary == 1) {
-                Imgproc.cvtColor(hsvThreshMat, outputMat, Imgproc.COLOR_GRAY2BGR, 3);
-            } else {
-                outputMat = inputMat;
-            }
-
-            FoundContours = visionProcess.FindContours(hsvThreshMat);
-            if (FoundContours.size() > 0) {
-                FilteredContours = visionProcess.FilterContours(FoundContours, currentPipeline.area, currentPipeline.ratio, currentPipeline.extent, currentPipeline.sort_mode, currentPipeline.target_intersection, currentPipeline.target_group);
-                if (FilteredContours.size() > 0) {
-                    GroupedContours = visionProcess.GroupTargets(FilteredContours, currentPipeline.target_intersection, currentPipeline.target_group);
-                    if (GroupedContours.size() > 0) {
-                        var finalRect = visionProcess.SortTargetsToOne(GroupedContours, currentPipeline.sort_mode);
-                        if (!currentPipeline.is_calibrated) {
-                            CalibratedX = camVals.CenterX;
-                            CalibratedY = camVals.CenterY;
-                        } else {
-                            CalibratedX = (finalRect.center.y - currentPipeline.B) / currentPipeline.M;
-                            CalibratedY = finalRect.center.x * currentPipeline.M + currentPipeline.B;
-                            Pitch = camVals.CalculatePitch(finalRect.center.y, CalibratedY);
-                            Yaw = camVals.CalculateYaw(finalRect.center.x, CalibratedX);
-                        }
-                        isValid = true;
-                        // Send calc using networktables
-                        // TODO Send pitch yaw distance and Raw Point using websockets to client for calib calc
-                        if (finalRect != null) {
-                            List<MatOfPoint> a = new ArrayList<>();
-                            Point[] vertices = new Point[4];
-                            finalRect.points(vertices);
-                            a.add(new MatOfPoint(vertices));
-                            Imgproc.drawContours(outputMat, a, 0, contourColor, 3);
-                        }
-
-                    } else {
-                        isValid = false;
-                    }
-                }
-            }
-
-            cv_publish.putFrame(outputMat);
             // calculate FPS after publishing output frame
             processTimeMs = (System.nanoTime() - startTime) * 1e-6;
             fps = 1000 / processTimeMs;
-//            System.out.printf("%s Process time: %fms, FPS: %.2f, FoundContours: %d, FilteredContours: %d, GroupedContours: %d\n",CameraName ,processTimeMs, fps, FoundContours.size(), FilteredContours.size(), GroupedContours.size());
+            System.out.printf("%s - Process time: %fms, FPS: %.2f, FoundContours: %d, FilteredContours: %d, GroupedContours: %d\n",CameraName ,processTimeMs, fps, FoundContours.size(), FilteredContours.size(), GroupedContours.size());
 
-            inputMat.release();
+            cameraInputMat.release();
             hsvThreshMat.release();
+
+            memManager.run(true);
         }
     }
 }

Main/src/main/java/com/chameleonvision/vision/process/PipelineResult.java

@@ -0,0 +1,9 @@
+package com.chameleonvision.vision.process;
+
+public class PipelineResult {
+	public boolean IsValid = false;
+	public double CalibratedX = 0.0;
+	public double CalibratedY = 0.0;
+	public double Pitch = 0.0;
+	public double Yaw = 0.0;
+}