From fca17885bbfa0e0c3a4850490d0a0c8e0049c678 Mon Sep 17 00:00:00 2001 From: Unknown Date: Tue, 17 Sep 2019 11:22:54 +0300 Subject: [PATCH] added pipeline listener methods and fixed centermost sort -added change camera values function -added pipeline listener - added driver mode listener -changed centermost to use Collections and grab the minimum distance --- .../vision/process/CameraProcess.java | 46 +++++++++++++++---- .../vision/process/VisionProcess.java | 3 +- 2 files changed, 39 insertions(+), 10 deletions(-) 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 0434f3bd6..c4bab59c6 100644 --- a/Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java +++ b/Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java @@ -4,9 +4,7 @@ import com.chameleonvision.MemoryManager; import com.chameleonvision.settings.SettingsManager; import com.chameleonvision.vision.CameraValues; import com.chameleonvision.vision.Pipeline; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.*; import edu.wpi.first.cameraserver.CameraServer; import org.apache.commons.math3.stat.descriptive.moment.Mean; import org.opencv.core.*; @@ -19,13 +17,33 @@ public class CameraProcess implements Runnable { private String CameraName; private CameraServer cs = CameraServer.getInstance(); - private NetworkTableEntry ntPipelineEntry, ntDriverModeEntry; + private NetworkTableEntry ntPipelineEntry, ntDriverModeEntry,ntYawEntry,ntPitchEntry,ntDistanceEntry,ntTimeStampEntry; private MemoryManager memManager = new MemoryManager(125); private int imgWidth, imgHeight; - - + private void ChangeCameraValues(int Exposure, int Brightness){ + SettingsManager.getInstance().UsbCameras.get(CameraName).setBrightness(Brightness); + SettingsManager.getInstance().UsbCameras.get(CameraName).setExposureManual(Exposure); + } + private void DriverModeListener(EntryNotification entryNotification){ + if (entryNotification.value.getBoolean()){ + ChangeCameraValues(25,15); + } else{ + Pipeline pipeline = SettingsManager.Cameras.get(CameraName).pipelines.get(SettingsManager.CamerasCurrentPipeline.get(CameraName)); + ChangeCameraValues(pipeline.exposure, pipeline.brightness); + } + } + private void PipelineListener(EntryNotification entryNotification){ + if (SettingsManager.Cameras.get(CameraName).pipelines.containsKey(entryNotification.value.getString())){ + SettingsManager.CamerasCurrentPipeline.put(CameraName,entryNotification.value.getString()); + Pipeline pipeline = SettingsManager.Cameras.get(CameraName).pipelines.get(SettingsManager.CamerasCurrentPipeline.get(CameraName)); + ChangeCameraValues(pipeline.exposure, pipeline.brightness); + //TODO Send Pipeline change using websocket to client + } else{ + ntPipelineEntry.setString(SettingsManager.CamerasCurrentPipeline.get(CameraName)); + } + } public CameraProcess(String CameraName) { this.CameraName = CameraName; @@ -36,7 +54,14 @@ public class CameraProcess implements Runnable { 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"); + ntDriverModeEntry.addListener(this::DriverModeListener, EntryListenerFlags.kUpdate); + ntPipelineEntry.addListener(this::PipelineListener, EntryListenerFlags.kUpdate); + ntDriverModeEntry.setBoolean(false); + ntPipelineEntry.setString(SettingsManager.CamerasCurrentPipeline.get(CameraName)); imgWidth = SettingsManager.Cameras.get(CameraName).camVideoMode.width; imgHeight = SettingsManager.Cameras.get(CameraName).camVideoMode.height; } @@ -97,6 +122,9 @@ public class CameraProcess implements Runnable { GroupedContours = visionProcess.GroupTargets(FilteredContours, currentPipeline.target_intersection, currentPipeline.target_group); if (GroupedContours.size() > 0) { var finalRect = visionProcess.SortTargetsToOne(GroupedContours, currentPipeline.sort_mode); + // TODO Add calibration calc + //TODO Calc Pitch Yaw And Distance Send it them using networktables + // TODO Send pitch yaw distance and Raw Point using websockets to client for calic calc if (finalRect != null) { List a = new ArrayList<>(); Point[] vertices = new Point[4]; @@ -104,16 +132,16 @@ public class CameraProcess implements Runnable { a.add(new MatOfPoint(vertices)); Imgproc.drawContours(outputMat, a, 0, contourColor, 3); } + } } } cv_publish.putFrame(outputMat); - // 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()); + System.out.printf("%s Process time: %fms, FPS: %.2f, FoundContours: %d, FilteredContours: %d, GroupedContours: %d\n",CameraName ,processTimeMs, fps, FoundContours.size(), FilteredContours.size(), GroupedContours.size()); inputMat.release(); hsvThreshMat.release(); 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 fdba65375..2b7bbc43b 100644 --- a/Main/src/main/java/com/chameleonvision/vision/process/VisionProcess.java +++ b/Main/src/main/java/com/chameleonvision/vision/process/VisionProcess.java @@ -105,7 +105,8 @@ public class VisionProcess { case "Rightmost": return Collections.max(inputRects, Comparator.comparing(rect -> rect.center.x)); case "Centermost": - return inputRects.stream().sorted(SortByCentermostComparator).collect(Collectors.toList()).get(0); + return Collections.min(inputRects, SortByCentermostComparator); +// 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 }