PathPlannerPath.h

#pragma once
 
#include "pathplanner/lib/path/RotationTarget.h"
#include "pathplanner/lib/path/PointTowardsZone.h"
#include "pathplanner/lib/path/ConstraintsZone.h"
#include "pathplanner/lib/path/EventMarker.h"
#include "pathplanner/lib/path/PathConstraints.h"
#include "pathplanner/lib/path/IdealStartingState.h"
#include "pathplanner/lib/path/GoalEndState.h"
#include "pathplanner/lib/path/PathPoint.h"
#include "pathplanner/lib/path/Waypoint.h"
#include "pathplanner/lib/trajectory/PathPlannerTrajectory.h"
#include "pathplanner/lib/config/RobotConfig.h"
#include <vector>
#include <optional>
#include <frc/geometry/Translation2d.h>
#include <frc/kinematics/ChassisSpeeds.h>
#include <wpi/json.h>
#include <string>
#include <unordered_map>
#include <units/length.h>
#include <memory>
#include <initializer_list>
 
namespace pathplanner {
class PathPlannerPath: public std::enable_shared_from_this<PathPlannerPath> {
public:
    std::string name;
    bool preventFlipping = false;
 
    PathPlannerPath(std::vector<Waypoint> waypoints,
            std::vector<RotationTarget> rotationTargets,
            std::vector<PointTowardsZone> pointTowardsZones,
            std::vector<ConstraintsZone> constraintZones,
            std::vector<EventMarker> eventMarkers,
            PathConstraints globalConstraints,
            std::optional<IdealStartingState> idealStartingState,
            GoalEndState goalEndState, bool reversed);
 
    PathPlannerPath(std::vector<Waypoint> waypoints,
            PathConstraints constraints,
            std::optional<IdealStartingState> idealStartingState,
            GoalEndState goalEndState, bool reversed = false) : PathPlannerPath(
            waypoints, std::vector<RotationTarget>(),
            std::vector<PointTowardsZone>(), std::vector<ConstraintsZone>(),
            std::vector<EventMarker>(), constraints, idealStartingState,
            goalEndState, reversed) {
    }
 
    PathPlannerPath(PathConstraints constraints, GoalEndState goalEndState);
 
    void hotReload(const wpi::json &json);
 
    static std::vector<Waypoint> waypointsFromPoses(
            std::vector<frc::Pose2d> poses);
 
    [[deprecated("Renamed to waypointsFromPoses")]]
    static inline std::vector<Waypoint> bezierFromPoses(
            std::vector<frc::Pose2d> poses) {
        return waypointsFromPoses(poses);
    }
 
    static std::shared_ptr<PathPlannerPath> fromPathFile(std::string pathName);
 
    static std::shared_ptr<PathPlannerPath> fromChoreoTrajectory(
            std::string trajectoryName);
 
    static inline std::shared_ptr<PathPlannerPath> fromChoreoTrajectory(
            std::string trajectoryName, size_t splitIndex) {
        std::string cacheName = trajectoryName + "."
                + std::to_string(splitIndex);
        if (getChoreoPathCache().contains(cacheName)) {
            return getChoreoPathCache()[cacheName];
        }
 
        // Path is not in the cache, load the main trajectory to load all splits
        loadChoreoTrajectoryIntoCache(trajectoryName);
        return getChoreoPathCache()[cacheName];
    }
 
    frc::Pose2d getStartingDifferentialPose();
 
    std::optional<frc::Pose2d> getStartingHolonomicPose();
 
    static std::shared_ptr<PathPlannerPath> fromPathPoints(
            std::vector<PathPoint> pathPoints,
            PathConstraints globalConstraints, GoalEndState goalEndState);
 
    constexpr const std::vector<PathPoint>& getAllPathPoints() const {
        return m_allPoints;
    }
 
    inline size_t numPoints() const {
        return m_allPoints.size();
    }
 
    inline const PathPoint& getPoint(size_t index) const {
        return m_allPoints[index];
    }
 
    std::optional<PathPlannerTrajectory> getIdealTrajectory(
            RobotConfig robotConfig);
 
    inline frc::Rotation2d getInitialHeading() const {
        return (getPoint(1).position - getPoint(0).position).Angle();
    }
 
    constexpr std::vector<Waypoint>& getWaypoints() {
        return m_waypoints;
    }
 
    constexpr std::vector<RotationTarget>& getRotationTargets() {
        return m_rotationTargets;
    }
 
    constexpr std::vector<PointTowardsZone>& getPointTowardsZones() {
        return m_pointTowardsZones;
    }
 
