Since this is essentially an alternative and much more advanced PathPlanner implementation, this version should be created as a new module (AdvancedPathPlanner?).  Then we can keep the old PathPlanner in parallel for simpler tasks or while the new version is being developed and tested.  This also means we don't need to block until it works and is tested for Vtol and Ground.