FollowPathCommand.h

#pragma once
 
#include <frc2/command/Command.h>
#include <frc2/command/CommandHelper.h>
#include <frc2/command/Requirements.h>
#include <memory>
#include <functional>
#include <deque>
#include <frc/geometry/Pose2d.h>
#include <frc/kinematics/ChassisSpeeds.h>
#include <frc/Timer.h>
#include <units/velocity.h>
#include <units/length.h>
#include <units/time.h>
#include "pathplanner/lib/path/PathPlannerPath.h"
#include "pathplanner/lib/trajectory/PathPlannerTrajectory.h"
#include "pathplanner/lib/controllers/PathFollowingController.h"
#include "pathplanner/lib/config/RobotConfig.h"
#include "pathplanner/lib/util/PathPlannerLogging.h"
#include "pathplanner/lib/util/PPLibTelemetry.h"
#include "pathplanner/lib/events/EventScheduler.h"
#include "pathplanner/lib/util/DriveFeedforwards.h"
 
namespace pathplanner {
class FollowPathCommand: public frc2::CommandHelper<frc2::Command,
        FollowPathCommand> {
public:
    FollowPathCommand(std::shared_ptr<PathPlannerPath> path,
            std::function<frc::Pose2d()> poseSupplier,
            std::function<frc::ChassisSpeeds()> speedsSupplier,
            std::function<
                    void(const frc::ChassisSpeeds&, const DriveFeedforwards&)> output,
            std::shared_ptr<PathFollowingController> controller,
            RobotConfig robotConfig, std::function<bool()> shouldFlipPath,
            frc2::Requirements requirements);
 
    void Initialize() override;
 
    void Execute() override;
 
    bool IsFinished() override;
 
    void End(bool interrupted) override;
 
private:
    frc::Timer m_timer;
    std::shared_ptr<PathPlannerPath> m_originalPath;
    std::function<frc::Pose2d()> m_poseSupplier;
    std::function<frc::ChassisSpeeds()> m_speedsSupplier;
    std::function<void(const frc::ChassisSpeeds&, const DriveFeedforwards&)> m_output;
    std::shared_ptr<PathFollowingController> m_controller;
    RobotConfig m_robotConfig;
    std::function<bool()> m_shouldFlipPath;
 
    EventScheduler m_eventScheduler;
 
    std::shared_ptr<PathPlannerPath> m_path;
    PathPlannerTrajectory m_trajectory;
};
}