Skip to content

feat: adds enable_overrun parameter to CM #2540

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

feat: adds enable_overrun parameter to CM #2540

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
b3e52d9
feat: adds enable_overrun parameter to CM
BarisYazici Sep 16, 2025
55dffe7
disable the sleep with overrun disabled
BarisYazici Sep 16, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion controller_manager/src/controller_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<double>(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)
{
Expand Down
6 changes: 6 additions & 0 deletions controller_manager/src/controller_manager_parameters.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
19 changes: 13 additions & 6 deletions controller_manager/src/ros2_control_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<bool>("lock_memory", has_realtime);
Expand All @@ -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<int>("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))
Expand Down Expand Up @@ -149,26 +151,31 @@ int main(int argc, char ** argv)
{
cm->get_clock()->sleep_until(current_time + period);
}
else
else if(enable_overrun)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

With this we are simply removing the needed sleep too. Is this intended?

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, as stated in your PR -> It seems to be 'impossible' (without more complicated hardware setups), to have a synced loop. Aka, relative to my notebook, the master controller of the robot doesn't run with 1000 hz but in the example yesterday with 1003 hz. Hence, the sleep only works against us but not for us...

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

IMO, I don't think that part should ever be removed. Sleep is the one that ensures the frequency for everyone.

{
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<double>(
std::chrono::duration_cast<std::chrono::nanoseconds>(time_now - next_iteration_time)
.count()) /
static_cast<double>(std::chrono::duration_cast<std::chrono::nanoseconds>(
time_now - next_iteration_time)
.count()) /
1.e6;
const double cm_period = 1.e3 / static_cast<double>(cm->get_update_rate());
const int overrun_count = static_cast<int>(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);
}
}
Expand Down