waypoints: Add active goals list functionality

Push, pop, and selection