diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index a67438949a..6dbb6811e2 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -273,19 +273,6 @@ public void setLED(VisionLEDMode led) { ledModeRequest.set(led.value); } - /** - * Returns whether the latest target result has targets. - * - *

This method is deprecated; {@link PhotonPipelineResult#hasTargets()} should be used instead. - * - * @deprecated This method should be replaced with {@link PhotonPipelineResult#hasTargets()} - * @return Whether the latest target result has targets. - */ - @Deprecated - public boolean hasTargets() { - return getLatestResult().hasTargets(); - } - /** * Returns the name of the camera. This will return the same value that was given to the * constructor as cameraName. diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java b/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java deleted file mode 100644 index d6314afd6c..0000000000 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java +++ /dev/null @@ -1,179 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package org.photonvision.simulation; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.numbers.N5; -import edu.wpi.first.networktables.NetworkTableInstance; -import java.util.Arrays; -import java.util.List; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonTargetSortMode; -import org.photonvision.common.networktables.NTTopicSet; -import org.photonvision.targeting.MultiTargetPNPResult; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; - -/** - * @deprecated Use {@link PhotonCameraSim} instead - */ -@Deprecated -@SuppressWarnings("unused") -public class SimPhotonCamera { - NTTopicSet ts = new NTTopicSet(); - PhotonPipelineResult latestResult; - private long heartbeatCounter = 0; - - /** - * Constructs a Simulated PhotonCamera from a root table. - * - * @param instance The NetworkTableInstance to pull data from. This can be a custom instance in - * simulation, but should *usually* be the default NTInstance from - * NetworkTableInstance::getDefault - * @param cameraName The name of the camera, as seen in the UI. - */ - public SimPhotonCamera(NetworkTableInstance instance, String cameraName) { - ts.removeEntries(); - ts.subTable = instance.getTable(PhotonCamera.kTableName).getSubTable(cameraName); - ts.updateEntries(); - } - - /** - * Publishes the camera intrinsics matrix. The matrix should be in the form: spotless:off - * fx 0 cx - * 0 fy cy - * 0 0 1 - * @param cameraMatrix The cam matrix - * spotless:on - */ - public void setCameraIntrinsicsMat(Matrix cameraMatrix) { - ts.cameraIntrinsicsPublisher.set(cameraMatrix.getData()); - } - - /** - * Publishes the camera distortion matrix. The matrix should be in the form [k1 k2 p1 p2 k3]. See - * more: https://docs.opencv.org/3.4/d4/d94/tutorial_camera_calibration.html - * - * @param distortionMat The distortion mat - */ - public void setCameraDistortionMat(Matrix distortionMat) { - ts.cameraDistortionPublisher.set(distortionMat.getData()); - } - - /** - * Constructs a Simulated PhotonCamera from the name of the camera. - * - * @param cameraName The nickname of the camera (found in the PhotonVision UI). - */ - public SimPhotonCamera(String cameraName) { - this(NetworkTableInstance.getDefault(), cameraName); - } - - /** - * Simulate one processed frame of vision data, putting one result to NT. - * - * @param latencyMillis Latency of the provided frame - * @param targets Each target detected - */ - public void submitProcessedFrame(double latencyMillis, PhotonTrackedTarget... targets) { - submitProcessedFrame(latencyMillis, Arrays.asList(targets)); - } - - /** - * Simulate one processed frame of vision data, putting one result to NT. - * - * @param latencyMillis Latency of the provided frame - * @param sortMode Order in which to sort targets - * @param targets Each target detected - */ - public void submitProcessedFrame( - double latencyMillis, PhotonTargetSortMode sortMode, PhotonTrackedTarget... targets) { - submitProcessedFrame(latencyMillis, sortMode, Arrays.asList(targets)); - } - - /** - * Simulate one processed frame of vision data, putting one result to NT. - * - * @param latencyMillis Latency of the provided frame - * @param targetList List of targets detected - */ - public void submitProcessedFrame(double latencyMillis, List targetList) { - submitProcessedFrame(latencyMillis, null, targetList); - } - - /** - * Simulate one processed frame of vision data, putting one result to NT. - * - * @param latencyMillis Latency of the provided frame - * @param sortMode Order in which to sort targets - * @param targetList List of targets detected - */ - public void submitProcessedFrame( - double latencyMillis, PhotonTargetSortMode sortMode, List targetList) { - ts.latencyMillisEntry.set(latencyMillis); - - if (sortMode != null) { - targetList.sort(sortMode.getComparator()); - } - - PhotonPipelineResult newResult = - new PhotonPipelineResult(latencyMillis, targetList, new MultiTargetPNPResult()); - - ts.resultPublisher.set(newResult, newResult.getPacketSize()); - - boolean hasTargets = newResult.hasTargets(); - ts.hasTargetEntry.set(hasTargets); - if (!hasTargets) { - ts.targetPitchEntry.set(0.0); - ts.targetYawEntry.set(0.0); - ts.targetAreaEntry.set(0.0); - ts.targetPoseEntry.set(new double[] {0.0, 0.0, 0.0}); - ts.targetSkewEntry.set(0.0); - } else { - var bestTarget = newResult.getBestTarget(); - - ts.targetPitchEntry.set(bestTarget.getPitch()); - ts.targetYawEntry.set(bestTarget.getYaw()); - ts.targetAreaEntry.set(bestTarget.getArea()); - ts.targetSkewEntry.set(bestTarget.getSkew()); - - var transform = bestTarget.getBestCameraToTarget(); - double[] poseData = { - transform.getX(), transform.getY(), transform.getRotation().toRotation2d().getDegrees() - }; - ts.targetPoseEntry.set(poseData); - } - - ts.heartbeatPublisher.set(heartbeatCounter++); - - latestResult = newResult; - } - - PhotonPipelineResult getLatestResult() { - return latestResult; - } -} diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java b/photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java deleted file mode 100644 index df16412de5..0000000000 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java +++ /dev/null @@ -1,271 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package org.photonvision.simulation; - -import edu.wpi.first.apriltag.AprilTag; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import java.util.ArrayList; -import java.util.List; -import org.photonvision.targeting.PhotonTrackedTarget; -import org.photonvision.targeting.TargetCorner; - -/** - * @deprecated Use {@link VisionSystemSim} instead - */ -@Deprecated -public class SimVisionSystem { - SimPhotonCamera cam; - - double camHorizFOVDegrees; - double camVertFOVDegrees; - double cameraHeightOffGroundMeters; - double maxLEDRangeMeters; - int cameraResWidth; - int cameraResHeight; - double minTargetArea; - Transform3d robotToCamera; - - Field2d dbgField; - FieldObject2d dbgRobot; - FieldObject2d dbgCamera; - - ArrayList tgtList; - - /** - * Create a simulated vision system involving a camera and coprocessor mounted on a mobile robot - * running PhotonVision, detecting one or more targets scattered around the field. This assumes a - * fairly simple and distortion-less pinhole camera model. - * - * @param camName Name of the PhotonVision camera to create. Align it with the settings you use in - * the PhotonVision GUI. - * @param camDiagFOVDegrees Diagonal Field of View of the camera used. Align it with the - * manufacturer specifications, and/or whatever is configured in the PhotonVision Setting - * page. - * @param robotToCamera Transform to move from the center of the robot to the camera's mount - * position - * @param maxLEDRangeMeters Maximum distance at which your camera can illuminate the target and - * make it visible. Set to 9000 or more if your vision system does not rely on LED's. - * @param cameraResWidth Width of your camera's image sensor in pixels - * @param cameraResHeight Height of your camera's image sensor in pixels - * @param minTargetArea Minimum area that that the target should be before it's recognized as a - * target by the camera. Match this with your contour filtering settings in the PhotonVision - * GUI. - */ - public SimVisionSystem( - String camName, - double camDiagFOVDegrees, - Transform3d robotToCamera, - double maxLEDRangeMeters, - int cameraResWidth, - int cameraResHeight, - double minTargetArea) { - this.robotToCamera = robotToCamera; - this.maxLEDRangeMeters = maxLEDRangeMeters; - this.cameraResWidth = cameraResWidth; - this.cameraResHeight = cameraResHeight; - this.minTargetArea = minTargetArea; - - // Calculate horizontal/vertical FOV by similar triangles - double hypotPixels = Math.hypot(cameraResWidth, cameraResHeight); - this.camHorizFOVDegrees = camDiagFOVDegrees * cameraResWidth / hypotPixels; - this.camVertFOVDegrees = camDiagFOVDegrees * cameraResHeight / hypotPixels; - - cam = new SimPhotonCamera(camName); - tgtList = new ArrayList<>(); - - dbgField = new Field2d(); - dbgRobot = dbgField.getRobotObject(); - dbgCamera = dbgField.getObject(camName + " Camera"); - SmartDashboard.putData(camName + " Sim Field", dbgField); - } - - /** - * Add a target on the field which your vision system is designed to detect. The PhotonCamera from - * this system will report the location of the robot relative to the subset of these targets which - * are visible from the given robot position. - * - * @param target Target to add to the simulated field - */ - public void addSimVisionTarget(SimVisionTarget target) { - tgtList.add(target); - dbgField.getObject("Target " + target.targetID).setPose(target.targetPose.toPose2d()); - } - - /** - * Adds all apriltags from the provided {@link AprilTagFieldLayout} as sim vision targets. The - * poses added will preserve the tag layout's alliance origin at the time of calling this method. - * - * @param tagLayout The field tag layout to get Apriltag poses and IDs from - */ - public void addVisionTargets(AprilTagFieldLayout tagLayout) { - for (AprilTag tag : tagLayout.getTags()) { - addSimVisionTarget( - new SimVisionTarget( - tagLayout.getTagPose(tag.ID).get(), // preserve alliance rotation - Units.inchesToMeters(6), - Units.inchesToMeters(6), - tag.ID)); - } - } - - /** - * Clears all sim vision targets. This is useful for switching alliances and needing to repopulate - * the sim targets. NOTE: Old targets will still show on the Field2d unless overwritten by new - * targets with the same ID - */ - public void clearVisionTargets() { - tgtList.clear(); - } - - /** - * Adjust the camera position relative to the robot. Use this if your camera is on a gimbal or - * turret or some other mobile platform. - * - * @param newRobotToCamera New Transform from the robot to the camera - */ - public void moveCamera(Transform3d newRobotToCamera) { - this.robotToCamera = newRobotToCamera; - } - - /** - * Periodic update. Call this once per frame of image data you wish to process and send to - * NetworkTables - * - * @param robotPoseMeters current pose of the robot on the field. Will be used to calculate which - * targets are actually in view, where they are at relative to the robot, and relevant - * PhotonVision parameters. - */ - public void processFrame(Pose2d robotPoseMeters) { - processFrame(new Pose3d(robotPoseMeters)); - } - - /** - * Periodic update. Call this once per frame of image data you wish to process and send to - * NetworkTables - * - * @param robotPoseMeters current pose of the robot in space. Will be used to calculate which - * targets are actually in view, where they are at relative to the robot, and relevant - * PhotonVision parameters. - */ - public void processFrame(Pose3d robotPoseMeters) { - Pose3d cameraPose = robotPoseMeters.transformBy(robotToCamera); - - dbgRobot.setPose(robotPoseMeters.toPose2d()); - dbgCamera.setPose(cameraPose.toPose2d()); - - ArrayList visibleTgtList = new ArrayList<>(tgtList.size()); - - tgtList.forEach( - (tgt) -> { - var camToTargetTrans = new Transform3d(cameraPose, tgt.targetPose); - - // Generate a transformation from camera to target, - // ignoring rotation. - var t = camToTargetTrans.getTranslation(); - - // Rough approximation of the alternate solution, which is (so far) always incorrect. - var altTrans = - new Translation3d( - t.getX(), - -1.0 * t.getY(), - t.getZ()); // mirrored across camera axis in Y direction - var altRot = camToTargetTrans.getRotation().times(-1.0); // flipped - var camToTargetTransAlt = new Transform3d(altTrans, altRot); - - double distMeters = t.getNorm(); - - double area_px = tgt.tgtAreaMeters2 / getM2PerPx(distMeters); - - var translationAlongGround = - new Translation2d( - tgt.targetPose.toPose2d().getX() - cameraPose.toPose2d().getX(), - tgt.targetPose.toPose2d().getY() - cameraPose.toPose2d().getY()); - - var camAngle = cameraPose.getRotation().toRotation2d(); - var camToTgtRotation = - new Rotation2d(translationAlongGround.getX(), translationAlongGround.getY()); - double yawDegrees = camToTgtRotation.minus(camAngle).getDegrees(); - - double camHeightAboveGround = cameraPose.getZ(); - double tgtHeightAboveGround = tgt.targetPose.getZ(); - double camPitchDegrees = Units.radiansToDegrees(cameraPose.getRotation().getY()); - - double distAlongGround = translationAlongGround.getNorm(); - - double pitchDegrees = - Units.radiansToDegrees( - Math.atan2((tgtHeightAboveGround - camHeightAboveGround), distAlongGround)) - - camPitchDegrees; - - if (camCanSeeTarget(distMeters, yawDegrees, pitchDegrees, area_px)) { - // TODO simulate target corners - visibleTgtList.add( - new PhotonTrackedTarget( - yawDegrees, - pitchDegrees, - area_px, - 0.0, - tgt.targetID, - camToTargetTrans, - camToTargetTransAlt, - 0.0, // TODO - simulate ambiguity when straight on? - List.of( - new TargetCorner(0, 0), new TargetCorner(0, 0), - new TargetCorner(0, 0), new TargetCorner(0, 0)), - List.of( - new TargetCorner(0, 0), new TargetCorner(0, 0), - new TargetCorner(0, 0), new TargetCorner(0, 0)))); - } - }); - - cam.submitProcessedFrame(0.0, visibleTgtList); - } - - double getM2PerPx(double dist) { - double widthMPerPx = - 2 * dist * Math.tan(Units.degreesToRadians(this.camHorizFOVDegrees) / 2) / cameraResWidth; - double heightMPerPx = - 2 * dist * Math.tan(Units.degreesToRadians(this.camVertFOVDegrees) / 2) / cameraResHeight; - return widthMPerPx * heightMPerPx; - } - - boolean camCanSeeTarget(double distMeters, double yaw, double pitch, double area) { - boolean inRange = (distMeters < this.maxLEDRangeMeters); - boolean inHorizAngle = Math.abs(yaw) < (this.camHorizFOVDegrees / 2); - boolean inVertAngle = Math.abs(pitch) < (this.camVertFOVDegrees / 2); - boolean targetBigEnough = area > this.minTargetArea; - return (inRange && inHorizAngle && inVertAngle && targetBigEnough); - } -} diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimVisionTarget.java b/photon-lib/src/main/java/org/photonvision/simulation/SimVisionTarget.java deleted file mode 100644 index 0d66154d9f..0000000000 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimVisionTarget.java +++ /dev/null @@ -1,55 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package org.photonvision.simulation; - -import edu.wpi.first.math.geometry.Pose3d; - -/** - * @deprecated Use {@link VisionTargetSim} instead - */ -@Deprecated -public class SimVisionTarget { - Pose3d targetPose; - double targetWidthMeters; - double targetHeightMeters; - double tgtAreaMeters2; - int targetID; - - /** - * Describes a vision target located somewhere on the field that your SimVisionSystem can detect. - * - * @param targetPos Pose3d of the target in field-relative coordinates - * @param targetWidthMeters Width of the outer bounding box of the target in meters. - * @param targetHeightMeters Pair Height of the outer bounding box of the target in meters. - */ - public SimVisionTarget( - Pose3d targetPos, double targetWidthMeters, double targetHeightMeters, int targetID) { - this.targetPose = targetPos; - this.targetWidthMeters = targetWidthMeters; - this.targetHeightMeters = targetHeightMeters; - this.tgtAreaMeters2 = targetWidthMeters * targetHeightMeters; - this.targetID = targetID; - } -} diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index 99aa52c61b..d9cf2574e6 100644 --- a/photon-lib/src/main/native/include/photon/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -37,7 +37,6 @@ #include #include #include -#include #include "photon/targeting//PhotonPipelineResult.h" @@ -152,18 +151,6 @@ class PhotonCamera { std::optional GetCameraMatrix(); std::optional GetDistCoeffs(); - /** - * Returns whether the latest target result has targets. - * This method is deprecated; {@link PhotonPipelineResult#hasTargets()} should - * be used instead. - * @deprecated This method should be replaced with {@link - * PhotonPipelineResult#HasTargets()} - * @return Whether the latest target result has targets. - */ - WPI_DEPRECATED( - "This method should be replaced with PhotonPipelineResult::HasTargets()") - bool HasTargets() { return GetLatestResult().HasTargets(); } - inline static void SetVersionCheckEnabled(bool enabled) { PhotonCamera::VERSION_CHECK_ENABLED = enabled; }