diff --git a/choreolib/src/main/native/include/choreo/Choreo.h b/choreolib/src/main/native/include/choreo/Choreo.h index c4ff151e75..c9cad1c79c 100644 --- a/choreolib/src/main/native/include/choreo/Choreo.h +++ b/choreolib/src/main/native/include/choreo/Choreo.h @@ -4,7 +4,6 @@ #include #include -#include #include #include @@ -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 -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> LoadTrajectory( - std::string_view trajectoryName) { - if (cache.contains(trajectoryName)) { - return cache[trajectoryName]; - } else { - cache[trajectoryName] = - Choreo::LoadTrajectory(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> 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> - cache; -}; - } // namespace choreo diff --git a/choreolib/src/main/native/include/choreo/auto/AutoBindings.h b/choreolib/src/main/native/include/choreo/auto/AutoBindings.h new file mode 100644 index 0000000000..59127f358c --- /dev/null +++ b/choreolib/src/main/native/include/choreo/auto/AutoBindings.h @@ -0,0 +1,71 @@ +// Copyright (c) Choreo contributors + +#pragma once + +#include +#include +#include +#include + +#include + +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 cmd) & { + bindings.emplace(name, 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>& + GetBindings() const { + return bindings; + } + + private: + void Merge(AutoBindings&& other) { + for (auto& [key, value] : other.bindings) { + bindings.emplace(key, value); + } + other.bindings.clear(); + } + + std::unordered_map> bindings; +}; +} // namespace choreo diff --git a/choreolib/src/main/native/include/choreo/auto/AutoChooser.h b/choreolib/src/main/native/include/choreo/auto/AutoChooser.h new file mode 100644 index 0000000000..5666757f1a --- /dev/null +++ b/choreolib/src/main/native/include/choreo/auto/AutoChooser.h @@ -0,0 +1,176 @@ +// Copyright (c) Choreo contributors + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#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 +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& factory, + std::string_view tableName) + : factory{factory} { + std::string path = + nt::NetworkTable::NormalizeKey(tableName, true) + "/AutoChooser"; + std::shared_ptr table = + nt::NetworkTableInstance::GetDefault().GetTable(path); + + selected = table->GetStringTopic("selected").GetEntry(NONE_NAME); + + metaType = table->GetStringTopic(".type").Publish(); + metaType.Set("String Chooser"); + + defaultChoice = table->GetStringTopic("default").Publish(); + defaultChoice.Set(NONE_NAME); + + active = table->GetStringTopic("active").GetEntry(NONE_NAME); + + std::vector keys; + + AddAutoRoutine(NONE_NAME, [] { return frc2::cmd::None(); }); + + 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; + 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 generator) { + autoRoutines[name] = generator; + + std::vector 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 autoRoutines[lastAutoRoutineName](); + } + + private: + static inline std::string NONE_NAME = "Nothing"; + std::unordered_map> + autoRoutines; + + nt::StringEntry selected; + nt::StringEntry active; + + nt::StringPublisher metaType; + nt::StringPublisher defaultChoice; + + nt::StringArrayEntry options; + + AutoFactory& factory; + + std::string lastAutoRoutineName = NONE_NAME; + frc2::CommandPtr lastAutoRoutine{frc2::cmd::None()}; +}; +} // namespace choreo diff --git a/choreolib/src/main/native/include/choreo/auto/AutoFactory.h b/choreolib/src/main/native/include/choreo/auto/AutoFactory.h index fad43db02e..9bbb6e161b 100644 --- a/choreolib/src/main/native/include/choreo/auto/AutoFactory.h +++ b/choreolib/src/main/native/include/choreo/auto/AutoFactory.h @@ -3,77 +3,18 @@ #pragma once #include -#include +#include #include -#include #include #include -#include "choreo/Choreo.h" #include "choreo/auto/AutoLoop.h" +#include "choreo/auto/AutoTrajectory.h" +#include "choreo/auto/TrajectoryCache.h" +#include "choreo/util/AllianceFlipperUtil.h" namespace choreo { - -/** - * A class used to bind commands to events in all trajectories created by this - * factory. - */ -class AutoBindings { - public: - /** - * Default constructor. - */ - AutoBindings() = default; - - AutoBindings(const AutoBindings&) = delete; - AutoBindings& operator=(const AutoBindings&) = delete; - - /** - * Move constructor. - */ - AutoBindings(AutoBindings&&) = default; - - /** - * Move assignment operator. - * - * @return This. - */ - AutoBindings& operator=(AutoBindings&&) = default; - - /** - * Binds a command to an event in all trajectories created by the factory - * using this bindings. - * - * @param name The name of the event to bind the command to. - * @param cmd The command to bind to the event. - * @return The bindings object for chaining. - */ - AutoBindings Bind(std::string_view name, frc2::CommandPtr cmd) && { - bindings.emplace(name, std::move(cmd)); - return std::move(*this); - } - - private: - std::unordered_map bindings; - - void Merge(AutoBindings&& other) { - for (auto& [key, value] : other.bindings) { - bindings.emplace(std::move(key), std::move(value)); - } - other.bindings.clear(); - } - - /** - * Gets the bindings map. - * - * @return The bindings map. - */ - const std::unordered_map& GetBindings() const { - return bindings; - } -}; - /** * A factory used to create autonomous routines. * @@ -115,8 +56,9 @@ class AutoBindings { * * * @tparam SampleType The type of samples in the trajectory. + * @tparam Year The field year. Defaults to the current year. */ -template +template class AutoFactory { public: /** @@ -126,35 +68,35 @@ class AutoFactory { * @param poseSupplier Choreo::CreateAutoFactory() * @param controller Choreo::CreateAutoFactory() * @param mirrorTrajectory Choreo::CreateAutoFactory() - * @param driveSubsystem Choreo::CreateAutoFactory() - * @param bindings Choreo::CreateAutoFactory() + * @param drivebaseRequirements Choreo::CreateAutoFactory() * @param trajectoryLogger Choreo::CreateAutoFactory() */ AutoFactory(std::function poseSupplier, std::function controller, std::function mirrorTrajectory, - const frc2::Subsystem& driveSubsystem, AutoBindings bindings, - std::optional trajectoryLogger) + frc2::Requirements drivebaseRequirements, + std::optional> trajectoryLogger) : poseSupplier{std::move(poseSupplier)}, controller{controller}, mirrorTrajectory{std::move(mirrorTrajectory)}, - driveSubsystem{driveSubsystem}, - autoBindings{std::move(bindings)}, + drivebaseRequirements{drivebaseRequirements}, + autoBindings{std::make_shared()}, trajectoryLogger{std::move(trajectoryLogger)} {} /** * Creates a new auto loop to be used to make an auto routine. * + * @param name The name of the event loop. * @return A new auto loop. * @see AutoLoop */ - AutoLoop NewLoop() const { + AutoLoop NewLoop(std::string_view name) { // Clear cache in simulation to allow a form of "hot-reloading" trajectories - if (RobotBase::IsSimulation()) { + if (frc::RobotBase::IsSimulation()) { ClearCache(); } - return AutoLoop(); + return AutoLoop(name); } /** @@ -164,10 +106,10 @@ class AutoFactory { * @param loop The auto loop to use as the triggers polling context. * @return A new auto trajectory. */ - AutoTrajectory Trajectory(std::string_view trajectoryName, - AutoLoop loop) const { + AutoTrajectory Trajectory( + std::string_view trajectoryName, AutoLoop& loop) const { std::optional> optTraj = - Choreo::LoadTrajectory(trajectoryName); + trajectoryCache.LoadTrajectory(trajectoryName); choreo::Trajectory trajectory; if (optTraj.has_value()) { trajectory = optTraj.value(); @@ -175,11 +117,12 @@ class AutoFactory { FRC_ReportError(frc::warn::Warning, "Could not load trajectory: {}", trajectoryName); } - AutoTrajectory autoTraj{ - trajectoryName, trajectory, poseSupplier, controller, - mirrorTrajectory, trajectoryLogger, driveSubsystem, loop.GetLoop(), - autoBindings, loop.OnNewTrajectory}; - loop.AddTrajectory(autoTraj); + AutoTrajectory autoTraj{ + trajectoryName, trajectory, + poseSupplier, controller, + mirrorTrajectory, trajectoryLogger, + drivebaseRequirements, loop.GetLoop(), + autoBindings}; return autoTraj; } @@ -191,11 +134,11 @@ class AutoFactory { * @param loop The auto loop to use as the triggers polling context. * @return A new auto trajectory. */ - AutoTrajectory Trajectory(std::string_view trajectoryName, - int splitIndex, - AutoLoop loop) const { + AutoTrajectory Trajectory( + std::string_view trajectoryName, int splitIndex, + AutoLoop& loop) const { std::optional> optTraj = - Choreo::LoadTrajectory(trajectoryName); + trajectoryCache.LoadTrajectory(trajectoryName); choreo::Trajectory trajectory; if (optTraj.has_value()) { trajectory = optTraj.value(); @@ -213,14 +156,14 @@ class AutoFactory { * @param loop The auto loop to use as the triggers polling context. * @return A new auto trajectory. */ - AutoTrajectory Trajectory( + AutoTrajectory Trajectory( choreo::Trajectory trajectory, - AutoLoop loop) const { - AutoTrajectory autoTraj{ - trajectory.name, trajectory, poseSupplier, controller, - mirrorTrajectory, trajectoryLogger, driveSubsystem, loop.GetLoop(), - autoBindings, loop.OnNewTrajectory}; - loop.AddTrajectory(autoTraj); + AutoLoop& loop) const { + AutoTrajectory autoTraj{trajectory.name, trajectory, + poseSupplier, controller, + mirrorTrajectory, trajectoryLogger, + drivebaseRequirements, loop.GetLoop(), + autoBindings}; return autoTraj; } @@ -228,19 +171,26 @@ class AutoFactory { * Binds a command to an event in all trajectories created after this point. * * @param name The name of the trajectory to bind the command to. - * @param cmd The command to bind to the trajectory. + * @param cmdFactory A function that retuns a CommandPtr to bind */ - void Bind(std::string_view name, frc2::CommandPtr cmd) { - autoBindings = std::move(autoBindings).Bind(name, cmd); + void Bind(std::string_view name, + std::function cmdFactory) { + autoBindings->Bind(name, std::move(cmdFactory)); } + /** + * Empties the interal trajecectory cache + */ + void ClearCache() { trajectoryCache.Clear(); } + private: std::function poseSupplier; std::function controller; std::function mirrorTrajectory; - const frc2::Subsystem& driveSubsystem; - AutoBindings autoBindings{}; - std::optional trajectoryLogger; + frc2::Requirements drivebaseRequirements; + std::shared_ptr autoBindings; + std::optional> trajectoryLogger; + TrajectoryCache trajectoryCache; }; } // namespace choreo diff --git a/choreolib/src/main/native/include/choreo/auto/AutoLoop.h b/choreolib/src/main/native/include/choreo/auto/AutoLoop.h index 8afa84f37a..840b03ca78 100644 --- a/choreolib/src/main/native/include/choreo/auto/AutoLoop.h +++ b/choreolib/src/main/native/include/choreo/auto/AutoLoop.h @@ -3,10 +3,17 @@ #pragma once #include +#include +#include #include -#include -#include "choreo/auto/AutoTrajectory.h" +#include +#include +#include +#include + +#include "choreo/trajectory/TrajectorySample.h" +#include "choreo/util/AllianceFlipperUtil.h" namespace choreo { @@ -17,29 +24,72 @@ namespace choreo { * This loop should **not** be shared across multiple autonomous routines. * * @tparam SampleType The type of samples in the trajectory. + * @tparam The field year. Defaults to the current year. */ -template +template class AutoLoop { public: - AutoLoop() = default; + /** + * Creates a new loop with a specific name + * + * @param name The name of the loop + * @see AutoFactory#newLoop Creating a loop from a AutoFactory + */ + explicit AutoLoop(std::string_view name) + : loop{std::make_unique()}, name{name} {} /** * A constructor to be used when inhereting this class to instantiate a custom * inner loop * + * @param name The name of the loop * @param loop The inner EventLoop */ - explicit AutoLoop(frc::EventLoop loop) : loop{std::move(loop)} {} + AutoLoop(std::string_view name, frc::EventLoop&& loop) + : loop{std::make_unique(std::move(loop))}, name{name} {} + + AutoLoop(const AutoLoop&) = delete; + AutoLoop& operator=(const AutoLoop&) = delete; /** - * Polls the loop. Should be called in the autonomous periodic method. + * The default move constructor + * + * @param other the AutoLoop to move into the instance */ + AutoLoop(AutoLoop&& other) noexcept = default; + + /** + * The default move assignment operator + * + * @param other the AutoLoop to move into the instance + * @return the moved AutoLoop + */ + AutoLoop& operator=(AutoLoop&& other) noexcept = default; + + /** + * Returns a frc2::Trigger that is true while this autonomous loop is being + * polled. + * + * Using a frc2::Trigger.OnFalse() will do nothing as when this is false the + * loop is not being polled anymore. + * + * @return A frc2::Trigger that is true while this autonomous loop is being + * polled. + */ + frc2::Trigger Enabled() { + return frc2::Trigger{loop.get(), [this] { + return isActive && + frc::DriverStation::IsAutonomousEnabled(); + }}; + } + + /// Polls the loop. Should be called in the autonomous periodic method. void Poll() { if (!frc::DriverStation::IsAutonomousEnabled() || isKilled) { isActive = false; return; } - loop.Poll(); + loop->Poll(); isActive = true; } @@ -48,17 +98,14 @@ class AutoLoop { * * @return The event loop that this loop is using. */ - frc::EventLoop* GetLoop() { return &loop; } + frc::EventLoop* GetLoop() { return loop.get(); } /** * Resets the loop. This can either be called on auto init or auto end to * reset the loop incase you run it again. If this is called on a loop that * doesn't need to be reset it will do nothing. */ - void Reset() { - isActive = false; - OnNewTrajectory(); - } + void Reset() { isActive = false; } /** * Kills the loop and prevents it from running again. @@ -111,25 +158,8 @@ class AutoLoop { } private: - void OnNewTrajectory() { - for (AutoTrajectory trajectory : trajectories) { - trajectory.OnNewTrajectory(); - } - } - - frc2::Trigger Enabled() { - return frc2::Trigger{loop, [this] { - return isActive && - frc::DriverStation::IsAutonomousEnabled(); - }}; - } - - void AddTrajectory(AutoTrajectory trajectory) { - trajectories.add(std::move(trajectory)); - } - - std::vector> trajectories; - frc::EventLoop loop; + std::unique_ptr loop; + std::string name; bool isActive = false; bool isKilled = false; }; diff --git a/choreolib/src/main/native/include/choreo/auto/AutoTrajectory.h b/choreolib/src/main/native/include/choreo/auto/AutoTrajectory.h index 09f9b66a0a..29c74135d6 100644 --- a/choreolib/src/main/native/include/choreo/auto/AutoTrajectory.h +++ b/choreolib/src/main/native/include/choreo/auto/AutoTrajectory.h @@ -3,38 +3,89 @@ #pragma once #include +#include #include #include #include +#include #include #include #include #include +#include #include #include #include #include +#include "choreo/auto/AutoBindings.h" #include "choreo/trajectory/Trajectory.h" #include "choreo/trajectory/TrajectorySample.h" +#include "choreo/util/AllianceFlipperUtil.h" namespace choreo { -using TrajectoryLogger = std::function; +template +using TrajectoryLogger = std::function, bool)>; static constexpr units::meter_t DEFAULT_TOLERANCE = 3_in; +/** + * A struct to hold CommandsPtrs and keep track if they were triggered along the + * path + * + */ +struct ScheduledEvent { + /** + * The time through the path the command is supposed to be triggered + */ + units::second_t triggerTime; + /** + * The name of the event marker + */ + std::string name; + /** + * The Command Factory to get the command from + */ + std::function commandFactory; + /** + * The CommandPtr to run + */ + frc2::CommandPtr command; + /** + * If the event has been triggered yet + */ + bool hasTriggered = false; +}; + /** * A class that represents a trajectory that can be used in an autonomous * routine and have triggers based off of it. * * @tparam SampleType The type of samples in the trajectory. - * @tparam Year The field year. + * @tparam Year The field year. Defaults to the current year. */ -template +template class AutoTrajectory { public: + AutoTrajectory() = default; + + AutoTrajectory(const AutoTrajectory&) = delete; + AutoTrajectory& operator=(const AutoTrajectory&) = delete; + + /** + * The move constructor for an auto trajectory + */ + AutoTrajectory(AutoTrajectory&&) = default; + + /** + * The move assignment operator for an auto trajectory + * + * @return the moved trajectory + */ + AutoTrajectory& operator=(AutoTrajectory&&) = default; + /** * Constructs an AutoTrajectory. * @@ -45,28 +96,32 @@ class AutoTrajectory { * @param mirrorTrajectory Getter that determines whether to mirror * trajectory. * @param trajectoryLogger Optional trajectory logger. - * @param driveSubsystem Drive subsystem. + * @param drivebaseRequirements Requirements for the drivebase subsystem * @param loop Event loop. - * @param newTrajectoryCallback New trajectory callback. + * @param autoBindings A shared pointer to mapped choreolib markers to + * CommandPtr factories */ AutoTrajectory(std::string_view name, const choreo::Trajectory& trajectory, std::function poseSupplier, std::function controller, std::function mirrorTrajectory, - std::optional trajectoryLogger, - const frc2::Subsystem& driveSubsystem, frc::EventLoop* loop, - std::function newTrajectoryCallback) + std::optional> trajectoryLogger, + frc2::Requirements drivebaseRequirements, frc::EventLoop* loop, + std::shared_ptr autoBindings) : name{name}, trajectory{trajectory}, poseSupplier{std::move(poseSupplier)}, controller{controller}, mirrorTrajectory{std::move(mirrorTrajectory)}, trajectoryLogger{std::move(trajectoryLogger)}, - driveSubsystem{driveSubsystem}, + drivebaseRequirements{drivebaseRequirements}, loop(loop), - newTrajectoryCallback{std::move(newTrajectoryCallback)}, - offTrigger(loop, [] { return false; }) {} + offTrigger(loop, [] { return false; }) { + for (const auto& [key, cmdFactory] : autoBindings->GetBindings()) { + AddScheduledEvent(key, cmdFactory); + } + } /** * Creates a command that allocates the drive subsystem and follows the @@ -74,18 +129,22 @@ class AutoTrajectory { * * @return The command that will follow the trajectory */ - frc2::CommandPtr Cmd() const { + frc2::CommandPtr Cmd() { if (trajectory.samples.size() == 0) { - return frc2::cmd::RunOnce([] { + return frc2::cmd::RunOnce([this] { FRC_ReportError(frc::warn::Warning, - fmt::format("Trajectory {} has no samples", name); + "Trajectory {} has no samples", name); }) .WithName("Trajectory_" + name); } return frc2::FunctionalCommand( - [this] { return CmdInitiazlize(); }, - [this] { return CmdExecute(); }, [this] { return CmdEnd(); }, - [this] { return CmdIsFinished(); }, {driveSubsystem}) + [this] { return CmdInitialize(); }, + [this] { + CmdExecute(); + CheckAndTriggerEvents(); + }, + [this](bool interrupted) { return CmdEnd(interrupted); }, + [this] { return CmdIsFinished(); }, drivebaseRequirements) .WithName("Trajectory_" + name); } @@ -150,7 +209,16 @@ class AutoTrajectory { * @return A trigger that is true when the command is finished. */ frc2::Trigger Done() { - return frc2::Trigger(loop, [this] { return isDone; }); + return frc2::Trigger(loop, [this] { + if (isActive) { + wasJustActive = true; + return false; + } else if (wasJustActive) { + wasJustActive = false; + return true; + } + return false; + }); } /** @@ -163,26 +231,32 @@ class AutoTrajectory { frc2::Trigger AtTime(units::second_t timeSinceStart) { if (timeSinceStart < 0_s) { FRC_ReportError(frc::warn::Warning, - "Trigger time cannot be negative for" + name); + "Trigger time cannot be negative for {}", name); return offTrigger; } if (timeSinceStart > TotalTime()) { FRC_ReportError( frc::warn::Warning, - "Trigger time cannout be greater than total trajectory time for " + - name); + "Trigger time cannout be greater than total trajectory time for {}", + name); return offTrigger; } - return frc2::Trigger( - loop, [this, timeSinceStart, lastTimestamp = timer.Get()]() mutable { - units::second_t nowTimestamp = timer.Get(); - bool shouldTrigger = - lastTimestamp < nowTimestamp && nowTimestamp >= timeSinceStart; - lastTimestamp = nowTimestamp; - return shouldTrigger; - }); + return frc2::Trigger(loop, + [this, timeSinceStart, triggered = false]() mutable { + if (!isActive) { + return false; + } + if (triggered) { + return false; + } + if (TimeIntoTraj() >= timeSinceStart) { + triggered = true; + return true; + } + return false; + }); } /** @@ -201,7 +275,7 @@ class AutoTrajectory { frc2::Trigger trig = offTrigger; for (const auto& event : trajectory.GetEvents(eventName)) { - trig = frc2::Trigger(trig || AtTime(event.timestamp)); + trig = frc2::Trigger{trig || AtTime(event.timestamp)}; foundEvent = true; } @@ -232,7 +306,8 @@ class AutoTrajectory { for (const auto& event : trajectory.GetEvents(eventName)) { frc::Pose2d pose = - trajectory.SampleAt(event.timestamp, mirrorTrajectory()) + trajectory + .template SampleAt(event.timestamp, mirrorTrajectory()) .GetPose(); trig = frc2::Trigger(trig || AtPose(pose, tolerance)); foundEvent = true; @@ -266,35 +341,33 @@ class AutoTrajectory { } private: - void OnNewTrajectory() { - isDone = false; - isActive = false; - } + units::second_t TimeIntoTraj() const { return timer.Get() + timeOffset; } - units::second_t TimeIntoTraj() { return timer.Get() + timeOffset; } - - units::second_t TotalTime() { return trajectory.GetTotalTime(); } + units::second_t TotalTime() const { return trajectory.GetTotalTime(); } void LogTrajectory(bool starting) { if (trajectoryLogger.has_value()) { - trajectoryLogger.value()(trajectory.GetPoses(), starting); + trajectoryLogger.value()( + mirrorTrajectory() ? trajectory.template Flipped() : trajectory, + starting); } } - void CmdInitiazlize() { - newTrajectoryCallback(); + void CmdInitialize() { timer.Restart(); - isDone = false; isActive = true; - timeOffset = 0.0; + timeOffset = 0.0_s; + for (auto& event : scheduledEvents) { + event.hasTriggered = false; + } LogTrajectory(true); } void CmdExecute() { - SampleType sample = - trajectory.SampleAt(TimeIntoTraj(), mirrorTrajectory()); - controller(poseSupplier(), sample); - currentSample = sample; + auto sampleOpt = + trajectory.template SampleAt(TimeIntoTraj(), mirrorTrajectory()); + controller(poseSupplier(), sampleOpt.value()); + currentSample = sampleOpt.value(); } void CmdEnd(bool interrupted) { @@ -302,9 +375,8 @@ class AutoTrajectory { if (interrupted) { controller(currentSample.GetPose(), currentSample); } else { - controller(poseSupplier(), trajectory.GetFinalSample()); + controller(poseSupplier(), trajectory.GetFinalSample().value()); } - isDone = true; isActive = false; LogTrajectory(false); } @@ -314,8 +386,8 @@ class AutoTrajectory { frc2::Trigger AtPose(frc::Pose2d pose, units::meter_t tolerance) { frc::Translation2d checkedTrans = mirrorTrajectory() - ? frc::Translation2d{16.5410515_m - pose.Translation().X(), - pose.Translation().Y} + ? frc::Translation2d{util::fieldLength - pose.Translation().X(), + pose.Translation().Y()} : pose.Translation(); return frc2::Trigger{ loop, [this, checkedTrans, tolerance] { @@ -324,20 +396,39 @@ class AutoTrajectory { }}; } + void AddScheduledEvent(std::string_view eventName, + std::function cmdFactory) { + for (const auto& event : trajectory.GetEvents(eventName)) { + scheduledEvents.push_back({event.timestamp, std::string(eventName), + cmdFactory, frc2::cmd::None(), false}); + } + } + + void CheckAndTriggerEvents() { + auto currentTime = TimeIntoTraj(); + for (auto& event : scheduledEvents) { + if (!event.hasTriggered && isActive && currentTime >= event.triggerTime) { + event.hasTriggered = true; + event.command = event.commandFactory(); + event.command.Schedule(); + } + } + } + std::string name; - const choreo::Trajectory& trajectory; + choreo::Trajectory trajectory; std::function poseSupplier; std::function controller; std::function mirrorTrajectory; - std::optional trajectoryLogger; - const frc2::Subsystem& driveSubsystem; + std::optional> trajectoryLogger; + frc2::Requirements drivebaseRequirements; frc::EventLoop* loop; - std::function newTrajectoryCallback; + std::vector scheduledEvents; SampleType currentSample; frc::Timer timer; - bool isDone = false; bool isActive = false; + bool wasJustActive = false; units::second_t timeOffset = 0_s; frc2::Trigger offTrigger; }; diff --git a/choreolib/src/main/native/include/choreo/auto/TrajectoryCache.h b/choreolib/src/main/native/include/choreo/auto/TrajectoryCache.h new file mode 100644 index 0000000000..9f100b3985 --- /dev/null +++ b/choreolib/src/main/native/include/choreo/auto/TrajectoryCache.h @@ -0,0 +1,89 @@ +// Copyright (c) Choreo contributors + +#pragma once + +#include +#include + +#include "choreo/Choreo.h" +#include "choreo/trajectory/Trajectory.h" +#include "choreo/trajectory/TrajectorySample.h" + +namespace choreo { +/** + * A utility for caching loaded trajectories. This allows for loading + * trajectories only once, and then reusing them. + */ +template +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> LoadTrajectory( + std::string_view trajectoryName) { + if (cache.contains(std::string{trajectoryName})) { + return cache[std::string{trajectoryName}]; + } else { + cache[std::string{trajectoryName}] = + Choreo::LoadTrajectory(trajectoryName); + return cache[std::string{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> LoadTrajectory( + std::string_view trajectoryName, int splitIndex) { + std::string key = fmt::format("{}.:.{}", trajectoryName, splitIndex); + + if (!cache.contains(key)) { + if (cache.contains(std::string{trajectoryName})) { + cache[key] = cache[std::string{trajectoryName}].GetSplit(splitIndex); + } else { + auto possibleTrajectory = LoadTrajectory(trajectoryName); + cache[std::string{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, std::optional>> + cache; +}; +} // namespace choreo diff --git a/choreolib/src/main/native/include/choreo/trajectory/Trajectory.h b/choreolib/src/main/native/include/choreo/trajectory/Trajectory.h index 453a8253f4..2548126ea6 100644 --- a/choreolib/src/main/native/include/choreo/trajectory/Trajectory.h +++ b/choreolib/src/main/native/include/choreo/trajectory/Trajectory.h @@ -16,7 +16,6 @@ #include "choreo/trajectory/EventMarker.h" #include "choreo/trajectory/SwerveSample.h" #include "choreo/trajectory/TrajectorySample.h" -#include "choreo/util/AllianceFlipperUtil.h" namespace choreo { @@ -55,7 +54,7 @@ class Trajectory { * * @return The first sample in the trajectory. */ - std::optional GetInitialState() { + std::optional GetInitialState() const { if (samples.size() == 0) { return {}; } @@ -69,7 +68,7 @@ class Trajectory { * * @return The last sample in the trajectory. */ - std::optional GetFinalSample() { + std::optional GetFinalSample() const { if (samples.size() == 0) { return {}; } @@ -81,7 +80,7 @@ class Trajectory { * * This function will return an empty optional if the trajectory is empty. * - * @tparam Year The field year (default: the current year). + * @tparam Year The field year. Defaults to the current year. * @param timestamp The timestamp of this sample relative to the beginning of * the trajectory. * @param mirrorForRedAlliance whether or not to return the sample mirrored. @@ -89,7 +88,7 @@ class Trajectory { */ template std::optional SampleAt(units::second_t timestamp, - bool mirrorForRedAlliance = false) { + bool mirrorForRedAlliance = false) const { std::optional state{}; if (samples.size() == 0) { return {}; @@ -111,12 +110,12 @@ class Trajectory { * * Will return an empty optional if the trajectory is empty * - * @tparam Year The field year (default: the current year). + * @tparam Year The field year. Defaults to the current year. * @param mirrorForRedAlliance whether or not to return the Pose mirrored. * @return The first Pose in the trajectory. */ template - std::optional GetInitialPose(bool mirrorForRedAlliance) { + std::optional GetInitialPose(bool mirrorForRedAlliance) const { if (samples.size() == 0) { return {}; } @@ -131,12 +130,12 @@ class Trajectory { * * Will return an empty optional if the trajectory is empty * - * @tparam Year The field year (default: the current year). + * @tparam Year The field year. Defaults to the current year. * @param mirrorForRedAlliance whether or not to return the Pose mirrored. * @return The last Pose in the trajectory. */ template - std::optional GetFinalPose(bool mirrorForRedAlliance) { + std::optional GetFinalPose(bool mirrorForRedAlliance) const { if (samples.size() == 0) { return {}; } @@ -152,7 +151,7 @@ class Trajectory { * @return The total time the trajectory will take to follow, if empty will * return 0 seconds. */ - units::second_t GetTotalTime() { + units::second_t GetTotalTime() const { if (samples.size() == 0) { return 0_s; } @@ -164,7 +163,7 @@ class Trajectory { * * @return the vector of poses corresponding to the trajectory. */ - std::vector GetPoses() { + std::vector GetPoses() const { std::vector poses; for (const auto& sample : samples) { poses.push_back(sample.GetPose()); @@ -175,11 +174,11 @@ class Trajectory { /** * Returns this trajectory, mirrored across the field midline. * - * @tparam Year The field year (default: the current year). + * @tparam Year The field year. Defaults to the current year. * @return this trajectory, mirrored across the field midline. */ template - Trajectory Flipped() { + Trajectory Flipped() const { std::vector flippedStates; for (const auto& state : samples) { flippedStates.push_back(state.template Flipped()); @@ -194,7 +193,7 @@ class Trajectory { * @return A vector of all events with the given name in the trajectory, if no * events are found, an empty vector is returned. */ - std::vector GetEvents(std::string_view eventName) { + std::vector GetEvents(std::string_view eventName) const { std::vector matchingEvents; for (const auto& event : events) { if (event.event == eventName) { @@ -290,7 +289,7 @@ class Trajectory { std::vector events; private: - std::optional SampleInternal(units::second_t timestamp) { + std::optional SampleInternal(units::second_t timestamp) const { if (timestamp < samples[0].GetTimestamp()) { return GetInitialState(); } diff --git a/choreolib/src/main/native/include/choreo/util/AllianceFlipperUtil.h b/choreolib/src/main/native/include/choreo/util/AllianceFlipperUtil.h index 48d4c3db81..7bf17080b9 100644 --- a/choreolib/src/main/native/include/choreo/util/AllianceFlipperUtil.h +++ b/choreolib/src/main/native/include/choreo/util/AllianceFlipperUtil.h @@ -107,7 +107,7 @@ inline constexpr int kDefaultYear = 2024; * Grabs the instance of the flipper for the supplied template parameter. Will * not compile if an invalid year is supplied. * - * @tparam Year The field year (default: the current year). + * @tparam Year The field year. Defaults to the current year. */ template constexpr auto GetFlipperForYear() {