Class Path

All Implemented Interfaces:
Serializable, Cloneable, Iterable<Vertex>, Collection<Vertex>, List<Vertex>, RandomAccess

public class Path extends ArrayList<Vertex>
A Path is a list of Vertices that represent a path from a start point to a target point.
See Also:
  • Field Details

  • Constructor Details

    • Path

      public Path(Vertex start, Vertex target, Pathfinder pathfinder)
      Creates a new Path with the given start and target vertices.
      Parameters:
      start - The start vertex of the path.
      target - The target vertex of the path.
      pathfinder - The Pathfinder that created this path.
    • Path

      public Path(Vertex start, Vertex target, ArrayList<Vertex> points)
      Creates a new Path with the given start and target vertices and the given
      Parameters:
      start - The start vertex of the path.
      target - The target vertex of the path.
      points - The points in the path.
  • Method Details

    • setUnsnappedTarget

      public void setUnsnappedTarget(Vertex unsnappedTarget)
      Set the original unsnapped target vertex (the target vertex before it was snapped to be outside of all obstacles).
      Parameters:
      unsnappedTarget - The original unsnapped target vertex.
    • addPath

      public void addPath(Path path)
      Add a path to the end of this path, to be used BEFORE processing the path.
      Parameters:
      path - The path to add.
    • getUnsnappedTarget

      public Vertex getUnsnappedTarget()
      Get the unsnapped target vertex.
      Returns:
      The unsnapped target vertex
    • end

      public Vertex end()
      Get the last vertex in the path (not including the target vertex)
      Returns:
      The last vertex in the path.
    • processPath

      public void processPath(Pathfinder.PathfindSnapMode snapMode)
      Apply all processing to the path to prepare it for use.
      Parameters:
      snapMode - The snap mode to use.
    • getFinalRot

      public edu.wpi.first.math.geometry.Rotation2d getFinalRot()
      Get rotation at the final point of the path
      Returns:
      The final rotation of the path.
    • toDoubleArray

      public double[] toDoubleArray()
      Get the path as a double array [x, y, rotation, x, y, rotation, ...]. Rotation is in degrees.
      Returns:
      The path as a double array.
    • asPose2dList

      public ArrayList<edu.wpi.first.math.geometry.Pose2d> asPose2dList()
      Get the path as a Pose2d ArrayList.
      Returns:
      An ArrayList of Pose2ds representing the path.
    • asTrajectory

      public edu.wpi.first.math.trajectory.Trajectory asTrajectory(edu.wpi.first.math.trajectory.TrajectoryConfig config) throws ImpossiblePathException
      Get the path as a Trajectory.
      Parameters:
      config - The TrajectoryConfig to use.
      Returns:
      The path as a Trajectory.
      Throws:
      ImpossiblePathException - if the trajectory could not be generated.
    • getStart

      public Vertex getStart()
      Get the start vertex.
      Returns:
      The start vertex
    • getTarget

      public Vertex getTarget()
      Get the target vertex.
      Returns:
      The target vertex
    • getFullPath

      public ArrayList<Vertex> getFullPath()
      Get the full path including the start and target vertices.
      Returns:
      The full path including the start and target vertices.