Added test commands to set waypoints.