[commands] RepeatCommand: restart on following iteration (#4706)

This fixes InstantCommand.repeatedly().

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

@@ -19,6 +19,7 @@ import static edu.wpi.first.wpilibj2.command.CommandGroupBase.requireUngrouped;
  */
 public class RepeatCommand extends CommandBase {
   protected final Command m_command;
+  private boolean m_ended;
 
   /**
    * Creates a new RepeatCommand. Will run another command repeatedly, restarting it whenever it
@@ -35,16 +36,21 @@ public class RepeatCommand extends CommandBase {
 
   @Override
   public void initialize() {
+    m_ended = false;
     m_command.initialize();
   }
 
   @Override
   public void execute() {
+    if (m_ended) {
+      m_ended = false;
+      m_command.initialize();
+    }
     m_command.execute();
     if (m_command.isFinished()) {
       // restart command
       m_command.end(false);
-      m_command.initialize();
+      m_ended = true;
     }
   }
 
@@ -62,4 +68,9 @@ public class RepeatCommand extends CommandBase {
   public boolean runsWhenDisabled() {
     return m_command.runsWhenDisabled();
   }
+
+  @Override
+  public InterruptionBehavior getInterruptionBehavior() {
+    return m_command.getInterruptionBehavior();
+  }
 }

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

@@ -16,15 +16,20 @@ RepeatCommand::RepeatCommand(std::unique_ptr<Command>&& command) {
 }
 
 void RepeatCommand::Initialize() {
+  m_ended = false;
   m_command->Initialize();
 }
 
 void RepeatCommand::Execute() {
+  if (m_ended) {
+    m_ended = false;
+    m_command->Initialize();
+  }
   m_command->Execute();
   if (m_command->IsFinished()) {
     // restart command
     m_command->End(false);
-    m_command->Initialize();
+    m_ended = true;
   }
 }
 
@@ -39,3 +44,7 @@ void RepeatCommand::End(bool interrupted) {
 bool RepeatCommand::RunsWhenDisabled() const {
   return m_command->RunsWhenDisabled();
 }
+
+Command::InterruptionBehavior RepeatCommand::GetInterruptionBehavior() const {
+  return m_command->GetInterruptionBehavior();
+}

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

@@ -69,8 +69,11 @@ class RepeatCommand : public CommandHelper<CommandBase, RepeatCommand> {
 
   bool RunsWhenDisabled() const override;
 
+  Command::InterruptionBehavior GetInterruptionBehavior() const override;
+
  private:
   std::unique_ptr<Command> m_command;
+  bool m_ended;
 };
 }  // namespace frc2
 

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

@@ -6,19 +6,16 @@ package edu.wpi.first.wpilibj2.command;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.simulation.DriverStationSim;
 import java.util.concurrent.atomic.AtomicBoolean;
 import java.util.concurrent.atomic.AtomicInteger;
 import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.EnumSource;
+import org.junit.jupiter.params.provider.ValueSource;
 
-class RepeatCommandTest {
+class RepeatCommandTest extends CommandTestBase {
   @Test
   void callsMethodsCorrectly() {
-    HAL.initialize(500, 0);
-    // enable so that we don't need to mess with `runsWhenDisabled` for each command
-    DriverStationSim.setEnabled(true);
-
     var initCounter = new AtomicInteger(0);
     var exeCounter = new AtomicInteger(0);
     var isFinishedCounter = new AtomicInteger(0);
@@ -56,7 +53,7 @@ class RepeatCommandTest {
 
     isFinishedHook.set(true);
     CommandScheduler.getInstance().run();
-    assertEquals(2, initCounter.get());
+    assertEquals(1, initCounter.get());
     assertEquals(2, exeCounter.get());
     assertEquals(2, isFinishedCounter.get());
     assertEquals(1, endCounter.get());
@@ -68,4 +65,19 @@ class RepeatCommandTest {
     assertEquals(3, isFinishedCounter.get());
     assertEquals(1, endCounter.get());
   }
+
+  @EnumSource(Command.InterruptionBehavior.class)
+  @ParameterizedTest
+  void interruptible(Command.InterruptionBehavior interruptionBehavior) {
+    var command =
+        new WaitUntilCommand(() -> false).withInterruptBehavior(interruptionBehavior).repeatedly();
+    assertEquals(interruptionBehavior, command.getInterruptionBehavior());
+  }
+
+  @ValueSource(booleans = {true, false})
+  @ParameterizedTest
+  void runWhenDisabled(boolean runsWhenDisabled) {
+    var command = new WaitUntilCommand(() -> false).ignoringDisable(runsWhenDisabled).repeatedly();
+    assertEquals(runsWhenDisabled, command.runsWhenDisabled());
+  }
 }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/TriggerTest.java

@@ -85,6 +85,8 @@ class TriggerTest extends CommandTestBase {
     scheduler.run();
     assertEquals(1, inits.get());
     scheduler.run();
+    assertEquals(1, inits.get());
+    scheduler.run();
     assertEquals(2, inits.get());
     button.setPressed(false);
     scheduler.run();

wpilibNewCommands/src/test/native/cpp/frc2/command/RepeatCommandTest.cpp

@@ -3,6 +3,7 @@
 // the WPILib BSD license file in the root directory of this project.
 
 #include "CommandTestBase.h"
+#include "frc2/command/Commands.h"
 #include "frc2/command/FunctionalCommand.h"
 
 using namespace frc2;
@@ -47,7 +48,7 @@ TEST_F(RepeatCommandTest, CallsMethodsCorrectly) {
 
   isFinishedHook = true;
   scheduler.Run();
-  EXPECT_EQ(2, initCounter);
+  EXPECT_EQ(1, initCounter);
   EXPECT_EQ(2, exeCounter);
   EXPECT_EQ(2, isFinishedCounter);
   EXPECT_EQ(1, endCounter);
@@ -59,3 +60,31 @@ TEST_F(RepeatCommandTest, CallsMethodsCorrectly) {
   EXPECT_EQ(3, isFinishedCounter);
   EXPECT_EQ(1, endCounter);
 }
+
+class RepeatCommandInterruptibilityTest
+    : public CommandTestBaseWithParam<Command::InterruptionBehavior> {};
+
+TEST_P(RepeatCommandInterruptibilityTest, Interruptibility) {
+  CommandPtr command = cmd::WaitUntil([] { return false; })
+                           .WithInterruptBehavior(GetParam())
+                           .Repeatedly();
+  EXPECT_EQ(GetParam(), command.get()->GetInterruptionBehavior());
+}
+
+INSTANTIATE_TEST_SUITE_P(
+    RepeatCommandTests, RepeatCommandInterruptibilityTest,
+    testing::Values(Command::InterruptionBehavior::kCancelIncoming,
+                    Command::InterruptionBehavior::kCancelSelf));
+
+class RepeatCommandRunsWhenDisabledTest
+    : public CommandTestBaseWithParam<bool> {};
+
+TEST_P(RepeatCommandRunsWhenDisabledTest, RunsWhenDisabled) {
+  CommandPtr command = cmd::WaitUntil([] { return false; })
+                           .IgnoringDisable(GetParam())
+                           .Repeatedly();
+  EXPECT_EQ(GetParam(), command.get()->RunsWhenDisabled());
+}
+
+INSTANTIATE_TEST_SUITE_P(RepeatCommandTests, RepeatCommandRunsWhenDisabledTest,
+                         testing::Bool());

wpilibNewCommands/src/test/native/cpp/frc2/command/button/TriggerTest.cpp

@@ -70,6 +70,8 @@ TEST_F(TriggerTest, WhileTrueRepeatedly) {
   scheduler.Run();
   EXPECT_EQ(1, inits);
   scheduler.Run();
+  EXPECT_EQ(1, inits);
+  scheduler.Run();
   EXPECT_EQ(2, inits);
   pressed = false;
   scheduler.Run();