Implement Heading Precision

quail/src/main/java/com/mineinjava/quail/pathing/PathFollower.java

@@ -28,11 +28,7 @@
 import com.mineinjava.quail.util.geometry.Vec2d;
 import java.util.ArrayList;
 
-/**
- * Class that helps you follow paths
- *
- * <p>TODO: Don't treat point as reached if angle is outside angular precision
- */
+/** Class that helps you follow paths */
 public class PathFollower {
   private Path path;
   private double speed;
@@ -41,6 +37,7 @@ public class PathFollower {
   private double maxAcceleration;
   private MiniPID turnController;
   private double precision;
+  private double headingPrecision;
   private Localizer localizer;
   private double slowDownDistance;
 
@@ -64,6 +61,7 @@ public PathFollower(
       double maxAcceleration,
       MiniPID turnController,
       double precision,
+      double headingPrecision,
       double slowDownDistance,
       double kP,
       double minVelocity) {
@@ -75,6 +73,7 @@ public PathFollower(
     this.maxAcceleration = maxAcceleration;
     this.turnController = turnController;
     this.precision = precision;
+    this.headingPrecision = headingPrecision;
     this.slowDownDistance =
         slowDownDistance; // TODO: in the future add an option to calculate it based on max accel.
     this.kP = kP;
@@ -89,6 +88,7 @@ public PathFollower(
       double maxAcceleration,
       MiniPID turnController,
       double precision,
+      double headingPrecision,
       double slowDownDistance,
       double kP,
       double minVelocity) {
@@ -101,6 +101,7 @@ public PathFollower(
         maxAcceleration,
         turnController,
         precision,
+        headingPrecision,
         slowDownDistance,
         kP,
         minVelocity);
@@ -113,6 +114,7 @@ public PathFollower(
       ConstraintsPair rotationPair,
       MiniPID turnController,
       double precision,
+      double headingPrecision,
       double slowDownDistance,
       double kP,
       double minVelocity) {
@@ -125,6 +127,7 @@ public PathFollower(
         translationPair.getMaxAcceleration(),
         turnController,
         precision,
+        headingPrecision,
         slowDownDistance,
         kP,
         minVelocity);
@@ -143,11 +146,12 @@ public RobotMovement calculateNextDriveMovement() {
           "localizer is null, ensure that you have instantiated the localizer object");
     }
 
+    this.currentPose = this.localizer.getPose(); // get the current pose of the robot
+
     if (this.isFinished()) {
       return new RobotMovement(0, new Vec2d(0, 0)); // the path is over
     }
 
-    this.currentPose = this.localizer.getPose();
     this.loopTime = (System.currentTimeMillis() - this.lastTime) / 1000.0;
     this.lastTime = System.currentTimeMillis();
 
@@ -229,7 +233,25 @@ public RobotMovement calculateNextDriveMovement() {
 
   /** returns true if the robot is finished following the path. */
   public Boolean isFinished() {
-    return this.path.isFinished();
+    this.currentPose = this.localizer.getPose(); // ensure latest pose is updated
+
+    // Check if the path is finished
+    if (this.path.isFinished()) {
+      // Check if the heading difference is within the allowed precision for the last point
+      double headingDifference =
+          Math.abs(
+              this.path.points.get(this.path.points.size() - 1).heading - this.currentPose.heading);
+      return headingDifference <= this.headingPrecision;
+    }
+
+    // Check if the heading difference is within the allowed precision for the current point
+    double headingDifference =
+        Math.abs(this.path.points.get(this.path.lastPointIndex).heading - this.currentPose.heading);
+    if (headingDifference >= this.headingPrecision) {
+      return false;
+    }
+
+    return false;
   }
 
   /**

quail/src/test/java/quail/pathing/PathFollowerTest.java

@@ -21,11 +21,13 @@
 package quail.pathing;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertThrows;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
 import com.mineinjava.quail.RobotMovement;
 import com.mineinjava.quail.localization.KalmanFilterLocalizer;
+import com.mineinjava.quail.localization.SwerveOdometry;
 import com.mineinjava.quail.pathing.ConstraintsPair;
 import com.mineinjava.quail.pathing.Path;
 import com.mineinjava.quail.pathing.PathFollower;
@@ -72,6 +74,7 @@ void setUp() {
 
     MiniPID turnController = new MiniPID(1, 0, 0);
     double precision = 0.2;
+    double headingPrecision = 0.1;
     double slowDownDistance = 0.25d;
     double kP = 1;
     double minVelocity = 0;
@@ -84,6 +87,7 @@ void setUp() {
             rotationPair,
             turnController,
             precision,
+            headingPrecision,
             slowDownDistance,
             kP,
             minVelocity);
@@ -156,4 +160,62 @@ void driveMovementsConvergeToPathAndFinishPath() {
     }
     assertTrue(this.pathFollower.isFinished());
   }
+
+  @Test
+  void isFinishedWorksWithDifferentFinalHeading() {
+    // Modify the final point to have a different heading
+    Pose2d finalPoseWithDifferentHeading = new Pose2d(0, 2, Math.PI / 2); // 90 degrees
+    Path newPath =
+        new Path(
+            new ArrayList<Pose2d>() {
+              {
+                add(poseStart);
+                add(poseSecond);
+                add(poseThird);
+                add(poseFourth);
+                add(finalPoseWithDifferentHeading);
+              }
+            });
+
+    // Create a new PathFollower with a SwerveOdometry localizer (won't work with Kalman)
+    SwerveOdometry swerveOdometry =
+        new SwerveOdometry(
+            new ArrayList<Vec2d>() {
+              {
+                add(new Vec2d(1, 0));
+                add(new Vec2d(-1, 0));
+              }
+            });
+
+    // Set the new localizer and path
+    pathFollower.setLocalizer(swerveOdometry);
+    pathFollower.setPath(newPath);
+
+    // Simulate the robot moving along the path
+    for (int i = 0; i < path.points.size(); i++) {
+      pathFollower.getLocalizer().setPose(path.points.get(i)); // Set the pose to the next pose
+      pathFollower.calculateNextDriveMovement(); // Update
+
+      // Check if the path is finished
+      if (i < path.points.size() - 1) {
+        assertFalse(pathFollower.isFinished(), "Path should not be finished yet");
+      } else {
+        // Check if the path is finished on final point with incorrect heading, then correct heading
+        Pose2d finalPoseWithWrongHeading = new Pose2d(0, 2, Math.PI); // Outside heading precision
+        pathFollower.getLocalizer().setPose(finalPoseWithWrongHeading);
+        assertFalse(
+            pathFollower.isFinished(), "Path should not be finished due to incorrect heading");
+
+        Pose2d finalPoseWithCorrectHeading =
+            new Pose2d(0, 2, Math.PI / 2 - 0.05); // Within heading precision
+        pathFollower.getLocalizer().setPose(finalPoseWithCorrectHeading);
+        assertTrue(pathFollower.isFinished(), "Path should be finished, within heading precision");
+
+        pathFollower
+            .getLocalizer()
+            .setPose(finalPoseWithDifferentHeading); // Within heading precision
+        assertTrue(pathFollower.isFinished(), "Path should be finished, correct heading");
+      }
+    }
+  }
 }