photonlibpy 2025.3.2__tar.gz → 2026.0.1a1__tar.gz

{photonlibpy-2025.3.2 → photonlibpy-2026.0.1a1}/PKG-INFO

@@ -1,7 +1,7 @@
 Metadata-Version: 2.1
 Name: photonlibpy
-Version: 2025.3.2
-Summary: Pure-python implementation of PhotonLib for interfacing with PhotonVision on coprocessors. Implemented with PhotonVision version v2025.3.2 .
+Version: 2026.0.1a1
+Summary: Pure-python implementation of PhotonLib for interfacing with PhotonVision on coprocessors. Implemented with PhotonVision version v2026.0.1-alpha-1 .
 Home-page: https://photonvision.org
 Author: Photonvision Development Team
 Classifier: License :: OSI Approved :: MIT License

{photonlibpy-2025.3.2 → photonlibpy-2026.0.1a1}/photonlibpy/photonCamera.py

@@ -18,6 +18,7 @@
 from enum import Enum
 from typing import List
 
+import hal
 import ntcore
 
 # magical import to make serde stuff work
@@ -48,6 +49,8 @@ def setVersionCheckEnabled(enabled: bool):
 
 
 class PhotonCamera:
+    instance_count = 1
+
     def __init__(self, cameraName: str):
         """Constructs a PhotonCamera from the name of the camera.
 
@@ -108,6 +111,13 @@ class PhotonCamera:
         # Start the time sync server
         inst.start()
 
+        # Usage reporting
+        hal.report(
+            hal.tResourceType.kResourceType_PhotonCamera.value,
+            PhotonCamera.instance_count,
+        )
+        PhotonCamera.instance_count += 1
+
     def getAllUnreadResults(self) -> List[PhotonPipelineResult]:
         """
         The list of pipeline results sent by PhotonVision since the last call to getAllUnreadResults().

{photonlibpy-2025.3.2 → photonlibpy-2026.0.1a1}/photonlibpy/photonPoseEstimator.py

@@ -18,9 +18,20 @@
 import enum
 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
