Currently only checking if the 3D position is in an Euclidean tolerance. ## Add Check if yaw is in tolerance [Deg] before sending next waypoint.