[commands] Improve error message when composing commands twice in sam…

…e composition (#6091)

Also disallow deadline command from also appearing in other commands.

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

@@ -581,10 +581,19 @@ public void onCommandFinish(Consumer<Command> action) {
    * directly or added to a composition.
    *
    * @param commands the commands to register
-   * @throws IllegalArgumentException if the given commands have already been composed.
+   * @throws IllegalArgumentException if the given commands have already been composed, or the array
+   *     of commands has duplicates.
    */
   public void registerComposedCommands(Command... commands) {
-    var commandSet = Set.of(commands);
+    Set<Command> commandSet;
+    try {
+      commandSet = Set.of(commands);
+    } catch (IllegalArgumentException e) {
+      throw new IllegalArgumentException(
+          "Cannot compose a command twice in the same composition! (Original exception: "
+              + e
+              + ")");
+    }
     requireNotComposedOrScheduled(commandSet);
     var exception = new Exception("Originally composed at:");
     exception.fillInStackTrace();

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

@@ -233,12 +233,13 @@ public static Command race(Command... commands) {
    * the others.
    *
    * @param deadline the deadline command
-   * @param commands the commands to include
+   * @param otherCommands the other commands to include
    * @return the command group
    * @see ParallelDeadlineGroup
+   * @throws IllegalArgumentException if the deadline command is also in the otherCommands argument
    */
-  public static Command deadline(Command deadline, Command... commands) {
-    return new ParallelDeadlineGroup(deadline, commands);
+  public static Command deadline(Command deadline, Command... otherCommands) {
+    return new ParallelDeadlineGroup(deadline, otherCommands);
   }
 
   private Commands() {

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

@@ -29,32 +29,38 @@ public class ParallelDeadlineGroup extends Command {
   private InterruptionBehavior m_interruptBehavior = InterruptionBehavior.kCancelIncoming;
 
   /**
-   * Creates a new ParallelDeadlineGroup. The given commands (including the deadline) will be
+   * Creates a new ParallelDeadlineGroup. The given commands, including the deadline, will be
    * executed simultaneously. The composition will finish when the deadline finishes, interrupting
    * all other still-running commands. If the composition is interrupted, only the commands still
    * running will be interrupted.
    *
    * @param deadline the command that determines when the composition ends
-   * @param commands the commands to be executed
+   * @param otherCommands the other commands to be executed
+   * @throws IllegalArgumentException if the deadline command is also in the otherCommands argument
    */
-  public ParallelDeadlineGroup(Command deadline, Command... commands) {
-    m_deadline = deadline;
-    addCommands(commands);
-    if (!m_commands.containsKey(deadline)) {
-      addCommands(deadline);
-    }
+  public ParallelDeadlineGroup(Command deadline, Command... otherCommands) {
+    addCommands(otherCommands);
+    setDeadline(deadline);
   }
 
   /**
    * Sets the deadline to the given command. The deadline is added to the group if it is not already
    * contained.
    *
    * @param deadline the command that determines when the group ends
+   * @throws IllegalArgumentException if the deadline command is already in the composition
    */
   public void setDeadline(Command deadline) {
-    if (!m_commands.containsKey(deadline)) {
-      addCommands(deadline);
+    @SuppressWarnings("PMD.CompareObjectsWithEquals")
+    boolean isAlreadyDeadline = deadline == m_deadline;
+    if (isAlreadyDeadline) {
+      return;
+    }
+    if (m_commands.containsKey(deadline)) {
+      throw new IllegalArgumentException(
+          "The deadline command cannot also be in the other commands!");
     }
+    addCommands(deadline);
     m_deadline = deadline;
   }
 
@@ -74,7 +80,7 @@ public final void addCommands(Command... commands) {
     for (Command command : commands) {
       if (!Collections.disjoint(command.getRequirements(), m_requirements)) {
         throw new IllegalArgumentException(
-            "Multiple commands in a parallel group cannot" + "require the same subsystems");
+            "Multiple commands in a parallel group cannot require the same subsystems");
       }
       m_commands.put(command, false);
       m_requirements.addAll(command.getRequirements());

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

@@ -5,6 +5,7 @@
 package edu.wpi.first.wpilibj2.command;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
 import static org.junit.jupiter.params.provider.Arguments.arguments;
 
 import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
@@ -112,4 +113,19 @@ void runsWhenDisabled(
     var command = compose(command1, command2, command3);
     assertEquals(expected, command.runsWhenDisabled());
   }
+
+  static Stream<Arguments> composeDuplicates() {
+    Command a = new InstantCommand(() -> {});
+    Command b = new InstantCommand(() -> {});
+    return Stream.of(
+        arguments("AA", new Command[] {a, a}),
+        arguments("ABA", new Command[] {a, b, a}),
+        arguments("BAA", new Command[] {b, a, a}));
+  }
+
+  @MethodSource
+  @ParameterizedTest(name = "composeDuplicates[{index}]: {0}")
+  void composeDuplicates(@SuppressWarnings("unused") String name, Command[] commands) {
+    assertThrows(IllegalArgumentException.class, () -> compose(commands));
+  }
 }

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

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj2.command;
 
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertThrows;
 import static org.junit.jupiter.api.Assertions.assertTrue;
@@ -124,6 +125,21 @@ void parallelDeadlineRequirementErrorTest() {
         IllegalArgumentException.class, () -> new ParallelDeadlineGroup(command1, command2));
   }
 
+  @Test
+  void parallelDeadlineSetDeadlineToDeadlineTest() {
+    Command a = new InstantCommand(() -> {});
+    ParallelDeadlineGroup group = new ParallelDeadlineGroup(a);
+    assertDoesNotThrow(() -> group.setDeadline(a));
+  }
+
+  @Test
+  void parallelDeadlineSetDeadlineDuplicateTest() {
+    Command a = new InstantCommand(() -> {});
+    Command b = new InstantCommand(() -> {});
+    ParallelDeadlineGroup group = new ParallelDeadlineGroup(a, b);
+    assertThrows(IllegalArgumentException.class, () -> group.setDeadline(b));
+  }
+
   @Override
   public ParallelDeadlineGroup compose(Command... members) {
     return new ParallelDeadlineGroup(members[0], Arrays.copyOfRange(members, 1, members.length));