maybe add && !waypoint_traj.empty() ?