Fix crash in pathfinder when adding nodes