[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>
2026-06-19 00:41:41 +00:00 · 2025-08-01 11:04:01 -07:00
parent cefaa313df
commit 29e24bbac2
7 changed files with 250 additions and 65 deletions
--- a/.gitignore
+++ b/.gitignore
@@ -5,7 +5,8 @@ __pycache__/

 /.vs
 backend/settings/
-.vscode/
+.vscode/*
+!.vscode/settings.json
 # Docs
 _build
 # Compiled class file
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -0,0 +1,5 @@
+{
+    "python.testing.unittestEnabled": false,
+    "python.testing.pytestEnabled": true,
+    "python.testing.cwd": "photon-lib/py"
+}
--- a/photon-lib/py/photonlibpy/photonPoseEstimator.py
+++ b/photon-lib/py/photonlibpy/photonPoseEstimator.py
@@ -20,8 +20,18 @@ from typing import Optional

 import hal
 import wpilib
+import wpimath.units
 from robotpy_apriltag import AprilTagFieldLayout
-from wpimath.geometry import Pose2d, Pose3d, Transform3d
+from wpimath.geometry import (
+    Pose2d,
+    Pose3d,
+    Rotation2d,
+    Rotation3d,
+    Transform3d,
+    Translation2d,
+    Translation3d,
+)
+from wpimath.interpolation import TimeInterpolatableRotation2dBuffer

 from .estimatedRobotPose import EstimatedRobotPose
 from .photonCamera import PhotonCamera
@@ -60,6 +70,17 @@ class PoseStrategy(enum.Enum):
    This runs on the RoboRIO, and can take a lot of time.
    """

+    PNP_DISTANCE_TRIG_SOLVE = enum.auto()
+    """
+    Use distance data from best visible tag to compute a Pose. This runs on
+    the RoboRIO in order to access the robot's yaw heading, and MUST have
+    addHeadingData called every frame so heading data is up-to-date.
+
+    Produces a Pose2d in estimatedRobotPose (0 for z, roll, pitch).
+
+    See https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98
+    """
+

 class PhotonPoseEstimator:
    instance_count = 1
@@ -98,6 +119,7 @@ class PhotonPoseEstimator:
        self._poseCacheTimestampSeconds = -1.0
        self._lastPose: Optional[Pose3d] = None
        self._referencePose: Optional[Pose3d] = None
+        self._headingBuffer = TimeInterpolatableRotation2dBuffer(1)

        # Usage reporting
        hal.report(
@@ -210,6 +232,32 @@ class PhotonPoseEstimator:
        if oldObj != newObj and oldObj is not None and oldObj is not newObj:
            self._invalidatePoseCache()

+    def addHeadingData(
+        self, timestampSeconds: wpimath.units.seconds, heading: Rotation2d | Rotation3d
+    ) -> None:
+        """
+        Add robot heading data to buffer. Must be called periodically for the **PNP_DISTANCE_TRIG_SOLVE** strategy.
+
+        :param timestampSeconds :timestamp of the robot heading data
+        :param heading: field-relative robot heading at given timestamp
+        """
+        if isinstance(heading, Rotation3d):
+            heading = heading.toRotation2d()
+        self._headingBuffer.addSample(timestampSeconds, heading)
+
+    def resetHeadingData(
+        self, timestampSeconds: wpimath.units.seconds, heading: Rotation2d | Rotation3d
+    ) -> None:
+        """
+        Clears all heading data in the buffer, and adds a new seed. Useful for preventing estimates
+        from utilizing heading data provided prior to a pose or rotation reset.
+
+        :param timestampSeconds: timestamp of the robot heading data
+        :param  heading: field-relative robot heading at given timestamp
+        """
+        self._headingBuffer.clear()
+        self.addHeadingData(timestampSeconds, heading)
+
    def update(
        self, cameraResult: Optional[PhotonPipelineResult] = None
    ) -> Optional[EstimatedRobotPose]:
@@ -263,6 +311,8 @@ class PhotonPoseEstimator:
            estimatedPose = self._lowestAmbiguityStrategy(cameraResult)
        elif strat is PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR:
            estimatedPose = self._multiTagOnCoprocStrategy(cameraResult)
+        elif strat is PoseStrategy.PNP_DISTANCE_TRIG_SOLVE:
+            estimatedPose = self._pnpDistanceTrigSolveStrategy(cameraResult)
        else:
            wpilib.reportError(
                "[PhotonPoseEstimator] Unknown Position Estimation Strategy!", False
@@ -274,6 +324,52 @@ class PhotonPoseEstimator:

        return estimatedPose

+    def _pnpDistanceTrigSolveStrategy(
+        self, result: PhotonPipelineResult
+    ) -> Optional[EstimatedRobotPose]:
+        if (bestTarget := result.getBestTarget()) is None:
+            return None
+
+        if (
+            headingSample := self._headingBuffer.sample(result.getTimestampSeconds())
+        ) is None:
+            return None
+
+        if (tagPose := self._fieldTags.getTagPose(bestTarget.fiducialId)) is None:
+            return None
+
+        camToTagTranslation = (
+            Translation3d(
+                bestTarget.getBestCameraToTarget().translation().norm(),
+                Rotation3d(
+                    0,
+                    -wpimath.units.degreesToRadians(bestTarget.getPitch()),
+                    -wpimath.units.degreesToRadians(bestTarget.getYaw()),
+                ),
+            )
+            .rotateBy(self.robotToCamera.rotation())
+            .toTranslation2d()
+            .rotateBy(headingSample)
+        )
+
+        fieldToCameraTranslation = (
+            tagPose.toPose2d().translation() - camToTagTranslation
+        )
+        camToRobotTranslation: Translation2d = -(
+            self.robotToCamera.translation().toTranslation2d()
+        )
+        camToRobotTranslation = camToRobotTranslation.rotateBy(headingSample)
+        robotPose = Pose2d(
+            fieldToCameraTranslation + camToRobotTranslation, headingSample
+        )
+
+        return EstimatedRobotPose(
+            Pose3d(robotPose),
+            result.getTimestampSeconds(),
+            result.getTargets(),
+            PoseStrategy.PNP_DISTANCE_TRIG_SOLVE,
+        )
+
    def _multiTagOnCoprocStrategy(
        self, result: PhotonPipelineResult
    ) -> Optional[EstimatedRobotPose]:
--- a/photon-lib/py/photonlibpy/simulation/photonCameraSim.py
+++ b/photon-lib/py/photonlibpy/simulation/photonCameraSim.py
@@ -420,14 +420,15 @@ class PhotonCameraSim:

        # put this simulated data to NT
        self.heartbeatCounter += 1
-        now_micros = wpilib.Timer.getFPGATimestamp() * 1e6
+        publishTimestampMicros = wpilib.Timer.getFPGATimestamp() * 1e6
        return PhotonPipelineResult(
+            ntReceiveTimestampMicros=int(publishTimestampMicros + 10),
            metadata=PhotonPipelineMetadata(
-                int(now_micros - latency * 1e6),
-                int(now_micros),
-                self.heartbeatCounter,
+                captureTimestampMicros=int(publishTimestampMicros - latency * 1e6),
+                publishTimestampMicros=int(publishTimestampMicros),
+                sequenceID=self.heartbeatCounter,
                # Pretend like we heard a pong recently
-                int(np.random.uniform(950, 1050)),
+                timeSinceLastPong=int(np.random.uniform(950, 1050)),
            ),
            targets=detectableTgts,
            multitagResult=multiTagResults,
--- a/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py
+++ b/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py
@@ -47,13 +47,10 @@ class PhotonPipelineResult:
        timestamp, coproc timebase))
        """
        # TODO - we don't trust NT4 to correctly latency-compensate ntReceiveTimestampMicros
-        return (
-            self.ntReceiveTimestampMicros
-            - (
-                self.metadata.publishTimestampMicros
-                - self.metadata.captureTimestampMicros
-            )
-        ) / 1e6
+        latency = (
+            self.metadata.publishTimestampMicros - self.metadata.captureTimestampMicros
+        )
+        return (self.ntReceiveTimestampMicros - latency) / 1e6

    def getTargets(self) -> list[PhotonTrackedTarget]:
        return self.targets
--- a/photon-lib/py/test/photonPoseEstimator_test.py
+++ b/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),
--- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
+++ b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
@@ -27,6 +27,7 @@ package org.photonvision;
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertNotNull;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 import static org.junit.jupiter.api.Assertions.fail;

@@ -526,63 +527,63 @@ class PhotonPoseEstimatorTest {
    @Test
    void pnpDistanceTrigSolve() {
        PhotonCameraInjector cameraOne = new PhotonCameraInjector();
-        PhotonCameraSim cameraOneSim =
-                new PhotonCameraSim(cameraOne, SimCameraProperties.PERFECT_90DEG());
-
        List<VisionTargetSim> simTargets =
                aprilTags.getTags().stream()
                        .map((AprilTag x) -> new VisionTargetSim(x.pose, TargetModel.kAprilTag36h11, x.ID))
                        .toList();
+        try (PhotonCameraSim cameraOneSim =
+                new PhotonCameraSim(cameraOne, SimCameraProperties.PERFECT_90DEG())) {
+            /* Compound Rolled + Pitched + Yaw */
+            Transform3d compoundTestTransform =
+                    new Transform3d(
+                            -Units.inchesToMeters(12),
+                            -Units.inchesToMeters(11),
+                            3,
+                            new Rotation3d(
+                                    Units.degreesToRadians(37),
+                                    Units.degreesToRadians(6),
+                                    Units.degreesToRadians(60)));

-        /* Compound Rolled + Pitched + Yaw */
+            var estimator =
+                    new PhotonPoseEstimator(
+                            aprilTags, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform);

-        Transform3d compoundTestTransform =
-                new Transform3d(
-                        -Units.inchesToMeters(12),
-                        -Units.inchesToMeters(11),
-                        3,
-                        new Rotation3d(
-                                Units.degreesToRadians(37), Units.degreesToRadians(6), Units.degreesToRadians(60)));
+            /* this is the real pose of the robot base we test against */
+            var realPose = new Pose3d(7.3, 4.42, 0, new Rotation3d(0, 0, 2.197));
+            PhotonPipelineResult result =
+                    cameraOneSim.process(
+                            1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets);
+            var bestTarget = result.getBestTarget();
+            assertNotNull(bestTarget);
+            assertEquals(0, bestTarget.fiducialId);

-        var estimator =
-                new PhotonPoseEstimator(
-                        aprilTags, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform);
+            estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
+            var estimatedPose = estimator.update(result);

-        /* this is the real pose of the robot base we test against */
-        var realPose = new Pose3d(7.3, 4.42, 0, new Rotation3d(0, 0, 2.197));
-        PhotonPipelineResult result =
-                cameraOneSim.process(
-                        1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets);
+            var pose = estimatedPose.get().estimatedPose;
+            assertEquals(realPose.getX(), pose.getX(), .01);
+            assertEquals(realPose.getY(), pose.getY(), .01);
+            assertEquals(0.0, pose.getZ(), .01);

-        estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
+            /* Straight on */
+            Transform3d straightOnTestTransform = new Transform3d(0, 0, 3, Rotation3d.kZero);

-        var estimatedPose = estimator.update(result);
-        var pose = estimatedPose.get().estimatedPose;
+            estimator.setRobotToCameraTransform(straightOnTestTransform);

-        assertEquals(realPose.getX(), pose.getX(), .01);
-        assertEquals(realPose.getY(), pose.getY(), .01);
-        assertEquals(0.0, pose.getZ(), .01);
+            /* Pose to compare with */
+            realPose = new Pose3d(4.81, 2.38, 0, new Rotation3d(0, 0, 2.818));
+            result =
+                    cameraOneSim.process(
+                            1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets);

-        /* Straight on */
+            estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
+            estimatedPose = estimator.update(result);

-        Transform3d straightOnTestTransform = new Transform3d(0, 0, 3, new Rotation3d(0, 0, 0));
-
-        estimator.setRobotToCameraTransform(straightOnTestTransform);
-
-        /* Pose to compare with */
-        realPose = new Pose3d(4.81, 2.38, 0, new Rotation3d(0, 0, 2.818));
-        result =
-                cameraOneSim.process(
-                        1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets);
-
-        estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
-
-        estimatedPose = estimator.update(result);
-        pose = estimatedPose.get().estimatedPose;
-
-        assertEquals(realPose.getX(), pose.getX(), .01);
-        assertEquals(realPose.getY(), pose.getY(), .01);
-        assertEquals(0.0, pose.getZ(), .01);
+            pose = estimatedPose.get().estimatedPose;
+            assertEquals(realPose.getX(), pose.getX(), .01);
+            assertEquals(realPose.getY(), pose.getY(), .01);
+            assertEquals(0.0, pose.getZ(), .01);
+        }
    }

    @Test