Mppi goal to critic (backport #4822)

[mergify]

Reopen #4812 against main

---

This is an automatic backport of pull request #4822 done by Mergify.

[mergify]

Cherry-pick of d11de56 has failed:

On branch mergify/bp/jazzy/pr-4822
Your branch is up to date with 'origin/jazzy'.

You are currently cherry-picking commit d11de564.
  (fix conflicts and run "git cherry-pick --continue")
  (use "git cherry-pick --skip" to skip this patch)
  (use "git cherry-pick --abort" to cancel the cherry-pick operation)

Changes to be committed:
	modified:   nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp
	modified:   nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp
	modified:   nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp
	modified:   nav2_mppi_controller/src/controller.cpp
	modified:   nav2_mppi_controller/src/critics/cost_critic.cpp
	modified:   nav2_mppi_controller/src/critics/goal_angle_critic.cpp
	modified:   nav2_mppi_controller/src/critics/goal_critic.cpp
	modified:   nav2_mppi_controller/src/critics/obstacles_critic.cpp
	modified:   nav2_mppi_controller/src/critics/path_align_critic.cpp
	modified:   nav2_mppi_controller/src/critics/path_angle_critic.cpp
	modified:   nav2_mppi_controller/src/critics/path_follow_critic.cpp
	modified:   nav2_mppi_controller/src/critics/prefer_forward_critic.cpp
	modified:   nav2_mppi_controller/src/critics/twirling_critic.cpp
	modified:   nav2_mppi_controller/src/optimizer.cpp
	modified:   nav2_mppi_controller/src/path_handler.cpp
	modified:   nav2_mppi_controller/test/controller_state_transition_test.cpp
	modified:   nav2_mppi_controller/test/critic_manager_test.cpp
	modified:   nav2_mppi_controller/test/critics_tests.cpp
	modified:   nav2_mppi_controller/test/optimizer_smoke_test.cpp
	modified:   nav2_mppi_controller/test/optimizer_unit_tests.cpp
	modified:   nav2_mppi_controller/test/utils_test.cpp

Unmerged paths:
  (use "git add <file>..." to mark resolution)
	both modified:   nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

To fix up this pull request, you can check it out locally. See documentation: https://docs.github.com/en/pull-requests/collaborating-with-pull-requests/reviewing-changes-in-pull-requests/checking-out-pull-requests-locally

[mergify]

@mergify[bot], all pull requests must be targeted towards the main development branch.
Once merged into main, it is possible to backport to @jazzy, but it must be in main
to have these changes reflected into new distributions.