Fix goal handle was not freed correctly#4137

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

iceuw:mainiceuw/navigation2:mainCopy head branch name to clipboard