Merge pull request #56 from Manchester-Central/nearest-field-point

Adds getNearestPoint to FieldPose

Author: jnallard

src/main/java/com/chaos131/poses/FieldPose.java

@@ -1,17 +1,19 @@
 package com.chaos131.poses;
 
+import static edu.wpi.first.units.Units.Meters;
+
 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.Translation2d;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
-
-import static edu.wpi.first.units.Units.Meters;
-
 import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Comparator;
 import java.util.HashMap;
+import java.util.List;
 import java.util.Map;
 
 public abstract class FieldPose {
@@ -142,4 +144,37 @@ public static Pose2d getClosestKnownPose(Pose2d robotPose) {
     FieldPoses.forEach((name, pose) -> poses.add(pose.getCurrentAlliancePose()));
     return robotPose.nearest(poses);
   }
+
+  /**
+   * Finds the closest FieldPose to the sourcePose given the current alliance. Will throw a runtime
+   * error if no FieldPoses are added.
+   *
+   * @param sourcePose the pose to compare to
+   * @param fieldPoses the poses to search through
+   */
+  public static FieldPose getClosestPose(Pose2d sourcePose, FieldPose... fieldPoses) {
+    return getClosestPose(sourcePose, Arrays.asList(fieldPoses));
+  }
+
+  /**
+   * Finds the closest FieldPose to the sourcePose given the current alliance. Will throw a runtime
+   * error if no FieldPoses are added.
+   *
+   * @param sourcePose the pose to compare to
+   * @param fieldPoses the poses to search through
+   */
+  public static FieldPose getClosestPose(Pose2d sourcePose, List<FieldPose> fieldPoses) {
+    // spotless:off
+    return fieldPoses
+      .stream()
+      .min(Comparator.comparingDouble(pose -> getDistanceFromLocations(sourcePose, pose.getCurrentAlliancePose()).in(Meters)))
+      .get();
+    // spotless:on
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "%s(name: %s, bluePose: %s)", getClass().getSimpleName(), m_name, m_bluePose);
+  }
 }

src/test/java/com/chaos131/poses/MirroredDrivePoseTests.java

@@ -76,7 +76,7 @@ public void testMirroringAngle() {
   }
 
   @Test
-  public void testGetClosestPose() {
+  public void testGetClosestKnownPose() {
     var pose1 = new DrivePoseImpl("pose1", new Pose2d(0, 0, Rotation2d.fromDegrees(0)));
     var pose2 = new DrivePoseImpl("pose2", new Pose2d(6, 6, Rotation2d.fromDegrees(0)));
     var pose3 = new DrivePoseImpl("pose3", new Pose2d(9, 0, Rotation2d.fromDegrees(0)));
@@ -92,6 +92,21 @@ public void testGetClosestPose() {
         pose3.getBluePose());
   }
 
+  @Test
+  public void testGetClosestPose() {
+    var pose1 = new DrivePoseImpl("pose1", new Pose2d(0, 0, Rotation2d.fromDegrees(0)));
+    var pose2 = new DrivePoseImpl("pose2", new Pose2d(6, 6, Rotation2d.fromDegrees(0)));
+
+    var drivePose1 = new Pose2d(0, 0, Rotation2d.fromDegrees(180));
+    assertEquals(DrivePoseImpl.getClosestPose(drivePose1, pose1, pose2), pose1);
+
+    var drivePose2 = new Pose2d(1, 0, Rotation2d.fromDegrees(0));
+    assertEquals(DrivePoseImpl.getClosestPose(drivePose2, pose1, pose2), pose1);
+
+    var drivePose3 = new Pose2d(5, 5, Rotation2d.fromDegrees(180));
+    assertEquals(DrivePoseImpl.getClosestPose(drivePose3, pose1, pose2), pose2);
+  }
+
   // @Test
   // public void testAngleFrom() {
   //     var pose1 = new DrivePoseImpl(new Pose2d());