V2.1 (#37)

* Toggle mode

* RangeSlider able to be disabled

* Added another window from the press of the button

* removed warning

* Major changes to colorpicker page

* Major changes to threshold tab

* fix for BGR not RGB

* Reverted changes of colorpicker.vue

* Updated Readme.md

Mainly Development Setup

* Update readme

small changes

* updated readme.md

Authors credit update

* updated readme.md 

added usb camera to hardware

* Updated UI readme

added nodejs installation and explanation

* Unit test vision (and implement networktable sending)

* finished color picker in the ui

* bugfix for correct tab

* cleanup and commenting

* Reveted changes to range slider

* bug fix for color picker

* Pipeline mat copy fix

* Added crosshair pipe

* compilation fix

* removed unsued comment

* moved calibration stuff to static class under 2d pipeline settings

* Added debug print commented

* Reverted subclass "Calibration"
Fixed bug where single point calibration causes crash when no target is found

* disable dual point crosshair for now

* Proper no target found fix, and snackbar error now showing

* Removed java fix for crash bug

* fixed problem with clear points in single point calibration

* change compatator to sqared values to make caluclation faster (no need for sqare root function)

* fixed roated camera too heigh for the UI

* Revert "fixed roated camera too heigh for the UI"

This reverts commit 50c8ecd345.

* fixed rotated camera too heigh for UI and corrected scalling

* updated pom to fix the camera buffer issue

* added pom variables

* Created new CaptureStaticProperties when rotating cam

* File stucture change, moved Pipeline implementation and their settings to /impl folder, refracted imports

* ignore auto created iml file

* ignore auto created iml file

* Fix lack of access problems after moving pipleline impl out of the same package

* Removed commented lines

* Fixed bug when selecting the same 90 deg rotation twice causes crosshair to move

* cleaned up single point calibration and bug fixed isBinary in driverMode

* Enum fix for the UI

* typo

* fixed Stream divisor problems see issue 19 in github issues

* fixed compilation error for test class

* Fixed problem when rotating camera with stream divisor

* Spelling + button style change

* Add speed limit to UI Updates

* Added runtime arg to fix settings permissions, and on-first-init

* Added FileHelper class to manage setting permissions for all files.

* Removed unnecessary argument

* NT can set pipeline to out on bound indexes

* bugfix in platform check

* added pipeline popup

* Round steam divisor resolution

* added regex test for camera name and pipe name

* Fix for Renaming pipeline leaves old config file #14

* Version change

* adde ( ) - and . to regex

* Replace NT timestamp with latency

* added back NT flush

* Add solvePNP, 3d tab on the UI, and some other misc bug fixes (#35)

* Rebase solvePNP on master

* added 3D tab minimap and csv reader

* More solvePNP

* Create draw pipe for pnp data

* SolvePNP piping work

* Move sorting into solvepnppipe

* Create calibration pipeline

* Update CalibrateSolvePNPPipeline.java

* add camera tilt angle

* Add calibration slider and snapshot button to 3D view

* Mirror updates in the socket handler

* add 3d calibration mode to the pipeline manager

* created calibration functions in ui and backend

* Start plumbing calibration

* Add snapshot and other handling to the RequestHandler

* added select resolution before starting calibration

* Rename solvePNPPipe to bounding box solve pnp pipe

* Update BoundingBoxSolvePNPPipe.java

* Add Mat serializer and CameraCalibrationConfig

* Begun calibration saving, fixed UI/Backend snapshot count mismatch

* Add (unplumbed) option to set checkerboard size

This will allow users to change the units their calibration is in

* Create chessboard.png

* Fix calibration NPE

* changed string serialization to a json send

* bug fixed cancellation button

* Fix spelling of snapshot in 3d.vue

* Plumb resolution change

* Set resolution during config, start on config serialization

* Update .gitignore

* Config fixes

* Start transition away from cvpipeline3d

* fix NPE on uncalibrated cameras

* clear list on fail

* Fix video mode index error

* ignore getters in camera calibration config

* Create json constructor for jsonmat

* get solvePNP mostly returning sane values

* Fix solvePNP bug and add unit test

* FIx calibration mat truncation

* added capture amount model upload and minimap data

* Standardize on meters in calibration and bounding box

* fix json out of bounds and handle null calibration more gracefully

* don't put text on calibrate image, go back to inches

* convert distance to meters

this means calibration will need to be in inches

* Actually save raw contor

* Update GroupContoursPipe.java

* Add all calibration return to camera capture

* hard code 2019 target

* bugfixed draw2d added fail calib popup, merge end and cancel

added the res index to the calib start

* Clarify error message and draw more fancy rectangles

* Cleanup memory in solvepnp

* re did minimap component

* fix npe if left/right is null

* remove references to 2d

* try-catch running the current pipeline

* Add method to find corners using the harris corner detector

* Possibly fix left/right missmatch

* Fix 3D Tab error

* FIx file permissions, mat serializer adjustments

* fixed mini map for field coordinates

* mini map changes fov

* Update SolvePNPPipe.java

* get rid of target corners

* some memory leak fixes

* fixed mini map location

* added position under minimap

* changed player fov look

* put all targets in the web send

* re did target send to ui added target tables, bugfix calibration

* fixed y position

* Add tilt angle to capture properties

* maybe fix y axis in minimap

* Add square size to onCalibrationEnding

* Possibly add square size to UI

* fix NPE with pitch

* Fix bug with sending multiple targets

* Only instantiate 3d stuff if we are in 3d mode

* Fix array list exceptions

* Fix bug in sort contors

list was truncated too early

* added download chess, tilt setting and ordinal tilt,

* added square size connection

* removed unused code

* Update pom version to 2.1-RELEASE

* Send camera calibrations to UI

* Stream pose list to a LIst

* Only stream necessary parts of the aux list entry

* Make broadcastMessage synchronized to prevent ConcurrentModificationExceptions

* added fps counter changed squaresize steps bug fixes in tables

* bugfix camera settings cam wont change

Authored-by: oriagranat9 <oriagranat9@gmail.com>

* disable pose print

* standardize on calibration in inches and add square size as a divisor

This should do the same thing as calibrating in the correct units but it doesnt like meeeeee

* updated pom for release, 3d calibration change, cosmetic changes

* WIP: 3D res filter

* bug fix in index parsing

* add good features to track point finder

* offset found corners by old tl position

* Add method to find most extreme per-quadrant points for solvePNP

* re did pipeline duplication

* Add release method to standard cv pipeline

* remove contor from tracked target

* Explicitly release all the intermediate results

* avoid creating new mats in group contors pipe

* removed mat release

* updated pom

* Actually cache the last target

We were trying to but i never hooked this part up

* Fix memory leak in sort contros pipe

* Fix memory leak in sort contors pipe

* Help more with memory usage in speckle reject and draw pipes

* minor bugfixes to the ui

* Start moving stream into vision process

Should probs move into a pipe to be idimatic

* justify bug fix

* Fix sort left/right bug

* Fix target grouping bug

* Avoid allocating a new mat in solvePNP, perf. increases in group contors pipe

* Event scripts (#36)

* Begin scripting work

* More scripting work

* Finalize scripting system

* Begin implementing script events

* Finalize script system

Co-authored-by: Banks T <btrout.dhrs@gmail.com>

Co-authored-by: OmerZ7 <zipory.omer@gmail.com>
Co-authored-by: Matt <matthew.morley.ca@gmail.com>
Co-authored-by: Banks T <btrout.dhrs@gmail.com>

chameleon-server/src/main/java/com/chameleonvision/vision/VisionProcess.java

@@ -1,17 +1,26 @@
 package com.chameleonvision.vision;
 
 import com.chameleonvision.Debug;
+import com.chameleonvision.config.CameraCalibrationConfig;
+import com.chameleonvision.config.CameraConfig;
 import com.chameleonvision.config.ConfigManager;
+import com.chameleonvision.scripting.ScriptEventType;
+import com.chameleonvision.scripting.ScriptManager;
+import com.chameleonvision.config.FullCameraConfiguration;
 import com.chameleonvision.util.LoopingRunnable;
-import com.chameleonvision.vision.camera.CameraCapture;
+import com.chameleonvision.util.MathHandler;
 import com.chameleonvision.vision.camera.CameraStreamer;
 import com.chameleonvision.vision.camera.USBCameraCapture;
 import com.chameleonvision.vision.pipeline.*;
+import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
+import com.chameleonvision.vision.pipeline.impl.DriverVisionPipeline;
+import com.chameleonvision.vision.pipeline.impl.StandardCVPipelineSettings;
 import com.chameleonvision.web.SocketHandler;
 import com.fasterxml.jackson.core.JsonProcessingException;
 import com.fasterxml.jackson.databind.ObjectMapper;
 import edu.wpi.cscore.VideoMode;
 import edu.wpi.first.networktables.*;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpiutil.CircularBuffer;
 import org.apache.commons.lang3.tuple.Pair;
 import org.opencv.core.Mat;
@@ -21,6 +30,7 @@ import java.util.HashMap;
 import java.util.List;
 import java.util.concurrent.BlockingQueue;
 import java.util.concurrent.LinkedBlockingDeque;
+import java.util.stream.Collectors;
 
 
 public class VisionProcess {
@@ -28,6 +38,7 @@ public class VisionProcess {
     private final USBCameraCapture cameraCapture;
     private final CameraStreamerRunnable streamRunnable;
     private final VisionProcessRunnable visionRunnable;
+    private final CameraConfig fileConfig;
     public final CameraStreamer cameraStreamer;
     public PipelineManager pipelineManager;
 
@@ -37,6 +48,7 @@ public class VisionProcess {
 
     // network table stuff
     private final NetworkTable defaultTable;
+    private NetworkTableInstance tableInstance;
     private NetworkTableEntry ntPipelineEntry;
     public NetworkTableEntry ntDriverModeEntry;
     private int ntDriveModeListenerID;
@@ -45,17 +57,22 @@ public class VisionProcess {
     private NetworkTableEntry ntPitchEntry;
     private NetworkTableEntry ntAuxListEntry;
     private NetworkTableEntry ntAreaEntry;
-    private NetworkTableEntry ntTimeStampEntry;
+    private NetworkTableEntry ntLatencyEntry;
     private NetworkTableEntry ntValidEntry;
+    private NetworkTableEntry ntPoseEntry;
     private ObjectMapper objectMapper = new ObjectMapper();
 
-    VisionProcess(USBCameraCapture cameraCapture, String name, List<CVPipelineSettings> loadedPipelineSettings) {
+    private long lastUIUpdateMs = 0;
+
+    VisionProcess(USBCameraCapture cameraCapture, FullCameraConfiguration config) {
         this.cameraCapture = cameraCapture;
 
-        pipelineManager = new PipelineManager(this, loadedPipelineSettings);
+        fileConfig = config.fileConfig;
+
+        pipelineManager = new PipelineManager(this, config.pipelines);
 
         // Thread to put frames on the dashboard
-        this.cameraStreamer = new CameraStreamer(cameraCapture, name);
+        this.cameraStreamer = new CameraStreamer(cameraCapture, config.cameraConfig.name, pipelineManager.getCurrentPipeline().settings.streamDivisor);
         this.streamRunnable = new CameraStreamerRunnable(30, cameraStreamer);
 
         // Thread to process vision data
@@ -74,10 +91,10 @@ public class VisionProcess {
         visionThread.setName(getCamera().getProperties().name + " - Vision Thread");
         visionThread.start();
 
-        System.out.println("Starting stream thread.");
-        var streamThread = new Thread(streamRunnable);
-        streamThread.setName(getCamera().getProperties().name + " - Stream Thread");
-        streamThread.start();
+//        System.out.println("Starting stream thread.");
+//        var streamThread = new Thread(streamRunnable);
+//        streamThread.setName(getCamera().getProperties().name + " - Stream Thread");
+//        streamThread.start();
     }
 
     /**
@@ -99,14 +116,16 @@ public class VisionProcess {
     }
 
     private void initNT(NetworkTable newTable) {
+        tableInstance = newTable.getInstance();
         ntPipelineEntry = newTable.getEntry("pipeline");
         ntDriverModeEntry = newTable.getEntry("driver_mode");
         ntPitchEntry = newTable.getEntry("pitch");
         ntYawEntry = newTable.getEntry("yaw");
         ntAreaEntry = newTable.getEntry("area");
-        ntTimeStampEntry = newTable.getEntry("timestamp");
+        ntLatencyEntry = newTable.getEntry("latency");
         ntValidEntry = newTable.getEntry("is_valid");
         ntAuxListEntry = newTable.getEntry("aux_targets");
+        ntPoseEntry = newTable.getEntry("poseList");
         ntDriveModeListenerID = ntDriverModeEntry.addListener(this::setDriverMode, EntryListenerFlags.kUpdate);
         ntPipelineListenerID = ntPipelineEntry.addListener(this::setPipeline, EntryListenerFlags.kUpdate);
         ntDriverModeEntry.setBoolean(false);
@@ -120,11 +139,13 @@ public class VisionProcess {
 
     public void setDriverMode(boolean driverMode) {
         pipelineManager.setDriverMode(driverMode);
+        ScriptManager.queueEvent(driverMode ? ScriptEventType.kEnterDriverMode : ScriptEventType.kExitDriverMode);
         SocketHandler.sendFullSettings();
     }
 
     /**
      * Method called by the nt entry listener to update the next pipeline.
+     *
      * @param notification the notification
      */
     private void setPipeline(EntryNotification notification) {
@@ -136,76 +157,101 @@ public class VisionProcess {
 
         // if it's null, we haven't even started the program yet, so just return
         // otherwise, set it.
-        if(ntDriverModeEntry != null) {
+        if (ntDriverModeEntry != null) {
             ntDriverModeEntry.setBoolean(isDriverMode);
         }
     }
 
     private void updateUI(CVPipelineResult data) {
-        if(cameraCapture.getProperties().name.equals(ConfigManager.settings.currentCamera)) {
-            HashMap<String, Object> WebSend = new HashMap<>();
-            HashMap<String, Object> point = new HashMap<>();
-            HashMap<String, Object> calculated = new HashMap<>();
-            List<Double> center = new ArrayList<>();
-            if (data.hasTarget) {
-                if(data instanceof CVPipeline2d.CVPipeline2dResult) {
-                    CVPipeline2d.CVPipeline2dResult result = (CVPipeline2d.CVPipeline2dResult) data;
-                    CVPipeline2d.Target2d bestTarget = result.targets.get(0);
-                    center.add(bestTarget.rawPoint.center.x);
-                    center.add(bestTarget.rawPoint.center.y);
-                    calculated.put("pitch", bestTarget.pitch);
-                    calculated.put("yaw", bestTarget.yaw);
-                    calculated.put("area", bestTarget.area);
-                } else if (data instanceof CVPipeline3d.CVPipeline3dResult) {
-                    // TODO: (2.1) 3d stuff in UI
+        // 30 "FPS" update rate
+        long currentMillis = System.currentTimeMillis();
+        if (currentMillis - lastUIUpdateMs > 1000 / 30) {
+            lastUIUpdateMs = currentMillis;
+
+
+            if (cameraCapture.getProperties().name.equals(ConfigManager.settings.currentCamera)) {
+                HashMap<String, Object> WebSend = new HashMap<>();
+                HashMap<String, Object> point = new HashMap<>();
+                HashMap<String, Object> pointMap = new HashMap<>();
+                ArrayList<Object> webTargets = new ArrayList<Object>();
+                List<Double> center = new ArrayList<>();
+
+
+
+                if (data.hasTarget) {
+                    if (data instanceof StandardCVPipeline.StandardCVPipelineResult) {
+                        StandardCVPipeline.StandardCVPipelineResult result = (StandardCVPipeline.StandardCVPipelineResult) data;
+                        StandardCVPipeline.TrackedTarget bestTarget = result.targets.get(0);
+                        if (((StandardCVPipelineSettings) pipelineManager.getCurrentPipeline().settings).multiple) {
+                            for (var target : result.targets) {
+                                pointMap = new HashMap<>();
+                                pointMap.put("pitch", target.pitch);
+                                pointMap.put("yaw", target.yaw);
+                                pointMap.put("area", target.area);
+                                pointMap.put("pose", target.cameraRelativePose);
+                                webTargets.add(pointMap);
+                            }
+                        } else {
+                            pointMap.put("pitch", bestTarget.pitch);
+                            pointMap.put("yaw", bestTarget.yaw);
+                            pointMap.put("area", bestTarget.area);
+                            pointMap.put("pose", bestTarget.cameraRelativePose);
+                            webTargets.add(pointMap);
+                        }
+                        center.add(bestTarget.minAreaRect.center.x);
+                        center.add(bestTarget.minAreaRect.center.y);
+
+                    }
                 } else {
+                    pointMap.put("pitch", null);
+                    pointMap.put("yaw", null);
+                    pointMap.put("area", null);
+                    pointMap.put("pose", new Pose2d());
+                    webTargets.add(pointMap);
                     center.add(null);
                     center.add(null);
-                    calculated.put("pitch", null);
-                    calculated.put("yaw", null);
-                    calculated.put("area", null);
                 }
-            } else {
-                center.add(null);
-                center.add(null);
-                calculated.put("pitch", null);
-                calculated.put("yaw", null);
-                calculated.put("area", null);
+
+                point.put("fps", visionRunnable.fps);
+                point.put("targets", webTargets);
+                point.put("rawPoint", center);
+                WebSend.put("point", point);
+                SocketHandler.broadcastMessage(WebSend);
             }
-            point.put("fps", visionRunnable.fps);
-            point.put("calculated", calculated);
-            point.put("rawPoint", center);
-            WebSend.put("point", point);
-            SocketHandler.broadcastMessage(WebSend);
         }
     }
 
     private void updateNetworkTableData(CVPipelineResult data) {
         ntValidEntry.setBoolean(data.hasTarget);
-        if(data.hasTarget && !(data instanceof DriverVisionPipeline.DriverPipelineResult)) {
-            if(data instanceof CVPipeline2d.CVPipeline2dResult) {
+        if (data.hasTarget && !(data instanceof DriverVisionPipeline.DriverPipelineResult)) {
+            if (data instanceof StandardCVPipeline.StandardCVPipelineResult) {
 
                 //noinspection unchecked
-                List<CVPipeline2d.Target2d> targets = (List<CVPipeline2d.Target2d>) data.targets;
-                ntTimeStampEntry.setDouble(data.imageTimestamp);
+                List<StandardCVPipeline.TrackedTarget> targets = (List<StandardCVPipeline.TrackedTarget>) data.targets;
+                ntLatencyEntry.setDouble(MathHandler.roundTo(data.processTime * 1e-6, 3));
                 ntPitchEntry.setDouble(targets.get(0).pitch);
                 ntYawEntry.setDouble(targets.get(0).yaw);
                 ntAreaEntry.setDouble(targets.get(0).area);
                 try {
-                    ntAuxListEntry.setString(objectMapper.writeValueAsString(targets));
+                    ntAuxListEntry.setString(objectMapper.writeValueAsString(targets.stream()
+                            .map(it -> List.of(it.pitch, it.yaw, it.area, it.cameraRelativePose))
+                            .collect(Collectors.toList())));
+
+                    // TODO: (2.1) 3d stuff...
+                    ntPoseEntry.setString(objectMapper.writeValueAsString(targets.stream().map(target -> target.cameraRelativePose).collect(Collectors.toList())));
                 } catch (JsonProcessingException e) {
                     e.printStackTrace();
                 }
-            } else if (data instanceof CVPipeline3d.CVPipeline3dResult) {
-                // TODO: (2.1) 3d stuff...
+            } else {
+                ntPitchEntry.setDouble(0.0);
+                ntYawEntry.setDouble(0.0);
+                ntAreaEntry.setDouble(0.0);
+                ntLatencyEntry.setDouble(0.0);
+                ntAuxListEntry.setString("");
             }
-        } else {
-            ntPitchEntry.setDouble(0.0);
-            ntYawEntry.setDouble(0.0);
-            ntAreaEntry.setDouble(0.0);
-            ntTimeStampEntry.setDouble(0.0);
-            ntAuxListEntry.setString("");
         }
+        tableInstance.flush();
+
     }
 
     public void setVideoMode(VideoMode newMode) {
@@ -229,6 +275,27 @@ public class VisionProcess {
         return pipelineManager.driverModePipeline.settings;
     }
 
+    public void addCalibration(CameraCalibrationConfig cal) {
+        cameraCapture.addCalibrationData(cal);
+        System.out.println("saving to file");
+        fileConfig.saveCalibration(cameraCapture.getConfig());
+    }
+
+    public void setIs3d(Boolean value) {
+        var settings = pipelineManager.getCurrentPipeline().settings;
+        if (settings instanceof StandardCVPipelineSettings) {
+            ((StandardCVPipelineSettings) settings).is3D = value;
+        }
+    }
+
+    public boolean getIs3d() {
+        var settings = pipelineManager.getCurrentPipeline().settings;
+        if (settings instanceof StandardCVPipelineSettings) {
+            return ((StandardCVPipelineSettings) settings).is3D;
+        }
+        return false;
+    }
+
     /**
      * VisionProcessRunnable will process images as quickly as possible
      */
@@ -240,14 +307,22 @@ public class VisionProcess {
         @Override
         public void run() {
             var lastUpdateTimeNanos = System.nanoTime();
-            while(!Thread.interrupted()) {
+            var lastStreamTimeMs = System.currentTimeMillis();
+            while (!Thread.interrupted()) {
 
                 // blocking call, will block until camera has a new frame.
                 Pair<Mat, Long> camData = cameraCapture.getFrame();
 
                 Mat camFrame = camData.getLeft();
                 if (camFrame.cols() > 0 && camFrame.rows() > 0) {
-                    CVPipelineResult result = pipelineManager.getCurrentPipeline().runPipeline(camFrame);
+                    CVPipelineResult result = null;
+                    try {
+                        result = pipelineManager.getCurrentPipeline().runPipeline(camFrame);
+                    } catch (Exception e) {
+                        System.err.println("Exception in vision process " + getCamera().getProperties().getNickname() + "!");
+                        e.printStackTrace();
+                    }
+
                     camFrame.release();
 
                     if (result != null) {
@@ -259,8 +334,16 @@ public class VisionProcess {
                 }
 
                 try {
-                    streamFrameQueue.clear();
-                    streamFrameQueue.add(lastPipelineResult.outputMat);
+//                    streamFrameQueue.clear();
+//                    streamFrameQueue.add(lastPipelineResult.outputMat);
+                    var currentTime = System.currentTimeMillis();
+                    if((currentTime - lastStreamTimeMs)/1000d > 1.0 / 30.0) {
+                        cameraStreamer.runStream(lastPipelineResult.outputMat);
+//                        System.out.println("Ran stream in " + (System.currentTimeMillis() - currentTime) + "ms!");
+                        lastStreamTimeMs = currentTime;
+                        lastPipelineResult.outputMat.release();
+                    }
+
                 } catch (Exception e) {
                     Debug.printInfo("Vision running faster than stream.");
                 }
@@ -274,7 +357,7 @@ public class VisionProcess {
 
         double getAverageFPS() {
             var temp = 0.0;
-            for(int i = 0; i < 7; i++) {
+            for (int i = 0; i < 7; i++) {
                 temp += fpsAveragingBuffer.get(i);
             }
             temp /= 7.0;
@@ -290,7 +373,7 @@ public class VisionProcess {
 
         private CameraStreamerRunnable(int cameraFPS, CameraStreamer streamer) {
             // add 2 FPS to allow for a bit of overhead
-            super(1000L/(cameraFPS + 2));
+            super(1000L / (cameraFPS + 2));
             this.streamer = streamer;
         }