AutoBuilder.h

#pragma once
 
#include <functional>
#include <frc2/command/CommandPtr.h>
#include <frc2/command/Commands.h>
#include <frc/geometry/Pose2d.h>
#include <frc/kinematics/ChassisSpeeds.h>
#include <frc/controller/RamseteController.h>
#include <vector>
#include <map>
#include <frc2/command/Command.h>
#include <frc/smartdashboard/SendableChooser.h>
#include <memory>
#include <wpi/json.h>
#include <wpi/array.h>
#include <string>
#include "pathplanner/lib/path/PathPlannerPath.h"
#include "pathplanner/lib/config/RobotConfig.h"
#include "pathplanner/lib/controllers/PathFollowingController.h"
#include "pathplanner/lib/util/DriveFeedforwards.h"
#include "pathplanner/lib/util/FlippingUtil.h"
 
namespace pathplanner {
 
class PathPlannerAuto;
 
class AutoBuilder {
public:
    static void configure(std::function<frc::Pose2d()> poseSupplier,
            std::function<void(const frc::Pose2d&)> resetPose,
            std::function<frc::ChassisSpeeds()> robotRelativeSpeedsSupplier,
            std::function<
                    void(const frc::ChassisSpeeds&, const DriveFeedforwards&)> output,
            std::shared_ptr<PathFollowingController> controller,
            RobotConfig robotConfig, std::function<bool()> shouldFlipPath,
            frc2::Subsystem *driveSubsystem);
 
    static inline void configure(std::function<frc::Pose2d()> poseSupplier,
            std::function<void(const frc::Pose2d&)> resetPose,
            std::function<frc::ChassisSpeeds()> robotRelativeSpeedsSupplier,
            std::function<void(const frc::ChassisSpeeds&)> output,
            std::shared_ptr<PathFollowingController> controller,
            RobotConfig robotConfig, std::function<bool()> shouldFlipPath,
            frc2::Subsystem *driveSubsystem) {
        configure(poseSupplier, resetPose, robotRelativeSpeedsSupplier,
                [output](auto &&speeds, auto &&feedforwards) {
                    output(speeds);
                }, std::move(controller), std::move(robotConfig),
                shouldFlipPath, driveSubsystem);
    }
 
    static void configureCustom(std::function<frc::Pose2d()> poseSupplier,
            std::function<frc2::CommandPtr(std::shared_ptr<PathPlannerPath>)> pathFollowingCommandBuilder,
            std::function<void(const frc::Pose2d&)> resetPose, bool isHolonomic,
            std::function<bool()> shouldFlipPose = []() {
                return false;
            });
 
    static inline bool isConfigured() {
        return m_configured;
    }
 
    static inline bool isHolonomic() {
        return m_isHolonomic;
    }
 
    static inline frc::Pose2d getCurrentPose() {
        return m_poseSupplier();
    }
 
    static inline bool shouldFlip() {
        return m_shouldFlipPath();
    }
 
    static frc2::CommandPtr followPath(std::shared_ptr<PathPlannerPath> path);
 
    static inline frc2::CommandPtr buildAuto(std::string autoName);
 
    static frc2::CommandPtr resetOdom(frc::Pose2d bluePose);
 
    static frc2::CommandPtr pathfindToPose(frc::Pose2d pose,
            PathConstraints constraints, units::meters_per_second_t goalEndVel =
                    0_mps);
 
    static frc2::CommandPtr pathfindToPoseFlipped(frc::Pose2d pose,
            PathConstraints constraints, units::meters_per_second_t goalEndVel =
                    0_mps) {
        return frc2::cmd::Either(
                pathfindToPose(FlippingUtil::flipFieldPose(pose), constraints,
                        goalEndVel),
                pathfindToPose(pose, constraints, goalEndVel), m_shouldFlipPath);
    }
 
    static frc2::CommandPtr pathfindThenFollowPath(
            std::shared_ptr<PathPlannerPath> goalPath,
            PathConstraints pathfindingConstraints);
 
    static void regenerateSendableReferences();
 
    static frc::SendableChooser<frc2::Command*> buildAutoChooser(
            std::string defaultAutoName = "");
 
    static frc::SendableChooser<frc2::Command*> buildAutoChooserFilter(
            std::function<bool(const PathPlannerAuto&)> filter,
            std::string defaultAutoName = "");
 
    static frc::SendableChooser<frc2::Command*> buildAutoChooserFilterPath(
            std::function<bool(const PathPlannerAuto&, std::filesystem::path)> filter,
            std::string defaultAutoName = "");
 
    static std::vector<std::string> getAllAutoNames();
 
    static std::vector<std::filesystem::path> getAllAutoPaths();
 
private:
    static bool m_configured;
    static std::function<frc2::CommandPtr(std::shared_ptr<PathPlannerPath>)> m_pathFollowingCommandBuilder;
    static std::function<frc::Pose2d()> m_poseSupplier;
    static std::function<void(const frc::Pose2d&)> m_resetPose;
    static std::function<bool()> m_shouldFlipPath;
    static bool m_isHolonomic;
 
    static bool m_commandRefsGeneratedForSendable;
    static frc2::CommandPtr m_noneCommand;
    static std::map<std::filesystem::path, frc2::CommandPtr> m_autoCommands;
 
    static bool m_pathfindingConfigured;
    static std::function<
            frc2::CommandPtr(frc::Pose2d, PathConstraints,
                    units::meters_per_second_t)> m_pathfindToPoseCommandBuilder;
    static std::function<
            frc2::CommandPtr(std::shared_ptr<PathPlannerPath>, PathConstraints)> m_pathfindThenFollowPathCommandBuilder;
};
}