nav goal pose now works