Sim target sorting and "easy" NT entries (#262)

* Typo fixes, add target sorting

* Add easy NT entries

* spotttttttttttttttttlessssssssssssss

* Run wpiformat

* Fix return on no targets

* formatting hell 2 electric boogaloo

Co-authored-by: Matt <matthew.morley.ca@gmail.com>

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 <https://www.gnu.org/licenses/>.
+ */
+
+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<PhotonTrackedTarget> m_comparator;
+
+    PhotonTargetSortMode(Comparator<PhotonTrackedTarget> comparator) {
+        m_comparator = comparator;
+    }
+
+    public Comparator<PhotonTrackedTarget> getComparator() {
+        return m_comparator;
+    }
+}

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<PhotonTrackedTarget> 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<PhotonTrackedTarget> 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<PhotonTrackedTarget> 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);
         }
     }
 }

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<SimVisionTarget>();
+        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.
     */

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,