Path planning added back in.