diff --git a/photon-lib/py/buildAndTest.sh b/photon-lib/py/buildAndTest.sh index f8c71654f1..ecf17e93c0 100755 --- a/photon-lib/py/buildAndTest.sh +++ b/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 diff --git a/photon-lib/py/photonlibpy/simulation/photonCameraSim.py b/photon-lib/py/photonlibpy/simulation/photonCameraSim.py index 8f3472732a..17f8849682 100644 --- a/photon-lib/py/photonlibpy/simulation/photonCameraSim.py +++ b/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 diff --git a/photon-lib/py/photonlibpy/simulation/visionSystemSim.py b/photon-lib/py/photonlibpy/simulation/visionSystemSim.py index 7abd3c2045..51268e14eb 100644 --- a/photon-lib/py/photonlibpy/simulation/visionSystemSim.py +++ b/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,7 +22,14 @@ 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 @@ -30,6 +37,8 @@ def __init__(self, visionSystemName: str): 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) diff --git a/photon-lib/py/test/photonPoseEstimator_test.py b/photon-lib/py/test/photonPoseEstimator_test.py index ad9693664b..c4d783d50b 100644 --- a/photon-lib/py/test/photonPoseEstimator_test.py +++ b/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 diff --git a/photon-lib/py/test/visionSystemSim_test.py b/photon-lib/py/test/visionSystemSim_test.py index df5dba4c13..7854cda905 100644 --- a/photon-lib/py/test/visionSystemSim_test.py +++ b/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()) diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index d20af69586..6ad0c0d47b 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -25,7 +25,6 @@ package org.photonvision.simulation; import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.cscore.CvSource; import edu.wpi.first.cscore.OpenCvLoader; @@ -84,8 +83,6 @@ public class PhotonCameraSim implements AutoCloseable { private double minTargetAreaPercent; private PhotonTargetSortMode sortMode = PhotonTargetSortMode.Largest; - private final AprilTagFieldLayout tagLayout; - // video stream simulation private final CvSource videoSimRaw; private final Mat videoSimFrameRaw = new Mat(); @@ -132,24 +129,8 @@ public PhotonCameraSim(PhotonCamera camera) { * @param prop Properties of this camera such as FOV and FPS */ public PhotonCameraSim(PhotonCamera camera, SimCameraProperties prop) { - this(camera, prop, AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField)); - } - - /** - * Constructs a handle for simulating {@link PhotonCamera} values. Processing simulated targets - * through this class will change the associated PhotonCamera's results. - * - *

By default, the minimum target area is 100 pixels and there is no maximum sight range. - * - * @param camera The camera to be simulated - * @param prop Properties of this camera such as FOV and FPS - * @param tagLayout The {@link AprilTagFieldLayout} used to solve for tag positions. - */ - public PhotonCameraSim( - PhotonCamera camera, SimCameraProperties prop, AprilTagFieldLayout tagLayout) { this.cam = camera; this.prop = prop; - this.tagLayout = tagLayout; setMinTargetAreaPixels(kDefaultMinAreaPx); videoSimRaw = @@ -181,30 +162,6 @@ public PhotonCameraSim( SimCameraProperties prop, double minTargetAreaPercent, double maxSightRangeMeters) { - this(camera, prop, AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField)); - this.minTargetAreaPercent = minTargetAreaPercent; - this.maxSightRangeMeters = maxSightRangeMeters; - } - - /** - * Constructs a handle for simulating {@link PhotonCamera} values. Processing simulated targets - * through this class will change the associated PhotonCamera's results. - * - * @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. - * @param tagLayout AprilTag field layout to use for multi-tag estimation - */ - public PhotonCameraSim( - PhotonCamera camera, - SimCameraProperties prop, - double minTargetAreaPercent, - double maxSightRangeMeters, - AprilTagFieldLayout tagLayout) { this(camera, prop); this.minTargetAreaPercent = minTargetAreaPercent; this.maxSightRangeMeters = maxSightRangeMeters; @@ -418,7 +375,10 @@ public void enableProcessedStream(boolean enabled) { } public PhotonPipelineResult process( - double latencyMillis, Pose3d cameraPose, List targets) { + double latencyMillis, + Pose3d cameraPose, + List targets, + AprilTagFieldLayout tagLayout) { // sort targets by distance to camera targets = new ArrayList<>(targets); targets.sort( diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java b/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java index 29b79a8f0d..460a06676b 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java @@ -26,6 +26,7 @@ import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform3d; @@ -67,6 +68,9 @@ public class VisionSystemSim { private final Transform3d kEmptyTrf = new Transform3d(); + private final AprilTagFieldLayout tagLayout; + private final TargetModel targetModel; + /** * A simulated vision system involving a camera(s) and coprocessor(s) mounted on a mobile robot * running PhotonVision, detecting targets placed on the field. {@link VisionTargetSim}s added to @@ -77,9 +81,42 @@ public class VisionSystemSim { * @param visionSystemName The specific identifier for this vision system in NetworkTables. */ public VisionSystemSim(String visionSystemName) { + this(visionSystemName, AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField)); + } + + /** + * A simulated vision system involving a camera(s) and coprocessor(s) mounted on a mobile robot + * running PhotonVision, detecting targets placed on the field. {@link VisionTargetSim}s added to + * this class will be detected by the {@link 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 + */ + public VisionSystemSim(String visionSystemName, AprilTagFieldLayout tagLayout) { + this(visionSystemName, tagLayout, TargetModel.kAprilTag36h11); + } + + /** + * A simulated vision system involving a camera(s) and coprocessor(s) mounted on a mobile robot + * running PhotonVision, detecting targets placed on the field. {@link VisionTargetSim}s added to + * this class will be detected by the {@link 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 + * @param targetModel The model to use for vision targets. + */ + public VisionSystemSim( + String visionSystemName, AprilTagFieldLayout tagLayout, TargetModel targetModel) { dbgField = new Field2d(); String tableName = "VisionSystemSim-" + visionSystemName; SmartDashboard.putData(tableName + "/Sim Field", dbgField); + this.tagLayout = tagLayout; + this.targetModel = targetModel; + addAprilTags(tagLayout); } /** Get one of the simulated cameras. */ @@ -269,7 +306,7 @@ public void addAprilTags(AprilTagFieldLayout tagLayout) { "apriltag", new VisionTargetSim( tagLayout.getTagPose(tag.ID).get(), // preserve alliance rotation - TargetModel.kAprilTag36h11, + targetModel, tag.ID)); } } @@ -431,7 +468,7 @@ public void update(Pose3d robotPoseMeters) { cameraPoses2d.add(lateCameraPose.toPose2d()); // process a PhotonPipelineResult with visible targets - var camResult = camSim.process(latencyMillis, lateCameraPose, allTargets); + var camResult = camSim.process(latencyMillis, lateCameraPose, allTargets, tagLayout); // publish this info to NT at estimated timestamp of receive camSim.submitProcessedFrame(camResult, timestampNT); // display debug results diff --git a/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp b/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp index ed6d2bd359..0e95450cf0 100644 --- a/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp +++ b/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp @@ -34,14 +34,11 @@ namespace photon { PhotonCameraSim::PhotonCameraSim(PhotonCamera* camera) - : PhotonCameraSim(camera, photon::SimCameraProperties::PERFECT_90DEG(), - frc::AprilTagFieldLayout::LoadField( - frc::AprilTagField::kDefaultField)) {} + : PhotonCameraSim(camera, photon::SimCameraProperties::PERFECT_90DEG()) {} PhotonCameraSim::PhotonCameraSim(PhotonCamera* camera, - const SimCameraProperties& props, - const frc::AprilTagFieldLayout& tagLayout) - : prop{props}, cam{camera}, tagLayout{tagLayout} { + const SimCameraProperties& props) + : prop{props}, cam{camera} { SetMinTargetAreaPixels(kDefaultMinAreaPx); videoSimRaw = frc::CameraServer::PutVideo(std::string{camera->GetCameraName()} + "-raw", @@ -109,7 +106,8 @@ std::optional PhotonCameraSim::ConsumeNextEntryTime() { } PhotonPipelineResult PhotonCameraSim::Process( units::second_t latency, const frc::Pose3d& cameraPose, - std::vector targets) { + std::vector targets, + const frc::AprilTagFieldLayout& tagLayout) { std::sort(targets.begin(), targets.end(), [cameraPose](const VisionTargetSim& t1, const VisionTargetSim& t2) { units::meter_t dist1 = diff --git a/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h index 955bda85a7..8e47949426 100644 --- a/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h +++ b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h @@ -72,13 +72,8 @@ class PhotonCameraSim { * * @param camera The camera to be simulated * @param prop Properties of this camera such as FOV and FPS - * @param tagLayout The AprilTagFieldLayout used to solve for tag - * positions. */ - PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props, - const frc::AprilTagFieldLayout& tagLayout = - frc::AprilTagFieldLayout::LoadField( - frc::AprilTagField::kDefaultField)); + PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props); /** * Constructs a handle for simulating PhotonCamera values. Processing @@ -235,7 +230,8 @@ class PhotonCameraSim { } PhotonPipelineResult Process(units::second_t latency, const frc::Pose3d& cameraPose, - std::vector targets); + std::vector targets, + const frc::AprilTagFieldLayout& tagLayout); void SubmitProcessedFrame(const PhotonPipelineResult& result); void SubmitProcessedFrame(const PhotonPipelineResult& result, @@ -255,8 +251,6 @@ class PhotonCameraSim { static constexpr double kDefaultMinAreaPx{100}; double minTargetAreaPercent; - frc::AprilTagFieldLayout tagLayout; - cs::CvSource videoSimRaw; cv::Mat videoSimFrameRaw{}; bool videoSimRawEnabled{true}; diff --git a/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h b/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h index 81f1a87a6b..a20a1e52db 100644 --- a/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h +++ b/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h @@ -57,10 +57,19 @@ class VisionSystemSim { * * @param visionSystemName The specific identifier for this vision system in * NetworkTables. + * @param tagLayout The field layout to use for AprilTag detection + * @param targetModel The model to use for vision targets */ - explicit VisionSystemSim(std::string visionSystemName) { + explicit VisionSystemSim( + std::string visionSystemName, + const frc::AprilTagFieldLayout tagLayout = + frc::AprilTagFieldLayout::LoadField( + frc::AprilTagField::kDefaultField), + const photon::TargetModel targetModel = photon::kAprilTag36h11) + : tagLayout(tagLayout), targetModel(targetModel) { std::string tableName = "VisionSystemSim-" + visionSystemName; frc::SmartDashboard::PutData(tableName + "/Sim Field", &dbgField); + AddAprilTags(tagLayout); } /** Get one of the simulated cameras. */ @@ -316,7 +325,7 @@ class VisionSystemSim { std::vector targets; for (const frc::AprilTag& tag : layout.GetTags()) { targets.emplace_back(VisionTargetSim{layout.GetTagPose(tag.ID).value(), - photon::kAprilTag36h11, tag.ID}); + targetModel, tag.ID}); } AddVisionTargets("apriltag", targets); } @@ -451,7 +460,8 @@ class VisionSystemSim { lateRobotPose + GetRobotToCamera(camSim, timestampCapture).value(); cameraPoses2d.push_back(lateCameraPose.ToPose2d()); - auto camResult = camSim->Process(latency, lateCameraPose, allTargets); + auto camResult = + camSim->Process(latency, lateCameraPose, allTargets, tagLayout); camSim->SubmitProcessedFrame(camResult, timestampNt); for (const auto& target : camResult.GetTargets()) { auto trf = target.GetBestCameraToTarget(); @@ -479,5 +489,7 @@ class VisionSystemSim { std::unordered_map> targetSets{}; frc::Field2d dbgField{}; const frc::Transform3d kEmptyTrf{}; + const frc::AprilTagFieldLayout tagLayout; + const photon::TargetModel targetModel; }; } // namespace photon diff --git a/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java index 9e8f337462..aa7ee01833 100644 --- a/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java @@ -550,7 +550,10 @@ void pnpDistanceTrigSolve() { var realPose = new Pose3d(7.3, 4.42, 0, new Rotation3d(0, 0, 2.197)); PhotonPipelineResult result = cameraOneSim.process( - 1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets); + 1, + realPose.transformBy(estimator.getRobotToCameraTransform()), + simTargets, + aprilTags); var bestTarget = result.getBestTarget(); assertNotNull(bestTarget); assertEquals(0, bestTarget.fiducialId); @@ -572,7 +575,10 @@ void pnpDistanceTrigSolve() { realPose = new Pose3d(4.81, 2.38, 0, new Rotation3d(0, 0, 2.818)); result = cameraOneSim.process( - 1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets); + 1, + realPose.transformBy(estimator.getRobotToCameraTransform()), + simTargets, + aprilTags); estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d()); estimatedPose = estimator.update(result); diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java index bfc169aa89..9b0b141708 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java @@ -537,7 +537,10 @@ void pnpDistanceTrigSolve() { var realPose = new Pose3d(7.3, 4.42, 0, new Rotation3d(0, 0, 2.197)); PhotonPipelineResult result = cameraOneSim.process( - 1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets); + 1, + realPose.transformBy(estimator.getRobotToCameraTransform()), + simTargets, + aprilTags); var bestTarget = result.getBestTarget(); assertNotNull(bestTarget); assertEquals(0, bestTarget.fiducialId); @@ -559,7 +562,10 @@ void pnpDistanceTrigSolve() { realPose = new Pose3d(4.81, 2.38, 0, new Rotation3d(0, 0, 2.818)); result = cameraOneSim.process( - 1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets); + 1, + realPose.transformBy(estimator.getRobotToCameraTransform()), + simTargets, + aprilTags); estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d()); estimatedPose = estimator.estimatePnpDistanceTrigSolvePose(result); diff --git a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java index 3d85c48257..e711a8b9e8 100644 --- a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java +++ b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java @@ -112,14 +112,16 @@ public void testEmpty() { @Test public void testVisibilityCupidShuffle() { + final var targetModel = new TargetModel(1.0, 1.0); final var targetPose = new Pose3d(new Translation3d(15.98, 0, 2), new Rotation3d(0, 0, Math.PI)); - var visionSysSim = new VisionSystemSim("Test"); + var visionSysSim = + new VisionSystemSim("Test", new AprilTagFieldLayout(new ArrayList<>(), 0, 0), targetModel); var camera = new PhotonCamera(inst, "camera"); var cameraSim = new PhotonCameraSim(camera); visionSysSim.addCamera(cameraSim, new Transform3d()); cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80)); - visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(1.0, 1.0), 3)); + visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, targetModel, 3)); // To the right, to the right var robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(-70)); @@ -181,14 +183,16 @@ public void testVisibilityCupidShuffle() { @Test public void testNotVisibleVert1() { + final var targetModel = new TargetModel(3.0, 3.0); final var targetPose = new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI)); - var visionSysSim = new VisionSystemSim("Test"); + var visionSysSim = + new VisionSystemSim("Test", new AprilTagFieldLayout(new ArrayList<>(), 0, 0), targetModel); var camera = new PhotonCamera(inst, "camera"); var cameraSim = new PhotonCameraSim(camera); visionSysSim.addCamera(cameraSim, new Transform3d()); cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80)); - visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(3.0, 3.0), 3)); + visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, targetModel, 3)); var robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(5)); visionSysSim.update(robotPose); @@ -204,16 +208,18 @@ public void testNotVisibleVert1() { @Test public void testNotVisibleVert2() { + final var targetModel = new TargetModel(0.5, 0.5); final var targetPose = new Pose3d(new Translation3d(15.98, 0, 2), new Rotation3d(0, 0, Math.PI)); var robotToCamera = new Transform3d(new Translation3d(0, 0, 1), new Rotation3d(0, -Math.PI / 4, 0)); - var visionSysSim = new VisionSystemSim("Test"); + var visionSysSim = + new VisionSystemSim("Test", new AprilTagFieldLayout(new ArrayList<>(), 0, 0), targetModel); var camera = new PhotonCamera(inst, "camera"); var cameraSim = new PhotonCameraSim(camera); visionSysSim.addCamera(cameraSim, robotToCamera); cameraSim.prop.setCalibration(1234, 1234, Rotation2d.fromDegrees(80)); - visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(0.5, 0.5), 1736)); + visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, targetModel, 1736)); var robotPose = new Pose2d(new Translation2d(13.98, 0), Rotation2d.fromDegrees(5)); visionSysSim.update(robotPose); @@ -230,15 +236,17 @@ public void testNotVisibleVert2() { @Test public void testNotVisibleTgtSize() { + final var targetModel = new TargetModel(0.1, 0.1); final var targetPose = new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI)); - var visionSysSim = new VisionSystemSim("Test"); + var visionSysSim = + new VisionSystemSim("Test", new AprilTagFieldLayout(new ArrayList<>(), 0, 0), targetModel); var camera = new PhotonCamera(inst, "camera"); var cameraSim = new PhotonCameraSim(camera); visionSysSim.addCamera(cameraSim, new Transform3d()); cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80)); cameraSim.setMinTargetAreaPixels(20.0); - visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(0.1, 0.1), 24)); + visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, targetModel, 24)); var robotPose = new Pose2d(new Translation2d(12, 0), Rotation2d.fromDegrees(5)); visionSysSim.update(robotPose); @@ -253,16 +261,18 @@ public void testNotVisibleTgtSize() { @Test public void testNotVisibleTooFarForLEDs() { + final var targetModel = new TargetModel(1.0, 1.0); final var targetPose = new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI)); - var visionSysSim = new VisionSystemSim("Test"); + var visionSysSim = + new VisionSystemSim("Test", new AprilTagFieldLayout(new ArrayList<>(), 0, 0), targetModel); var camera = new PhotonCamera(inst, "camera"); var cameraSim = new PhotonCameraSim(camera); visionSysSim.addCamera(cameraSim, new Transform3d()); cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80)); cameraSim.setMaxSightRange(10); cameraSim.setMinTargetAreaPixels(1.0); - visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(1.0, 1), 78)); + visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, targetModel, 78)); var robotPose = new Pose2d(new Translation2d(10, 0), Rotation2d.fromDegrees(5)); visionSysSim.update(robotPose); @@ -278,15 +288,17 @@ public void testNotVisibleTooFarForLEDs() { @ParameterizedTest @ValueSource(doubles = {-10, -5, -0, -1, -2, 5, 7, 10.23}) public void testYawAngles(double testYaw) throws InterruptedException { + final var targetModel = new TargetModel(0.5, 0.5); final var targetPose = new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, 3 * Math.PI / 4)); - var visionSysSim = new VisionSystemSim("Test"); + var visionSysSim = + new VisionSystemSim("Test", new AprilTagFieldLayout(new ArrayList<>(), 0, 0), targetModel); var camera = new PhotonCamera(inst, "camera"); var cameraSim = new PhotonCameraSim(camera); visionSysSim.addCamera(cameraSim, new Transform3d()); cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80)); cameraSim.setMinTargetAreaPixels(0.0); - visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(0.5, 0.5), 3)); + visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, targetModel, 3)); // If the robot is rotated x deg (CCW+), the target yaw should be x deg (CW+) var robotPose = new Pose2d(new Translation2d(10, 0), Rotation2d.fromDegrees(testYaw)); @@ -301,16 +313,18 @@ public void testYawAngles(double testYaw) throws InterruptedException { @ParameterizedTest @ValueSource(doubles = {-10, -5, -0, -1, -2, 5, 7, 10.23, 20.21, -19.999}) public void testPitchAngles(double testPitch) throws InterruptedException { + final var targetModel = new TargetModel(0.5, 0.5); final var targetPose = new Pose3d(new Translation3d(15.98, 0, 0), new Rotation3d(0, 0, 3 * Math.PI / 4)); final var robotPose = new Pose2d(new Translation2d(10, 0), new Rotation2d(0)); - var visionSysSim = new VisionSystemSim("Test"); + var visionSysSim = + new VisionSystemSim("Test", new AprilTagFieldLayout(new ArrayList<>(), 0, 0), targetModel); var camera = new PhotonCamera(inst, "camera"); var cameraSim = new PhotonCameraSim(camera); visionSysSim.addCamera(cameraSim, new Transform3d()); cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(120)); cameraSim.setMinTargetAreaPixels(0.0); - visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(0.5, 0.5), 3)); + visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, targetModel, 3)); // Transform is now robot -> camera visionSysSim.adjustCamera( @@ -361,6 +375,7 @@ public void testDistanceCalc(double testDist, double testPitch, double testHeigh // Assume dist along ground and tgt height the same. Iterate over other // parameters. + final var targetModel = new TargetModel(0.5, 0.5); final var targetPose = new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI * 0.98)); final var robotPose = @@ -372,14 +387,16 @@ public void testDistanceCalc(double testDist, double testPitch, double testHeigh var visionSysSim = new VisionSystemSim( - "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysohowsyourdaygoingihopegoodhaveagreatrestofyourlife!"); + "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysohowsyourdaygoingihopegoodhaveagreatrestofyourlife!", + new AprilTagFieldLayout(new ArrayList<>(), 0, 0), + targetModel); var camera = new PhotonCamera(inst, "camera"); var cameraSim = new PhotonCameraSim(camera); visionSysSim.addCamera(cameraSim, new Transform3d()); cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(160)); cameraSim.setMinTargetAreaPixels(0.0); visionSysSim.adjustCamera(cameraSim, robotToCamera); - visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(0.5, 0.5), 0)); + visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, targetModel, 0)); visionSysSim.update(robotPose); @@ -416,7 +433,9 @@ public void testMultipleTargets() { final var targetPoseR = new Pose3d(new Translation3d(15.98, -2, 0), new Rotation3d(0, 0, Math.PI)); - var visionSysSim = new VisionSystemSim("Test"); + var visionSysSim = + new VisionSystemSim( + "Test", new AprilTagFieldLayout(new ArrayList<>(), 0, 0), TargetModel.kAprilTag16h5); var camera = new PhotonCamera(inst, "camera"); var cameraSim = new PhotonCameraSim(camera); visionSysSim.addCamera(cameraSim, new Transform3d()); @@ -503,13 +522,6 @@ public void testMultipleTargets() { @Test public void testPoseEstimation() { - var visionSysSim = new VisionSystemSim("Test"); - var camera = new PhotonCamera(inst, "camera"); - var cameraSim = new PhotonCameraSim(camera); - visionSysSim.addCamera(cameraSim, new Transform3d()); - cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(90)); - cameraSim.setMinTargetAreaPixels(20.0); - List tagList = new ArrayList<>(); tagList.add(new AprilTag(0, new Pose3d(12, 3, 1, new Rotation3d(0, 0, Math.PI)))); tagList.add(new AprilTag(1, new Pose3d(12, 1, -1, new Rotation3d(0, 0, Math.PI)))); @@ -517,6 +529,14 @@ public void testPoseEstimation() { double fieldLength = Units.feetToMeters(54.0); double fieldWidth = Units.feetToMeters(27.0); AprilTagFieldLayout layout = new AprilTagFieldLayout(tagList, fieldLength, fieldWidth); + + var visionSysSim = new VisionSystemSim("Test", layout, TargetModel.kAprilTag16h5); + var camera = new PhotonCamera(inst, "camera"); + var cameraSim = new PhotonCameraSim(camera); + visionSysSim.addCamera(cameraSim, new Transform3d()); + cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(90)); + cameraSim.setMinTargetAreaPixels(20.0); + Pose2d robotPose = new Pose2d(5, 1, Rotation2d.fromDegrees(5)); visionSysSim.addVisionTargets( diff --git a/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp index 0445be6b1e..445eb79f4c 100644 --- a/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp @@ -337,7 +337,7 @@ TEST(LegacyPhotonPoseEstimatorTest, PnpDistanceTrigSolve) { photon::PhotonPipelineResult result = cameraOneSim.Process( 1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()), - targets); + targets, aprilTags); cameraOne.testResult = {result}; cameraOne.testResult[0].SetReceiveTimestamp(17_s); @@ -367,7 +367,7 @@ TEST(LegacyPhotonPoseEstimatorTest, PnpDistanceTrigSolve) { frc::Rotation3d(0_rad, 0_rad, 2.818_rad)); result = cameraOneSim.Process( 1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()), - targets); + targets, aprilTags); cameraOne.testResult = {result}; cameraOne.testResult[0].SetReceiveTimestamp(18_s); diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index 5d5caeade6..6afe2b9cb2 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -330,7 +330,7 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) { photon::PhotonPipelineResult result = cameraOneSim.Process( 1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()), - targets); + targets, aprilTags); cameraOne.testResult = {result}; cameraOne.testResult[0].SetReceiveTimestamp(17_s); @@ -360,7 +360,7 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) { frc::Rotation3d(0_rad, 0_rad, 2.818_rad)); result = cameraOneSim.Process( 1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()), - targets); + targets, aprilTags); cameraOne.testResult = {result}; cameraOne.testResult[0].SetReceiveTimestamp(18_s); diff --git a/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp index 501baa9c51..2dacf80829 100644 --- a/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp +++ b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp @@ -55,17 +55,19 @@ class VisionSystemSimTestDistanceParamsTest std::tuple> {}; TEST_F(VisionSystemSimTest, TestVisibilityCupidShuffle) { + photon::TargetModel targetModel{1.0_m, 1.0_m}; frc::Pose3d targetPose{ frc::Translation3d{15.98_m, 0_m, 2_m}, frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; - photon::VisionSystemSim visionSysSim{"Test"}; + photon::VisionSystemSim visionSysSim{"Test", frc::AprilTagFieldLayout(), + targetModel}; photon::PhotonCamera camera{"camera"}; photon::PhotonCameraSim cameraSim{&camera}; visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); - visionSysSim.AddVisionTargets({photon::VisionTargetSim{ - targetPose, photon::TargetModel{1.0_m, 1.0_m}, 3}}); + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{targetPose, targetModel, 3}}); // To the right, to the right frc::Pose2d robotPose{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{-70_deg}}; @@ -118,17 +120,19 @@ TEST_F(VisionSystemSimTest, TestVisibilityCupidShuffle) { } TEST_F(VisionSystemSimTest, TestNotVisibleVert1) { + photon::TargetModel targetModel{3.0_m, 3.0_m}; frc::Pose3d targetPose{ frc::Translation3d{15.98_m, 0_m, 1_m}, frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; - photon::VisionSystemSim visionSysSim{"Test"}; + photon::VisionSystemSim visionSysSim{"Test", frc::AprilTagFieldLayout(), + targetModel}; photon::PhotonCamera camera{"camera"}; photon::PhotonCameraSim cameraSim{&camera}; visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); - visionSysSim.AddVisionTargets({photon::VisionTargetSim{ - targetPose, photon::TargetModel{3.0_m, 3.0_m}, 3}}); + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{targetPose, targetModel, 3}}); frc::Pose2d robotPose{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{5_deg}}; visionSysSim.Update(robotPose); @@ -144,6 +148,7 @@ TEST_F(VisionSystemSimTest, TestNotVisibleVert1) { } TEST_F(VisionSystemSimTest, TestNotVisibleVert2) { + photon::TargetModel targetModel{0.5_m, 0.5_m}; frc::Pose3d targetPose{ frc::Translation3d{15.98_m, 0_m, 2_m}, frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; @@ -152,13 +157,14 @@ TEST_F(VisionSystemSimTest, TestNotVisibleVert2) { frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{0_rad, units::radian_t{-std::numbers::pi / 4}, 0_rad}}; - photon::VisionSystemSim visionSysSim{"Test"}; + photon::VisionSystemSim visionSysSim{"Test", frc::AprilTagFieldLayout(), + targetModel}; photon::PhotonCamera camera{"camera"}; photon::PhotonCameraSim cameraSim{&camera}; visionSysSim.AddCamera(&cameraSim, robotToCamera); cameraSim.prop.SetCalibration(1234, 1234, frc::Rotation2d{80_deg}); - visionSysSim.AddVisionTargets({photon::VisionTargetSim{ - targetPose, photon::TargetModel{0.5_m, 0.5_m}, 1736}}); + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{targetPose, targetModel, 1736}}); frc::Pose2d robotPose{frc::Translation2d{13.98_m, 0_m}, frc::Rotation2d{5_deg}}; @@ -171,18 +177,20 @@ TEST_F(VisionSystemSimTest, TestNotVisibleVert2) { } TEST_F(VisionSystemSimTest, TestNotVisibleTargetSize) { + photon::TargetModel targetModel{0.1_m, 0.1_m}; frc::Pose3d targetPose{ frc::Translation3d{15.98_m, 0_m, 1_m}, frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; - photon::VisionSystemSim visionSysSim{"Test"}; + photon::VisionSystemSim visionSysSim{"Test", frc::AprilTagFieldLayout(), + targetModel}; photon::PhotonCamera camera{"camera"}; photon::PhotonCameraSim cameraSim{&camera}; visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); cameraSim.SetMinTargetAreaPixels(20.0); - visionSysSim.AddVisionTargets({photon::VisionTargetSim{ - targetPose, photon::TargetModel{0.1_m, 0.1_m}, 24}}); + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{targetPose, targetModel, 24}}); frc::Pose2d robotPose{frc::Translation2d{12_m, 0_m}, frc::Rotation2d{5_deg}}; visionSysSim.Update(robotPose); @@ -194,11 +202,13 @@ TEST_F(VisionSystemSimTest, TestNotVisibleTargetSize) { } TEST_F(VisionSystemSimTest, TestNotVisibleTooFarLeds) { + photon::TargetModel targetModel{1_m, 1_m}; frc::Pose3d targetPose{ frc::Translation3d{15.98_m, 0_m, 1_m}, frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; - photon::VisionSystemSim visionSysSim{"Test"}; + photon::VisionSystemSim visionSysSim{"Test", frc::AprilTagFieldLayout(), + targetModel}; photon::PhotonCamera camera{"camera"}; photon::PhotonCameraSim cameraSim{&camera}; visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); @@ -206,7 +216,7 @@ TEST_F(VisionSystemSimTest, TestNotVisibleTooFarLeds) { cameraSim.SetMinTargetAreaPixels(1.0); cameraSim.SetMaxSightRange(10_m); visionSysSim.AddVisionTargets( - {photon::VisionTargetSim{targetPose, photon::TargetModel{1_m, 1_m}, 25}}); + {photon::VisionTargetSim{targetPose, targetModel, 25}}); frc::Pose2d robotPose{frc::Translation2d{10_m, 0_m}, frc::Rotation2d{5_deg}}; visionSysSim.Update(robotPose); @@ -218,17 +228,19 @@ TEST_F(VisionSystemSimTest, TestNotVisibleTooFarLeds) { } TEST_P(VisionSystemSimTestWithParamsTest, YawAngles) { + photon::TargetModel targetModel{0.5_m, 0.5_m}; const frc::Pose3d targetPose{ {15.98_m, 0_m, 0_m}, frc::Rotation3d{0_deg, 0_deg, units::radian_t{3 * std::numbers::pi / 4}}}; - photon::VisionSystemSim visionSysSim{"Test"}; + photon::VisionSystemSim visionSysSim{"Test", frc::AprilTagFieldLayout(), + targetModel}; photon::PhotonCamera camera{"camera"}; photon::PhotonCameraSim cameraSim{&camera}; visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); cameraSim.SetMinTargetAreaPixels(0.0); - visionSysSim.AddVisionTargets({photon::VisionTargetSim{ - targetPose, photon::TargetModel{0.5_m, 0.5_m}, 3}}); + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{targetPose, targetModel, 3}}); // If the robot is rotated x deg (CCW+), the target yaw should be x deg (CW+) frc::Pose2d robotPose{frc::Translation2d{10_m, 0_m}, @@ -241,18 +253,20 @@ TEST_P(VisionSystemSimTestWithParamsTest, YawAngles) { } TEST_P(VisionSystemSimTestWithParamsTest, PitchAngles) { + photon::TargetModel targetModel{0.5_m, 0.5_m}; const frc::Pose3d targetPose{ {15.98_m, 0_m, 0_m}, frc::Rotation3d{0_deg, 0_deg, units::radian_t{3 * std::numbers::pi / 4}}}; frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{GetParam() * -1.0}}; - photon::VisionSystemSim visionSysSim{"Test"}; + photon::VisionSystemSim visionSysSim{"Test", frc::AprilTagFieldLayout(), + targetModel}; photon::PhotonCamera camera{"camera"}; photon::PhotonCameraSim cameraSim{&camera}; visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{120_deg}); cameraSim.SetMinTargetAreaPixels(0.0); - visionSysSim.AddVisionTargets({photon::VisionTargetSim{ - targetPose, photon::TargetModel{0.5_m, 0.5_m}, 3}}); + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{targetPose, targetModel, 3}}); robotPose = frc::Pose2d{frc::Translation2d{10_m, 0_m}, frc::Rotation2d{-1 * GetParam()}}; @@ -278,6 +292,7 @@ TEST_P(VisionSystemSimTestDistanceParamsTest, DistanceCalc) { units::foot_t heightParam; std::tie(distParam, pitchParam, heightParam) = GetParam(); + photon::TargetModel targetModel{0.5_m, 0.5_m}; const frc::Pose3d targetPose{ {15.98_m, 0_m, 1_m}, frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi * 0.98}}}; @@ -286,15 +301,16 @@ TEST_P(VisionSystemSimTestDistanceParamsTest, DistanceCalc) { frc::Rotation3d{0_deg, pitchParam, 0_deg}}; photon::VisionSystemSim visionSysSim{ "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysoho" - "wsyourdaygoingihopegoodhaveagreatrestofyourlife"}; + "wsyourdaygoingihopegoodhaveagreatrestofyourlife", + frc::AprilTagFieldLayout(), targetModel}; photon::PhotonCamera camera{"camera"}; photon::PhotonCameraSim cameraSim{&camera}; visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{160_deg}); cameraSim.SetMinTargetAreaPixels(0.0); visionSysSim.AdjustCamera(&cameraSim, robotToCamera); - visionSysSim.AddVisionTargets({photon::VisionTargetSim{ - targetPose, photon::TargetModel{0.5_m, 0.5_m}, 0}}); + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{targetPose, targetModel, 0}}); visionSysSim.Update(robotPose); photon::PhotonPipelineResult res = camera.GetLatestResult(); @@ -341,7 +357,8 @@ TEST_F(VisionSystemSimTest, TestMultipleTargets) { frc::Translation3d{15.98_m, -2_m, 0_m}, frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; - photon::VisionSystemSim visionSysSim{"Test"}; + photon::VisionSystemSim visionSysSim{"Test", frc::AprilTagFieldLayout(), + photon::kAprilTag16h5}; photon::PhotonCamera camera{"camera"}; photon::PhotonCameraSim cameraSim{&camera}; visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); @@ -402,13 +419,6 @@ TEST_F(VisionSystemSimTest, TestMultipleTargets) { } TEST_F(VisionSystemSimTest, TestPoseEstimation) { - photon::VisionSystemSim visionSysSim{"Test"}; - photon::PhotonCamera camera{"camera"}; - photon::PhotonCameraSim cameraSim{&camera}; - visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); - cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{90_deg}); - cameraSim.SetMinTargetAreaPixels(20.0); - std::vector tagList; tagList.emplace_back(frc::AprilTag{ 0, frc::Pose3d{12_m, 3_m, 1_m, @@ -425,6 +435,13 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) { units::meter_t fieldLength{54}; units::meter_t fieldWidth{27}; frc::AprilTagFieldLayout layout{tagList, fieldLength, fieldWidth}; + photon::VisionSystemSim visionSysSim{"Test", layout, photon::kAprilTag16h5}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{90_deg}); + cameraSim.SetMinTargetAreaPixels(20.0); + frc::Pose2d robotPose{frc::Translation2d{5_m, 1_m}, frc::Rotation2d{5_deg}}; visionSysSim.AddVisionTargets( {photon::VisionTargetSim{tagList[0].pose, photon::kAprilTag16h5, 0}}); @@ -476,13 +493,6 @@ TEST_F(VisionSystemSimTest, TestPoseEstimationRotated) { frc::Transform3d robotToCamera{frc::Translation3d{6_in, 6_in, 6_in}, frc::Rotation3d{0_deg, -30_deg, 25.5_deg}}; - photon::VisionSystemSim visionSysSim{"Test"}; - photon::PhotonCamera camera{"cameraRotated"}; - photon::PhotonCameraSim cameraSim{&camera}; - visionSysSim.AddCamera(&cameraSim, robotToCamera); - cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{90_deg}); - cameraSim.SetMinTargetAreaPixels(20.0); - std::vector tagList; tagList.emplace_back(frc::AprilTag{ 0, frc::Pose3d{12_m, 3_m, 1_m, @@ -499,6 +509,14 @@ TEST_F(VisionSystemSimTest, TestPoseEstimationRotated) { units::meter_t fieldLength{54}; units::meter_t fieldWidth{27}; frc::AprilTagFieldLayout layout{tagList, fieldLength, fieldWidth}; + + photon::VisionSystemSim visionSysSim{"Test", layout}; + photon::PhotonCamera camera{"cameraRotated"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, robotToCamera); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{90_deg}); + cameraSim.SetMinTargetAreaPixels(20.0); + frc::Pose2d robotPose{frc::Translation2d{5_m, 1_m}, frc::Rotation2d{-5_deg}}; visionSysSim.AddVisionTargets( {photon::VisionTargetSim{tagList[0].pose, photon::kAprilTag36h11, 0}}); @@ -553,7 +571,7 @@ TEST_F(VisionSystemSimTest, TestPoseEstimationRotated) { } TEST_F(VisionSystemSimTest, TestTagAmbiguity) { - photon::VisionSystemSim visionSysSim{"Test"}; + photon::VisionSystemSim visionSysSim{"Test", frc::AprilTagFieldLayout()}; photon::PhotonCamera camera{"camera"}; photon::PhotonCameraSim cameraSim{&camera}; visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});