Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Open up pose estimator strategy methods (#2252)

Browse Source
This commit is contained in:
Gold856
2026-01-11 13:58:53 -05:00
committed by GitHub
parent 8e9fe38477
commit a5dc9ec0d6
17 changed files with 2493 additions and 1052 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
photonlib-python-examples/poseest/robot.py
View File

@@ -27,7 +27,7 @@
import drivetrain
import wpilib
import wpimath.geometry
from photonlibpy import PhotonCamera, PhotonPoseEstimator, PoseStrategy
from photonlibpy import PhotonCamera, PhotonPoseEstimator
from robotpy_apriltag import AprilTagField, AprilTagFieldLayout
kRobotToCam = wpimath.geometry.Transform3d(
@@ -44,14 +44,15 @@ class MyRobot(wpilib.TimedRobot):
self.cam = PhotonCamera("YOUR CAMERA NAME")
self.camPoseEst = PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagField.kDefaultField),
PoseStrategy.LOWEST_AMBIGUITY,
self.cam,
kRobotToCam,
)
def robotPeriodic(self) -> None:
camEstPose = self.camPoseEst.update()
if camEstPose:
for result in self.cam.getAllUnreadResults():
camEstPose = self.camPoseEst.estimateCoprocMultiTagPose(result)
if camEstPose is None:
camEstPose = self.camPoseEst.estimateLowestAmbiguityPose(result)
self.swerve.addVisionPoseEstimate(
camEstPose.estimatedPose, camEstPose.timestampSeconds
)