Move ApriltagFieldLayout into the VisionSystemSim constructor

photon-lib/py/buildAndTest.sh

@@ -3,11 +3,21 @@
 set -euo pipefail
 cd -- "$(dirname -- "$0")"
 
+# Create and activate virtual environment
+if [ ! -d ".venv" ]; then
+    echo "Creating virtual environment in .venv"
+    python3 -m venv .venv
+fi
+
+# Activate the virtual environment
+source .venv/bin/activate
+
 # Uninstall if it already was installed
 python3 -m pip uninstall -y photonlibpy
 
 # Build wheel
-python3 -m pip install wheel
+python3 -m pip install --upgrade pip
+python3 -m pip install wheel setuptools pytest mypy
 python3 setup.py bdist_wheel
 
 # Install whatever wheel was made
@@ -18,3 +28,6 @@ done
 
 # Run the test suite
 pytest -rP
+
+cd ../../
+mypy --show-column-numbers --config-file photon-lib/py/pyproject.toml photon-lib

photon-lib/py/photonlibpy/simulation/photonCameraSim.py

@@ -5,7 +5,7 @@
 import cv2 as cv
 import numpy as np
 import wpilib
-from robotpy_apriltag import AprilTagField, AprilTagFieldLayout
+from robotpy_apriltag import AprilTagFieldLayout
 from wpimath.geometry import Pose3d, Transform3d
 from wpimath.units import meters, seconds
 
@@ -36,9 +36,6 @@ def __init__(
         self,
         camera: PhotonCamera,
         props: SimCameraProperties = SimCameraProperties.PERFECT_90DEG(),
-        tagLayout: AprilTagFieldLayout = AprilTagFieldLayout.loadField(
-            AprilTagField.kDefaultField
-        ),
         minTargetAreaPercent: float | None = None,
         maxSightRange: meters | None = None,
     ):
@@ -67,7 +64,6 @@ def __init__(
         self.videoSimProcEnabled: bool = False
         self.heartbeatCounter: int = 0
         self.nextNtEntryTime = wpilib.Timer.getFPGATimestamp()
-        self.tagLayout = tagLayout
 
         self.cam = camera
         self.prop = props
@@ -254,7 +250,11 @@ def enableProcessedStream(self, enabled: bool) -> None:
         raise Exception("Processed stream not implemented")
 
     def process(
-        self, latency: seconds, cameraPose: Pose3d, targets: list[VisionTargetSim]
+        self,
+        latency: seconds,
+        cameraPose: Pose3d,
+        targets: list[VisionTargetSim],
+        tagLayout: AprilTagFieldLayout | None,
     ) -> PhotonPipelineResult:
         # Sort targets by distance to camera - furthest to closest
         def distance(target: VisionTargetSim):
@@ -400,23 +400,24 @@ def distance(target: VisionTargetSim):
 
         multiTagResults: MultiTargetPNPResult | None = None
 
-        visibleLayoutTags = VisionEstimation.getVisibleLayoutTags(
-            detectableTgts, self.tagLayout
-        )
-
-        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(),
-                self.prop.getDistCoeffs(),
-                detectableTgts,
-                self.tagLayout,
-                TargetModel.AprilTag36h11(),
+        if tagLayout is not None:
+            visibleLayoutTags = VisionEstimation.getVisibleLayoutTags(
+                detectableTgts, tagLayout
             )
-            if pnpResult is not None:
-                multiTagResults = MultiTargetPNPResult(pnpResult, usedIds)
+
+            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(),
+                    self.prop.getDistCoeffs(),
+                    detectableTgts,
+                    tagLayout,
+                    TargetModel.AprilTag36h11(),
+                )
+                if pnpResult is not None:
+                    multiTagResults = MultiTargetPNPResult(pnpResult, usedIds)
 
         # put this simulated data to NT
         self.heartbeatCounter += 1

photon-lib/py/photonlibpy/simulation/visionSystemSim.py

@@ -1,7 +1,7 @@
 import typing
 
 import wpilib
-from robotpy_apriltag import AprilTagFieldLayout
+from robotpy_apriltag import AprilTagField, AprilTagFieldLayout
 from wpilib import Field2d
 from wpimath.geometry import Pose2d, Pose3d, Transform3d
 
@@ -22,14 +22,23 @@ class VisionSystemSim:
     camera target info.
     """
 
-    def __init__(self, visionSystemName: str):
+    def __init__(
+        self,
+        visionSystemName: str,
+        tagLayout: AprilTagFieldLayout | None = AprilTagFieldLayout.loadField(
+            AprilTagField.kDefaultField
+        ),
+        targetModel: TargetModel = TargetModel.AprilTag36h11(),
+    ):
         """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.
