Control loop missed its desired rate of 20.0000 Hz. Current loop rate is 50.0000 Hz.

[gennartan]

I have a strange log fom the controller server that I am seeing when starting a navigation task. This is not a new issue, but I ignored this warning up to now.
I am not sure if it is an error in rclcpp [2], in the controller_server [1] integration or a configuration issue with my setup.

Bug report

Required Info:

• Operating System:
  • Ubuntu 24.04
• Computer:
  • Lenovo thinkpad (simulation with gazebo) / Jetson Orin (actual robot)
• ROS2 Version:
  • Jazzy
• Version or commit hash:
  • 1.3.9
• DDS implementation:
  • CycloneDDS

Steps to reproduce issue

When I start a navigation task, I see inconsistents warnings from the controller server.

Expected behavior

Warnings from the controller server should be consistent (no warning from higher rate than the expected rate)

Actual behavior

[controller_server-12] [WARN] [1763737299.426491803] [controller_server]: Control loop missed its desired rate of 20.0000 Hz. Current loop rate is 50.0000 Hz.
[controller_server-12] [WARN] [1763737299.640464429] [controller_server]: Control loop missed its desired rate of 20.0000 Hz. Current loop rate is 25.0000 Hz.
[controller_server-12] [WARN] [1763737299.978117290] [controller_server]: Control loop missed its desired rate of 20.0000 Hz. Current loop rate is 50.0000 Hz.
[controller_server-12] [WARN] [1763737300.113303873] [controller_server]: Control loop missed its desired rate of 20.0000 Hz. Current loop rate is 33.3333 Hz.
[controller_server-12] [WARN] [1763737300.284088452] [controller_server]: Control loop missed its desired rate of 20.0000 Hz. Current loop rate is 50.0000 Hz.
[controller_server-12] [WARN] [1763737300.615473816] [controller_server]: Control loop missed its desired rate of 20.0000 Hz. Current loop rate is 33.3333 Hz.
[controller_server-12] [WARN] [1763737300.778039515] [controller_server]: Control loop missed its desired rate of 20.0000 Hz. Current loop rate is 50.0000 Hz.
[controller_server-12] [WARN] [1763737301.289526740] [controller_server]: Control loop missed its desired rate of 20.0000 Hz. Current loop rate is 50.0000 Hz.

Additional information

I tried to understand the cause of the bug, but I haven't found it yet. From what I understand, the warning is shown because the loop_rate returns False. It returns False when the configured rate cannot be reached, but the rate is reached since the logs' content.

[1]

navigation2/nav2_controller/src/controller_server.cpp

Line 523 in 88ca7c4

