Package me.nabdev.pathfinding

Class Pathfinder

java.lang.Object

me.nabdev.pathfinding.Pathfinder

public class Pathfinder
extends Object

The main pathfinder class, and the only one you should need to interact with.

Nested Class Summary

Nested Classes

Modifier and Type	Class	Description
static enum	Pathfinder.PathfindSnapMode	Determines how vertices will be snapped to the nearest obstacle edge if they are inside of an obstacle.

Field Summary

Fields

Modifier and Type	Field	Description
final Map	map	The map this pathfinder will use

Constructor Summary

Constructors

Constructor	Description
Pathfinder(FieldLoader.FieldData field, double pointSpacing, double cornerPointSpacing, double cornerDist, double clearance, double cornerSplitPercent, boolean injectPoints, boolean normalizeCorners, SearchAlgorithm.SearchAlgorithmType searchAlgorithmType, boolean profiling, double endgameTime)	Create a new pathfinder.

Method Summary

Modifier and Type	Method	Description
final double	clearance()	How far to inflate obstacles
Path	generatePath(edu.wpi.first.math.geometry.Pose2d start, edu.wpi.first.math.geometry.Pose2d target)	Snaps the start and target poses to be outside of obstacles and generates the best path.
Path	generatePath(edu.wpi.first.math.geometry.Pose2d start, edu.wpi.first.math.geometry.Pose2d target, Pathfinder.PathfindSnapMode snapMode)	Snaps the start and target poses according to the snap mode and generates the best path.
Path	generatePath(edu.wpi.first.math.geometry.Pose2d start, ArrayList<edu.wpi.first.math.geometry.Pose2d> target)	Snaps the start and target vertices to be outside of obstacles and generates the best path that passes through all waypoints.
Path	generatePath(Vertex start, Vertex target)	Snaps the start and target vertices to be outside of obstacles and generates the best path.
Path	generatePath(Vertex start, Vertex target, Pathfinder.PathfindSnapMode snapMode)	Snaps the start and target vertices according to the snap mode and generates the best path.
Path	generatePath(Vertex start, Vertex target, Pathfinder.PathfindSnapMode snapMode, ArrayList<Vertex> dynamicVertices)	Snaps the start and target vertices according to the snap mode, calculates visibility graph for dynamic elements, and generates the best path.
edu.wpi.first.math.trajectory.Trajectory	generateTrajectory(edu.wpi.first.math.geometry.Pose2d start, edu.wpi.first.math.geometry.Pose2d target, edu.wpi.first.math.trajectory.TrajectoryConfig config)	Snaps the start and target vertices to be outside of obstacles and generates the best path as a wpilib trajectory.
edu.wpi.first.math.trajectory.Trajectory	generateTrajectory(edu.wpi.first.math.geometry.Pose2d start, edu.wpi.first.math.geometry.Pose2d target, Pathfinder.PathfindSnapMode snapMode, edu.wpi.first.math.trajectory.TrajectoryConfig config)	Snaps the start and target vertices according to the snap mode and generates the best path a wpilib trajectory.
edu.wpi.first.math.trajectory.Trajectory	generateTrajectory(edu.wpi.first.math.geometry.Pose2d start, edu.wpi.first.math.geometry.Pose2d target, Pathfinder.PathfindSnapMode snapMode, ArrayList<Vertex> dynamicVertices, edu.wpi.first.math.trajectory.TrajectoryConfig config)	Snaps the start and target vertices according to the snap mode, calculates visibility graph for dynamic elements, and generates the best path as a wpilib trajectory.
edu.wpi.first.math.trajectory.Trajectory	generateTrajectory(edu.wpi.first.math.geometry.Pose2d start, ArrayList<edu.wpi.first.math.geometry.Pose2d> target, edu.wpi.first.math.trajectory.TrajectoryConfig config)	Snaps the start and target vertices to be outside of obstacles and generates the best path that passes through all waypoints as a wpilib trajectory.
edu.wpi.first.math.trajectory.Trajectory	generateTrajectory(edu.wpi.first.math.geometry.Pose2d start, ArrayList<edu.wpi.first.math.geometry.Pose2d> target, Pathfinder.PathfindSnapMode snapMode, edu.wpi.first.math.trajectory.TrajectoryConfig config)	Snaps the start and target vertices accoridng to the snap mode the best path that passes through all waypoints as a wpilib trajectory.
double	getCornerDist()	How far back along the straightaway to dedicate to making corners
double	getCornerPointSpacing()	Space between points on corners of the path (percent of the curve length)
double	getCornerSplitPercent()	How far each corner should move towards the other point if the distance is too short to allow both corners the full corner distance
static double	getEndgameTime()	Time in seconds when the robot should start to consider endgame obstacles.
boolean	getInjectPoints()	Whether or not to inject points on straightaways
boolean	getNormalizeCorners()	Whether or not to normalize distance between corner points
double	getPointSpacing()	Space between injected points on straightaways in the path (meters)
boolean	getProfiling()	Whether or not to profile the pathfinding process
SearchAlgorithm.SearchAlgorithmType	getSearchAlgorithmType()	The search algorithm to use
void	periodic()	Updates the modifier cache based on data from the driver station and wpilib.
void	setCornerDist(double newCornerDist)	How far back along the straightaway to dedicate to making corners
void	setCornerPointSpacing(double newCornerPointSpacing)	Space between points on corners of the path (percent of the curve length)
void	setCornerSplitPercent(double newCornerSplitPercent)	How far each corner should move towards the other point if the distance is too short to allow both corners the full corner distance
static void	setEndgameTime(double newEndgameTime)	Time in seconds when the robot should start to consider endgame obstacles.
void	setInjectPoints(boolean newInjectPoints)	Whether or not to inject points on straightaways
void	setNormalizeCorners(boolean newNormalizeCorners)	Whether or not to normalize distance between corner points
void	setPointSpacing(double newPointSpacing)	Space between injected points on straightaways in the path (meters)
void	setProfiling(boolean newProfiling)	Whether or not to profile the pathfinding process
void	setSearchAlgorithmType(SearchAlgorithm.SearchAlgorithmType newSearchAlgorithm)	The search algorithm to use
ArrayList<Edge>	visualizeEdges()	Intended for use with our custom advantagescope fork for debugging new fields.
ArrayList<Vertex>	visualizeInflatedVertices()	Intended for use with our custom advantagescope fork for debugging new fields.
ArrayList<Edge>	visualizeNeighbors()	Intended for use with our custom advantagescope fork for debugging the visibility graph.
ArrayList<Vertex>	visualizePathVertices()	Intended for use with our custom advantagescope fork for debugging the visibility graph.
ArrayList<Vertex>	visualizeVertices()	Intended for use with our custom advantagescope fork for debugging new fields.

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Field Details

