Add Multi-Lane Support For Navigation (Global Planner)

[Bonifatius94]

Issue:

• currently, lane changes cannot be planned by the global planner
• routes requiring alignment for a right / left turn need to consider lane changes, otherwise the algorithm might plan really weird routes for avoiding lane changes to turning lanes by driving in circles, etc.

Tasks:

• connect roads with multiple lanes correctly (e.g. when entering a highway)
• connect parallel lanes of the same direction
• think of a reasonable cost for switching lanes, so the planning algorithm considers them when appropriate
• prefer using the rightmost lane during planning if possible

Goals:

• plan routes requiring lane changes to turning lanes
• being at least able to properly navigate Town04 (small city + highway)

[Bonifatius94]

Note: Turns out that there are a lot more flaws than expected, in order to work on this ticket. Let's put it on hold for now.

[Bonifatius94]

Status for PAF meeting 10-03-2022: There's an implementation in place, but the goal cannot be tested yet due to other navigation bugs