if (!loop_rate.sleep()) {

[2] https://github.com/ros2/rclcpp/blob/f79f57feb2bbc04ec8c2c4f3d21f72df1cfa659d/rclcpp/src/rclcpp/rate.cpp#L43

[mini-1235]

@gennartan can you provide your config and steps to reproduce?

[SteveMacenski]

Can you highlight when you see this happen? You mention 'I am seeing when starting a navigation task' but the logs you provide seem to be ~a control loop apart. So is this continuous? Do you see it on both the hardware using system time and in simulation using sim time?

25.0, 50.0, and 33.3 are very oddly round numbers, so that is making me extra suspicious

[gennartan]

Hello,
I made additional tests. The behavior seems slightly different in simulation than in real life.

In simulation:

• Oddly round number (25.0, 50.0, 33.3)
• Errors all the time during navigation

In real life:

• Mixed number (see below, most of the time, the warning is accurate (freq < 20.0), but sometime, a logged rate is higher )
• At the beginning of the navigation task (only when using the rotationShimController, when the control is given to the RPP controller ,there are no more warning)

---

My best guess is that we fall in [1] because the controller is too slow. In the next cycle, the loop will have less time to perform the same operation (since the next_interval = last_interval + period and not now + period). So it will fail again. However the log will be the same since the control loop is logging the actual loop time [2]. After a few iterations, the remaining loop time for a single cycle is thus reduced, the the loop_rate.sleep() will certainly fail, even if it performs its operation in less than a cycle time (since it started later than expected).

--> This explains what I see in real life, and would also mean that the rotation shim controller might require a bit of optimization. But this does not explain what happens in simulation where I see the inconsistents logs all the time.

[1] https://github.com/ros2/rclcpp/blob/jazzy/rclcpp/src/rclcpp/rate.cpp#L55-L66

  // Update the interval
  last_interval_ += period_;
  // If the time_to_sleep is negative or zero, don't sleep
  if (next_interval <= now) {
    // If an entire cycle was missed then reset next interval.
    // This might happen if the loop took more than a cycle.
    // Or if time jumps forward.
    if (now > next_interval + period_) {
      last_interval_ = now + period_;
    }
    // Either way do not sleep and return false
    return false;
  }

[2]

navigation2/nav2_controller/src/controller_server.cpp

Line 522 in 88ca7c4

auto cycle_duration = this->now() - start_time;

---

Logs when running on the robot:

[controller_server-13] [WARN] [1763990628.892901565] [controller_server]: Control loop missed its desired rate of 20.0000 Hz. Current loop rate is 19.2703 Hz.
[controller_server-13] [WARN] [1763990628.936918498] [controller_server]: Control loop missed its desired rate of 20.0000 Hz. Current loop rate is 22.7951 Hz.
[controller_server-13] [WARN] [1763990628.988235953] [controller_server]: Control loop missed its desired rate of 20.0000 Hz. Current loop rate is 19.5462 Hz.
[controller_server-13] [WARN] [1763990629.048028976] [controller_server]: Control loop missed its desired rate of 20.0000 Hz. Current loop rate is 16.7648 Hz.
[controller_server-13] [WARN] [1763990629.089240251] [controller_server]: Control loop missed its desired rate of 20.0000 Hz. Current loop rate is 24.3472 Hz.
[controller_server-13] [WARN] [1763990629.140696367] [controller_server]: Control loop missed its desired rate of 20.0000 Hz. Current loop rate is 19.4897 Hz.
[controller_server-13] [WARN] [1763990629.192922620] [controller_server]: Control loop missed its desired rate of 20.0000 Hz. Current loop rate is 19.2402 Hz.
[controller_server-13] [WARN] [1763990629.236149383] [controller_server]: Control loop missed its desired rate of 20.0000 Hz. Current loop rate is 23.7986 Hz.
[controller_server-13] [WARN] [1763990629.287645949] [controller_server]: Control loop missed its desired rate of 20.0000 Hz. Current loop rate is 19.4848 Hz.
[controller_server-13] [WARN] [1763990629.341572704] [controller_server]: Control loop missed its desired rate of 20.0000 Hz. Current loop rate is 18.5968 Hz.
[controller_server-13] [WARN] [1763990629.384329244] [controller_server]: Control loop missed its desired rate of 20.0000 Hz. Current loop rate is 23.4731 Hz.
[controller_server-13] [WARN] [1763990629.436324385] [controller_server]: Control loop missed its desired rate of 20.0000 Hz. Current loop rate is 19.2843 Hz.
[controller_server-13] [WARN] [1763990629.488544366] [controller_server]: Control loop missed its desired rate of 20.0000 Hz. Current loop rate is 19.2061 Hz.
[controller_server-13] [WARN] [1763990629.539996353] [controller_server]: Control loop missed its desired rate of 20.0000 Hz. Current loop rate is 19.4925 Hz.

---

My configuration

controller_server:
  ros__parameters:
    use_sim_time: False
    controller_frequency: 20.0 
    costmap_update_timeout: 0.3
    use_realtime_priorité: True
    publish_zero_velocity: True
    failure_tolerance: 0.3 
    progress_checker_plugins: ["progress_checker"]
    goal_checker_plugins: ["goal_checker"]
    controller_plugins: ["FollowPath"]
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.001
    min_theta_velocity_threshold: 0.001
    failure_tolerance: 0.0
    speed_limit_topic: "/speed_limit"
    odom_topic: "/odometry/filtered/odom"
    odom_duration: 0.3
    enable_stamped_cmd_vel: False
    bond_heartbeat_period: 0.1
    introspection_mode: "disabled"
    allow_parameter_qos_overrides: True

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 20.0
    # Goal checker parameters
    goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.30
      yaw_goal_tolerance: 3.14
      stateful: True

    FollowPath:
      plugin: "nav2_rotation_shim_controller::RotationShimController"
      primary_controller: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"

     # Rotation Shim controller
      angular_dist_threshold: 0.3
      angular_disengage_threshold: 0.2
      forward_sampling_distance: 0.3
      rotate_to_goal_heading: true
      max_angular_accel: 5.0 # Also used by RPP
      rotate_to_heading_angular_vel: 0.5 # also used by RPP
      simulate_ahead_time: 1.0

      # Regulated pure pursuit
      desired_linear_vel: 0.75
      lookahead_dist: 1.5 # only when use_velocity_scaled_lookahead_dist is False
      min_lookahead_dist: 0.25 # only when use_velocity_scaled_lookahead_dist is True
      max_lookahead_dist: 0.9 # only when use_velocity_scaled_lookahead_dist is True
      lookahead_time: 1.0
      transform_tolerance: 0.1
      use_velocity_scaled_lookahead_dist: true
      min_approach_linear_velocity: 0.05
      approach_velocity_scaling_dist: 1.0
      use_collision_detection: false
      max_allowed_time_to_collision_up_to_carrot: 1.0
      use_regulated_linear_velocity_scaling: true # slow down on high curvature paths
      use_cost_regulated_linear_velocity_scaling: false # needed for obstacle detection
      cost_scaling_dist: 0.3 # only when use_cost_regulated_linear_velocity_scaling is enabled
      cost_scaling_gain: 1.0 # only when use_cost_regulated_linear_velocity_scaling is enabled
      regulated_linear_scaling_min_radius: 1.5
      regulated_linear_scaling_min_speed: 0.10
      use_fixed_curvature_lookahead: true
      curvature_lookahead_dist: 1.2
      use_rotate_to_heading: false
      allow_reversing: false
      rotate_to_heading_min_angle: 0.785
      use_cancel_deceleration: false
      max_robot_pose_search_dist: 2.0 # default to local costmap width / 2
      interpolate_curvature_after_goal: false
      stateful: true
      allow_parameter_qos_overrides: true
      min_distance_to_obstacle: -1.0

[SteveMacenski]

What do you think the action item is here for us? Check on cycle_duration for whether to log or not instead?

[gennartan]

I don't think that checking on cycle_duration is the good approach here. The loop_rate.sleep() is returning the necessary information to be able to tell if we are on time or not.

Maybe we should reset [1] the wall rate to ensure that the next iteration has the same budget as the other ones ?

Another idea would be to update rclcpp to have an access to the private variable last_interval_. That would allow use to have a more accurate log based on the WallRate internal state. But that seems more complex for a small win.

[1] https://github.com/ros2/rclcpp/blob/f79f57feb2bbc04ec8c2c4f3d21f72df1cfa659d/rclcpp/include/rclcpp/rate.hpp#L115

[SteveMacenski]

Maybe we should reset [1] the wall rate to ensure that the next iteration has the same budget as the other ones ?

Where should we reset? The point of this Rate sleep is to smooth out the iteration cycles to account for the time the control algorithm takes to be the frequency a user set (or if missing its deadline, immediately triggering the next cycle to support as best it can). We want to make sure we maintain that behavior.

[gennartan]

I would place the reset like this:

    while (rclcpp::ok()) {
      do_controller_logic()

      auto cycle_duration = this->now() - start_time;
      if (!loop_rate.sleep()) {
        RCLCPP_WARN(
          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()
      }
    }

I made a small schematic to show the current behavior and the behavior with a reset.

• The current behavior is that if a single iteration takes longer that expected, all the following iterations will be impacted. In this case, the actual controller frequency might even be higher than the configured frequency.
• The suggested behavior is that after an iteration that took longer than expected, the loop_rate is reset so that the next iteration has the same budget (a single period) than the other ones.

This is useful if the controller sometimes has single iterations that are longer than expected. Actually this might even ensure that the actual frequency is never higher than the expected frequency.
This is not useful if the controller always exceeds the budget (but in this case this should not change the behavior since there will be a reset after each iteration)
This doesn't change anything if the controller always meets the budget

[SteveMacenski]

I agree, please open a PR!

[SteveMacenski]

PR #5723 merged