Change goal align to be based on wall follow dir

Change goal align to be based on wall follow direction.