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

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

@@ -27,6 +27,12 @@ class OpenCVHelp:
@staticmethod
def translationToTVec(translations: list[Translation3d]) -> np.ndarray:
"""Creates a new :class:`np.array` with these 3d translations. The opencv tvec is a vector with
three elements representing {x, y, z} in the EDN coordinate system.
:param translations: The translations to convert into a np.array
"""
retVal: list[list] = []
for translation in translations:
trl = OpenCVHelp.translationNWUtoEDN(translation)
@@ -38,6 +44,13 @@ class OpenCVHelp:
@staticmethod
def rotationToRVec(rotation: Rotation3d) -> np.ndarray:
"""Creates a new :class:`.np.array` with this 3d rotation. The opencv rvec Mat is a vector with
three elements representing the axis scaled by the angle in the EDN coordinate system. (angle =
norm, and axis = rvec / norm)
:param rotation: The rotation to convert into a np.array
"""
retVal: list[np.ndarray] = []
rot = OpenCVHelp.rotationNWUtoEDN(rotation)
rotVec = rot.getQuaternion().toRotationVector()
@@ -88,6 +101,25 @@ class OpenCVHelp:
def reorderCircular(
elements: list[Any] | np.ndarray, backwards: bool, shiftStart: int
) -> list[Any]:
"""Reorders the list, optionally indexing backwards and wrapping around to the last element after
the first, and shifting all indices in the direction of indexing.
e.g.
({1,2,3}, false, 1) == {2,3,1}
({1,2,3}, true, 0) == {1,3,2}
({1,2,3}, true, 1) == {3,2,1}
:param elements: list elements
:param backwards: If indexing should happen in reverse (0, size-1, size-2, ...)
:param shiftStart: How much the initial index should be shifted (instead of starting at index 0,
start at shiftStart, negated if backwards)
:returns: Reordered list
"""
size = len(elements)
reordered = []
dir = -1 if backwards else 1
@@ -100,18 +132,39 @@ class OpenCVHelp:
@staticmethod
def translationEDNToNWU(trl: Translation3d) -> Translation3d:
"""Convert a rotation delta from EDN to NWU. For example, if you have a rotation X,Y,Z {1, 0, 0}
in EDN, this would be {0, -1, 0} in NWU.
"""
return trl.rotateBy(EDN_TO_NWU)
@staticmethod
def rotationEDNToNWU(rot: Rotation3d) -> Rotation3d:
"""Convert a rotation delta from NWU to EDN. For example, if you have a rotation X,Y,Z {1, 0, 0}
in NWU, this would be {0, 0, 1} in EDN.
"""
return -EDN_TO_NWU + (rot + EDN_TO_NWU)
@staticmethod
def tVecToTranslation(tvecInput: np.ndarray) -> Translation3d:
"""Returns a new 3d translation from this :class:`.Mat`. The opencv tvec is a vector with three
elements representing {x, y, z} in the EDN coordinate system.
:param tvecInput: The tvec to create a Translation3d from
"""
return OpenCVHelp.translationEDNToNWU(Translation3d(tvecInput))
@staticmethod
def rVecToRotation(rvecInput: np.ndarray) -> Rotation3d:
"""Returns a 3d rotation from this :class:`.Mat`. The opencv rvec Mat is a vector with three
elements representing the axis scaled by the angle in the EDN coordinate system. (angle = norm,
and axis = rvec / norm)
:param rvecInput: The rvec to create a Rotation3d from
"""
return OpenCVHelp.rotationEDNToNWU(Rotation3d(rvecInput))
@staticmethod
@@ -121,6 +174,33 @@ class OpenCVHelp:
modelTrls: list[Translation3d],
imagePoints: np.ndarray,
) -> PnpResult | None:
"""Finds the transformation(s) that map the camera's pose to the target's pose. The camera's pose
relative to the target is determined by the supplied 3d points of the target's model and their
associated 2d points imaged by the camera. The supplied model translations must be relative to
the target's pose.
For planar targets, there may be an alternate solution which is plausible given the 2d image
points. This has an associated "ambiguity" which describes the ratio of reprojection error
between the "best" and "alternate" solution.
This method is intended for use with individual AprilTags, and will not work unless 4 points
are provided.
:param cameraMatrix: The camera intrinsics matrix in standard opencv form
:param distCoeffs: The camera distortion matrix in standard opencv form
:param modelTrls: The translations of the object corners. These should have the object pose as
their origin. These must come in a specific, pose-relative order (in NWU):
- Point 0: [0, -squareLength / 2, squareLength / 2]
- Point 1: [0, squareLength / 2, squareLength / 2]
- Point 2: [0, squareLength / 2, -squareLength / 2]
- Point 3: [0, -squareLength / 2, -squareLength / 2]
:param imagePoints: The projection of these 3d object points into the 2d camera image. The order
should match the given object point translations.
:returns: The resulting transformation that maps the camera pose to the target pose and the
ambiguity if an alternate solution is available.
"""
modelTrls = OpenCVHelp.reorderCircular(modelTrls, True, -1)
imagePoints = np.array(OpenCVHelp.reorderCircular(imagePoints, True, -1))
objectMat = np.array(OpenCVHelp.translationToTVec(modelTrls))
@@ -130,6 +210,7 @@ class OpenCVHelp:
best: Transform3d = Transform3d()
for tries in range(2):
# calc rvecs/tvecs and associated reprojection error from image points
retval, rvecs, tvecs, reprojectionError = cv.solvePnPGeneric(
objectMat,
imagePoints,
@@ -138,6 +219,7 @@ class OpenCVHelp:
flags=cv.SOLVEPNP_IPPE_SQUARE,
)
# convert to wpilib coordinates
best = Transform3d(
OpenCVHelp.tVecToTranslation(tvecs[0]),
OpenCVHelp.rVecToRotation(rvecs[0]),
@@ -148,6 +230,7 @@ class OpenCVHelp:
OpenCVHelp.rVecToRotation(rvecs[1]),
)
# check if we got a NaN result
if reprojectionError is not None and not math.isnan(
reprojectionError[0, 0]
):
@@ -158,6 +241,7 @@ class OpenCVHelp:
pt[0, 1] -= 0.001
imagePoints[0] = pt
# solvePnP failed
if reprojectionError is None or math.isnan(reprojectionError[0, 0]):
print("SolvePNP_Square failed!")
return None
@@ -186,6 +270,27 @@ class OpenCVHelp:
modelTrls: list[Translation3d],
imagePoints: np.ndarray,
) -> PnpResult | None:
"""Finds the transformation that maps the camera's pose to the origin of the supplied object. An
"object" is simply a set of known 3d translations that correspond to the given 2d points. If,
for example, the object translations are given relative to close-right corner of the blue
alliance(the default origin), a camera-to-origin transformation is returned. If the
translations are relative to a target's pose, a camera-to-target transformation is returned.
There must be at least 3 points to use this method. This does not return an alternate
solution-- if you are intending to use solvePNP on a single AprilTag, see {@link
#solvePNP_SQUARE} instead.
:param cameraMatrix: The camera intrinsics matrix in standard opencv form
:param distCoeffs: The camera distortion matrix in standard opencv form
:param objectTrls: The translations of the object corners, relative to the field.
:param imagePoints: The projection of these 3d object points into the 2d camera image. The order
should match the given object point translations.
:returns: The resulting transformation that maps the camera pose to the target pose. If the 3d
model points are supplied relative to the origin, this transformation brings the camera to
the origin.
"""
objectMat = np.array(OpenCVHelp.translationToTVec(modelTrls))
retval, rvecs, tvecs, reprojectionError = cv.solvePnPGeneric(

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

@@ -4,24 +4,38 @@ from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d
class RotTrlTransform3d:
"""Represents a transformation that first rotates a pose around the origin, and then translates it."""
def __init__(
self, rot: Rotation3d = Rotation3d(), trl: Translation3d = Translation3d()
):
"""A rotation-translation transformation.
Applying this RotTrlTransform3d to poses will preserve their current origin-to-pose
transform as if the origin was transformed by these components instead.
:param rot: The rotation component
:param trl: The translation component
"""
self.rot = rot
self.trl = trl
def inverse(self) -> Self:
"""The inverse of this transformation. Applying the inverse will "undo" this transformation."""
invRot = -self.rot
invTrl = -(self.trl.rotateBy(invRot))
return type(self)(invRot, invTrl)
def getTransform(self) -> Transform3d:
"""This transformation as a Transform3d (as if of the origin)"""
return Transform3d(self.trl, self.rot)
def getTranslation(self) -> Translation3d:
"""The translation component of this transformation"""
return self.trl
def getRotation(self) -> Rotation3d:
"""The rotation component of this transformation"""
return self.rot
def applyTranslation(self, trlToApply: Translation3d) -> Translation3d:
@@ -44,6 +58,11 @@ class RotTrlTransform3d:
@classmethod
def makeRelativeTo(cls, pose: Pose3d) -> Self:
"""The rotation-translation transformation that makes poses in the world consider this pose as the
new origin, or change the basis to this pose.
:param pose: The new origin
"""
return cls(pose.rotation(), pose.translation()).inverse()
@classmethod

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

@@ -8,14 +8,27 @@ from . import RotTrlTransform3d
class TargetModel:
"""Describes the 3d model of a target."""
def __init__(self):
"""Default constructor for initialising internal class members. DO NOT USE THIS!!! USE THE createPlanar,
createCuboid, createSpheroid or create Arbitrary
"""
self.vertices: List[Translation3d] = []
self.isPlanar = False
self.isSpherical = False
@classmethod
def createPlanar(cls, width: meters, height: meters) -> Self:
"""Creates a rectangular, planar target model given the width and height. The model has four
vertices:
- Point 0: [0, -width/2, -height/2]
- Point 1: [0, width/2, -height/2]
- Point 2: [0, width/2, height/2]
- Point 3: [0, -width/2, height/2]
"""
tm = cls()
tm.isPlanar = True
@@ -30,6 +43,18 @@ class TargetModel:
@classmethod
def createCuboid(cls, length: meters, width: meters, height: meters) -> Self:
"""Creates a cuboid target model given the length, width, height. The model has eight vertices:
- Point 0: [length/2, -width/2, -height/2]
- Point 1: [length/2, width/2, -height/2]
- Point 2: [length/2, width/2, height/2]
- Point 3: [length/2, -width/2, height/2]
- Point 4: [-length/2, -width/2, height/2]
- Point 5: [-length/2, width/2, height/2]
- Point 6: [-length/2, width/2, -height/2]
- Point 7: [-length/2, -width/2, -height/2]
"""
tm = cls()
verts = [
Translation3d(length / 2.0, -width / 2.0, -height / 2.0),
@@ -48,6 +73,20 @@ class TargetModel:
@classmethod
def createSpheroid(cls, diameter: meters) -> Self:
"""Creates a spherical target model which has similar dimensions regardless of its rotation. This
model has four vertices:
- Point 0: [0, -radius, 0]
- Point 1: [0, 0, -radius]
- Point 2: [0, radius, 0]
- Point 3: [0, 0, radius]
*Q: Why these vertices?* A: This target should be oriented to the camera every frame, much
like a sprite/decal, and these vertices represent the ellipse vertices (maxima). These vertices
are used for drawing the image of this sphere, but do not match the corners that will be
published by photonvision.
"""
tm = cls()
tm.isPlanar = False
@@ -63,6 +102,14 @@ class TargetModel:
@classmethod
def createArbitrary(cls, verts: List[Translation3d]) -> Self:
"""Creates a target model from arbitrary 3d vertices. Automatically determines if the given
vertices are planar(x == 0). More than 2 vertices must be given. If this is a planar model, the
vertices should define a non-intersecting contour.
:param vertices: Translations representing the vertices of this target model relative to its
pose.
"""
tm = cls()
tm._common_construction(verts)
@@ -83,6 +130,12 @@ class TargetModel:
self.vertices = verts
def getFieldVertices(self, targetPose: Pose3d) -> List[Translation3d]:
"""This target's vertices offset from its field pose.
Note: If this target is spherical, use {@link #getOrientedPose(Translation3d,
Translation3d)} with this method.
"""
basisChange = RotTrlTransform3d(targetPose.rotation(), targetPose.translation())
retVal = []
@@ -94,6 +147,16 @@ class TargetModel:
@classmethod
def getOrientedPose(cls, tgtTrl: Translation3d, cameraTrl: Translation3d):
"""Returns a Pose3d with the given target translation oriented (with its relative x-axis aligned)
to the camera translation. This is used for spherical targets which should not have their
projection change regardless of their own rotation.
:param tgtTrl: This target's translation
:param cameraTrl: Camera's translation
:returns: This target's pose oriented to the camera
"""
relCam = cameraTrl - tgtTrl
orientToCam = Rotation3d(
0.0,

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

@@ -11,6 +11,7 @@ class VisionEstimation:
def getVisibleLayoutTags(
visTags: list[PhotonTrackedTarget], layout: AprilTagFieldLayout
) -> list[AprilTag]:
"""Get the visible :class:`.AprilTag`s which are in the tag layout using the visible tag IDs."""
retVal: list[AprilTag] = []
for tag in visTags:
id = tag.getFiducialId()
@@ -30,12 +31,31 @@ class VisionEstimation:
layout: AprilTagFieldLayout,
tagModel: TargetModel,
) -> PnpResult | None:
"""Performs solvePNP using 3d-2d point correspondences of visible AprilTags to estimate the
field-to-camera transformation. If only one tag is visible, the result may have an alternate
solution.
**Note:** The returned transformation is from the field origin to the camera pose!
With only one tag: {@link OpenCVHelp#solvePNP_SQUARE}
With multiple tags: {@link OpenCVHelp#solvePNP_SQPNP}
:param cameraMatrix: The camera intrinsics matrix in standard opencv form
:param distCoeffs: The camera distortion matrix in standard opencv form
:param visTags: The visible tags reported by PV. Non-tag targets are automatically excluded.
:param tagLayout: The known tag layout on the field
:returns: The transformation that maps the field origin to the camera pose. Ensure the {@link
PnpResult} are present before utilizing them.
"""
if len(visTags) == 0:
return None
corners: list[TargetCorner] = []
knownTags: list[AprilTag] = []
# ensure these are AprilTags in our layout
for tgt in visTags:
id = tgt.getFiducialId()
maybePose = layout.getTagPose(id)
@@ -53,6 +73,7 @@ class VisionEstimation:
points = OpenCVHelp.cornersToPoints(corners)
# single-tag pnp
if len(knownTags) == 1:
camToTag = OpenCVHelp.solvePNP_Square(
cameraMatrix, distCoeffs, tagModel.getVertices(), points
@@ -74,6 +95,7 @@ class VisionEstimation:
altReprojErr=camToTag.altReprojErr,
)
return result
# multi-tag pnp
else:
objectTrls: list[Translation3d] = []
for tag in knownTags:

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

@@ -9,6 +9,13 @@ PhotonPipelineResult_TYPE_STRING = (
class NTTopicSet:
"""This class is a wrapper around all per-pipeline NT topics that PhotonVision should be publishing
It's split here so the sim and real-camera implementations can share a common implementation of
the naming and registration of the NT content.
However, we do expect that the actual logic which fills out values in the entries will be
different for sim vs. real camera
"""
def __init__(self, tableName: str, cameraName: str) -> None:
instance = nt.NetworkTableInstance.getDefault()

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

@@ -48,6 +48,10 @@ def setVersionCheckEnabled(enabled: bool):
class PhotonCamera:
def __init__(self, cameraName: str):
"""Constructs a PhotonCamera from the name of the camera.
:param cameraName: The nickname of the camera (found in the PhotonVision UI).
"""
instance = ntcore.NetworkTableInstance.getDefault()
self._name = cameraName
self._tableName = "photonvision"
@@ -132,6 +136,14 @@ class PhotonCamera:
return ret
def getLatestResult(self) -> PhotonPipelineResult:
"""Returns the latest pipeline result. This is simply the most recent result Received via NT.
Calling this multiple times will always return the most recent result.
Replaced by :meth:`.getAllUnreadResults` over getLatestResult, as this function can miss
results, or provide duplicate ones!
TODO implement the thing that will take this ones place...
"""
self._versionCheck()
now = RobotController.getFPGATime()
@@ -149,34 +161,85 @@ class PhotonCamera:
return retVal
def getDriverMode(self) -> bool:
"""Returns whether the camera is in driver mode.
:returns: Whether the camera is in driver mode.
"""
return self._driverModeSubscriber.get()
def setDriverMode(self, driverMode: bool) -> None:
"""Toggles driver mode.
:param driverMode: Whether to set driver mode.
"""
self._driverModePublisher.set(driverMode)
def takeInputSnapshot(self) -> None:
"""Request the camera to save a new image file from the input camera stream with overlays. Images
take up space in the filesystem of the PhotonCamera. Calling it frequently will fill up disk
space and eventually cause the system to stop working. Clear out images in
/opt/photonvision/photonvision_config/imgSaves frequently to prevent issues.
"""
self._inputSaveImgEntry.set(self._inputSaveImgEntry.get() + 1)
def takeOutputSnapshot(self) -> None:
"""Request the camera to save a new image file from the output stream with overlays. Images take
up space in the filesystem of the PhotonCamera. Calling it frequently will fill up disk space
and eventually cause the system to stop working. Clear out images in
/opt/photonvision/photonvision_config/imgSaves frequently to prevent issues.
"""
self._outputSaveImgEntry.set(self._outputSaveImgEntry.get() + 1)
def getPipelineIndex(self) -> int:
"""Returns the active pipeline index.
:returns: The active pipeline index.
"""
return self._pipelineIndexState.get(0)
def setPipelineIndex(self, index: int) -> None:
"""Allows the user to select the active pipeline index.
:param index: The active pipeline index.
"""
self._pipelineIndexRequest.set(index)
def getLEDMode(self) -> VisionLEDMode:
"""Returns the current LED mode.
:returns: The current LED mode.
"""
mode = self._ledModeState.get()
return VisionLEDMode(mode)
def setLEDMode(self, led: VisionLEDMode) -> None:
"""Sets the LED mode.
:param led: The mode to set to.
"""
self._ledModeRequest.set(led.value)
def getName(self) -> str:
"""Returns the name of the camera. This will return the same value that was given to the
constructor as cameraName.
:returns: The name of the camera.
"""
return self._name
def isConnected(self) -> bool:
"""Returns whether the camera is connected and actively returning new data. Connection status is
debounced.
:returns: True if the camera is actively sending frame data, false otherwise.
"""
curHeartbeat = self._heartbeatEntry.get()
now = Timer.getFPGATimestamp()
@@ -197,6 +260,8 @@ class PhotonCamera:
_lastVersionTimeCheck = Timer.getFPGATimestamp()
# 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.
if not self._heartbeatEntry.exists():
cameraNames = (
self._cameraTable.getInstance().getTable(self._tableName).getSubTables()
@@ -222,6 +287,7 @@ class PhotonCamera:
True,
)
# Check for connection status. Warn if disconnected.
elif not self.isConnected():
wpilib.reportWarning(
f"PhotonVision coprocessor at path {self._path} is not sending new data.",
@@ -229,8 +295,9 @@ class PhotonCamera:
)
versionString = self.versionEntry.get(defaultValue="")
localUUID = PhotonPipelineResult.photonStruct.MESSAGE_VERSION
# Check mdef UUID
localUUID = PhotonPipelineResult.photonStruct.MESSAGE_VERSION
remoteUUID = str(self._rawBytesEntry.getTopic().getProperty("message_uuid"))
if not remoteUUID:

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

@@ -26,6 +26,10 @@ from .visionTargetSim import VisionTargetSim
class PhotonCameraSim:
"""A handle for simulating :class:`.PhotonCamera` values. Processing simulated targets through this
class will change the associated PhotonCamera's results.
"""
kDefaultMinAreaPx: float = 100
def __init__(
@@ -35,6 +39,21 @@ class PhotonCameraSim:
minTargetAreaPercent: float | None = None,
maxSightRange: meters | None = None,
):
"""Constructs a handle for simulating :class:`.PhotonCamera` values. Processing simulated targets
through this class will change the associated PhotonCamera's results.
By default, this constructor's camera has a 90 deg FOV with no simulated lag if props!
By default, the minimum target area is 100 pixels and there is no maximum sight range unless both params are passed to override.
:param camera: The camera to be simulated
:param prop: Properties of this camera such as FOV and FPS
:param minTargetAreaPercent: The minimum percentage(0 - 100) a detected target must take up of
the camera's image to be processed. Match this with your contour filtering settings in the
PhotonVision GUI.
:param maxSightRangeMeters: Maximum distance at which the target is illuminated to your camera.
Note that minimum target area of the image is separate from this.
"""
self.minTargetAreaPercent: float = 0.0
self.maxSightRange: float = 1.0e99
@@ -103,22 +122,39 @@ class PhotonCameraSim:
return self.videoSimFrameRaw
def canSeeTargetPose(self, camPose: Pose3d, target: VisionTargetSim) -> bool:
"""Determines if this target's pose should be visible to the camera without considering its
projected image points. Does not account for image area.
:param camPose: Camera's 3d pose
:param target: Vision target containing pose and shape
:returns: If this vision target can be seen before image projection.
"""
rel = CameraTargetRelation(camPose, target.getPose())
return (
(
# target translation is outside of camera's FOV
abs(rel.camToTargYaw.degrees())
< self.prop.getHorizFOV().degrees() / 2.0
and abs(rel.camToTargPitch.degrees())
< self.prop.getVertFOV().degrees() / 2.0
)
and (
# camera is behind planar target and it should be occluded
not target.getModel().getIsPlanar()
or abs(rel.targtoCamAngle.degrees()) < 90
)
# target is too far
and rel.camToTarg.translation().norm() <= self.maxSightRange
)
def canSeeCorner(self, points: np.ndarray) -> bool:
"""Determines if all target points are inside the camera's image.
:param points: The target's 2d image points
"""
assert points.shape[1] == 1
assert points.shape[2] == 2
for pt in points:
@@ -130,51 +166,88 @@ class PhotonCameraSim:
or y < 0
or y > self.prop.getResHeight()
):
return False
return False # point is outside of resolution
return True
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
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
ready
"""
# check if this camera is ready for another frame update
now = int(wpilib.Timer.getFPGATimestamp() * 1e6)
timestamp = 0
iter = 0
# prepare next latest update
while now >= self.nextNtEntryTime:
timestamp = int(self.nextNtEntryTime)
frameTime = int(self.prop.estSecUntilNextFrame() * 1e6)
self.nextNtEntryTime += frameTime
# if frame time is very small, avoid blocking
iter += 1
if iter > 50:
timestamp = now
self.nextNtEntryTime = now + frameTime
break
# return the timestamp of the latest update
if timestamp != 0:
return timestamp
# or this camera isn't ready to process yet
return None
def setMinTargetAreaPercent(self, areaPercent: float) -> None:
"""The minimum percentage(0 - 100) a detected target must take up of the camera's image to be
processed.
"""
self.minTargetAreaPercent = areaPercent
def setMinTargetAreaPixels(self, areaPx: float) -> None:
"""The minimum number of pixels a detected target must take up in the camera's image to be
processed.
"""
self.minTargetAreaPercent = areaPx / self.prop.getResArea() * 100.0
def setMaxSightRange(self, range: meters) -> None:
"""Maximum distance at which the target is illuminated to your camera. Note that minimum target
area of the image is separate from this.
"""
self.maxSightRange = range
def enableRawStream(self, enabled: bool) -> None:
"""Sets whether the raw video stream simulation is enabled.
Note: This may increase loop times.
"""
self.videoSimRawEnabled = enabled
raise Exception("Raw stream not implemented")
def enableDrawWireframe(self, enabled: bool) -> None:
"""Sets whether a wireframe of the field is drawn to the raw video stream.
Note: This will dramatically increase loop times.
"""
self.videoSimWireframeEnabled = enabled
raise Exception("Wireframe not implemented")
def setWireframeResolution(self, resolution: float) -> None:
"""Sets the resolution of the drawn wireframe if enabled. Drawn line segments will be subdivided
into smaller segments based on a threshold set by the resolution.
:param resolution: Resolution as a fraction(0 - 1) of the video frame's diagonal length in
pixels
"""
self.videoSimWireframeResolution = resolution
def enableProcessedStream(self, enabled: bool) -> None:
"""Sets whether the processed video stream simulation is enabled."""
self.videoSimProcEnabled = enabled
raise Exception("Processed stream not implemented")
@@ -187,25 +260,32 @@ class PhotonCameraSim:
targets.sort(key=distance, reverse=True)
# all targets visible before noise
visibleTgts: list[typing.Tuple[VisionTargetSim, np.ndarray]] = []
# all targets actually detected by camera (after noise)
detectableTgts: list[PhotonTrackedTarget] = []
# basis change from world coordinates to camera coordinates
camRt = RotTrlTransform3d.makeRelativeTo(cameraPose)
for tgt in targets:
# pose isn't visible, skip to next
if not self.canSeeTargetPose(cameraPose, tgt):
continue
# find target's 3d corner points
fieldCorners = tgt.getFieldVertices()
isSpherical = tgt.getModel().getIsSpherical()
if isSpherical:
if isSpherical: # target is spherical
model = tgt.getModel()
# orient the model to the camera (like a sprite/decal) so it appears similar regardless of view
fieldCorners = model.getFieldVertices(
TargetModel.getOrientedPose(
tgt.getPose().translation(), cameraPose.translation()
)
)
# project 3d target points into 2d image points
imagePoints = OpenCVHelp.projectPoints(
self.prop.getIntrinsics(),
self.prop.getDistCoeffs(),
@@ -213,9 +293,11 @@ class PhotonCameraSim:
fieldCorners,
)
# spherical targets need a rotated rectangle of their midpoints for visualization
if isSpherical:
center = OpenCVHelp.avgPoint(imagePoints)
l: int = 0
# reference point (left side midpoint)
for i in range(4):
if imagePoints[i, 0, 0] < imagePoints[l, 0, 0].x:
l = i
@@ -239,6 +321,7 @@ class PhotonCameraSim:
for i in range(4):
if i != t and i != l and i != b:
r = i
# create RotatedRect from midpoints
rect = cv.RotatedRect(
(center[0, 0], center[0, 1]),
(
@@ -247,16 +330,23 @@ class PhotonCameraSim:
),
-angles[r],
)
# set target corners to rect corners
imagePoints = np.array([[p[0], p[1], p[2]] for p in rect.points()])
# save visible targets for raw video stream simulation
visibleTgts.append((tgt, imagePoints))
# estimate pixel noise
noisyTargetCorners = self.prop.estPixelNoise(imagePoints)
# find the minimum area rectangle of target corners
minAreaRect = OpenCVHelp.getMinAreaRect(noisyTargetCorners)
minAreaRectPts = minAreaRect.points()
# find the (naive) 2d yaw/pitch
centerPt = minAreaRect.center
centerRot = self.prop.getPixelRot(centerPt)
# find contour area
areaPercent = self.prop.getContourAreaPercent(noisyTargetCorners)
# projected target can't be detected, skip to next
if (
not self.canSeeCorner(noisyTargetCorners)
or not areaPercent >= self.minTargetAreaPercent
@@ -265,6 +355,7 @@ class PhotonCameraSim:
pnpSim: PnpResult | None = None
if tgt.fiducialId >= 0 and len(tgt.getFieldVertices()) == 4:
# single AprilTag solvePNP
pnpSim = OpenCVHelp.solvePNP_Square(
self.prop.getIntrinsics(),
self.prop.getDistCoeffs(),
@@ -295,6 +386,7 @@ class PhotonCameraSim:
# Video streams disabled for now
if self.videoSimRawEnabled:
# TODO Video streams disabled for now port and uncomment when implemented
# VideoSimUtil::UpdateVideoProp(videoSimRaw, prop);
# cv::Size videoFrameSize{prop.GetResWidth(), prop.GetResHeight()};
# cv::Mat blankFrame = cv::Mat::zeros(videoFrameSize, CV_8UC1);
@@ -312,6 +404,7 @@ class PhotonCameraSim:
if len(visibleLayoutTags) > 1:
usedIds = [tag.ID for tag in visibleLayoutTags]
# sort target order sorts in ascending order by default
usedIds.sort()
pnpResult = VisionEstimation.estimateCamPosePNP(
self.prop.getIntrinsics(),
@@ -323,6 +416,7 @@ class PhotonCameraSim:
if pnpResult is not None:
multiTagResults = MultiTargetPNPResult(pnpResult, usedIds)
# put this simulated data to NT
self.heartbeatCounter += 1
return PhotonPipelineResult(
metadata=PhotonPipelineMetadata(
@@ -335,6 +429,13 @@ class PhotonCameraSim:
def submitProcessedFrame(
self, result: PhotonPipelineResult, receiveTimestamp: float | None
):
"""Simulate one processed frame of vision data, putting one result to NT. Image capture timestamp
overrides :meth:`.PhotonPipelineResult.getTimestampSeconds` for more
precise latency simulation.
: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)

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

@@ -11,7 +11,22 @@ from ..estimation import RotTrlTransform3d
class SimCameraProperties:
"""Calibration and performance values for this camera.
The resolution will affect the accuracy of projected(3d to 2d) target corners and similarly
the severity of image noise on estimation(2d to 3d).
The camera intrinsics and distortion coefficients describe the results of calibration, and how
to map between 3d field points and 2d image points.
The performance values (framerate/exposure time, latency) determine how often results should
be updated and with how much latency in simulation. High exposure time causes motion blur which
can inhibit target detection while moving. Note that latency estimation does not account for
network latency and the latency reported will always be perfect.
"""
def __init__(self):
"""Default constructor which is the same as {@link #PERFECT_90DEG}"""
self.resWidth: int = -1
self.resHeight: int = -1
self.camIntrinsics: np.ndarray = np.zeros((3, 3)) # [3,3]
@@ -38,14 +53,18 @@ class SimCameraProperties:
fovWidth = Rotation2d(math.atan((diagRatio * (width / resDiag)) * 2))
fovHeight = Rotation2d(math.atan(diagRatio * (height / resDiag)) * 2)
# assume no distortion
newDistCoeffs = np.zeros((8, 1))
# assume centered principal point (pixels)
cx = width / 2.0 - 0.5
cy = height / 2.0 - 0.5
# use given fov to determine focal point (pixels)
fx = cx / math.tan(fovWidth.radians() / 2.0)
fy = cy / math.tan(fovHeight.radians() / 2.0)
# create camera intrinsics matrix
newCamIntrinsics = np.array([[fx, 0.0, cx], [0.0, fy, cy], [0.0, 0.0, 1.0]])
self.setCalibrationFromIntrinsics(
@@ -65,6 +84,7 @@ class SimCameraProperties:
self.camIntrinsics = newCamIntrinsics
self.distCoeffs = newDistCoeffs
# left, right, up, and down view planes
p = [
Translation3d(
1.0,
@@ -110,16 +130,33 @@ class SimCameraProperties:
self.errorStdDevPx = newErrorStdDevPx
def setFPS(self, fps: hertz):
"""
:param fps: The average frames per second the camera should process at. :strong:`Exposure time limits
FPS if set!`
"""
self.frameSpeed = max(1.0 / fps, self.exposureTime)
def setExposureTime(self, newExposureTime: seconds):
"""
:param newExposureTime: The amount of time the "shutter" is open for one frame. Affects motion
blur. **Frame speed(from FPS) is limited to this!**
"""
self.exposureTime = newExposureTime
self.frameSpeed = max(self.frameSpeed, self.exposureTime)
def setAvgLatency(self, newAvgLatency: seconds):
"""
:param newAvgLatency: The average latency (from image capture to data published) in milliseconds
a frame should have
"""
self.vgLatency = newAvgLatency
def setLatencyStdDev(self, newLatencyStdDev: seconds):
"""
:param latencyStdDevMs: The standard deviation in milliseconds of the latency
"""
self.latencyStdDev = newLatencyStdDev
def getResWidth(self) -> int:
@@ -156,21 +193,43 @@ class SimCameraProperties:
return self.latencyStdDev
def getContourAreaPercent(self, points: np.ndarray) -> float:
"""The percentage(0 - 100) of this camera's resolution the contour takes up in pixels of the
image.
:param points: Points of the contour
"""
return cv.contourArea(cv.convexHull(points)) / self.getResArea() * 100.0
def getPixelYaw(self, pixelX: float) -> Rotation2d:
"""The yaw from the principal point of this camera to the pixel x value. Positive values left."""
fx = self.camIntrinsics[0, 0]
# account for principal point not being centered
cx = self.camIntrinsics[0, 2]
xOffset = cx - pixelX
return Rotation2d(fx, xOffset)
def getPixelPitch(self, pixelY: float) -> Rotation2d:
"""The pitch from the principal point of this camera to the pixel y value. Pitch is positive down.
Note that this angle is naively computed and may be incorrect. See {@link
#getCorrectedPixelRot(Point)}.
"""
fy = self.camIntrinsics[1, 1]
# account for principal point not being centered
cy = self.camIntrinsics[1, 2]
yOffset = cy - pixelY
return Rotation2d(fy, -yOffset)
def getPixelRot(self, point: cv.typing.Point2f) -> Rotation3d:
"""Finds the yaw and pitch to the given image point. Yaw is positive left, and pitch is positive
down.
Note that pitch is naively computed and may be incorrect. See {@link
#getCorrectedPixelRot(Point)}.
"""
return Rotation3d(
0.0,
self.getPixelPitch(point[1]).radians(),
@@ -178,6 +237,27 @@ class SimCameraProperties:
)
def getCorrectedPixelRot(self, point: cv.typing.Point2f) -> Rotation3d:
"""Gives the yaw and pitch of the line intersecting the camera lens and the given pixel
coordinates on the sensor. Yaw is positive left, and pitch positive down.
The pitch traditionally calculated from pixel offsets do not correctly account for non-zero
values of yaw because of perspective distortion (not to be confused with lens distortion)-- for
example, the pitch angle is naively calculated as:
<pre>pitch = arctan(pixel y offset / focal length y)</pre>
However, using focal length as a side of the associated right triangle is not correct when the
pixel x value is not 0, because the distance from this pixel (projected on the x-axis) to the
camera lens increases. Projecting a line back out of the camera with these naive angles will
not intersect the 3d point that was originally projected into this 2d pixel. Instead, this
length should be:
<pre>focal length y ⟶ (focal length y / cos(arctan(pixel x offset / focal length x)))</pre>
:returns: Rotation3d with yaw and pitch of the line projected out of the camera from the given
pixel (roll is zero).
"""
fx = self.camIntrinsics[0, 0]
cx = self.camIntrinsics[0, 2]
xOffset = cx - point[0]
@@ -191,11 +271,13 @@ class SimCameraProperties:
return Rotation3d(0.0, pitch.radians(), yaw.radians())
def getHorizFOV(self) -> Rotation2d:
# sum of FOV left and right principal point
left = self.getPixelYaw(0)
right = self.getPixelYaw(self.resWidth)
return left - right
def getVertFOV(self) -> Rotation2d:
# sum of FOV above and below principal point
above = self.getPixelPitch(0)
below = self.getPixelPitch(self.resHeight)
return below - above
@@ -208,9 +290,34 @@ class SimCameraProperties:
def getVisibleLine(
self, camRt: RotTrlTransform3d, a: Translation3d, b: Translation3d
) -> typing.Tuple[float | None, float | None]:
"""Determines where the line segment defined by the two given translations intersects the camera's
frustum/field-of-vision, if at all.
The line is parametrized so any of its points <code>p = t * (b - a) + a</code>. This method
returns these values of t, minimum first, defining the region of the line segment which is
visible in the frustum. If both ends of the line segment are visible, this simply returns {0,
1}. If, for example, point b is visible while a is not, and half of the line segment is inside
the camera frustum, {0.5, 1} would be returned.
:param camRt: The change in basis from world coordinates to camera coordinates. See {@link
RotTrlTransform3d#makeRelativeTo(Pose3d)}.
:param a: The initial translation of the line
:param b: The final translation of the line
:returns: A Pair of Doubles. The values may be null:
- {Double, Double} : Two parametrized values(t), minimum first, representing which
segment of the line is visible in the camera frustum.
- {Double, null} : One value(t) representing a single intersection point. For example,
the line only intersects the intersection of two adjacent viewplanes.
- {null, null} : No values. The line segment is not visible in the camera frustum.
"""
# translations relative to the camera
relA = camRt.applyTranslation(a)
relB = camRt.applyTranslation(b)
# check if both ends are behind camera
if relA.X() <= 0.0 and relB.X() <= 0.0:
return (None, None)
@@ -221,6 +328,7 @@ class SimCameraProperties:
aVisible = True
bVisible = True
# check if the ends of the line segment are visible
for normal in self.viewplanes:
aVisibility = av.dot(normal)
if aVisibility < 0:
@@ -229,38 +337,54 @@ class SimCameraProperties:
bVisibility = bv.dot(normal)
if bVisibility < 0:
bVisible = False
# both ends are outside at least one of the same viewplane
if aVisibility <= 0 and bVisibility <= 0:
return (None, None)
# both ends are inside frustum
if aVisible and bVisible:
return (0.0, 1.0)
# parametrized (t=0 at a, t=1 at b) intersections with viewplanes
intersections = [float("nan"), float("nan"), float("nan"), float("nan")]
# Optionally 3x1 vector
ipts: typing.List[np.ndarray | None] = [None, None, None, None]
# find intersections
for i, normal in enumerate(self.viewplanes):
# // we want to know the value of t when the line intercepts this plane
# // parametrized: v = t * ab + a, where v lies on the plane
# // we can find the projection of a onto the plane normal
# // a_projn = normal.times(av.dot(normal) / normal.dot(normal));
a_projn = (av.dot(normal) / normal.dot(normal)) * normal
# // this projection lets us determine the scalar multiple t of ab where
# // (t * ab + a) is a vector which lies on the plane
if abs(abv.dot(normal)) < 1.0e-5:
continue
intersections[i] = a_projn.dot(a_projn) / -(abv.dot(a_projn))
# // vector a to the viewplane
apv = intersections[i] * abv
# av + apv = intersection point
intersectpt = av + apv
ipts[i] = intersectpt
# // discard intersections outside the camera frustum
for j in range(1, len(self.viewplanes)):
if j == 0:
continue
oi = (i + j) % len(self.viewplanes)
onormal = self.viewplanes[oi]
# if the dot of the intersection point with any plane normal is negative, it is outside
if intersectpt.dot(onormal) < 0:
intersections[i] = float("nan")
ipts[i] = None
break
# // discard duplicate intersections
if ipts[i] is None:
continue
@@ -275,6 +399,7 @@ class SimCameraProperties:
ipts[i] = None
break
# determine visible segment (minimum and maximum t)
inter1 = float("nan")
inter2 = float("nan")
for inter in intersections:
@@ -284,6 +409,7 @@ class SimCameraProperties:
else:
inter2 = inter
# // two viewplane intersections
if not math.isnan(inter2):
max_ = max(inter1, inter2)
min_ = min(inter1, inter2)
@@ -292,16 +418,19 @@ class SimCameraProperties:
if bVisible:
max_ = 1
return (min_, max_)
# // one viewplane intersection
elif not math.isnan(inter1):
if aVisible:
return (0, inter1)
if bVisible:
return (inter1, 1)
return (inter1, None)
# no intersections
else:
return (None, None)
def estPixelNoise(self, points: np.ndarray) -> np.ndarray:
"""Returns these points after applying this camera's estimated noise."""
assert points.shape[1] == 1, points.shape
assert points.shape[2] == 2, points.shape
if self.avgErrorPx == 0 and self.errorStdDevPx == 0:
@@ -309,6 +438,7 @@ class SimCameraProperties:
noisyPts: list[list] = []
for p in points:
# // error pixels in random direction
error = np.random.normal(self.avgErrorPx, self.errorStdDevPx, 1)[0]
errorAngle = np.random.uniform(-math.pi, math.pi)
noisyPts.append(
@@ -324,16 +454,25 @@ class SimCameraProperties:
return retval
def estLatency(self) -> seconds:
"""
:returns: Noisy estimation of a frame's processing latency
"""
return max(
float(np.random.normal(self.avgLatency, self.latencyStdDev, 1)[0]),
0.0,
)
def estSecUntilNextFrame(self) -> seconds:
"""
:returns: Estimate how long until the next frame should be processed in milliseconds
"""
# // exceptional processing latency blocks the next frame
return self.frameSpeed + max(0.0, self.estLatency() - self.frameSpeed)
@classmethod
def PERFECT_90DEG(cls) -> typing.Self:
"""960x720 resolution, 90 degree FOV, "perfect" lagless camera"""
return cls()
@classmethod

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

10
photon-lib/py/photonlibpy/simulation/visionTargetSim.py
View File

@@ -6,7 +6,16 @@ from ..estimation.targetModel import TargetModel
class VisionTargetSim:
"""Describes a vision target located somewhere on the field that your vision system can detect."""
def __init__(self, pose: Pose3d, model: TargetModel, id: int = -1):
"""Describes a fiducial tag located somewhere on the field that your vision system can detect.
:param pose: Pose3d of the tag in field-relative coordinates
:param model: TargetModel which describes the shape of the target(tag)
:param id: The ID of this fiducial tag
"""
self.pose: Pose3d = pose
self.model: TargetModel = model
self.fiducialId: int = id
@@ -47,4 +56,5 @@ class VisionTargetSim:
return self.model
def getFieldVertices(self) -> list[Translation3d]:
"""This target's vertices offset from its field pose."""
return self.model.getFieldVertices(self.pose)