+        :param tagLayout: The field layout to use for AprilTag detection. If not provided, the default field layout will be used.
+        :param targetModel: The model to use for vision targets.
         """
         self.dbgField: Field2d = Field2d()
         self.bufferLength: seconds = 1.5
@@ -45,6 +54,11 @@ def __init__(self, visionSystemName: str):
 
         self.tableName: str = "VisionSystemSim-" + visionSystemName
         wpilib.SmartDashboard.putData(self.tableName + "/Sim Field", self.dbgField)
+        self.tagLayout = tagLayout
+        self.targetModel = targetModel
+
+        if tagLayout is not None:
+            self.addAprilTags(tagLayout)
 
     def getCameraSim(self, name: str) -> PhotonCameraSim | None:
         """Get one of the simulated cameras."""
@@ -215,9 +229,7 @@ def addAprilTags(self, layout: AprilTagFieldLayout) -> None:
             tag_pose = layout.getTagPose(tag.ID)
             # TODO this was done to make the python gods happy. Confirm that this is desired or if types dont matter
             assert tag_pose is not None
-            targets.append(
-                VisionTargetSim(tag_pose, TargetModel.AprilTag36h11(), tag.ID)
-            )
+            targets.append(VisionTargetSim(tag_pose, self.targetModel, tag.ID))
         self.addVisionTargets(targets, "apriltag")
 
     def clearVisionTargets(self) -> None:
@@ -316,7 +328,9 @@ def update(self, robotPose: Pose2d | Pose3d) -> None:
             cameraPoses2d.append(lateCameraPose.toPose2d())
 
             # process a PhotonPipelineResult with visible targets
-            camResult = camSim.process(latency, lateCameraPose, allTargets)
+            camResult = camSim.process(
+                latency, lateCameraPose, allTargets, self.tagLayout
+            )
             # publish this info to NT at estimated timestamp of receive
             # needs a timestamp in microseconds
             camSim.submitProcessedFrame(camResult, timestampNt * 1.0e6)

photon-lib/py/test/photonPoseEstimator_test.py

@@ -184,7 +184,10 @@ def test_pnpDistanceTrigSolve():
 
     realPose = Pose3d(7.3, 4.42, 0, Rotation3d(0, 0, 2.197))  # Pose to compare with
     result = cameraOneSim.process(
-        latencySecs, realPose.transformBy(estimator.robotToCamera), simTargets
+        latencySecs,
+        realPose.transformBy(estimator.robotToCamera),
+        simTargets,
+        aprilTags,
     )
     bestTarget = result.getBestTarget()
     assert bestTarget is not None
@@ -210,7 +213,10 @@ def test_pnpDistanceTrigSolve():
     estimator.robotToCamera = straightOnTestTransform
     realPose = Pose3d(4.81, 2.38, 0, Rotation3d(0, 0, 2.818))  # Pose to compare with
     result = cameraOneSim.process(
-        latencySecs, realPose.transformBy(estimator.robotToCamera), simTargets
+        latencySecs,
+        realPose.transformBy(estimator.robotToCamera),
+        simTargets,
+        aprilTags,
     )
     bestTarget = result.getBestTarget()
     assert bestTarget is not None

photon-lib/py/test/visionSystemSim_test.py

@@ -20,7 +20,7 @@
 def test_VisibilityCupidShuffle() -> None:
     targetPose = Pose3d(Translation3d(15.98, 0.0, 2.0), Rotation3d(0, 0, math.pi))
 
-    visionSysSim = VisionSystemSim("Test")
+    visionSysSim = VisionSystemSim("Test", AprilTagFieldLayout())
     camera = PhotonCamera("camera")
     cameraSim = PhotonCameraSim(camera)
     visionSysSim.addCamera(cameraSim, Transform3d())
@@ -144,7 +144,7 @@ def test_NotVisibleVert2() -> None:
 def test_NotVisibleTargetSize() -> None:
     targetPose = Pose3d(Translation3d(15.98, 0.0, 1.0), Rotation3d(0, 0, math.pi))
 
-    visionSysSim = VisionSystemSim("Test")
+    visionSysSim = VisionSystemSim("Test", AprilTagFieldLayout())
     camera = PhotonCamera("camera")
     cameraSim = PhotonCameraSim(camera)
     visionSysSim.addCamera(cameraSim, Transform3d())
@@ -171,7 +171,7 @@ def test_NotVisibleTargetSize() -> None:
 def test_NotVisibleTooFarLeds() -> None:
     targetPose = Pose3d(Translation3d(15.98, 0.0, 1.0), Rotation3d(0, 0, math.pi))
 
-    visionSysSim = VisionSystemSim("Test")
+    visionSysSim = VisionSystemSim("Test", AprilTagFieldLayout())
     camera = PhotonCamera("camera")
     cameraSim = PhotonCameraSim(camera)
     visionSysSim.addCamera(cameraSim, Transform3d())
@@ -204,7 +204,7 @@ def test_YawAngles(expected_yaw) -> None:
         Translation3d(15.98, 0.0, 1.0), Rotation3d(0.0, 0.0, 3.0 * math.pi / 4.0)
     )
 
-    visionSysSim = VisionSystemSim("Test")
+    visionSysSim = VisionSystemSim("Test", AprilTagFieldLayout())
     camera = PhotonCamera("camera")
     cameraSim = PhotonCameraSim(camera)
 
@@ -240,7 +240,7 @@ def test_PitchAngles(expected_pitch) -> None:
     robotPose = Pose2d(
         Translation2d(10.0, 0.0), Rotation2d.fromDegrees(-expected_pitch)
     )
-    visionSysSim = VisionSystemSim("Test")
+    visionSysSim = VisionSystemSim("Test", AprilTagFieldLayout())
     camera = PhotonCamera("camera")
     cameraSim = PhotonCameraSim(camera)
     visionSysSim.addCamera(cameraSim, Transform3d())
@@ -307,7 +307,8 @@ def test_distanceCalc(distParam, pitchParam, heightParam) -> None:
     )
 
     visionSysSim = VisionSystemSim(
-        "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysohowsyourdaygoingihopegoodhaveagreatrestofyourlife"
+        "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysohowsyourdaygoingihopegoodhaveagreatrestofyourlife",
+        AprilTagFieldLayout(),
     )
     camera = PhotonCamera("camera")
     cameraSim = PhotonCameraSim(camera)
@@ -345,7 +346,7 @@ def test_MultipleTargets() -> None:
     targetPoseC = Pose3d(Translation3d(15.98, 0.0, 0.0), Rotation3d(0.0, 0.0, math.pi))
     targetPoseR = Pose3d(Translation3d(15.98, -2.0, 0.0), Rotation3d(0.0, 0.0, math.pi))
 
-    visionSysSim = VisionSystemSim("Test")
+    visionSysSim = VisionSystemSim("Test", AprilTagFieldLayout())
     camera = PhotonCamera("camera")
     cameraSim = PhotonCameraSim(camera)
     visionSysSim.addCamera(cameraSim, Transform3d())
@@ -443,14 +444,6 @@ def test_MultipleTargets() -> None:
 
 
 def test_PoseEstimation() -> None:
-    visionSysSim = VisionSystemSim("Test")
-    camera = PhotonCamera("camera")
-    cameraSim = PhotonCameraSim(camera)
-    visionSysSim.addCamera(cameraSim, Transform3d())
-
-    cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(90.0))
-    cameraSim.setMinTargetAreaPixels(20.0)
-
     tagList: list[AprilTag] = []
     at0 = AprilTag()
     at0.ID = 0
@@ -468,6 +461,15 @@ def test_PoseEstimation() -> None:
     fieldLength: meters = 54.0
     fieldWidth: meters = 27.0
     layout = AprilTagFieldLayout(tagList, fieldLength, fieldWidth)
+
+    visionSysSim = VisionSystemSim("Test", layout, TargetModel.AprilTag16h5())
+    camera = PhotonCamera("camera")
+    cameraSim = PhotonCameraSim(camera)
+    visionSysSim.addCamera(cameraSim, Transform3d())
+
+    cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(90.0))
+    cameraSim.setMinTargetAreaPixels(20.0)
+
     robotPose = Pose2d(Translation2d(5.0, 1.0), Rotation2d.fromDegrees(5.0))
     visionSysSim.addVisionTargets(
         [VisionTargetSim(tagList[0].pose, TargetModel.AprilTag16h5(), 0)]
@@ -518,14 +520,6 @@ def test_PoseEstimationRotated() -> None:
         Rotation3d(0.0, math.radians(-30.0), math.radians(25.5)),
     )
 
-    visionSysSim = VisionSystemSim("Test")
-    camera = PhotonCamera("camera")
-    cameraSim = PhotonCameraSim(camera)
-    visionSysSim.addCamera(cameraSim, robotToCamera)
-
-    cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(90.0))
-    cameraSim.setMinTargetAreaPixels(20.0)
-
     tagList: list[AprilTag] = []
     at0 = AprilTag()
     at0.ID = 0
@@ -543,6 +537,15 @@ def test_PoseEstimationRotated() -> None:
     fieldLength: meters = 54.0
     fieldWidth: meters = 27.0
     layout = AprilTagFieldLayout(tagList, fieldLength, fieldWidth)
+
+    visionSysSim = VisionSystemSim("Test", layout)
+    camera = PhotonCamera("camera")
+    cameraSim = PhotonCameraSim(camera)
+    visionSysSim.addCamera(cameraSim, robotToCamera)
+
+    cameraSim.prop.setCalibrationFromFOV(640, 480, fovDiag=Rotation2d.fromDegrees(90.0))
+    cameraSim.setMinTargetAreaPixels(20.0)
+
     robotPose = Pose2d(Translation2d(5.0, 1.0), Rotation2d.fromDegrees(-5.0))
     visionSysSim.addVisionTargets(
         [VisionTargetSim(tagList[0].pose, TargetModel.AprilTag36h11(), 0)]
@@ -590,7 +593,7 @@ def test_PoseEstimationRotated() -> None:
 
 
 def test_TagAmbiguity() -> None:
-    visionSysSim = VisionSystemSim("Test")
+    visionSysSim = VisionSystemSim("Test", AprilTagFieldLayout())
     camera = PhotonCamera("camera")
     cameraSim = PhotonCameraSim(camera)
     visionSysSim.addCamera(cameraSim, Transform3d())