map

public final Map map

The map this pathfinder will use

Constructor Details

Pathfinder

public Pathfinder(FieldLoader.FieldData field,
 double pointSpacing,
 double cornerPointSpacing,
 double cornerDist,
 double clearance,
 double cornerSplitPercent,
 boolean injectPoints,
 boolean normalizeCorners,
 SearchAlgorithm.SearchAlgorithmType searchAlgorithmType,
 boolean profiling,
 double endgameTime)

Create a new pathfinder. Should only be done once, at the start of the program.

Parameters:

field - The field json object to use

pointSpacing - The distance between points on the straightaways

cornerPointSpacing - The distance between points on the corners

cornerDist - How far back along the straightaway to dedicate to corners

clearance - The clearance to use when inflating obstacles

cornerSplitPercent - How far back along the straightaway to dedicate to a corner when the straightaway is too small to fit both corners (percentage, should be less than 0.5)

injectPoints - Whether or not to inject points on straightaways

normalizeCorners - Whether or not to normalize distance between corner points

searchAlgorithmType - The search algorithm to use

profiling - Whether or not to profile the pathfinding process

endgameTime - The time in seconds when the robot should start to consider endgame obstacles

Method Details

periodic

public void periodic()

Updates the modifier cache based on data from the driver station and wpilib. Should be called in robotPeriodic.

