[cmd] Add andThenWaitUntil, andThenWaitSeconds, andThenWaitTime command decorators

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

@@ -200,6 +200,58 @@ 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
+   * this 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 thenAwait(BooleanSupplier condition) {
+    return andThen(Commands.waitUntil(condition));
+  }
+
+    /**
+   * 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 seconds the timeout duration
+   * @return the command with the timeout to await
+   */
+  public SequentialCommandGroup thenAwaitTimeout(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 thenAwaitTimeout(Time time) {
+    return thenAwaitTimeout(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,48 @@ void untilOrderTest() {
     }
   }
 
+  @Test
+  void thenAwaitTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean finish = new AtomicBoolean();
+
+      Command command = new RunCommand(() -> {}).thenAwait(finish::get);
+
+      scheduler.schedule(command);
+      scheduler.run();
+
+      assertTrue(scheduler.isScheduled(command));
+
+      finish.set(true);
+      scheduler.run();
+
+      assertFalse(scheduler.isScheduled(command));
+    }
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void awaitTimeoutTest() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Command awaitTimeout = new RunCommand(() -> {}).thenAwaitTimeout(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()) {