Fixed bug in Wayset::first and other Wayset improvements...
Improvements to Wayset::sampleAtDistance
Cleanup and small fix in A* intermediate
Cleanup in HierarchicalPlanner
Fixed a very bad return-by-reference bug in SamplingTask
Cleaned up SubFlightPlanner
Cleanup usage of Wayset::positions trying to be returned as reference