Basic path planning working.