From f4c70fbb185f3ab823fbc6b229221cce55d0f770 Mon Sep 17 00:00:00 2001 From: shueja <32416547+shueja@users.noreply.github.com> Date: Thu, 5 Dec 2024 21:27:27 -0800 Subject: [PATCH] Make AutoRoutine command end immediately if alliance optional is empty on schedule. (#970) --- choreolib/src/main/java/choreo/Choreo.java | 61 +++++-- .../main/java/choreo/auto/AutoFactory.java | 55 ++++++- .../main/java/choreo/auto/AutoRoutine.java | 51 ++++-- .../main/java/choreo/auto/AutoTrajectory.java | 155 ++++++++++++++---- .../java/choreo/util/AllianceFlipUtil.java | 37 +++++ .../test/java/choreo/auto/AutoTestHelper.java | 13 +- .../java/choreo/auto/PoseFlippingTest.java | 98 +++++++++++ .../auto/RoutineKillNoAllianceTest.java | 80 +++++++++ docs/choreolib/auto-routines.md | 25 +-- 9 files changed, 498 insertions(+), 77 deletions(-) create mode 100644 choreolib/src/test/java/choreo/auto/PoseFlippingTest.java create mode 100644 choreolib/src/test/java/choreo/auto/RoutineKillNoAllianceTest.java diff --git a/choreolib/src/main/java/choreo/Choreo.java b/choreolib/src/main/java/choreo/Choreo.java index bf13f038b..b81b9a6fc 100644 --- a/choreolib/src/main/java/choreo/Choreo.java +++ b/choreolib/src/main/java/choreo/Choreo.java @@ -23,6 +23,7 @@ import edu.wpi.first.hal.HAL; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; @@ -330,11 +331,11 @@ public void clear() { * robot. * @param controller A function that receives the current {@link SampleType} and controls the * robot. - * @param mirrorTrajectory If this returns true, the path will be mirrored to the opposite side, - * while keeping the same coordinate system origin. This will be called every loop during the - * command. * @param driveSubsystem The drive {@link Subsystem} to require for {@link AutoTrajectory} {@link * Command}s. + * @param useAllianceFlipping If this returns true, when on the red alliance, the path will be + * mirrored to the opposite side, while keeping the same coordinate system origin. This will + * be called every loop during the command. * @param bindings Universal trajectory event bindings. * @return An {@link AutoFactory} that can be used to create {@link AutoRoutine} and {@link * AutoTrajectory}. @@ -343,14 +344,14 @@ public void clear() { public static > AutoFactory createAutoFactory( Supplier poseSupplier, Consumer controller, - BooleanSupplier mirrorTrajectory, + BooleanSupplier useAllianceFlipping, Subsystem driveSubsystem, AutoBindings bindings) { return new AutoFactory( requireNonNullParam(poseSupplier, "poseSupplier", "Choreo.createAutoFactory"), requireNonNullParam(controller, "controller", "Choreo.createAutoFactory"), - requireNonNullParam(mirrorTrajectory, "mirrorTrajectory", "Choreo.createAutoFactory"), requireNonNullParam(driveSubsystem, "driveSubsystem", "Choreo.createAutoFactory"), + requireNonNullParam(useAllianceFlipping, "useAllianceFlipping", "Choreo.createAutoFactory"), requireNonNullParam(bindings, "bindings", "Choreo.createAutoFactory"), Optional.empty()); } @@ -363,11 +364,11 @@ public static > AutoFactory crea * robot. * @param controller A function that receives the current {@link SampleType} and controls the * robot. - * @param mirrorTrajectory If this returns true, the path will be mirrored to the opposite side, - * while keeping the same coordinate system origin. This will be called every loop during the - * command. * @param driveSubsystem The drive {@link Subsystem} to require for {@link AutoTrajectory} {@link * Command}s. + * @param useAllianceFlipping If this returns true, when on the red alliance, the path will be + * mirrored to the opposite side, while keeping the same coordinate system origin. This will + * be called every loop during the command. * @param bindings Universal trajectory event bindings. * @param trajectoryLogger A {@link TrajectoryLogger} to log {@link Trajectory} as they start and * finish. @@ -378,16 +379,56 @@ public static > AutoFactory crea public static > AutoFactory createAutoFactory( Supplier poseSupplier, Consumer controller, - BooleanSupplier mirrorTrajectory, + BooleanSupplier useAllianceFlipping, Subsystem driveSubsystem, AutoBindings bindings, TrajectoryLogger trajectoryLogger) { return new AutoFactory( requireNonNullParam(poseSupplier, "poseSupplier", "Choreo.createAutoFactory"), requireNonNullParam(controller, "controller", "Choreo.createAutoFactory"), - requireNonNullParam(mirrorTrajectory, "mirrorTrajectory", "Choreo.createAutoFactory"), requireNonNullParam(driveSubsystem, "driveSubsystem", "Choreo.createAutoFactory"), + requireNonNullParam(useAllianceFlipping, "useAllianceFlipping", "Choreo.createAutoFactory"), requireNonNullParam(bindings, "bindings", "Choreo.createAutoFactory"), Optional.of(trajectoryLogger)); } + + /** + * Create a factory that can be used to create {@link AutoRoutine} and {@link AutoTrajectory}. + * + * @param The type of samples in the trajectory. + * @param poseSupplier A function that returns the current field-relative {@link Pose2d} of the + * robot. + * @param controller A function that receives the current {@link SampleType} and controls the + * robot. + * @param driveSubsystem The drive {@link Subsystem} to require for {@link AutoTrajectory} {@link + * Command}s. + * @param useAllianceFlipping If this returns true, when on the red alliance, the path will be + * mirrored to the opposite side, while keeping the same coordinate system origin. This will + * be called every loop during the command. + * @param bindings Universal trajectory event bindings. + * @param trajectoryLogger A {@link TrajectoryLogger} to log {@link Trajectory} as they start and + * finish. + * @param alliance A custom supplier of the current alliance to use instead of {@link + * DriverStation#getAlliance}. + * @return An {@link AutoFactory} that can be used to create {@link AutoRoutine} and {@link + * AutoTrajectory}. + * @see AutoChooser using this factory with AutoChooser to generate auto routines. + */ + public static > AutoFactory createAutoFactory( + Supplier poseSupplier, + Consumer controller, + Subsystem driveSubsystem, + BooleanSupplier useAllianceFlipping, + AutoBindings bindings, + TrajectoryLogger trajectoryLogger, + Supplier> alliance) { + return new AutoFactory( + requireNonNullParam(poseSupplier, "poseSupplier", "Choreo.createAutoFactory"), + requireNonNullParam(controller, "controller", "Choreo.createAutoFactory"), + requireNonNullParam(driveSubsystem, "driveSubsystem", "Choreo.createAutoFactory"), + requireNonNullParam(useAllianceFlipping, "useAllianceFlipping", "Choreo.createAutoFactory"), + requireNonNullParam(bindings, "bindings", "Choreo.createAutoFactory"), + Optional.of(trajectoryLogger), + requireNonNullParam(alliance, "alliance", "Choreo.createAutoFactory")); + } } diff --git a/choreolib/src/main/java/choreo/auto/AutoFactory.java b/choreolib/src/main/java/choreo/auto/AutoFactory.java index 24ee39066..651b1e28c 100644 --- a/choreolib/src/main/java/choreo/auto/AutoFactory.java +++ b/choreolib/src/main/java/choreo/auto/AutoFactory.java @@ -12,6 +12,7 @@ import edu.wpi.first.hal.HAL; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.event.EventLoop; import edu.wpi.first.wpilibj2.command.Command; @@ -96,40 +97,73 @@ HashMap getBindings() { private final TrajectoryCache trajectoryCache = new TrajectoryCache(); private final Supplier poseSupplier; private final Consumer> controller; - private final BooleanSupplier mirrorTrajectory; + private final Supplier> alliance; + private final BooleanSupplier useAllianceFlipping; private final Subsystem driveSubsystem; private final AutoBindings bindings = new AutoBindings(); private final Optional>> trajectoryLogger; /** - * Its recommended to use the {@link Choreo#createAutoFactory} to create a new instance of this + * It is recommended to use the {@link Choreo#createAutoFactory} to create a new instance of this * class. * * @param {@link Choreo#createAutoFactory} * @param poseSupplier {@link Choreo#createAutoFactory} * @param controller {@link Choreo#createAutoFactory} - * @param mirrorTrajectory {@link Choreo#createAutoFactory} * @param driveSubsystem {@link Choreo#createAutoFactory} + * @param useAllianceFlipping {@link Choreo#createAutoFactory} * @param bindings {@link Choreo#createAutoFactory} * @param trajectoryLogger {@link Choreo#createAutoFactory} + * @param alliance {@link Choreo#createAutoFactory} */ public > AutoFactory( Supplier poseSupplier, Consumer controller, - BooleanSupplier mirrorTrajectory, Subsystem driveSubsystem, + BooleanSupplier useAllianceFlipping, AutoBindings bindings, - Optional> trajectoryLogger) { + Optional> trajectoryLogger, + Supplier> alliance) { this.poseSupplier = poseSupplier; this.controller = controller; - this.mirrorTrajectory = mirrorTrajectory; this.driveSubsystem = driveSubsystem; + this.useAllianceFlipping = useAllianceFlipping; this.bindings.merge(bindings); this.trajectoryLogger = trajectoryLogger.map(logger -> (TrajectoryLogger>) logger); + this.alliance = alliance; HAL.report(tResourceType.kResourceType_ChoreoTrigger, 1); } + /** + * It is recommended to use the {@link Choreo#createAutoFactory} to create a new instance of this + * class. + * + * @param {@link Choreo#createAutoFactory} + * @param poseSupplier {@link Choreo#createAutoFactory} + * @param controller {@link Choreo#createAutoFactory} + * @param driveSubsystem {@link Choreo#createAutoFactory} + * @param useAllianceFlipping {@link Choreo#createAutoFactory} + * @param bindings {@link Choreo#createAutoFactory} + * @param trajectoryLogger {@link Choreo#createAutoFactory} + */ + public > AutoFactory( + Supplier poseSupplier, + Consumer controller, + Subsystem driveSubsystem, + BooleanSupplier useAllianceFlipping, + AutoBindings bindings, + Optional> trajectoryLogger) { + this( + poseSupplier, + controller, + driveSubsystem, + useAllianceFlipping, + bindings, + trajectoryLogger, + DriverStation::getAlliance); + } + /** * Creates a new {@link AutoRoutine}. * @@ -143,7 +177,11 @@ public AutoRoutine newRoutine(String name) { trajectoryCache.clear(); } - return new AutoRoutine(name); + return new AutoRoutine(name, this::allianceKnownOrIgnored); + } + + private boolean allianceKnownOrIgnored() { + return !useAllianceFlipping.getAsBoolean() || alliance.get().isPresent(); } /** @@ -220,7 +258,8 @@ public > AutoTrajectory trajecto solidTrajectory, poseSupplier, solidController, - mirrorTrajectory, + useAllianceFlipping, + alliance, solidLogger, driveSubsystem, routine, diff --git a/choreolib/src/main/java/choreo/auto/AutoRoutine.java b/choreolib/src/main/java/choreo/auto/AutoRoutine.java index 684c35190..4def05529 100644 --- a/choreolib/src/main/java/choreo/auto/AutoRoutine.java +++ b/choreolib/src/main/java/choreo/auto/AutoRoutine.java @@ -34,6 +34,9 @@ public class AutoRoutine { /** The amount of times the routine has been polled */ protected int pollCount = 0; + /** Returns true if the alliance is known or is irrelevant (i.e. flipping is not being done) */ + protected BooleanSupplier allianceKnownOrIgnored = () -> true; + /** * Creates a new loop with a specific name * @@ -45,6 +48,20 @@ public AutoRoutine(String name) { this.name = name; } + /** + * Creates a new loop with a specific name and a custom alliance supplier. + * + * @param name The name of the loop + * @param allianceKnownOrIgnored Returns true if the alliance is known or is irrelevant (i.e. + * flipping is not being done). + * @see AutoFactory#newRoutine Creating a loop from a AutoFactory + */ + public AutoRoutine(String name, BooleanSupplier allianceKnownOrIgnored) { + this.loop = new EventLoop(); + this.name = name; + this.allianceKnownOrIgnored = allianceKnownOrIgnored; + } + /** * A constructor to be used when inhereting this class to instantiate a custom inner loop * @@ -70,7 +87,9 @@ public Trigger running() { /** Polls the routine. Should be called in the autonomous periodic method. */ public void poll() { - if (!DriverStation.isAutonomousEnabled() || isKilled) { + if (!DriverStation.isAutonomousEnabled() + || !allianceKnownOrIgnored.getAsBoolean() + || isKilled) { isActive = false; return; } @@ -121,27 +140,39 @@ public void kill() { /** * Creates a command that will poll this event loop and reset it when it is cancelled. * + *

The command will end instantly and kill the routine if the alliance supplier returns an + * empty optional when the command is scheduled. + * * @return A command that will poll this event loop and reset it when it is cancelled. * @see #cmd(BooleanSupplier) A version of this method that takes a condition to finish the loop. */ public Command cmd() { - return Commands.run(this::poll) - .finallyDo(this::reset) - .until(() -> !DriverStation.isAutonomousEnabled()) - .withName(name); + return cmd(() -> false); } /** * Creates a command that will poll this event loop and reset it when it is finished or canceled. * + *

The command will end instantly and kill the routine if the alliance supplier returns an + * empty optional when the command is scheduled. + * * @param finishCondition A condition that will finish the loop when it is true. * @return A command that will poll this event loop and reset it when it is finished or canceled. - * @see #cmd() A version of this method that doesn't take a condition and never finishes. + * @see #cmd() A version of this method that doesn't take a condition and never finishes except if + * the alliance supplier returns an empty optional when scheduled. */ public Command cmd(BooleanSupplier finishCondition) { - return Commands.run(this::poll) - .finallyDo(this::reset) - .until(() -> !DriverStation.isAutonomousEnabled() || finishCondition.getAsBoolean()) - .withName(name); + return Commands.either( + Commands.run(this::poll) + .finallyDo(this::reset) + .until(() -> !DriverStation.isAutonomousEnabled() || finishCondition.getAsBoolean()) + .withName(name), + Commands.runOnce( + () -> { + DriverStation.reportWarning( + "[Choreo] Alliance not known when starting routine", false); + kill(); + }), + allianceKnownOrIgnored); } } diff --git a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java index 54c3f80fa..f46544c2e 100644 --- a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java +++ b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java @@ -14,6 +14,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.FunctionalCommand; @@ -44,7 +45,8 @@ public class AutoTrajectory { private final TrajectoryLogger> trajectoryLogger; private final Supplier poseSupplier; private final Consumer> controller; - private final BooleanSupplier mirrorTrajectory; + private final BooleanSupplier useAllianceFlipping; + private final Supplier> alliance; private final Timer timer = new Timer(); private final Subsystem driveSubsystem; private final AutoRoutine routine; @@ -79,7 +81,8 @@ > AutoTrajectory( Trajectory trajectory, Supplier poseSupplier, Consumer controller, - BooleanSupplier mirrorTrajectory, + BooleanSupplier useAllianceFlipping, + Supplier> alliance, Optional> trajectoryLogger, Subsystem driveSubsystem, AutoRoutine routine, @@ -88,7 +91,8 @@ > AutoTrajectory( this.trajectory = trajectory; this.poseSupplier = poseSupplier; this.controller = controller; - this.mirrorTrajectory = mirrorTrajectory; + this.useAllianceFlipping = useAllianceFlipping; + this.alliance = alliance; this.driveSubsystem = driveSubsystem; this.routine = routine; this.offTrigger = new Trigger(routine.loop(), () -> false); @@ -102,6 +106,24 @@ public void accept(Trajectory t, Boolean u) {} bindings.getBindings().forEach((key, value) -> active().and(atTime(key)).onTrue(value)); } + /** + * Returns true if alliance flipping is enabled and the alliance optional is present. Also returns + * true if alliance flipping is disabled. + */ + private boolean allowSampling() { + return routine.allianceKnownOrIgnored.getAsBoolean(); + } + + /** + * Returns true if alliance flipping is enabled and the alliance is red. + * + * @return + */ + private boolean doFlip() { + return useAllianceFlipping.getAsBoolean() + && alliance.get().map(a -> a == Alliance.Red).orElse(false); + } + @SuppressWarnings("unchecked") private void logTrajectory(boolean starting) { var sampleOpt = trajectory.getInitialSample(false); @@ -133,7 +155,7 @@ private void cmdInitialize() { @SuppressWarnings("unchecked") private void cmdExecute() { - var sampleOpt = trajectory.sampleAt(timer.get(), mirrorTrajectory.getAsBoolean()); + var sampleOpt = trajectory.sampleAt(timer.get(), doFlip()); if (sampleOpt.isEmpty()) { return; } @@ -202,25 +224,73 @@ Trajectory getRawTrajectory() { /** * Will get the starting pose of the trajectory. * - *

This position is mirrored based on the {@code mirrorTrajectory} boolean supplier in the - * factory used to make this trajectory + *

This position is flipped if alliance flipping is enabled and the alliance supplier returns + * Red. + * + *

This method 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 */ public Optional getInitialPose() { - return trajectory.getInitialPose(mirrorTrajectory.getAsBoolean()); + if (!allowSampling()) { + return Optional.empty(); + } + 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 flipp + */ + public Supplier> getInitialPoseSupplier() { + return AllianceFlipUtil.optionalFlipped( + trajectory.getInitialPose(false), alliance, useAllianceFlipping); } /** * Will get the ending pose of the trajectory. * - *

This position is mirrored based on the {@code mirrorTrajectory} boolean supplier in the - * factory used to make this trajectory + *

This position is flipped if alliance flipping is enabled and the alliance supplier returns + * Red. + * + *

This method 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 */ public Optional getFinalPose() { - return trajectory.getFinalPose(mirrorTrajectory.getAsBoolean()); + if (!allowSampling()) { + return Optional.empty(); + } + 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 flipp + */ + public Supplier> getFinalPoseSupplier() { + return AllianceFlipUtil.optionalFlipped( + trajectory.getFinalPose(false), alliance, useAllianceFlipping); } /** @@ -424,26 +494,51 @@ public Trigger atTime(String eventName) { /** * Returns a trigger that is true when the robot is within toleranceMeters of the given pose. * - *

This position is mirrored based on the {@code mirrorTrajectory} boolean supplier in the - * factory used to make this trajectory. + *

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 + * @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(Pose2d pose, double toleranceMeters) { - Translation2d checkedTrans = - mirrorTrajectory.getAsBoolean() - ? AllianceFlipUtil.flip(pose.getTranslation()) - : pose.getTranslation(); + public Trigger atPose(Optional pose, double toleranceMeters) { + Supplier> checkedPoseOptSup = optionalFlipped(pose); return new Trigger( routine.loop(), () -> { - Translation2d currentTrans = poseSupplier.get().getTranslation(); - return currentTrans.getDistance(checkedTrans) < toleranceMeters; + Optional checkedPoseOpt = checkedPoseOptSup.get(); + return checkedPoseOpt + .map( + (checkedPose) -> { + Translation2d currentTrans = poseSupplier.get().getTranslation(); + return currentTrans.getDistance(checkedPose.getTranslation()) < toleranceMeters; + }) + .orElse(false); }); } + /** + * 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(Pose2d pose, double toleranceMeters) { + return atPose(Optional.of(pose), toleranceMeters); + } + + private Supplier> optionalFlipped(Optional pose) { + return AllianceFlipUtil.optionalFlipped(pose, alliance, useAllianceFlipping); + } + /** * Returns a trigger that is true when the robot is within toleranceMeters of the given events * pose. @@ -463,16 +558,18 @@ public Trigger atPose(String eventName, double toleranceMeters) { Trigger trig = offTrigger; for (var event : trajectory.getEvents(eventName)) { - // This could create alot of objects, could be done a more efficient way + // 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 proposing memory issues we can look into this. + // If choreo starts showing memory issues we can look into this. Optional poseOpt = trajectory - .sampleAt(event.timestamp, mirrorTrajectory.getAsBoolean()) + // don't mirror here because the poses are mirrored themselves + // 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.get(), toleranceMeters)); + trig = trig.or(atPose(poseOpt, toleranceMeters)); foundEvent = true; } } @@ -557,16 +654,16 @@ public double[] collectEventTimes(String eventName) { * @see Event Markers in the * GUI */ - public Pose2d[] collectEventPoses(String eventName) { + public ArrayList>> collectEventPoses(String eventName) { var times = collectEventTimes(eventName); - ArrayList poses = new ArrayList(); + ArrayList>> poses = new ArrayList<>(); for (int i = 0; i < times.length; i++) { trajectory - .sampleAt(times[i], mirrorTrajectory.getAsBoolean()) + .sampleAt(times[i], false) .map(TrajectorySample::getPose) - .ifPresent(poses::add); + .ifPresent(pose -> poses.add(optionalFlipped(Optional.of(pose)))); } - return poses.toArray(Pose2d[]::new); + return poses; } @Override diff --git a/choreolib/src/main/java/choreo/util/AllianceFlipUtil.java b/choreolib/src/main/java/choreo/util/AllianceFlipUtil.java index 9ddd8cc0a..35080c164 100644 --- a/choreolib/src/main/java/choreo/util/AllianceFlipUtil.java +++ b/choreolib/src/main/java/choreo/util/AllianceFlipUtil.java @@ -11,6 +11,9 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import java.util.HashMap; +import java.util.Optional; +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; /** * A utility to standardize flipping of coordinate data based on the current alliance across @@ -226,4 +229,38 @@ public static Rotation3d flip(Rotation3d rotation) { public static Pose3d flip(Pose3d pose) { return new Pose3d(flip(pose.getTranslation()), flip(pose.getRotation())); } + + /** + * Creates a Supplier<Optional<Pose2d>> based on a + * Supplier<Optional<Alliance>> and original Optional<Pose2d> + * + * @param poseOpt The pose 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 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( + Optional poseOpt, Supplier> allianceOpt, BooleanSupplier doFlip) { + return () -> + doFlip.getAsBoolean() + ? allianceOpt + .get() + .flatMap(ally -> poseOpt.map(pose -> ally == Alliance.Red ? flip(pose) : pose)) + : poseOpt; + } + + /** + * 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> optionalFlipped( + Optional pose, Supplier> alliance) { + return optionalFlipped(pose, alliance, () -> true); + } } diff --git a/choreolib/src/test/java/choreo/auto/AutoTestHelper.java b/choreolib/src/test/java/choreo/auto/AutoTestHelper.java index 117d2fe71..595c126fd 100644 --- a/choreolib/src/test/java/choreo/auto/AutoTestHelper.java +++ b/choreolib/src/test/java/choreo/auto/AutoTestHelper.java @@ -4,23 +4,28 @@ import choreo.auto.AutoFactory.AutoBindings; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Subsystem; import java.util.Optional; import java.util.concurrent.atomic.AtomicReference; +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; public class AutoTestHelper { - public static AutoFactory factory(boolean redAlliance) { + public static AutoFactory factory( + Supplier> alliance, BooleanSupplier useAllianceFlipping) { AtomicReference pose = new AtomicReference<>(new Pose2d()); return new AutoFactory( () -> pose.get(), sample -> pose.set(sample.getPose()), - () -> redAlliance, new Subsystem() {}, + useAllianceFlipping, new AutoBindings(), - Optional.empty()); + Optional.empty(), + alliance); } public static AutoFactory factory() { - return factory(false); + return factory(() -> Optional.of(Alliance.Blue), () -> false); } } diff --git a/choreolib/src/test/java/choreo/auto/PoseFlippingTest.java b/choreolib/src/test/java/choreo/auto/PoseFlippingTest.java new file mode 100644 index 000000000..11f63bdd3 --- /dev/null +++ b/choreolib/src/test/java/choreo/auto/PoseFlippingTest.java @@ -0,0 +1,98 @@ +// Copyright (c) Choreo contributors + +package choreo.auto; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import choreo.trajectory.SwerveSample; +import choreo.trajectory.Trajectory; +import choreo.util.AllianceFlipUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import java.util.List; +import java.util.Optional; +import java.util.function.Supplier; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +public class PoseFlippingTest { + Optional alliance; + boolean useAllianceFlipping; + AutoFactory factory; + + @BeforeEach + void setup() { + factory = AutoTestHelper.factory(() -> alliance, () -> useAllianceFlipping); + } + + /** + * Test for a pose that flips when flipping is enabled and alliance is red, is unflipped whenever + * flipping is disabled, and is empty when flipping is enabled and alliance is empty + */ + void testPoseProperlyFlipped( + Pose2d unflipped, Pose2d flipped, Supplier> poseToTest) { + alliance = Optional.empty(); + useAllianceFlipping = true; + assertTrue(poseToTest.get().isEmpty()); + alliance = Optional.of(Alliance.Blue); + assertEquals(poseToTest.get(), Optional.of(unflipped)); + alliance = Optional.of(Alliance.Red); + assertEquals(poseToTest.get(), Optional.of(flipped)); + // initial pose is the unflipped pose if flipping is disabled, for all three alliances + useAllianceFlipping = false; + assertEquals(poseToTest.get(), Optional.of(unflipped)); + alliance = Optional.of(Alliance.Blue); + assertEquals(poseToTest.get(), Optional.of(unflipped)); + alliance = Optional.of(Alliance.Red); + assertEquals(poseToTest.get(), Optional.of(unflipped)); + } + + @Test + void testGetEndPose() { + Pose2d start = Pose2d.kZero; + Pose2d end = new Pose2d(1, 1, Rotation2d.fromRadians(1)); + Pose2d startFlipped = AllianceFlipUtil.flip(start); + Pose2d endFlipped = AllianceFlipUtil.flip(end); + Trajectory trajectory = + new Trajectory<>( + "test", + List.of( + new SwerveSample( + 0, + start.getX(), + start.getY(), + start.getRotation().getRadians(), + 0, + 0, + 0, + 0, + 0, + 0, + new double[4], + new double[4]), + new SwerveSample( + 0, + end.getX(), + end.getY(), + end.getRotation().getRadians(), + 0, + 0, + 0, + 0, + 0, + 0, + new double[4], + new double[4])), + List.of(), + List.of()); + + 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()); + } +} diff --git a/choreolib/src/test/java/choreo/auto/RoutineKillNoAllianceTest.java b/choreolib/src/test/java/choreo/auto/RoutineKillNoAllianceTest.java new file mode 100644 index 000000000..ab9995b51 --- /dev/null +++ b/choreolib/src/test/java/choreo/auto/RoutineKillNoAllianceTest.java @@ -0,0 +1,80 @@ +// Copyright (c) Choreo contributors + +package choreo.auto; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; + +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.SchedulerMaker; +import java.util.Optional; +import java.util.function.Supplier; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +public class RoutineKillNoAllianceTest { + Optional alliance; + boolean useAllianceFlipping; + AutoFactory factory = AutoTestHelper.factory(() -> alliance, () -> useAllianceFlipping); + CommandScheduler scheduler = SchedulerMaker.make(); + Supplier routine = () -> factory.newRoutine("testRoutineKill"); + + @BeforeEach + void setup() { + scheduler.cancelAll(); + factory = AutoTestHelper.factory(() -> alliance, () -> useAllianceFlipping); + } + + void testRoutineKill( + CommandScheduler scheduler, Supplier routineSupplier, boolean expectKill) { + AutoRoutine routine = routineSupplier.get(); + assertFalse(routine.isKilled); + scheduler.schedule(routine.cmd()); + // don't need to run, this should kill on schedule/initialize + assertEquals(expectKill, routine.isKilled); + scheduler.cancelAll(); + } + + @Test + void testDisabledEmpty() { + useAllianceFlipping = false; + alliance = Optional.empty(); + testRoutineKill(scheduler, routine, false); + } + + @Test + void testDisabledBlue() { + useAllianceFlipping = false; + alliance = Optional.of(Alliance.Blue); + testRoutineKill(scheduler, routine, false); + } + + @Test + void testDisabledRed() { + useAllianceFlipping = false; + alliance = Optional.of(Alliance.Red); + testRoutineKill(scheduler, routine, false); + } + + @Test + void testEnabledEmpty() { + useAllianceFlipping = true; + alliance = Optional.empty(); + testRoutineKill(scheduler, routine, true); + } + + @Test + void testEnabledBlue() { + useAllianceFlipping = true; + alliance = Optional.of(Alliance.Blue); + testRoutineKill(scheduler, routine, false); + } + + @Test + void testEnabledRed() { + useAllianceFlipping = true; + alliance = Optional.of(Alliance.Red); + testRoutineKill(scheduler, routine, false); + } +} diff --git a/docs/choreolib/auto-routines.md b/docs/choreolib/auto-routines.md index e4f48c8b8..f97047265 100644 --- a/docs/choreolib/auto-routines.md +++ b/docs/choreolib/auto-routines.md @@ -44,7 +44,7 @@ Uses the `Intake` `Subsystem`. Uses the `Intake` `Subsystem`. - `shootIfNoteOwned` - Shoots the note if the robot owns a note. Uses the `Shooter` `Subsystem`. -- `aimFor(Pose2d pose)` - Aims the shooter for a specific position, also keeps the wheels spun up. +- `aimFor(Supplier pose)` - Aims the shooter for a specific robot position. Also keeps the wheels spun up. Uses the `Shooter` `Subsystem`. - `spinnup` - Spins up the shooter wheels. Uses the `Shooter` `Subsystem`. @@ -107,7 +107,7 @@ public AutoRoutine fivePieceAutoTriggerSeg(AutoFactory factory) { race( intake(), ampToC1.cmd(), - aimFor(ampToC1.getFinalPose().orElseGet(Pose2d::new)) + aimFor(()->ampToC1.getFinalPose().orElseGet(Pose2d::new)) ) ).withName("fivePieceAuto entry point") ); @@ -128,7 +128,7 @@ public AutoRoutine fivePieceAutoTriggerSeg(AutoFactory factory) { c1ToM1.done().and(noteNotOwned(routine)).onTrue(m1ToM2.cmd()); // aims the shooter while traveling to shoot - m1ToS1.active().whileTrue(aimFor(m1ToS1.getFinalPose().orElseGet(Pose2d::new))); + m1ToS1.active().whileTrue(aimFor(()->m1ToS1.getFinalPose().orElseGet(Pose2d::new))); m1ToS1.done().onTrue(shootIfNoteOwned()); m1ToS1.done().onTrue(m1ToM2.cmd() .beforeStarting(waitUntil(noteNotOwned(routine)))); @@ -139,7 +139,7 @@ public AutoRoutine fivePieceAutoTriggerSeg(AutoFactory factory) { m1ToM2.done().onTrue(m2ToS1.cmd()); // aims the shooter while traveling to shoot - m2ToS1.active().whileTrue(aimFor(m2ToS1.getFinalPose().orElseGet(Pose2d::new))); + m2ToS1.active().whileTrue(aimFor(()->m2ToS1.getFinalPose().orElseGet(Pose2d::new))); m2ToS1.done().onTrue(shootIfNoteOwned()); m2ToS1.done().onTrue(s1ToC2.cmd() .beforeStarting(waitUntil(noteNotOwned(routine)))); @@ -148,7 +148,7 @@ public AutoRoutine fivePieceAutoTriggerSeg(AutoFactory factory) { // if the robot has the note, it shoots it // otherwise it goes to the third close note s1ToC2.active().whileTrue(intake()); - s1ToC2.active().whileTrue(aimFor(s1ToC2.getFinalPose().orElseGet(Pose2d::new))); + s1ToC2.active().whileTrue(aimFor(()->s1ToC2.getFinalPose().orElseGet(Pose2d::new))); s1ToC2.done().onTrue(shootIfNoteOwned()); s1ToC2.done().onTrue(c2ToC3.cmd() .beforeStarting(waitUntil(noteNotOwned(routine)))); @@ -186,7 +186,7 @@ public Command fivePieceAutoTriggerMono(AutoFactory factory) { .andThen(autoAimAndShoot(), trajectory.cmd()) .withName("fivePieceAuto entry point") ); - + trajectory.running().onTrue(aim()); // spinnup the shooter while no other command is running subsystemsAvailable(routine, spinnup().getRequirements()) .and(routine.running()).onTrue(spinnup()); @@ -194,16 +194,9 @@ public Command fivePieceAutoTriggerMono(AutoFactory factory) { // extends the intake when the intake event marker is reached trajectory.atTime("intake").onTrue(intake()); // shoots the note when the shoot event marker is reached - trajectory.atTime("shoot").onTrue(shootIfNoteOwned()); - - // aims the shooter when the aim event marker is reached, - // the aim command aims based on the next shoot event marker position - final AtomicInteger shootIndex = new AtomicInteger(0); - final Pose2d[] shootPositions = trajectory.collectEventPoses("shoot"); - trajectory.atTime("aim") - .onTrue(defer(() -> aimFor(shootPositions[shootIndex.getAndIncrement()]), Set.of(shooter))); + trajectory.atTime("shoot").onTrue(shootIfNoteOwned().andThen(new ScheduleCommand(aim()))); - return routine.cmd().beforeStarting(() -> shootIndex.set(0)).withName("fivePieceAuto"); + return routine.cmd().withName("fivePieceAuto"); } ``` @@ -239,7 +232,7 @@ public AutoRoutine fivePieceAutoCompositionSeg(AutoFactory factory) { }), autoAimAndShoot(), deadline( - ampToC1.cmd(), intake(), aimFor(ampToC1.getFinalPose().orElseGet(Pose2d::new))), + ampToC1.cmd(), intake(), aimFor(()->ampToC1.getFinalPose().orElseGet(Pose2d::new))), shootIfNoteOwned(), deadline(c1ToM1, waitSeconds(0.35).andThen(intake())), either(