diff --git a/Main/src/main/java/com/chameleonvision/Main.java b/Main/src/main/java/com/chameleonvision/Main.java index b0c2463ff..956633600 100644 --- a/Main/src/main/java/com/chameleonvision/Main.java +++ b/Main/src/main/java/com/chameleonvision/Main.java @@ -6,16 +6,16 @@ import com.chameleonvision.vision.process.VisionProcess; import com.chameleonvision.web.Server; public class Main { - public static void main(String[] args) { - if (CameraManager.initializeCameras()) { - SettingsManager.initialize(); - for (var camSet : CameraManager.getAllCamerasByName().entrySet()) { - new Thread(new VisionProcess(camSet.getValue())).start(); - } - // NetworkTableInstance.getDefault().startClientTeam(SettingsManager.GeneralSettings.team_number); - Server.main(8888); - } else { - System.err.println("No cameras connected!"); - } - } + public static void main(String[] args) { + if (CameraManager.initializeCameras()) { + SettingsManager.initialize(); + for (var camSet : CameraManager.getAllCamerasByName().entrySet()) { + new Thread(new VisionProcess(camSet.getValue())).start(); + } + // NetworkTableInstance.getDefault().startClientTeam(SettingsManager.GeneralSettings.team_number); + Server.main(8888); + } else { + System.err.println("No cameras connected!"); + } + } } diff --git a/Main/src/main/java/com/chameleonvision/vision/GeneralSettings.java b/Main/src/main/java/com/chameleonvision/vision/GeneralSettings.java index 2fb587b32..4ef6ab20b 100644 --- a/Main/src/main/java/com/chameleonvision/vision/GeneralSettings.java +++ b/Main/src/main/java/com/chameleonvision/vision/GeneralSettings.java @@ -1,12 +1,12 @@ package com.chameleonvision.vision; public class GeneralSettings { - public int team_number = 1577; - public String connection_type = "DHCP"; - public String ip = ""; - public String gateway = ""; - public String netmask = ""; - public String hostname = "Chameleon-vision"; - public String curr_camera = ""; - public Integer curr_pipeline = null; + public int team_number = 1577; + public String connection_type = "DHCP"; + public String ip = ""; + public String gateway = ""; + public String netmask = ""; + public String hostname = "Chameleon-vision"; + public String curr_camera = ""; + public Integer curr_pipeline = null; } diff --git a/Main/src/main/java/com/chameleonvision/vision/Pipeline.java b/Main/src/main/java/com/chameleonvision/vision/Pipeline.java index c62c27c72..ad965b37d 100644 --- a/Main/src/main/java/com/chameleonvision/vision/Pipeline.java +++ b/Main/src/main/java/com/chameleonvision/vision/Pipeline.java @@ -1,24 +1,25 @@ package com.chameleonvision.vision; + import java.util.Arrays; import java.util.List; public class Pipeline { - public int exposure = 50; - public int brightness = 50; - public String orientation = "Normal"; - public List hue = Arrays.asList(50,180); - public List saturation = Arrays.asList(50,255); - public List value = Arrays.asList(50,255); - public boolean erode = false; - public boolean dilate = false; - public List area = Arrays.asList(0,100); - public List ratio = Arrays.asList(0,20); - public List extent = Arrays.asList(0,100); - public int is_binary = 0; - public String sort_mode = "Largest"; - public String target_group = "Single"; - public String target_intersection = "Up"; - public double M = 1; - public double B = 0; - public boolean is_calibrated = false; + public int exposure = 50; + public int brightness = 50; + public String orientation = "Normal"; + public List hue = Arrays.asList(50, 180); + public List saturation = Arrays.asList(50, 255); + public List value = Arrays.asList(50, 255); + public boolean erode = false; + public boolean dilate = false; + public List area = Arrays.asList(0, 100); + public List ratio = Arrays.asList(0, 20); + public List extent = Arrays.asList(0, 100); + public int is_binary = 0; + public String sort_mode = "Largest"; + public String target_group = "Single"; + public String target_intersection = "Up"; + public double M = 1; + public double B = 0; + public boolean is_calibrated = false; } diff --git a/Main/src/main/java/com/chameleonvision/vision/camera/CamVideoMode.java b/Main/src/main/java/com/chameleonvision/vision/camera/CamVideoMode.java index 23555197b..3c6bdaad0 100644 --- a/Main/src/main/java/com/chameleonvision/vision/camera/CamVideoMode.java +++ b/Main/src/main/java/com/chameleonvision/vision/camera/CamVideoMode.java @@ -4,23 +4,50 @@ import edu.wpi.cscore.VideoMode; @SuppressWarnings("WeakerAccess") public class CamVideoMode { - public final int fps; - public final int width; - public final int height; - public final String pixel_format; + public final int fps; + public final int width; + public final int height; + public final String pixel_format; - public CamVideoMode(VideoMode videoMode) { - fps = videoMode.fps; - width = videoMode.width; - height = videoMode.height; - pixel_format = videoMode.pixelFormat.name(); - } + public CamVideoMode(VideoMode videoMode) { + fps = videoMode.fps; + width = videoMode.width; + height = videoMode.height; + pixel_format = videoMode.pixelFormat.name(); + } - public VideoMode.PixelFormat getActualPixelFormat() { - return VideoMode.PixelFormat.valueOf(pixel_format); - } + public VideoMode.PixelFormat getActualPixelFormat() { + return VideoMode.PixelFormat.valueOf(pixel_format); + } - public boolean isEqualToVideoMode(VideoMode videoMode) { - return videoMode.fps == fps && videoMode.width == width && videoMode.height == height && videoMode.pixelFormat == getActualPixelFormat(); - } + public boolean isEqualToVideoMode(VideoMode videoMode) { + return videoMode.fps == fps && videoMode.width == width && videoMode.height == height && videoMode.pixelFormat == getActualPixelFormat(); + } + + public boolean equals(VideoMode vm) { + return vm.fps == fps && + vm.width == width && + vm.height == height && + vm.pixelFormat == getActualPixelFormat(); + } + + @Override + public boolean equals(Object obj) { + if (obj == this) { + return true; + } + + if (obj instanceof CamVideoMode) { + var cvm = (CamVideoMode) obj; + return cvm.fps == fps && + cvm.width == width && + cvm.height == height && + cvm.pixel_format.equals(pixel_format); + } else if (obj instanceof VideoMode) { + var vm = (VideoMode) obj; + return equals(vm); + } else { + return false; + } + } } 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 581b43f52..8d784e5b9 100644 --- a/Main/src/main/java/com/chameleonvision/vision/camera/Camera.java +++ b/Main/src/main/java/com/chameleonvision/vision/camera/Camera.java @@ -1,16 +1,21 @@ package com.chameleonvision.vision.camera; import com.chameleonvision.vision.Pipeline; +import com.chameleonvision.web.ServerHandler; import edu.wpi.cscore.*; import edu.wpi.first.cameraserver.CameraServer; import org.opencv.core.Mat; +import java.util.Arrays; import java.util.HashMap; import java.util.stream.IntStream; public class Camera { - private static double defaultFOV = 60.8; + private static final double DEFAULT_FOV = 60.8; + private static final int MINIMUM_FPS = 30; + private static final int MINIMUM_WIDTH = 320; + private static final int MINIMUM_HEIGHT = 240; public final String name; public final String path; @@ -20,25 +25,21 @@ public class Camera { private final CameraServer cs = CameraServer.getInstance(); private final CvSink cvSink; + private final Object cvSourceLock = new Object(); private CvSource cvSource; - private double FOV; - private CameraValues camVals; private CamVideoMode camVideoMode; - private int currentPipelineIndex; private HashMap pipelines; - private final Object cvSourceLock = new Object(); - public Camera(String cameraName) { - this(cameraName, defaultFOV); + this(cameraName, DEFAULT_FOV); } public Camera(UsbCameraInfo usbCamInfo) { - this(usbCamInfo, defaultFOV); + this(usbCamInfo, DEFAULT_FOV); } public Camera(String cameraName, double fov) { @@ -62,8 +63,8 @@ public class Camera { this.pipelines = pipelines; - // set up video mode - availableVideoModes = UsbCam.enumerateVideoModes(); + // set up video modes according to minimums + availableVideoModes = Arrays.stream(UsbCam.enumerateVideoModes()).filter(v -> v.fps >= MINIMUM_FPS && v.width >= MINIMUM_WIDTH && v.height >= MINIMUM_HEIGHT).toArray(VideoMode[]::new); setCamVideoMode(new CamVideoMode(availableVideoModes[0])); cvSink = cs.getVideo(UsbCam); @@ -72,8 +73,17 @@ public class Camera { CameraManager.CameraPorts.put(name, s.getPort()); } + public VideoMode[] getAvailableVideoModes() { + return availableVideoModes; + } + + public int getStreamPort() { + var s = (MjpegServer) cs.getServer("serve_" + name); + return s.getPort(); + } + public void setCamVideoMode(int videoMode) { - setCamVideoMode(UsbCam.enumerateVideoModes()[videoMode]); + setCamVideoMode(availableVideoModes[videoMode]); } private void setCamVideoMode(VideoMode videoMode) { @@ -89,10 +99,12 @@ public class Camera { // update camera values camVals = new CameraValues(this); - if ( prevVideoMode != null && prevVideoMode.width != newVideoMode.width && prevVideoMode.height != newVideoMode.height) { // if resolution changed + if (prevVideoMode != null && !prevVideoMode.equals(newVideoMode)) { // if resolution changed synchronized (cvSourceLock) { - cvSource = cs.putVideo(name, newVideoMode.width, newVideoMode.height); + cvSource = cs.putVideo(name, newVideoMode.width, newVideoMode.height); } + ServerHandler.sendFullSettings(); +// ServerHandler.broadcastMessage(new HashMap(){}.put("port", getStreamPort())); } } @@ -117,6 +129,7 @@ public class Camera { if (pipelineNumber - 1 > pipelines.size()) return; currentPipelineIndex = pipelineNumber; } + public HashMap getPipelines() { return pipelines; } @@ -127,7 +140,7 @@ public class Camera { public int getVideoModeIndex() { return IntStream.range(0, availableVideoModes.length) - .filter(i -> camVideoMode.isEqualToVideoMode(availableVideoModes[i])) + .filter(i -> camVideoMode.equals(availableVideoModes[i])) .findFirst() .orElse(-1); } @@ -156,16 +169,16 @@ public class Camera { } public long grabFrame(Mat image) { - return cvSink.grabFrame(image); - } + return cvSink.grabFrame(image); + } - public CameraValues getCamVals() { + public CameraValues getCamVals() { return camVals; } - public void putFrame(Mat image) { - synchronized(cvSourceLock) { + public void putFrame(Mat image) { + synchronized (cvSourceLock) { cvSource.putFrame(image); } - } + } } diff --git a/Main/src/main/java/com/chameleonvision/vision/camera/CameraManager.java b/Main/src/main/java/com/chameleonvision/vision/camera/CameraManager.java index 0f7f5ec65..c04c548db 100644 --- a/Main/src/main/java/com/chameleonvision/vision/camera/CameraManager.java +++ b/Main/src/main/java/com/chameleonvision/vision/camera/CameraManager.java @@ -1,7 +1,7 @@ package com.chameleonvision.vision.camera; -import com.chameleonvision.FileHelper; import com.chameleonvision.CameraException; +import com.chameleonvision.FileHelper; import com.chameleonvision.settings.SettingsManager; import com.chameleonvision.vision.Pipeline; import com.google.gson.Gson; @@ -19,115 +19,111 @@ import java.util.List; public class CameraManager { - private static final Path CamConfigPath = Paths.get(SettingsManager.SettingsPath.toString(), "Cams"); + private static final Path CamConfigPath = Paths.get(SettingsManager.SettingsPath.toString(), "Cams"); + public static HashMap CameraPorts = new HashMap<>(); + static HashMap AllUsbCameraInfosByName = new HashMap<>() {{ + var suffix = 0; + for (var info : UsbCamera.enumerateUsbCameras()) { + var cap = new VideoCapture(info.dev); + if (cap.isOpened()) { + cap.release(); + var name = info.name; + while (this.containsKey(name)) { + suffix++; + name = String.format("%s(%s)", info.name, suffix); + } + put(name, info); + } + } + }}; + private static HashMap AllCamerasByName = new HashMap<>(); - static HashMap AllUsbCameraInfosByName = new HashMap<>() {{ - var suffix = 0; - for (var info : UsbCamera.enumerateUsbCameras()) { - var cap = new VideoCapture(info.dev); - if (cap.isOpened()) { - cap.release(); - var name = info.name; - while (this.containsKey(name)) { - suffix++; - name = String.format("%s(%s)", info.name, suffix); - } - put(name, info); - } - } - }}; + public static HashMap getAllCamerasByName() { + return AllCamerasByName; + } - private static HashMap AllCamerasByName = new HashMap<>(); + public static boolean initializeCameras() { + if (AllUsbCameraInfosByName.size() == 0) return false; + FileHelper.CheckPath(CamConfigPath); + for (var entry : AllUsbCameraInfosByName.entrySet()) { + var camPath = Paths.get(CamConfigPath.toString(), String.format("%s.json", entry.getKey())); + File camJsonFile = new File(camPath.toString()); + if (camJsonFile.exists() && camJsonFile.length() != 0) { + try { + Gson gson = new GsonBuilder().registerTypeAdapter(Camera.class, new CameraDeserializer()).create(); + var camJsonFileReader = new FileReader(camPath.toString()); + var gsonRead = gson.fromJson(camJsonFileReader, Camera.class); + AllCamerasByName.put(entry.getKey(), gsonRead); + } catch (FileNotFoundException ex) { + ex.printStackTrace(); + } + } else { + if (!addCamera(new Camera(entry.getKey()), entry.getKey())) { + System.err.println("Failed to add camera! Already exists!"); + } + } + } + return true; + } - public static HashMap getAllCamerasByName() { - return AllCamerasByName; - } + private static boolean addCamera(Camera camera, String cameraName) { + if (AllCamerasByName.containsKey(cameraName)) return false; + camera.addPipeline(); + AllCamerasByName.put(cameraName, camera); + return true; + } - public static HashMap CameraPorts = new HashMap<>(); + private static Camera getCamera(String cameraName) { + return AllCamerasByName.get(cameraName); + } - public static boolean initializeCameras() { - if (AllUsbCameraInfosByName.size() == 0) return false; - FileHelper.CheckPath(CamConfigPath); - for (var entry : AllUsbCameraInfosByName.entrySet()) { - var camPath = Paths.get(CamConfigPath.toString(), String.format("%s.json", entry.getKey())); - File camJsonFile = new File(camPath.toString()); - if (camJsonFile.exists() && camJsonFile.length() != 0) { - try { - Gson gson = new GsonBuilder().registerTypeAdapter(Camera.class, new CameraDeserializer()).create(); - var camJsonFileReader = new FileReader(camPath.toString()); - var gsonRead = gson.fromJson(camJsonFileReader, Camera.class); - AllCamerasByName.put(entry.getKey(), gsonRead); - } catch (FileNotFoundException ex) { - ex.printStackTrace(); - } - } else { - if (!addCamera(new Camera(entry.getKey()), entry.getKey())) { - System.err.println("Failed to add camera! Already exists!"); - } - } - } - return true; - } + public static Camera getCurrentCamera() throws CameraException { + if (AllCamerasByName.size() == 0) throw new CameraException(CameraException.CameraExceptionType.NO_CAMERA); + var curCam = AllCamerasByName.get(SettingsManager.GeneralSettings.curr_camera); + if (curCam == null) throw new CameraException(CameraException.CameraExceptionType.BAD_CAMERA); + return curCam; + } - private static boolean addCamera(Camera camera, String cameraName) { - if (AllCamerasByName.containsKey(cameraName)) return false; - camera.addPipeline(); - AllCamerasByName.put(cameraName, camera); - return true; - } + public static void setCurrentCamera(String cameraName) throws CameraException { + if (!AllCamerasByName.containsKey(cameraName)) + throw new CameraException(CameraException.CameraExceptionType.BAD_CAMERA); + SettingsManager.GeneralSettings.curr_camera = cameraName; + SettingsManager.updateCameraSetting(cameraName, getCurrentCamera().getCurrentPipelineIndex()); + } - private static Camera getCamera(String cameraName) { - return AllCamerasByName.get(cameraName); - } + public static Pipeline getCurrentPipeline() throws CameraException { + return getCurrentCamera().getCurrentPipeline(); + } - public static void setCurrentCamera(String cameraName) throws CameraException { - if (!AllCamerasByName.containsKey(cameraName)) - throw new CameraException(CameraException.CameraExceptionType.BAD_CAMERA); - SettingsManager.GeneralSettings.curr_camera = cameraName; - SettingsManager.updateCameraSetting(cameraName, getCurrentCamera().getCurrentPipelineIndex()); - } + public static void setCurrentPipeline(int pipelineNumber) throws CameraException { + if (!getCurrentCamera().getPipelines().containsKey(pipelineNumber)) + throw new CameraException(CameraException.CameraExceptionType.BAD_PIPELINE); + getCurrentCamera().setCurrentPipelineIndex(pipelineNumber); + SettingsManager.updatePipelineSetting(pipelineNumber); + } - public static Camera getCurrentCamera() throws CameraException { - if (AllCamerasByName.size() == 0) throw new CameraException(CameraException.CameraExceptionType.NO_CAMERA); - var curCam = AllCamerasByName.get(SettingsManager.GeneralSettings.curr_camera); - if (curCam == null) throw new CameraException(CameraException.CameraExceptionType.BAD_CAMERA); - return curCam; - } + public static List getResolutionList() throws CameraException { + if (!SettingsManager.GeneralSettings.curr_camera.equals("")) { + List list = new ArrayList<>(); + for (var res : CameraManager.getCamera(SettingsManager.GeneralSettings.curr_camera).getAvailableVideoModes()) { + list.add(String.format("%s X %s at %s fps", res.width, res.height, res.fps)); + } + return list; + } + throw new CameraException(CameraException.CameraExceptionType.NO_CAMERA); + } - public static void setCurrentPipeline(int pipelineNumber) throws CameraException { - if (!getCurrentCamera().getPipelines().containsKey(pipelineNumber)) - throw new CameraException(CameraException.CameraExceptionType.BAD_PIPELINE); - getCurrentCamera().setCurrentPipelineIndex(pipelineNumber); - SettingsManager.updatePipelineSetting(pipelineNumber); - } - - public static Pipeline getCurrentPipeline() throws CameraException { - return getCurrentCamera().getCurrentPipeline(); - } - - public static List getResolutionList() throws CameraException { - if (!SettingsManager.GeneralSettings.curr_camera.equals("")) { - List list = new ArrayList<>(); - var cam = CameraManager.getCamera(SettingsManager.GeneralSettings.curr_camera).UsbCam; - for (var res : cam.enumerateVideoModes()) { - list.add(String.format("%s X %s at %s fps", res.width, res.height, res.fps)); - } - return list; - } - throw new CameraException(CameraException.CameraExceptionType.NO_CAMERA); - } - - public static void saveCameras() { - for (var entry : AllCamerasByName.entrySet()) { - try { - Gson gson = new GsonBuilder().setPrettyPrinting().registerTypeAdapter(Camera.class, new CameraSerializer()).create(); - FileWriter writer = new FileWriter(Paths.get(CamConfigPath.toString(), String.format("%s.json", entry.getKey())).toString()); - gson.toJson(entry.getValue(), writer); - writer.flush(); - writer.close(); - } catch (IOException ex) { - ex.printStackTrace(); - } - } - } + public static void saveCameras() { + for (var entry : AllCamerasByName.entrySet()) { + try { + Gson gson = new GsonBuilder().setPrettyPrinting().registerTypeAdapter(Camera.class, new CameraSerializer()).create(); + FileWriter writer = new FileWriter(Paths.get(CamConfigPath.toString(), String.format("%s.json", entry.getKey())).toString()); + gson.toJson(entry.getValue(), writer); + writer.flush(); + writer.close(); + } catch (IOException ex) { + ex.printStackTrace(); + } + } + } } diff --git a/Main/src/main/java/com/chameleonvision/vision/process/CVProcess.java b/Main/src/main/java/com/chameleonvision/vision/process/CVProcess.java index dca8e94a0..6cbe4af36 100644 --- a/Main/src/main/java/com/chameleonvision/vision/process/CVProcess.java +++ b/Main/src/main/java/com/chameleonvision/vision/process/CVProcess.java @@ -4,196 +4,192 @@ import com.chameleonvision.vision.camera.CameraValues; import org.apache.commons.math3.util.FastMath; import org.jetbrains.annotations.NotNull; import org.opencv.core.*; -import org.opencv.imgproc.*; +import org.opencv.imgproc.Imgproc; import java.util.*; @SuppressWarnings("WeakerAccess") public class CVProcess { - private HashMapTargetGrouping= new HashMap<>() {{ - put("Single", 1); - put("Dual", 2); - put("Triple", 3); - put("Quadruple", 4); - put("Quintuple", 5); - }}; + private final CameraValues CamVals; + private HashMap TargetGrouping = new HashMap<>() {{ + put("Single", 1); + put("Dual", 2); + put("Triple", 3); + put("Quadruple", 4); + put("Quintuple", 5); + }}; + private Mat Kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(5, 5)); + private Mat hsvImage = new Mat(); + private List FoundContours = new ArrayList<>(); + private Mat binaryMat = new Mat(); + private List FilteredContours = new ArrayList<>(); + private Comparator SortByCentermostComparator = Comparator.comparingDouble(this::calcDistance); + private List FinalCountours = new ArrayList<>(); + private Mat intersectMatA = new Mat(); + private Mat intersectMatB = new Mat(); - private final CameraValues CamVals; + CVProcess(CameraValues camVals) { + CamVals = camVals; + } - CVProcess(CameraValues camVals){ - CamVals = camVals; - } + 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); + if (shouldErode) { + Imgproc.erode(dst, dst, Kernel); + } + if (shouldDilate) { + Imgproc.dilate(dst, dst, Kernel); + } + hsvImage.release(); + } - private Mat Kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(5, 5)); + List FindContours(Mat src) { + src.copyTo(binaryMat); + FoundContours.clear(); + Imgproc.findContours(binaryMat, FoundContours, new Mat(), Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_TC89_L1); + binaryMat.release(); + return FoundContours; + } - 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); - if (shouldErode){ - Imgproc.erode(dst, dst, Kernel); - } - if (shouldDilate){ - Imgproc.dilate(dst, dst, Kernel); - } - hsvImage.release(); - } + List FilterContours(List InputContours, List area, List ratio, List extent) { + for (MatOfPoint Contour : InputContours) { + try { + var contourArea = Imgproc.contourArea(Contour); + double targetArea = FastMath.round((contourArea / CamVals.ImageArea) * 100); + if (targetArea <= area.get(0) || targetArea >= area.get(1)) { + continue; + } + var rect = Imgproc.minAreaRect(new MatOfPoint2f(Contour.toArray())); + var targetFullness = (contourArea / rect.size.area()) * 100; + if (targetFullness <= extent.get(0) || targetArea >= extent.get(1)) { + continue; + } + var aspectRatio = rect.size.width / rect.size.height; + if (aspectRatio <= ratio.get(0) || aspectRatio >= ratio.get(1)) { + continue; + } + FilteredContours.add(Contour); + } catch (Exception ignored) { + } + } + return FilteredContours; + } - private List FoundContours = new ArrayList<>(); - private Mat binaryMat = new Mat(); - List FindContours(Mat src) { - src.copyTo(binaryMat); - FoundContours.clear(); - Imgproc.findContours(binaryMat, FoundContours, new Mat(), Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_TC89_L1); - binaryMat.release(); - return FoundContours; - } + 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 List FilteredContours = new ArrayList<>(); - List FilterContours(List InputContours, List area, List ratio, List extent) { - for (MatOfPoint Contour : InputContours){ - try{ - var contourArea = Imgproc.contourArea(Contour); - double targetArea = FastMath.round((contourArea / CamVals.ImageArea) * 100); - if (targetArea <= area.get(0) || targetArea >= area.get(1)){ - continue; - } - var rect = Imgproc.minAreaRect(new MatOfPoint2f(Contour.toArray())); - var targetFullness = (contourArea / rect.size.area()) * 100; - if (targetFullness <= extent.get(0) || targetArea >= extent.get(1)){ - continue; - } - var aspectRatio = rect.size.width / rect.size.height; - if (aspectRatio <= ratio.get(0) || aspectRatio >= ratio.get(1)){ - continue; - } - FilteredContours.add(Contour); - } - catch (Exception ignored) { } - } - return FilteredContours; - } + 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 Collections.min(inputRects, SortByCentermostComparator); + default: + return inputRects.get(0); // default to whatever the first contour is, but this should never happen + } + } - private double calcDistance(RotatedRect rect) { - return FastMath.sqrt(FastMath.pow(CamVals.CenterX - rect.center.x, 2) + FastMath.pow(CamVals.CenterY - rect.center.y, 2)); - } + 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++) { + try { + MatOfPoint firstContour = InputContours.get(i + c); + MatOfPoint secondContour = InputContours.get(i + c + 1); + if (IsIntersecting(firstContour, secondContour, IntersectionPoint)) { + FinalContourList.addAll(secondContour.toList()); + } + firstContour.release(); + secondContour.release(); + MatOfPoint2f contour = new MatOfPoint2f(); + contour.fromList(FinalContourList); + if (contour.cols() != 0 && contour.rows() != 0) { + RotatedRect rect = Imgproc.minAreaRect(contour); + FinalCountours.add(rect); + } + } catch (IndexOutOfBoundsException e) { + FinalContourList.clear(); + break; + } + } + } - private Comparator SortByCentermostComparator = Comparator.comparingDouble(this::calcDistance); + } else { + for (MatOfPoint inputContour : InputContours) { + MatOfPoint2f contour = new MatOfPoint2f(); + contour.fromArray(inputContour.toArray()); + if (contour.cols() != 0 && contour.rows() != 0) { + RotatedRect rect = Imgproc.minAreaRect(contour); + FinalCountours.add(rect); + } + } + } + return FinalCountours; + } - 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 Collections.min(inputRects, SortByCentermostComparator); - default: - return inputRects.get(0); // default to whatever the first contour is, but this should never happen - } - } - - private List FinalCountours = new ArrayList<>(); - 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++){ - try{ - MatOfPoint firstContour = InputContours.get(i + c); - MatOfPoint secondContour = InputContours.get(i + c + 1); - if (IsIntersecting(firstContour, secondContour, IntersectionPoint)){ - FinalContourList.addAll(secondContour.toList()); - } - firstContour.release(); - secondContour.release(); - MatOfPoint2f contour = new MatOfPoint2f(); - contour.fromList(FinalContourList); - if (contour.cols() != 0 && contour.rows() != 0){ - RotatedRect rect = Imgproc.minAreaRect(contour); - FinalCountours.add(rect); - } - } catch (IndexOutOfBoundsException e){ - FinalContourList.clear(); - break; - } - } - } - - } else { - for (MatOfPoint inputContour : InputContours) { - MatOfPoint2f contour = new MatOfPoint2f(); - contour.fromArray(inputContour.toArray()); - if (contour.cols() != 0 && contour.rows() != 0) { - RotatedRect rect = Imgproc.minAreaRect(contour); - FinalCountours.add(rect); - } - } - } - return FinalCountours; - } - - private Mat intersectMatA = new Mat(); - private Mat intersectMatB = new Mat(); - private boolean IsIntersecting(MatOfPoint ContourOne, MatOfPoint ContourTwo, String IntersectionPoint) { - if (IntersectionPoint.equals("None")){ - return true; - } - try { - Imgproc.fitLine(ContourOne, intersectMatA, Imgproc.CV_DIST_L2,0,0.01,0.01); - Imgproc.fitLine(ContourTwo, intersectMatB, Imgproc.CV_DIST_L2,0,0.01,0.01); - double vxA = intersectMatA.get(0,0)[0]; - double vyA = intersectMatA.get(1,0)[0]; - double x0A = intersectMatA.get(2,0)[0]; - double y0A = intersectMatA.get(3,0)[0]; - double mA = vyA / vxA; - double vxB = intersectMatB.get(0,0)[0]; - double vyB = intersectMatB.get(1,0)[0]; - double x0B = intersectMatB.get(2,0)[0]; - double y0B = intersectMatB.get(3,0)[0]; - double mB = vyB / vxB; - double intersectionX = (mA * x0A) - y0A - (mB * x0B) + y0B / (mA - mB); - double intersectionY = (mA * (intersectionX - x0A)) + y0A; - switch (IntersectionPoint){ - case "Up" :{ - if (intersectionY < CamVals.CenterY){ - return true; - } - break; - } - case "Down": { - if (intersectionY > CamVals.CenterY){ - return true; - } - break; - } - case "Left": { - if (intersectionX < CamVals.CenterX){ - return true; - } - break; - } - case "Right": { - if (intersectionX > CamVals.CenterX){ - return true; - } - break; - } - } - return false; - } - catch (Exception e){ - return false; - } - } + private boolean IsIntersecting(MatOfPoint ContourOne, MatOfPoint ContourTwo, String IntersectionPoint) { + if (IntersectionPoint.equals("None")) { + return true; + } + try { + Imgproc.fitLine(ContourOne, intersectMatA, Imgproc.CV_DIST_L2, 0, 0.01, 0.01); + Imgproc.fitLine(ContourTwo, intersectMatB, Imgproc.CV_DIST_L2, 0, 0.01, 0.01); + double vxA = intersectMatA.get(0, 0)[0]; + double vyA = intersectMatA.get(1, 0)[0]; + double x0A = intersectMatA.get(2, 0)[0]; + double y0A = intersectMatA.get(3, 0)[0]; + double mA = vyA / vxA; + double vxB = intersectMatB.get(0, 0)[0]; + double vyB = intersectMatB.get(1, 0)[0]; + double x0B = intersectMatB.get(2, 0)[0]; + double y0B = intersectMatB.get(3, 0)[0]; + double mB = vyB / vxB; + double intersectionX = (mA * x0A) - y0A - (mB * x0B) + y0B / (mA - mB); + double intersectionY = (mA * (intersectionX - x0A)) + y0A; + switch (IntersectionPoint) { + case "Up": { + if (intersectionY < CamVals.CenterY) { + return true; + } + break; + } + case "Down": { + if (intersectionY > CamVals.CenterY) { + return true; + } + break; + } + case "Left": { + if (intersectionX < CamVals.CenterX) { + return true; + } + break; + } + case "Right": { + if (intersectionX > CamVals.CenterX) { + return true; + } + break; + } + } + return false; + } catch (Exception e) { + return false; + } + } } 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 d7c1a0bd8..86aaf71c9 100644 --- a/Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java +++ b/Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java @@ -6,51 +6,60 @@ import org.opencv.core.Mat; public class CameraProcess implements Runnable { - private final Camera camera; - private final int maxFPS; - private Mat inputFrame; - private Mat outputFrame; - private long timestamp; + private final Camera camera; + private final int maxFPS; + private final Object inputFrameLock = new Object(); + private final Object outputFrameLock = new Object(); + private Mat inputFrame; + private Mat outputFrame; + private long timestamp; - private final Object inputFrameLock = new Object(); - private final Object outputFrameLock = new Object(); - - public CameraProcess(Camera camera) { - this.camera = camera; - maxFPS = camera.getVideoMode().fps; + CameraProcess(Camera camera) { + this.camera = camera; + maxFPS = camera.getVideoMode().fps; var camVals = camera.getCamVals(); inputFrame = new Mat(camVals.ImageWidth, camVals.ImageHeight, CvType.CV_8UC3); outputFrame = new Mat(camVals.ImageWidth, camVals.ImageHeight, CvType.CV_8UC3); - } + } - void updateFrame(Mat inputFrame) { - synchronized (inputFrameLock) { - inputFrame.copyTo(this.inputFrame); - } - } + private void updateFrameSize() { + var camVals = camera.getCamVals(); + synchronized (inputFrameLock) { + inputFrame = new Mat(camVals.ImageWidth, camVals.ImageHeight, CvType.CV_8UC3); + } + synchronized (outputFrameLock) { + outputFrame = new Mat(camVals.ImageWidth, camVals.ImageHeight, CvType.CV_8UC3); + } + } - long getLatestFrame(Mat outputFrame) { - synchronized (outputFrameLock) { - this.outputFrame.copyTo(outputFrame); - return timestamp; - } - } + void updateFrame(Mat inputFrame) { + synchronized (inputFrameLock) { + inputFrame.copyTo(this.inputFrame); + } + } - @Override - public void run() { - while(!Thread.interrupted()) { - synchronized (outputFrameLock) { - timestamp = camera.grabFrame(outputFrame); - } - synchronized (inputFrameLock) { - camera.putFrame(inputFrame); - } - var msToWait = (long)1000/maxFPS; - try { - Thread.sleep(msToWait); - } catch (InterruptedException e) { - e.printStackTrace(); - } - } - } + long getLatestFrame(Mat outputFrame) { + synchronized (outputFrameLock) { + this.outputFrame.copyTo(outputFrame); + return timestamp; + } + } + + @Override + public void run() { + while (!Thread.interrupted()) { + synchronized (outputFrameLock) { + timestamp = camera.grabFrame(outputFrame); + } + synchronized (inputFrameLock) { + camera.putFrame(inputFrame); + } + var msToWait = (long) 1000 / maxFPS; + try { + Thread.sleep(msToWait); + } catch (InterruptedException e) { + e.printStackTrace(); + } + } + } } 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 304bbbfef..c60c6c37a 100644 --- a/Main/src/main/java/com/chameleonvision/vision/process/VisionProcess.java +++ b/Main/src/main/java/com/chameleonvision/vision/process/VisionProcess.java @@ -14,227 +14,223 @@ import java.util.List; public class VisionProcess implements Runnable { - private final Camera camera; - private final String cameraName; + private final Camera camera; + private final String cameraName; + private final CameraProcess cameraProcess; + // NetworkTables + private NetworkTableEntry ntPipelineEntry; + private NetworkTableEntry ntDriverModeEntry; + private NetworkTableEntry ntYawEntry; + private NetworkTableEntry ntPitchEntry; + private NetworkTableEntry ntDistanceEntry; + private NetworkTableEntry ntTimeStampEntry; + private NetworkTableEntry ntValidEntry; + // chameleon specific + private Pipeline currentPipeline; + private CVProcess cvProcess; + // pipeline process items + private List FoundContours = new ArrayList<>(); + private List FilteredContours = new ArrayList<>(); + private List 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 long TimeStamp = 0; - // NetworkTables - private NetworkTableEntry ntPipelineEntry; - private NetworkTableEntry ntDriverModeEntry; - private NetworkTableEntry ntYawEntry; - private NetworkTableEntry ntPitchEntry; - private NetworkTableEntry ntDistanceEntry; - private NetworkTableEntry ntTimeStampEntry; - private NetworkTableEntry ntValidEntry; + public VisionProcess(Camera processCam) { + camera = processCam; + this.cameraName = camera.name; - // chameleon specific - private Pipeline currentPipeline; - private CVProcess cvProcess; + // NetworkTables + NetworkTable ntTable = NetworkTableInstance.getDefault().getTable("/chameleon-vision/" + cameraName); + ntPipelineEntry = ntTable.getEntry("Pipeline"); + ntDriverModeEntry = ntTable.getEntry("Driver_Mode"); + ntPitchEntry = ntTable.getEntry("Pitch"); + ntYawEntry = ntTable.getEntry("Yaw"); + ntDistanceEntry = ntTable.getEntry("Distance"); + ntTimeStampEntry = ntTable.getEntry("TimeStamp"); + ntValidEntry = ntTable.getEntry("Valid"); + ntDriverModeEntry.addListener(this::DriverModeListener, EntryListenerFlags.kUpdate); + ntPipelineEntry.addListener(this::PipelineListener, EntryListenerFlags.kUpdate); + ntDriverModeEntry.setBoolean(false); + ntPipelineEntry.setString("pipeline" + camera.getCurrentPipelineIndex()); - // pipeline process items - private List FoundContours = new ArrayList<>(); - private List FilteredContours = new ArrayList<>(); - private List 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 long TimeStamp = 0; + // camera settings + cvProcess = new CVProcess(camera.getCamVals()); + cameraProcess = new CameraProcess(camera); + } - private final CameraProcess cameraProcess; + private void DriverModeListener(EntryNotification entryNotification) { + if (entryNotification.value.getBoolean()) { + camera.setExposure(25); + camera.setBrightness(15); + } else { + Pipeline pipeline = camera.getCurrentPipeline(); + camera.setExposure(pipeline.exposure); + camera.setBrightness(pipeline.brightness); + } + } - private void DriverModeListener(EntryNotification entryNotification) { - if (entryNotification.value.getBoolean()) { - camera.setExposure(25); - camera.setBrightness(15); - } else { - Pipeline pipeline = camera.getCurrentPipeline(); - camera.setExposure(pipeline.exposure); - camera.setBrightness(pipeline.brightness); - } - } - - private void PipelineListener(EntryNotification entryNotification) { - var ntPipelineIndex = Integer.parseInt(entryNotification.value.getString().replace("pipeline", "")); - if (camera.getPipelines().containsKey(ntPipelineIndex)) { + private void PipelineListener(EntryNotification entryNotification) { + var ntPipelineIndex = Integer.parseInt(entryNotification.value.getString().replace("pipeline", "")); + if (camera.getPipelines().containsKey(ntPipelineIndex)) { // camera.setEntryNotification.value.getString()); - var pipeline = camera.getCurrentPipeline(); + var pipeline = camera.getCurrentPipeline(); - camera.setExposure(pipeline.exposure); - camera.setBrightness(pipeline.brightness); - HashMap pipeChange = new HashMap<>(); - pipeChange.put("curr_pipeline",ntPipelineIndex); - ServerHandler.broadcastMessage(pipeChange); + camera.setExposure(pipeline.exposure); + camera.setBrightness(pipeline.brightness); + HashMap pipeChange = new HashMap<>(); + pipeChange.put("curr_pipeline", ntPipelineIndex); + ServerHandler.broadcastMessage(pipeChange); - } else { - ntPipelineEntry.setString("pipeline" + camera.getCurrentPipelineIndex()); - } - } + } else { + ntPipelineEntry.setString("pipeline" + camera.getCurrentPipelineIndex()); + } + } - public VisionProcess(Camera processCam) { - camera = processCam; - this.cameraName = camera.name; + private void drawContour(Mat inputMat, RotatedRect contourRect) { + if (contourRect == null) return; + List 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); + } - // NetworkTables - NetworkTable ntTable = NetworkTableInstance.getDefault().getTable("/chameleon-vision/" + cameraName); - ntPipelineEntry = ntTable.getEntry("Pipeline"); - ntDriverModeEntry = ntTable.getEntry("Driver_Mode"); - ntPitchEntry = ntTable.getEntry("Pitch"); - ntYawEntry = ntTable.getEntry("Yaw"); - ntDistanceEntry = ntTable.getEntry("Distance"); - ntTimeStampEntry = ntTable.getEntry("TimeStamp"); - ntValidEntry = ntTable.getEntry("Valid"); - ntDriverModeEntry.addListener(this::DriverModeListener, EntryListenerFlags.kUpdate); - ntPipelineEntry.addListener(this::PipelineListener, EntryListenerFlags.kUpdate); - ntDriverModeEntry.setBoolean(false); - ntPipelineEntry.setString("pipeline" + camera.getCurrentPipelineIndex()); + private void updateNetworkTables(PipelineResult pipelineResult) { + ntValidEntry.setBoolean(pipelineResult.IsValid); + if (pipelineResult.IsValid) { + ntYawEntry.setNumber(pipelineResult.Yaw); + ntPitchEntry.setNumber(pipelineResult.Pitch); + } + ntTimeStampEntry.setNumber(TimeStamp); + } - // camera settings - cvProcess = new CVProcess(camera.getCamVals()); - cameraProcess = new CameraProcess(camera); - } + private PipelineResult runVisionProcess(Mat inputImage, Mat outputImage) { + var pipelineResult = new PipelineResult(); - private void drawContour(Mat inputMat, RotatedRect contourRect) { - if (contourRect == null) return; - List 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); - } + if (currentPipeline == null) { + return pipelineResult; + } + if (!currentPipeline.orientation.equals("Normal")) { + Core.flip(inputImage, inputImage, -1); + } + if (ntDriverModeEntry.getBoolean(false)) { + inputImage.copyTo(outputImage); + return 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)); - private void updateNetworkTables(PipelineResult pipelineResult) { - ntValidEntry.setBoolean(pipelineResult.IsValid); - if (pipelineResult.IsValid){ - ntYawEntry.setNumber(pipelineResult.Yaw); - ntPitchEntry.setNumber(pipelineResult.Pitch); - } - ntTimeStampEntry.setNumber(TimeStamp); - } + cvProcess.HSVThreshold(inputImage, hsvThreshMat, hsvLower, hsvUpper, currentPipeline.erode, currentPipeline.dilate); - private PipelineResult runVisionProcess(Mat inputImage, Mat outputImage) { - var pipelineResult = new PipelineResult(); + if (currentPipeline.is_binary == 1) { + Imgproc.cvtColor(hsvThreshMat, outputImage, Imgproc.COLOR_GRAY2BGR, 3); + } else { + inputImage.copyTo(outputImage); + } + FoundContours = cvProcess.FindContours(hsvThreshMat); + if (FoundContours.size() > 0) { + FilteredContours = cvProcess.FilterContours(FoundContours, currentPipeline.area, currentPipeline.ratio, currentPipeline.extent); + if (FilteredContours.size() > 0) { + GroupedContours = cvProcess.GroupTargets(FilteredContours, currentPipeline.target_intersection, currentPipeline.target_group); + if (GroupedContours.size() > 0) { + var finalRect = cvProcess.SortTargetsToOne(GroupedContours, currentPipeline.sort_mode); + pipelineResult.RawPoint = finalRect; + pipelineResult.IsValid = true; + if (!currentPipeline.is_calibrated) { + pipelineResult.CalibratedX = camera.getCamVals().CenterX; + pipelineResult.CalibratedY = camera.getCamVals().CenterY; + } else { + pipelineResult.CalibratedX = (finalRect.center.y - currentPipeline.B) / currentPipeline.M; + pipelineResult.CalibratedY = finalRect.center.x * currentPipeline.M + currentPipeline.B; + } + pipelineResult.Pitch = camera.getCamVals().CalculatePitch(finalRect.center.y, pipelineResult.CalibratedY); + pipelineResult.Yaw = camera.getCamVals().CalculateYaw(finalRect.center.x, pipelineResult.CalibratedX); + drawContour(outputImage, finalRect); + } + } + } - if (currentPipeline == null) { - return pipelineResult; - } - if (!currentPipeline.orientation.equals("Normal")){ - Core.flip(inputImage,inputImage,-1); - } - if (ntDriverModeEntry.getBoolean(false)){ - inputImage.copyTo(outputImage); - return 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)); + return pipelineResult; + } - cvProcess.HSVThreshold(inputImage, hsvThreshMat, hsvLower, hsvUpper, currentPipeline.erode, currentPipeline.dilate); + @Override + public void run() { + // processing time tracking + long startTime; + long fpsLastTime = 0; + double processTimeMs; + double fps = 0; + double uiFps = 0; + int maxFps = camera.getVideoMode().fps; - if (currentPipeline.is_binary == 1) { - Imgproc.cvtColor(hsvThreshMat, outputImage, Imgproc.COLOR_GRAY2BGR, 3); - } else { - inputImage.copyTo(outputImage); - } - FoundContours = cvProcess.FindContours(hsvThreshMat); - if (FoundContours.size() > 0) { - FilteredContours = cvProcess.FilterContours(FoundContours, currentPipeline.area, currentPipeline.ratio, currentPipeline.extent); - if (FilteredContours.size() > 0) { - GroupedContours = cvProcess.GroupTargets(FilteredContours, currentPipeline.target_intersection, currentPipeline.target_group); - if (GroupedContours.size() > 0) { - var finalRect = cvProcess.SortTargetsToOne(GroupedContours, currentPipeline.sort_mode); - pipelineResult.RawPoint = finalRect; - pipelineResult.IsValid = true; - if (!currentPipeline.is_calibrated) { - pipelineResult.CalibratedX = camera.getCamVals().CenterX; - pipelineResult.CalibratedY = camera.getCamVals().CenterY; - } else { - pipelineResult.CalibratedX = (finalRect.center.y - currentPipeline.B) / currentPipeline.M; - pipelineResult.CalibratedY = finalRect.center.x * currentPipeline.M + currentPipeline.B; - } - pipelineResult.Pitch = camera.getCamVals().CalculatePitch(finalRect.center.y, pipelineResult.CalibratedY); - pipelineResult.Yaw = camera.getCamVals().CalculateYaw(finalRect.center.x, pipelineResult.CalibratedX); - drawContour(outputImage, finalRect); - } - } - } + new Thread(cameraProcess).start(); - return pipelineResult; - } + long lastFrameEndNanosec = 0; - @Override - public void run() { - // processing time tracking - long startTime; - long fpsLastTime = 0; - double processTimeMs; - double fps = 0; - double uiFps = 0; - int maxFps = camera.getVideoMode().fps; + while (!Thread.interrupted()) { + startTime = System.nanoTime(); + if ((startTime - lastFrameEndNanosec) * 1e-6 >= 1000.0 / maxFps + 3) { // 3 additional fps to allow for overhead + FoundContours.clear(); + FilteredContours.clear(); + GroupedContours.clear(); - new Thread(cameraProcess).start(); + // update FPS for ui only every 0.5 seconds + if ((startTime - fpsLastTime) * 1e-6 >= 500) { + if (fps >= maxFps) { + uiFps = maxFps; + } else { + uiFps = fps; + } + fpsLastTime = System.nanoTime(); + } - long lastFrameEndNanosec = 0; + currentPipeline = camera.getCurrentPipeline(); + // start fps counter right before grabbing input frame + TimeStamp = cameraProcess.getLatestFrame(cameraInputMat); + if (cameraInputMat.cols() == 0 && cameraInputMat.rows() == 0) { + continue; + } - while (!Thread.interrupted()) { - startTime = System.nanoTime(); - if ((startTime - lastFrameEndNanosec) * 1e-6 >= 1000.0/maxFps + 3) { // 3 additional fps to allow for overhead - FoundContours.clear(); - FilteredContours.clear(); - GroupedContours.clear(); + // get vision data + var pipelineResult = runVisionProcess(cameraInputMat, streamOutputMat); + updateNetworkTables(pipelineResult); + if (cameraName.equals(SettingsManager.GeneralSettings.curr_camera)) { + HashMap WebSend = new HashMap<>(); + HashMap point = new HashMap<>(); + List center = new ArrayList<>(); + 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", uiFps); + WebSend.put("point", point); + WebSend.put("raw_point", center); + ServerHandler.broadcastMessage(WebSend); + } - // update FPS for ui only every 0.5 seconds - if ((startTime - fpsLastTime) * 1e-6 >= 500) { - if (fps >= maxFps) { - uiFps = maxFps; - } else { - uiFps = fps; - } - fpsLastTime = System.nanoTime(); - } + cameraProcess.updateFrame(streamOutputMat); - currentPipeline = camera.getCurrentPipeline(); - // start fps counter right before grabbing input frame - TimeStamp = cameraProcess.getLatestFrame(cameraInputMat); - if (cameraInputMat.cols() == 0 && cameraInputMat.rows() == 0) { - continue; - } + cameraInputMat.release(); + hsvThreshMat.release(); - // get vision data - var pipelineResult = runVisionProcess(cameraInputMat, streamOutputMat); - updateNetworkTables(pipelineResult); - if (cameraName.equals(SettingsManager.GeneralSettings.curr_camera)) { - HashMap WebSend = new HashMap<>(); - HashMap point = new HashMap<>(); - List center = new ArrayList<>(); - 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", uiFps); - WebSend.put("point", point); - WebSend.put("raw_point", center); - ServerHandler.broadcastMessage(WebSend); - } + // calculate FPS + lastFrameEndNanosec = System.nanoTime(); + processTimeMs = (lastFrameEndNanosec - startTime) * 1e-6; + fps = 1000 / processTimeMs; - cameraProcess.updateFrame(streamOutputMat); - - cameraInputMat.release(); - hsvThreshMat.release(); - - // calculate FPS - lastFrameEndNanosec = System.nanoTime(); - processTimeMs = (lastFrameEndNanosec - startTime) * 1e-6; - fps = 1000 / processTimeMs; - - System.out.printf("%s - Process time: %-5.2fms, FPS: %-5.2f, FoundContours: %d, FilteredContours: %d, GroupedContours: %d\n", cameraName, processTimeMs, fps, FoundContours.size(), FilteredContours.size(), GroupedContours.size()); - } - } - } + System.out.printf("%s - Process time: %-5.2fms, FPS: %-5.2f, FoundContours: %d, FilteredContours: %d, GroupedContours: %d\n", cameraName, processTimeMs, fps, FoundContours.size(), FilteredContours.size(), GroupedContours.size()); + } + } + } } diff --git a/Main/src/main/java/com/chameleonvision/web/ServerHandler.java b/Main/src/main/java/com/chameleonvision/web/ServerHandler.java index ab45c09bb..ac2c327b9 100644 --- a/Main/src/main/java/com/chameleonvision/web/ServerHandler.java +++ b/Main/src/main/java/com/chameleonvision/web/ServerHandler.java @@ -3,6 +3,7 @@ package com.chameleonvision.web; import com.chameleonvision.CameraException; import com.chameleonvision.settings.SettingsManager; import com.chameleonvision.vision.Pipeline; +import com.chameleonvision.vision.camera.Camera; import com.chameleonvision.vision.camera.CameraManager; import edu.wpi.cscore.VideoException; import io.javalin.websocket.WsCloseContext; @@ -83,9 +84,8 @@ public class ServerHandler { String newCamera = (String) value; System.out.printf("Changing camera to %s\n", newCamera); CameraManager.setCurrentCamera(newCamera); - broadcastMessage(new HashMap(){}.put("port",CameraManager.CameraPorts.get(SettingsManager.GeneralSettings.curr_camera))); + broadcastMessage(new HashMap(){}.put("port", CameraManager.getCurrentCamera().getStreamPort())); broadcastMessage(CameraManager.getCurrentCamera()); //TODO CHECK JSON FOR CAMERA CHANGE - break; case "curr_pipeline": String newPipeline = (String) value; @@ -172,7 +172,7 @@ public class ServerHandler { return map; } - private static void sendFullSettings() { + public static void sendFullSettings() { //General settings Map fullSettings = new HashMap<>(allFieldsToMap(SettingsManager.GeneralSettings)); fullSettings.put("cameraList", CameraManager.getAllCamerasByName().keySet()); @@ -183,7 +183,7 @@ public class ServerHandler { fullSettings.put("resolutionList", CameraManager.getResolutionList()); fullSettings.put("resolution", currentCamera.getVideoModeIndex()); fullSettings.put("FOV", currentCamera.getFOV()); - fullSettings.put("port", CameraManager.CameraPorts.get(SettingsManager.GeneralSettings.curr_camera)); + fullSettings.put("port", currentCamera.getStreamPort()); } catch (CameraException e) { System.err.println("No camera found!"); //TODO: add message to ui to inform that there are no cameras