outputting succeeded immediately when already in goal waypoint and top_nav also outputs succeeded immediately