From 2e04168bcd6351dd90083d45c4b4e27273d1c83d Mon Sep 17 00:00:00 2001 From: astr0clad Date: Wed, 8 Jan 2025 11:54:56 -0700 Subject: [PATCH] Create unit test + Resolve failing tests --- .../quail/pathing/PathFollower.java | 27 +++++--- .../java/quail/pathing/PathFollowerTest.java | 62 +++++++++++++++++++ 2 files changed, 82 insertions(+), 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 index 68b4d90..3cbc81f 100644 --- 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 index 49a144a..d8bd376 100644 --- 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() { + { + 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() { + { + 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"); + } + } + } }