Fixed bug with greedy planner getting stuck.

Turns out that it was a problem with coordinate conversion from LLA ->
ENU -> LLA etc. Having to do with calculating distance including
unwanted vertical (z) coordinate of ENU.