[commands] Revamp Interruptible (#4192)

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

@@ -67,6 +67,27 @@ std::unique_ptr<Command> Command::IgnoringDisable(bool doesRunWhenDisabled) && {
       std::move(*this).TransferOwnership(), doesRunWhenDisabled);
 }
 
+std::unique_ptr<Command> Command::WithInterruptBehavior(
+    InterruptionBehavior interruptBehavior) && {
+  class InterruptBehaviorCommand
+      : public CommandHelper<WrapperCommand, InterruptBehaviorCommand> {
+   public:
+    InterruptBehaviorCommand(std::unique_ptr<Command>&& command,
+                             InterruptionBehavior interruptBehavior)
+        : CommandHelper(std::move(command)),
+          m_interruptBehavior(interruptBehavior) {}
+    InterruptionBehavior GetInterruptionBehavior() const override {
+      return m_interruptBehavior;
+    }
+
+   private:
+    InterruptionBehavior m_interruptBehavior;
+  };
+
+  return std::make_unique<InterruptBehaviorCommand>(
+      std::move(*this).TransferOwnership(), interruptBehavior);
+}
+
 ParallelRaceGroup Command::WithInterrupt(std::function<bool()> condition) && {
   std::vector<std::unique_ptr<Command>> temp;
   temp.emplace_back(std::make_unique<WaitUntilCommand>(std::move(condition)));
@@ -130,8 +151,8 @@ ConditionalCommand Command::Unless(std::function<bool()> condition) && {
                             std::move(condition));
 }
 
