Create new function "get_current_waypoint_number"
Create new function "set_new_current_waypoint"