Fixed KeyNotFoundException when re-prioritizing a node in the frontier