diff --git a/photon-lib/src/main/java/org/photonvision/PhotonTargetSortMode.java b/photon-lib/src/main/java/org/photonvision/PhotonTargetSortMode.java new file mode 100644 index 000000000..a173cd5cf --- /dev/null +++ b/photon-lib/src/main/java/org/photonvision/PhotonTargetSortMode.java @@ -0,0 +1,43 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonvision; + +import java.util.Comparator; +import org.photonvision.targeting.PhotonTrackedTarget; + +public enum PhotonTargetSortMode { + Smallest(Comparator.comparingDouble(PhotonTrackedTarget::getArea)), + Largest(Smallest.m_comparator.reversed()), + Highest(Comparator.comparingDouble(PhotonTrackedTarget::getPitch)), + Lowest(Highest.m_comparator.reversed()), + Rightmost(Comparator.comparingDouble(PhotonTrackedTarget::getYaw)), + Leftmost(Rightmost.m_comparator.reversed()), + Centermost( + Comparator.comparingDouble( + target -> (Math.pow(target.getPitch(), 2) + Math.pow(target.getYaw(), 2)))); + + private final Comparator m_comparator; + + PhotonTargetSortMode(Comparator comparator) { + m_comparator = comparator; + } + + public Comparator getComparator() { + return m_comparator; + } +} diff --git a/photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java b/photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java index 29bdf855e..a972c82d7 100644 --- a/photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java @@ -18,13 +18,24 @@ package org.photonvision; import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; import java.util.Arrays; import java.util.List; import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; +@SuppressWarnings("unused") public class SimPhotonCamera extends PhotonCamera { + private final NetworkTableEntry latencyMillisEntry; + private final NetworkTableEntry hasTargetEntry; + private final NetworkTableEntry targetPitchEntry; + private final NetworkTableEntry targetYawEntry; + private final NetworkTableEntry targetAreaEntry; + private final NetworkTableEntry targetSkewEntry; + private final NetworkTableEntry targetPoseEntry; + /** * Constructs a Simulated PhotonCamera from a root table. * @@ -32,6 +43,14 @@ public class SimPhotonCamera extends PhotonCamera { */ public SimPhotonCamera(NetworkTable rootTable) { super(rootTable); + + latencyMillisEntry = rootTable.getEntry("latencyMillis"); + hasTargetEntry = rootTable.getEntry("hasTargetEntry"); + targetPitchEntry = rootTable.getEntry("targetPitchEntry"); + targetYawEntry = rootTable.getEntry("targetYawEntry"); + targetAreaEntry = rootTable.getEntry("targetAreaEntry"); + targetSkewEntry = rootTable.getEntry("targetSkewEntry"); + targetPoseEntry = rootTable.getEntry("targetPoseEntry"); } /** @@ -40,13 +59,13 @@ public class SimPhotonCamera extends PhotonCamera { * @param cameraName The nickname of the camera (found in the PhotonVision UI). */ public SimPhotonCamera(String cameraName) { - super(cameraName); + this(NetworkTableInstance.getDefault().getTable("photonvision").getSubTable(cameraName)); } /** * Simulate one processed frame of vision data, putting one result to NT. * - * @param latencyMillis + * @param latencyMillis Latency of the provided frame * @param targets Each target detected */ public void submitProcessedFrame(double latencyMillis, PhotonTrackedTarget... targets) { @@ -56,15 +75,66 @@ public class SimPhotonCamera extends PhotonCamera { /** * Simulate one processed frame of vision data, putting one result to NT. * - * @param latencyMillis - * @param tgtList List of targets detected + * @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, List tgtList) { - if (!getDriverMode()) { - PhotonPipelineResult newResult = new PhotonPipelineResult(latencyMillis, tgtList); - var newPacket = new Packet(newResult.getPacketSize()); - newResult.populatePacket(newPacket); - rawBytesEntry.setRaw(newPacket.getData()); + 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) { + latencyMillisEntry.setDouble(latencyMillis); + + if (sortMode != null) { + targetList.sort(sortMode.getComparator()); + } + + PhotonPipelineResult newResult = new PhotonPipelineResult(latencyMillis, targetList); + var newPacket = new Packet(newResult.getPacketSize()); + newResult.populatePacket(newPacket); + rawBytesEntry.setRaw(newPacket.getData()); + + boolean hasTargets = newResult.hasTargets(); + hasTargetEntry.setBoolean(hasTargets); + if (!hasTargets) { + targetPitchEntry.setDouble(0.0); + targetYawEntry.setDouble(0.0); + targetAreaEntry.setDouble(0.0); + targetPoseEntry.setDoubleArray(new double[] {0.0, 0.0, 0.0}); + targetSkewEntry.setDouble(0.0); + } else { + var bestTarget = newResult.getBestTarget(); + + targetPitchEntry.setDouble(bestTarget.getPitch()); + targetYawEntry.setDouble(bestTarget.getYaw()); + targetAreaEntry.setDouble(bestTarget.getArea()); + targetSkewEntry.setDouble(bestTarget.getSkew()); + + var transform = bestTarget.getCameraToTarget(); + double[] poseData = { + transform.getX(), transform.getY(), transform.getRotation().getDegrees() + }; + targetPoseEntry.setDoubleArray(poseData); } } } diff --git a/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java b/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java index 5e85483f7..0eec84729 100644 --- a/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java +++ b/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java @@ -41,10 +41,10 @@ public class SimVisionSystem { /** * 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 distortionless pinhole camera model. + * 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 + * @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 @@ -87,25 +87,25 @@ public class SimVisionSystem { this.camVertFOVDegrees = camDiagFOVDegrees * cameraResHeight / hypotPixels; cam = new SimPhotonCamera(camName); - tgtList = new ArrayList(); + tgtList = new ArrayList<>(); } /** - * 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 subste of these targets which + * 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 tgt + * @param target Target to add to the simulated field */ - public void addSimVisionTarget(SimVisionTarget tgt) { - tgtList.add(tgt); + public void addSimVisionTarget(SimVisionTarget target) { + tgtList.add(target); } /** * 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 newCameraToRobot New Tranform from the robot to the camera + * @param newCameraToRobot New Transform from the robot to the camera * @param newCamHeightMeters New height of the camera off the floor * @param newCamPitchDegrees New pitch of the camera axis back from horizontal */ @@ -120,7 +120,7 @@ public class SimVisionSystem { * 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 calcualte which + * @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. */ diff --git a/photon-lib/src/main/java/org/photonvision/SimVisionTarget.java b/photon-lib/src/main/java/org/photonvision/SimVisionTarget.java index 60b896d02..d823c7148 100644 --- a/photon-lib/src/main/java/org/photonvision/SimVisionTarget.java +++ b/photon-lib/src/main/java/org/photonvision/SimVisionTarget.java @@ -34,8 +34,8 @@ public class SimVisionTarget { * the middle of the field facing the target, the Y axis points to your left, and the X axis * points away from you. * @param targetHeightAboveGroundMeters Height of the target above the field plane, in meters. - * @param targetWidthMeters Width of the outter bounding box of the target in meters. - * @param targetHeightMeters Pair Height of the outter bounding box of the target in meters. + * @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( Pose2d targetPos, diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java index 3baf4f2b3..002c73724 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -54,6 +54,10 @@ public class PhotonTrackedTarget { return area; } + public double getSkew() { + return skew; + } + public Transform2d getCameraToTarget() { return cameraToTarget; }