From 653780834fad333145bfcc3caac54ccbf978cf33 Mon Sep 17 00:00:00 2001 From: shueja <32416547+shueja@users.noreply.github.com> Date: Fri, 6 Dec 2024 22:54:00 -0800 Subject: [PATCH] Improve atPose clarity, routine-active safety, alliance-flipping safety (#982) --- .../main/java/choreo/auto/AutoTrajectory.java | 216 +++++++++++------- .../java/choreo/util/AllianceFlipUtil.java | 32 ++- .../java/choreo/auto/PoseFlippingTest.java | 2 - 3 files changed, 152 insertions(+), 98 deletions(-) diff --git a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java index 0a4447e10..9b2dad4b1 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,32 +460,32 @@ public Trigger atTime(String eventName) { return trig; } - /** - * 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. - * @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); + private Trigger atPose( + Supplier> pose, double toleranceMeters, double toleranceRadians) { 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); - }); + 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()); + } + + private Trigger atPose(Optional pose, double toleranceMeters, double toleranceRadians) { + return atPose( + AllianceFlipUtil.optionalFlippedPose2d(pose, alliance, useAllianceFlipping), + toleranceMeters, + toleranceRadians); } /** @@ -527,31 +498,29 @@ 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 - * 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. * * @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 +535,9 @@ 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 +553,127 @@ 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); + }) + .and(active()); + } + + private Trigger atTranslation(Optional translation, double toleranceMeters) { + return atTranslation( + AllianceFlipUtil.optionalFlippedTranslation2d(translation, alliance, useAllianceFlipping), + toleranceMeters); } /** - * 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(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. + * @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..d8135ae75 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() @@ -251,16 +251,28 @@ public static Supplier> optionalFlipped( } /** - * Creates a Supplier<Optional<Pose2d>> based on a - * Supplier<Optional<Alliance>> and original Optional<Pose2d> + * Creates a Supplier<Optional<Translation2d>> that is flipped based on a + * Supplier<Optional<Alliance>> and original Optional<Translation2d> * - * @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 + * @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> optionalFlipped( - Optional pose, Supplier> alliance) { - return optionalFlipped(pose, alliance, () -> 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; } } diff --git a/choreolib/src/test/java/choreo/auto/PoseFlippingTest.java b/choreolib/src/test/java/choreo/auto/PoseFlippingTest.java index 11f63bdd3..d7869e1f1 100644 --- a/choreolib/src/test/java/choreo/auto/PoseFlippingTest.java +++ b/choreolib/src/test/java/choreo/auto/PoseFlippingTest.java @@ -91,8 +91,6 @@ 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()); } }