pose_controller: add goal reached logic and control