fix bug heap-buffer-overflow

[suifengersan123]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	(add tickets here #5726)
Primary OS tested on	ubuntu 20.04
Robotic platform tested on	N/A (detected via Asan)
Does this PR contain AI generated software?	No
Was this PR description generated by AI software?	No

---

Description of contribution in a few bullet points

Before accessing the array, I will check whether the index exceeds the length of the array at the time of initialization. I believe this can prevent the error of buffer overflow.

Description of documentation updates required from your changes

No user-facing documentation changes required (internal implementation fix)
No parameter changes or API modifications

Description of how this change was tested

AddressSanitizer validation: Compiled with -fsanitize=address and verified no heap-buffer-overflow errors occur

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
• Should this be backported to current distributions? If so, tag with backport-*.

[mergify]

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

[suifengersan123]

This error also exists in the main branch. If you have time, I would appreciate it if you could help me check it and let me know if this fix is correct.