Cpp Example: Automatic Pathfinding

PathPlannerLib now includes a set of commands that will automatically plan a path between two points while avoiding obstacles on the field. They can also be used to generate a path to another path, allowing you to chain together a generated and pre-planned path for finer control.

A few important considerations to note before attempting to use these commands:

• There must be a navgrid.json file present in your deploy/pathplanner directory in order for the system to know where obstacles are. This file will be created automatically when opening the project in pathplanner. You can edit the navgrid in the GUI, but you probably shouldn't have to.
• You have no control of the robot's heading at the start and end points. In other words, you can't attempt to pathfind to a position to the left of the robot, but have it arrive at that point while moving to the right. The shortest path from A to B will be used.
• Because of the above, this is more difficult to get great results with a differential drivetrain. It will still be possible, you just need to take more care with it. For example, doing a turn in place command if your robot is not facing the direction it will travel when pathfinding.
• Even with a holonomic drive train, its not that great at lining up with things (for example, a human player station) because of the heading restriction. This is why the ability to chain paths together exists. It is recommended to create a pre-planned path for doing the final line up with something, then pathfind to that path.
• The AD* algorithm used for pathfinding does not just produce one path, it produces a few as it further refines the path in the background. In some rare cases, the robot could start moving in one direction, then switch to the other direction when AD* figures out that direction is more optimal.
• You are able to customize the replanning config for the path you follow after pathfinding, but you should at least have initial replanning enabled for this path. It is essentially required for it to work properly.

The following examples will only show the holonomic command variants. But, the Ramsete and LTV variants are very similar and will have similar config to their normal path following command versions.

Pathfind to Pose

AutoBuilder

The easiest way to create a pathfinding command is by using AutoBuilder. See Cpp Example: Build an Auto to configure AutoBuilder.

#include <pathplanner/lib/auto/AutoBuilder.h>

using namespace pathplanner;

// Since we are using a holonomic drivetrain, the rotation component of this pose
// represents the goal holonomic rotation
frc::Pose2d targetPose = frc::Pose2d(10_m, 5_m, frc::Rotation2d(180_deg));

// Create the constraints to use while pathfinding
PathConstraints constraints = PathConstraints(
    3.0_mps, 4.0_mps_sq, 
    540_deg_per_s, 720_deg_per_s);

// Since AutoBuilder is configured, we can use it to build pathfinding commands
frc2::CommandPtr pathfindingCommand = AutoBuilder::pathfindToPose(
    targetPose,
    constraints,
    0.0_mps, // Goal end velocity in meters/sec
    0.0_m // Rotation delay distance in meters. This is how far the robot should travel before attempting to rotate.
).ToPtr();

Manual

#include <pathplanner/lib/commands/PathfindHolonomic.h>

using namespace pathplanner;

// Since we are using a holonomic drivetrain, the rotation component of this pose
// represents the goal holonomic rotation
frc::Pose2d targetPose = frc::Pose2d(10_m, 5_m, frc::Rotation2d(180_deg));

// Create the constraints to use while pathfinding
PathConstraints constraints = PathConstraints(
    3.0_mps, 4.0_mps_sq, 
    540_deg_per_s, 720_deg_per_s);

// See the "Follow a single path" example for more info on what gets passed here
// Assuming this is somewhere in the drive subsystem, so using "this"
frc2::CommandPtr pathfindingCommand = PathfindHolonomic(
    targetPose,
    constraints,
    0.0_mps, // Goal end velocity in m/s. Optional
    [this](){ return getPose(); },
    [this](){ return getRobotRelativeSpeeds(); },
    [this](frc::ChassisSpeeds speeds){ driveRobotRelative(speeds); }, // HolonomicPathFollwerConfig, see the API or "Follow a single path" example for more info
    Constants::Swerve::pathFollowingConfig, // HolonomicPathFollwerConfig, see the API or "Follow a single path" example for more info
    this, // Pointer to drive subsystem to set requirements
    0.0_m // Rotation delay distance in meters. This is how far the robot should travel before attempting to rotate. Optional
).ToPtr();

Pathfind Then Follow Path

AutoBuilder

The easiest way to create a pathfinding command is by using AutoBuilder. See Cpp Example: Build an Auto to configure AutoBuilder.

#include <pathplanner/lib/auto/AutoBuilder.h>

using namespace pathplanner;

// Load the path we want to pathfind to and follow
auto path = PathPlannerPath::fromPathFile("Example Human Player Pickup");

// Create the constraints to use while pathfinding. The constraints defined in the path will only be used for the path.
PathConstraints constraints = PathConstraints(
    3.0_mps, 4.0_mps_sq, 
    540_deg_per_s, 720_deg_per_s);

// Since AutoBuilder is configured, we can use it to build pathfinding commands
frc2::CommandPtr pathfindingCommand = AutoBuilder::pathfindThenFollowPath(
    path,
    constraints,
    3.0_m // Rotation delay distance in meters. This is how far the robot should travel before attempting to rotate.
).ToPtr();

Manual

#include <pathplanner/lib/commands/PathfindThenFollowPath.h>

using namespace pathplanner;

// Load the path we want to pathfind to and follow
auto path = PathPlannerPath::fromPathFile("Example Human Player Pickup");

// Create the constraints to use while pathfinding
PathConstraints constraints = PathConstraints(
    3.0_mps, 4.0_mps_sq, 
    540_deg_per_s, 720_deg_per_s);

// See the "Follow a single path" example for more info on what gets passed here
// Assuming this is somewhere in the drive subsystem, so using "this"
frc2::CommandPtr pathfindingCommand = PathfindThenFollowPathHolonomic(
    path,
    constraints,
    [this](){ return getPose(); },
    [this](){ return getRobotRelativeSpeeds(); },
    [this](frc::ChassisSpeeds speeds){ driveRobotRelative(speeds); }, // HolonomicPathFollwerConfig, see the API or "Follow a single path" example for more info
    Constants::Swerve::pathFollowingConfig, // HolonomicPathFollwerConfig, see the API or "Follow a single path" example for more info
    this, // Reference to drive subsystem to set requirements
    3.0_m, // Rotation delay distance in meters. This is how far the robot should travel before attempting to rotate. Optional
).ToPtr();