generatePath

public Path generatePath(Vertex start,
 Vertex target,
 Pathfinder.PathfindSnapMode snapMode,
 ArrayList<Vertex> dynamicVertices)
                  throws ImpossiblePathException

Snaps the start and target vertices according to the snap mode, calculates visibility graph for dynamic elements, and generates the best path.

Parameters:

start - The starting vertex

target - The target vertex

snapMode - The snap mode to use

dynamicVertices - An ArrayList of dynamic vertices

Returns:

The shortest path from the starting vertex to the target vertex that does not intersect any obstacles

Throws:

ImpossiblePathException - If no path can be found

generatePath

public Path generatePath(Vertex start,
 Vertex target)
                  throws ImpossiblePathException

Snaps the start and target vertices to be outside of obstacles and generates the best path. Defaults to PathfindSnapMode.SNAP_ALL

Parameters:

start - The starting vertex

target - The target vertex

Returns:

The shortest path from the starting vertex to the target vertex that does not intersect any obstacles

Throws:

ImpossiblePathException - If no path can be found

generatePath

public Path generatePath(edu.wpi.first.math.geometry.Pose2d start,
 edu.wpi.first.math.geometry.Pose2d target)
                  throws ImpossiblePathException

Snaps the start and target poses to be outside of obstacles and generates the best path. Defaults to PathfindSnapMode.SNAP_ALL

Parameters:

start - The starting poses

target - The target poses

Returns:

The shortest path from the starting vertex to the target vertex that does not intersect any obstacles

Throws:

ImpossiblePathException - If no path can be found

generatePath

public Path generatePath(Vertex start,
 Vertex target,
 Pathfinder.PathfindSnapMode snapMode)
                  throws ImpossiblePathException

Snaps the start and target vertices according to the snap mode and generates the best path.

Parameters:

start - The starting vertex

target - The target vertex

snapMode - The snap mode to use

Returns:

The shortest path from the starting vertex to the target vertex that does not intersect any obstacles

Throws:

ImpossiblePathException - If no path can be found

generatePath

public Path generatePath(edu.wpi.first.math.geometry.Pose2d start,
 edu.wpi.first.math.geometry.Pose2d target,
 Pathfinder.PathfindSnapMode snapMode)
                  throws ImpossiblePathException

Snaps the start and target poses according to the snap mode and generates the best path.

Parameters:

start - The starting pose

target - The target pose

snapMode - The snap mode to use

Returns:

The shortest path from the starting pose to the target pose that does not intersect any obstacles

Throws:

ImpossiblePathException - If no path can be found

generatePath

public Path generatePath(edu.wpi.first.math.geometry.Pose2d start,
 ArrayList<edu.wpi.first.math.geometry.Pose2d> target)
                  throws ImpossiblePathException

Snaps the start and target vertices to be outside of obstacles and generates the best path that passes through all waypoints. Defaults to PathfindSnapMode.SNAP_ALL

Parameters:

start - The starting pose

target - The target waypoints

Returns:

A trajectory from the starting vertex passing through all waypoints that does not intersect any obstacles

Throws:

ImpossiblePathException - If no path can be found

generateTrajectory

public edu.wpi.first.math.trajectory.Trajectory generateTrajectory(edu.wpi.first.math.geometry.Pose2d start,
 edu.wpi.first.math.geometry.Pose2d target,
 Pathfinder.PathfindSnapMode snapMode,
 edu.wpi.first.math.trajectory.TrajectoryConfig config)
                                                            throws ImpossiblePathException

Snaps the start and target vertices according to the snap mode and generates the best path a wpilib trajectory.

Parameters:

start - The starting pose

target - The target pose

snapMode - The snap mode to use

config - The trajectory config to use when generating the trajectory

Returns:

