Make sure that the robot turn to the direction of the next waypoint before moving straight