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/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ce872885..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; @@ -76,7 +80,11 @@ public Robot() { // Start AdvantageKit logger Logger.start(); CommandLogger.get().init(); - + + 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 new file mode 100644 index 00000000..e959e72d --- /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; + + 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; + 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]); + } + } +}