A trajectory from the starting vertex to the target vertex that does not intersect any obstacles

Throws:

ImpossiblePathException - If no path can be found

generateTrajectory

public edu.wpi.first.math.trajectory.Trajectory generateTrajectory(edu.wpi.first.math.geometry.Pose2d start,
 edu.wpi.first.math.geometry.Pose2d target,
 Pathfinder.PathfindSnapMode snapMode,
 ArrayList<Vertex> dynamicVertices,
 edu.wpi.first.math.trajectory.TrajectoryConfig config)
                                                            throws ImpossiblePathException

Snaps the start and target vertices according to the snap mode, calculates visibility graph for dynamic elements, and generates the best path as a wpilib trajectory.

Parameters:

start - The starting pose

target - The target pose

snapMode - The snap mode to use

dynamicVertices - An ArrayList of dynamic vertices

config - The trajectory config to use when generating the trajectory

Returns:

A trajectory from the starting vertex to the target vertex that does not intersect any obstacles

Throws:

ImpossiblePathException - If no path can be found

generateTrajectory

public edu.wpi.first.math.trajectory.Trajectory generateTrajectory(edu.wpi.first.math.geometry.Pose2d start,
 edu.wpi.first.math.geometry.Pose2d target,
 edu.wpi.first.math.trajectory.TrajectoryConfig config)
                                                            throws ImpossiblePathException

Snaps the start and target vertices to be outside of obstacles and generates the best path as a wpilib trajectory. Defaults to PathfindSnapMode.SNAP_ALL

Parameters:

start - The starting pose

target - The target pose

config - The trajectory config to use when generating the trajectory

Returns:

A trajectory from the starting vertex to the target vertex that does not intersect any obstacles

Throws:

ImpossiblePathException - If no path can be found

generateTrajectory

public edu.wpi.first.math.trajectory.Trajectory generateTrajectory(edu.wpi.first.math.geometry.Pose2d start,
 ArrayList<edu.wpi.first.math.geometry.Pose2d> target,
 edu.wpi.first.math.trajectory.TrajectoryConfig config)
                                                            throws ImpossiblePathException

Snaps the start and target vertices to be outside of obstacles and generates the best path that passes through all waypoints as a wpilib trajectory. Defaults to PathfindSnapMode.SNAP_ALL

Parameters:

start - The starting pose

target - The target pose

config - The trajectory config to use when generating the trajectory

Returns:

A trajectory from the starting vertex passing through all waypoints that does not intersect any obstacles

Throws:

ImpossiblePathException - If no path can be found

generateTrajectory

public edu.wpi.first.math.trajectory.Trajectory generateTrajectory(edu.wpi.first.math.geometry.Pose2d start,
 ArrayList<edu.wpi.first.math.geometry.Pose2d> target,
 Pathfinder.PathfindSnapMode snapMode,
 edu.wpi.first.math.trajectory.TrajectoryConfig config)
                                                            throws ImpossiblePathException

Snaps the start and target vertices accoridng to the snap mode the best path that passes through all waypoints as a wpilib trajectory.

Parameters:

start - The starting pose

target - The target pose

snapMode - The snap mode to use

config - The trajectory config to use when generating the trajectory

Returns:

A trajectory from the starting vertex passing through all waypoints that does not intersect any obstacles

Throws:

ImpossiblePathException - If no path can be found

visualizeNeighbors

public ArrayList<Edge> visualizeNeighbors()

Intended for use with our custom advantagescope fork for debugging the visibility graph.

Returns:

An arraylist containing edges to represent all neighbors, showing the visibility graph

visualizePathVertices

public ArrayList<Vertex> visualizePathVertices()

Intended for use with our custom advantagescope fork for debugging the visibility graph.

Returns:

An arraylist containing vertices to represent all vertices in the visibility graph. This is the same as visualizeVertices, but includes dynamic vertices like the start and end points.

visualizeEdges

public ArrayList<Edge> visualizeEdges()

Intended for use with our custom advantagescope fork for debugging new fields.

