Backporting #2857 and #2862 into galactic

[jonipol]

Cherry picked #2857 and #2862 from main into galactic

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#2835
Primary OS tested on	Ubuntu
Robotic platform tested on	gazebo simulation where the original issue was found

---

For Maintainers:

• Check that any new parameters added are updated in navigation.ros.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]

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

[mergify]

@jonipol, please properly fill in PR template in the future. @SteveMacenski, use this instead.

• Check that any new parameters added are updated in navigation.ros.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

[SteveMacenski]

@AlexeyMerzlyakov please review

[AlexeyMerzlyakov]

The patch and final files are identical with main branch. And there is no need for additional backport (of other commits) there. LGTM for merge.