Pulled waypoint integration into planner. Cleaned up extraneous comments.