    constexpr std::vector<ConstraintsZone>& getConstraintZones() {
        return m_constraintZones;
    }
 
    constexpr const PathConstraints& getGlobalConstraints() const {
        return m_globalConstraints;
    }
 
    constexpr const GoalEndState& getGoalEndState() const {
        return m_goalEndState;
    }
 
    constexpr const std::optional<IdealStartingState>& getIdealStartingState() const {
        return m_idealStartingState;
    }
 
    constexpr std::vector<EventMarker>& getEventMarkers() {
        return m_eventMarkers;
    }
 
    constexpr bool isReversed() const {
        return m_reversed;
    }
 
    constexpr bool isChoreoPath() const {
        return m_isChoreoPath;
    }
 
    inline PathPlannerTrajectory generateTrajectory(
            frc::ChassisSpeeds startingSpeeds, frc::Rotation2d startingRotation,
            const RobotConfig &config) {
        if (m_isChoreoPath) {
            return m_idealTrajectory.value();
        } else {
            return PathPlannerTrajectory(shared_from_this(), startingSpeeds,
                    startingRotation, config);
        }
    }
 
    std::shared_ptr<PathPlannerPath> flipPath();
 
    std::shared_ptr<PathPlannerPath> mirrorPath();
 
    inline std::vector<frc::Pose2d> getPathPoses() const {
        std::vector < frc::Pose2d > poses;
        for (const PathPoint &point : m_allPoints) {
            poses.emplace_back(point.position, frc::Rotation2d());
        }
        return poses;
    }
 
    static inline void clearPathCache() {
        PathPlannerPath::getPathCache().clear();
        PathPlannerPath::getChoreoPathCache().clear();
    }
 
private:
    std::vector<PathPoint> createPath();
 
    static std::shared_ptr<PathPlannerPath> fromJson(const wpi::json &json);
 
    static inline std::vector<Waypoint> waypointsFromJson(
            const wpi::json &waypointsJson) {
        std::vector < Waypoint > waypoints;
        for (wpi::json::const_reference waypoint : waypointsJson) {
            waypoints.emplace_back(Waypoint::fromJson(waypoint));
        }
        return waypoints;
    }
 
    static void loadChoreoTrajectoryIntoCache(std::string trajectoryName);
 
    void precalcValues();
 
    static units::meter_t getCurveRadiusAtPoint(size_t index,
            std::vector<PathPoint> &points);
 
    inline PathConstraints constraintsForWaypointPos(double pos) const {
        for (auto z : m_constraintZones) {
            if (pos >= z.getMinWaypointRelativePos()
                    && pos <= z.getMaxWaypointRelativePos()) {
                return z.getConstraints();
            }
        }
 
        // Check if constraints should be unlimited
        if (m_globalConstraints.isUnlimited()) {
            return PathConstraints::unlimitedConstraints(
                    m_globalConstraints.getNominalVoltage());
        }
 
        return m_globalConstraints;
    }
 
    inline std::optional<PointTowardsZone> pointZoneForWaypointPos(
            double pos) const {
        for (auto z : m_pointTowardsZones) {
            if (pos >= z.getMinWaypointRelativePos()
                    && pos <= z.getMaxWaypointRelativePos()) {
                return z;
            }
        }
        return std::nullopt;
    }
 
    static frc::Translation2d mirrorTranslation(frc::Translation2d translation);
 
    frc::Translation2d samplePath(double waypointRelativePos) const;
 
    static std::unordered_map<std::string, std::shared_ptr<PathPlannerPath>>& getPathCache();
 
    static std::unordered_map<std::string, std::shared_ptr<PathPlannerPath>>& getChoreoPathCache();
 
    std::vector<Waypoint> m_waypoints;
    std::vector<RotationTarget> m_rotationTargets;
    std::vector<PointTowardsZone> m_pointTowardsZones;
    std::vector<ConstraintsZone> m_constraintZones;
    std::vector<EventMarker> m_eventMarkers;
    PathConstraints m_globalConstraints;
    std::optional<IdealStartingState> m_idealStartingState;
    GoalEndState m_goalEndState;
    std::vector<PathPoint> m_allPoints;
    bool m_reversed;
 
    bool m_isChoreoPath;
    std::optional<PathPlannerTrajectory> m_idealTrajectory = std::nullopt;
 
    static int m_instances;
 
    static constexpr double targetIncrement = 0.05;
    static constexpr units::meter_t targetSpacing = 0.2_m;
};
}