Fixed start/end pose heuristic AGAIN
Cleanup of A* code
Fixes for SamplingTask and CoverageTask