[Graceful Controller] Fix Incorrect Motion Target Heading output by controller#5530

SteveMacenski merged 5 commits intoros-navigation:mainros-navigation/navigation2:mainfrom

SakshayMahna:graceful-lookahead-bugSakshayMahna/navigation2:graceful-lookahead-bugCopy head branch name to clipboard