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