@@ -59,8 +70,21 @@ 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
+
     """
     The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a
     given timestamp on the field to produce a single robot in field pose, using the strategy set
@@ -95,8 +119,14 @@ class PhotonPoseEstimator:
         self._poseCacheTimestampSeconds = -1.0
         self._lastPose: Optional[Pose3d] = None
         self._referencePose: Optional[Pose3d] = None
+        self._headingBuffer = TimeInterpolatableRotation2dBuffer(1)
 
-        # TODO: Implement HAL reporting
+        # Usage reporting
+        hal.report(
+            hal.tResourceType.kResourceType_PhotonPoseEstimator.value,
+            PhotonPoseEstimator.instance_count,
+        )
+        PhotonPoseEstimator.instance_count += 1
 
     @property
     def fieldTags(self) -> AprilTagFieldLayout:
@@ -199,9 +229,35 @@ class PhotonPoseEstimator:
         self._poseCacheTimestampSeconds = -1.0
 
     def _checkUpdate(self, oldObj, newObj) -> None:
-        if oldObj != newObj and oldObj is not None and oldObj is not newObj:
+        if oldObj != 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]:
@@ -255,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
@@ -266,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]:

{photonlibpy-2025.3.2 → photonlibpy-2026.0.1a1}/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,

{photonlibpy-2025.3.2 → photonlibpy-2026.0.1a1}/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

photonlibpy-2026.0.1a1/photonlibpy/version.py

@@ -0,0 +1,2 @@
+PHOTONLIB_VERSION="v2026.0.1.alpha.1"
+PHOTONVISION_VERSION="v2026.0.1-alpha-1"

{photonlibpy-2025.3.2 → photonlibpy-2026.0.1a1}/photonlibpy.egg-info/PKG-INFO

@@ -1,7 +1,7 @@
 Metadata-Version: 2.1
 Name: photonlibpy
-Version: 2025.3.2
-Summary: Pure-python implementation of PhotonLib for interfacing with PhotonVision on coprocessors. Implemented with PhotonVision version v2025.3.2 .
+Version: 2026.0.1a1
+Summary: Pure-python implementation of PhotonLib for interfacing with PhotonVision on coprocessors. Implemented with PhotonVision version v2026.0.1-alpha-1 .
 Home-page: https://photonvision.org
 Author: Photonvision Development Team
 Classifier: License :: OSI Approved :: MIT License

{photonlibpy-2025.3.2 → photonlibpy-2026.0.1a1}/photonlibpy.egg-info/SOURCES.txt

@@ -39,4 +39,12 @@ photonlibpy/targeting/multiTargetPNPResult.py
 photonlibpy/targeting/photonPipelineResult.py
 photonlibpy/targeting/photonTrackedTarget.py
 photonlibpy/timesync/__init__.py
-photonlibpy/timesync/timeSyncServer.py
+photonlibpy/timesync/timeSyncServer.py
+test/__init__.py
+test/openCVHelp_test.py
+test/photonCamera_test.py
+test/photonPoseEstimator_test.py
+test/photonlibpy_test.py
+test/simCameraProperties_test.py
+test/testUtil.py
+test/visionSystemSim_test.py

{photonlibpy-2025.3.2 → photonlibpy-2026.0.1a1}/photonlibpy.egg-info/top_level.txt

@@ -1 +1,2 @@
 photonlibpy
+test

photonlibpy-2026.0.1a1/test/openCVHelp_test.py

@@ -0,0 +1,205 @@
+import math
+
+import ntcore as nt
+import pytest
+from photonlibpy.estimation import RotTrlTransform3d, TargetModel
+from photonlibpy.estimation.openCVHelp import OpenCVHelp
+from photonlibpy.photonCamera import setVersionCheckEnabled
+from photonlibpy.simulation import SimCameraProperties, VisionTargetSim
+from wpimath.geometry import Pose3d, Rotation3d, Translation3d
+
+
+@pytest.fixture(autouse=True)
+def setupCommon() -> None:
+    nt.NetworkTableInstance.getDefault().startServer()
+    setVersionCheckEnabled(False)
+
+
+def test_TrlConvert():
+    trl = Translation3d(0.75, -0.4, 0.1)
+    tvec = OpenCVHelp.translationToTVec([trl])
+    result = OpenCVHelp.tVecToTranslation(tvec[0])
+
+    assert result.X() == pytest.approx(trl.X(), 0.005)
+    assert result.Y() == pytest.approx(trl.Y(), 0.005)
+    assert result.Z() == pytest.approx(trl.Z(), 0.005)
+
+
+def test_RotConvert():
+    rot = Rotation3d(0.5, 1, -1)
+    rvec = OpenCVHelp.rotationToRVec(rot)
+    result = OpenCVHelp.rVecToRotation(rvec[0])
+
+    assert result.X() == pytest.approx(rot.X(), 0.25)
+    assert result.Y() == pytest.approx(rot.Y(), 0.25)
+    assert result.Z() == pytest.approx(rot.Z(), 0.25)
+
+
+def test_Projection():
+    prop = SimCameraProperties()
+
+    target = VisionTargetSim(
+        Pose3d(Translation3d(1.0, 0.0, 0.0), Rotation3d(0.0, 0.0, math.pi)),
+        TargetModel.AprilTag16h5(),
+        4774,
+    )
+
+    cameraPose = Pose3d(Translation3d(), Rotation3d())
+
+    camRt = RotTrlTransform3d.makeRelativeTo(cameraPose)
+    imagePoints = OpenCVHelp.projectPoints(
+        prop.getIntrinsics(), prop.getDistCoeffs(), camRt, target.getFieldVertices()
+    )
+
+    # find circulation (counter/clockwise-ness)
+    circulation = 0.0
+    for i in range(0, len(imagePoints)):
+        xDiff = imagePoints[(i + 1) % 4][0][0] - imagePoints[i][0][0]
+        ySum = imagePoints[(i + 1) % 4][0][1] + imagePoints[i][0][1]
+        circulation += xDiff * ySum
+
+    assert circulation > 0, "2d fiducial points aren't counter-clockwise"
+
+    # TODO Uncomment after OpenCVHelp.undistortPoints is implemented
+    # # undo projection distortion
+    # imagePoints = OpenCVHelp.undistortPoints(
+    #     prop.getIntrinsics(), prop.getDistCoeffs(), imagePoints
+    # )
+    # # test projection results after moving camera
+    # avgCenterRot1 = prop.getPixelRot(OpenCVHelp.avgPoint(imagePoints))
+    # cameraPose = cameraPose + Transform3d(Translation3d(), Rotation3d(0.0, 0.25, 0.25))
+    # camRt = RotTrlTransform3d.makeRelativeTo(cameraPose)
+    # imagePoints = OpenCVHelp.projectPoints(
+    #     prop.getIntrinsics(), prop.getDistCoeffs(), camRt, target.getFieldVertices()
+    # )
+    # avgCenterRot2 = prop.getPixelRot(OpenCVHelp.avgPoint(imagePoints))
+
+    # yaw2d = Rotation2d(avgCenterRot2.Z())
+    # pitch2d = Rotation2d(avgCenterRot2.Y())
+    # yawDiff = yaw2d - Rotation2d(avgCenterRot1.Z())
+    # pitchDiff = pitch2d - Rotation2d(avgCenterRot2.Y())
+    # assert yawDiff.radians() < 0.0, "2d points don't follow yaw"
+    # assert pitchDiff.radians() < 0.0, "2d points don't follow pitch"
+
+    # actualRelation = CameraTargetRelation(cameraPose, target.getPose())
+
+    # assert actualRelation.camToTargPitch.degrees() == pytest.approx(
+    #     pitchDiff.degrees()
+    #     * math.cos(yaw2d.radians()),  # adjust for unaccounted perspective distortion
+    #     abs=0.25,
+    # ), "2d pitch doesn't match 3d"
+    # assert actualRelation.camToTargYaw.degrees() == pytest.approx(
+    #     yawDiff.degrees(), abs=0.25
+    # ), "2d yaw doesn't match 3d"
+
+
+def test_SolvePNP_SQUARE():
+    prop = SimCameraProperties()
+
+    # square AprilTag target
+    target = VisionTargetSim(
+        Pose3d(Translation3d(5.0, 0.5, 1.0), Rotation3d(0.0, 0.0, math.pi)),
+        TargetModel.AprilTag16h5(),
+        4774,
+    )
+    cameraPose = Pose3d(Translation3d(), Rotation3d())
+    camRt = RotTrlTransform3d.makeRelativeTo(cameraPose)
+
+    # target relative to camera
+    relTarget = camRt.applyPose(target.getPose())
+
+    # simulate solvePNP estimation
+    targetCorners = OpenCVHelp.projectPoints(
+        prop.getIntrinsics(), prop.getDistCoeffs(), camRt, target.getFieldVertices()
+    )
+
+    pnpSim = OpenCVHelp.solvePNP_Square(
+        prop.getIntrinsics(),
+        prop.getDistCoeffs(),
+        target.getModel().vertices,
+        targetCorners,
+    )
+
+    assert pnpSim is not None
+
+    # check solvePNP estimation accuracy
+    assert relTarget.rotation().X() == pytest.approx(
+        pnpSim.best.rotation().X(), abs=0.25
+    )
+    assert relTarget.rotation().Y() == pytest.approx(
+        pnpSim.best.rotation().Y(), abs=0.25
+    )
+    assert relTarget.rotation().Z() == pytest.approx(
+        pnpSim.best.rotation().Z(), abs=0.25
+    )
+
+    assert relTarget.translation().X() == pytest.approx(
+        pnpSim.best.translation().X(), abs=0.005
+    )
+    assert relTarget.translation().Y() == pytest.approx(
+        pnpSim.best.translation().Y(), abs=0.005
+    )
+    assert relTarget.translation().Z() == pytest.approx(
+        pnpSim.best.translation().Z(), abs=0.005
+    )
+
+
+def test_SolvePNP_SQPNP():
+    prop = SimCameraProperties()
+
+    # (for targets with arbitrary number of non-colinear points > 2)
+    target = VisionTargetSim(
+        Pose3d(Translation3d(5.0, 0.5, 1.0), Rotation3d(0.0, 0.0, math.pi)),
+        TargetModel.createArbitrary(
+            verts=[
+                Translation3d(0.0, 0.0, 0.0),
+                Translation3d(1.0, 0.0, 0.0),
+                Translation3d(0.0, 1.0, 0.0),
+                Translation3d(0.0, 0.0, 1.0),
+                Translation3d(0.125, 0.25, 0.5),
+                Translation3d(0.0, 0.0, -1.0),
+                Translation3d(0.0, -1.0, 0.0),
+                Translation3d(-1.0, 0.0, 0.0),
+            ]
+        ),
+        4774,
+    )
+    cameraPose = Pose3d(Translation3d(), Rotation3d())
+    camRt = RotTrlTransform3d.makeRelativeTo(cameraPose)
+    # target relative to camera
+    relTarget = camRt.applyPose(target.getPose())
+
+    # simulate solvePNP estimation
+    targetCorners = OpenCVHelp.projectPoints(
+        prop.getIntrinsics(), prop.getDistCoeffs(), camRt, target.getFieldVertices()
+    )
+
+    pnpSim = OpenCVHelp.solvePNP_SQPNP(
+        prop.getIntrinsics(),
+        prop.getDistCoeffs(),
+        target.getModel().vertices,
+        targetCorners,
+    )
+
+    assert pnpSim is not None
+
+    # check solvePNP estimation accuracy
+    assert relTarget.rotation().X() == pytest.approx(
+        pnpSim.best.rotation().X(), abs=0.25
+    )
+    assert relTarget.rotation().Y() == pytest.approx(
+        pnpSim.best.rotation().Y(), abs=0.25
+    )
+    assert relTarget.rotation().Z() == pytest.approx(
+        pnpSim.best.rotation().Z(), abs=0.25
+    )
+
+    assert relTarget.translation().X() == pytest.approx(
+        pnpSim.best.translation().X(), abs=0.005
+    )
+    assert relTarget.translation().Y() == pytest.approx(
+        pnpSim.best.translation().Y(), abs=0.005
+    )
+    assert relTarget.translation().Z() == pytest.approx(
+        pnpSim.best.translation().Z(), abs=0.005
+    )

photonlibpy-2026.0.1a1/test/photonCamera_test.py

@@ -0,0 +1,36 @@
+import pytest
+from photonlibpy import Packet
+from photonlibpy.targeting import PhotonPipelineResult
+
+
+@pytest.fixture(autouse=True)
+def setupCommon() -> None:
+    pass
+
+
+def test_Empty() -> None:
+    packet = Packet(b"1")
+    PhotonPipelineResult()
+    packet.setData(bytes(0))
+    PhotonPipelineResult.photonStruct.unpack(packet)
+    # There is no need for an assert as we are checking
+    # if this throws an exception (it should not)
+
+
+@pytest.mark.parametrize(
+    "robotStart, coprocStart, robotRestart, coprocRestart",
+    [
+        [1, 10, 30, 30],
+        [10, 2, 30, 30],
+        [10, 10, 30, 30],
+        # Reboot just the robot
+        [1, 1, 10, 30],
+        # Reboot just the coproc
+        [1, 1, 30, 10],
+    ],
+)
+def test_RestartingRobotandCoproc(
+    robotStart: int, coprocStart: int, robotRestart: int, coprocRestart: int
+):
+    # Python doesn't have a TimeSyncClient so we can't run this yet
+    pass