PathfindingCommand.h

#pragma once
 
#include <frc2/command/Command.h>
#include <frc2/command/CommandHelper.h>
#include <frc2/command/Requirements.h>
#include <memory>
#include <functional>
#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/util/DriveFeedforwards.h"
 
namespace pathplanner {
class PathfindingCommand: public frc2::CommandHelper<frc2::Command,
        PathfindingCommand> {
public:
    PathfindingCommand(std::shared_ptr<PathPlannerPath> targetPath,
            PathConstraints constraints,
            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);
 
    PathfindingCommand(frc::Pose2d targetPose, PathConstraints constraints,
            units::meters_per_second_t goalEndVel,
            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, 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_targetPath;
    frc::Pose2d m_targetPose;
    frc::Pose2d m_originalTargetPose;
    GoalEndState m_goalEndState;
    PathConstraints m_constraints;
    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;
 
    std::shared_ptr<PathPlannerPath> m_currentPath;
    PathPlannerTrajectory m_currentTrajectory;
 
    units::second_t m_timeOffset;
 
    static int m_instances;
};
}