Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-23 01:21:40 +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/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)