[photon-lib] Python support for PNP_DISTANCE_TRIG_SOLVE

.gitignore

@@ -5,7 +5,8 @@ __pycache__/
 
 /.vs
 backend/settings/
-.vscode/
+.vscode/*
+!.vscode/settings.json
 # Docs
 _build
 # Compiled class file

.vscode/settings.json

@@ -0,0 +1,5 @@
+{
+    "python.testing.unittestEnabled": false,
+    "python.testing.pytestEnabled": true,
+    "python.testing.cwd": "photon-lib/py"
+}

photon-lib/py/photonlibpy/photonPoseEstimator.py

@@ -20,8 +20,18 @@
 
 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 @@ def __init__(
         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 @@ def _checkUpdate(self, oldObj, newObj) -> None:
         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 @@ def _update(
             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 @@ def _update(
 
         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]:

photon-lib/py/photonlibpy/simulation/photonCameraSim.py

@@ -420,14 +420,15 @@ def distance(target: VisionTargetSim):
 
         # 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,

photon-lib/py/photonlibpy/targeting/photonPipelineResult.py

@@ -47,13 +47,10 @@ def getTimestampSeconds(self) -> float:
         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

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 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),