Pure Pursuit: Handle edge case and add logging message

[chfriedrich98]

Solved Problem

Pure pursuit edge case

The pure pursuit library can now handle the following edge case:
If the acceptance radius of a waypoint is bigger than the lookahead radius of the pure pursuit controller the following edge case can happen:

The linesegment is outside of the lookahead which makes the rover default to calculating the bearing towards the closest point on the extended line segment (red dot). However, in this edge case this leads the rover to take a suboptimal path (red). The rover will now instead directly target the previous waypoint if the closest point on the path is not on the actual line segment leading to the more direct path (yellow).
In the same fashion, if the closest point on the path is ahead of the line segment the rover will target the current waypoint instead.

Logging message

This PR also adds a message for the purePursuit library to log relevant values.
This makes tuning of the pure pursuit algorithm easier and also deprecates the 3 [rover_type]_guidance_status.msg messages.

To use the library and publish the pure pursuit message the function 'updatePurePursuit' has to be called.
If the logging is not required then the function calcTargetBearing (renamed from calcDesiredHeading) can also be called direclty, which will return the target bearing without publishing the pure pursuit message.

Test coverage

• SITL tested