Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-07-02 02:51: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

2
photon-lib/py/photonlibpy/estimatedRobotPose.py
View File

@@ -17,7 +17,7 @@
from dataclasses import dataclass
from wpimath.geometry import Pose3d
from wpimath import Pose3d
from .targeting.photonTrackedTarget import PhotonTrackedTarget

2
photon-lib/py/photonlibpy/estimation/cameraTargetRelation.py
View File

@@ -1,6 +1,6 @@
import math
from wpimath.geometry import Pose3d, Rotation2d, Transform3d
from wpimath import Pose3d, Rotation2d, Transform3d
from wpimath.units import meters

6
photon-lib/py/photonlibpy/estimation/openCVHelp.py
View File

@@ -4,7 +4,7 @@ from typing import Any
import cv2 as cv
import numpy as np
from wpimath.geometry import Rotation3d, Transform3d, Translation3d
from wpimath import Rotation3d, Transform3d, Translation3d
from ..targeting import PnpResult, TargetCorner
from .rotTrlTransform3d import RotTrlTransform3d
@@ -26,7 +26,7 @@ class OpenCVHelp:
@staticmethod
def rotationNWUtoEDN(rot: Rotation3d) -> Rotation3d:
return -NWU_TO_EDN + (rot + NWU_TO_EDN)
return NWU_TO_EDN.inverse().rotateBy(rot.rotateBy(NWU_TO_EDN))
@staticmethod
def translationToTVec(translations: list[Translation3d]) -> np.ndarray:
@@ -147,7 +147,7 @@ class OpenCVHelp:
in NWU, this would be {0, 0, 1} in EDN.
"""
return -EDN_TO_NWU + (rot + EDN_TO_NWU)
return EDN_TO_NWU.inverse().rotateBy(rot.rotateBy(EDN_TO_NWU))
@staticmethod
def tVecToTranslation(tvecInput: np.ndarray) -> Translation3d:

12
photon-lib/py/photonlibpy/estimation/rotTrlTransform3d.py
View File

@@ -1,6 +1,6 @@
from typing import Self
from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d
from wpimath import Pose3d, Rotation3d, Transform3d, Translation3d
class RotTrlTransform3d:
@@ -22,7 +22,7 @@ class RotTrlTransform3d:
def inverse(self) -> Self:
"""The inverse of this transformation. Applying the inverse will "undo" this transformation."""
invRot = -self.rot
invRot = self.rot.inverse()
invTrl = -(self.trl.rotateBy(invRot))
return type(self)(invRot, invTrl)
@@ -42,7 +42,7 @@ class RotTrlTransform3d:
return trlToApply.rotateBy(self.rot) + self.trl
def applyRotation(self, rotToApply: Rotation3d) -> Rotation3d:
return rotToApply + self.rot
return rotToApply.rotateBy(self.rot)
def applyPose(self, poseToApply: Pose3d) -> Pose3d:
return Pose3d(
@@ -68,7 +68,9 @@ class RotTrlTransform3d:
@classmethod
def makeBetweenPoses(cls, initial: Pose3d, last: Pose3d) -> Self:
return cls(
last.rotation() - initial.rotation(),
last.rotation().relativeTo(initial.rotation()),
last.translation()
- initial.translation().rotateBy(last.rotation() - initial.rotation()),
- initial.translation().rotateBy(
last.rotation().relativeTo(initial.rotation())
),
)

2
photon-lib/py/photonlibpy/estimation/targetModel.py
View File

@@ -1,7 +1,7 @@
import math
from typing import List, Self
from wpimath.geometry import Pose3d, Rotation2d, Rotation3d, Translation3d
from wpimath import Pose3d, Rotation2d, Rotation3d, Translation3d
from wpimath.units import meters
from . import RotTrlTransform3d

2
photon-lib/py/photonlibpy/estimation/visionEstimation.py
View File

@@ -1,6 +1,6 @@
import numpy as np
from robotpy_apriltag import AprilTag, AprilTagFieldLayout
from wpimath.geometry import Pose3d, Transform3d, Translation3d
from wpimath import Pose3d, Transform3d, Translation3d
from ..targeting import PhotonTrackedTarget, PnpResult, TargetCorner
from . import OpenCVHelp, TargetModel

4
photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py
View File

@@ -49,7 +49,9 @@ class PhotonPipelineResultSerde:
ret = Packet()
# metadata is of non-intrinsic type PhotonPipelineMetadata
ret.encodeBytes(PhotonPipelineMetadata.photonStruct.pack(value.metadata).getData())
ret.encodeBytes(
PhotonPipelineMetadata.photonStruct.pack(value.metadata).getData()
)
# targets is a custom VLA!
ret.encodeList(value.targets, PhotonTrackedTarget.photonStruct)

2
photon-lib/py/photonlibpy/networktables/NTTopicSet.py
View File

@@ -1,5 +1,5 @@
import ntcore as nt
from wpimath.geometry import Transform3d
from wpimath import Transform3d
from ..generated.PhotonPipelineResultSerde import PhotonPipelineResultSerde

7
photon-lib/py/photonlibpy/packet.py
View File

@@ -19,7 +19,7 @@ import struct
from typing import Generic, Optional, Protocol, TypeVar
import wpilib
from wpimath.geometry import Quaternion, Rotation3d, Transform3d, Translation3d
from wpimath import Quaternion, Rotation3d, Transform3d, Translation3d
T = TypeVar("T")
@@ -57,17 +57,16 @@ class Packet:
"""
def _getNextByteAsInt(self) -> int:
retVal = 0x00
if not self.outOfBytes:
try:
retVal = 0x00FF & self.packetData[self.readPos]
self.readPos += 1
return retVal
except IndexError:
wpilib.reportError(Packet._NO_MORE_BYTES_MESSAGE, True)
self.outOfBytes = True
return retVal
return 0x00
def getData(self) -> bytes:
"""

15
photon-lib/py/photonlibpy/photonCamera.py
View File

@@ -118,13 +118,16 @@ class PhotonCamera:
)
self._prevHeartbeat = 0
self._prevHeartbeatChangeTime = Timer.getFPGATimestamp()
self._prevHeartbeatChangeTime = Timer.getMonotonicTimestamp()
# Start the time sync server
inst.start()
# Usage reporting
hal.reportUsage("PhotonVision/PhotonCamera", PhotonCamera.instance_count, "")
hal.reportUsage(
"PhotonVision/PhotonCamera", # Not 100% sure if this is correct
str(PhotonCamera.instance_count),
)
PhotonCamera.instance_count += 1
def getAllUnreadResults(self) -> List[PhotonPipelineResult]:
@@ -169,7 +172,7 @@ class PhotonCamera:
self._versionCheck()
now = RobotController.getFPGATime()
now = RobotController.getMonotonicTime()
packetWithTimestamp = self._rawBytesEntry.getAtomic()
byteList = packetWithTimestamp.value
packetWithTimestamp.time
@@ -297,7 +300,7 @@ class PhotonCamera:
"""
curHeartbeat = self._heartbeatEntry.get()
now = Timer.getFPGATimestamp()
now = Timer.getMonotonicTimestamp()
if curHeartbeat != self._prevHeartbeat:
self._prevHeartbeat = curHeartbeat
@@ -311,10 +314,10 @@ class PhotonCamera:
if not _VERSION_CHECK_ENABLED:
return
if (Timer.getFPGATimestamp() - _lastVersionTimeCheck) < 5.0:
if (Timer.getMonotonicTimestamp() - _lastVersionTimeCheck) < 5.0:
return
_lastVersionTimeCheck = Timer.getFPGATimestamp()
_lastVersionTimeCheck = Timer.getMonotonicTimestamp()
# Heartbeat entry is assumed to always be present. If it's not present, we
# assume that a camera with that name was never connected in the first place.