-void Command::Schedule(bool interruptible) {
-  CommandScheduler::GetInstance().Schedule(interruptible, this);
+void Command::Schedule() {
+  CommandScheduler::GetInstance().Schedule(this);
 }
 
 void Command::Cancel() {

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

@@ -19,16 +19,14 @@
 #include <wpi/sendable/SendableRegistry.h>
 
 #include "frc2/command/CommandGroupBase.h"
-#include "frc2/command/CommandState.h"
 #include "frc2/command/Subsystem.h"
 
 using namespace frc2;
 
 class CommandScheduler::Impl {
  public:
-  // A map from commands to their scheduling state.  Also used as a set of the
-  // currently-running commands.
-  wpi::DenseMap<Command*, CommandState> scheduledCommands;
+  // A set of the currently-running commands.
+  wpi::SmallSet<Command*, 12> scheduledCommands;
 
   // A map from required subsystems to their requiring commands.  Also used as a
   // set of the currently-required subsystems.
@@ -56,7 +54,7 @@ class CommandScheduler::Impl {
   // scheduled/canceled during run
 
   bool inRunLoop = false;
-  wpi::DenseMap<Command*, bool> toSchedule;
+  wpi::SmallVector<Command*, 4> toSchedule;
   wpi::SmallVector<Command*, 4> toCancel;
 };
 
@@ -112,9 +110,9 @@ void CommandScheduler::ClearButtons() {
   m_impl->activeButtonLoop->Clear();
 }
 
-void CommandScheduler::Schedule(bool interruptible, Command* command) {
+void CommandScheduler::Schedule(Command* command) {
   if (m_impl->inRunLoop) {
-    m_impl->toSchedule.try_emplace(command, interruptible);
+    m_impl->toSchedule.emplace_back(command);
     return;
   }
 
@@ -126,7 +124,7 @@ void CommandScheduler::Schedule(bool interruptible, Command* command) {
   }
   if (m_impl->disabled ||
       (frc::RobotState::IsDisabled() && !command->RunsWhenDisabled()) ||
-      ContainsKey(m_impl->scheduledCommands, command)) {
+      m_impl->scheduledCommands.contains(command)) {
     return;
   }
 
@@ -139,8 +137,8 @@ void CommandScheduler::Schedule(bool interruptible, Command* command) {
   for (auto&& i1 : m_impl->requirements) {
     if (requirements.find(i1.first) != requirements.end()) {
       isDisjoint = false;
-      allInterruptible &=
-          m_impl->scheduledCommands[i1.second].IsInterruptible();
+      allInterruptible &= (i1.second->GetInterruptionBehavior() ==
+                           Command::InterruptionBehavior::kCancelSelf);
       intersection.emplace_back(i1.second);
     }
   }
@@ -151,7 +149,7 @@ void CommandScheduler::Schedule(bool interruptible, Command* command) {
         Cancel(cmdToCancel);
       }
     }
-    m_impl->scheduledCommands[command] = CommandState{interruptible};
+    m_impl->scheduledCommands.insert(command);
     for (auto&& requirement : requirements) {
       m_impl->requirements[requirement] = command;
     }
@@ -163,33 +161,15 @@ void CommandScheduler::Schedule(bool interruptible, Command* command) {
   }
 }
 
-void CommandScheduler::Schedule(Command* command) {
-  Schedule(true, command);
-}
-
-void CommandScheduler::Schedule(bool interruptible,
-                                wpi::span<Command* const> commands) {
-  for (auto command : commands) {
-    Schedule(interruptible, command);
-  }
-}
-
-void CommandScheduler::Schedule(bool interruptible,
-                                std::initializer_list<Command*> commands) {
-  for (auto command : commands) {
-    Schedule(interruptible, command);
-  }
-}
-
 void CommandScheduler::Schedule(wpi::span<Command* const> commands) {
   for (auto command : commands) {
-    Schedule(true, command);
+    Schedule(command);
   }
 }
 
 void CommandScheduler::Schedule(std::initializer_list<Command*> commands) {
   for (auto command : commands) {
-    Schedule(true, command);
+    Schedule(command);
   }
 }
 
@@ -218,10 +198,7 @@ void CommandScheduler::Run() {
 
   m_impl->inRunLoop = true;
   // Run scheduled commands, remove finished commands.
-  for (auto iterator = m_impl->scheduledCommands.begin();
-       iterator != m_impl->scheduledCommands.end(); iterator++) {
-    Command* command = iterator->getFirst();
-
+  for (Command* command : m_impl->scheduledCommands) {
     if (!command->RunsWhenDisabled() && frc::RobotState::IsDisabled()) {
       Cancel(command);
       continue;
@@ -243,14 +220,14 @@ void CommandScheduler::Run() {
         m_impl->requirements.erase(requirement);
       }
 
-      m_impl->scheduledCommands.erase(iterator);
+      m_impl->scheduledCommands.erase(command);
       m_watchdog.AddEpoch(command->GetName() + ".End(false)");
     }
   }
   m_impl->inRunLoop = false;
 
-  for (auto&& commandInterruptible : m_impl->toSchedule) {
-    Schedule(commandInterruptible.second, commandInterruptible.first);
+  for (Command* command : m_impl->toSchedule) {
+    Schedule(command);
   }
 
   for (auto&& command : m_impl->toCancel) {
@@ -336,7 +313,7 @@ void CommandScheduler::Cancel(Command* command) {
   if (find == m_impl->scheduledCommands.end()) {
     return;
   }
-  m_impl->scheduledCommands.erase(find);
+  m_impl->scheduledCommands.erase(*find);
   for (auto&& requirement : m_impl->requirements) {
     if (requirement.second == command) {
       m_impl->requirements.erase(requirement.first);
@@ -364,20 +341,11 @@ void CommandScheduler::Cancel(std::initializer_list<Command*> commands) {
 void CommandScheduler::CancelAll() {
   wpi::SmallVector<Command*, 16> commands;
   for (auto&& command : m_impl->scheduledCommands) {
-    commands.emplace_back(command.first);
+    commands.emplace_back(command);
   }
   Cancel(commands);
 }
 
-units::second_t CommandScheduler::TimeSinceScheduled(
-    const Command* command) const {
-  auto find = m_impl->scheduledCommands.find(command);
-  if (find != m_impl->scheduledCommands.end()) {
-    return find->second.TimeSinceInitialized();
-  } else {
-    return -1_s;
-  }
-}
 bool CommandScheduler::IsScheduled(
     wpi::span<const Command* const> commands) const {
   for (auto command : commands) {
@@ -399,8 +367,7 @@ bool CommandScheduler::IsScheduled(
 }
 
 bool CommandScheduler::IsScheduled(const Command* command) const {
-  return m_impl->scheduledCommands.find(command) !=
-         m_impl->scheduledCommands.end();
+  return m_impl->scheduledCommands.contains(command);
 }
 
 Command* CommandScheduler::Requiring(const Subsystem* subsystem) const {
@@ -458,9 +425,9 @@ void CommandScheduler::InitSendable(nt::NTSendableBuilder& builder) {
 
     wpi::SmallVector<std::string, 8> names;
     wpi::SmallVector<double, 8> ids;
-    for (auto&& command : m_impl->scheduledCommands) {
-      names.emplace_back(command.first->GetName());
-      uintptr_t ptrTmp = reinterpret_cast<uintptr_t>(command.first);
+    for (Command* command : m_impl->scheduledCommands) {
+      names.emplace_back(command->GetName());
+      uintptr_t ptrTmp = reinterpret_cast<uintptr_t>(command);
       ids.emplace_back(static_cast<double>(ptrTmp));
     }
     nt::NetworkTableEntry(namesEntry).SetStringArray(names);
@@ -470,5 +437,13 @@ void CommandScheduler::InitSendable(nt::NTSendableBuilder& builder) {
 
 void CommandScheduler::SetDefaultCommandImpl(Subsystem* subsystem,
                                              std::unique_ptr<Command> command) {
+  if (command->GetInterruptionBehavior() ==
+      Command::InterruptionBehavior::kCancelIncoming) {
+    std::puts(
+        "Registering a non-interruptible default command!\n"
+        "This will likely prevent any other commands from "
+        "requiring this subsystem.");
+    // Warn, but allow -- there might be a use case for this.
+  }
   m_impl->subsystems[subsystem] = std::move(command);
 }

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

@@ -1,27 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-#include "frc2/command/CommandState.h"
-
-#include <frc/Timer.h>
-
-using namespace frc2;
-CommandState::CommandState(bool interruptible)
-    : m_interruptible{interruptible} {
-  StartTiming();
-  StartRunning();
-}
-
-void CommandState::StartTiming() {
-  m_startTime = frc::Timer::GetFPGATimestamp();
-}
-
-void CommandState::StartRunning() {
-  m_startTime = -1_s;
-}
-
-units::second_t CommandState::TimeSinceInitialized() const {
-  return m_startTime != -1_s ? frc::Timer::GetFPGATimestamp() - m_startTime
-                             : -1_s;
-}

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

@@ -4,6 +4,8 @@
 
 #include "frc2/command/WrapperCommand.h"
 
+#include "frc2/command/Command.h"
+
 using namespace frc2;
 
 WrapperCommand::WrapperCommand(std::unique_ptr<Command>&& command) {
@@ -33,3 +35,11 @@ void WrapperCommand::End(bool interrupted) {
 bool WrapperCommand::RunsWhenDisabled() const {
   return m_command->RunsWhenDisabled();
 }
+
+Command::InterruptionBehavior WrapperCommand::GetInterruptionBehavior() const {
+  return m_command->GetInterruptionBehavior();
+}
+
+wpi::SmallSet<Subsystem*, 4> WrapperCommand::GetRequirements() const {
+  return m_command->GetRequirements();
+}

wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp

@@ -8,8 +8,8 @@ using namespace frc2;
 
 Button::Button(std::function<bool()> isPressed) : Trigger(isPressed) {}
 
-Button Button::WhenPressed(Command* command, bool interruptible) {
-  WhenActive(command, interruptible);
+Button Button::WhenPressed(Command* command) {
+  WhenActive(command);
   return *this;
 }
 
@@ -25,8 +25,8 @@ Button Button::WhenPressed(std::function<void()> toRun,
   return *this;
 }
 
-Button Button::WhileHeld(Command* command, bool interruptible) {
-  WhileActiveContinous(command, interruptible);
+Button Button::WhileHeld(Command* command) {
+  WhileActiveContinous(command);
   return *this;
 }
 
@@ -42,13 +42,13 @@ Button Button::WhileHeld(std::function<void()> toRun,
   return *this;
 }
 
-Button Button::WhenHeld(Command* command, bool interruptible) {
-  WhileActiveOnce(command, interruptible);
+Button Button::WhenHeld(Command* command) {
+  WhileActiveOnce(command);
   return *this;
 }
 
-Button Button::WhenReleased(Command* command, bool interruptible) {
-  WhenInactive(command, interruptible);
+Button Button::WhenReleased(Command* command) {
+  WhenInactive(command);
   return *this;
 }
 
@@ -64,8 +64,8 @@ Button Button::WhenReleased(std::function<void()> toRun,
   return *this;
 }
 
-Button Button::ToggleWhenPressed(Command* command, bool interruptible) {
-  ToggleWhenActive(command, interruptible);
+Button Button::ToggleWhenPressed(Command* command) {
+  ToggleWhenActive(command);
   return *this;
 }
 

wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp

@@ -13,9 +13,8 @@ using namespace frc2;
 
 Trigger::Trigger(const Trigger& other) = default;
 
-Trigger Trigger::WhenActive(Command* command, bool interruptible) {
-  this->Rising().IfHigh(
-      [command, interruptible] { command->Schedule(interruptible); });
+Trigger Trigger::WhenActive(Command* command) {
+  this->Rising().IfHigh([command] { command->Schedule(); });
   return *this;
 }
 
@@ -30,8 +29,8 @@ Trigger Trigger::WhenActive(std::function<void()> toRun,
   return WhenActive(InstantCommand(std::move(toRun), requirements));
 }
 
-Trigger Trigger::WhileActiveContinous(Command* command, bool interruptible) {
-  this->IfHigh([command, interruptible] { command->Schedule(interruptible); });
+Trigger Trigger::WhileActiveContinous(Command* command) {
+  this->IfHigh([command] { command->Schedule(); });
   this->Falling().IfHigh([command] { command->Cancel(); });
   return *this;
 }
@@ -48,16 +47,14 @@ Trigger Trigger::WhileActiveContinous(
   return WhileActiveContinous(InstantCommand(std::move(toRun), requirements));
 }
 
-Trigger Trigger::WhileActiveOnce(Command* command, bool interruptible) {
-  this->Rising().IfHigh(
-      [command, interruptible] { command->Schedule(interruptible); });
+Trigger Trigger::WhileActiveOnce(Command* command) {
+  this->Rising().IfHigh([command] { command->Schedule(); });
   this->Falling().IfHigh([command] { command->Cancel(); });
   return *this;
 }
 
-Trigger Trigger::WhenInactive(Command* command, bool interruptible) {
-  this->Falling().IfHigh(
-      [command, interruptible] { command->Schedule(interruptible); });
+Trigger Trigger::WhenInactive(Command* command) {
+  this->Falling().IfHigh([command] { command->Schedule(); });
   return *this;
 }
 
@@ -72,12 +69,12 @@ Trigger Trigger::WhenInactive(std::function<void()> toRun,
   return WhenInactive(InstantCommand(std::move(toRun), requirements));
 }
 
-Trigger Trigger::ToggleWhenActive(Command* command, bool interruptible) {
-  this->Rising().IfHigh([command, interruptible] {
+Trigger Trigger::ToggleWhenActive(Command* command) {
+  this->Rising().IfHigh([command] {
     if (command->IsScheduled()) {
       command->Cancel();
     } else {
-      command->Schedule(interruptible);
+      command->Schedule();
     }
   });
   return *this;