Following the path

From a path to steering

Step 5 handed its path to rover.follow. That's switched off now: you'll write the follower. A path is just a list of points 1 cm apart. The rover needs a speed and a turn rate, 50 times a second.

Pure pursuit

Pure pursuit chases a carrot. Each time round the loop:

1. Find the path point closest to the rover.
2. From there, walk along the path to the first point at least the lookahead distance $L$ away from the rover (or the last point, near the end). That's the lookahead point $(x_p, y_p)$.
3. Put it in the rover's frame: its distance $d$, and the angle $\alpha$ between the rover's heading and the direction to it, $\alpha = \operatorname{wrap}\big(\operatorname{atan2}(y_p - y,\ x_p - x) - \theta\big)$.
4. Steer along the circle that leaves the rover straight ahead and passes through the point. Its curvature (one over its radius) is $2\sin\alpha / d$, so at speed $v$:

As the rover moves, the point slides further along the path, so the rover is always turning towards where the path is going.

Choosing the lookahead

$L$ is the one knob. A short lookahead hugs the path, cutting corners less, but reacts to every kink and wobbles. A long one drives smoothly but cuts corners, and in a 12 cm corridor a cut corner is a bump. The program uses 3 cm. Try 2 cm and 10 cm and watch the Plots tab: it shows the cross-track error, the rover's distance from the path.

Your task

1. Write lookahead_point(path, pose, lookahead): steps 1 and 2, returning the point.
2. Write pure_pursuit(path, pose, lookahead), returning $(v, \omega)$: $v$ = SPEED and $\omega$ from the formula above.

The program plans across the maze with step 5's A* and drives the plan with your controller. The grader tests pure_pursuit on 6 fixed poses, then wants the rover at the goal in under 60 s, never more than 2 cm from the path, and without a bump.