Package com.pathplanner.lib.pathfinding
Class LocalADStar
java.lang.Object
com.pathplanner.lib.pathfinding.LocalADStar
- All Implemented Interfaces:
Pathfinder
Implementation of AD* running locally in a background thread
I would like to apologize to anyone trying to understand this code. The implementation I translated it from was much worse.
-
Nested Class Summary
Modifier and TypeClassDescriptionstatic final record
Represents a node in the pathfinding grid -
Constructor Summary
ConstructorDescriptionCreate a new pathfinder that runs AD* locally in a background thread -
Method Summary
Modifier and TypeMethodDescriptiongetCurrentPath
(PathConstraints constraints, GoalEndState goalEndState) Get the most recently calculated pathboolean
Get if a new path has been calculated since the last time a path was retrievedvoid
setDynamicObstacles
(List<Pair<Translation2d, Translation2d>> obs, Translation2d currentRobotPos) Set the dynamic obstacles that should be avoided while pathfinding.void
setGoalPosition
(Translation2d goalPosition) Set the goal position to pathfind tovoid
setStartPosition
(Translation2d startPosition) Set the start position to pathfind from
-
Constructor Details
-
LocalADStar
public LocalADStar()Create a new pathfinder that runs AD* locally in a background thread
-
-
Method Details
-
isNewPathAvailable
public boolean isNewPathAvailable()Get if a new path has been calculated since the last time a path was retrieved- Specified by:
isNewPathAvailable
in interfacePathfinder
- Returns:
- True if a new path is available
-
getCurrentPath
Get the most recently calculated path- Specified by:
getCurrentPath
in interfacePathfinder
- Parameters:
constraints
- The path constraints to use when creating the pathgoalEndState
- The goal end state to use when creating the path- Returns:
- The PathPlannerPath created from the points calculated by the pathfinder
-
setStartPosition
Set the start position to pathfind from- Specified by:
setStartPosition
in interfacePathfinder
- Parameters:
startPosition
- Start position on the field. If this is within an obstacle it will be moved to the nearest non-obstacle node.
-
setGoalPosition
Set the goal position to pathfind to- Specified by:
setGoalPosition
in interfacePathfinder
- Parameters:
goalPosition
- Goal position on the field. f this is within an obstacle it will be moved to the nearest non-obstacle node.
-
setDynamicObstacles
public void setDynamicObstacles(List<Pair<Translation2d, Translation2d>> obs, Translation2d currentRobotPos) Set the dynamic obstacles that should be avoided while pathfinding.- Specified by:
setDynamicObstacles
in interfacePathfinder
- Parameters:
obs
- A List of Translation2d pairs representing obstacles. Each Translation2d represents opposite corners of a bounding box.currentRobotPos
- The current position of the robot. This is needed to change the start position of the path if the robot is now within an obstacle.
-