diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 687a79512d..2d008dc843 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3181,7 +3181,7 @@ void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration execution_time_.total_time = execution_time_.write_time + execution_time_.update_time + execution_time_.read_time; const double expected_cycle_time = 1.e6 / static_cast(get_update_rate()); - if (execution_time_.total_time > expected_cycle_time && !use_sim_time_) + if (execution_time_.total_time > expected_cycle_time && !use_sim_time_ && params_->enable_overrun) { if (execution_time_.switch_time > 0.0) { diff --git a/controller_manager/src/controller_manager_parameters.yaml b/controller_manager/src/controller_manager_parameters.yaml index 55536d0522..a4fd7a0604 100644 --- a/controller_manager/src/controller_manager_parameters.yaml +++ b/controller_manager/src/controller_manager_parameters.yaml @@ -6,6 +6,12 @@ controller_manager: description: "The frequency of controller manager's real-time update loop. This loop reads states from hardware, updates controllers and writes commands to hardware." } + enable_overrun: { + type: bool, + default_value: true, + description: "If true, the controller manager will detect timing overruns, log warnings, and adjust the next iteration time to compensate for missed cycles." + } + enforce_command_limits: { type: bool, default_value: true, diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index c1fc38055f..fd88a15ec7 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -58,6 +58,7 @@ int main(int argc, char ** argv) executor, manager_node_name, "", cm_node_options); const bool use_sim_time = cm->get_parameter_or("use_sim_time", false); + const bool enable_overrun = cm->get_parameter_or("enable_overrun", true); const bool has_realtime = realtime_tools::has_realtime_kernel(); const bool lock_memory = cm->get_parameter_or("lock_memory", has_realtime); @@ -75,13 +76,14 @@ int main(int argc, char ** argv) cm->get_clock()->sleep_for(rclcpp::Duration::from_seconds(1.0 / cm->get_update_rate())); RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate()); + RCLCPP_INFO(cm->get_logger(), "Overrun logic: %s", enable_overrun ? "enabled " : "disabled"); const int thread_priority = cm->get_parameter_or("thread_priority", kSchedPriority); RCLCPP_INFO( cm->get_logger(), "Spawning %s RT thread with scheduler priority: %d", cm->get_name(), thread_priority); std::thread cm_thread( - [cm, thread_priority, use_sim_time]() + [cm, thread_priority, use_sim_time, enable_overrun]() { rclcpp::Parameter cpu_affinity_param; if (cm->get_parameter("cpu_affinity", cpu_affinity_param)) @@ -149,26 +151,31 @@ int main(int argc, char ** argv) { cm->get_clock()->sleep_until(current_time + period); } - else + else if(enable_overrun) { next_iteration_time += period; + const auto time_now = std::chrono::steady_clock::now(); if (next_iteration_time < time_now) { const double time_diff = - static_cast( - std::chrono::duration_cast(time_now - next_iteration_time) - .count()) / + static_cast(std::chrono::duration_cast( + time_now - next_iteration_time) + .count()) / 1.e6; const double cm_period = 1.e3 / static_cast(cm->get_update_rate()); const int overrun_count = static_cast(std::ceil(time_diff / cm_period)); + RCLCPP_WARN_THROTTLE( cm->get_logger(), *cm->get_clock(), 1000, - "Overrun detected! The controller manager missed its desired rate of %d Hz. The loop " + "Overrun detected! The controller manager missed its desired rate of %d Hz. The " + "loop " "took %f ms (missed cycles : %d).", cm->get_update_rate(), time_diff + cm_period, overrun_count + 1); + next_iteration_time += (overrun_count * period); } + std::this_thread::sleep_until(next_iteration_time); } }