Sim Updates for 2023 (#512)

* WIP updating sim stuff for 2023 and pose3d's

* vision system build fixups, but test not yet passing.

* WIP Sim fixups and working on testcases

* Still doesn't work, but closer

* tests pass

* removed C++ sim support

* formatting update

* adjusted target height above ground per review

* Turns out its unused

* missed example removal

photonlib-java-examples/src/main/java/org/photonlib/examples/simaimandrange/sim/DrivetrainSim.java

@@ -25,9 +25,10 @@
 package org.photonlib.examples.simaimandrange.sim;
 
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.system.LinearSystem;
 import edu.wpi.first.math.system.plant.DCMotor;
@@ -73,7 +74,7 @@ public class DrivetrainSim {
     // Configure these to match your PhotonVision Camera,
     // pipeline, and LED setup.
     double camDiagFOV = 170.0; // degrees - assume wide-angle camera
-    double camPitch = Units.radiansToDegrees(Robot.CAMERA_PITCH_RADIANS); // degrees
+    double camPitch = Robot.CAMERA_PITCH_RADIANS; // degrees
     double camHeightOffGround = Robot.CAMERA_HEIGHT_METERS; // meters
     double maxLEDRange = 20; // meters
     int camResolutionWidth = 640; // pixels
@@ -84,9 +85,8 @@ public class DrivetrainSim {
             new SimVisionSystem(
                     "photonvision",
                     camDiagFOV,
-                    camPitch,
-                    new Transform2d(),
-                    camHeightOffGround,
+                    new Transform3d(
+                            new Translation3d(0, 0, camHeightOffGround), new Rotation3d(0, camPitch, 0)),
                     maxLEDRange,
                     camResolutionWidth,
                     camResolutionHeight,
@@ -105,13 +105,15 @@ public class DrivetrainSim {
     double tgtXPos = Units.feetToMeters(54);
     double tgtYPos =
             Units.feetToMeters(27 / 2) - Units.inchesToMeters(43.75) - Units.inchesToMeters(48.0 / 2.0);
-    Pose2d farTargetPose = new Pose2d(new Translation2d(tgtXPos, tgtYPos), new Rotation2d(0.0));
+    Pose3d farTargetPose =
+            new Pose3d(
+                    new Translation3d(tgtXPos, tgtYPos, Robot.TARGET_HEIGHT_METERS),
+                    new Rotation3d(0.0, 0.0, 0.0));
 
     Field2d field = new Field2d();
 
     public DrivetrainSim() {
-        simVision.addSimVisionTarget(
-                new SimVisionTarget(farTargetPose, Robot.TARGET_HEIGHT_METERS, targetWidth, targetHeight));
+        simVision.addSimVisionTarget(new SimVisionTarget(farTargetPose, targetWidth, targetHeight, -1));
         SmartDashboard.putData("Field", field);
     }
 

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

@@ -87,7 +87,6 @@ public class Constants {
     // page 197
     public static final double targetHeight =
             Units.inchesToMeters(98.19) - Units.inchesToMeters(81.19); // meters
-    public static final double targetHeightAboveGround = Units.inchesToMeters(81.19); // meters
 
     // See https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf
     // pages 4 and 5
@@ -103,6 +102,5 @@ public class Constants {
                     new Rotation3d(0.0, 0.0, Units.degreesToRadians(180)));
 
     public static final SimVisionTarget kFarTarget =
-            new SimVisionTarget(
-                    kFarTargetPose.toPose2d(), targetHeightAboveGround, targetWidth, targetHeight);
+            new SimVisionTarget(kFarTargetPose, targetWidth, targetHeight, 42);
 }

photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/sim/DrivetrainSim.java

@@ -89,11 +89,7 @@ public class DrivetrainSim {
             new SimVisionSystem(
                     Constants.kCamName,
                     camDiagFOV,
-                    camPitch,
-                    new Transform2d(
-                            Constants.kCameraToRobot.getTranslation().toTranslation2d(),
-                            Constants.kCameraToRobot.getRotation().toRotation2d()),
-                    camHeightOffGround,
+                    Constants.kCameraToRobot,
                     maxLEDRange,
                     camResolutionWidth,
                     camResolutionHeight,