Copter: RTL with rally point finds shortest path using Dijkstras (WIP)

[rmackay9]

This enhancement causes RTL to fly to the Rally point with the shortest flight distance instead of the closest Rally point. Normally these are the same but if a fence separates the vehicle from a rally point it can actually be quicker to fly to another.

The change appears to be very large but most of it is just moving code as part of separating Dijkatras into more manageable pieces. These pieces are:

1. AP_OADijkstra_Common : shared enums and error reporting
2. AP_OADijkstra_StaticData: handling of fences including calculating inner fences
3. AP_OADijkstra_CalcPath: the heart of Dijkstras which involves finding the shortest safe path

Other changes are:

• AP_OADijkstra_CalcPathis enhanced to allow multiple callers to request the shortest path to different destinations
• AP_Rally is enhanced to use Dijkstras when calculating the the "closets" rally point
• Copter's ModeRTL's build_path method essentially becomes asyncronous so the vehicle waits until AP_Rally calculates the shortest Rally point (previously this was all done immediately in the main thread)
• AP_OAPathPlanner is simply updated to use the slightly modified Dijkstras interface

This video demonstrates the enhancement https://youtu.be/nH_pnZ0FbP0

This has been tested in SITL but not yet on a real vehicle

[Ryanf55]

inclusion_polygon_fence_28542.txt

Here's a similar fence to try to reproduce this demo with: