photonlibpy 2025.0.0b2__py3-none-any.whl → 2025.0.0b4__py3-none-any.whl

photonlibpy/generated/PnpResultSerde.py

@@ -20,8 +20,13 @@
 ##                        --> DO NOT MODIFY <--
 ###############################################################################
 
+from typing import TYPE_CHECKING
+
 from ..packet import Packet
-from ..targeting import *
+from ..targeting import *  # noqa
+
+if TYPE_CHECKING:
+    from ..targeting import PnpResult  # noqa
 
 
 class PnpResultSerde:

photonlibpy/generated/TargetCornerSerde.py

@@ -20,8 +20,13 @@
 ##                        --> DO NOT MODIFY <--
 ###############################################################################
 
+from typing import TYPE_CHECKING
+
 from ..packet import Packet
-from ..targeting import *
+from ..targeting import *  # noqa
+
+if TYPE_CHECKING:
+    from ..targeting import TargetCorner  # noqa
 
 
 class TargetCornerSerde:

photonlibpy/networktables/NTTopicSet.py

@@ -9,9 +9,18 @@ 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.
 
-    def __init__(self) -> None:
-        self.subTable = nt.NetworkTableInstance.getDefault()
+    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()
+        photonvision_root_table = instance.getTable(tableName)
+        self.subTable = photonvision_root_table.getSubTable(cameraName)
 
     def updateEntries(self) -> None:
         options = nt.PubSubOptions()

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 @@ 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,9 +295,10 @@ class PhotonCamera:
             )
 
         versionString = self.versionEntry.get(defaultValue="")
-        localUUID = PhotonPipelineResult.photonStruct.MESSAGE_VERSION
 
-        remoteUUID = self._rawBytesEntry.getTopic().getProperty("message_uuid")
+        # Check mdef UUID
+        localUUID = PhotonPipelineResult.photonStruct.MESSAGE_VERSION
+        remoteUUID = str(self._rawBytesEntry.getTopic().getProperty("message_uuid"))
 
         if not remoteUUID:
             wpilib.reportWarning(

photonlibpy/simulation/photonCameraSim.py

@@ -4,8 +4,8 @@ import typing
 import cscore as cs
 import cv2 as cv
 import numpy as np
-import robotpy_apriltag
 import wpilib
+from robotpy_apriltag import AprilTagField, AprilTagFieldLayout
 from wpimath.geometry import Pose3d, Transform3d
 from wpimath.units import meters, seconds
 
@@ -26,52 +26,46 @@ 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__(
         self,
         camera: PhotonCamera,
-        props: SimCameraProperties | None = None,
+        props: SimCameraProperties = SimCameraProperties.PERFECT_90DEG(),
         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
         self.videoSimRawEnabled: bool = False
         self.videoSimWireframeEnabled: bool = False
         self.videoSimWireframeResolution: float = 0.1
-        self.videoSimProcEnabled: bool = True
-        self.ts = NTTopicSet()
+        self.videoSimProcEnabled: bool = (
+            False  # TODO switch this back to default True when the functionality is enabled
+        )
         self.heartbeatCounter: int = 0
         self.nextNtEntryTime = int(wpilib.Timer.getFPGATimestamp() * 1e6)
-        self.tagLayout = robotpy_apriltag.loadAprilTagLayoutField(
-            robotpy_apriltag.AprilTagField.k2024Crescendo
-        )
-
-        if (
-            camera is not None
-            and props is None
-            and minTargetAreaPercent is None
-            and maxSightRange is None
-        ):
-            props = SimCameraProperties.PERFECT_90DEG()
-        elif (
-            camera is not None
-            and props is not None
-            and minTargetAreaPercent is not None
-            and maxSightRange is not None
-        ):
-            pass
-        elif (
-            camera is not None
-            and props is not None
-            and minTargetAreaPercent is None
-            and maxSightRange is None
-        ):
-            pass
-        else:
-            raise Exception("Invalid Constructor Called")
+        self.tagLayout = AprilTagFieldLayout.loadField(AprilTagField.k2024Crescendo)
 
         self.cam = camera
         self.prop = props
@@ -101,16 +95,11 @@ class PhotonCameraSim:
             (self.prop.getResWidth(), self.prop.getResHeight())
         )
 
-        self.ts.subTable = self.cam._cameraTable
+        self.ts = NTTopicSet("photonvision", self.cam.getName())
         self.ts.updateEntries()
 
         # Handle this last explicitly for this function signature because the other constructor is called in the initialiser list
-        if (
-            camera is not None
-            and props is not None
-            and minTargetAreaPercent is not None
-            and maxSightRange is not None
-        ):
+        if minTargetAreaPercent is not None and maxSightRange is not None:
             self.minTargetAreaPercent = minTargetAreaPercent
             self.maxSightRange = maxSightRange
 
@@ -133,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:
@@ -160,53 +166,90 @@ 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")
-        # self.videoSimRawEnabled = enabled
 
     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")
-        # self.videoSimWireframeEnabled = enabled
 
     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")
-        # self.videoSimProcEnabled = enabled
 
     def process(
         self, latency: seconds, cameraPose: Pose3d, targets: list[VisionTargetSim]
@@ -217,27 +260,32 @@ class PhotonCameraSim:
 
         targets.sort(key=distance, reverse=True)
 
-        visibleTgts: list[
-            typing.Tuple[VisionTargetSim, list[typing.Tuple[float, float]]]
-        ] = []
+        # 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(),
@@ -245,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
@@ -258,6 +308,7 @@ class PhotonCameraSim:
                 ] * 4
                 t = (l + 1) % 4
                 b = (l + 1) % 4
+                r = 0
                 for i in range(4):
                     if i == l:
                         continue
@@ -270,24 +321,32 @@ 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,
+                    (center[0, 0], center[0, 1]),
                     (
                         imagePoints[r, 0, 0] - lc[0, 0],
                         imagePoints[b, 0, 1] - imagePoints[t, 0, 1],
                     ),
                     -angles[r],
                 )
