Skip to content

Commit 2f80369

Browse files
committed
Throttle the overrun log
1 parent 274b22e commit 2f80369

File tree

1 file changed

+14
-11
lines changed

1 file changed

+14
-11
lines changed

controller_manager/src/controller_manager.cpp

Lines changed: 14 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -3159,17 +3159,20 @@ void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration
31593159
execution_time_.total_time =
31603160
execution_time_.write_time + execution_time_.update_time + execution_time_.read_time;
31613161
const double expected_cycle_time = 1.e6 / static_cast<double>(get_update_rate());
3162-
const bool print_log = execution_time_.total_time > expected_cycle_time;
3163-
RCLCPP_WARN_EXPRESSION(
3164-
get_logger(), print_log,
3165-
"Overrun might occur, Total time : %.3f us (Expected < %.3f us) --> Read time : %.3f us, "
3166-
"Update time : %.3f us (Switch time : %.3f us (Switch chained mode time : %.3f us, perform "
3167-
"mode change time : %.3f us, Activation time : %.3f us, Deactivation time : %.3f us)), Write "
3168-
"time : %.3f us",
3169-
execution_time_.total_time, expected_cycle_time, execution_time_.read_time,
3170-
execution_time_.update_time, execution_time_.switch_time,
3171-
execution_time_.switch_chained_mode_time, execution_time_.switch_perform_mode_time,
3172-
execution_time_.activation_time, execution_time_.deactivation_time, execution_time_.write_time);
3162+
if (execution_time_.total_time > expected_cycle_time)
3163+
{
3164+
RCLCPP_WARN_THROTTLE(
3165+
get_logger(), *get_clock(), 1000,
3166+
"Overrun might occur, Total time : %.3f us (Expected < %.3f us) --> Read time : %.3f us, "
3167+
"Update time : %.3f us (Switch time : %.3f us (Switch chained mode time : %.3f us, perform "
3168+
"mode change time : %.3f us, Activation time : %.3f us, Deactivation time : %.3f us)), Write "
3169+
"time : %.3f us",
3170+
execution_time_.total_time, expected_cycle_time, execution_time_.read_time,
3171+
execution_time_.update_time, execution_time_.switch_time,
3172+
execution_time_.switch_chained_mode_time, execution_time_.switch_perform_mode_time,
3173+
execution_time_.activation_time, execution_time_.deactivation_time,
3174+
execution_time_.write_time);
3175+
}
31733176
}
31743177

31753178
std::vector<ControllerSpec> &

0 commit comments

Comments
 (0)