Package trclib.pathdrive

Class TrcPath

java.lang.Object
trclib.pathdrive.TrcPath
public class TrcPath extends Object
This class implements a path. A path is consists of an array of waypoints, and can be used for path following, such as motion profiling, pure pursuit, etc. Since heading could be in degrees or radians, each path object specifies the units of its heading value, and can be converted. If the timesteps are not specified, velocity and position data can be used to infer approximate timesteps. Paths can be read from CSV files.

  • Field Summary

    Fields
    Modifier and Type
    Field
    Description
    private boolean
    inDegrees
     
    private final String
    moduleName
     
    private static final TrcDbgTrace.MsgLevel
    msgLevel
     
    final TrcDbgTrace
    tracer
     
    private final TrcWaypoint[]
    waypoints
     

  • Constructor Summary

    Constructors
    Constructor
    Description
    TrcPath(boolean inDegrees, TrcWaypoint... waypoints)
    Constructor: Create a new TrcPath object.
    TrcPath(TrcWaypoint... waypoints)
    Constructor: Create a new TrcPath object.

  • Method Summary

    Modifier and Type
    Method
    Description
    TrcPath
    clone()
    This method makes a copy of this waypoint.
    TrcWaypoint[]
    getAllWaypoints()
    This method returns the array of waypoints of the path.
    double
    getArcLength()
    Get the curved length of the entire path.
    TrcWaypoint
    getLastWaypoint()
    This method returns the last waypoint in the path.
    double
    getPathDuration()
    Get the estimated duration of the entire path.
    int
    getSize()
    This method returns the number of waypoints in this path.
    TrcWaypoint
    getWaypoint(int index)
    This method returns the waypoint at the given index of the path.
    void
    inferAccelerations()
    This method calculates the acceleration of each waypoint in the path.
    void
    inferTimeSteps()
    Use velocity and position data to infer the timesteps of the waypoint.
    TrcPath
    insertWaypoint(int index, TrcWaypoint waypoint)
    Insert a waypoint in the middle of the path.
    private double
    interpolate(double start, double end, double weight)
    This method returns an interpolated value between the two specified values with the specified weight between the two values.
    private TrcWaypoint
    interpolate(TrcWaypoint point1, TrcWaypoint point2, double weight)
    This method returns a waypoint that is interpolated between the two specified waypoints with the specified weight between the points.
    boolean
    isInDegrees()
    Check if this path defines heading using degrees.
    static TrcPath
    loadPathFromCsv(boolean inDegrees, String path, boolean loadFromResources)
    This method loads waypoints from a CSV file and create a path with them.
    void
    mapSelfToDegrees()
    If not already in degrees, convert this path's heading values to degrees.
    void
    mapSelfToRadians()
    If not already in radians, convert this path's heading values to radians.
    TrcPath
    relativeTo(TrcPose2D referencePose)
    This method translates all the waypoints in the path relative to the specified referencePose.
    TrcPath
    relativeToStart()
    This method translates all the waypoints in the path relative to the first waypoint in the path.
    TrcPath
    toAbsolute(TrcPose2D absoluteStartPose)
    This method returns a path that converts all the waypoints to absolute positions.
    TrcPath
    toDegrees()
    Create a new path identical to this one, except the heading values are in degrees.
    TrcPath
    toRadians()
    Create a new path identical to this one, except the heading values are in radians.
    String
    toString()
    This method returns the path info in string form.
    TrcPath
    translate(double x, double y)
    Translate the path by an x and y offset.
    TrcPath
    trapezoidVelocity(double maxVel, double maxAccel, double maxDecel)
    Set the velocity and accelerations of the waypoints in the path to follow a trapezoidal velocity profile.

    Methods inherited from class java.lang.Object

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

  • Field Details

  • Constructor Details

    • TrcPath

      public TrcPath(boolean inDegrees, TrcWaypoint... waypoints)
      Constructor: Create a new TrcPath object. Must supply at least 2 entries.
      Parameters:
      inDegrees - specifies true if the heading values are in degrees, false if they are radians.
      waypoints - specifies the array of points that will constitute this path. Cannot be null, and must have at least 2 waypoints.

    • TrcPath

      public TrcPath(TrcWaypoint... waypoints)
      Constructor: Create a new TrcPath object. Must supply at least 2 entries.
      Parameters:
      waypoints - specifies the array of points that will constitute this path. Cannot be null, and must have at least 2 waypoints.

  • Method Details

    • loadPathFromCsv

      public static TrcPath loadPathFromCsv(boolean inDegrees, String path, boolean loadFromResources)
      This method loads waypoints from a CSV file and create a path with them.
      Parameters:
      inDegrees - specifies true if the heading values are in degrees, false if they are radians.
      path - specifies the file path or the resource name where we load the waypoints.
      loadFromResources - specifies true if waypoints are loaded from resources, false if from file path.
      Returns:
      created path with the loaded waypoints.

    • toString

      public String toString()
      This method returns the path info in string form.
      Overrides:
      toString in class Object
      Returns:
      path info in string form.

    • translate

      public TrcPath translate(double x, double y)
      Translate the path by an x and y offset.
      Parameters:
      x - Amount to offset in x axis.
      y - Amount to offset in y axis.
      Returns:
      new TrcPath object which is this path with an offset.

    • insertWaypoint

      public TrcPath insertWaypoint(int index, TrcWaypoint waypoint)
      Insert a waypoint in the middle of the path.
      Parameters:
      index - The index to insert the new point, in the range [0, getSize()]
      waypoint - The waypoint to insert.
      Returns:
      A new TrcPath object with the waypoint inserted.

    • clone

      public TrcPath clone()
      This method makes a copy of this waypoint.
      Overrides:
      clone in class Object
      Returns:
      a copy of this waypoint.

    • relativeTo

      public TrcPath relativeTo(TrcPose2D referencePose)
      This method translates all the waypoints in the path relative to the specified referencePose.
      Parameters:
      referencePose - specifies the reference pose to translate all waypoints to be referenced to.
      Returns:
      the translated path.

    • toAbsolute

      public TrcPath toAbsolute(TrcPose2D absoluteStartPose)
      This method returns a path that converts all the waypoints to absolute positions.
      Parameters:
      absoluteStartPose - specifies the absolute pose of the first point in the path.
      Returns:
      path with absolute waypoints.

    • relativeToStart

      public TrcPath relativeToStart()
      This method translates all the waypoints in the path relative to the first waypoint in the path.
      Returns:
      the translated path.

    • trapezoidVelocity

      public TrcPath trapezoidVelocity(double maxVel, double maxAccel, double maxDecel)
      Set the velocity and accelerations of the waypoints in the path to follow a trapezoidal velocity profile. This is characterized by maxVel and maxAccel. Up to two points will be inserted into the path to make the robot accelerate at maxAccel. Between those two points, the robot velocity will be maxVel.
      Parameters:
      maxVel - The maximum velocity magnitude of the path, with matching units.
      maxAccel - The maximum acceleration magnitude of the path, with matching units.
      maxDecel - The maximum deceleration magnitude of the path, with matching units.
      Returns:
      A new TrcPath object, with the velocities and accelerations matching the profile.

    • interpolate

      private TrcWaypoint interpolate(TrcWaypoint point1, TrcWaypoint point2, double weight)
      This method returns a waypoint that is interpolated between the two specified waypoints with the specified weight between the points.
      Parameters:
      point1 - specifies the first waypoint.
      point2 - specifies the second waypoint.
      weight - specifies the weighted distance between the two points.

    • interpolate

      private double interpolate(double start, double end, double weight)
      This method returns an interpolated value between the two specified values with the specified weight between the two values.
      Parameters:
      start - specifies the first value.
      end - specifies the second value.
      weight - specifies the weighted distance between the two values.

    • getLastWaypoint

      public TrcWaypoint getLastWaypoint()
      This method returns the last waypoint in the path.
      Returns:
      last waypoint.

    • getWaypoint

      public TrcWaypoint getWaypoint(int index)
      This method returns the waypoint at the given index of the path.
      Parameters:
      index - specifies the index to the path array.
      Returns:
      the waypoint at the given index.

    • isInDegrees

      public boolean isInDegrees()
      Check if this path defines heading using degrees.
      Returns:
      True if degrees are used, false if radians.

    • getSize

      public int getSize()
      This method returns the number of waypoints in this path.
      Returns:
      the number of waypoints in this path.

    • getAllWaypoints

      public TrcWaypoint[] getAllWaypoints()
      This method returns the array of waypoints of the path.
      Returns:
      the waypoint array. This is the same instance, so modifying it will affect other users of this path.

    • toDegrees

      public TrcPath toDegrees()
      Create a new path identical to this one, except the heading values are in degrees. If this path's headings are already in degrees, just return deep copy.
      Returns:
      A TrcPath object which the same waypoints except all heading values are now in degrees, if they weren't before.

    • toRadians

      public TrcPath toRadians()
      Create a new path identical to this one, except the heading values are in radians. If this path's headings are already in radians, just return deep copy.
      Returns:
      A TrcPath object which the same waypoints except all heading values are now in radians, if they weren't before.

    • mapSelfToDegrees

      public void mapSelfToDegrees()
      If not already in degrees, convert this path's heading values to degrees.

    • mapSelfToRadians

      public void mapSelfToRadians()
      If not already in radians, convert this path's heading values to radians.

    • getPathDuration

      public double getPathDuration()
      Get the estimated duration of the entire path.
      Returns:
      The estimated time duration, in the same units as timeStep.

    • getArcLength

      public double getArcLength()
      Get the curved length of the entire path.
      Returns:
      The curved length of the entire path, in the same units as x and y.

    • inferAccelerations

      public void inferAccelerations()
      This method calculates the acceleration of each waypoint in the path.

    • inferTimeSteps

      public void inferTimeSteps()
      Use velocity and position data to infer the timesteps of the waypoint.