From 3b4dd5ee18e7d88ba5bef3045f78e6536331157a Mon Sep 17 00:00:00 2001 From: codetoad Date: Sun, 16 Mar 2025 15:33:00 -0400 Subject: [PATCH 1/3] refactored CameraThread --- src/main/java/frc/robot/Robot.java | 2 +- .../java/frc/robot/camera/CameraThread.java | 131 +++++++++--------- .../subsystems/swervev3/io/SwerveModule.java | 10 +- 3 files changed, 78 insertions(+), 65 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f6694c38..a6eeda83 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -83,7 +83,7 @@ public Robot() { CommandLogger.get().init(); robotContainer = new RobotContainer(); - new CameraThread(); + new CameraThread().start(); } public static RobotMode getMode() { diff --git a/src/main/java/frc/robot/camera/CameraThread.java b/src/main/java/frc/robot/camera/CameraThread.java index 23dd1e18..1178c11a 100644 --- a/src/main/java/frc/robot/camera/CameraThread.java +++ b/src/main/java/frc/robot/camera/CameraThread.java @@ -4,6 +4,7 @@ import edu.wpi.first.cscore.CvSink; import edu.wpi.first.cscore.CvSource; import edu.wpi.first.cscore.UsbCamera; +import edu.wpi.first.wpilibj.DriverStation; import org.littletonrobotics.junction.Logger; import org.opencv.core.Core; import org.opencv.core.Mat; @@ -11,7 +12,7 @@ import org.opencv.core.Scalar; import org.opencv.imgproc.Imgproc; -public class CameraThread { +public class CameraThread extends Thread { private static final int WIDTH = 640 / 4; private static final int HEIGHT = 480 / 4; @@ -24,81 +25,87 @@ public class CameraThread { private static final double HORIZ_LINE = 85.0; + private UsbCamera camera; + private CvSink cvSink; + private CvSource outputStream; + private Mat cameraMat; + private Mat rotatedMat; + private int errorCount; + public CameraThread() { - CameraRunner runner = new CameraRunner(); - Thread cameraThread = new Thread(runner, "CameraThread"); - cameraThread.setDaemon(true); - cameraThread.start(); + super("CameraThread"); + setDaemon(true); } - private class CameraRunner implements Runnable { - @Override - public void run() { + @Override + public synchronized void start() { + camera = CameraServer.startAutomaticCapture(); + if (!camera.isValid() || !camera.isConnected()) { + DriverStation.reportError("Driver Camera is not connected!", false); + return; + } + // Get the USB Camera from the camera server + camera.setResolution(WIDTH, HEIGHT); + + // Get a CvSink. This will capture Mats from the Camera + // Setup a CvSource. This will send images back to the dashboard + cvSink = CameraServer.getVideo(); + outputStream = CameraServer.putVideo(LOGGING_PREFIX, WIDTH, HEIGHT); + + // Mats are very expensive. Let's reuse this Mat. + cameraMat = new Mat(); + errorCount = 0; + rotatedMat = Mat.zeros(HEIGHT, WIDTH, 0); + super.start(); + } + + @Override + public void run() { + while (true) { try { processImage(); } catch (Exception e) { - e.printStackTrace(); - return; + DriverStation.reportError("CameraServerException", true); } } + } - private void processImage() { - UsbCamera camera = CameraServer.startAutomaticCapture(); - // Get the USB Camera from the camera server - camera.setResolution(WIDTH, HEIGHT); - - // Get a CvSink. This will capture Mats from the Camera - // Setup a CvSource. This will send images back to the dashboard - CvSink cvSink = CameraServer.getVideo(); - CvSource outputStream = CameraServer.putVideo(LOGGING_PREFIX, WIDTH, HEIGHT); - - // Mats are very expensive. Let's reuse this Mat. - Mat cameraMat = new Mat(); - - int errorCount = 0; - - Mat rotatedMat = Mat.zeros(HEIGHT, WIDTH, 0); - - while (true) { - - long startTime = System.currentTimeMillis(); - // Tell the CvSink to grab a frame from the camera and put it - // in the source mat. If there is an error notify the output. - if (cvSink.grabFrame(cameraMat) == 0) { - errorCount++; - Logger.recordOutput(LOGGING_PREFIX + "/errorCount", errorCount); - // Send the output the error. - outputStream.notifyError(cvSink.getError()); - // skip the rest of the current iteration - continue; - } - - long mark1 = System.currentTimeMillis(); - Logger.recordOutput(LOGGING_PREFIX + "/mark1", (mark1 - startTime)); + private void processImage() { + double startTime = System.nanoTime() / 1e-6; + // Tell the CvSink to grab a frame from the camera and put it + // in the source mat. If there is an error notify the output. + if (cvSink.grabFrame(cameraMat) == 0) { + errorCount++; + Logger.recordOutput(LOGGING_PREFIX + "/errorCount", errorCount); + // Send the output the error. + outputStream.notifyError(cvSink.getError()); + // skip the rest of the current iteration + return; + } - drawLines(cameraMat); + double mark1 = System.nanoTime() / 1e-6; + Logger.recordOutput(LOGGING_PREFIX + "/mark1", (mark1 - startTime)); - Core.flip(cameraMat, cameraMat, 0); - Core.transpose(cameraMat, rotatedMat); + drawLines(cameraMat); - // Give the output stream a new image to display - outputStream.putFrame(rotatedMat); - long endTime = System.currentTimeMillis(); - Logger.recordOutput(LOGGING_PREFIX + "/frameProcessingTimeMS", (endTime - startTime)); - } - } + Core.flip(cameraMat, cameraMat, 0); + Core.transpose(cameraMat, rotatedMat); - private void drawLines(Mat mat) { + // Give the output stream a new image to display + outputStream.putFrame(rotatedMat); + double endTime = System.nanoTime() / 1e-6; + Logger.recordOutput(LOGGING_PREFIX + "/frameProcessingTimeMS", (endTime - startTime)); + } - Imgproc.line( - mat, new Point(HORIZ_LINE, 0), new Point(HORIZ_LINE, HEIGHT), new Scalar(20, 97, 255)); - Imgproc.line( - mat, new Point(0, HEIGHT / 2), new Point(WIDTH, HEIGHT / 2), new Scalar(20, 97, 255)); + private void drawLines(Mat mat) { + Imgproc.line( + mat, new Point(HORIZ_LINE, 0), new Point(HORIZ_LINE, HEIGHT), new Scalar(20, 97, 255)); + Imgproc.line( + mat, new Point(0, HEIGHT / 2.0), new Point(WIDTH, HEIGHT / 2.0), new Scalar(20, 97, 255)); - // Leaving this commented out right now, may bring back two lines - // Imgproc.line(mat, new Point(0, TOP_Y), new Point(WIDTH, TOP_Y), new Scalar(0, 255, 0)); - // Imgproc.line(mat, new Point(0, BOTTOM_Y), new Point(WIDTH, BOTTOM_Y), new Scalar(0, 255, - // 0)); - } + // Leaving this commented out right now, may bring back two lines + // Imgproc.line(mat, new Point(0, TOP_Y), new Point(WIDTH, TOP_Y), new Scalar(0, 255, 0)); + // Imgproc.line(mat, new Point(0, BOTTOM_Y), new Point(WIDTH, BOTTOM_Y), new Scalar(0, 255, + // 0)); } } diff --git a/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java b/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java index dc563b46..f59f49ac 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java @@ -46,9 +46,15 @@ public SwerveModule( TrapezoidProfile.Constraints goalConstraint, String moduleName) { MotorInputs driveInputs = - new MotorInputBuilder<>("Drivetrain/" + moduleName + "/Drive").addEncoder().motorCurrent().build(); + new MotorInputBuilder<>("Drivetrain/" + moduleName + "/Drive") + .addEncoder() + .motorCurrent() + .build(); MotorInputs steerInputs = - new MotorInputBuilder<>("Drivetrain/" + moduleName + "/Steer").addEncoder().motorCurrent().build(); + new MotorInputBuilder<>("Drivetrain/" + moduleName + "/Steer") + .addEncoder() + .motorCurrent() + .build(); this.driveSystem = new LoggableSystem<>(driveMotorIO, driveInputs); this.steerSystem = new LoggableSystem<>(steerMotorIO, steerInputs); this.absSystem = new LoggableSystem<>(absIO, new SwerveAbsInput("Drivetrain/" + moduleName)); From 551ac23a0ec2ae079808d454c3c6401dc1754369 Mon Sep 17 00:00:00 2001 From: codetoad Date: Mon, 17 Mar 2025 09:34:32 -0400 Subject: [PATCH 2/3] moved camera init into run instead of overriding start. --- .../java/frc/robot/camera/CameraThread.java | 31 ++++++++++--------- 1 file changed, 17 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/camera/CameraThread.java b/src/main/java/frc/robot/camera/CameraThread.java index 1178c11a..419c57f5 100644 --- a/src/main/java/frc/robot/camera/CameraThread.java +++ b/src/main/java/frc/robot/camera/CameraThread.java @@ -31,6 +31,7 @@ public class CameraThread extends Thread { private Mat cameraMat; private Mat rotatedMat; private int errorCount; + private boolean running = true; public CameraThread() { super("CameraThread"); @@ -38,11 +39,24 @@ public CameraThread() { } @Override - public synchronized void start() { + public void run() { + if (!initializeCamera()){ + return; + } + while (running) { + try { + processImage(); + } catch (Exception e) { + DriverStation.reportError("CameraServerException", true); + } + } + } + + private boolean initializeCamera() { camera = CameraServer.startAutomaticCapture(); if (!camera.isValid() || !camera.isConnected()) { DriverStation.reportError("Driver Camera is not connected!", false); - return; + return false; } // Get the USB Camera from the camera server camera.setResolution(WIDTH, HEIGHT); @@ -56,18 +70,7 @@ public synchronized void start() { cameraMat = new Mat(); errorCount = 0; rotatedMat = Mat.zeros(HEIGHT, WIDTH, 0); - super.start(); - } - - @Override - public void run() { - while (true) { - try { - processImage(); - } catch (Exception e) { - DriverStation.reportError("CameraServerException", true); - } - } + return true; } private void processImage() { From 7ec80a097a5ab844984eb51842063ac085999db0 Mon Sep 17 00:00:00 2001 From: Code Toad <55628278+goatfanboi23@users.noreply.github.com> Date: Wed, 19 Mar 2025 08:48:01 -0400 Subject: [PATCH 3/3] added return on error --- src/main/java/frc/robot/camera/CameraThread.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/camera/CameraThread.java b/src/main/java/frc/robot/camera/CameraThread.java index 419c57f5..5329ae58 100644 --- a/src/main/java/frc/robot/camera/CameraThread.java +++ b/src/main/java/frc/robot/camera/CameraThread.java @@ -48,6 +48,7 @@ public void run() { processImage(); } catch (Exception e) { DriverStation.reportError("CameraServerException", true); +return; } } }