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

[python] Correct time units (#1605)

Browse Source
This commit is contained in:
James Ward
2024-11-29 03:12:52 +11:00
committed by GitHub
parent 7e9da4133d
commit 1dbd2e5990
5 changed files with 74 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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()

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

@@ -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()

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

@@ -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)

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

@@ -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()

38
photon-lib/py/test/visionSystemSim_test.py
View File

@@ -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)