Add Path Finding Test and integration