diff --git a/photon-lib/py/photonlibpy/estimatedRobotPose.py b/photon-lib/py/photonlibpy/estimatedRobotPose.py index 39ab44369..e1853c844 100644 --- a/photon-lib/py/photonlibpy/estimatedRobotPose.py +++ b/photon-lib/py/photonlibpy/estimatedRobotPose.py @@ -19,7 +19,7 @@ class EstimatedRobotPose: timestampSeconds: float """The estimated time the frame used to derive the robot pose was taken""" - targetsUsed: [PhotonTrackedTarget] + targetsUsed: list[PhotonTrackedTarget] """A list of the targets used to compute this pose""" strategy: "PoseStrategy" diff --git a/photon-lib/py/photonlibpy/packet.py b/photon-lib/py/photonlibpy/packet.py index c3816d94f..559134091 100644 --- a/photon-lib/py/photonlibpy/packet.py +++ b/photon-lib/py/photonlibpy/packet.py @@ -4,7 +4,7 @@ import wpilib class Packet: - def __init__(self, data: list[int]): + def __init__(self, data: bytes): """ * Constructs an empty packet. * @@ -30,7 +30,7 @@ class Packet: matches the version of photonlib running in the robot code. """ - def _getNextByte(self) -> int: + def _getNextByteAsInt(self) -> int: retVal = 0x00 if not self.outOfBytes: @@ -43,7 +43,7 @@ class Packet: return retVal - def getData(self) -> list[int]: + def getData(self) -> bytes: """ * Returns the packet data. * @@ -51,7 +51,7 @@ class Packet: """ return self.packetData - def setData(self, data: list[int]): + def setData(self, data: bytes): """ * Sets the packet data. * @@ -65,7 +65,7 @@ class Packet: # Read ints in from the data buffer intList = [] for _ in range(numBytes): - intList.append(self._getNextByte()) + intList.append(self._getNextByteAsInt()) # Interpret the bytes as a floating point number value = struct.unpack(unpackFormat, bytes(intList))[0] diff --git a/photon-lib/py/photonlibpy/photonCamera.py b/photon-lib/py/photonlibpy/photonCamera.py index bd468c702..0f0e8bf77 100644 --- a/photon-lib/py/photonlibpy/photonCamera.py +++ b/photon-lib/py/photonlibpy/photonCamera.py @@ -4,7 +4,7 @@ from wpilib import Timer import wpilib from photonlibpy.packet import Packet from photonlibpy.photonPipelineResult import PhotonPipelineResult -from photonlibpy.version import PHOTONVISION_VERSION, PHOTONLIB_VERSION +from photonlibpy.version import PHOTONVISION_VERSION, PHOTONLIB_VERSION # type: ignore[import-untyped] class VisionLEDMode(Enum): @@ -86,7 +86,8 @@ class PhotonCamera: if len(byteList) < 1: return retVal else: - retVal.populateFromPacket(Packet(byteList)) + pkt = Packet(byteList) + retVal.populateFromPacket(pkt) # NT4 allows us to correct the timestamp based on when the message was sent retVal.setTimestampSeconds( timestamp / 1e-6 - retVal.getLatencyMillis() / 1e-3 diff --git a/photon-lib/py/photonlibpy/photonPipelineResult.py b/photon-lib/py/photonlibpy/photonPipelineResult.py index 8c1d200cb..fb045546f 100644 --- a/photon-lib/py/photonlibpy/photonPipelineResult.py +++ b/photon-lib/py/photonlibpy/photonPipelineResult.py @@ -38,3 +38,6 @@ class PhotonPipelineResult: def getTargets(self) -> list[PhotonTrackedTarget]: return self.targets + + def hasTargets(self) -> bool: + return len(self.targets) > 0 diff --git a/photon-lib/py/photonlibpy/photonPoseEstimator.py b/photon-lib/py/photonlibpy/photonPoseEstimator.py index 02fbffabd..a53c10618 100644 --- a/photon-lib/py/photonlibpy/photonPoseEstimator.py +++ b/photon-lib/py/photonlibpy/photonPoseEstimator.py @@ -75,7 +75,7 @@ class PhotonPoseEstimator: self._multiTagFallbackStrategy = PoseStrategy.LOWEST_AMBIGUITY self._reportedErrors: set[int] = set() - self._poseCacheTimestampSeconds = -1 + self._poseCacheTimestampSeconds = -1.0 self._lastPose: Optional[Pose3d] = None self._referencePose: Optional[Pose3d] = None @@ -143,7 +143,7 @@ class PhotonPoseEstimator: self._multiTagFallbackStrategy = strategy @property - def referencePose(self) -> Pose3d: + def referencePose(self) -> Optional[Pose3d]: """Return the reference position that is being used by the estimator. :returns: the referencePose @@ -163,7 +163,7 @@ class PhotonPoseEstimator: self._referencePose = referencePose @property - def lastPose(self) -> Pose3d: + def lastPose(self) -> Optional[Pose3d]: return self._lastPose @lastPose.setter @@ -178,10 +178,10 @@ class PhotonPoseEstimator: self._checkUpdate(self._lastPose, lastPose) self._lastPose = lastPose - def _invalidatePoseCache(self): - self._poseCacheTimestampSeconds = -1 + def _invalidatePoseCache(self) -> None: + self._poseCacheTimestampSeconds = -1.0 - def _checkUpdate(self, oldObj, newObj): + def _checkUpdate(self, oldObj, newObj) -> None: if oldObj != newObj and oldObj is not None and oldObj is not newObj: self._invalidatePoseCache() @@ -204,27 +204,27 @@ class PhotonPoseEstimator: if not cameraResult: if not self._camera: wpilib.reportError("[PhotonPoseEstimator] Missing camera!", False) - return + return None cameraResult = self._camera.getLatestResult() if cameraResult.timestampSec < 0: - return + return None # If the pose cache timestamp was set, and the result is from the same # timestamp, return an # empty result if ( - self._poseCacheTimestampSeconds > 0 + self._poseCacheTimestampSeconds > 0.0 and abs(self._poseCacheTimestampSeconds - cameraResult.timestampSec) < 1e-6 ): - return + return None # Remember the timestamp of the current result used self._poseCacheTimestampSeconds = cameraResult.timestampSec # If no targets seen, trivial case -- return empty result if not cameraResult.targets: - return + return None return self._update(cameraResult, self._primaryStrategy) @@ -239,7 +239,7 @@ class PhotonPoseEstimator: wpilib.reportError( "[PhotonPoseEstimator] Unknown Position Estimation Strategy!", False ) - return + return None if not estimatedPose: self._lastPose = None @@ -280,7 +280,7 @@ class PhotonPoseEstimator: """ lowestAmbiguityTarget = None - lowestAmbiguityScore = 10 + lowestAmbiguityScore = 10.0 for target in result.targets: targetPoseAmbiguity = target.poseAmbiguity @@ -293,7 +293,7 @@ class PhotonPoseEstimator: # Although there are confirmed to be targets, none of them may be fiducial # targets. if not lowestAmbiguityTarget: - return + return None targetFiducialId = lowestAmbiguityTarget.fiducialId @@ -301,7 +301,7 @@ class PhotonPoseEstimator: if not targetPosition: self._reportFiducialPoseError(targetFiducialId) - return + return None return EstimatedRobotPose( targetPosition.transformBy(