Fix Navigation Goal Rejection in sm_nav2_unit_test_1#689

brettpac merged 2 commits intojazzyrobosoft-ai/SMACC2:jazzyfrom

brettpac_branchrobosoft-ai/SMACC2:brettpac_branchCopy head branch name to clipboard