Waypoint Follower

In order to drive along the planned driving path consisting of waypoints with associated target speeds and yaw angles, an implementation of the Pure Pursuit algorithm is used.

This algorithm receives the upcoming waypoints from Waypoint Updater and publishes a target linear velocity and angular velocity for DBW Node to control throttle/brake and steering actuators.

Pure Pursuit Algorithm Overview

The Pure Pursuit algorithm basically finds the radius of a circular path that is tangent to the current orientation of the vehicle and crosses through a point on the target driving path some distance ahead (look-ahead point) and calculates the corresponding target angular velocity to follow it. As the vehicle drives forward and steers along this arc, the look-ahead point continues to be pushed further ahead so the vehicle gradually approaches and straightens along the path.

The amount of look-ahead distance alters the sharpness of the steering angles, where a short look ahead causes larger steering angles but can follow sharp turns, and a large look ahead causes smaller steering angles but may cut the corners of the driving path. The look-ahead distance is variable in proportion to the vehicle speed.

The target linear velocity from this algorithm simply passes through the target velocity associated with the closest waypoint.

See this link for some helpful geometric diagrams.

Autoware Open-source Pure Pursuit Library (Waypoint Follower)

For this project, the Autoware open-source ROS library called Waypoint Follower has been included by Udacity to perform the pure pursuit algorithm. However, the base C++ algorithm had some areas that needed further improvement, described below.

Project Improvements Made to Waypoint Follower Library

• Tune verifyFollowing() judgment thresholds for distance displacement_threshold_ and relative angle relative_angle_threshold_ to keep more continuous steering adjustments to prevent ping-ponging around target path.

• Perform closest waypoint search to pick up the correct waypoint speed targets when the car has driven past the initial waypoints in the list (in case of lag or Waypoint Follower updating at a faster rate than Waypoint Updater).

• Perform look-ahead waypoint search starting from closest waypoint instead of initial waypoint in the list to prevent searching behind the car when some waypoints remain behind the car.

• Tune minimum_lookahead_distance_ to be able to steer around tighter turns such as the Udacity test lot.

• Fix possibility for negative target velocity twist command at high lateral velocities (PR #76).