Implement A* on the generated tree to find optimal route

[wrongsyntax]

Return list of waypoints in order.

[wrongsyntax]

Not necessarily A*.

Could be some other method.