Create unit test + Resolve failing tests

Mineinjava · Jan 8, 2025 · 2e04168 · 2e04168
1 parent 319604c
commit 2e04168
Show file tree

Hide file tree

Showing 2 changed files with 82 additions and 7 deletions.
diff --git a/quail/src/main/java/com/mineinjava/quail/pathing/PathFollower.java b/quail/src/main/java/com/mineinjava/quail/pathing/PathFollower.java
@@ -28,10 +28,7 @@
 import com.mineinjava.quail.util.geometry.Vec2d;
 import java.util.ArrayList;
 
-/**
- * Class that helps you follow paths
- *
- */
+/** Class that helps you follow paths */
 public class PathFollower {
   private Path path;
   private double speed;
@@ -149,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();
 
@@ -235,10 +233,25 @@ public RobotMovement calculateNextDriveMovement() {
 
   /** returns true if the robot is finished following the path. */
   public Boolean isFinished() {
-    if (Math.abs(this.path.getCurrentPoint().heading - currentPose.heading) > this.headingPrecision) {
+    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 this.path.isFinished();
+
+    return false;
   }
 
   /**

diff --git a/quail/src/test/java/quail/pathing/PathFollowerTest.java b/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");
+      }
+    }
+  }
 }