From 29e24bbac227aaf9fb0a2eb8a55752b278e24b59 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Fri, 1 Aug 2025 11:04:01 -0700 Subject: [PATCH] [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 --- .gitignore | 3 +- .vscode/settings.json | 5 + .../py/photonlibpy/photonPoseEstimator.py | 98 ++++++++++++++++++- .../photonlibpy/simulation/photonCameraSim.py | 11 ++- .../targeting/photonPipelineResult.py | 11 +-- .../py/test/photonPoseEstimator_test.py | 98 +++++++++++++++++-- .../photonvision/PhotonPoseEstimatorTest.java | 89 ++++++++--------- 7 files changed, 250 insertions(+), 65 deletions(-) create mode 100644 .vscode/settings.json diff --git a/.gitignore b/.gitignore index 63d518c8d..dda0d4712 100644 --- a/.gitignore +++ b/.gitignore @@ -5,7 +5,8 @@ __pycache__/ /.vs backend/settings/ -.vscode/ +.vscode/* +!.vscode/settings.json # Docs _build # Compiled class file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 000000000..376e6c941 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,5 @@ +{ + "python.testing.unittestEnabled": false, + "python.testing.pytestEnabled": true, + "python.testing.cwd": "photon-lib/py" +} \ No newline at end of file diff --git a/photon-lib/py/photonlibpy/photonPoseEstimator.py b/photon-lib/py/photonlibpy/photonPoseEstimator.py index a1b0c881e..3c735981d 100644 --- 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]: diff --git a/photon-lib/py/photonlibpy/simulation/photonCameraSim.py b/photon-lib/py/photonlibpy/simulation/photonCameraSim.py index a6f09b232..347bda12c 100644 --- 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, diff --git a/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py b/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py index dc1ff2436..24d5d709c 100644 --- 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 diff --git a/photon-lib/py/test/photonPoseEstimator_test.py b/photon-lib/py/test/photonPoseEstimator_test.py index 5f9787e43..3c0dc67d4 100644 --- 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 . ############################################################################### -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), diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java index d4359a915..c95d452bc 100644 --- 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 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