sphinxify docs for VisionSystemSim

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,26 +292,34 @@ 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:
                 return 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():