Added comments to A* intermediate planner
Cleanup of DubinsIntermediate
Created "Smart" intermediate which uses Dubins but falls back to A*
"Smart" intermediate is now the default intermediate planner in HierarchicalPlanner