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

sphinxify java docs for python code (#1575)

Browse Source
This commit is contained in:
Lucien Morey
2024-11-16 13:01:33 +11:00
committed by GitHub
parent 9bbf49bc6b
commit 04191efc51
10 changed files with 628 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -15,7 +15,22 @@ from .visionTargetSim import VisionTargetSim
class VisionSystemSim:
"""A simulated vision system involving a camera(s) and coprocessor(s) mounted on a mobile robot
running PhotonVision, detecting targets placed on the field. :class:`.VisionTargetSim`s added to
this class will be detected by the :class:`.PhotonCameraSim`s added to this class. This class
should be updated periodically with the robot's current pose in order to publish the simulated
camera target info.
"""
def __init__(self, visionSystemName: str):
"""A simulated vision system involving a camera(s) and coprocessor(s) mounted on a mobile robot
running PhotonVision, detecting targets placed on the field. :class:`.VisionTargetSim`s added to
this class will be detected by the :class:`.PhotonCameraSim`s added to this class. This class
should be updated periodically with the robot's current pose in order to publish the simulated
camera target info.
:param visionSystemName: The specific identifier for this vision system in NetworkTables.
"""
self.dbgField: Field2d = Field2d()
self.bufferLength: seconds = 1.5
@@ -32,12 +47,21 @@ class VisionSystemSim:
wpilib.SmartDashboard.putData(self.tableName + "/Sim Field", self.dbgField)
def getCameraSim(self, name: str) -> PhotonCameraSim | None:
"""Get one of the simulated cameras."""
return self.camSimMap.get(name, None)
def getCameraSims(self) -> list[PhotonCameraSim]:
"""Get all the simulated cameras."""
return [*self.camSimMap.values()]
def addCamera(self, cameraSim: PhotonCameraSim, robotToCamera: Transform3d) -> None:
"""Adds a simulated camera to this vision system with a specified robot-to-camera transformation.
The vision targets registered with this vision system simulation will be observed by the
simulated :class:`.PhotonCamera`.
:param cameraSim: The camera simulation
:param robotToCamera: The transform from the robot pose to the camera pose
"""
name = cameraSim.getCamera().getName()
if name not in self.camSimMap:
self.camSimMap[name] = cameraSim
@@ -49,10 +73,15 @@ class VisionSystemSim:
)
def clearCameras(self) -> None:
"""Remove all simulated cameras from this vision system."""
self.camSimMap.clear()
self.camTrfMap.clear()
def removeCamera(self, cameraSim: PhotonCameraSim) -> bool:
"""Remove a simulated camera from this vision system.
:returns: If the camera was present and removed
"""
name = cameraSim.getCamera().getName()
if name in self.camSimMap:
del self.camSimMap[name]
@@ -65,6 +94,14 @@ class VisionSystemSim:
cameraSim: PhotonCameraSim,
time: seconds = wpilib.Timer.getFPGATimestamp(),
) -> Transform3d | None:
"""Get a simulated camera's position relative to the robot. If the requested camera is invalid, an
empty optional is returned.
:param cameraSim: The specific camera to get the robot-to-camera transform of
:param timeSeconds: Timestamp in seconds of when the transform should be observed
:returns: The transform of this camera, or an empty optional if it is invalid
"""
if cameraSim in self.camTrfMap:
trfBuffer = self.camTrfMap[cameraSim]
sample = trfBuffer.sample(time)
@@ -80,6 +117,13 @@ class VisionSystemSim:
cameraSim: PhotonCameraSim,
time: seconds = wpilib.Timer.getFPGATimestamp(),
) -> Pose3d | None:
"""Get a simulated camera's position on the field. If the requested camera is invalid, an empty
optional is returned.
:param cameraSim: The specific camera to get the field pose of
:returns: The pose of this camera, or an empty optional if it is invalid
"""
robotToCamera = self.getRobotToCamera(cameraSim, time)
if robotToCamera is None:
return None
@@ -93,6 +137,14 @@ class VisionSystemSim:
def adjustCamera(
self, cameraSim: PhotonCameraSim, robotToCamera: Transform3d
) -> bool:
"""Adjust a camera's position relative to the robot. Use this if your camera is on a gimbal or
turret or some other mobile platform.
:param cameraSim: The simulated camera to change the relative position of
:param robotToCamera: New transform from the robot to the camera
:returns: If the cameraSim was valid and transform was adjusted
"""
if cameraSim in self.camTrfMap:
self.camTrfMap[cameraSim].addSample(
wpilib.Timer.getFPGATimestamp(), Pose3d() + robotToCamera
@@ -102,6 +154,7 @@ class VisionSystemSim:
return False
def resetCameraTransforms(self, cameraSim: PhotonCameraSim | None = None) -> None:
"""Reset the transform history for this camera to just the current transform."""
now = wpilib.Timer.getFPGATimestamp()
def resetSingleCamera(self, cameraSim: PhotonCameraSim) -> bool:
@@ -133,12 +186,30 @@ class VisionSystemSim:
def addVisionTargets(
self, targets: list[VisionTargetSim], targetType: str = "targets"
) -> None:
"""Adds targets on the field which your vision system is designed to detect. The {@link
PhotonCamera}s simulated from this system will report the location of the camera relative to
the subset of these targets which are visible from the given camera position.
:param targets: Targets to add to the simulated field
:param type: Type of target (e.g. "cargo").
"""
if targetType not in self.targetSets:
self.targetSets[targetType] = targets
else:
self.targetSets[targetType] += targets
def addAprilTags(self, layout: AprilTagFieldLayout) -> None:
"""Adds targets on the field which your vision system is designed to detect. The {@link
PhotonCamera}s simulated from this system will report the location of the camera relative to
the subset of these targets which are visible from the given camera position.
The AprilTags from this layout will be added as vision targets under the type "apriltag".
The poses added preserve the tag layout's current alliance origin. If the tag layout's alliance
origin is changed, these added tags will have to be cleared and re-added.
:param tagLayout: The field tag layout to get Apriltag poses and IDs from
"""
targets: list[VisionTargetSim] = []
for tag in layout.getTags():
tag_pose = layout.getTagPose(tag.ID)
@@ -172,9 +243,15 @@ class VisionSystemSim:
def getRobotPose(
self, timestamp: seconds = wpilib.Timer.getFPGATimestamp()
) -> Pose3d | None:
"""Get the robot pose in meters saved by the vision system at this timestamp.
:param timestamp: Timestamp of the desired robot pose
"""
return self.robotPoseBuffer.sample(timestamp)
def resetRobotPose(self, robotPose: Pose2d | Pose3d) -> None:
"""Clears all previous robot poses and sets robotPose at current time."""
if type(robotPose) is Pose2d:
robotPose = Pose3d(robotPose)
assert type(robotPose) is Pose3d
@@ -186,16 +263,23 @@ class VisionSystemSim:
return self.dbgField
def update(self, robotPose: Pose2d | Pose3d) -> None:
"""Periodic update. Ensure this is called repeatedly-- camera performance is used to automatically
determine if a new frame should be submitted.
:param robotPoseMeters: The simulated robot pose in meters
"""
if type(robotPose) is Pose2d:
robotPose = Pose3d(robotPose)
assert type(robotPose) is Pose3d
# update vision targets on field
for targetType, targets in self.targetSets.items():
posesToAdd: list[Pose2d] = []
for target in targets:
posesToAdd.append(target.getPose().toPose2d())
self.dbgField.getObject(targetType).setPoses(posesToAdd)
# save "real" robot poses over time
now = wpilib.Timer.getFPGATimestamp()
self.robotPoseBuffer.addSample(now, robotPose)
self.dbgField.setRobotPose(robotPose.toPose2d())
@@ -208,17 +292,22 @@ class VisionSystemSim:
visTgtPoses2d: list[Pose2d] = []
cameraPoses2d: list[Pose2d] = []
processed = False
# process each camera
for camSim in self.camSimMap.values():
# check if this camera is ready to process and get latency
optTimestamp = camSim.consumeNextEntryTime()
if optTimestamp is None:
continue
else:
processed = True
# when this result "was" read by NT
timestampNt = optTimestamp
latency = camSim.prop.estLatency()
# the image capture timestamp in seconds of this result
timestampCapture = timestampNt * 1.0e-6 - latency
# use camera pose from the image capture timestamp
lateRobotPose = self.getRobotPose(timestampCapture)
robotToCamera = self.getRobotToCamera(camSim, timestampCapture)
if lateRobotPose is None or robotToCamera is None:
@@ -226,8 +315,11 @@ class VisionSystemSim:
lateCameraPose = lateRobotPose + robotToCamera
cameraPoses2d.append(lateCameraPose.toPose2d())
# 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)
# display debug results
for tgt in camResult.getTargets():
trf = tgt.getBestCameraToTarget()
if trf == Transform3d():