[WIP] Simulation Overhaul (#742)

### What does this do?

- Deprecates previous sim classes
- Has a `CameraProperties` class for describing a camera's basic/calibration info, and performance values for simulation. Calibration values can be loaded from the `config.json` in the settings exported by photonvision.
- `OpenCVHelp` provides convenience functions for using opencv methods with wpilib/photonvision classes, mainly to project 3d points to a camera's 2d image and perform solvePnP with the above camera calibration info.
  - `TargetModel`s describe the 3d shape of a target, both for projecting into the camera's 2d image and use in solvePnP.
- `PhotonCameraSim` uses camera properties to simulate how 3d targets would appear in its view, and has simulated noise, latency, and FPS. For apriltags, the best/alternate camera-to-target transform is also estimated with solvePnP.
  - `VideoSimUtil` has helper functions for drawing apriltags to a simulated raw and processed MJPEG stream for each camera using the projected tag corners.
- `VisionSystemSim` stores `VisionTargetSim`s and `PhotonCameraSim`s, and is periodically updated with the robot's simulated pose. When updating, camera sims are automatically processed and published with their visible targets from their respective poses with proper latency.

### What's still not working?

- Mac Arm builds are broken
- More examples
- Update website/docs

photonlib-java-examples/apriltagExample/simgui-ds.json

@@ -1,11 +1,13 @@
 {
+  "Keyboard 0 Settings": {
+    "window": {
+      "visible": true
+    }
+  },
   "keyboardJoysticks": [
     {
       "axisConfig": [
-        {
-          "decKey": 65,
-          "incKey": 68
-        },
+        {},
         {
           "decKey": 87,
           "incKey": 83
@@ -15,9 +17,14 @@
           "decayRate": 0.0,
           "incKey": 82,
           "keyRate": 0.009999999776482582
+        },
+        {},
+        {
+          "decKey": 65,
+          "incKey": 68
         }
       ],
-      "axisCount": 3,
+      "axisCount": 5,
       "buttonCount": 4,
       "buttonKeys": [
         90,

photonlib-java-examples/apriltagExample/simgui.json

@@ -1,20 +1,46 @@
 {
+  "HALProvider": {
+    "Other Devices": {
+      "AnalogGyro[0]": {
+        "header": {
+          "open": true
+        }
+      }
+    }
+  },
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",
-      "/SmartDashboard/Field": "Field2d"
+      "/LiveWindow/Ungrouped/AnalogGyro[0]": "Gyro",
+      "/LiveWindow/Ungrouped/MotorControllerGroup[1]": "Motor Controller",
+      "/LiveWindow/Ungrouped/MotorControllerGroup[2]": "Motor Controller",
+      "/LiveWindow/Ungrouped/PIDController[1]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[2]": "PIDController",
+      "/SmartDashboard/Field": "Field2d",
+      "/SmartDashboard/VisionSystemSim-Test/Sim Field": "Field2d",
+      "/SmartDashboard/VisionSystemSim-YOUR CAMERA NAME/Sim Field": "Field2d"
     },
     "windows": {
       "/SmartDashboard/Field": {
         "window": {
           "visible": true
         }
+      },
+      "/SmartDashboard/VisionSystemSim-Test/Sim Field": {
+        "window": {
+          "visible": true
+        }
+      },
+      "/SmartDashboard/VisionSystemSim-YOUR CAMERA NAME/Sim Field": {
+        "window": {
+          "visible": true
+        }
       }
     }
   },
   "NetworkTables": {
     "transitory": {
-      "Shuffleboard": {
+      "LiveWindow": {
         "open": true
       },
       "photonvision": {
@@ -24,6 +50,9 @@
         "USB_Camera": {
           "open": true
         },
+        "YOUR CAMERA NAME": {
+          "open": true
+        },
         "open": true,
         "testCamera": {
           "open": true

photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Drivetrain.java

@@ -38,6 +38,7 @@ import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.wpilibj.AnalogGyro;
 import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
@@ -50,6 +51,9 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants.DriveTrainConstants;
 import java.util.Optional;
 import org.photonvision.EstimatedRobotPose;
+import org.photonvision.simulation.PhotonCameraSim;
+import org.photonvision.simulation.SimCameraProperties;
+import org.photonvision.simulation.VisionSystemSim;
 
 /** Represents a differential drive style drivetrain. */
 public class Drivetrain {
@@ -94,7 +98,7 @@ public class Drivetrain {
     private final EncoderSim m_rightEncoderSim = new EncoderSim(m_rightEncoder);
     private final Field2d m_fieldSim = new Field2d();
     private final LinearSystem<N2, N2, N2> m_drivetrainSystem =
-            LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3);
+            LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.8);
     private final DifferentialDrivetrainSim m_drivetrainSimulator =
             new DifferentialDrivetrainSim(
                     m_drivetrainSystem,
@@ -104,6 +108,9 @@ public class Drivetrain {
                     DriveTrainConstants.kWheelRadius,
                     null);
 
+    // Simulated PhotonCamera -- only used in sim!
+    VisionSystemSim m_visionSystemSim;
+
     /**
      * Constructs a differential drive object. Sets the encoder distance per pulse and resets the
      * gyro.
@@ -128,6 +135,15 @@ public class Drivetrain {
         m_rightEncoder.reset();
 
         SmartDashboard.putData("Field", m_fieldSim);
+
+        // Only simulate our PhotonCamera in simulation
+        if (RobotBase.isSimulation()) {
+            m_visionSystemSim = new VisionSystemSim(Constants.VisionConstants.cameraName);
+            var cameraSim =
+                    new PhotonCameraSim(pcw.photonCamera, SimCameraProperties.PI4_LIFECAM_640_480());
+            m_visionSystemSim.addCamera(cameraSim, Constants.VisionConstants.robotToCam);
+            m_visionSystemSim.addVisionTargets(pcw.photonPoseEstimator.getFieldTags());
+        }
     }
 
     /**
@@ -174,6 +190,9 @@ public class Drivetrain {
         m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters());
         m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond());
         m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees());
+
+        // Update results from vision as well
+        m_visionSystemSim.update(m_drivetrainSimulator.getPose());
     }
 
     /** Updates the field-relative position. */

photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java

@@ -37,8 +37,8 @@ import org.photonvision.PhotonPoseEstimator;
 import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 
 public class PhotonCameraWrapper {
-    private PhotonCamera photonCamera;
-    private PhotonPoseEstimator photonPoseEstimator;
+    PhotonCamera photonCamera;
+    PhotonPoseEstimator photonPoseEstimator;
 
     public PhotonCameraWrapper() {
         // Change the name of your camera here to whatever it is in the PhotonVision UI.

photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Robot.java

@@ -42,6 +42,9 @@ public class Robot extends TimedRobot {
     public void robotInit() {
         if (Robot.isSimulation()) {
             NetworkTableInstance instance = NetworkTableInstance.getDefault();
+
+            // We have to have Photon running and set to NT server mode for it to connect
+            // to our computer instead of to a roboRIO.
             instance.stopServer();
             // set the NT server if simulating this code.
             // "localhost" for photon on desktop, or "photonvision.local" / "[ip-address]" for coprocessor
@@ -51,6 +54,10 @@ public class Robot extends TimedRobot {
 
         m_controller = new XboxController(0);
         m_drive = new Drivetrain();
+
+        // Flush NetworkTables every loop. This ensures that robot pose and other values
+        // are sent during every iteration. This only applies to local NT connections!
+        setNetworkTablesFlushEnabled(true);
     }
 
     @Override

photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java

@@ -41,8 +41,8 @@ import edu.wpi.first.wpilibj.simulation.PWMSim;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Robot;
-import org.photonvision.SimVisionSystem;
-import org.photonvision.SimVisionTarget;
+import org.photonvision.simulation.SimVisionSystem;
+import org.photonvision.simulation.SimVisionTarget;
 
 /**
  * Implementation of a simulation of robot physics, sensors, motor controllers Includes a Simulated

photonlib-java-examples/simposeest/src/main/java/frc/robot/Constants.java

@@ -30,7 +30,7 @@ import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
 import edu.wpi.first.math.util.Units;
-import org.photonvision.SimVisionTarget;
+import org.photonvision.simulation.SimVisionTarget;
 
 /**
  * Holding class for all physical constants that must be used throughout the codebase. These values

photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainSim.java

@@ -38,7 +38,7 @@ import edu.wpi.first.wpilibj.simulation.AnalogGyroSim;
 import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
 import edu.wpi.first.wpilibj.simulation.EncoderSim;
 import edu.wpi.first.wpilibj.simulation.PWMSim;
-import org.photonvision.SimVisionSystem;
+import org.photonvision.simulation.SimVisionSystem;
 
 /**
  * Implementation of a simulation of robot physics, sensors, motor controllers Includes a Simulated