Fix: reset controller_server loop_rate when missed desired rate #5712

[gennartan]

When a single iteration takes longer than the expected period, this impacts the next iterations behavior since they will have less time to perform the control logic, thus increasing the actual controller_frequency. By resetting the loop_rate on sleep() failure, this ensures that each iteration will have a full period to exectue its logic.

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#5712
Primary OS tested on
Robotic platform tested on
Does this PR contain AI generated software?
Was this PR description generated by AI software?

---

Description of contribution in a few bullet points

Fix controller frequency loop rate

• Ensures that the loop_rate makes the controller logic be executed at the desired frequency and not faster

Description of documentation updates required from your changes

Reset the loop_rate on error so that the next iteration has a full period to execute its logic.

Description of how this change was tested

---

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-*.

[codecov]

Codecov Report

✅ All modified and coverable lines are covered by tests.

Files with missing lines	Coverage Δ
nav2_controller/src/controller_server.cpp	83.41% <100.00%> (+0.03%)	⬆️

... and 10 files with indirect coverage changes

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.