Added Streaming camera

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
+            }
           }
         ]
       }

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();

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

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";
 }

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<String> 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]);
+    }
+  }
+}