delphyne
|
PurePursuit computes the required steering angle to achieve a goal point on an continuous planar path.
The path represents as the set of r = 0
positions along a Maliput lane, and a goal point is selected as a pre-defined lookahead distance along the path in the intended direction of travel. The algorithm outputs the steering angle required to guide the vehicle toward the goal point based on its current position in global coordinates.
See [1] and the corresponding .cc file for details on the algorithm.
Instantiated templates for the following kinds of T's are provided:
They are already available to link against in the containing library.
[1] Coulter, R. Implementation of the Pure Pursuit Path Tracking Algorithm. Carnegie Mellon University, Pittsburgh, Pennsylvania, Jan
#include <src/systems/pure_pursuit.h>
Public Member Functions | |
PurePursuit ()=delete | |
Static Public Member Functions | |
static T | Evaluate (const PurePursuitParams< T > &pp_params, const SimpleCarParams< T > &car_params, const LaneDirection &lane_direction, const drake::systems::rendering::PoseVector< T > &pose) |
Evaluates the required steering angle in radians using the pure-pursuit method. More... | |
static const maliput::api::InertialPosition | ComputeGoalPoint (const T &s_lookahead, const LaneDirection &lane_direction, const drake::systems::rendering::PoseVector< T > &pose) |
Computes the goal point at a distance s_lookahead from the closest position on the curve in the intended direction of travel, and with_s and pose are the direction of travel and PoseVector for the ego vehicle. More... | |
|
delete |
|
static |
Computes the goal point at a distance s_lookahead
from the closest position on the curve in the intended direction of travel, and with_s
and pose
are the direction of travel and PoseVector for the ego vehicle.
|
static |
Evaluates the required steering angle in radians using the pure-pursuit method.
Assumes zero elevation and superelevation.
pp_params | contains the lookahead_distance , the distance along the path based on the closest position on the path to the vehicle. |
car_params | contains the wheelbase of the vehicle. |
lane_direction | is a LaneDirection containing a reference lane and the direction of travel along the positive-s coordinate. |
pose | is the PoseVector for the ego vehicle. |