From f884bb885bec5f3072947e7d4a0fd47e192d7b53 Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Mon, 16 Feb 2026 15:06:02 -0500 Subject: [PATCH 1/3] Getting Camera to work --- src/main/deploy/elastic/drivercam-auto.json | 31 ++++ src/main/java/frc/robot/Robot.java | 2 + .../frc/robot/subsystems/CameraThread.java | 175 ++++++++++++++++++ 3 files changed, 208 insertions(+) create mode 100644 src/main/deploy/elastic/drivercam-auto.json create mode 100644 src/main/java/frc/robot/subsystems/CameraThread.java diff --git a/src/main/deploy/elastic/drivercam-auto.json b/src/main/deploy/elastic/drivercam-auto.json new file mode 100644 index 00000000..043b71c2 --- /dev/null +++ b/src/main/deploy/elastic/drivercam-auto.json @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "grid_size": 128, + "tabs": [ + { + "name": "drivercam", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "DriverCam", + "x": 0.0, + "y": 0.0, + "width": 1280.0, + "height": 768.0, + "type": "Camera Stream", + "properties": { + "streamsTopic": "/CameraPublisher/USB Camera 0/streams", + "streams-topic": "/CameraPublisher/USB Camera 0/streams", + "streams_topic": "/CameraPublisher/USB Camera 0/streams", + "topic": "/CameraPublisher/USB Camera 0/streams", + "sourceUrl": "http://roborio-4048-frc.local:1181/?action=stream", + "sourceURL": "http://roborio-4048-frc.local:1181/?action=stream", + "source-url": "http://roborio-4048-frc.local:1181/?action=stream" + } + } + ] + } + } + ] +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ce872885..98933124 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -24,6 +24,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.autochooser.FieldLocation; import frc.robot.constants.Constants; +import frc.robot.subsystems.CameraThread; import frc.robot.utils.logging.commands.CommandLogger; /** @@ -76,6 +77,7 @@ public Robot() { // Start AdvantageKit logger Logger.start(); CommandLogger.get().init(); + new CameraThread(); // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. diff --git a/src/main/java/frc/robot/subsystems/CameraThread.java b/src/main/java/frc/robot/subsystems/CameraThread.java new file mode 100644 index 00000000..f9dd6281 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/CameraThread.java @@ -0,0 +1,175 @@ +package frc.robot.subsystems; + +import java.util.ArrayList; + +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; + +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.cscore.CvSink; +import edu.wpi.first.cscore.CvSource; +import edu.wpi.first.cscore.MjpegServer; +import edu.wpi.first.cscore.UsbCamera; +import edu.wpi.first.cscore.VideoSource; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StringArrayPublisher; +import edu.wpi.first.util.PixelFormat; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class CameraThread { + + private static final int INPUT_WIDTH = 640 / 4; + private static final int INPUT_HEIGHT = 480 / 4; + private static final int OUTPUT_WIDTH = INPUT_HEIGHT; + private static final int OUTPUT_HEIGHT = INPUT_WIDTH; + + private static final String LOGGING_PREFIX = "DriverCam"; + private static final String OUTPUT_SERVER_NAME = "serve_" + LOGGING_PREFIX; + private static final int OUTPUT_PORT = 1182; + private static final int CAMERA_INDEX = 0; + private static final int FRAME_ERROR_RETRY_MS = 20; + + // Leaving this commented out now, may bring back double lines + // private static final double TOP_Y = 45; + // private static final double BOTTOM_Y = 72; + + private static final double HORIZ_LINE = 85.0; + + public CameraThread() { + CameraRunner runner = new CameraRunner(); + Thread cameraThread = new Thread(runner, "CameraThread"); + cameraThread.setDaemon(true); + cameraThread.start(); + } + + private class CameraRunner implements Runnable { + @Override + public void run() { + try { + processImage(); + } catch (Throwable e) { + e.printStackTrace(); + Logger.recordOutput(LOGGING_PREFIX + "/fatalError", e.toString()); + return; + } + } + + private void processImage() { + UsbCamera camera = CameraServer.startAutomaticCapture("USB Camera " + CAMERA_INDEX, CAMERA_INDEX); + camera.setConnectionStrategy(VideoSource.ConnectionStrategy.kKeepOpen); + camera.setResolution(INPUT_WIDTH, INPUT_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(camera); + CvSource outputStream = + new CvSource(LOGGING_PREFIX, PixelFormat.kBGR, OUTPUT_WIDTH, OUTPUT_HEIGHT, 30); + CameraServer.addCamera(outputStream); + + MjpegServer outputServer = CameraServer.addServer(OUTPUT_SERVER_NAME, OUTPUT_PORT); + outputServer.setSource(outputStream); + Logger.recordOutput(LOGGING_PREFIX + "/streamPort", outputServer.getPort()); + int teamNumber = RobotController.getTeamNumber(); + String[] streamUrls = buildStreamUrls(teamNumber, outputServer.getPort()); + NetworkTable cameraPublisherTable = + NetworkTableInstance.getDefault().getTable("CameraPublisher").getSubTable(LOGGING_PREFIX); + StringArrayPublisher streamsPublisher = cameraPublisherTable.getStringArrayTopic("streams").publish(); + streamsPublisher.set(streamUrls); + SmartDashboard.putString(LOGGING_PREFIX + "URL", streamUrls[0]); + Logger.recordOutput(LOGGING_PREFIX + "/streamUrl", streamUrls[0]); + Logger.recordOutput(LOGGING_PREFIX + "/streams", streamUrls); + + // Mats are very expensive. Let's reuse this Mat. + Mat cameraMat = new Mat(); + + int errorCount = 0; + + Mat rotatedMat = new Mat(); + + while (!Thread.currentThread().isInterrupted()) { + + 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); + Logger.recordOutput(LOGGING_PREFIX + "/lastError", cvSink.getError()); + // Send the output the error. + outputStream.notifyError(cvSink.getError()); + try { + Thread.sleep(FRAME_ERROR_RETRY_MS); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + break; + } + // skip the rest of the current iteration + continue; + } + + long mark1 = System.currentTimeMillis(); + Logger.recordOutput(LOGGING_PREFIX + "/mark1", (mark1 - startTime)); + + drawLines(cameraMat); + + Core.flip(cameraMat, cameraMat, 0); + Core.transpose(cameraMat, rotatedMat); + + // Give the output stream a new image to display + outputStream.putFrame(rotatedMat); + long endTime = System.currentTimeMillis(); + Logger.recordOutput(LOGGING_PREFIX + "/frameProcessingTimeMS", (endTime - startTime)); + } + } + + private void drawLines(Mat mat) { + + Imgproc.line( + mat, + new Point(HORIZ_LINE, 0), + new Point(HORIZ_LINE, INPUT_HEIGHT), + new Scalar(20, 97, 255)); + Imgproc.line( + mat, + new Point(0, INPUT_HEIGHT / 2.0), + new Point(INPUT_WIDTH, INPUT_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(INPUT_WIDTH, TOP_Y), new Scalar(0, 255, + // 0)); + // Imgproc.line(mat, new Point(0, BOTTOM_Y), new Point(INPUT_WIDTH, BOTTOM_Y), new Scalar(0, + // 255, 0)); + } + + private String[] buildStreamUrls(int teamNumber, int port) { + ArrayList urls = new ArrayList<>(); + if (teamNumber > 0) { + String teamIpUrl = + "http://10." + + (teamNumber / 100) + + "." + + (teamNumber % 100) + + ".2:" + + port + + "/?action=stream"; + urls.add(teamIpUrl); + urls.add("mjpg:" + teamIpUrl); + + String mdnsUrl = "http://roborio-" + teamNumber + "-frc.local:" + port + "/?action=stream"; + urls.add(mdnsUrl); + } + + String localUrl = "http://localhost:" + port + "/?action=stream"; + urls.add(localUrl); + urls.add("mjpg:" + localUrl); + return urls.toArray(new String[0]); + } + } +} From f6e7152a12f2ee06630d2ac62266d85e10816e33 Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Mon, 16 Feb 2026 16:14:24 -0500 Subject: [PATCH 2/3] Fixed Elastic layout with drive cam --- .../elastic-4048-2026-dev-v1.0.json | 25 ++++++++------- src/main/deploy/elastic/drivercam-auto.json | 31 ------------------- 2 files changed, 13 insertions(+), 43 deletions(-) delete mode 100644 src/main/deploy/elastic/drivercam-auto.json diff --git a/elastic_layouts/elastic-4048-2026-dev-v1.0.json b/elastic_layouts/elastic-4048-2026-dev-v1.0.json index 903fd438..be39e65e 100644 --- a/elastic_layouts/elastic-4048-2026-dev-v1.0.json +++ b/elastic_layouts/elastic-4048-2026-dev-v1.0.json @@ -62,18 +62,6 @@ } } ] - }, - { - "title": "camera goes here", - "x": 768.0, - "y": 0.0, - "width": 512.0, - "height": 384.0, - "type": "List Layout", - "properties": { - "label_position": "TOP" - }, - "children": [] } ], "containers": [ @@ -127,6 +115,19 @@ "period": 0.06, "sort_options": false } + }, + { + "title": "DriverCam", + "x": 768.0, + "y": 0.0, + "width": 512.0, + "height": 384.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/DriverCam", + "period": 0.06, + "rotation_turns": 0 + } } ] } diff --git a/src/main/deploy/elastic/drivercam-auto.json b/src/main/deploy/elastic/drivercam-auto.json deleted file mode 100644 index 043b71c2..00000000 --- a/src/main/deploy/elastic/drivercam-auto.json +++ /dev/null @@ -1,31 +0,0 @@ -{ - "version": 1.0, - "grid_size": 128, - "tabs": [ - { - "name": "drivercam", - "grid_layout": { - "layouts": [], - "containers": [ - { - "title": "DriverCam", - "x": 0.0, - "y": 0.0, - "width": 1280.0, - "height": 768.0, - "type": "Camera Stream", - "properties": { - "streamsTopic": "/CameraPublisher/USB Camera 0/streams", - "streams-topic": "/CameraPublisher/USB Camera 0/streams", - "streams_topic": "/CameraPublisher/USB Camera 0/streams", - "topic": "/CameraPublisher/USB Camera 0/streams", - "sourceUrl": "http://roborio-4048-frc.local:1181/?action=stream", - "sourceURL": "http://roborio-4048-frc.local:1181/?action=stream", - "source-url": "http://roborio-4048-frc.local:1181/?action=stream" - } - } - ] - } - } - ] -} From da27ca5ecf9be5bb6d25947db80efbc4c07c9d17 Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Mon, 16 Feb 2026 22:12:46 -0500 Subject: [PATCH 3/3] Got camera to work --- src/main/java/frc/robot/Robot.java | 12 +++++++++--- src/main/java/frc/robot/RobotContainer.java | 15 +++++++++------ .../java/frc/robot/constants/GameConstants.java | 1 + .../java/frc/robot/subsystems/CameraThread.java | 2 +- 4 files changed, 20 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 98933124..59c7ee63 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -14,7 +14,11 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import edu.wpi.first.cscore.VideoSource; + +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.cscore.UsbCamera; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -24,7 +28,6 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.autochooser.FieldLocation; import frc.robot.constants.Constants; -import frc.robot.subsystems.CameraThread; import frc.robot.utils.logging.commands.CommandLogger; /** @@ -77,8 +80,11 @@ public Robot() { // Start AdvantageKit logger Logger.start(); CommandLogger.get().init(); - new CameraThread(); - + + UsbCamera camera = CameraServer.startAutomaticCapture("DriverCam", 0); + camera.setConnectionStrategy(VideoSource.ConnectionStrategy.kKeepOpen); + CameraServer.addCamera(camera); + // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. robotContainer = new RobotContainer(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d684ab9e..1d134a34 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,8 +5,7 @@ package frc.robot; import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.Filesystem; + import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -49,13 +48,10 @@ import frc.robot.utils.logging.io.gyro.ThreadedGyro; import frc.robot.utils.logging.io.gyro.ThreadedGyroSwerveIMU; import frc.robot.utils.simulation.RobotVisualizer; +import frc.robot.subsystems.CameraThread; import swervelib.SwerveInputStream; import swervelib.imu.SwerveIMU; -import frc.robot.utils.logging.io.BaseIoImpl; -import frc.robot.apriltags.ApriltagInputs; import frc.robot.apriltags.ApriltagReading; -import frc.robot.apriltags.MockApriltagIo; -import frc.robot.apriltags.TCPApriltagIo; import java.io.File; @@ -69,6 +65,11 @@ * subsystems, commands, and trigger mappings) should be declared here. */ public class RobotContainer { + private static final String DRIVER_CAM_PREFIX = CameraThread.LOGGING_PREFIX; + private static final String DRIVER_CAM_SERVER_NAME = "serve_" + DRIVER_CAM_PREFIX; + private static final int DRIVER_CAM_PORT = 1182; + private static final int DRIVER_CAM_FPS = 30; + // Instantiate the autochooser. private final AutoChooser autoChooser = new AutoChooser(); // The robot's subsystems and commands are defined here... @@ -166,8 +167,10 @@ public RobotContainer() { configureBindings(); putShuffleboardCommands(); + } + /** * Use this method to define your trigger->command mappings. Triggers can be * created via the diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index ae9c5f56..cef0289e 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -125,4 +125,5 @@ public enum Mode { public static final int SHIFT_4_START = 55; public static final int ENDGAME_START = 30; + public static final String DRIVER_CAM_IP_ADDRESS = "10.40.48.2:1181/?action=stream"; } diff --git a/src/main/java/frc/robot/subsystems/CameraThread.java b/src/main/java/frc/robot/subsystems/CameraThread.java index f9dd6281..e959e72d 100644 --- a/src/main/java/frc/robot/subsystems/CameraThread.java +++ b/src/main/java/frc/robot/subsystems/CameraThread.java @@ -29,7 +29,7 @@ public class CameraThread { private static final int OUTPUT_WIDTH = INPUT_HEIGHT; private static final int OUTPUT_HEIGHT = INPUT_WIDTH; - private static final String LOGGING_PREFIX = "DriverCam"; + public static final String LOGGING_PREFIX = "DriverCam"; private static final String OUTPUT_SERVER_NAME = "serve_" + LOGGING_PREFIX; private static final int OUTPUT_PORT = 1182; private static final int CAMERA_INDEX = 0;