diff --git a/Main/src/main/java/com/chameleonvision/settings/SettingsManager.java b/Main/src/main/java/com/chameleonvision/settings/SettingsManager.java index cd922368f..f10254417 100644 --- a/Main/src/main/java/com/chameleonvision/settings/SettingsManager.java +++ b/Main/src/main/java/com/chameleonvision/settings/SettingsManager.java @@ -190,6 +190,13 @@ public class SettingsManager { throw new NoCameraException(); } + public UsbCamera GetCurrentUsbCamera() throws NoCameraException { + if (!GeneralSettings.curr_camera.equals("")) { + return UsbCameras.get(GeneralSettings.curr_camera); + } + throw new NoCameraException(); + } + public List GetResolutionList() throws NoCameraException { if (!GeneralSettings.curr_camera.equals("")) { List list = new ArrayList(); @@ -206,8 +213,6 @@ public class SettingsManager { if (Cameras.containsKey(CamName)) { GeneralSettings.curr_camera = CamName; GeneralSettings.curr_pipeline = GetCurrentCamera().pipelines.keySet().stream().findFirst().toString(); - - } } 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 a29a30126..0434f3bd6 100644 --- a/Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java +++ b/Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java @@ -8,6 +8,7 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.cameraserver.CameraServer; +import org.apache.commons.math3.stat.descriptive.moment.Mean; import org.opencv.core.*; import org.opencv.imgproc.Imgproc; @@ -25,7 +26,7 @@ public class CameraProcess implements Runnable { private int imgWidth, imgHeight; - public CameraProcess(String CameraName){ + public CameraProcess(String CameraName) { this.CameraName = CameraName; // add pipeline @@ -34,16 +35,15 @@ public class CameraProcess implements Runnable { // NetworkTables NetworkTable ntTable = NetworkTableInstance.getDefault().getTable("/chameleon-vision/" + CameraName); ntPipelineEntry = ntTable.getEntry("Pipeline"); - ntDriverModeEntry = ntTable.getEntry("Driver_Mode"); + ntDriverModeEntry = ntTable.getEntry("Driver_Mode"); imgWidth = SettingsManager.Cameras.get(CameraName).camVideoMode.width; imgHeight = SettingsManager.Cameras.get(CameraName).camVideoMode.height; } - - @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; @@ -51,69 +51,72 @@ public class CameraProcess implements Runnable { VisionProcess visionProcess = new VisionProcess(camVals); Pipeline currentPipeline; + // actual OpenCV objects List FoundContours = new ArrayList<>(); List FilteredContours = new ArrayList<>(); List GroupedContours = new ArrayList<>(); Mat inputMat = new Mat(); - Mat bgrMat = new Mat(); Mat hsvThreshMat = new Mat(); Mat outputMat = new Mat(); - Mat contourBoxPointsMat = new Mat(); Scalar contourColor = new Scalar(255, 0, 0); - long startTime, endTime; - startTime = System.nanoTime(); - int duration = 1; - int counter = 0; - double fps = 0; - while (!Thread.interrupted()) { + // processing time tracking + long startTime; + double processTimeMs; + double fps; + + while (!Thread.interrupted()) { FoundContours.clear(); FilteredContours.clear(); + GroupedContours.clear(); currentPipeline = SettingsManager.Cameras.get(CameraName).pipelines.get(SettingsManager.CamerasCurrentPipeline.get(CameraName)); + // start fps counter right before grabbing input frame + startTime = System.nanoTime(); cv_sink.grabFrame(inputMat); - if (inputMat.cols() == 0 && inputMat.rows() == 0) { continue; } - - // Imgproc.cvtColor(inputMat, bgrMat, Imgproc.COLOR_RGB2BGR, 3); + if (inputMat.cols() == 0 && inputMat.rows() == 0) { + continue; + } 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(inputMat, hsvThreshMat, hsvLower, hsvUpper, currentPipeline.erode, currentPipeline.dilate); - FoundContours = visionProcess.FindContours(hsvThreshMat); - FilteredContours = visionProcess.FilterContours(FoundContours, currentPipeline.area, currentPipeline.ratio, currentPipeline.extent, currentPipeline.sort_mode, currentPipeline.target_intersection, currentPipeline.target_group); - GroupedContours = visionProcess.GroupTargets(FilteredContours,currentPipeline.target_intersection,currentPipeline.target_group); if (currentPipeline.is_binary == 1) { - Imgproc.cvtColor(hsvThreshMat, hsvThreshMat, Imgproc.COLOR_GRAY2BGR, 3); - outputMat = hsvThreshMat; + Imgproc.cvtColor(hsvThreshMat, outputMat, Imgproc.COLOR_GRAY2BGR, 3); } else { outputMat = inputMat; } - if (GroupedContours.size() > 0) { - List a = new ArrayList<>(); - Point[] vertices = new Point[4]; - GroupedContours.get(0).points(vertices); - a.add(new MatOfPoint(vertices)); - Imgproc.drawContours(outputMat,a, 0, contourColor, 3); + 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 (finalRect != null) { + List a = new ArrayList<>(); + Point[] vertices = new Point[4]; + finalRect.points(vertices); + a.add(new MatOfPoint(vertices)); + Imgproc.drawContours(outputMat, a, 0, contourColor, 3); + } + } + } } + cv_publish.putFrame(outputMat); - System.out.println("fps: " + fps); + + // calculate FPS after publishing output frame + processTimeMs = (System.nanoTime() - startTime) * 1e-6; + fps = 1000 / processTimeMs; + System.out.printf("Process time: %fms, FPS: %.2f, FoundContours: %d, FilteredContours: %d, GroupedContours: %d\n", processTimeMs, fps, FoundContours.size(), FilteredContours.size(), GroupedContours.size()); + inputMat.release(); hsvThreshMat.release(); - for (MatOfPoint oldMat : FoundContours) { oldMat.release(); } - for (MatOfPoint oldMat1 : FilteredContours) { oldMat1.release(); } - memManager.run(); - counter++; - if ((System.nanoTime() - startTime)*1e-9 > duration){ - fps = (counter / ((System.nanoTime() - startTime)*1e-9 )); - counter = 0; - startTime = System.nanoTime(); - } - } - } } diff --git a/Main/src/main/java/com/chameleonvision/vision/process/VisionProcess.java b/Main/src/main/java/com/chameleonvision/vision/process/VisionProcess.java index b7c987daf..fdba65375 100644 --- a/Main/src/main/java/com/chameleonvision/vision/process/VisionProcess.java +++ b/Main/src/main/java/com/chameleonvision/vision/process/VisionProcess.java @@ -1,13 +1,13 @@ package com.chameleonvision.vision.process; import com.chameleonvision.vision.CameraValues; +import org.apache.commons.math3.util.FastMath; import org.jetbrains.annotations.NotNull; import org.opencv.core.*; import org.opencv.imgproc.*; -import java.util.ArrayList; -import java.util.HashMap; -import java.util.List; +import java.util.*; +import java.util.stream.Collectors; public class VisionProcess { @@ -28,7 +28,6 @@ public class VisionProcess { private Mat Kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(5, 5)); private Mat hsvImage = new Mat(); - void HSVThreshold(Mat srcImage, Mat dst, @NotNull Scalar hsvLower, @NotNull Scalar hsvUpper, boolean shouldErode, boolean shouldDilate) { Imgproc.cvtColor(srcImage, hsvImage, Imgproc.COLOR_RGB2HSV,3); Core.inRange(hsvImage, hsvLower, hsvUpper, dst); @@ -61,7 +60,7 @@ public class VisionProcess { continue; } var rect = Imgproc.minAreaRect(new MatOfPoint2f(Contour.toArray())); - var targetFullness = (contourArea/rect.size.area())*100; + var targetFullness = (contourArea / rect.size.area()) * 100; if (targetFullness <= extent.get(0) || targetArea >= extent.get(1)){ continue; } @@ -76,16 +75,81 @@ public class VisionProcess { return FilteredContours; } + private static Comparator SortByLargestComparator = (rect1, rect2) -> Double.compare(rect2.size.area(), rect1.size.area()); + private static Comparator SortBySmallestComparator = SortByLargestComparator.reversed(); + + private static Comparator SortByHighestComparator = (rect1, rect2) -> Double.compare(rect2.center.y, rect1.center.y); + private static Comparator SortByLowestComparator = SortByHighestComparator.reversed(); + + private static Comparator SortByLeftmostComparator = Comparator.comparingDouble(rect -> rect.center.x); + private static Comparator SortByRightmostComparator = SortByLeftmostComparator.reversed(); + + private double calcDistance(RotatedRect rect) { + return FastMath.sqrt(FastMath.pow(CamVals.CenterX - rect.center.x, 2) + FastMath.pow(CamVals.CenterY - rect.center.y, 2)); + } + + private Comparator SortByCentermostComparator = Comparator.comparingDouble(this::calcDistance); + + RotatedRect SortTargetsToOne(List inputRects, String sortMode) { + switch (sortMode) { + case "Largest": + return Collections.max(inputRects, Comparator.comparing(rect -> rect.size.area())); + case "Smallest": + return Collections.min(inputRects, Comparator.comparing(rect -> rect.size.area())); + case "Highest": + return Collections.min(inputRects, Comparator.comparing(rect -> rect.center.y)); + case "Lowest": + return Collections.max(inputRects, Comparator.comparing(rect -> rect.center.y)); + case "Leftmost": + return Collections.min(inputRects, Comparator.comparing(rect -> rect.center.x)); + case "Rightmost": + return Collections.max(inputRects, Comparator.comparing(rect -> rect.center.x)); + case "Centermost": + return inputRects.stream().sorted(SortByCentermostComparator).collect(Collectors.toList()).get(0); + default: + return inputRects.get(0); // default to whatever the first contour is, but this should never happen + } + } + + + void SortTargets(List inputRects, String sortMode) { + switch (sortMode) { + case "Largest": + inputRects.sort(SortByLargestComparator); + break; + case "Smallest": + inputRects.sort(SortBySmallestComparator); + break; + case "Highest": + inputRects.sort(SortByHighestComparator); + break; + case "Lowest": + inputRects.sort(SortByLowestComparator); + break; + case "Leftmost": + inputRects.sort(SortByLeftmostComparator); + break; + case "Rightmost": + inputRects.sort(SortByRightmostComparator); + break; + case "Centermost": + inputRects.sort(SortByCentermostComparator); + break; + default: + break; + } + } + private List FinalCountours = new ArrayList<>(); - public List GroupTargets(List InputContours, String IntersectionPoint, String TargetGroup) { + List GroupTargets(List InputContours, String IntersectionPoint, String TargetGroup) { FinalCountours.clear(); if (!TargetGroup.equals("Single")){ for (var i = 0; i < InputContours.size(); i++){ List FinalContourList = new ArrayList<>(InputContours.get(i).toList()); - for (var c = 0; c < (TargetGrouping.get(TargetGroup)-1);c++){ + for (var c = 0; c < (TargetGrouping.get(TargetGroup) - 1); c++){ try{ MatOfPoint firstContour = InputContours.get(i + c); - MatOfPoint secondContour = InputContours.get(i+c+1); + MatOfPoint secondContour = InputContours.get(i + c + 1); if (IsIntersecting(firstContour, secondContour, IntersectionPoint)){ FinalContourList.addAll(secondContour.toList()); } @@ -93,7 +157,7 @@ public class VisionProcess { secondContour.release(); MatOfPoint2f contour = new MatOfPoint2f(); contour.fromList(FinalContourList); - if (contour.cols() !=0 && contour.rows() != 0){ + if (contour.cols() != 0 && contour.rows() != 0){ RotatedRect rect = Imgproc.minAreaRect(contour); FinalCountours.add(rect); } @@ -105,10 +169,10 @@ public class VisionProcess { } } else { - for (var i = 0; i < InputContours.size(); i++){ + for (MatOfPoint inputContour : InputContours) { MatOfPoint2f contour = new MatOfPoint2f(); - contour.fromArray(InputContours.get(i).toArray()); - if (contour.cols() !=0 && contour.rows() != 0) { + contour.fromArray(inputContour.toArray()); + if (contour.cols() != 0 && contour.rows() != 0) { RotatedRect rect = Imgproc.minAreaRect(contour); FinalCountours.add(rect); } diff --git a/Main/src/main/java/com/chameleonvision/web/Server.java b/Main/src/main/java/com/chameleonvision/web/Server.java index fc8f504d4..b6b4fdf7d 100644 --- a/Main/src/main/java/com/chameleonvision/web/Server.java +++ b/Main/src/main/java/com/chameleonvision/web/Server.java @@ -38,42 +38,67 @@ public class Server { broadcastMessage(ctx, ctx.message()); JSONObject jsonObject = new JSONObject(ctx.message()); String key = null; + var jsonKeySetArray = jsonObject.keySet().toArray(); try { - key = jsonObject.keySet().toArray()[0].toString(); + key = jsonKeySetArray[0].toString(); } catch (Exception ex) { - ex.printStackTrace(); + System.err.println("WebSocket JSON data was empty!"); } if (key == null) return; Object value = jsonObject.get(key); +// System.out.printf("Got websocket json data: [%s, %s]\n", key, value); if (!setField(SettingsManager.getInstance().GetCurrentPipeline(), key, value)) { //If field not in pipeline switch (key) { case "change_general_settings_values": JSONObject newSettings = (JSONObject) value; - setFields(SettingsManager.getInstance().GeneralSettings, newSettings); + setFields(SettingsManager.GeneralSettings, newSettings); break; case "curr_camera": - SettingsManager.getInstance().SetCurrentCamera((String) value); + String newCamera = (String) value; + System.out.printf("Changing camera to %s\n", newCamera); + SettingsManager.getInstance().SetCurrentCamera(newCamera); //broadcastMessage((Map) new HashMap(){}.put("port",SettingsManager.CameraPorts.get(SettingsManager.GeneralSettings.curr_camera))); -// broadcastMessage(ctx, SettingsManager.getInstance().GetCurrentCamera());//TODO CHECK JSON FOR CAMERA CHANGE + //broadcastMessage(ctx, SettingsManager.getInstance().GetCurrentCamera()); //TODO CHECK JSON FOR CAMERA CHANGE break; case "curr_pipeline": - SettingsManager.getInstance().SetCurrentPipeline((String) value); - SettingsManager.CamerasCurrentPipeline.put(SettingsManager.GeneralSettings.curr_camera, (String) value); + String newPipeline = (String) value; + System.out.printf("Changing pipeline to %s\n", newPipeline); + SettingsManager.getInstance().SetCurrentPipeline(newPipeline); + SettingsManager.CamerasCurrentPipeline.put(SettingsManager.GeneralSettings.curr_camera, newPipeline); break; case "resolution": - System.out.println("change res"); - SettingsManager.getInstance().GetCurrentCamera().resolution = (int) value; - SettingsManager.getInstance().SetCameraSettings(SettingsManager.GeneralSettings.curr_camera, "resolution", value); + int newResolution = (int) value; + System.out.printf("Changing resolution mode to %d\n", newResolution); + SettingsManager.getInstance().GetCurrentCamera().resolution = newResolution; + SettingsManager.getInstance().SetCameraSettings(SettingsManager.GeneralSettings.curr_camera, "resolution", newResolution); SettingsManager.getInstance().SaveSettings(); break; case "fov": - System.out.println("change fov"); - SettingsManager.getInstance().GetCurrentCamera().FOV = (double) value; + double newFov = (double) value; + System.out.printf("Changing FOV to %d\n", newFov); + SettingsManager.getInstance().GetCurrentCamera().FOV = newFov; SettingsManager.getInstance().SaveSettings(); break; default: - System.out.println("Unexpected value"); + System.out.printf("Unexpected value from websocket: [%s, %s]\n", key, value); + break; + } + } else { // + switch (key) { + case "exposure": + int newExposure = (int) value; + System.out.printf("Changing exposure to %d\n", newExposure); + SettingsManager.getInstance().GetCurrentPipeline().exposure = newExposure; + SettingsManager.getInstance().GetCurrentUsbCamera().setExposureManual(newExposure); + SettingsManager.getInstance().SaveSettings(); + break; + case "brightness": + int newBrightness = (int) value; + System.out.printf("Changing brightness to %d\n", newBrightness); + SettingsManager.getInstance().GetCurrentPipeline().brightness = newBrightness; + SettingsManager.getInstance().GetCurrentUsbCamera().setBrightness(newBrightness); + SettingsManager.getInstance().SaveSettings(); break; } } @@ -124,7 +149,7 @@ public class Server { private static void broadcastMessage(WsContext sendingUser, Object obj) {//TODO chekc if session id is a good way to differentiate users for (var user : users) { - if (sendingUser!=null&&user.getSessionId()==sendingUser.getSessionId()) { + if (sendingUser!=null&& user.getSessionId().equals(sendingUser.getSessionId())) { continue; } if (obj.getClass() == String.class)