-                imagePoints = rect.points()
+                # 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
@@ -296,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(),
@@ -325,13 +385,14 @@ class PhotonCameraSim:
             )
 
         # Video streams disabled for now
-        if self.enableRawStream:
+        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);
             # blankFrame.assignTo(videoSimFrameRaw);
             pass
-        if self.enableProcessedStream:
+        if self.videoSimProcEnabled:
             # VideoSimUtil::UpdateVideoProp(videoSimProcessed, prop);
             pass
 
@@ -343,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(),
@@ -354,10 +416,16 @@ class PhotonCameraSim:
             if pnpResult is not None:
                 multiTagResults = MultiTargetPNPResult(pnpResult, usedIds)
 
+        # put this simulated data to NT
         self.heartbeatCounter += 1
+        now_micros = wpilib.Timer.getFPGATimestamp() * 1e6
         return PhotonPipelineResult(
             metadata=PhotonPipelineMetadata(
-                self.heartbeatCounter, int(latency * 1e6), 1000000
+                self.heartbeatCounter,
+                int(now_micros - latency * 1e6),
+                int(now_micros),
+                # Pretend like we heard a pong recently
+                int(np.random.uniform(950, 1050)),
             ),
             targets=detectableTgts,
             multitagResult=multiTagResults,
@@ -366,6 +434,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)
@@ -385,6 +460,7 @@ class PhotonCameraSim:
             self.ts.targetSkewEntry.set(0.0, receiveTimestamp)
         else:
             bestTarget = result.getBestTarget()
+            assert bestTarget
 
             self.ts.targetPitchEntry.set(bestTarget.getPitch(), receiveTimestamp)
             self.ts.targetYawEntry.set(bestTarget.getYaw(), receiveTimestamp)