Addition of position goal checker, fix of rotation shim controller (#…#1

mini-1235 wants to merge 2 commits intobp/5034mini-1235/navigation2:bp/5034from

bp/5162mini-1235/navigation2:bp/5162Copy head branch name to clipboard