7
photon-lib/py/photonlibpy/photonPoseEstimator.py
View File

@@ -21,16 +21,16 @@ import hal
import wpilib
import wpimath.units
from robotpy_apriltag import AprilTagFieldLayout
from wpimath.geometry import (
from wpimath import (
Pose2d,
Pose3d,
Rotation2d,
Rotation3d,
TimeInterpolatableRotation2dBuffer,
Transform3d,
Translation2d,
Translation3d,
)
from wpimath.interpolation import TimeInterpolatableRotation2dBuffer
from .estimatedRobotPose import EstimatedRobotPose
from .targeting.photonPipelineResult import PhotonPipelineResult
@@ -67,7 +67,8 @@ class PhotonPoseEstimator:
# Usage reporting
hal.reportUsage(
"PhotonVision/PhotonPoseEstimator", PhotonPoseEstimator.instance_count, ""
"PhotonVision/PhotonPoseEstimator",
str(PhotonPoseEstimator.instance_count),
)
PhotonPoseEstimator.instance_count += 1

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

2
photon-lib/py/photonlibpy/targeting/multiTargetPNPResult.py
View File

@@ -1,7 +1,7 @@
from dataclasses import dataclass, field
from typing import TYPE_CHECKING, ClassVar
from wpimath.geometry import Transform3d
from wpimath import Transform3d
if TYPE_CHECKING:
from ..generated.MultiTargetPNPResultSerde import MultiTargetPNPResultSerde

2
photon-lib/py/photonlibpy/targeting/photonTrackedTarget.py
View File

@@ -1,7 +1,7 @@
from dataclasses import dataclass, field
from typing import TYPE_CHECKING, ClassVar
from wpimath.geometry import Transform3d
from wpimath import Transform3d
from ..packet import Packet
from .TargetCorner import TargetCorner

2
photon-lib/py/photonlibpy/timesync/timeSyncServer.py
View File

@@ -53,7 +53,7 @@ class TimeSyncServer:
PORT = 5810
def __init__(self, time_provider: Optional[Callable[[], int]] = None):
self.time_provider = time_provider or Timer.getFPGATimestamp
self.time_provider = time_provider or Timer.getMonotonicTimestamp
self._process: Optional[threading.Thread] = None
self.logger = logging.getLogger("PhotonVision-TimeSyncServer")