From b2b2d00dce8b6b15682e17241c51067a3ef03272 Mon Sep 17 00:00:00 2001 From: Unknown Date: Wed, 18 Sep 2019 13:48:34 +0300 Subject: [PATCH] added networktables send and websocket send of current camera --- Main/chameleon-vision.iml | 63 +++++++++++++++++++ .../vision/process/CameraProcess.java | 32 ++++++++-- .../vision/process/PipelineResult.java | 3 + .../java/com/chameleonvision/web/Server.java | 2 +- 4 files changed, 93 insertions(+), 7 deletions(-) create mode 100644 Main/chameleon-vision.iml diff --git a/Main/chameleon-vision.iml b/Main/chameleon-vision.iml new file mode 100644 index 000000000..d50a9a25c --- /dev/null +++ b/Main/chameleon-vision.iml @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file 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 aa4bcf62a..f92a85a36 100644 --- a/Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java +++ b/Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java @@ -4,6 +4,7 @@ import com.chameleonvision.MemoryManager; import com.chameleonvision.settings.SettingsManager; import com.chameleonvision.vision.CameraValues; import com.chameleonvision.vision.Pipeline; +import com.chameleonvision.web.Server; import edu.wpi.cscore.CvSink; import edu.wpi.cscore.CvSource; import edu.wpi.first.networktables.*; @@ -12,6 +13,7 @@ import org.opencv.core.*; import org.opencv.imgproc.Imgproc; import java.util.ArrayList; +import java.util.HashMap; import java.util.List; public class CameraProcess implements Runnable { @@ -73,7 +75,7 @@ public class CameraProcess implements Runnable { public CameraProcess(String cameraName) { CameraName = cameraName; - + SettingsManager.CamerasCurrentPipeline.put(CameraName,SettingsManager.Cameras.get(CameraName).pipelines.keySet().stream().findFirst().toString()); // NetworkTables NetworkTable ntTable = NetworkTableInstance.getDefault().getTable("/chameleon-vision/" + cameraName); ntPipelineEntry = ntTable.getEntry("Pipeline"); @@ -131,6 +133,7 @@ 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); + pipelineResult.RawPoint = finalRect; pipelineResult.IsValid = true; if (!currentPipeline.is_calibrated) { pipelineResult.CalibratedX = camVals.CenterX; @@ -141,7 +144,6 @@ public class CameraProcess implements Runnable { pipelineResult.Pitch = camVals.CalculatePitch(finalRect.center.y, pipelineResult.CalibratedY); pipelineResult.Yaw = camVals.CalculateYaw(finalRect.center.x, pipelineResult.CalibratedX); } - // Send calc using networktables // TODO Send pitch yaw distance and Raw Point using websockets to client for calib calc drawContour(outputImage, finalRect); } @@ -154,21 +156,20 @@ public class CameraProcess implements Runnable { @Override public void run() { // processing time tracking - long startTime; + long startTime, TimeStamp; double processTimeMs; - double fps; + double fps = 0; while (!Thread.interrupted()) { FoundContours.clear(); FilteredContours.clear(); GroupedContours.clear(); - SettingsManager.CamerasCurrentPipeline.put(CameraName,SettingsManager.Cameras.get(CameraName).pipelines.keySet().stream().findFirst().toString()); currentPipeline = SettingsManager.Cameras.get(CameraName).pipelines.get(SettingsManager.CamerasCurrentPipeline.get(CameraName)); // System.out.println(SettingsManager.CamerasCurrentPipeline.get(CameraName)); // start fps counter right before grabbing input frame startTime = System.nanoTime(); - cvSink.grabFrame(cameraInputMat); + TimeStamp = cvSink.grabFrame(cameraInputMat); if (cameraInputMat.cols() == 0 && cameraInputMat.rows() == 0) { continue; } @@ -176,6 +177,25 @@ public class CameraProcess implements Runnable { // get vision data var pipelineResult = runVisionProcess(cameraInputMat, streamOutputMat); + ntValidEntry.setBoolean(pipelineResult.IsValid); + if (pipelineResult.IsValid){ + ntYawEntry.setNumber(pipelineResult.Yaw); + ntPitchEntry.setNumber(pipelineResult.Pitch); + } + ntTimeStampEntry.setNumber(TimeStamp); + if (CameraName.equals(SettingsManager.GeneralSettings.curr_camera)){ + HashMap WebSend = new HashMap<>(); + HashMap point = new HashMap<>(); + List center = new ArrayList(); + center.add(pipelineResult.RawPoint.center.x); + center.add(pipelineResult.RawPoint.center.y); + point.put("pitch", pipelineResult.Pitch); + point.put("yaw", pipelineResult.Yaw); + point.put("fps", fps); + WebSend.put("point", point); + WebSend.put("raw_point",center); + Server.broadcastMessage(WebSend); + } cvPublish.putFrame(streamOutputMat); // calculate FPS after publishing output frame processTimeMs = (System.nanoTime() - startTime) * 1e-6; diff --git a/Main/src/main/java/com/chameleonvision/vision/process/PipelineResult.java b/Main/src/main/java/com/chameleonvision/vision/process/PipelineResult.java index 76643cb7a..47c7bbe1a 100644 --- a/Main/src/main/java/com/chameleonvision/vision/process/PipelineResult.java +++ b/Main/src/main/java/com/chameleonvision/vision/process/PipelineResult.java @@ -1,9 +1,12 @@ package com.chameleonvision.vision.process; +import org.opencv.core.RotatedRect; + public class PipelineResult { public boolean IsValid = false; public double CalibratedX = 0.0; public double CalibratedY = 0.0; public double Pitch = 0.0; public double Yaw = 0.0; + RotatedRect RawPoint; } diff --git a/Main/src/main/java/com/chameleonvision/web/Server.java b/Main/src/main/java/com/chameleonvision/web/Server.java index 42c61c665..89e7db432 100644 --- a/Main/src/main/java/com/chameleonvision/web/Server.java +++ b/Main/src/main/java/com/chameleonvision/web/Server.java @@ -150,7 +150,7 @@ public class Server { } } - private static void broadcastMessage(Object obj) { + public static void broadcastMessage(Object obj) { broadcastMessage(obj, null);//Broadcasts the message to ever user }