Fixed the waypoint test error!

Surprisingly easy!