[photon-lib] Python support for PNP_DISTANCE_TRIG_SOLVE (#2015)

This adds support for PNP_DISTANCE_TRIG_SOLVE in the the python
PhotonPoseEstimator, mirroring the implementation in the Java
PhotonPoseEstimator.

Changes:
- Add PoseStrategy.PNP_DISTANCE_TRIG_SOLVE
- Add addHeadingData() and resetHeadingData() to PhotonPoseEstimator
- Fix PhotonCameraSim.process() to set ntReceiveTimestampMicros in the
result
- Minor readability improvements to PhotonPipelineResult
- Minor test improvements to PhotonPoseEstimatorTest
- Add .vscode/settings.json (to make running python tests in VSCode
easier)

Merge checklist:
- [x] Pull Request title is [short, imperative
summary](https://cbea.ms/git-commit/) of proposed changes
- [x] The description documents the _what_ and _why_
- [ ] If this PR changes behavior or adds a feature, user documentation
is updated
- [ ] If this PR touches photon-serde, all messages have been
regenerated and hashes have not changed unexpectedly
- [ ] If this PR touches configuration, this is backwards compatible
with settings back to v2024.3.1
- [ ] If this PR touches pipeline settings or anything related to data
exchange, the frontend typing is updated
- [ ] If this PR addresses a bug, a regression test for it is added

---------

Co-authored-by: Sam948-byte <samf.236@proton.me>

photon-lib/py/test/photonPoseEstimator_test.py

@@ -15,7 +15,10 @@
 ## along with this program.  If not, see <https://www.gnu.org/licenses/>.
 ###############################################################################
 
-from photonlibpy import PhotonPoseEstimator, PoseStrategy
+import wpimath.units
+from photonlibpy import PhotonCamera, PhotonPoseEstimator, PoseStrategy
+from photonlibpy.estimation import TargetModel
+from photonlibpy.simulation import PhotonCameraSim, SimCameraProperties, VisionTargetSim
 from photonlibpy.targeting import (
     PhotonPipelineMetadata,
     PhotonTrackedTarget,
@@ -27,14 +30,17 @@ from robotpy_apriltag import AprilTag, AprilTagFieldLayout
 from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d
 
 
-class PhotonCameraInjector:
+class PhotonCameraInjector(PhotonCamera):
     result: PhotonPipelineResult
 
+    def __init__(self, cameraName="camera"):
+        super().__init__(cameraName)
+
     def getLatestResult(self) -> PhotonPipelineResult:
         return self.result
 
 
-def setupCommon() -> AprilTagFieldLayout:
+def fakeAprilTagFieldLayout() -> AprilTagFieldLayout:
     tagList = []
     tagPoses = (
         Pose3d(3, 3, 3, Rotation3d()),
@@ -53,8 +59,7 @@ def setupCommon() -> AprilTagFieldLayout:
 
 
 def test_lowestAmbiguityStrategy():
-    aprilTags = setupCommon()
-
+    aprilTags = fakeAprilTagFieldLayout()
     cameraOne = PhotonCameraInjector()
     cameraOne.result = PhotonPipelineResult(
         int(11 * 1e6),
@@ -146,6 +151,86 @@ def test_lowestAmbiguityStrategy():
     assertEquals(2, pose.z, 0.01)
 
 
+def test_pnpDistanceTrigSolve():
+    aprilTags = fakeAprilTagFieldLayout()
+    cameraOne = PhotonCameraInjector()
+    latencySecs: wpimath.units.seconds = 1
+    fakeTimestampSecs: wpimath.units.seconds = 9 + latencySecs
+
+    cameraOneSim = PhotonCameraSim(cameraOne, SimCameraProperties.PERFECT_90DEG())
+    simTargets = [
+        VisionTargetSim(tag.pose, TargetModel.AprilTag36h11(), tag.ID)
+        for tag in aprilTags.getTags()
+    ]
+
+    # Compound Rolled + Pitched + Yaw
+    compoundTestTransform = Transform3d(
+        -wpimath.units.inchesToMeters(12),
+        -wpimath.units.inchesToMeters(11),
+        3,
+        Rotation3d(
+            wpimath.units.degreesToRadians(37),
+            wpimath.units.degreesToRadians(6),
+            wpimath.units.degreesToRadians(60),
+        ),
+    )
+
+    estimator = PhotonPoseEstimator(
+        aprilTags,
+        PoseStrategy.PNP_DISTANCE_TRIG_SOLVE,
+        cameraOne,
+        compoundTestTransform,
+    )
+
+    realPose = Pose3d(7.3, 4.42, 0, Rotation3d(0, 0, 2.197))  # Pose to compare with
+    result = cameraOneSim.process(
+        latencySecs, realPose.transformBy(estimator.robotToCamera), simTargets
+    )
+    bestTarget = result.getBestTarget()
+    assert bestTarget is not None
+    assert bestTarget.fiducialId == 0
+    assert result.ntReceiveTimestampMicros > 0
+    # Make test independent of the FPGA time.
+    result.ntReceiveTimestampMicros = fakeTimestampSecs * 1e6
+
+    estimator.addHeadingData(
+        result.getTimestampSeconds(), realPose.rotation().toRotation2d()
+    )
+    estimatedRobotPose = estimator.update(result)
+
+    assert estimatedRobotPose is not None
+    pose = estimatedRobotPose.estimatedPose
+    assertEquals(realPose.x, pose.x, 0.01)
+    assertEquals(realPose.y, pose.y, 0.01)
+    assertEquals(0.0, pose.z, 0.01)
+
+    # Straight on
+    fakeTimestampSecs += 60
+    straightOnTestTransform = Transform3d(0, 0, 3, Rotation3d())
+    estimator.robotToCamera = straightOnTestTransform
+    realPose = Pose3d(4.81, 2.38, 0, Rotation3d(0, 0, 2.818))  # Pose to compare with
+    result = cameraOneSim.process(
+        latencySecs, realPose.transformBy(estimator.robotToCamera), simTargets
+    )
+    bestTarget = result.getBestTarget()
+    assert bestTarget is not None
+    assert bestTarget.fiducialId == 0
+    assert result.ntReceiveTimestampMicros > 0
+    # Make test independent of the FPGA time.
+    result.ntReceiveTimestampMicros = fakeTimestampSecs * 1e6
+
+    estimator.addHeadingData(
+        result.getTimestampSeconds(), realPose.rotation().toRotation2d()
+    )
+    estimatedRobotPose = estimator.update(result)
+
+    assert estimatedRobotPose is not None
+    pose = estimatedRobotPose.estimatedPose
+    assertEquals(realPose.x, pose.x, 0.01)
+    assertEquals(realPose.y, pose.y, 0.01)
+    assertEquals(0.0, pose.z, 0.01)
+
+
 def test_multiTagOnCoprocStrategy():
     cameraOne = PhotonCameraInjector()
     cameraOne.result = PhotonPipelineResult(
@@ -202,8 +287,7 @@ def test_multiTagOnCoprocStrategy():
 
 
 def test_cacheIsInvalidated():
-    aprilTags = setupCommon()
-
+    aprilTags = fakeAprilTagFieldLayout()
     cameraOne = PhotonCameraInjector()
     result = PhotonPipelineResult(
         int(20 * 1e6),