From c9a0ab9cc2eb2f6825d8233c5d469a2095e54c53 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 29 Nov 2024 10:41:45 +0100 Subject: [PATCH 1/4] add logic for 'params_file' to handle both string and string_array (#1898) --- controller_manager/src/controller_manager.cpp | 41 +++++++++++-------- .../test/test_spawner_unspawner.cpp | 38 +++++++++++++++++ .../hardware_interface/controller_info.hpp | 2 +- 3 files changed, 64 insertions(+), 17 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index ebab7a2674..82546aaeda 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -551,16 +551,28 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c // read_only params, dynamic maps lists etc // Now check if the parameters_file parameter exist const std::string param_name = controller_name + ".params_file"; - std::vector parameters_files; + controller_spec.info.parameters_files.clear(); - // Check if parameter has been declared - if (!has_parameter(param_name)) + // get_parameter checks if parameter has been declared/set + rclcpp::Parameter params_files_parameter; + if (get_parameter(param_name, params_files_parameter)) { - declare_parameter(param_name, rclcpp::ParameterType::PARAMETER_STRING_ARRAY); - } - if (get_parameter(param_name, parameters_files) && !parameters_files.empty()) - { - controller_spec.info.parameters_files = parameters_files; + if (params_files_parameter.get_type() == rclcpp::ParameterType::PARAMETER_STRING_ARRAY) + { + controller_spec.info.parameters_files = params_files_parameter.as_string_array(); + } + else if (params_files_parameter.get_type() == rclcpp::ParameterType::PARAMETER_STRING) + { + controller_spec.info.parameters_files.push_back(params_files_parameter.as_string()); + } + else + { + RCLCPP_ERROR( + get_logger(), + "The 'params_file' param needs to be a string or a string array for '%s', but it is of " + "type %s", + controller_name.c_str(), params_files_parameter.get_type_name().c_str()); + } } const std::string fallback_ctrl_param = controller_name + ".fallback_controllers"; @@ -3250,17 +3262,14 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( node_options_arguments.push_back(arg); } - if (controller.info.parameters_files.has_value()) + for (const auto & parameters_file : controller.info.parameters_files) { - for (const auto & parameters_file : controller.info.parameters_files.value()) + if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG)) { - if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG)) - { - node_options_arguments.push_back(RCL_ROS_ARGS_FLAG); - } - node_options_arguments.push_back(RCL_PARAM_FILE_FLAG); - node_options_arguments.push_back(parameters_file); + node_options_arguments.push_back(RCL_ROS_ARGS_FLAG); } + node_options_arguments.push_back(RCL_PARAM_FILE_FLAG); + node_options_arguments.push_back(parameters_file); } // ensure controller's `use_sim_time` parameter matches controller_manager's diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index ca580b1130..cd2eee31f8 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -255,6 +255,44 @@ TEST_F(TestLoadController, multi_ctrls_test_type_in_param) } } +TEST_F(TestLoadController, spawner_test_with_params_file_string_parameter) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + + cm_->set_parameter(rclcpp::Parameter( + "ctrl_with_parameters_and_type.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter( + rclcpp::Parameter("ctrl_with_parameters_and_type.params_file", test_file_path)); + + ControllerManagerRunner cm_runner(this); + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner("ctrl_with_parameters_and_type --load-only -c test_controller_manager"), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path); + auto ctrl_node = ctrl_with_parameters_and_type.c->get_node(); + ASSERT_THAT( + ctrl_with_parameters_and_type.info.parameters_files, + std::vector({test_file_path})); + if (!ctrl_node->has_parameter("joint_names")) + { + ctrl_node->declare_parameter("joint_names", std::vector({"random_joint"})); + } + ASSERT_THAT( + ctrl_node->get_parameter("joint_names").as_string_array(), + std::vector({"joint0"})); +} + TEST_F(TestLoadController, spawner_test_type_in_params_file) { const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + diff --git a/hardware_interface/include/hardware_interface/controller_info.hpp b/hardware_interface/include/hardware_interface/controller_info.hpp index 4563e2dc75..3ad89551d5 100644 --- a/hardware_interface/include/hardware_interface/controller_info.hpp +++ b/hardware_interface/include/hardware_interface/controller_info.hpp @@ -34,7 +34,7 @@ struct ControllerInfo std::string type; /// Controller param file - std::optional> parameters_files; + std::vector parameters_files; /// List of claimed interfaces by the controller. std::vector claimed_interfaces; From 2b22d0cdf28412fc4fde7200c367473dfba754af Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 29 Nov 2024 13:12:46 +0100 Subject: [PATCH 2/4] Add more parameter overriding tests by parsing multiple parameter files (#1899) --- controller_manager/CMakeLists.txt | 3 + ...test_controller_overriding_parameters.yaml | 5 + .../test_controller_spawner_with_type.yaml | 1 + .../test/test_spawner_unspawner.cpp | 114 ++++++++++++++++++ 4 files changed, 123 insertions(+) create mode 100644 controller_manager/test/test_controller_overriding_parameters.yaml diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 1bb84eb32c..ea5fc176cd 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -210,6 +210,9 @@ if(BUILD_TESTING) install(FILES test/test_controller_spawner_with_type.yaml DESTINATION test) + install(FILES test/test_controller_overriding_parameters.yaml + DESTINATION test) + ament_add_gmock(test_hardware_management_srvs test/test_hardware_management_srvs.cpp ) diff --git a/controller_manager/test/test_controller_overriding_parameters.yaml b/controller_manager/test/test_controller_overriding_parameters.yaml new file mode 100644 index 0000000000..115d0ed993 --- /dev/null +++ b/controller_manager/test/test_controller_overriding_parameters.yaml @@ -0,0 +1,5 @@ +ctrl_with_parameters_and_type: + ros__parameters: + interface_name: "impedance" + joint_offset: 0.2 + joint_names: ["joint10"] diff --git a/controller_manager/test/test_controller_spawner_with_type.yaml b/controller_manager/test/test_controller_spawner_with_type.yaml index 087994bd23..23fd69b216 100644 --- a/controller_manager/test/test_controller_spawner_with_type.yaml +++ b/controller_manager/test/test_controller_spawner_with_type.yaml @@ -2,6 +2,7 @@ ctrl_with_parameters_and_type: ros__parameters: type: "controller_manager/test_controller" joint_names: ["joint0"] + interface_name: "position" /**: chainable_ctrl_with_parameters_and_type: diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index cd2eee31f8..70a5480a06 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -291,6 +291,12 @@ TEST_F(TestLoadController, spawner_test_with_params_file_string_parameter) ASSERT_THAT( ctrl_node->get_parameter("joint_names").as_string_array(), std::vector({"joint0"})); + + if (!ctrl_node->has_parameter("interface_name")) + { + ctrl_node->declare_parameter("interface_name", "invalid_interface"); + } + ASSERT_EQ(ctrl_node->get_parameter("interface_name").as_string(), "position"); } TEST_F(TestLoadController, spawner_test_type_in_params_file) @@ -397,6 +403,114 @@ TEST_F(TestLoadController, unload_on_kill_activate_as_group) ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul); } +TEST_F(TestLoadController, spawner_test_to_check_parameter_overriding) +{ + const std::string main_test_file_path = + ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + const std::string overriding_test_file_path = + ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_overriding_parameters.yaml"; + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager -p " + + main_test_file_path + " -p " + overriding_test_file_path), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_THAT( + cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array(), + std::vector({main_test_file_path, overriding_test_file_path})); + auto ctrl_node = ctrl_with_parameters_and_type.c->get_node(); + ASSERT_THAT( + ctrl_with_parameters_and_type.info.parameters_files, + std::vector({main_test_file_path, overriding_test_file_path})); + if (!ctrl_node->has_parameter("joint_names")) + { + ctrl_node->declare_parameter("joint_names", std::vector({"random_joint"})); + } + ASSERT_THAT( + ctrl_node->get_parameter("joint_names").as_string_array(), + std::vector({"joint10"})); + + if (!ctrl_node->has_parameter("interface_name")) + { + ctrl_node->declare_parameter("interface_name", "invalid_interface"); + } + ASSERT_EQ(ctrl_node->get_parameter("interface_name").as_string(), "impedance") + << "The parameter should be overridden"; + + if (!ctrl_node->has_parameter("joint_offset")) + { + ctrl_node->declare_parameter("joint_offset", -M_PI); + } + ASSERT_EQ(ctrl_node->get_parameter("joint_offset").as_double(), 0.2); +} + +TEST_F(TestLoadController, spawner_test_to_check_parameter_overriding_reverse) +{ + const std::string main_test_file_path = + ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_overriding_parameters.yaml"; + const std::string overriding_test_file_path = + ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager -p " + + main_test_file_path + " -p " + overriding_test_file_path), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_THAT( + cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array(), + std::vector({main_test_file_path, overriding_test_file_path})); + auto ctrl_node = ctrl_with_parameters_and_type.c->get_node(); + ASSERT_THAT( + ctrl_with_parameters_and_type.info.parameters_files, + std::vector({main_test_file_path, overriding_test_file_path})); + if (!ctrl_node->has_parameter("joint_names")) + { + ctrl_node->declare_parameter("joint_names", std::vector({"random_joint"})); + } + ASSERT_THAT( + ctrl_node->get_parameter("joint_names").as_string_array(), + std::vector({"joint0"})); + + if (!ctrl_node->has_parameter("interface_name")) + { + ctrl_node->declare_parameter("interface_name", "invalid_interface"); + } + ASSERT_EQ(ctrl_node->get_parameter("interface_name").as_string(), "position") + << "The parameter should be overridden"; + + if (!ctrl_node->has_parameter("joint_offset")) + { + ctrl_node->declare_parameter("joint_offset", -M_PI); + } + ASSERT_EQ(ctrl_node->get_parameter("joint_offset").as_double(), 0.2); +} + TEST_F(TestLoadController, spawner_test_fallback_controllers) { const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + From dd8441417cb36a880f947feac43d9ee1eb200dfa Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 1 Dec 2024 11:10:20 +0100 Subject: [PATCH 3/4] Bump version of pre-commit hooks (#1902) Co-authored-by: christophfroehlich <3367244+christophfroehlich@users.noreply.github.com> --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 205e0f63ab..75c5402ffe 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -63,7 +63,7 @@ repos: # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v19.1.3 + rev: v19.1.4 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] @@ -133,7 +133,7 @@ repos: exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.29.4 + rev: 0.30.0 hooks: - id: check-github-workflows args: ["--verbose"] From 89fd4ef05e1fc107c9d1b2b122292665ccca6f36 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 3 Dec 2024 16:28:40 +0100 Subject: [PATCH 4/4] [Diagnostics] Add diagnostics of execution time and periodicity of the controllers and controller_manager (#1871) --- .../controller_interface_base.hpp | 27 +- .../src/controller_interface_base.cpp | 24 +- controller_manager/CMakeLists.txt | 11 +- .../doc/parameters_context.yaml | 21 ++ controller_manager/doc/userdoc.rst | 36 +-- .../controller_manager/controller_manager.hpp | 10 +- .../controller_manager/controller_spec.hpp | 13 + controller_manager/package.xml | 2 + controller_manager/src/controller_manager.cpp | 286 ++++++++++++++---- .../src/controller_manager_parameters.yaml | 136 +++++++++ controller_manager/src/ros2_control_node.cpp | 2 +- .../test/test_controller_manager.cpp | 242 ++++++++++++++- .../test/test_hardware_management_srvs.cpp | 4 +- 13 files changed, 703 insertions(+), 111 deletions(-) create mode 100644 controller_manager/doc/parameters_context.yaml create mode 100644 controller_manager/src/controller_manager_parameters.yaml diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index ae3804919c..e41779cf7b 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -71,6 +71,25 @@ struct ControllerUpdateStats unsigned int total_triggers; unsigned int failed_triggers; }; + +/** + * Struct to store the status of the controller update method. + * The status contains information if the update was triggered successfully, the result of the + * update method and the execution duration of the update method. The status is used to provide + * feedback to the controller_manager. + * @var successful: true if the update was triggered successfully, false if not. + * @var result: return_type::OK if update is successfully, otherwise return_type::ERROR. + * @var execution_time: duration of the execution of the update method. + * @var period: period of the update method. + */ +struct ControllerUpdateStatus +{ + bool successful = true; + return_type result = return_type::OK; + std::optional execution_time = std::nullopt; + std::optional period = std::nullopt; +}; + /** * Base interface class for an controller. The interface may not be used to implement a controller. * The class provides definitions for `ControllerInterface` and `ChainableControllerInterface` @@ -175,13 +194,11 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy * * \param[in] time The time at the start of this control loop iteration * \param[in] period The measured time taken by the last control loop iteration - * \returns A pair with the first element being a boolean indicating if the async callback method - * was triggered and the second element being the last return value of the async function. For - * more details check the AsyncFunctionHandler implementation in `realtime_tools` package. + * \returns ControllerUpdateStatus. The status contains information if the update was triggered + * successfully, the result of the update method and the execution duration of the update method. */ CONTROLLER_INTERFACE_PUBLIC - std::pair trigger_update( - const rclcpp::Time & time, const rclcpp::Duration & period); + ControllerUpdateStatus trigger_update(const rclcpp::Time & time, const rclcpp::Duration & period); CONTROLLER_INTERFACE_PUBLIC std::shared_ptr get_node(); diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 0713ec3c25..065fe77061 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -159,12 +159,14 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() c return node_->get_current_state(); } -std::pair ControllerInterfaceBase::trigger_update( +ControllerUpdateStatus ControllerInterfaceBase::trigger_update( const rclcpp::Time & time, const rclcpp::Duration & period) { + ControllerUpdateStatus status; trigger_stats_.total_triggers++; if (is_async()) { + const rclcpp::Time last_trigger_time = async_handler_->get_current_callback_time(); const auto result = async_handler_->trigger_async_callback(time, period); if (!result.first) { @@ -174,12 +176,28 @@ std::pair ControllerInterfaceBase::trigger_update( "The controller missed %u update cycles out of %u total triggers.", trigger_stats_.failed_triggers, trigger_stats_.total_triggers); } - return result; + status.successful = result.first; + status.result = result.second; + const auto execution_time = async_handler_->get_last_execution_time(); + if (execution_time.count() > 0) + { + status.execution_time = execution_time; + } + if (last_trigger_time.get_clock_type() != RCL_CLOCK_UNINITIALIZED) + { + status.period = time - last_trigger_time; + } } else { - return std::make_pair(true, update(time, period)); + const auto start_time = std::chrono::steady_clock::now(); + status.successful = true; + status.result = update(time, period); + status.execution_time = std::chrono::duration_cast( + std::chrono::steady_clock::now() - start_time); + status.period = period; } + return status; } std::shared_ptr ControllerInterfaceBase::get_node() diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index ea5fc176cd..dba9531e45 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -16,6 +16,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp realtime_tools std_msgs + libstatistics_collector + generate_parameter_library ) find_package(ament_cmake REQUIRED) @@ -27,6 +29,10 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +generate_parameter_library(controller_manager_parameters + src/controller_manager_parameters.yaml +) + add_library(controller_manager SHARED src/controller_manager.cpp ) @@ -35,6 +41,9 @@ target_include_directories(controller_manager PUBLIC $ $ ) +target_link_libraries(controller_manager PUBLIC + controller_manager_parameters +) ament_target_dependencies(controller_manager PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, @@ -236,7 +245,7 @@ install( DESTINATION include/controller_manager ) install( - TARGETS controller_manager + TARGETS controller_manager controller_manager_parameters EXPORT export_controller_manager RUNTIME DESTINATION bin LIBRARY DESTINATION lib diff --git a/controller_manager/doc/parameters_context.yaml b/controller_manager/doc/parameters_context.yaml new file mode 100644 index 0000000000..a015765c79 --- /dev/null +++ b/controller_manager/doc/parameters_context.yaml @@ -0,0 +1,21 @@ +hardware_components_initial_state: | + Map of parameters for controlled lifecycle management of hardware components. + The names of the components are defined as attribute of ````-tag in ``robot_description``. + Hardware components found in ``robot_description``, but without explicit state definition will be immediately activated. + Detailed explanation of each parameter is given below. + The full structure of the map is given in the following example: + + .. code-block:: yaml + + hardware_components_initial_state: + unconfigured: + - "arm1" + - "arm2" + inactive: + - "base3" + +diagnostics.threshold.controllers.periodicity: | + The ``periodicity`` diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity. + +diagnostics.threshold.controllers.execution_time: | + The ``execution_time`` diagnostics will be published for all controllers. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period cycle to execute it's update cycle. diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 5a86c3373b..872a695d0f 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -57,32 +57,6 @@ robot_description [std_msgs::msg::String] Parameters ----------- -hardware_components_initial_state - Map of parameters for controlled lifecycle management of hardware components. - The names of the components are defined as attribute of ````-tag in ``robot_description``. - Hardware components found in ``robot_description``, but without explicit state definition will be immediately activated. - Detailed explanation of each parameter is given below. - The full structure of the map is given in the following example: - -.. code-block:: yaml - - hardware_components_initial_state: - unconfigured: - - "arm1" - - "arm2" - inactive: - - "base3" - -hardware_components_initial_state.unconfigured (optional; list; default: empty) - Defines which hardware components will be only loaded immediately when controller manager is started. - -hardware_components_initial_state.inactive (optional; list; default: empty) - Defines which hardware components will be configured immediately when controller manager is started. - -update_rate (mandatory; integer) - The frequency of controller manager's real-time update loop. - This loop reads states from hardware, updates controller and writes commands to hardware. - .type Name of a plugin exported using ``pluginlib`` for a controller. This is a class from which controller's instance with name "``controller_name``" is created. @@ -99,6 +73,16 @@ update_rate (mandatory; integer) The fallback controllers activation is subject to the availability of the state and command interfaces at the time of activation. It is recommended to test the fallback strategy in simulation before deploying it on the real robot. +.. generate_parameter_library_details:: + ../src/controller_manager_parameters.yaml + parameters_context.yaml + +**An example parameter file:** + +.. generate_parameter_library_default:: + ../src/controller_manager_parameters.yaml + + Handling Multiple Controller Managers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 068eefc1f9..332d565238 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -50,6 +50,8 @@ namespace controller_manager { +class ParamListener; +class Params; using ControllersListIterator = std::vector::const_iterator; CONTROLLER_MANAGER_PUBLIC rclcpp::NodeOptions get_cm_node_options(); @@ -346,7 +348,7 @@ class ControllerManager : public rclcpp::Node // Per controller update rate support unsigned int update_loop_counter_ = 0; - unsigned int update_rate_ = 100; + unsigned int update_rate_; std::vector> chained_controllers_configuration_; std::unique_ptr resource_manager_; @@ -357,6 +359,8 @@ class ControllerManager : public rclcpp::Node const std::string & command_interface); void init_controller_manager(); + void initialize_parameters(); + /** * Clear request lists used when switching controllers. The lists are shared between "callback" * and "control loop" threads. @@ -473,6 +477,8 @@ class ControllerManager : public rclcpp::Node */ rclcpp::NodeOptions determine_controller_node_options(const ControllerSpec & controller) const; + std::shared_ptr cm_param_listener_; + std::shared_ptr params_; diagnostic_updater::Updater diagnostics_updater_; std::shared_ptr executor_; @@ -603,6 +609,8 @@ class ControllerManager : public rclcpp::Node rclcpp::Subscription::SharedPtr robot_description_subscription_; rclcpp::TimerBase::SharedPtr robot_description_notification_timer_; + controller_manager::MovingAverageStatistics periodicity_stats_; + struct SwitchParams { void reset() diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 0f44867814..9cc508772d 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -24,9 +24,13 @@ #include #include "controller_interface/controller_interface_base.hpp" #include "hardware_interface/controller_info.hpp" +#include "libstatistics_collector/moving_average_statistics/moving_average.hpp" namespace controller_manager { + +using MovingAverageStatistics = + libstatistics_collector::moving_average_statistics::MovingAverageStatistics; /// Controller Specification /** * This struct contains both a pointer to a given controller, \ref c, as well @@ -35,9 +39,18 @@ namespace controller_manager */ struct ControllerSpec { + ControllerSpec() + { + last_update_cycle_time = std::make_shared(0, 0, RCL_CLOCK_UNINITIALIZED); + execution_time_statistics = std::make_shared(); + periodicity_statistics = std::make_shared(); + } + hardware_interface::ControllerInfo info; controller_interface::ControllerInterfaceBaseSharedPtr c; std::shared_ptr last_update_cycle_time; + std::shared_ptr execution_time_statistics; + std::shared_ptr periodicity_statistics; }; struct ControllerChainSpec diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 18189d5d16..42211e4e40 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -28,6 +28,8 @@ ros2param ros2run std_msgs + libstatistics_collector + generate_parameter_library ament_cmake_gmock ament_cmake_pytest diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 82546aaeda..56e95c9f7b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -28,6 +28,8 @@ #include "rclcpp/version.h" #include "rclcpp_lifecycle/state.hpp" +#include "controller_manager_parameters.hpp" + namespace // utility { static constexpr const char * kControllerInterfaceNamespace = "controller_interface"; @@ -237,6 +239,7 @@ ControllerManager::ControllerManager( kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), cm_node_options_(options) { + initialize_parameters(); init_controller_manager(); } @@ -245,10 +248,6 @@ ControllerManager::ControllerManager( bool activate_all_hw_components, const std::string & manager_node_name, const std::string & node_namespace, const rclcpp::NodeOptions & options) : rclcpp::Node(manager_node_name, node_namespace, options), - update_rate_(get_parameter_or("update_rate", 100)), - resource_manager_(std::make_unique( - urdf, this->get_node_clock_interface(), this->get_node_logging_interface(), - activate_all_hw_components, update_rate_)), diagnostics_updater_(this), executor_(executor), loader_(std::make_shared>( @@ -258,6 +257,10 @@ ControllerManager::ControllerManager( kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), cm_node_options_(options) { + initialize_parameters(); + resource_manager_ = std::make_unique( + urdf, this->get_node_clock_interface(), this->get_node_logging_interface(), + activate_all_hw_components, params_->update_rate); init_controller_manager(); } @@ -276,18 +279,13 @@ ControllerManager::ControllerManager( kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), cm_node_options_(options) { + initialize_parameters(); init_controller_manager(); } void ControllerManager::init_controller_manager() { // Get parameters needed for RT "update" loop to work - if (!get_parameter("update_rate", update_rate_)) - { - RCLCPP_WARN( - get_logger(), "'update_rate' parameter not set, using default value of %d Hz.", update_rate_); - } - if (is_resource_manager_initialized()) { init_services(); @@ -313,6 +311,7 @@ void ControllerManager::init_controller_manager() robot_description_subscription_->get_topic_name()); // Setup diagnostics + periodicity_stats_.Reset(); diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); @@ -324,6 +323,25 @@ void ControllerManager::init_controller_manager() &ControllerManager::controller_manager_diagnostic_callback); } +void ControllerManager::initialize_parameters() +{ + // Initialize parameters + try + { + 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); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + this->get_logger(), + "Exception thrown while initializing controller manager parameters: %s \n", e.what()); + throw e; + } +} + void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description) { RCLCPP_INFO(get_logger(), "Received robot description from topic."); @@ -365,51 +383,52 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript using lifecycle_msgs::msg::State; auto set_components_to_state = - [&](const std::string & parameter_name, rclcpp_lifecycle::State state) + [&](const std::vector & components_to_set, rclcpp_lifecycle::State state) { - std::vector components_to_set = std::vector({}); - if (get_parameter(parameter_name, components_to_set)) + for (const auto & component : components_to_set) { - for (const auto & component : components_to_set) + if (component.empty()) { - if (component.empty()) - { - continue; - } - if (components_to_activate.find(component) == components_to_activate.end()) - { - RCLCPP_WARN( - get_logger(), "Hardware component '%s' is unknown, therefore not set in '%s' state.", - component.c_str(), state.label().c_str()); - } - else + continue; + } + if (components_to_activate.find(component) == components_to_activate.end()) + { + RCLCPP_WARN( + get_logger(), "Hardware component '%s' is unknown, therefore not set in '%s' state.", + component.c_str(), state.label().c_str()); + } + else + { + RCLCPP_INFO( + get_logger(), "Setting component '%s' to '%s' state.", component.c_str(), + state.label().c_str()); + if ( + resource_manager_->set_component_state(component, state) == + hardware_interface::return_type::ERROR) { - RCLCPP_INFO( - get_logger(), "Setting component '%s' to '%s' state.", component.c_str(), - state.label().c_str()); - if ( - resource_manager_->set_component_state(component, state) == - hardware_interface::return_type::ERROR) - { - throw std::runtime_error( - "Failed to set the initial state of the component : " + component + " to " + - state.label()); - } - components_to_activate.erase(component); + throw std::runtime_error( + "Failed to set the initial state of the component : " + component + " to " + + state.label()); } + components_to_activate.erase(component); } } }; + if (cm_param_listener_->is_old(*params_)) + { + *params_ = cm_param_listener_->get_params(); + } + // unconfigured (loaded only) set_components_to_state( - "hardware_components_initial_state.unconfigured", + params_->hardware_components_initial_state.unconfigured, rclcpp_lifecycle::State( State::PRIMARY_STATE_UNCONFIGURED, hardware_interface::lifecycle_state_names::UNCONFIGURED)); // inactive (configured) set_components_to_state( - "hardware_components_initial_state.inactive", + params_->hardware_components_initial_state.inactive, rclcpp_lifecycle::State( State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE)); @@ -545,6 +564,8 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_spec.info.type = controller_type; controller_spec.last_update_cycle_time = std::make_shared( 0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()); + controller_spec.execution_time_statistics = std::make_shared(); + controller_spec.periodicity_statistics = std::make_shared(); // We have to fetch the parameters_file at the time of loading the controller, because this way we // can load them at the creation of the LifeCycleNode and this helps in using the features such as @@ -1774,6 +1795,8 @@ void ControllerManager::activate_controllers( try { + found_it->periodicity_statistics->Reset(); + found_it->execution_time_statistics->Reset(); const auto new_state = controller->get_node()->activate(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { @@ -2255,6 +2278,7 @@ std::vector ControllerManager::get_controller_names() void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & period) { + periodicity_stats_.AddMeasurement(1.0 / period.seconds()); auto [ok, failed_hardware_names] = resource_manager_->read(time, period); if (!ok) @@ -2367,12 +2391,14 @@ controller_interface::return_type ControllerManager::update( : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); const rclcpp::Time current_time = get_clock()->now(); + bool first_update_cycle = false; if ( *loaded_controller.last_update_cycle_time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) { + first_update_cycle = true; // it is zero after activation - *loaded_controller.last_update_cycle_time = current_time - controller_period; + *loaded_controller.last_update_cycle_time = current_time; RCLCPP_DEBUG( get_logger(), "Setting last_update_cycle_time to %fs for the controller : %s", loaded_controller.last_update_cycle_time->seconds(), loaded_controller.info.name.c_str()); @@ -2390,7 +2416,7 @@ controller_interface::return_type ControllerManager::update( run_controller_at_cm_rate || (time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) || - (controller_actual_period.seconds() * controller_update_rate >= 0.99); + (controller_actual_period.seconds() * controller_update_rate >= 0.99) || first_update_cycle; RCLCPP_DEBUG( get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '", @@ -2404,8 +2430,20 @@ controller_interface::return_type ControllerManager::update( // Catch exceptions thrown by the controller update function try { - std::tie(trigger_status, controller_ret) = - loaded_controller.c->trigger_update(time, controller_actual_period); + const auto trigger_result = + loaded_controller.c->trigger_update(current_time, controller_actual_period); + trigger_status = trigger_result.successful; + controller_ret = trigger_result.result; + if (trigger_status && trigger_result.execution_time.has_value()) + { + loaded_controller.execution_time_statistics->AddMeasurement( + static_cast(trigger_result.execution_time.value().count()) / 1.e3); + } + if (!first_update_cycle && trigger_status && trigger_result.period.has_value()) + { + loaded_controller.periodicity_statistics->AddMeasurement( + 1.0 / trigger_result.period.value().seconds()); + } } catch (const std::exception & e) { @@ -3062,34 +3100,140 @@ void ControllerManager::controller_activity_diagnostic_callback( std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); const std::vector & controllers = rt_controllers_wrapper_.get_updated_list(guard); bool all_active = true; + const std::string periodicity_suffix = ".periodicity"; + const std::string exec_time_suffix = ".execution_time"; + const std::string state_suffix = ".state"; + + if (cm_param_listener_->is_old(*params_)) + { + *params_ = cm_param_listener_->get_params(); + } + + auto make_stats_string = + [](const auto & statistics_data, const std::string & measurement_unit) -> std::string + { + std::ostringstream oss; + oss << std::fixed << std::setprecision(2); + oss << "Avg: " << statistics_data.average << " [" << statistics_data.min << " - " + << statistics_data.max << "] " << measurement_unit + << ", StdDev: " << statistics_data.standard_deviation; + return oss.str(); + }; + + // Variable to define the overall status of the controller diagnostics + auto level = diagnostic_msgs::msg::DiagnosticStatus::OK; + + std::vector high_exec_time_controllers; + std::vector bad_periodicity_async_controllers; for (size_t i = 0; i < controllers.size(); ++i) { + const bool is_async = controllers[i].c->is_async(); if (!is_controller_active(controllers[i].c)) { all_active = false; } - stat.add(controllers[i].info.name, controllers[i].c->get_lifecycle_state().label()); + stat.add( + controllers[i].info.name + state_suffix, controllers[i].c->get_lifecycle_state().label()); + if (is_controller_active(controllers[i].c)) + { + const auto periodicity_stats = controllers[i].periodicity_statistics->GetStatistics(); + const auto exec_time_stats = controllers[i].execution_time_statistics->GetStatistics(); + stat.add( + controllers[i].info.name + exec_time_suffix, make_stats_string(exec_time_stats, "us")); + if (is_async) + { + stat.add( + controllers[i].info.name + periodicity_suffix, + make_stats_string(periodicity_stats, "Hz") + + " -> Desired : " + std::to_string(controllers[i].c->get_update_rate()) + " Hz"); + const double periodicity_error = std::abs( + periodicity_stats.average - static_cast(controllers[i].c->get_update_rate())); + if ( + periodicity_error > + params_->diagnostics.threshold.controllers.periodicity.mean_error.error || + periodicity_stats.standard_deviation > + params_->diagnostics.threshold.controllers.periodicity.standard_deviation.error) + { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + add_element_to_list(bad_periodicity_async_controllers, controllers[i].info.name); + } + else if ( + periodicity_error > + params_->diagnostics.threshold.controllers.periodicity.mean_error.warn || + periodicity_stats.standard_deviation > + params_->diagnostics.threshold.controllers.periodicity.standard_deviation.warn) + { + if (level != diagnostic_msgs::msg::DiagnosticStatus::ERROR) + { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } + add_element_to_list(bad_periodicity_async_controllers, controllers[i].info.name); + } + } + const double max_exp_exec_time = is_async ? 1.e6 / controllers[i].c->get_update_rate() : 0.0; + if ( + (exec_time_stats.average - max_exp_exec_time) > + params_->diagnostics.threshold.controllers.execution_time.mean_error.error || + exec_time_stats.standard_deviation > + params_->diagnostics.threshold.controllers.execution_time.standard_deviation.error) + { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + high_exec_time_controllers.push_back(controllers[i].info.name); + } + else if ( + (exec_time_stats.average - max_exp_exec_time) > + params_->diagnostics.threshold.controllers.execution_time.mean_error.warn || + exec_time_stats.standard_deviation > + params_->diagnostics.threshold.controllers.execution_time.standard_deviation.warn) + { + if (level != diagnostic_msgs::msg::DiagnosticStatus::ERROR) + { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } + high_exec_time_controllers.push_back(controllers[i].info.name); + } + } + } + + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::OK, + all_active ? "All controllers are active" : "Not all controllers are active"); + + if (!high_exec_time_controllers.empty()) + { + std::string high_exec_time_controllers_string; + for (const auto & controller : high_exec_time_controllers) + { + high_exec_time_controllers_string.append(controller); + high_exec_time_controllers_string.append(" "); + } + stat.mergeSummary( + level, + "\nControllers with high execution time : [ " + high_exec_time_controllers_string + "]"); + } + if (!bad_periodicity_async_controllers.empty()) + { + std::string bad_periodicity_async_controllers_string; + for (const auto & controller : bad_periodicity_async_controllers) + { + bad_periodicity_async_controllers_string.append(controller); + bad_periodicity_async_controllers_string.append(" "); + } + stat.mergeSummary( + level, + "\nControllers with bad periodicity : [ " + bad_periodicity_async_controllers_string + "]"); } if (!atleast_one_hw_active) { - stat.summary( + stat.mergeSummary( diagnostic_msgs::msg::DiagnosticStatus::ERROR, "No hardware components are currently active to activate controllers"); } - else + else if (controllers.empty()) { - if (controllers.empty()) - { - stat.summary( - diagnostic_msgs::msg::DiagnosticStatus::WARN, "No controllers are currently loaded"); - } - else - { - stat.summary( - diagnostic_msgs::msg::DiagnosticStatus::OK, - all_active ? "All controllers are active" : "Not all controllers are active"); - } + stat.mergeSummary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "No controllers are currently loaded"); } } @@ -3142,7 +3286,14 @@ void ControllerManager::hardware_components_diagnostic_callback( void ControllerManager::controller_manager_diagnostic_callback( diagnostic_updater::DiagnosticStatusWrapper & stat) { + const std::string periodicity_stat_name = "periodicity"; + const auto cm_stats = periodicity_stats_.GetStatistics(); stat.add("update_rate", std::to_string(get_update_rate())); + stat.add(periodicity_stat_name + ".average", std::to_string(cm_stats.average)); + stat.add( + periodicity_stat_name + ".standard_deviation", std::to_string(cm_stats.standard_deviation)); + stat.add(periodicity_stat_name + ".min", std::to_string(cm_stats.min)); + stat.add(periodicity_stat_name + ".max", std::to_string(cm_stats.max)); if (is_resource_manager_initialized()) { stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Controller Manager is running"); @@ -3161,6 +3312,27 @@ void ControllerManager::controller_manager_diagnostic_callback( "Resource Manager is not initialized properly!"); } } + + const double periodicity_error = std::abs(cm_stats.average - get_update_rate()); + const std::string diag_summary = + "Controller Manager has bad periodicity : " + std::to_string(cm_stats.average) + + " Hz. Expected consistent " + std::to_string(get_update_rate()) + " Hz"; + if ( + periodicity_error > + params_->diagnostics.threshold.controller_manager.periodicity.mean_error.error || + cm_stats.standard_deviation > + params_->diagnostics.threshold.controller_manager.periodicity.standard_deviation.error) + { + stat.mergeSummary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, diag_summary); + } + else if ( + periodicity_error > + params_->diagnostics.threshold.controller_manager.periodicity.mean_error.warn || + cm_stats.standard_deviation > + params_->diagnostics.threshold.controller_manager.periodicity.standard_deviation.warn) + { + stat.mergeSummary(diagnostic_msgs::msg::DiagnosticStatus::WARN, diag_summary); + } } void ControllerManager::update_list_with_controller_chain( diff --git a/controller_manager/src/controller_manager_parameters.yaml b/controller_manager/src/controller_manager_parameters.yaml new file mode 100644 index 0000000000..1bb9b152b1 --- /dev/null +++ b/controller_manager/src/controller_manager_parameters.yaml @@ -0,0 +1,136 @@ +controller_manager: + update_rate: { + type: int, + default_value: 100, + read_only: true, + description: "The frequency of controller manager's real-time update loop. This loop reads states from hardware, updates controllers and writes commands to hardware." + } + + hardware_components_initial_state: + unconfigured: { + type: string_array, + default_value: [], + description: "Defines which hardware components will be only loaded when controller manager is started. These hardware components will need to be configured and activated manually or via a hardware spawner.", + validation: { + unique<>: null, + } + } + + inactive: { + type: string_array, + default_value: [], + description: "Defines which hardware components will be configured when controller manager is started. These hardware components will need to be activated manually or via a hardware spawner.", + validation: { + unique<>: null, + } + } + + diagnostics: + threshold: + controller_manager: + periodicity: + mean_error: + warn: { + type: double, + default_value: 5.0, + description: "The warning threshold for the mean error of the controller manager's periodicity. If the mean error exceeds this threshold, a warning diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 10.0, + description: "The error threshold for the mean error of the controller manager's periodicity. If the mean error exceeds this threshold, an error diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + standard_deviation: + warn: { + type: double, + default_value: 5.0, + description: "The warning threshold for the standard deviation of the controller manager's periodicity. If the standard deviation exceeds this threshold, a warning diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 10.0, + description: "The error threshold for the standard deviation of the controller manager's periodicity. If the standard deviation exceeds this threshold, an error diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + controllers: + periodicity: + mean_error: + warn: { + type: double, + default_value: 5.0, + description: "The warning threshold for the mean error of the controller update loop. If the mean error exceeds this threshold, a warning diagnostic will be published. This diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 10.0, + description: "The error threshold for the mean error of the controller update loop. If the mean error exceeds this threshold, an error diagnostic will be published. This diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.", + validation: { + gt<>: 0.0, + } + } + standard_deviation: + warn: { + type: double, + default_value: 5.0, + description: "The warning threshold for the standard deviation of the controller update loop. If the standard deviation exceeds this threshold, a warning diagnostic will be published. This diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 10.0, + description: "The error threshold for the standard deviation of the controller update loop. If the standard deviation exceeds this threshold, an error diagnostic will be published. This diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.", + validation: { + gt<>: 0.0, + } + } + execution_time: + mean_error: + warn: { + type: double, + default_value: 1000.0, + description: "The warning threshold for the mean error of the controller's update cycle execution time in microseconds. If the mean error exceeds this threshold, a warning diagnostic will be published. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period cycle to execute it's update cycle", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 2000.0, + description: "The error threshold for the mean error of the controller's update cycle execution time in microseconds. If the mean error exceeds this threshold, an error diagnostic will be published. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period cycle to execute it's update cycle", + validation: { + gt<>: 0.0, + } + } + standard_deviation: + warn: { + type: double, + default_value: 100.0, + description: "The warning threshold for the standard deviation of the controller's update cycle execution time. If the standard deviation exceeds this threshold, a warning diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 200.0, + description: "The error threshold for the standard deviation of the controller's update cycle execution time. If the standard deviation exceeds this threshold, an error diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 5dace36d34..da79de76a3 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -109,7 +109,7 @@ int main(int argc, char ** argv) next_iteration_time{cm_now}; // for calculating the measured period of the loop - rclcpp::Time previous_time = cm->now(); + rclcpp::Time previous_time = cm->now() - period; while (rclcpp::ok()) { diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 0c4e51985f..b715bc23dc 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -366,6 +366,7 @@ TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle) EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); + time_ = cm_->get_clock()->now(); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -373,6 +374,21 @@ TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle) std::this_thread::sleep_for( std::chrono::milliseconds(1000 / (test_controller->get_update_rate()))); EXPECT_EQ(test_controller->internal_counter, 1u); + EXPECT_EQ(test_controller->update_period_.seconds(), 0.0) + << "The first trigger cycle should have zero period"; + + const double exp_period = (cm_->get_clock()->now() - time_).seconds(); + time_ = cm_->get_clock()->now(); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(test_controller->internal_counter, 1u); + std::this_thread::sleep_for( + std::chrono::milliseconds(1000 / (test_controller->get_update_rate()))); + EXPECT_EQ(test_controller->internal_counter, 2u); + EXPECT_THAT( + test_controller->update_period_.seconds(), + testing::AllOf(testing::Gt(0.6 * exp_period), testing::Lt((1.4 * exp_period)))); size_t last_internal_counter = test_controller->internal_counter; // Stop controller, will take effect at the end of the update function @@ -576,7 +592,26 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd // [cm_update_rate, 2*cm_update_rate) EXPECT_THAT( test_controller->update_period_.seconds(), - testing::AllOf(testing::Ge(0.9 / cm_update_rate), testing::Lt((1.1 / cm_update_rate)))); + testing::AllOf(testing::Ge(0.85 / cm_update_rate), testing::Lt((1.15 / cm_update_rate)))); + ASSERT_EQ( + test_controller->internal_counter, + cm_->get_loaded_controllers()[0].execution_time_statistics->GetCount()); + ASSERT_EQ( + test_controller->internal_counter - 1, + cm_->get_loaded_controllers()[0].periodicity_statistics->GetCount()) + << "The first update is not counted in periodicity statistics"; + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Average(), + testing::AllOf( + testing::Ge(0.95 * cm_->get_update_rate()), testing::Lt((1.05 * cm_->get_update_rate())))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Min(), + testing::AllOf( + testing::Ge(0.85 * cm_->get_update_rate()), testing::Lt((1.2 * cm_->get_update_rate())))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Max(), + testing::AllOf( + testing::Ge(0.85 * cm_->get_update_rate()), testing::Lt((1.2 * cm_->get_update_rate())))); loop_rate.sleep(); } // if we do 2 times of the controller_manager update rate, the internal counter should be @@ -683,6 +718,8 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) const auto initial_counter = test_controller->internal_counter; // don't start with zero to check if the period is correct if controller is activated anytime rclcpp::Time time = time_; + const auto exp_periodicity = + cm_update_rate / std::ceil(static_cast(cm_update_rate) / controller_update_rate); for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter) { rclcpp::Time old_time = time; @@ -691,19 +728,36 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time, rclcpp::Duration::from_seconds(0.01))); - // In case of a non perfect divisor, the update period should respect the rule - // [controller_update_rate, 2*controller_update_rate) - EXPECT_THAT( - test_controller->update_period_.seconds(), - testing::AllOf( - testing::Gt(0.99 * controller_period), - testing::Lt((1.05 * controller_period) + PERIOD.seconds()))) - << "update_counter: " << update_counter << " desired controller period: " << controller_period - << " actual controller period: " << test_controller->update_period_.seconds(); - if (update_counter % cm_update_rate == 0) + if (test_controller->internal_counter - initial_counter > 0) + { + // In case of a non perfect divisor, the update period should respect the rule + // [controller_update_rate, 2*controller_update_rate) + EXPECT_THAT( + test_controller->update_period_.seconds(), + testing::AllOf( + testing::Gt(0.99 * controller_period), + testing::Lt((1.05 * controller_period) + PERIOD.seconds()))) + << "update_counter: " << update_counter + << " desired controller period: " << controller_period + << " actual controller period: " << test_controller->update_period_.seconds(); + } + else + { + // Check that the first cycle update period is zero + EXPECT_EQ(test_controller->update_period_.seconds(), 0.0); + } + + if (update_counter > 0 && update_counter % cm_update_rate == 0) { const double no_of_secs_passed = static_cast(update_counter) / cm_update_rate; + const auto actual_counter = test_controller->internal_counter - initial_counter; + const unsigned int exp_counter = + static_cast(exp_periodicity * no_of_secs_passed); + SCOPED_TRACE( + "The internal counter is : " + std::to_string(actual_counter) + " [" + + std::to_string(exp_counter - 1) + ", " + std::to_string(exp_counter + 1) + + "] and number of seconds passed : " + std::to_string(no_of_secs_passed)); // NOTE: here EXPECT_NEAR is used because it is observed that in the first iteration of whole // cycle of cm_update_rate counts, there is one count missing, but in rest of the 9 cycles it // is clearly tracking, so adding 1 here won't affect the final count. @@ -711,10 +765,26 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) // cycle and then on accumulating 37 on every other update cycle so at the end of the 10 // cycles it will have 369 instead of 370. EXPECT_THAT( - test_controller->internal_counter - initial_counter, - testing::AnyOf( - testing::Ge((controller_update_rate - 1) * no_of_secs_passed), - testing::Lt((controller_update_rate * no_of_secs_passed)))); + actual_counter, testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter + 1))); + ASSERT_EQ( + test_controller->internal_counter, + cm_->get_loaded_controllers()[0].execution_time_statistics->GetCount()); + ASSERT_EQ( + test_controller->internal_counter - 1, + cm_->get_loaded_controllers()[0].periodicity_statistics->GetCount()) + << "The first update is not counted in periodicity statistics"; + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Average(), + testing::AllOf(testing::Ge(0.95 * exp_periodicity), testing::Lt((1.05 * exp_periodicity)))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Min(), + testing::AllOf(testing::Ge(0.85 * exp_periodicity), testing::Lt((1.2 * exp_periodicity)))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Max(), + testing::AllOf(testing::Ge(0.85 * exp_periodicity), testing::Lt((1.2 * exp_periodicity)))); + EXPECT_LT( + cm_->get_loaded_controllers()[0].execution_time_statistics->Average(), + 50.0); // 50 microseconds } } } @@ -723,6 +793,148 @@ INSTANTIATE_TEST_SUITE_P( per_controller_update_rate_check, TestControllerUpdateRates, testing::Values(10, 12, 16, 23, 37, 40, 50, 63, 71, 85, 90)); +class TestAsyncControllerUpdateRates +: public ControllerManagerFixture +{ +}; + +TEST_F(TestAsyncControllerUpdateRates, check_the_async_controller_update_rate_and_stats) +{ + const unsigned int ctrl_update_rate = cm_->get_update_rate() / 2; + auto test_controller = std::make_shared(); + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller.use_count()); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); + + test_controller->get_node()->set_parameter({"update_rate", static_cast(ctrl_update_rate)}); + test_controller->get_node()->set_parameter({"is_async", true}); + // configure controller + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + } + ASSERT_TRUE(test_controller->is_async()); + time_ = test_controller->get_node()->now(); // set to something nonzero + cm_->get_clock()->sleep_until(time_ + PERIOD); + time_ = cm_->get_clock()->now(); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + std::vector start_controllers = {test_controller::TEST_CONTROLLER_NAME}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + + cm_->get_clock()->sleep_until(time_ + PERIOD); + time_ = cm_->get_clock()->now(); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update"; + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); + + EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate); + const auto cm_update_rate = cm_->get_update_rate(); + const auto controller_update_rate = test_controller->get_update_rate(); + const double controller_period = 1.0 / controller_update_rate; + + const auto initial_counter = test_controller->internal_counter; + // don't start with zero to check if the period is correct if controller is activated anytime + rclcpp::Time time = time_; + const auto exp_periodicity = + cm_update_rate / std::ceil(static_cast(cm_update_rate) / controller_update_rate); + for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter) + { + rclcpp::Time old_time = time; + cm_->get_clock()->sleep_until(old_time + PERIOD); + time = cm_->get_clock()->now(); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time, rclcpp::Duration::from_seconds(0.01))); + + // the async controllers will have to wait for one cycle to have the correct update period in + // the controller + if (test_controller->internal_counter - initial_counter > 1) + { + EXPECT_THAT( + test_controller->update_period_.seconds(), + testing::AllOf( + testing::Gt(0.99 * controller_period), + testing::Lt((1.05 * controller_period) + PERIOD.seconds()))) + << "update_counter: " << update_counter + << " desired controller period: " << controller_period + << " actual controller period: " << test_controller->update_period_.seconds(); + } + // else + // { + // // Check that the first cycle update period is zero + // EXPECT_EQ(test_controller->update_period_.seconds(), 0.0); + // } + + if (update_counter > 0 && update_counter % cm_update_rate == 0) + { + const double no_of_secs_passed = static_cast(update_counter) / cm_update_rate; + const auto actual_counter = test_controller->internal_counter - initial_counter; + const unsigned int exp_counter = + static_cast(exp_periodicity * no_of_secs_passed); + SCOPED_TRACE( + "The internal counter is : " + std::to_string(actual_counter) + " [" + + std::to_string(exp_counter - 1) + ", " + std::to_string(exp_counter + 1) + + "] and number of seconds passed : " + std::to_string(no_of_secs_passed)); + // NOTE: here EXPECT_THAT is used because it is observed that in the first iteration of whole + // cycle of cm_update_rate counts, there is one count missing, but in rest of the 9 cycles it + // is clearly tracking, so adding 1 here won't affect the final count. + // For instance, a controller with update rate 37 Hz, seems to have 36 in the first update + // cycle and then on accumulating 37 on every other update cycle so at the end of the 10 + // cycles it will have 369 instead of 370. + EXPECT_THAT( + actual_counter, testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter + 1))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].execution_time_statistics->GetCount(), + testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->GetCount(), + testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Average(), + testing::AllOf(testing::Ge(0.95 * exp_periodicity), testing::Lt((1.05 * exp_periodicity)))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Min(), + testing::AllOf(testing::Ge(0.85 * exp_periodicity), testing::Lt((1.2 * exp_periodicity)))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Max(), + testing::AllOf(testing::Ge(0.85 * exp_periodicity), testing::Lt((1.2 * exp_periodicity)))); + EXPECT_LT( + cm_->get_loaded_controllers()[0].execution_time_statistics->Average(), + 12000); // more or less 12 milliseconds considering the waittime in the controller + } + } +} + class TestControllerManagerFallbackControllers : public ControllerManagerFixture, public testing::WithParamInterface diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index c7b67e0cfe..bdd48f15ae 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -65,6 +65,7 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs cm_ = std::make_shared(executor_, TEST_CM_NAME); run_updater_ = false; + SetUpSrvsCMExecutor(); cm_->set_parameter(rclcpp::Parameter( "hardware_components_initial_state.unconfigured", std::vector({TEST_SYSTEM_HARDWARE_NAME}))); @@ -72,11 +73,10 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs "hardware_components_initial_state.inactive", std::vector({TEST_SENSOR_HARDWARE_NAME}))); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); auto msg = std_msgs::msg::String(); msg.data = ros2_control_test_assets::minimal_robot_urdf; cm_->robot_description_callback(msg); - - SetUpSrvsCMExecutor(); } void check_component_fileds(