From 04191efc51b34bdabd0a6e73dfc5ca9ec650dd71 Mon Sep 17 00:00:00 2001 From: Lucien Morey Date: Sat, 16 Nov 2024 13:01:33 +1100 Subject: [PATCH] sphinxify java docs for python code (#1575) --- .../py/photonlibpy/estimation/openCVHelp.py | 105 +++++++++++++ .../estimation/rotTrlTransform3d.py | 19 +++ .../py/photonlibpy/estimation/targetModel.py | 63 ++++++++ .../estimation/visionEstimation.py | 22 +++ .../photonlibpy/networktables/NTTopicSet.py | 7 + photon-lib/py/photonlibpy/photonCamera.py | 69 ++++++++- .../photonlibpy/simulation/photonCameraSim.py | 105 ++++++++++++- .../simulation/simCameraProperties.py | 139 ++++++++++++++++++ .../photonlibpy/simulation/visionSystemSim.py | 92 ++++++++++++ .../photonlibpy/simulation/visionTargetSim.py | 10 ++ 10 files changed, 628 insertions(+), 3 deletions(-) diff --git a/photon-lib/py/photonlibpy/estimation/openCVHelp.py b/photon-lib/py/photonlibpy/estimation/openCVHelp.py index eaf3106245..2f687c189f 100644 --- a/photon-lib/py/photonlibpy/estimation/openCVHelp.py +++ b/photon-lib/py/photonlibpy/estimation/openCVHelp.py @@ -27,6 +27,12 @@ def rotationNWUtoEDN(rot: Rotation3d) -> Rotation3d: @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 @@ def translationToTVec(translations: list[Translation3d]) -> np.ndarray: @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 @@ def projectPoints( 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 @@ def reorderCircular( @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 @@ def solvePNP_Square( 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 @@ def solvePNP_Square( 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 @@ def solvePNP_Square( flags=cv.SOLVEPNP_IPPE_SQUARE, ) + # convert to wpilib coordinates best = Transform3d( OpenCVHelp.tVecToTranslation(tvecs[0]), OpenCVHelp.rVecToRotation(rvecs[0]), @@ -148,6 +230,7 @@ def solvePNP_Square( 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 @@ def solvePNP_Square( 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 @@ def solvePNP_SQPNP( 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( diff --git a/photon-lib/py/photonlibpy/estimation/rotTrlTransform3d.py b/photon-lib/py/photonlibpy/estimation/rotTrlTransform3d.py index 9f98f4f2bb..e4c99c3940 100644 --- a/photon-lib/py/photonlibpy/estimation/rotTrlTransform3d.py +++ b/photon-lib/py/photonlibpy/estimation/rotTrlTransform3d.py @@ -4,24 +4,38 @@ 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 @@ def applyTrls(self, rots: list[Rotation3d]) -> list[Rotation3d]: @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 diff --git a/photon-lib/py/photonlibpy/estimation/targetModel.py b/photon-lib/py/photonlibpy/estimation/targetModel.py index d8d8b0f3a9..61ee54b798 100644 --- a/photon-lib/py/photonlibpy/estimation/targetModel.py +++ b/photon-lib/py/photonlibpy/estimation/targetModel.py @@ -8,14 +8,27 @@ 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 @@ def createPlanar(cls, width: meters, height: meters) -> Self: @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 @@ def createCuboid(cls, length: meters, width: meters, height: meters) -> Self: @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 @@ def createSpheroid(cls, diameter: meters) -> Self: @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 @@ def _common_construction(self, verts: List[Translation3d]) -> None: 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 @@ def getFieldVertices(self, targetPose: Pose3d) -> List[Translation3d]: @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, diff --git a/photon-lib/py/photonlibpy/estimation/visionEstimation.py b/photon-lib/py/photonlibpy/estimation/visionEstimation.py index eaa4b0d334..3ab7da4a13 100644 --- a/photon-lib/py/photonlibpy/estimation/visionEstimation.py +++ b/photon-lib/py/photonlibpy/estimation/visionEstimation.py @@ -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 @@ def estimateCamPosePNP( 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 @@ def estimateCamPosePNP( points = OpenCVHelp.cornersToPoints(corners) + # single-tag pnp if len(knownTags) == 1: camToTag = OpenCVHelp.solvePNP_Square( cameraMatrix, distCoeffs, tagModel.getVertices(), points @@ -74,6 +95,7 @@ def estimateCamPosePNP( altReprojErr=camToTag.altReprojErr, ) return result + # multi-tag pnp else: objectTrls: list[Translation3d] = [] for tag in knownTags: diff --git a/photon-lib/py/photonlibpy/networktables/NTTopicSet.py b/photon-lib/py/photonlibpy/networktables/NTTopicSet.py index 462c480729..0a0938efef 100644 --- a/photon-lib/py/photonlibpy/networktables/NTTopicSet.py +++ b/photon-lib/py/photonlibpy/networktables/NTTopicSet.py @@ -9,6 +9,13 @@ 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() diff --git a/photon-lib/py/photonlibpy/photonCamera.py b/photon-lib/py/photonlibpy/photonCamera.py index c7635dca58..03ab2cafbe 100644 --- a/photon-lib/py/photonlibpy/photonCamera.py +++ b/photon-lib/py/photonlibpy/photonCamera.py @@ -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 @@ def getAllUnreadResults(self) -> List[PhotonPipelineResult]: 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 @@ def getLatestResult(self) -> PhotonPipelineResult: 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 @@ def _versionCheck(self) -> None: _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 @@ def _versionCheck(self) -> None: 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 @@ def _versionCheck(self) -> None: ) 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: diff --git a/photon-lib/py/photonlibpy/simulation/photonCameraSim.py b/photon-lib/py/photonlibpy/simulation/photonCameraSim.py index b50a917050..d5433a0f7b 100644 --- a/photon-lib/py/photonlibpy/simulation/photonCameraSim.py +++ b/photon-lib/py/photonlibpy/simulation/photonCameraSim.py @@ -26,6 +26,10 @@ 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 @@ def __init__( 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 @@ def getVideoSimFrameRaw(self) -> np.ndarray: 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 @@ def canSeeCorner(self, points: np.ndarray) -> bool: 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 @@ def distance(target: VisionTargetSim): 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 @@ def distance(target: VisionTargetSim): 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 @@ def distance(target: VisionTargetSim): 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 @@ def distance(target: VisionTargetSim): ), -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 @@ def distance(target: VisionTargetSim): 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 @@ def distance(target: VisionTargetSim): # 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 @@ def distance(target: VisionTargetSim): 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 @@ def distance(target: VisionTargetSim): 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 @@ def distance(target: VisionTargetSim): 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) diff --git a/photon-lib/py/photonlibpy/simulation/simCameraProperties.py b/photon-lib/py/photonlibpy/simulation/simCameraProperties.py index 4e54e09900..4cefc944a6 100644 --- a/photon-lib/py/photonlibpy/simulation/simCameraProperties.py +++ b/photon-lib/py/photonlibpy/simulation/simCameraProperties.py @@ -11,7 +11,22 @@ 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 @@ def setCalibrationFromFOV( 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 @@ def setCalibrationFromIntrinsics( self.camIntrinsics = newCamIntrinsics self.distCoeffs = newDistCoeffs + # left, right, up, and down view planes p = [ Translation3d( 1.0, @@ -110,16 +130,33 @@ def setCalibError(self, newAvgErrorPx: float, newErrorStdDevPx: float): 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 @@ def getLatencyStdDev(self) -> seconds: 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 @@ def getPixelRot(self, point: cv.typing.Point2f) -> Rotation3d: ) 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: + + 
pitch = arctan(pixel y offset / focal length y)
+ + 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: + + 
focal length y ⟶ (focal length y / cos(arctan(pixel x offset / focal length x)))
+ + :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 @@ def getCorrectedPixelRot(self, point: cv.typing.Point2f) -> Rotation3d: 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 @@ def getDiagFOV(self) -> Rotation2d: 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 p = t * (b - a) + a. 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 @@ def getVisibleLine( 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 @@ def getVisibleLine( 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 @@ def getVisibleLine( 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 @@ def getVisibleLine( else: inter2 = inter + # // two viewplane intersections if not math.isnan(inter2): max_ = max(inter1, inter2) min_ = min(inter1, inter2) @@ -292,16 +418,19 @@ def getVisibleLine( 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 @@ def estPixelNoise(self, points: np.ndarray) -> np.ndarray: 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 @@ def estPixelNoise(self, points: np.ndarray) -> np.ndarray: 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 diff --git a/photon-lib/py/photonlibpy/simulation/visionSystemSim.py b/photon-lib/py/photonlibpy/simulation/visionSystemSim.py index ca42004c07..88a7c4f117 100644 --- a/photon-lib/py/photonlibpy/simulation/visionSystemSim.py +++ b/photon-lib/py/photonlibpy/simulation/visionSystemSim.py @@ -15,7 +15,22 @@ 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 @@ def __init__(self, visionSystemName: str): 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 @@ def addCamera(self, cameraSim: PhotonCameraSim, robotToCamera: Transform3d) -> N ) 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 @@ def getRobotToCamera( 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 @@ def getCameraPose( 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 @@ def getCameraPose( 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 @@ def adjustCamera( 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 @@ def getVisionTargets(self, targetType: str | None = None) -> list[VisionTargetSi 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 @@ def removeVisionTargets( 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 @@ def getDebugField(self) -> Field2d: 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 @@ def update(self, robotPose: Pose2d | Pose3d) -> None: 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 @@ def update(self, robotPose: Pose2d | Pose3d) -> None: 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(): diff --git a/photon-lib/py/photonlibpy/simulation/visionTargetSim.py b/photon-lib/py/photonlibpy/simulation/visionTargetSim.py index 97a0e8b176..f2bd0b5430 100644 --- a/photon-lib/py/photonlibpy/simulation/visionTargetSim.py +++ b/photon-lib/py/photonlibpy/simulation/visionTargetSim.py @@ -6,7 +6,16 @@ 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 @@ def getModel(self) -> TargetModel: return self.model def getFieldVertices(self) -> list[Translation3d]: + """This target's vertices offset from its field pose.""" return self.model.getFieldVertices(self.pose)