From be79de2ffc0abb78386d63f60c85d69db42a4910 Mon Sep 17 00:00:00 2001 From: shueja-personal Date: Fri, 6 Dec 2024 15:59:23 -0800 Subject: [PATCH 1/4] atPose now checks rotation, atTranslation is added for prior functionality --- .../main/java/choreo/auto/AutoTrajectory.java | 195 +++++++++++------- .../java/choreo/util/AllianceFlipUtil.java | 40 +++- 2 files changed, 158 insertions(+), 77 deletions(-) diff --git a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java index 0a4447e10..fc84dc0e5 100644 --- a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java +++ b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java @@ -11,6 +11,7 @@ import choreo.trajectory.TrajectorySample; import choreo.util.AllianceFlipUtil; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; @@ -39,7 +40,7 @@ public class AutoTrajectory { // and far between. This helps with more novice users private static final double DEFAULT_TOLERANCE_METERS = Units.inchesToMeters(3); - + private static final double DEFAULT_TOLERANCE_RADIANS = Units.degreesToRadians(3); private final String name; private final Trajectory> trajectory; private final TrajectoryLogger> trajectoryLogger; @@ -240,23 +241,6 @@ public Optional getInitialPose() { return trajectory.getInitialPose(doFlip()); } - /** - * Will get the starting pose of the trajectory as a Supplier - * - *

This position is flipped when alliance flipping is enabled and the alliance supplier returns - * Red. - * - *

This Supplier returns an empty Optional if the trajectory is empty. This method returns an - * empty Optional if alliance flipping is enabled and the alliance supplier returns an empty - * Optional. - * - * @return The starting pose as a Supplier that will flip - */ - public Supplier> getInitialPoseSupplier() { - return AllianceFlipUtil.optionalFlipped( - trajectory.getInitialPose(false), alliance, useAllianceFlipping); - } - /** * Will get the ending pose of the trajectory. * @@ -276,23 +260,6 @@ public Optional getFinalPose() { return trajectory.getFinalPose(doFlip()); } - /** - * Will get the ending pose of the trajectory as a Supplier - * - *

This position is flipped when alliance flipping is enabled and the alliance supplier returns - * Red. - * - *

This Supplier returns an empty Optional if the trajectory is empty. This method returns an - * empty Optional if alliance flipping is enabled and the alliance supplier returns an empty - * Optional. - * - * @return The ending pose as a Supplier that will flip - */ - public Supplier> getFinalPoseSupplier() { - return AllianceFlipUtil.optionalFlipped( - trajectory.getFinalPose(false), alliance, useAllianceFlipping); - } - /** * Returns a trigger that is true while the trajectory is scheduled. * @@ -415,6 +382,10 @@ public Trigger done() { return done(0); } + private Supplier> optionalFlipped(Optional pose) { + return AllianceFlipUtil.optionalFlippedPose2d(pose, alliance, useAllianceFlipping); + } + /** * Returns a trigger that will go true for 1 cycle when the desired time has elapsed * @@ -489,6 +460,22 @@ public Trigger atTime(String eventName) { return trig; } + private Trigger atPose(Supplier> pose, double toleranceMeters, double toleranceRadians) { + return new Trigger( + routine.loop(), + () -> { + Optional checkedPoseOpt = pose.get(); + return checkedPoseOpt + .map( + (checkedPose) -> { + Translation2d currentTrans = poseSupplier.get().getTranslation(); + Rotation2d currentRot = poseSupplier.get().getRotation(); + return currentTrans.getDistance(checkedPose.getTranslation()) < toleranceMeters + && Math.abs(currentRot.minus(checkedPose.getRotation()).getRadians()) < toleranceRadians; + }) + .orElse(false); + }); + } /** * Returns a trigger that is true when the robot is within toleranceMeters of the given pose. * @@ -499,22 +486,12 @@ public Trigger atTime(String eventName) { * * @param pose The pose to check against, unflipped. * @param toleranceMeters The tolerance in meters. + * @param toleranceRadians The heading tolerance in radians. * @return A trigger that is true when the robot is within toleranceMeters of the given pose. */ - public Trigger atPose(Optional pose, double toleranceMeters) { - Supplier> checkedPoseOptSup = optionalFlipped(pose); - return new Trigger( - routine.loop(), - () -> { - Optional checkedPoseOpt = checkedPoseOptSup.get(); - return checkedPoseOpt - .map( - (checkedPose) -> { - Translation2d currentTrans = poseSupplier.get().getTranslation(); - return currentTrans.getDistance(checkedPose.getTranslation()) < toleranceMeters; - }) - .orElse(false); - }); + public Trigger atPose(Optional pose, double toleranceMeters, double toleranceRadians) { + return atPose(AllianceFlipUtil.optionalFlippedPose2d(pose, alliance, useAllianceFlipping), + toleranceMeters, toleranceRadians); } /** @@ -527,18 +504,15 @@ public Trigger atPose(Optional pose, double toleranceMeters) { * * @param pose The pose to check against, unflipped. * @param toleranceMeters The tolerance in meters. + * @param toleranceRadians The heading tolerance in radians. * @return A trigger that is true when the robot is within toleranceMeters of the given pose. */ - public Trigger atPose(Pose2d pose, double toleranceMeters) { - return atPose(Optional.of(pose), toleranceMeters); - } - - private Supplier> optionalFlipped(Optional pose) { - return AllianceFlipUtil.optionalFlipped(pose, alliance, useAllianceFlipping); + public Trigger atPose(Pose2d pose, double toleranceMeters, double toleranceRadians) { + return atPose(Optional.of(pose), toleranceMeters, toleranceRadians); } /** - * Returns a trigger that is true when the robot is within toleranceMeters of the given events + * Returns a trigger that is true when the robot is within toleranceMeters and toleranceRadians of the given event's * pose. * *

A warning will be printed to the DriverStation if the event is not found and the trigger @@ -546,12 +520,13 @@ private Supplier> optionalFlipped(Optional pose) { * * @param eventName The name of the event. * @param toleranceMeters The tolerance in meters. + * @param toleranceRadians The heading tolerance in radians. * @return A trigger that is true when the robot is within toleranceMeters of the given events * pose. * @see Event Markers in the * GUI */ - public Trigger atPose(String eventName, double toleranceMeters) { + public Trigger atPose(String eventName, double toleranceMeters, double toleranceRadians) { boolean foundEvent = false; Trigger trig = offTrigger; @@ -566,8 +541,8 @@ public Trigger atPose(String eventName, double toleranceMeters) { // this also lets atPose be called before the alliance is ready .sampleAt(event.timestamp, false) .map(TrajectorySample::getPose); - if (poseOpt.isPresent()) { - trig = trig.or(atPose(poseOpt, toleranceMeters)); + if (poseOpt.isPresent()) { // atPose accepts empty optionals but it would just be a false trigger + trig = trig.or(atPose(poseOpt, toleranceMeters, toleranceRadians)); foundEvent = true; } } @@ -583,53 +558,125 @@ public Trigger atPose(String eventName, double toleranceMeters) { } /** - * Returns a trigger that is true when the robot is within 3 inches of the given events pose. + * Returns a trigger that is true when the robot is within 3 inches and 3 degrees of the given event's pose. * *

A warning will be printed to the DriverStation if the event is not found and the trigger * will always be false. * * @param eventName The name of the event. - * @return A trigger that is true when the robot is within 3 inches of the given events pose. + * @return A trigger that is true when the robot is within 3 inches and 3 degrees of the given event's pose. * @see Event Markers in the * GUI */ public Trigger atPose(String eventName) { - return atPose(eventName, DEFAULT_TOLERANCE_METERS); + return atPose(eventName, DEFAULT_TOLERANCE_METERS, DEFAULT_TOLERANCE_RADIANS); } + private Trigger atTranslation(Supplier> translation, double toleranceMeters) { + return new Trigger( + routine.loop(), + () -> { + Optional checkedTranslationOpt = translation.get(); + return checkedTranslationOpt + .map( + (checkedTranslation) -> { + Translation2d currentTrans = poseSupplier.get().getTranslation(); + return currentTrans.getDistance(checkedTranslation) < toleranceMeters; + }) + .orElse(false); + }); + } /** - * Returns a trigger that is true when the event with the given name has been reached based on - * time and the robot is within toleranceMeters of the given events pose. + * Returns a trigger that is true when the robot is within toleranceMeters of the given translation. + * + *

The translation is flipped if alliance flipping is enabled and the alliance supplier returns Red. + * + *

While alliance flipping is enabled and the alliance supplier returns empty, the trigger will + * return false. + * + * @param translation The translation to check against, unflipped. + * @param toleranceMeters The tolerance in meters. + * @return A trigger that is true when the robot is within toleranceMeters of the given translation. + */ + public Trigger atTranslation(Optional translation, double toleranceMeters) { + return atTranslation(AllianceFlipUtil.optionalFlippedTranslation2d(translation, alliance, useAllianceFlipping), toleranceMeters); + } + + /** + * Returns a trigger that is true when the robot is within toleranceMeters of the given translation. + * + *

The translation is flipped if alliance flipping is enabled and the alliance supplier returns Red. + * + *

While alliance flipping is enabled and the alliance supplier returns empty, the trigger will + * return false. + * + * @param translation The translation to check against, unflipped. + * @param toleranceMeters The tolerance in meters. + * @return A trigger that is true when the robot is within toleranceMeters of the given translation. + */ + public Trigger atTranslation(Translation2d translation, double toleranceMeters) { + return atTranslation(Optional.of(translation), toleranceMeters); + } + + /** + * Returns a trigger that is true when the robot is within toleranceMeters and toleranceRadians of the given event's + * translation. * *

A warning will be printed to the DriverStation if the event is not found and the trigger * will always be false. * * @param eventName The name of the event. * @param toleranceMeters The tolerance in meters. - * @return A trigger that is true when the event with the given name has been reached based on - * time and the robot is within toleranceMeters of the given events pose. + * @param toleranceRadians The heading tolerance in radians. + * @return A trigger that is true when the robot is within toleranceMeters of the given events + * translation. * @see Event Markers in the * GUI */ - public Trigger atTimeAndPose(String eventName, double toleranceMeters) { - return atTime(eventName).and(atPose(eventName, toleranceMeters)); + public Trigger atTranslation(String eventName, double toleranceMeters) { + boolean foundEvent = false; + Trigger trig = offTrigger; + + for (var event : trajectory.getEvents(eventName)) { + // This could create a lot of objects, could be done a more efficient way + // with having it all be 1 trigger that just has a list of possess and checks each one each + // cycle or something like that. + // If choreo starts showing memory issues we can look into this. + Optional translationOpt = + trajectory + // don't mirror here because the translations are mirrored themselves + // this also lets atTranslation be called before the alliance is ready + .sampleAt(event.timestamp, false) + .map(TrajectorySample::getPose).map(Pose2d::getTranslation); + if (translationOpt.isPresent()) { // atTranslation accepts empty optionals but it would just be a false trigger + trig = trig.or(atTranslation(translationOpt, toleranceMeters)); + foundEvent = true; + } + } + + // The user probably expects an event to exist if they're trying to do something at that event, + // report the missing event. + if (!foundEvent) { + DriverStation.reportWarning( + "[Choreo] Event \"" + eventName + "\" not found for " + name, true); + } + + return trig; } /** - * Returns a trigger that is true when the event with the given name has been reached based on - * time and the robot is within 3 inches of the given events pose. + * Returns a trigger that is true when the robot is within 3 inches and 3 degrees of the given event's translation. * *

A warning will be printed to the DriverStation if the event is not found and the trigger * will always be false. * * @param eventName The name of the event. - * @return A trigger that is true when the event with the given name has been reached based on - * time and the robot is within 3 inches of the given events pose. + * @return A trigger that is true when the robot is within 3 inches and 3 degrees of the given event's translation. * @see Event Markers in the * GUI */ - public Trigger atTimeAndPose(String eventName) { - return atTimeAndPose(eventName, DEFAULT_TOLERANCE_METERS); + public Trigger atTranslation(String eventName) { + return atTranslation(eventName, DEFAULT_TOLERANCE_METERS); } /** diff --git a/choreolib/src/main/java/choreo/util/AllianceFlipUtil.java b/choreolib/src/main/java/choreo/util/AllianceFlipUtil.java index 35080c164..109c893e1 100644 --- a/choreolib/src/main/java/choreo/util/AllianceFlipUtil.java +++ b/choreolib/src/main/java/choreo/util/AllianceFlipUtil.java @@ -240,7 +240,7 @@ public static Pose3d flip(Pose3d pose) { * @return empty if the alliance is empty; the original pose optional if the alliance is blue or * doFlip is false; the flipped pose optional if the alliance is red and doFlip is true */ - public static Supplier> optionalFlipped( + public static Supplier> optionalFlippedPose2d( Optional poseOpt, Supplier> allianceOpt, BooleanSupplier doFlip) { return () -> doFlip.getAsBoolean() @@ -259,8 +259,42 @@ public static Supplier> optionalFlipped( * @return empty if the alliance is empty; the original pose if the alliance is blue; the flipped * pose if the alliance is red */ - public static Supplier> optionalFlipped( + public static Supplier> optionalFlippedPose2d( Optional pose, Supplier> alliance) { - return optionalFlipped(pose, alliance, () -> true); + return optionalFlippedPose2d(pose, alliance, () -> true); + } + + /** + * Creates a Supplier<Optional<Translation2d>> based on a + * Supplier<Optional<Alliance>> and original Optional<Translation2d> + * + * @param translationOpt The translation to flip + * @param allianceOpt The current alliance + * @param doFlip Returns true if flipping based on the alliance should be done + * @return empty if the alliance is empty; the original translation optional if the alliance is blue or + * doFlip is false; the flipped translation optional if the alliance is red and doFlip is true + */ + public static Supplier> optionalFlippedTranslation2d( + Optional translationOpt, Supplier> allianceOpt, BooleanSupplier doFlip) { + return () -> + doFlip.getAsBoolean() + ? allianceOpt + .get() + .flatMap(ally -> translationOpt.map(translation -> ally == Alliance.Red ? flip(translation) : translation)) + : translationOpt; + } + + /** + * Creates a Supplier<Optional<Translation2d>> based on a + * Supplier<Optional<Alliance>> and original Optional<Translation2d> + * + * @param translation The translation to flip + * @param alliance The current alliance + * @return empty if the alliance is empty; the original translation if the alliance is blue; the flipped + * translation if the alliance is red + */ + public static Supplier> optionalFlippedTranslation2d( + Optional translation, Supplier> alliance) { + return optionalFlippedTranslation2d(translation, alliance, () -> true); } } From 4faab48d253a71f2c0e4d89209c2d603a6b11f68 Mon Sep 17 00:00:00 2001 From: shueja-personal Date: Fri, 6 Dec 2024 16:05:11 -0800 Subject: [PATCH 2/4] Format and fix javadoc --- .../main/java/choreo/auto/AutoTrajectory.java | 119 +++++++++++------- .../java/choreo/util/AllianceFlipUtil.java | 20 +-- 2 files changed, 85 insertions(+), 54 deletions(-) diff --git a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java index fc84dc0e5..0f00b4127 100644 --- a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java +++ b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java @@ -460,22 +460,27 @@ public Trigger atTime(String eventName) { return trig; } - private Trigger atPose(Supplier> pose, double toleranceMeters, double toleranceRadians) { + private Trigger atPose( + Supplier> pose, double toleranceMeters, double toleranceRadians) { return new Trigger( - routine.loop(), - () -> { - Optional checkedPoseOpt = pose.get(); - return checkedPoseOpt - .map( - (checkedPose) -> { - Translation2d currentTrans = poseSupplier.get().getTranslation(); - Rotation2d currentRot = poseSupplier.get().getRotation(); - return currentTrans.getDistance(checkedPose.getTranslation()) < toleranceMeters - && Math.abs(currentRot.minus(checkedPose.getRotation()).getRadians()) < toleranceRadians; - }) - .orElse(false); - }); + routine.loop(), + () -> { + Optional checkedPoseOpt = pose.get(); + return checkedPoseOpt + .map( + (checkedPose) -> { + Translation2d currentTrans = poseSupplier.get().getTranslation(); + Rotation2d currentRot = poseSupplier.get().getRotation(); + return currentTrans.getDistance(checkedPose.getTranslation()) + < toleranceMeters + && Math.abs(currentRot.minus(checkedPose.getRotation()).getRadians()) + < toleranceRadians; + }) + .orElse(false); + }) + .and(active()); } + /** * Returns a trigger that is true when the robot is within toleranceMeters of the given pose. * @@ -490,8 +495,10 @@ private Trigger atPose(Supplier> pose, double toleranceMeters, * @return A trigger that is true when the robot is within toleranceMeters of the given pose. */ public Trigger atPose(Optional pose, double toleranceMeters, double toleranceRadians) { - return atPose(AllianceFlipUtil.optionalFlippedPose2d(pose, alliance, useAllianceFlipping), - toleranceMeters, toleranceRadians); + return atPose( + AllianceFlipUtil.optionalFlippedPose2d(pose, alliance, useAllianceFlipping), + toleranceMeters, + toleranceRadians); } /** @@ -512,8 +519,8 @@ public Trigger atPose(Pose2d pose, double toleranceMeters, double toleranceRadia } /** - * Returns a trigger that is true when the robot is within toleranceMeters and toleranceRadians of the given event's - * pose. + * Returns a trigger that is true when the robot is within toleranceMeters and toleranceRadians of + * the given event's pose. * *

A warning will be printed to the DriverStation if the event is not found and the trigger * will always be false. @@ -541,7 +548,8 @@ public Trigger atPose(String eventName, double toleranceMeters, double tolerance // this also lets atPose be called before the alliance is ready .sampleAt(event.timestamp, false) .map(TrajectorySample::getPose); - if (poseOpt.isPresent()) { // atPose accepts empty optionals but it would just be a false trigger + if (poseOpt + .isPresent()) { // atPose accepts empty optionals but it would just be a false trigger trig = trig.or(atPose(poseOpt, toleranceMeters, toleranceRadians)); foundEvent = true; } @@ -558,13 +566,15 @@ public Trigger atPose(String eventName, double toleranceMeters, double tolerance } /** - * Returns a trigger that is true when the robot is within 3 inches and 3 degrees of the given event's pose. + * Returns a trigger that is true when the robot is within 3 inches and 3 degrees of the given + * event's pose. * *

A warning will be printed to the DriverStation if the event is not found and the trigger * will always be false. * * @param eventName The name of the event. - * @return A trigger that is true when the robot is within 3 inches and 3 degrees of the given event's pose. + * @return A trigger that is true when the robot is within 3 inches and 3 degrees of the given + * event's pose. * @see Event Markers in the * GUI */ @@ -572,62 +582,72 @@ public Trigger atPose(String eventName) { return atPose(eventName, DEFAULT_TOLERANCE_METERS, DEFAULT_TOLERANCE_RADIANS); } - private Trigger atTranslation(Supplier> translation, double toleranceMeters) { + private Trigger atTranslation( + Supplier> translation, double toleranceMeters) { return new Trigger( - routine.loop(), - () -> { - Optional checkedTranslationOpt = translation.get(); - return checkedTranslationOpt - .map( - (checkedTranslation) -> { - Translation2d currentTrans = poseSupplier.get().getTranslation(); - return currentTrans.getDistance(checkedTranslation) < toleranceMeters; - }) - .orElse(false); - }); + routine.loop(), + () -> { + Optional checkedTranslationOpt = translation.get(); + return checkedTranslationOpt + .map( + (checkedTranslation) -> { + Translation2d currentTrans = poseSupplier.get().getTranslation(); + return currentTrans.getDistance(checkedTranslation) < toleranceMeters; + }) + .orElse(false); + }) + .and(active()); } + /** - * Returns a trigger that is true when the robot is within toleranceMeters of the given translation. + * Returns a trigger that is true when the robot is within toleranceMeters of the given + * translation. * - *

The translation is flipped if alliance flipping is enabled and the alliance supplier returns Red. + *

The translation is flipped if alliance flipping is enabled and the alliance supplier returns + * Red. * *

While alliance flipping is enabled and the alliance supplier returns empty, the trigger will * return false. * * @param translation The translation to check against, unflipped. * @param toleranceMeters The tolerance in meters. - * @return A trigger that is true when the robot is within toleranceMeters of the given translation. + * @return A trigger that is true when the robot is within toleranceMeters of the given + * translation. */ public Trigger atTranslation(Optional translation, double toleranceMeters) { - return atTranslation(AllianceFlipUtil.optionalFlippedTranslation2d(translation, alliance, useAllianceFlipping), toleranceMeters); + return atTranslation( + AllianceFlipUtil.optionalFlippedTranslation2d(translation, alliance, useAllianceFlipping), + toleranceMeters); } /** - * Returns a trigger that is true when the robot is within toleranceMeters of the given translation. + * Returns a trigger that is true when the robot is within toleranceMeters of the given + * translation. * - *

The translation is flipped if alliance flipping is enabled and the alliance supplier returns Red. + *

The translation is flipped if alliance flipping is enabled and the alliance supplier returns + * Red. * *

While alliance flipping is enabled and the alliance supplier returns empty, the trigger will * return false. * * @param translation The translation to check against, unflipped. * @param toleranceMeters The tolerance in meters. - * @return A trigger that is true when the robot is within toleranceMeters of the given translation. + * @return A trigger that is true when the robot is within toleranceMeters of the given + * translation. */ public Trigger atTranslation(Translation2d translation, double toleranceMeters) { return atTranslation(Optional.of(translation), toleranceMeters); } /** - * Returns a trigger that is true when the robot is within toleranceMeters and toleranceRadians of the given event's - * translation. + * Returns a trigger that is true when the robot is within toleranceMeters and toleranceRadians of + * the given event's translation. * *

A warning will be printed to the DriverStation if the event is not found and the trigger * will always be false. * * @param eventName The name of the event. * @param toleranceMeters The tolerance in meters. - * @param toleranceRadians The heading tolerance in radians. * @return A trigger that is true when the robot is within toleranceMeters of the given events * translation. * @see Event Markers in the @@ -647,8 +667,11 @@ public Trigger atTranslation(String eventName, double toleranceMeters) { // don't mirror here because the translations are mirrored themselves // this also lets atTranslation be called before the alliance is ready .sampleAt(event.timestamp, false) - .map(TrajectorySample::getPose).map(Pose2d::getTranslation); - if (translationOpt.isPresent()) { // atTranslation accepts empty optionals but it would just be a false trigger + .map(TrajectorySample::getPose) + .map(Pose2d::getTranslation); + if (translationOpt + .isPresent()) { // atTranslation accepts empty optionals but it would just be a false + // trigger trig = trig.or(atTranslation(translationOpt, toleranceMeters)); foundEvent = true; } @@ -665,13 +688,15 @@ public Trigger atTranslation(String eventName, double toleranceMeters) { } /** - * Returns a trigger that is true when the robot is within 3 inches and 3 degrees of the given event's translation. + * Returns a trigger that is true when the robot is within 3 inches and 3 degrees of the given + * event's translation. * *

A warning will be printed to the DriverStation if the event is not found and the trigger * will always be false. * * @param eventName The name of the event. - * @return A trigger that is true when the robot is within 3 inches and 3 degrees of the given event's translation. + * @return A trigger that is true when the robot is within 3 inches and 3 degrees of the given + * event's translation. * @see Event Markers in the * GUI */ diff --git a/choreolib/src/main/java/choreo/util/AllianceFlipUtil.java b/choreolib/src/main/java/choreo/util/AllianceFlipUtil.java index 109c893e1..7e4974df5 100644 --- a/choreolib/src/main/java/choreo/util/AllianceFlipUtil.java +++ b/choreolib/src/main/java/choreo/util/AllianceFlipUtil.java @@ -264,23 +264,29 @@ public static Supplier> optionalFlippedPose2d( return optionalFlippedPose2d(pose, alliance, () -> true); } - /** + /** * Creates a Supplier<Optional<Translation2d>> based on a * Supplier<Optional<Alliance>> and original Optional<Translation2d> * * @param translationOpt The translation to flip * @param allianceOpt The current alliance * @param doFlip Returns true if flipping based on the alliance should be done - * @return empty if the alliance is empty; the original translation optional if the alliance is blue or - * doFlip is false; the flipped translation optional if the alliance is red and doFlip is true + * @return empty if the alliance is empty; the original translation optional if the alliance is + * blue or doFlip is false; the flipped translation optional if the alliance is red and doFlip + * is true */ public static Supplier> optionalFlippedTranslation2d( - Optional translationOpt, Supplier> allianceOpt, BooleanSupplier doFlip) { + Optional translationOpt, + Supplier> allianceOpt, + BooleanSupplier doFlip) { return () -> doFlip.getAsBoolean() ? allianceOpt .get() - .flatMap(ally -> translationOpt.map(translation -> ally == Alliance.Red ? flip(translation) : translation)) + .flatMap( + ally -> + translationOpt.map( + translation -> ally == Alliance.Red ? flip(translation) : translation)) : translationOpt; } @@ -290,8 +296,8 @@ public static Supplier> optionalFlippedTranslation2d( * * @param translation The translation to flip * @param alliance The current alliance - * @return empty if the alliance is empty; the original translation if the alliance is blue; the flipped - * translation if the alliance is red + * @return empty if the alliance is empty; the original translation if the alliance is blue; the + * flipped translation if the alliance is red */ public static Supplier> optionalFlippedTranslation2d( Optional translation, Supplier> alliance) { From 7f836f002dc08a6e42c560c6fbb31f5deb3143f8 Mon Sep 17 00:00:00 2001 From: shueja-personal Date: Fri, 6 Dec 2024 22:27:47 -0800 Subject: [PATCH 3/4] Fix tests and reduce API surface --- .../main/java/choreo/auto/AutoTrajectory.java | 32 ++----------------- .../java/choreo/util/AllianceFlipUtil.java | 30 +---------------- .../java/choreo/auto/PoseFlippingTest.java | 3 +- 3 files changed, 4 insertions(+), 61 deletions(-) diff --git a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java index 0f00b4127..9b2dad4b1 100644 --- a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java +++ b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java @@ -481,20 +481,7 @@ private Trigger atPose( .and(active()); } - /** - * Returns a trigger that is true when the robot is within toleranceMeters of the given pose. - * - *

The pose is flipped if alliance flipping is enabled and the alliance supplier returns Red. - * - *

While alliance flipping is enabled and the alliance supplier returns empty, the trigger will - * return false. - * - * @param pose The pose to check against, unflipped. - * @param toleranceMeters The tolerance in meters. - * @param toleranceRadians The heading tolerance in radians. - * @return A trigger that is true when the robot is within toleranceMeters of the given pose. - */ - public Trigger atPose(Optional pose, double toleranceMeters, double toleranceRadians) { + private Trigger atPose(Optional pose, double toleranceMeters, double toleranceRadians) { return atPose( AllianceFlipUtil.optionalFlippedPose2d(pose, alliance, useAllianceFlipping), toleranceMeters, @@ -599,22 +586,7 @@ private Trigger atTranslation( .and(active()); } - /** - * Returns a trigger that is true when the robot is within toleranceMeters of the given - * translation. - * - *

The translation is flipped if alliance flipping is enabled and the alliance supplier returns - * Red. - * - *

While alliance flipping is enabled and the alliance supplier returns empty, the trigger will - * return false. - * - * @param translation The translation to check against, unflipped. - * @param toleranceMeters The tolerance in meters. - * @return A trigger that is true when the robot is within toleranceMeters of the given - * translation. - */ - public Trigger atTranslation(Optional translation, double toleranceMeters) { + private Trigger atTranslation(Optional translation, double toleranceMeters) { return atTranslation( AllianceFlipUtil.optionalFlippedTranslation2d(translation, alliance, useAllianceFlipping), toleranceMeters); diff --git a/choreolib/src/main/java/choreo/util/AllianceFlipUtil.java b/choreolib/src/main/java/choreo/util/AllianceFlipUtil.java index 7e4974df5..d8135ae75 100644 --- a/choreolib/src/main/java/choreo/util/AllianceFlipUtil.java +++ b/choreolib/src/main/java/choreo/util/AllianceFlipUtil.java @@ -251,21 +251,7 @@ public static Supplier> optionalFlippedPose2d( } /** - * Creates a Supplier<Optional<Pose2d>> based on a - * Supplier<Optional<Alliance>> and original Optional<Pose2d> - * - * @param pose The pose to flip - * @param alliance The current alliance - * @return empty if the alliance is empty; the original pose if the alliance is blue; the flipped - * pose if the alliance is red - */ - public static Supplier> optionalFlippedPose2d( - Optional pose, Supplier> alliance) { - return optionalFlippedPose2d(pose, alliance, () -> true); - } - - /** - * Creates a Supplier<Optional<Translation2d>> based on a + * Creates a Supplier<Optional<Translation2d>> that is flipped based on a * Supplier<Optional<Alliance>> and original Optional<Translation2d> * * @param translationOpt The translation to flip @@ -289,18 +275,4 @@ public static Supplier> optionalFlippedTranslation2d( translation -> ally == Alliance.Red ? flip(translation) : translation)) : translationOpt; } - - /** - * Creates a Supplier<Optional<Translation2d>> based on a - * Supplier<Optional<Alliance>> and original Optional<Translation2d> - * - * @param translation The translation to flip - * @param alliance The current alliance - * @return empty if the alliance is empty; the original translation if the alliance is blue; the - * flipped translation if the alliance is red - */ - public static Supplier> optionalFlippedTranslation2d( - Optional translation, Supplier> alliance) { - return optionalFlippedTranslation2d(translation, alliance, () -> true); - } } diff --git a/choreolib/src/test/java/choreo/auto/PoseFlippingTest.java b/choreolib/src/test/java/choreo/auto/PoseFlippingTest.java index 11f63bdd3..eec86cf2b 100644 --- a/choreolib/src/test/java/choreo/auto/PoseFlippingTest.java +++ b/choreolib/src/test/java/choreo/auto/PoseFlippingTest.java @@ -91,8 +91,7 @@ void testGetEndPose() { AutoTrajectory autoTrajectory = factory.trajectory(trajectory, factory.newRoutine("testroutine")); testPoseProperlyFlipped(start, startFlipped, autoTrajectory::getInitialPose); - testPoseProperlyFlipped(start, startFlipped, autoTrajectory.getInitialPoseSupplier()); testPoseProperlyFlipped(end, endFlipped, autoTrajectory::getFinalPose); - testPoseProperlyFlipped(end, endFlipped, autoTrajectory.getFinalPoseSupplier()); + } } From 3e2b4566734a9ba26183e6b574451c3966ad9da3 Mon Sep 17 00:00:00 2001 From: shueja-personal Date: Fri, 6 Dec 2024 22:38:18 -0800 Subject: [PATCH 4/4] Format --- choreolib/src/test/java/choreo/auto/PoseFlippingTest.java | 1 - 1 file changed, 1 deletion(-) diff --git a/choreolib/src/test/java/choreo/auto/PoseFlippingTest.java b/choreolib/src/test/java/choreo/auto/PoseFlippingTest.java index eec86cf2b..d7869e1f1 100644 --- a/choreolib/src/test/java/choreo/auto/PoseFlippingTest.java +++ b/choreolib/src/test/java/choreo/auto/PoseFlippingTest.java @@ -92,6 +92,5 @@ void testGetEndPose() { factory.trajectory(trajectory, factory.newRoutine("testroutine")); testPoseProperlyFlipped(start, startFlipped, autoTrajectory::getInitialPose); testPoseProperlyFlipped(end, endFlipped, autoTrajectory::getFinalPose); - } }