[cmd] Add andThenWaitUntil, andThenWaitSeconds, andThenWaitTime command decorators

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java

@@ -200,6 +200,57 @@ public ParallelRaceGroup until(BooleanSupplier condition) {
     return raceWith(new WaitUntilCommand(condition));
   }
 
+  /**
+   * Decorates this command with a condition to wait for after this command is finished. The command
+   * will finish after the specified condition is met.
+   *
+   * <p>Note: This decorator works by adding this command to a composition. The command the
+   * decorator was called on cannot be scheduled independently or be added to a different
+   * composition (namely, decorators), unless it is manually cleared from the list of composed
+   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
+   * returned from this method can be further decorated without issue.
+   *
+   * @param condition the condition to wait for after this command is finished
+   * @return the command with the condition to await
+   */
+  public SequentialCommandGroup andThenWaitUntil(BooleanSupplier condition) {
+    return andThen(Commands.waitUntil(condition));
+  }
+
+  /**
+   * Decorates this command with a time to pass after this command is finished. The command will
+   * finish after this time has passed.
+   *
+   * <p>Note: This decorator works by adding this command to a composition. The command the
+   * decorator was called on cannot be scheduled independently or be added to a different
+   * composition (namely, decorators), unless it is manually cleared from the list of composed
+   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
+   * returned from this method can be further decorated without issue.
+   *
+   * @param seconds the timeout duration
+   * @return the command with the time to wait
+   */
+  public SequentialCommandGroup andThenWaitSeconds(double seconds) {
+    return andThen(Commands.waitSeconds(seconds));
+  }
+
+  /**
+   * Decorates this command with a timeout to pass for after this command is finished. The command
+   * will finish after this timeout has passed.
+   *
+   * <p>Note: This decorator works by adding this command to a composition. The command the
+   * decorator was called on cannot be scheduled independently or be added to a different
+   * composition (namely, decorators), unless it is manually cleared from the list of composed
+   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
+   * returned from this method can be further decorated without issue.
+   *
+   * @param time the timeout duration
+   * @return the command with the timeout to await
+   */
+  public SequentialCommandGroup andThenWaitTime(Time time) {
+    return andThenWaitSeconds(time.in(Seconds));
+  }
+
   /**
    * Decorates this command with a run condition. If the specified condition becomes false before
    * the command finishes normally, the command will be interrupted and un-scheduled.

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java

@@ -88,6 +88,47 @@ void untilOrderTest() {
     }
   }
 
+  @Test
+  void andThenWaitUntilTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean finish = new AtomicBoolean();
+
+      Command command = new RunCommand(() -> {}).andThenWaitUntil(finish::get);
+
+      scheduler.schedule(command);
+      scheduler.run();
+
+      assertTrue(scheduler.isScheduled(command));
+
+      finish.set(true);
+      scheduler.run();
+
+      assertFalse(scheduler.isScheduled(command));
+    }
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void andThenWaitSecondsTest() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Command awaitTimeout = new RunCommand(() -> {}).andThenWaitSeconds(0.1);
+
+      scheduler.schedule(awaitTimeout);
+      scheduler.run();
+
+      assertTrue(scheduler.isScheduled(awaitTimeout));
+
+      SimHooks.stepTiming(0.15);
+      scheduler.run();
+
+      assertFalse(scheduler.isScheduled(awaitTimeout));
+    } finally {
+      SimHooks.resumeTiming();
+    }
+  }
+
   @Test
   void onlyWhileTest() {
     try (CommandScheduler scheduler = new CommandScheduler()) {