Add goal pose to CriticData

[redvinaa]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	(add tickets here #4809 )
Primary OS tested on	Ubuntu
Robotic platform tested on	custom gazebo simulation, custom hardware
Does this PR contain AI generated software?	No

---

Description of contribution in a few bullet points

• Added goal pose to MPPI CriticData
• Now all critics can activate based on distance from global goal pose instead of pruned

Description of documentation updates required from your changes

• Bugfix, usage doesn't change

---

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[mergify]

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

[redvinaa]

Please check implementation and when everything is correct, I'll port to rolling

[SteveMacenski]

Generally looks good to me

[redvinaa]

Can I have some help with the CI errors? I'm not sure it's because of my changes

[SteveMacenski]

@redvinaa the CI errors are because we don't expect PRs directly to existing distributions so its not setup correctly with the caching. You could have build / test issues, but we'd need to to have this opened against main to check for them. This PR is opened in the opposite direction we ususally do (main branch -> then backport to humble, if possible).

I'll review as-if build/tests are passing, but I won't merge into Humble until the same code on main is passing as a proxy

[SteveMacenski]

Otherwise LGTM