PathPlannerLib: Cpp Usage

Documentation

While the Java version of PathPlannerLib can function as a drop-in replacement of a WPILib trajectory, this is unfortunately not possible for the C++ version due to how inheritance works in C++. The C++ library is still designed to work very similarly to a WPILib trajectory, so any code that can follow one can easily be modified to run the PathPlanner version. This library fully utilizes the WPILib Units Library for any number with an associated unit.

Examples

Basic Path

#include <pathplanner/lib/PathPlanner.h>

using namespace pathplanner;

// This will load the file "Example Path.path" and generate it with a max velocity of 4 m/s and a max acceleration of 3 m/s^2
PathPlannerTrajectory examplePath = PathPlanner::loadPath("Example Path", PathConstraints(4_mps, 3_mps_sq));

// Sample the state of the path at 1.2 seconds
PathPlannerTrajectory::PathPlannerState exampleState = examplePath.sample(1.2_s);

Path Group

#include <pathplanner/lib/PathPlanner.h>

using namespace pathplanner;

// This will load the file "Example Path Group.path" and generate it with a max velocity of 4 m/s and a max acceleration of 3 m/s^2
// for every path in the group
std::vector<PathPlannerTrajectory> examplePathGroup1 = PathPlanner::loadPathGroup("Example Path Group", {PathConstraints(4_mps, 3_mps_sq)});

// This will load the file "Example Path Group.path" and generate it different constraints for every path in the group
std::vector<PathPlannerTrajectory> examplePathGroup2 = PathPlanner::loadPathGroup("Example Path Group", {PathConstraints(4_mps, 3_mps_sq), PathConstraints(2_mps, 2_mps_sq), PathConstraints(3_mps, 3_mps_sq)});

AutoBuilder

This example will use PPLib's auto builder functionality to generate a full autonomous command. This will make use of path groups, event markers, and stop events to build a full auto. This specific example will use the SwerveAutoBuilder, but other options are available for Mecanum and Ramsete. Custom auto builders can easily be created by extending the BaseAutoBuilder class and overriding its methods to customize functionality.

#include <pathplanner/lib/auto/SwerveAutoBuilder.h>
#include <pathplanner/lib/PathPlanner.h>

using namespace pathplanner;

// This will load the file "FullAuto.path" and generate it with a max velocity of 4 m/s and a max acceleration of 3 m/s^2
// for every path in the group
std::vector<PathPlannerTrajectory> pathGroup = PathPlanner::loadPathGroup("FullAuto", {PathConstraints(4_mps, 3_mps_sq)});

// This is just an example event map. It would be better to have a constant, global event map
// in your code that will be used by all path following commands/autobuilders.
std::unordered_map<std::string, std::shared_ptr<frc2::Command>> eventMap;
eventMap.emplace("marker1", std::make_shared<frc2::PrintCommand>("Passed Marker 1"));
eventMap.emplace("intakeDown", std::make_shared<IntakeDown>());

// Create the AutoBuilder. This only needs to be created once when robot code starts, not every time you want to create an auto command. A good place to put this could be in RobotContainer along with your subsystems

SwerveAutoBuilder autoBuilder(
    [this]() { return swerveSubsystem.getPose(); }, // Function to supply current robot pose
    [this](auto initPose) { swerveSubsystem.resetPose(initPose); }, // Function used to reset odometry at the beginning of auto
    PIDConstants(5.0, 0.0, 0.0), // PID constants to correct for translation error (used to create the X and Y PID controllers)
    PIDConstants(0.5, 0.0, 0.0), // PID constants to correct for rotation error (used to create the rotation controller)
    [this](auto speeds) { swerveSubsystem.driveFieldRelative(speeds); }, // Output function that accepts field relative ChassisSpeeds
    eventMap, // Our event map
    { &swerveSubsystem }, // Drive requirements, usually just a single drive subsystem
    true // Should the path be automatically mirrored depending on alliance color. Optional, defaults to true
);

CommandPtr fullAuto = autoBuilder.fullAuto(pathGroup);

FollowPathWithEvents

This example shows how to use the FollowPathWithEvents command. This command will run a given path following command, as well as trigger events at markers along the path.

#include <pathplanner/lib/PathPlannerTrajectory.h>
#include <pathplanner/lib/PathPlanner.h>
#include <pathplanner/lib/commands/FollowPathWithEvents.h>

using namespace pathplanner;

// This will load the file "Example Path.path" and generate it with a max velocity of 4 m/s and a max acceleration of 3 m/s^2
PathPlannerTrajectory examplePath = PathPlanner::loadPath("Example Path", PathConstrains(4, 3));

// This is just an example event map. It would be better to have a constant, global event map
// in your code that will be used by all path following commands.
std::unordered_map<std::string, std::shared_ptr<frc2::Command> eventMap;
eventMap.emplace("marker1", std::make_shared<frc2::PrintCommand>("Passed marker 1"));
eventMap.emplace("intakeDown", std::make_shared<IntakeDown>());

FollowPathWithEvents command(
    getPathFollowingCommand(examplePath),
    examplePath.getMarkers(),
    eventMap
);

On-the-fly Generation

This example uses the generatePath method to generate paths that have not been planned in advance with a .path file

#include <pathplanner/lib/PathPlannerTrajectory.h>
#include <pathplanner/lib/PathPlanner.h>
#include <pathplanner/lib/PathPoint.h>

using namespace pathplanner;

// Simple path without holonomic rotation. Stationary start/end. Max velocity of 4 m/s and max accel of 3 m/s^2
PathPlannerTrajectory traj1 = PathPlanner::generatePath(
    PathConstraints(4_mps, 3_mps_sq), 
    PathPoint(frc::Translation2d(1_m, 1_m), frc::Rotation2d(0_deg)), // position, heading
    PathPoint(frc::Translation2d(3_m, 3_m), frc::Rotation2d(45_deg)) // position, heading
);

// Simple path with holonomic rotation. Stationary start/end. Max velocity of 4 m/s and max accel of 3 m/s^2
PathPlannerTrajectory traj2 = PathPlanner::generatePath(
    PathConstraints(4_mps, 3_mps_sq), 
    PathPoint(frc::Translation2d(1_m, 1_m), frc::Rotation2d(0_deg), frc::Rotation2d(0_deg)), // position, heading(direction of travel), holonomic rotation
    PathPoint(frc::Translation2d(3_m, 3_m), frc::Rotation2d(45_deg), frc::Rotation2d(-90_deg) // position, heading(direction of travel) holonomic rotation
);

// More complex path with holonomic rotation. Non-zero starting velocity of 2 m/s. Max velocity of 4 m/s and max accel of 3 m/s^2
PathPlannerTrajectory traj2 = PathPlanner::generatePath(
    PathConstraints(4_mps, 3_mps_sq), 
    PathPoint(frc::Translation2d(1_m, 1_m), frc::Rotation2d(0_deg), frc::Rotation2d(0_deg), 2_mps), // position, heading(direction of travel), holonomic rotation, velocity override
    PathPoint(frc::Translation2d(3_m, 3_m), frc::Rotation2d(45_deg), frc::Rotation2d(-90_deg) // position, heading(direction of travel), holonomic rotation
    {
        PathPoint(frc::Translation2d(5_m, 3_m), frc::Rotation2d(0_deg), frc::Rotation2d(-30_deg)) // position, heading(direction of travel), holonomic rotation
    }
);