From 1dbd2e5990d115af7d34a3cfd124eb6ba2f69560 Mon Sep 17 00:00:00 2001 From: James Ward Date: Fri, 29 Nov 2024 03:12:52 +1100 Subject: [PATCH] [python] Correct time units (#1605) --- .../photonlibpy/networktables/NTTopicSet.py | 9 +-- .../photonlibpy/simulation/photonCameraSim.py | 69 ++++++++++--------- .../simulation/simCameraProperties.py | 2 +- .../photonlibpy/simulation/visionSystemSim.py | 5 +- photon-lib/py/test/visionSystemSim_test.py | 38 +++++++--- 5 files changed, 74 insertions(+), 49 deletions(-) diff --git a/photon-lib/py/photonlibpy/networktables/NTTopicSet.py b/photon-lib/py/photonlibpy/networktables/NTTopicSet.py index 0a0938efe..d503965a9 100644 --- a/photon-lib/py/photonlibpy/networktables/NTTopicSet.py +++ b/photon-lib/py/photonlibpy/networktables/NTTopicSet.py @@ -17,10 +17,11 @@ class NTTopicSet: different for sim vs. real camera """ - def __init__(self, tableName: str, cameraName: str) -> None: - instance = nt.NetworkTableInstance.getDefault() - photonvision_root_table = instance.getTable(tableName) - self.subTable = photonvision_root_table.getSubTable(cameraName) + def __init__( + self, + ntSubTable: nt.NetworkTable, + ) -> None: + self.subTable = ntSubTable def updateEntries(self) -> None: options = nt.PubSubOptions() diff --git a/photon-lib/py/photonlibpy/simulation/photonCameraSim.py b/photon-lib/py/photonlibpy/simulation/photonCameraSim.py index 5d5d9731b..e4e899184 100644 --- a/photon-lib/py/photonlibpy/simulation/photonCameraSim.py +++ b/photon-lib/py/photonlibpy/simulation/photonCameraSim.py @@ -64,7 +64,7 @@ class PhotonCameraSim: False # TODO switch this back to default True when the functionality is enabled ) self.heartbeatCounter: int = 0 - self.nextNtEntryTime = int(wpilib.Timer.getFPGATimestamp() * 1e6) + self.nextNtEntryTime = wpilib.Timer.getFPGATimestamp() self.tagLayout = AprilTagFieldLayout.loadField(AprilTagField.k2024Crescendo) self.cam = camera @@ -95,7 +95,7 @@ class PhotonCameraSim: (self.prop.getResWidth(), self.prop.getResHeight()) ) - self.ts = NTTopicSet("photonvision", self.cam.getName()) + self.ts = NTTopicSet(self.cam._cameraTable) self.ts.updateEntries() # Handle this last explicitly for this function signature because the other constructor is called in the initialiser list @@ -173,20 +173,20 @@ class PhotonCameraSim: def consumeNextEntryTime(self) -> float | None: """Determine if this camera should process a new frame based on performance metrics and the time since the last update. This returns an Optional which is either empty if no update should occur - or a Long of the timestamp in microseconds of when the frame which should be received by NT. If + or a float of the timestamp in seconds of when the frame which should be received by NT. If a timestamp is returned, the last frame update time becomes that timestamp. - :returns: Optional long which is empty while blocked or the NT entry timestamp in microseconds if + :returns: Optional float which is empty while blocked or the NT entry timestamp in seconds if ready """ # check if this camera is ready for another frame update - now = int(wpilib.Timer.getFPGATimestamp() * 1e6) - timestamp = 0 + now = wpilib.Timer.getFPGATimestamp() + timestamp = 0.0 iter = 0 # prepare next latest update while now >= self.nextNtEntryTime: - timestamp = int(self.nextNtEntryTime) - frameTime = int(self.prop.estSecUntilNextFrame() * 1e6) + timestamp = self.nextNtEntryTime + frameTime = self.prop.estSecUntilNextFrame() self.nextNtEntryTime += frameTime # if frame time is very small, avoid blocking @@ -432,7 +432,9 @@ class PhotonCameraSim: ) def submitProcessedFrame( - self, result: PhotonPipelineResult, receiveTimestamp: float | None + self, + result: PhotonPipelineResult, + receiveTimestamp_us: float | None = None, ): """Simulate one processed frame of vision data, putting one result to NT. Image capture timestamp overrides :meth:`.PhotonPipelineResult.getTimestampSeconds` for more @@ -441,44 +443,45 @@ class PhotonCameraSim: :param result: The pipeline result to submit :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 is None: - receiveTimestamp = wpilib.Timer.getFPGATimestamp() * 1e6 - receiveTimestamp = int(receiveTimestamp) + if receiveTimestamp_us is None: + receiveTimestamp_us = wpilib.Timer.getFPGATimestamp() * 1e6 + receiveTimestamp_us = int(receiveTimestamp_us) - self.ts.latencyMillisEntry.set(result.getLatencyMillis(), receiveTimestamp) + self.ts.latencyMillisEntry.set(result.getLatencyMillis(), receiveTimestamp_us) newPacket = PhotonPipelineResult.photonStruct.pack(result) - self.ts.rawBytesEntry.set(newPacket.getData(), receiveTimestamp) + self.ts.rawBytesEntry.set(newPacket.getData(), receiveTimestamp_us) hasTargets = result.hasTargets() - self.ts.hasTargetEntry.set(hasTargets, receiveTimestamp) + self.ts.hasTargetEntry.set(hasTargets, receiveTimestamp_us) if not hasTargets: - self.ts.targetPitchEntry.set(0.0, receiveTimestamp) - self.ts.targetYawEntry.set(0.0, receiveTimestamp) - self.ts.targetAreaEntry.set(0.0, receiveTimestamp) - self.ts.targetPoseEntry.set(Transform3d(), receiveTimestamp) - self.ts.targetSkewEntry.set(0.0, receiveTimestamp) + self.ts.targetPitchEntry.set(0.0, receiveTimestamp_us) + self.ts.targetYawEntry.set(0.0, receiveTimestamp_us) + self.ts.targetAreaEntry.set(0.0, receiveTimestamp_us) + self.ts.targetPoseEntry.set(Transform3d(), receiveTimestamp_us) + self.ts.targetSkewEntry.set(0.0, receiveTimestamp_us) else: bestTarget = result.getBestTarget() assert bestTarget - self.ts.targetPitchEntry.set(bestTarget.getPitch(), receiveTimestamp) - self.ts.targetYawEntry.set(bestTarget.getYaw(), receiveTimestamp) - self.ts.targetAreaEntry.set(bestTarget.getArea(), receiveTimestamp) - self.ts.targetSkewEntry.set(bestTarget.getSkew(), receiveTimestamp) + self.ts.targetPitchEntry.set(bestTarget.getPitch(), receiveTimestamp_us) + self.ts.targetYawEntry.set(bestTarget.getYaw(), receiveTimestamp_us) + self.ts.targetAreaEntry.set(bestTarget.getArea(), receiveTimestamp_us) + self.ts.targetSkewEntry.set(bestTarget.getSkew(), receiveTimestamp_us) self.ts.targetPoseEntry.set( - bestTarget.getBestCameraToTarget(), receiveTimestamp + bestTarget.getBestCameraToTarget(), receiveTimestamp_us ) - intrinsics = self.prop.getIntrinsics() - intrinsicsView = intrinsics.flatten().tolist() - self.ts.cameraIntrinsicsPublisher.set(intrinsicsView, receiveTimestamp) + intrinsics = self.prop.getIntrinsics() + intrinsicsView = intrinsics.flatten().tolist() + self.ts.cameraIntrinsicsPublisher.set(intrinsicsView, receiveTimestamp_us) - distortion = self.prop.getDistCoeffs() - distortionView = distortion.flatten().tolist() - self.ts.cameraDistortionPublisher.set(distortionView, receiveTimestamp) + distortion = self.prop.getDistCoeffs() + distortionView = distortion.flatten().tolist() + self.ts.cameraDistortionPublisher.set(distortionView, receiveTimestamp_us) - self.ts.heartbeatPublisher.set(self.heartbeatCounter, receiveTimestamp) + self.ts.heartbeatPublisher.set(self.heartbeatCounter, receiveTimestamp_us) + self.heartbeatCounter += 1 - self.ts.subTable.getInstance().flush() + self.ts.subTable.getInstance().flush() diff --git a/photon-lib/py/photonlibpy/simulation/simCameraProperties.py b/photon-lib/py/photonlibpy/simulation/simCameraProperties.py index 41de51ccc..cdd675a1c 100644 --- a/photon-lib/py/photonlibpy/simulation/simCameraProperties.py +++ b/photon-lib/py/photonlibpy/simulation/simCameraProperties.py @@ -467,7 +467,7 @@ class SimCameraProperties: def estSecUntilNextFrame(self) -> seconds: """ - :returns: Estimate how long until the next frame should be processed in milliseconds + :returns: Estimate how long until the next frame should be processed in seconds """ # // exceptional processing latency blocks the next frame return self.frameSpeed + max(0.0, self.estLatency() - self.frameSpeed) diff --git a/photon-lib/py/photonlibpy/simulation/visionSystemSim.py b/photon-lib/py/photonlibpy/simulation/visionSystemSim.py index 88a7c4f11..7abd3c204 100644 --- a/photon-lib/py/photonlibpy/simulation/visionSystemSim.py +++ b/photon-lib/py/photonlibpy/simulation/visionSystemSim.py @@ -305,7 +305,7 @@ class VisionSystemSim: timestampNt = optTimestamp latency = camSim.prop.estLatency() # the image capture timestamp in seconds of this result - timestampCapture = timestampNt * 1.0e-6 - latency + timestampCapture = timestampNt - latency # use camera pose from the image capture timestamp lateRobotPose = self.getRobotPose(timestampCapture) @@ -318,7 +318,8 @@ class VisionSystemSim: # process a PhotonPipelineResult with visible targets camResult = camSim.process(latency, lateCameraPose, allTargets) # publish this info to NT at estimated timestamp of receive - camSim.submitProcessedFrame(camResult, timestampNt) + # needs a timestamp in microseconds + camSim.submitProcessedFrame(camResult, timestampNt * 1.0e6) # display debug results for tgt in camResult.getTargets(): trf = tgt.getBestCameraToTarget() diff --git a/photon-lib/py/test/visionSystemSim_test.py b/photon-lib/py/test/visionSystemSim_test.py index 28b5c8f30..40e68fb49 100644 --- a/photon-lib/py/test/visionSystemSim_test.py +++ b/photon-lib/py/test/visionSystemSim_test.py @@ -1,9 +1,8 @@ import math -import ntcore as nt import pytest from photonlibpy.estimation import TargetModel, VisionEstimation -from photonlibpy.photonCamera import PhotonCamera, setVersionCheckEnabled +from photonlibpy.photonCamera import PhotonCamera from photonlibpy.simulation import PhotonCameraSim, VisionSystemSim, VisionTargetSim from robotpy_apriltag import AprilTag, AprilTagFieldLayout from wpimath.geometry import ( @@ -18,12 +17,6 @@ from wpimath.geometry import ( from wpimath.units import feetToMeters, meters -@pytest.fixture(autouse=True) -def setupCommon() -> None: - nt.NetworkTableInstance.getDefault().startServer() - setVersionCheckEnabled(False) - - def test_VisibilityCupidShuffle() -> None: targetPose = Pose3d(Translation3d(15.98, 0.0, 2.0), Rotation3d(0, 0, math.pi)) @@ -32,6 +25,8 @@ def test_VisibilityCupidShuffle() -> None: cameraSim = PhotonCameraSim(camera) visionSysSim.addCamera(cameraSim, Transform3d()) + # Set massive FPS so timing isn't an issue + cameraSim.prop.setFPS(1e6) cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(80.0)) visionSysSim.addVisionTargets( @@ -93,6 +88,8 @@ def test_NotVisibleVert1() -> None: cameraSim = PhotonCameraSim(camera) visionSysSim.addCamera(cameraSim, Transform3d()) + # Set massive FPS so timing isn't an issue + cameraSim.prop.setFPS(1e6) cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(80.0)) visionSysSim.addVisionTargets( @@ -128,6 +125,8 @@ def test_NotVisibleVert2() -> None: cameraSim = PhotonCameraSim(camera) visionSysSim.addCamera(cameraSim, robotToCamera) + # Set massive FPS so timing isn't an issue + cameraSim.prop.setFPS(1e6) cameraSim.prop.setCalibrationFromFOV( 4774, 4774, fovDiag=Rotation2d.fromDegrees(80.0) ) @@ -156,6 +155,8 @@ def test_NotVisibleTargetSize() -> None: cameraSim = PhotonCameraSim(camera) visionSysSim.addCamera(cameraSim, Transform3d()) + # Set massive FPS so timing isn't an issue + cameraSim.prop.setFPS(1e6) cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(80.0)) cameraSim.setMinTargetAreaPixels(20.0) visionSysSim.addVisionTargets( @@ -183,6 +184,8 @@ def test_NotVisibleTooFarLeds() -> None: cameraSim = PhotonCameraSim(camera) visionSysSim.addCamera(cameraSim, Transform3d()) + # Set massive FPS so timing isn't an issue + cameraSim.prop.setFPS(1e6) cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(80.0)) cameraSim.setMinTargetAreaPixels(1.0) cameraSim.setMaxSightRange(10.0) @@ -216,6 +219,9 @@ def test_YawAngles(expected_yaw) -> None: cameraSim = PhotonCameraSim(camera) visionSysSim.addCamera(cameraSim, Transform3d()) + + # Set massive FPS so timing isn't an issue + cameraSim.prop.setFPS(1e6) cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(80.0)) cameraSim.setMinTargetAreaPixels(0.0) visionSysSim.addVisionTargets( @@ -250,6 +256,9 @@ def test_PitchAngles(expected_pitch) -> None: camera = PhotonCamera("camera") cameraSim = PhotonCameraSim(camera) visionSysSim.addCamera(cameraSim, Transform3d()) + + # Set massive FPS so timing isn't an issue + cameraSim.prop.setFPS(1e6) cameraSim.prop.setCalibrationFromFOV( 640, 480, fovDiag=Rotation2d.fromDegrees(120.0) ) @@ -316,8 +325,10 @@ def test_distanceCalc(distParam, pitchParam, heightParam) -> None: ) camera = PhotonCamera("camera") cameraSim = PhotonCameraSim(camera) - visionSysSim.addCamera(cameraSim, Transform3d()) + + # Set massive FPS so timing isn't an issue + cameraSim.prop.setFPS(1e6) cameraSim.prop.setCalibrationFromFOV( 640, 480, fovDiag=Rotation2d.fromDegrees(160.0) ) @@ -354,6 +365,9 @@ def test_MultipleTargets() -> None: camera = PhotonCamera("camera") cameraSim = PhotonCameraSim(camera) visionSysSim.addCamera(cameraSim, Transform3d()) + + # Set massive FPS so timing isn't an issue + cameraSim.prop.setFPS(1e6) cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(80.0)) cameraSim.setMinTargetAreaPixels(20.0) @@ -451,6 +465,9 @@ def test_PoseEstimation() -> None: camera = PhotonCamera("camera") cameraSim = PhotonCameraSim(camera) visionSysSim.addCamera(cameraSim, Transform3d()) + + # Set massive FPS so timing isn't an issue + cameraSim.prop.setFPS(1e6) cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(90.0)) cameraSim.setMinTargetAreaPixels(20.0) @@ -525,6 +542,9 @@ def test_PoseEstimationRotated() -> None: camera = PhotonCamera("camera") cameraSim = PhotonCameraSim(camera) visionSysSim.addCamera(cameraSim, robotToCamera) + + # Set massive FPS so timing isn't an issue + cameraSim.prop.setFPS(1e6) cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(90.0)) cameraSim.setMinTargetAreaPixels(20.0)