Path planning added