Fix improper use of the heapq module.

Reduces nodes looked at and speeds up pathfinding a bit more.