SCRIPT Move subprojects

commandsv2/src/main/native/cpp/frc2/command/CommandPtr.cpp

@@ -0,0 +1,303 @@
+// 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/CommandPtr.h"
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include <frc/Errors.h>
+
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelCommandGroup.h"
+#include "frc2/command/ParallelDeadlineGroup.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/ProxyCommand.h"
+#include "frc2/command/RepeatCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+#include "frc2/command/WaitCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+#include "frc2/command/WrapperCommand.h"
+
+using namespace frc2;
+
+CommandPtr::CommandPtr(std::unique_ptr<Command>&& command)
+    : m_ptr(std::move(command)) {
+  AssertValid();
+}
+
+CommandPtr::CommandPtr(CommandPtr&& rhs) {
+  m_ptr = std::move(rhs.m_ptr);
+  AssertValid();
+  rhs.m_moveOutSite = wpi::GetStackTrace(1);
+}
+
+void CommandPtr::AssertValid() const {
+  if (!m_ptr) {
+    throw FRC_MakeError(frc::err::CommandIllegalUse,
+                        "Moved-from CommandPtr object used!\nMoved out at:\n{}",
+                        m_moveOutSite);
+  }
+}
+
+CommandPtr CommandPtr::Repeatedly() && {
+  AssertValid();
+  m_ptr = std::make_unique<RepeatCommand>(std::move(m_ptr));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::AsProxy() && {
+  AssertValid();
+  m_ptr = std::make_unique<ProxyCommand>(std::move(m_ptr));
+  return std::move(*this);
+}
+
+class RunsWhenDisabledCommand : public WrapperCommand {
+ public:
+  RunsWhenDisabledCommand(std::unique_ptr<Command>&& command,
+                          bool doesRunWhenDisabled)
+      : WrapperCommand(std::move(command)),
+        m_runsWhenDisabled(doesRunWhenDisabled) {}
+
+  bool RunsWhenDisabled() const override { return m_runsWhenDisabled; }
+
+ private:
+  bool m_runsWhenDisabled;
+};
+
+CommandPtr CommandPtr::IgnoringDisable(bool doesRunWhenDisabled) && {
+  AssertValid();
+  m_ptr = std::make_unique<RunsWhenDisabledCommand>(std::move(m_ptr),
+                                                    doesRunWhenDisabled);
+  return std::move(*this);
+}
+
+using InterruptionBehavior = Command::InterruptionBehavior;
+class InterruptBehaviorCommand : public WrapperCommand {
+ public:
+  InterruptBehaviorCommand(std::unique_ptr<Command>&& command,
+                           InterruptionBehavior interruptBehavior)
+      : WrapperCommand(std::move(command)),
+        m_interruptBehavior(interruptBehavior) {}
+
+  InterruptionBehavior GetInterruptionBehavior() const override {
+    return m_interruptBehavior;
+  }
+
+ private:
+  InterruptionBehavior m_interruptBehavior;
+};
+
+CommandPtr CommandPtr::WithInterruptBehavior(
+    InterruptionBehavior interruptBehavior) && {
+  AssertValid();
+  m_ptr = std::make_unique<InterruptBehaviorCommand>(std::move(m_ptr),
+                                                     interruptBehavior);
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::AndThen(std::function<void()> toRun,
+                               Requirements requirements) && {
+  AssertValid();
+  return std::move(*this).AndThen(CommandPtr(
+      std::make_unique<InstantCommand>(std::move(toRun), requirements)));
+}
+
+CommandPtr CommandPtr::AndThen(CommandPtr&& next) && {
+  AssertValid();
+  std::vector<std::unique_ptr<Command>> temp;
+  temp.emplace_back(std::move(m_ptr));
+  temp.emplace_back(std::move(next).Unwrap());
+  m_ptr = std::make_unique<SequentialCommandGroup>(std::move(temp));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::BeforeStarting(std::function<void()> toRun,
+                                      Requirements requirements) && {
+  AssertValid();
+  return std::move(*this).BeforeStarting(CommandPtr(
+      std::make_unique<InstantCommand>(std::move(toRun), requirements)));
+}
+
+CommandPtr CommandPtr::BeforeStarting(CommandPtr&& before) && {
+  AssertValid();
+  std::vector<std::unique_ptr<Command>> temp;
+  temp.emplace_back(std::move(before).Unwrap());
+  temp.emplace_back(std::move(m_ptr));
+  m_ptr = std::make_unique<SequentialCommandGroup>(std::move(temp));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::WithTimeout(units::second_t duration) && {
+  AssertValid();
+  std::vector<std::unique_ptr<Command>> temp;
+  temp.emplace_back(std::move(m_ptr));
+  temp.emplace_back(std::make_unique<WaitCommand>(duration));
+  m_ptr = std::make_unique<ParallelRaceGroup>(std::move(temp));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::Until(std::function<bool()> condition) && {
+  AssertValid();
+  std::vector<std::unique_ptr<Command>> temp;
+  temp.emplace_back(std::move(m_ptr));
+  temp.emplace_back(std::make_unique<WaitUntilCommand>(std::move(condition)));
+  m_ptr = std::make_unique<ParallelRaceGroup>(std::move(temp));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::OnlyWhile(std::function<bool()> condition) && {
+  AssertValid();
+  return std::move(*this).Until(std::not_fn(std::move(condition)));
+}
+
+CommandPtr CommandPtr::Unless(std::function<bool()> condition) && {
+  AssertValid();
+  m_ptr = std::make_unique<ConditionalCommand>(
+      std::make_unique<InstantCommand>(), std::move(m_ptr),
+      std::move(condition));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::OnlyIf(std::function<bool()> condition) && {
+  AssertValid();
+  return std::move(*this).Unless(std::not_fn(std::move(condition)));
+}
+
+CommandPtr CommandPtr::WithDeadline(CommandPtr&& deadline) && {
+  AssertValid();
+  std::vector<std::unique_ptr<Command>> vec;
+  vec.emplace_back(std::move(m_ptr));
+  m_ptr = std::make_unique<ParallelDeadlineGroup>(std::move(deadline).Unwrap(),
+                                                  std::move(vec));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::DeadlineWith(CommandPtr&& parallel) && {
+  AssertValid();
+  std::vector<std::unique_ptr<Command>> vec;
+  vec.emplace_back(std::move(parallel).Unwrap());
+  m_ptr =
+      std::make_unique<ParallelDeadlineGroup>(std::move(m_ptr), std::move(vec));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::DeadlineFor(CommandPtr&& parallel) && {
+  AssertValid();
+  std::vector<std::unique_ptr<Command>> vec;
+  vec.emplace_back(std::move(parallel).Unwrap());
+  m_ptr =
+      std::make_unique<ParallelDeadlineGroup>(std::move(m_ptr), std::move(vec));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::AlongWith(CommandPtr&& parallel) && {
+  AssertValid();
+  std::vector<std::unique_ptr<Command>> vec;
+  vec.emplace_back(std::move(m_ptr));
+  vec.emplace_back(std::move(parallel).Unwrap());
+  m_ptr = std::make_unique<ParallelCommandGroup>(std::move(vec));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::RaceWith(CommandPtr&& parallel) && {
+  AssertValid();
+  std::vector<std::unique_ptr<Command>> vec;
+  vec.emplace_back(std::move(m_ptr));
+  vec.emplace_back(std::move(parallel).Unwrap());
+  m_ptr = std::make_unique<ParallelRaceGroup>(std::move(vec));
+  return std::move(*this);
+}
+
+namespace {
+class FinallyCommand : public WrapperCommand {
+ public:
+  FinallyCommand(std::unique_ptr<Command>&& command,
+                 std::function<void(bool)> end)
+      : WrapperCommand(std::move(command)), m_end(std::move(end)) {}
+
+  void End(bool interrupted) override {
+    WrapperCommand::End(interrupted);
+    m_end(interrupted);
+  }
+
+ private:
+  std::function<void(bool)> m_end;
+};
+}  // namespace
+
+CommandPtr CommandPtr::FinallyDo(std::function<void(bool)> end) && {
+  AssertValid();
+  m_ptr = std::make_unique<FinallyCommand>(std::move(m_ptr), std::move(end));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::FinallyDo(std::function<void()> end) && {
+  AssertValid();
+  return std::move(*this).FinallyDo(
+      [endHandler = std::move(end)](bool interrupted) { endHandler(); });
+}
+
+CommandPtr CommandPtr::HandleInterrupt(std::function<void()> handler) && {
+  AssertValid();
+  return std::move(*this).FinallyDo(
+      [handler = std::move(handler)](bool interrupted) {
+        if (interrupted) {
+          handler();
+        }
+      });
+}
+
+CommandPtr CommandPtr::WithName(std::string_view name) && {
+  AssertValid();
+  WrapperCommand wrapper{std::move(m_ptr)};
+  wrapper.SetName(name);
+  return std::move(wrapper).ToPtr();
+}
+
+Command* CommandPtr::get() const& {
+  AssertValid();
+  return m_ptr.get();
+}
+
+std::unique_ptr<Command> CommandPtr::Unwrap() && {
+  AssertValid();
+  return std::move(m_ptr);
+}
+
+void CommandPtr::Schedule() const& {
+  AssertValid();
+  CommandScheduler::GetInstance().Schedule(*this);
+}
+
+void CommandPtr::Cancel() const& {
+  AssertValid();
+  CommandScheduler::GetInstance().Cancel(*this);
+}
+
+bool CommandPtr::IsScheduled() const& {
+  AssertValid();
+  return CommandScheduler::GetInstance().IsScheduled(*this);
+}
+
+bool CommandPtr::HasRequirement(Subsystem* requirement) const& {
+  AssertValid();
+  return m_ptr->HasRequirement(requirement);
+}
+
+CommandPtr::operator bool() const& {
+  return m_ptr.operator bool();
+}
+
+std::vector<std::unique_ptr<Command>> CommandPtr::UnwrapVector(
+    std::vector<CommandPtr>&& vec) {
+  std::vector<std::unique_ptr<Command>> ptrs;
+  for (auto&& ptr : vec) {
+    ptrs.emplace_back(std::move(ptr).Unwrap());
+  }
+  return ptrs;
+}