photonvision memory leak again

Author: FRC-6201

build.gradle

@@ -33,6 +33,10 @@ deploy {
                     jvmArgs.add("-XX:GCTimeRatio=5")
                     jvmArgs.add("-XX:+UseSerialGC")
                     jvmArgs.add("-XX:MaxGCPauseMillis=50")
+                    // Enable automatic heap dumps on OutOfMemoryError
+                    // Note: the heap dump path here is a path on a USB flash drive, see below
+                    // jvmArgs.add("-XX:+HeapDumpOnOutOfMemoryError")
+                    // jvmArgs.add("-XX:HeapDumpPath=/tmp/frc-usercode.hprof")
 
                     // The options below may improve performance, but should only be enabled on the RIO 2
                     //

src/main/java/frc/robot/Robot.java

@@ -7,6 +7,7 @@
 
 package frc.robot;
 
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -26,8 +27,10 @@
 public class Robot extends LoggedRobot {
   private Command autonomousCommand;
   private RobotContainer robotContainer;
+  Timer gcTimer = new Timer();
 
   public Robot() {
+    gcTimer.start();
     // Record metadata
     Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
     Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
@@ -88,6 +91,11 @@ public void robotPeriodic() {
 
     // Return to non-RT thread priority (do not modify the first argument)
     // Threads.setCurrentThreadPriority(false, 10);
+
+    // run the garbage collector every 5 seconds
+    if (gcTimer.advanceIfElapsed(5)) {
+      System.gc();
+    }
   }
 
   /** This function is called once when the robot is disabled. */

src/main/java/frc/robot/RobotContainer.java

@@ -100,15 +100,15 @@ public RobotContainer() {
                 new ModuleIOTalonFX(TunerConstants.BackRight));
         vision =
             new Vision(
-                drive::addVisionMeasurement,
-                new VisionIOPhotonVision(
-                    VisionConstants.camera0Name, VisionConstants.robotToCamera0),
-                new VisionIOPhotonVision(
-                    VisionConstants.camera1Name, VisionConstants.robotToCamera1),
-                new VisionIOPhotonVision(
-                    VisionConstants.camera2Name, VisionConstants.robotToCamera2),
-                new VisionIOPhotonVision(
-                    VisionConstants.camera3Name, VisionConstants.robotToCamera3));
+                drive::addVisionMeasurement, new VisionIO(){});
+                // new VisionIOPhotonVision(
+                //     VisionConstants.camera0Name, VisionConstants.robotToCamera0),
+                // new VisionIOPhotonVision(
+                //     VisionConstants.camera1Name, VisionConstants.robotToCamera1));
+        // new VisionIOPhotonVision(
+        //     VisionConstants.camera2Name, VisionConstants.robotToCamera2),
+        // new VisionIOPhotonVision(
+        //     VisionConstants.camera3Name, VisionConstants.robotToCamera3));
         shooter = new Shooter();
         hopper = new Hopper();
         configureShooterTestBindings(); // configureButtonBindings();

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -29,6 +29,8 @@
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 // import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -46,6 +48,7 @@ public class Drive extends SubsystemBase {
   private final PIDController xController = new PIDController(10.0, 0.0, 0.0);
   private final PIDController yController = new PIDController(10.0, 0.0, 0.0);
   private final PIDController headingController = new PIDController(7.5, 0.0, 0.0);
+  private final Field2d loggedField = new Field2d();
 
   // TunerConstants doesn't include these constants, so they are declared locally
   static final double ODOMETRY_FREQUENCY = TunerConstants.kCANBus.isNetworkFD() ? 250.0 : 100.0;
@@ -117,6 +120,8 @@ public Drive(
     // Start odometry thread
     PhoenixOdometryThread.getInstance().start();
 
+    SmartDashboard.putData("Field", loggedField);
+
     // Configure AutoBuilder for PathPlanner
     // AutoBuilder.configure(
     //     this::getPose,
@@ -233,6 +238,8 @@ public void periodic() {
 
     // Update gyro alert
     gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.currentMode != Mode.SIM);
+
+    loggedField.setRobotPose(getPose());
   }
 
   /**