Returns:

An arraylist containing edges to represent all defined field edges

visualizeVertices

public ArrayList<Vertex> visualizeVertices()

Intended for use with our custom advantagescope fork for debugging new fields.

Returns:

An arraylist containing vertices to represent all defined, uninflated obstacle vertices

visualizeInflatedVertices

public ArrayList<Vertex> visualizeInflatedVertices()

Intended for use with our custom advantagescope fork for debugging new fields.

Returns:

An arraylist containing vertices to represent all uninflated obstacle vertices

getPointSpacing

public double getPointSpacing()

Space between injected points on straightaways in the path (meters)

Returns:

The space between injected points on straightaways (meters)

getCornerPointSpacing

public double getCornerPointSpacing()

Space between points on corners of the path (percent of the curve length)

Returns:

The space between points on corners of the path (percent of the curve

getCornerDist

public double getCornerDist()

How far back along the straightaway to dedicate to making corners

Returns:

The distance back along the straightaway dedicated to making corners

clearance

public final double clearance()

How far to inflate obstacles

Returns:

The clearance to use when inflating obstacles

getInjectPoints

public boolean getInjectPoints()

Whether or not to inject points on straightaways

Returns:

Whether or not to inject points on straightaways

getNormalizeCorners

public boolean getNormalizeCorners()

Whether or not to normalize distance between corner points

Returns:

Whether or not to normalize distance between corner points

getCornerSplitPercent

public double getCornerSplitPercent()

How far each corner should move towards the other point if the distance is too short to allow both corners the full corner distance

Returns:

The corner split percentage

getSearchAlgorithmType

public SearchAlgorithm.SearchAlgorithmType getSearchAlgorithmType()

The search algorithm to use

Returns:

The search algorithm to use

getProfiling

public boolean getProfiling()

Whether or not to profile the pathfinding process

Returns:

Whether or not to profile the pathfinding process

getEndgameTime

public static double getEndgameTime()

Time in seconds when the robot should start to consider endgame obstacles.

Returns:

The time in seconds when the robot should start to consider endgame

setPointSpacing

public void setPointSpacing(double newPointSpacing)

Space between injected points on straightaways in the path (meters)

Parameters:

newPointSpacing - The new space between injected points on straightaways (meters)

setCornerPointSpacing

public void setCornerPointSpacing(double newCornerPointSpacing)

Space between points on corners of the path (percent of the curve length)

Parameters:

newCornerPointSpacing - The new space between points on corners of the path (percent of the curve length)

setCornerDist

public void setCornerDist(double newCornerDist)

How far back along the straightaway to dedicate to making corners

Parameters:

newCornerDist - The new distance back along the straightaway dedicated to making corners

setInjectPoints

public void setInjectPoints(boolean newInjectPoints)

Whether or not to inject points on straightaways

Parameters:

newInjectPoints - Whether or not to inject points on straightaways

setNormalizeCorners

public void setNormalizeCorners(boolean newNormalizeCorners)

Whether or not to normalize distance between corner points

Parameters:

newNormalizeCorners - Whether or not to normalize distance between corner points

setCornerSplitPercent

public void setCornerSplitPercent(double newCornerSplitPercent)

How far each corner should move towards the other point if the distance is too short to allow both corners the full corner distance

Parameters:

newCornerSplitPercent - The new corner split percentage

setSearchAlgorithmType

public void setSearchAlgorithmType(SearchAlgorithm.SearchAlgorithmType newSearchAlgorithm)

The search algorithm to use

Parameters:

newSearchAlgorithm - The new search algorithm to use

setProfiling

public void setProfiling(boolean newProfiling)

Whether or not to profile the pathfinding process

Parameters:

newProfiling - Whether or not to profile the pathfinding process

setEndgameTime

public static void setEndgameTime(double newEndgameTime)

Time in seconds when the robot should start to consider endgame obstacles.

Parameters:

newEndgameTime - The new time in seconds when the robot should start to consider endgame obstacles