Replaced std priority queues by boost priority queues in the pathfinding (they work better when we want to change the priority of elements)