[command] Make cancel safe to call from initialize (#2440)

Fixes #2388.

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

@@ -140,15 +140,16 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
    * @param requirements  The command requirements
    */
   private void initCommand(Command command, boolean interruptible, Set<Subsystem> requirements) {
-    command.initialize();
     CommandState scheduledCommand = new CommandState(interruptible);
     m_scheduledCommands.put(command, scheduledCommand);
-    for (Consumer<Command> action : m_initActions) {
-      action.accept(command);
-    }
+    command.initialize();
     for (Subsystem requirement : requirements) {
       m_requirements.put(requirement, command);
     }
+    for (Consumer<Command> action : m_initActions) {
+      action.accept(command);
+    }
+
     m_watchdog.addEpoch(command.getName() + ".initialize()");
   }
 
@@ -381,9 +382,11 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
   }
 
   /**
-   * Cancels commands.  The scheduler will only call the interrupted method of a canceled command,
-   * not the end method (though the interrupted method may itself call the end method).  Commands
-   * will be canceled even if they are not scheduled as interruptible.
+   * Cancels commands. The scheduler will only call {@link Command#end(boolean)} method
+   * of the canceled command with {@code true},
+   * indicating they were canceled (as opposed to finishing normally).
+   *
+   * <p>Commands will be canceled even if they are not scheduled as interruptible.
    *
    * @param commands the commands to cancel
    */

wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp

@@ -144,12 +144,12 @@ void CommandScheduler::Schedule(bool interruptible, Command* command) {
     }
     command->Initialize();
     m_impl->scheduledCommands[command] = CommandState{interruptible};
-    for (auto&& action : m_impl->initActions) {
-      action(*command);
-    }
     for (auto&& requirement : requirements) {
       m_impl->requirements[requirement] = command;
     }
+    for (auto&& action : m_impl->initActions) {
+      action(*command);
+    }
     m_watchdog.AddEpoch(command->GetName() + ".Initialize()");
   }
 }

wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h

@@ -209,30 +209,36 @@ class CommandScheduler final : public frc::Sendable,
   Command* GetDefaultCommand(const Subsystem* subsystem) const;
 
   /**
-   * Cancels a command.  The scheduler will only call the interrupted method of
-   * a canceled command, not the end method (though the interrupted method may
-   * itself call the end method).  Commands will be canceled even if they are
-   * not scheduled as interruptible.
+   * Cancels commands. The scheduler will only call Command::End()
+   * method of the canceled command with true, indicating they were
+   * canceled (as opposed to finishing normally).
    *
-   * @param command the command to cancel
+   * <p>Commands will be canceled even if they are not scheduled as
+   * interruptible.
+   *
+   * @param commands the commands to cancel
    */
   void Cancel(Command* command);
 
   /**
-   * Cancels commands.  The scheduler will only call the interrupted method of a
-   * canceled command, not the end method (though the interrupted method may
-   * itself call the end method).  Commands will be canceled even if they are
-   * not scheduled as interruptible.
+   * Cancels commands. The scheduler will only call Command::End()
+   * method of the canceled command with true, indicating they were
+   * canceled (as opposed to finishing normally).
+   *
+   * <p>Commands will be canceled even if they are not scheduled as
+   * interruptible.
    *
    * @param commands the commands to cancel
    */
   void Cancel(wpi::ArrayRef<Command*> commands);
 
   /**
-   * Cancels commands.  The scheduler will only call the interrupted method of a
-   * canceled command, not the end method (though the interrupted method may
-   * itself call the end method).  Commands will be canceled even if they are
-   * not scheduled as interruptible.
+   * Cancels commands. The scheduler will only call Command::End()
+   * method of the canceled command with true, indicating they were
+   * canceled (as opposed to finishing normally).
+   *
+   * <p>Commands will be canceled even if they are not scheduled as
+   * interruptible.
    *
    * @param commands the commands to cancel
    */