added goal pose to visibility graph path

added goal pose to visibility graph path #1445