Update C++ Autonomous Functionality

choreolib/src/main/native/include/choreo/Choreo.h

@@ -4,7 +4,6 @@
 
 #include <string>
 #include <string_view>
-#include <unordered_map>
 #include <vector>
 
 #include <fmt/format.h>
@@ -59,7 +58,7 @@ class Choreo {
       std::error_code ec;
       auto fileBuffer =
           wpi::MemoryBuffer::GetFile(matchingFiles[0].string(), ec);
-      if (!fileBuffer || !ec) {
+      if (!fileBuffer || ec) {
         FRC_ReportError(frc::warn::Warning,
                         "Could not open choreo project file");
       }
@@ -104,7 +103,7 @@ class Choreo {
 
     std::error_code ec;
     auto fileBuffer = wpi::MemoryBuffer::GetFile(trajectoryFileName, ec);
-    if (!fileBuffer || !ec) {
+    if (!fileBuffer || ec) {
       FRC_ReportError(frc::warn::Warning, "Could not find trajectory file: {}",
                       trajectoryName);
       return {};
@@ -157,81 +156,4 @@ class Choreo {
 
   Choreo();
 };
-
-/**
- * A utility for caching loaded trajectories. This allows for loading
- * trajectories only once, and then reusing them.
- */
-template <choreo::TrajectorySample SampleType>
-class TrajectoryCache {
- public:
-  /**
-   * Load a trajectory from the deploy directory. Choreolib expects .traj files
-   * to be placed in src/main/deploy/choreo/[trajectoryName].traj.
-   *
-   * This method will cache the loaded trajectory and reused it if it is
-   * requested again.
-   *
-   * @param trajectoryName the path name in Choreo, which matches the file name
-   * in the deploy directory, file extension is optional.
-   * @return the loaded trajectory, or `empty std::optional` if the trajectory
-   * could not be loaded.
-   * @see Choreo#LoadTrajectory(std::string_view)
-   */
-  static std::optional<choreo::Trajectory<SampleType>> LoadTrajectory(
-      std::string_view trajectoryName) {
-    if (cache.contains(trajectoryName)) {
-      return cache[trajectoryName];
-    } else {
-      cache[trajectoryName] =
-          Choreo::LoadTrajectory<SampleType>(trajectoryName);
-      return cache[trajectoryName];
-    }
-  }
-
-  /**
-   * Load a section of a split trajectory from the deploy directory. Choreolib
-   * expects .traj files to be placed in
-   * src/main/deploy/choreo/[trajectoryName].traj.
-   *
-   * This method will cache the loaded trajectory and reused it if it is
-   * requested again. The trajectory that is split off of will also be cached.
-   *
-   * @param trajectoryName the path name in Choreo, which matches the file name
-   * in the deploy directory, file extension is optional.
-   * @param splitIndex the index of the split trajectory to load
-   * @return the loaded trajectory, or `empty std::optional` if the trajectory
-   * could not be loaded.
-   * @see Choreo#LoadTrajectory(std::string_view)
-   */
-  static std::optional<choreo::Trajectory<SampleType>> LoadTrajectory(
-      std::string_view trajectoryName, int splitIndex) {
-    std::string key = fmt::format("{}.:.{}", trajectoryName, splitIndex);
-
-    if (!cache.contains(key)) {
-      if (cache.contains(trajectoryName)) {
-        cache[key] = cache[trajectoryName].GetSplit(splitIndex);
-      } else {
-        auto possibleTrajectory = LoadTrajectory(trajectoryName);
-        cache[trajectoryName] = possibleTrajectory;
-
-        if (possibleTrajectory.has_value()) {
-          cache[key] = possibleTrajectory.value().GetSplit(splitIndex);
-        }
-      }
-    }
-
-    return cache[key];
-  }
-
-  /**
-   * Clears the trajectory cache.
-   */
-  static void Clear() { cache.clear(); }
-
- private:
-  static inline std::unordered_map<std::string, choreo::Trajectory<SampleType>>
-      cache;
-};
-
 }  // namespace choreo

choreolib/src/main/native/include/choreo/auto/AutoBindings.h

@@ -0,0 +1,71 @@
+// Copyright (c) Choreo contributors
+
+#pragma once
+
+#include <string>
+#include <string_view>
+#include <unordered_map>
+#include <utility>
+
+#include <frc2/command/CommandPtr.h>
+
+namespace choreo {
+
+/**
+ * A class used to bind commands to events in all trajectories created by an
+ * AutoFactory.
+ */
+class AutoBindings {
+ public:
+  AutoBindings() = default;
+  AutoBindings(const AutoBindings&) = delete;
+  AutoBindings& operator=(const AutoBindings&) = delete;
+
+  /**
+   * The default move constructor
+   */
+  AutoBindings(AutoBindings&&) = default;
+
+  /**
+   * The default move assignment operator
+   *
+   * @return the moved autobindings
+   */
+  AutoBindings& operator=(AutoBindings&&) = default;
+
+  /**
+   * Binds a command to an event in all trajectories created by the owener of
+   * the bindings.
+   *
+   * @param name The name of the event to bind the command to
+   * @param cmd A function that returns a CommandPtr that you want to bind.
+   * @return a reference to itself because we need to keep track of our moved
+   * binds (and function chaining)
+   */
+  AutoBindings& Bind(std::string_view name,
+                     std::function<frc2::CommandPtr()> cmd) & {
+    bindings.emplace(name, std::move(cmd));
+    return *this;
+  }
+
+  /**
+   * Gets a read-only reference to the underlying map of events -> Commands
+   *
+   * @return the underlying map of event names to command factories
+   */
+  const std::unordered_map<std::string, std::function<frc2::CommandPtr()>>&
+  GetBindings() const {
+    return bindings;
+  }
+
+ private:
+  void Merge(AutoBindings&& other) {
+    for (auto& [key, value] : other.bindings) {
+      bindings.emplace(std::move(key), std::move(value));
+    }
+    other.bindings.clear();
+  }
+
+  std::unordered_map<std::string, std::function<frc2::CommandPtr()>> bindings;
+};
+}  // namespace choreo

choreolib/src/main/native/include/choreo/auto/AutoChooser.h

@@ -0,0 +1,169 @@
+// Copyright (c) Choreo contributors
+
+#pragma once
+
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableInstance.h>
+#include <networktables/StringArrayTopic.h>
+#include <networktables/StringTopic.h>
+
+#include <functional>
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include <frc/DriverStation.h>
+#include <frc/Errors.h>
+#include <frc/RobotBase.h>
+#include <frc2/command/CommandPtr.h>
+#include <frc2/command/Commands.h>
+
+#include "choreo/auto/AutoFactory.h"
+#include "choreo/util/AllianceFlipperUtil.h"
+#include "networktables/Topic.h"
+
+namespace choreo {
+
+/**
+ * An auto chooser that allows for the selection of auto routines at runtime.
+ *
+ * This chooser takes a lazy loading approach to auto routines, only generating
+ * the auto routine when it is selected. This approach has the benefit of not
+ * loading all autos on startup, but also not loading the auto during auto start
+ * causing a delay.
+ *
+ * Once the AutoChooser is made you can add auto routines to it using the
+ * AddAutoRoutine() function . Unlike Sendable Chooser this chooser has to be
+ * updated every cycle by calling the Update function in your Robot periodic
+ * function.
+ *
+ * You can retrieve the auto routine CommandPtr that is currently
+ * selected by calling the GetSelectedAutoRoutine() function.
+ *
+ * @tparam SampleType The type of samples in the trajectory.
+ * @tparam Year The field year. Defaults to the current year.
+ */
+template <choreo::TrajectorySample SampleType, int Year = util::kDefaultYear>
+class AutoChooser {
+ public:
+  /**
+   * Create a new auto chooser.
+   *
+   * @param factory The auto factory to use for auto routine generation.
+   * @param tableName The name of the network table to use for the chooser,
+   * passing in an empty string will put this chooser at the root of the network
+   * tables.
+   */
+  AutoChooser(AutoFactory<SampleType, Year>& factory,
+              std::string_view tableName)
+      : factory{factory} {
+    std::string path =
+        nt::NetworkTable::NormalizeKey(tableName, true) + "/AutoChooser";
+    std::shared_ptr<nt::NetworkTable> table =
+        nt::NetworkTableInstance::GetDefault().GetTable(path);
+
+    selected = table->GetStringTopic("selected").GetEntry(NONE_NAME);
+    table->GetStringTopic(".type").Publish().Set("String Chooser");
+    table->GetStringTopic("default").Publish().SetDefault(NONE_NAME);
+    active = table->GetStringTopic("active").GetEntry(NONE_NAME);
+
+    std::vector<std::string> keys;
+    for (const auto& pair : autoRoutines) {
+      keys.push_back(pair.first);
+    }
+
+    options = table->GetStringArrayTopic("options").GetEntry(keys);
+  }
+
+  /**
+   * Update the auto chooser.
+   *
+   * This method should be called every cycle in the Robot periodic function. It
+   * will check if the selected auto routine has changed and update the active
+   * auto routine.
+   */
+  void Update() {
+    if (frc::DriverStation::IsDisabled() || frc::RobotBase::IsSimulation()) {
+      std::string selectedString = selected.Get();
+      if (selectedString == lastAutoRoutineName) {
+        return;
+      }
+      if (!autoRoutines.contains(selectedString)) {
+        selected.Set(NONE_NAME);
+        selectedString = NONE_NAME;
+        FRC_ReportError(frc::warn::Warning,
+                        "Selected an auto that isn't an option!");
+      }
+      lastAutoRoutineName = selectedString;
+      lastAutoRoutine = autoRoutines[lastAutoRoutineName](factory);
+      active.Set(lastAutoRoutineName);
+    }
+  }
+
+  /**
+   * Add an auto routine to the chooser.
+   *
+   * An auto routine is a function that takes an AutoFactory and returns a
+   * CommandPtr. These functions can be static, a lambda or belong to a local
+   * variable.
+   *
+   * A good paradigm is making an `AutoRoutines` class that has a reference
+   * to all your subsystems and has helper methods for auto commands inside it.
+   * Then you crate methods inside that class that take an `AutoFactory` and
+   * return a `CommandPtr`.
+   *
+   * @param name The name of the auto routine.
+   * @param generator The function that generates the auto routine.
+   */
+  void AddAutoRoutine(
+      std::string name,
+      std::function<frc2::CommandPtr(AutoFactory<SampleType, Year>& factory)>
+          generator) {
+    autoRoutines[name] = generator;
+
+    std::vector<std::string> keys;
+    for (const auto& pair : autoRoutines) {
+      keys.push_back(pair.first);
+    }
+
+    options.Set(keys);
+  }
+
+  /**
+   * Choose an auto routine by name.
+   *
+   * @param choice The name of the auto routine to choose.
+   */
+  void Choose(std::string_view choice) {
+    selected.Set(choice);
+    Update();
+  }
+
+  /**
+   * Get the currently selected auto routine.
+   *
+   * @return The currently selected auto routine.
+   */
+  frc2::CommandPtr GetSelectedAutoRoutine() {
+    return std::move(lastAutoRoutine);
+  }
+
+ private:
+  static inline std::string NONE_NAME = "Nothing";
+  std::unordered_map<std::string, std::function<frc2::CommandPtr(
+                                      AutoFactory<SampleType, Year>& factory)>>
+      autoRoutines;
+
+  nt::StringEntry selected;
+  nt::StringEntry active;
+
+  nt::StringArrayEntry options;
+
+  AutoFactory<SampleType, Year>& factory;
+
+  std::string lastAutoRoutineName = NONE_NAME;
+  frc2::CommandPtr lastAutoRoutine{frc2::cmd::None()};
+};
+}  // namespace choreo