Fix: reset controller_server loop_rate when missed desired rate #5712 (#5723)

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.

Signed-off-by: agennart <[email protected]>
Co-authored-by: agennart <[email protected]>

Author: gennartan

nav2_controller/src/controller_server.cpp

@@ -517,6 +517,7 @@ void ControllerServer::computeControl()
           get_logger(),
           "Control loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz.",
           controller_frequency_, 1 / cycle_duration.seconds());
+        loop_rate.reset();
       }
     }
   } catch (nav2_core::InvalidController & e) {