diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index ea985c95d8..2405b48c8b 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -383,6 +383,12 @@ thread_priority (optional; int; default: 50) use_sim_time (optional; bool; default: false) Enables the use of simulation time in the ``controller_manager`` node. +overruns.manage (optional; bool; default: true) + Enables or disables the handling of overruns in the real-time loop of the ``controller_manager`` node. + If set to true, the controller manager will detect overruns caused by system time changes or longer execution times of the controllers and hardware components. + If an overrun is detected, the controller manager will print a warning message to the console. + When used with ``use_sim_time`` set to true, this parameter is ignored and the overrun handling is disabled. + Concepts ----------- diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 210a34cd95..db0b5bd110 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -583,11 +583,23 @@ void ControllerManager::initialize_parameters() // Initialize parameters try { + use_sim_time_ = this->get_parameter("use_sim_time").as_bool(); + + if (!this->has_parameter("overruns.print_warnings")) + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = + "If true, the controller manager will print a warning message to the console if an overrun " + "is detected in its real-time loop (read, update and write). By default, it is set to " + "true, except when used with use_sim_time parameter set to true."; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterValue(!use_sim_time_); + this->declare_parameter("overruns.print_warnings", parameter, descriptor); + } cm_param_listener_ = std::make_shared( this->get_node_parameters_interface(), this->get_logger()); params_ = std::make_shared(cm_param_listener_->get_params()); update_rate_ = static_cast(params_->update_rate); - use_sim_time_ = this->get_parameter("use_sim_time").as_bool(); trigger_clock_ = use_sim_time_ ? this->get_clock() : std::make_shared(RCL_STEADY_TIME); RCLCPP_INFO( @@ -3195,7 +3207,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 (params_->overruns.print_warnings && execution_time_.total_time > expected_cycle_time) { 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 796e6efe85..1c61f698aa 100644 --- a/controller_manager/src/controller_manager_parameters.yaml +++ b/controller_manager/src/controller_manager_parameters.yaml @@ -251,3 +251,8 @@ controller_manager: gt<>: 0.0, } } + overruns: + print_warnings: { + type: bool, + description: "If true, the controller manager will print a warning message to the console if an overrun is detected in its real-time loop (``read``, ``update`` and ``write``). By default, it is set to true, except when used with ``use_sim_time`` parameter set to true.", + } diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index c1fc38055f..271d91b4c9 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -75,13 +75,16 @@ 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()); + const bool manage_overruns = cm->get_parameter_or("overruns.manage", true); + RCLCPP_INFO( + cm->get_logger(), "Overruns handling is : %s", manage_overruns ? "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, manage_overruns]() { rclcpp::Parameter cpu_affinity_param; if (cm->get_parameter("cpu_affinity", cpu_affinity_param)) @@ -153,7 +156,7 @@ int main(int argc, char ** argv) { next_iteration_time += period; const auto time_now = std::chrono::steady_clock::now(); - if (next_iteration_time < time_now) + if (manage_overruns && next_iteration_time < time_now) { const double time_diff = static_cast( diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 3e4836e58e..355076c389 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -10,6 +10,7 @@ controller_manager * The default strictness of the ``switch_controllers`` can now we be chosen using ROS 2 parameters. The default behaviour is still left to ``BEST_EFFORT`` (`#2168 `_). * Parameter ``shutdown_on_initial_state_failure`` was added to avoid shutting down on hardware initial state failure (`#2230 `_). * The controller manager now publishes ``~/statistics/names`` and ``~/statistics/values`` topics to introspect the execution time and periodicity of the different entities running in the realtime loop (`#2449 `_). +* New parameters ``overruns.manage`` and ``overruns.print_warnings`` were added to control the behavior of the controller manager/ros2_control_node when overruns occur (`#2546 `_). hardware_interface ******************