Skip to content

Refactored CameraThread #266

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
noahheller wants to merge 3 commits into
base: main
Choose a base branch
from
Open

Refactored CameraThread #266

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
3b4dd5e
refactored CameraThread
Mar 16, 2025
551ac23
moved camera init into run instead of overriding start.
Mar 17, 2025
7ec80a0
added return on error
noahheller Mar 19, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/Robot.java
View file
Open in desktop
Copy link
Contributor

@bengold22 bengold22 Mar 16, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would like to understand what the issue was in replay rather than merge this as-is. Switching to Thread instead of Runnable is essentially switching "6" for a "half-dozen", it's semantically the same thing. The only difference I can see is the check at the start to see if the camera is connected, which could have been the only refactoring needed. Moving everything to instance variables doesn't really make any functional sense to me either.

Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,7 @@ public Robot() {
CommandLogger.get().init();
robotContainer = new RobotContainer();

new CameraThread();
new CameraThread().start();
}

public static RobotMode getMode() {
Expand Down
133 changes: 72 additions & 61 deletions src/main/java/frc/robot/camera/CameraThread.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,14 +4,15 @@
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;
import org.opencv.core.Point;
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;
Expand All @@ -24,81 +25,91 @@ 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;
private boolean running = true;

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 void run() {
if (!initializeCamera()){
return;
}
while (running) {
try {
processImage();
} catch (Exception e) {
e.printStackTrace();
return;
DriverStation.reportError("CameraServerException", true);
return;
}
}
}

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;
}
private boolean initializeCamera() {
camera = CameraServer.startAutomaticCapture();
if (!camera.isValid() || !camera.isConnected()) {
DriverStation.reportError("Driver Camera is not connected!", false);
return false;
}
// 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);
return true;
}

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));
}
}
10 changes: 8 additions & 2 deletions src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand Down