Add pure pursuit library

[chfriedrich98]

Solved Problem

The pure pursuit algorithm is used by the new ackermann rover module and soon also by the updated differential rover and the upcoming boat module.
Creating a library for the algorithm avoids having to copy the same code into all the modules.

Solution

The algorithm calculates the intersection point between a circle around the vehicle and the line segment connecting the previous and current waypoint and returns the heading towards that point. The radius of the circle around the vehicle is used to tune the controller and is often referred to as look-ahead distance ($l_d$).

Test coverage

• Unit test for edge cases
• SITL tested (ackermann rover module)

[Jaeyoung-Lim]

Awesome!

Added some minor stylistic comments

[chfriedrich98]

@Jaeyoung-Lim Thanks for the review!
I implemented most of your suggestions and left some comments.

[chfriedrich98]

After discussing the implementation with @bresch I made some major changes to how the intersection point is calculated. It is now done using vectors which eliminates many of the edgecases I had to consider.
Also to keep everything consistent with @Jaeyoung-Lim's input:

If you return a zero, there is no way outside of the library to detect whether if the input you have provided is valid or not.
It is actually a positive feature if something breaks down the line. It means that you need to handle these cases explicitly for your runtime application, rather than concealing it in the library. The way it is implemented now, an invalid position estimate will result in aligning the vehicle into zero heading, which is a behavior that is unrelated to the failure.

the algorithm now returns NAN if any of the inputs are invalid rather than using the bearing to the current waypoint as a fallback.

[Jaeyoung-Lim]

Looks much nicer :)

[chfriedrich98]

rebased on main and squashed