Add pathfinding in navigation subsystem