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

Upgrade to wpilib alpha-6 (#2434)

Browse Source 
Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
Co-authored-by: Ryanforce08 <rradtke1208@gmail.com>
Co-authored-by: PJ Reiniger <pj.reiniger@gmail.com>
Co-authored-by: Jade Turner <spacey-sooty@proton.me>
Co-authored-by: Matt Morley <matthew.morley.ca@gmail.com>
This commit is contained in:
Sam Freund
2026-05-26 21:47:48 -04:00
committed by GitHub
parent d3de87f72b
commit e9006a2803
97 changed files with 732 additions and 1139 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
photon-lib/py/photonlibpy/simulation/photonCameraSim.py
View File

@@ -6,8 +6,9 @@ import cv2 as cv
import numpy as np
import wpilib
from robotpy_apriltag import AprilTagField, AprilTagFieldLayout
from wpimath.geometry import Pose3d, Transform3d
from wpimath import Pose3d, Transform3d
from wpimath.units import meters, seconds
from wpiutil import PixelFormat
from ..estimation import OpenCVHelp, RotTrlTransform3d, TargetModel, VisionEstimation
from ..estimation.cameraTargetRelation import CameraTargetRelation
@@ -66,7 +67,7 @@ class PhotonCameraSim:
# TODO switch this back to default True when the functionality is enabled
self.videoSimProcEnabled: bool = False
self.heartbeatCounter: int = 0
self.nextNtEntryTime = wpilib.Timer.getFPGATimestamp()
self.nextNtEntryTime = wpilib.Timer.getMonotonicTimestamp()
self.tagLayout = tagLayout
self.cam = camera
@@ -76,7 +77,7 @@ class PhotonCameraSim:
# TODO Check fps is right
self.videoSimRaw = cs.CvSource(
self.cam.getName() + "-raw",
cs.VideoMode.PixelFormat.kGray,
PixelFormat.GRAY,
self.prop.getResWidth(),
self.prop.getResHeight(),
1,
@@ -88,7 +89,7 @@ class PhotonCameraSim:
# TODO Check fps is right
self.videoSimProcessed = cs.CvSource(
self.cam.getName() + "-processed",
cs.VideoMode.PixelFormat.kGray,
PixelFormat.GRAY,
self.prop.getResWidth(),
self.prop.getResHeight(),
1,
@@ -182,7 +183,7 @@ class PhotonCameraSim:
ready
"""
# check if this camera is ready for another frame update
now = wpilib.Timer.getFPGATimestamp()
now = wpilib.Timer.getMonotonicTimestamp()
timestamp = 0.0
iter = 0
# prepare next latest update
@@ -434,7 +435,7 @@ class PhotonCameraSim:
# put this simulated data to NT
self.heartbeatCounter += 1
publishTimestampMicros = wpilib.Timer.getFPGATimestamp() * 1e6
publishTimestampMicros = wpilib.Timer.getMonotonicTimestamp() * 1e6
return PhotonPipelineResult(
ntReceiveTimestampMicros=int(publishTimestampMicros + 10),
metadata=PhotonPipelineMetadata(
@@ -461,7 +462,7 @@ class PhotonCameraSim:
:param receiveTimestamp: The (sim) timestamp when this result was read by NT in microseconds. If not passed image capture time is assumed be (current time - latency)
"""
if receiveTimestamp_us is None:
receiveTimestamp_us = wpilib.Timer.getFPGATimestamp() * 1e6
receiveTimestamp_us = wpilib.Timer.getMonotonicTimestamp() * 1e6
receiveTimestamp_us = int(receiveTimestamp_us)
self.ts.latencyMillisEntry.set(result.getLatencyMillis(), receiveTimestamp_us)

2
photon-lib/py/photonlibpy/simulation/simCameraProperties.py
View File

@@ -5,7 +5,7 @@ import typing
import cv2 as cv
import numpy as np
import numpy.typing as npt
from wpimath.geometry import Rotation2d, Rotation3d, Translation3d
from wpimath import Rotation2d, Rotation3d, Translation3d
from wpimath.units import hertz, milliseconds, seconds
from ..estimation import RotTrlTransform3d

28
photon-lib/py/photonlibpy/simulation/visionSystemSim.py
View File

@@ -3,10 +3,9 @@ import typing
import wpilib
from robotpy_apriltag import AprilTagFieldLayout
from wpilib import Field2d
from wpimath.geometry import Pose2d, Pose3d, Transform3d
# TODO(auscompgeek): update import path when RobotPy re-exports are fixed
from wpimath.interpolation._interpolation import TimeInterpolatablePose3dBuffer
from wpimath import Pose2d, Pose3d, TimeInterpolatablePose3dBuffer, Transform3d
from wpimath.units import seconds
from ..estimation import TargetModel
@@ -69,7 +68,7 @@ class VisionSystemSim:
self.bufferLength
)
self.camTrfMap[cameraSim].addSample(
wpilib.Timer.getFPGATimestamp(), Pose3d() + robotToCamera
wpilib.Timer.getMonotonicTimestamp(), Pose3d() + robotToCamera
)
def clearCameras(self) -> None:
@@ -92,7 +91,7 @@ class VisionSystemSim:
def getRobotToCamera(
self,
cameraSim: PhotonCameraSim,
time: seconds = wpilib.Timer.getFPGATimestamp(),
time: seconds | None = None,
) -> Transform3d | None:
"""Get a simulated camera's position relative to the robot. If the requested camera is invalid, an
empty optional is returned.
@@ -102,6 +101,8 @@ class VisionSystemSim:
:returns: The transform of this camera, or an empty optional if it is invalid
"""
if time is None:
time = wpilib.Timer.getMonotonicTimestamp()
if cameraSim in self.camTrfMap:
trfBuffer = self.camTrfMap[cameraSim]
sample = trfBuffer.sample(time)
@@ -115,7 +116,7 @@ class VisionSystemSim:
def getCameraPose(
self,
cameraSim: PhotonCameraSim,
time: seconds = wpilib.Timer.getFPGATimestamp(),
time: seconds | None = None,
) -> Pose3d | None:
"""Get a simulated camera's position on the field. If the requested camera is invalid, an empty
optional is returned.
@@ -124,6 +125,8 @@ class VisionSystemSim:
:returns: The pose of this camera, or an empty optional if it is invalid
"""
if time is None:
time = wpilib.Timer.getMonotonicTimestamp()
robotToCamera = self.getRobotToCamera(cameraSim, time)
if robotToCamera is None:
return None
@@ -147,7 +150,7 @@ class VisionSystemSim:
"""
if cameraSim in self.camTrfMap:
self.camTrfMap[cameraSim].addSample(
wpilib.Timer.getFPGATimestamp(), Pose3d() + robotToCamera
wpilib.Timer.getMonotonicTimestamp(), Pose3d() + robotToCamera
)
return True
else:
@@ -155,7 +158,7 @@ class VisionSystemSim:
def resetCameraTransforms(self, cameraSim: PhotonCameraSim | None = None) -> None:
"""Reset the transform history for this camera to just the current transform."""
now = wpilib.Timer.getFPGATimestamp()
now = wpilib.Timer.getMonotonicTimestamp()
def resetSingleCamera(self, cameraSim: PhotonCameraSim) -> bool:
if cameraSim in self.camTrfMap:
@@ -240,14 +243,13 @@ class VisionSystemSim:
currentTargets.remove(target)
return removedList
def getRobotPose(
self, timestamp: seconds = wpilib.Timer.getFPGATimestamp()
) -> Pose3d | None:
def getRobotPose(self, timestamp: seconds | None = None) -> Pose3d | None:
"""Get the robot pose in meters saved by the vision system at this timestamp.
:param timestamp: Timestamp of the desired robot pose
"""
if timestamp is None:
timestamp = wpilib.Timer.getMonotonicTimestamp()
return self.robotPoseBuffer.sample(timestamp)
def resetRobotPose(self, robotPose: Pose2d | Pose3d) -> None:
@@ -257,7 +259,7 @@ class VisionSystemSim:
assert type(robotPose) is Pose3d
self.robotPoseBuffer.clear()
self.robotPoseBuffer.addSample(wpilib.Timer.getFPGATimestamp(), robotPose)
self.robotPoseBuffer.addSample(wpilib.Timer.getMonotonicTimestamp(), robotPose)
def getDebugField(self) -> Field2d:
return self.dbgField
@@ -280,7 +282,7 @@ class VisionSystemSim:
self.dbgField.getObject(targetType).setPoses(posesToAdd)
# save "real" robot poses over time
now = wpilib.Timer.getFPGATimestamp()
now = wpilib.Timer.getMonotonicTimestamp()
self.robotPoseBuffer.addSample(now, robotPose)
self.dbgField.setRobotPose(robotPose.toPose2d())

2
photon-lib/py/photonlibpy/simulation/visionTargetSim.py
View File

@@ -1,7 +1,7 @@
import math
from typing import overload
from wpimath.geometry import Pose3d, Translation3d
from wpimath import Pose3d, Translation3d
from ..estimation.targetModel import TargetModel