From d55def1923fe15b3b7a6e6dd3ffb3a90cfe67997 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 17 Oct 2024 17:03:10 +0200 Subject: [PATCH 1/2] [CM] Async Function Handler for Controllers (#1489) --------- Co-authored-by: Jordan Palacios Co-authored-by: atzaros <128592691+atzaros@users.noreply.github.com> Co-authored-by: Bence Magyar Co-authored-by: Dr. Denis --- controller_interface/CMakeLists.txt | 1 + .../controller_interface/async_controller.hpp | 112 ----------- .../controller_interface_base.hpp | 45 +++++ controller_interface/package.xml | 4 + .../src/controller_interface_base.cpp | 56 +++++- .../controller_manager/controller_manager.hpp | 4 - controller_manager/src/controller_manager.cpp | 51 ++--- .../test/test_controller/test_controller.cpp | 4 + .../test/test_controller_manager.cpp | 185 ++++++++++++++++++ 9 files changed, 320 insertions(+), 142 deletions(-) delete mode 100644 controller_interface/include/controller_interface/async_controller.hpp diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index cad5810ee5..85294c68d1 100644 --- a/controller_interface/CMakeLists.txt +++ b/controller_interface/CMakeLists.txt @@ -9,6 +9,7 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS hardware_interface rclcpp_lifecycle + realtime_tools ) find_package(ament_cmake REQUIRED) diff --git a/controller_interface/include/controller_interface/async_controller.hpp b/controller_interface/include/controller_interface/async_controller.hpp deleted file mode 100644 index 357b3a2ce3..0000000000 --- a/controller_interface/include/controller_interface/async_controller.hpp +++ /dev/null @@ -1,112 +0,0 @@ -// Copyright 2024 ros2_control development team -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef CONTROLLER_INTERFACE__ASYNC_CONTROLLER_HPP_ -#define CONTROLLER_INTERFACE__ASYNC_CONTROLLER_HPP_ - -#include -#include -#include - -#include "controller_interface_base.hpp" -#include "lifecycle_msgs/msg/state.hpp" - -namespace controller_interface -{ - -class AsyncControllerThread -{ -public: - /// Constructor for the AsyncControllerThread object. - /** - * - * \param[in] controller shared pointer to a controller. - * \param[in] cm_update_rate the controller manager's update rate. - */ - AsyncControllerThread( - std::shared_ptr & controller, int cm_update_rate) - : terminated_(false), controller_(controller), thread_{}, cm_update_rate_(cm_update_rate) - { - } - - AsyncControllerThread(const AsyncControllerThread & t) = delete; - AsyncControllerThread(AsyncControllerThread && t) = delete; - - // Destructor, called when the component is erased from its map. - ~AsyncControllerThread() - { - terminated_.store(true, std::memory_order_seq_cst); - if (thread_.joinable()) - { - thread_.join(); - } - } - - /// Creates the controller's thread. - /** - * Called when the controller is activated. - * - */ - void activate() - { - thread_ = std::thread(&AsyncControllerThread::controller_update_callback, this); - } - - /// Periodically execute the controller's update method. - /** - * Callback of the async controller's thread. - * **Not synchronized with the controller manager's write and read currently** - * - */ - void controller_update_callback() - { - using TimePoint = std::chrono::system_clock::time_point; - unsigned int used_update_rate = - controller_->get_update_rate() == 0 ? cm_update_rate_ : controller_->get_update_rate(); - - auto previous_time = controller_->get_node()->now(); - while (!terminated_.load(std::memory_order_relaxed)) - { - auto const period = std::chrono::nanoseconds(1'000'000'000 / used_update_rate); - TimePoint next_iteration_time = - TimePoint(std::chrono::nanoseconds(controller_->get_node()->now().nanoseconds())); - - if ( - controller_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - auto const current_time = controller_->get_node()->now(); - auto const measured_period = current_time - previous_time; - previous_time = current_time; - controller_->update( - controller_->get_node()->now(), - (controller_->get_update_rate() != cm_update_rate_ && controller_->get_update_rate() != 0) - ? rclcpp::Duration::from_seconds(1.0 / controller_->get_update_rate()) - : measured_period); - } - - next_iteration_time += period; - std::this_thread::sleep_until(next_iteration_time); - } - } - -private: - std::atomic terminated_; - std::shared_ptr controller_; - std::thread thread_; - unsigned int cm_update_rate_; -}; - -} // namespace controller_interface - -#endif // CONTROLLER_INTERFACE__ASYNC_CONTROLLER_HPP_ diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 2bd01cc326..211716f301 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -17,9 +17,11 @@ #include #include +#include #include #include "controller_interface/visibility_control.h" +#include "realtime_tools/async_function_handler.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/loaned_command_interface.hpp" @@ -58,6 +60,17 @@ struct InterfaceConfiguration std::vector names = {}; }; +struct ControllerUpdateStats +{ + void reset() + { + total_triggers = 0; + failed_triggers = 0; + } + + unsigned int total_triggers; + unsigned int failed_triggers; +}; /** * Base interface class for an controller. The interface may not be used to implement a controller. * The class provides definitions for `ControllerInterface` and `ChainableControllerInterface` @@ -153,6 +166,23 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC virtual return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) = 0; + /** + * Trigger update method. This method is used by the controller_manager to trigger the update + * method of the controller. + * The method is used to trigger the update method of the controller synchronously or + * asynchronously, based on the controller configuration. + * **The method called in the (real-time) control loop.** + * + * \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. + */ + CONTROLLER_INTERFACE_PUBLIC + std::pair trigger_update( + const rclcpp::Time & time, const rclcpp::Duration & period); + CONTROLLER_INTERFACE_PUBLIC std::shared_ptr get_node(); @@ -270,15 +300,30 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC virtual bool is_in_chained_mode() const = 0; + /** + * Method to wait for any running async update cycle to finish after finishing the current cycle. + * This is needed to be called before deactivating the controller by the controller_manager, so + * that the interfaces still exist when the controller finishes its cycle and then it's exits. + * + * \note **The method is not real-time safe and shouldn't be called in the control loop.** + * + * If the controller is running in async mode, the method will wait for the current async update + * to finish. If the controller is not running in async mode, the method will do nothing. + */ + CONTROLLER_INTERFACE_PUBLIC + void wait_for_trigger_update_to_finish(); + protected: std::vector command_interfaces_; std::vector state_interfaces_; private: std::shared_ptr node_; + std::unique_ptr> async_handler_; unsigned int update_rate_ = 0; bool is_async_ = false; std::string urdf_ = ""; + ControllerUpdateStats trigger_stats_; }; using ControllerInterfaceBaseSharedPtr = std::shared_ptr; diff --git a/controller_interface/package.xml b/controller_interface/package.xml index dce1d79f86..09955c46c9 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -12,12 +12,16 @@ ament_cmake_gen_version_h hardware_interface + realtime_tools rclcpp_lifecycle sensor_msgs hardware_interface + realtime_tools rclcpp_lifecycle + realtime_tools + ament_cmake_gmock geometry_msgs sensor_msgs diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index e2c1fa480a..a6e0da988f 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -16,7 +16,6 @@ #include #include -#include #include #include "lifecycle_msgs/msg/state.hpp" @@ -37,6 +36,7 @@ return_type ControllerInterfaceBase::init( { auto_declare("update_rate", update_rate_); auto_declare("is_async", false); + auto_declare("thread_priority", 50); } catch (const std::exception & e) { @@ -57,7 +57,14 @@ return_type ControllerInterfaceBase::init( std::bind(&ControllerInterfaceBase::on_configure, this, std::placeholders::_1)); node_->register_on_cleanup( - std::bind(&ControllerInterfaceBase::on_cleanup, this, std::placeholders::_1)); + [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn + { + if (is_async() && async_handler_ && async_handler_->is_running()) + { + async_handler_->stop_thread(); + } + return on_cleanup(previous_state); + }); node_->register_on_activate( std::bind(&ControllerInterfaceBase::on_activate, this, std::placeholders::_1)); @@ -106,6 +113,21 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() } is_async_ = get_node()->get_parameter("is_async").as_bool(); } + if (is_async_) + { + const unsigned int thread_priority = + static_cast(get_node()->get_parameter("thread_priority").as_int()); + RCLCPP_INFO( + get_node()->get_logger(), "Starting async handler with scheduler priority: %d", + thread_priority); + async_handler_ = std::make_unique>(); + async_handler_->init( + std::bind( + &ControllerInterfaceBase::update, this, std::placeholders::_1, std::placeholders::_2), + thread_priority); + async_handler_->start_thread(); + } + trigger_stats_.reset(); return get_node()->configure(); } @@ -129,6 +151,29 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() c return node_->get_current_state(); } +std::pair ControllerInterfaceBase::trigger_update( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + trigger_stats_.total_triggers++; + if (is_async()) + { + const auto result = async_handler_->trigger_async_callback(time, period); + if (!result.first) + { + trigger_stats_.failed_triggers++; + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 20000, + "The controller missed %u update cycles out of %u total triggers.", + trigger_stats_.failed_triggers, trigger_stats_.total_triggers); + } + return result; + } + else + { + return std::make_pair(true, update(time, period)); + } +} + std::shared_ptr ControllerInterfaceBase::get_node() { if (!node_.get()) @@ -153,4 +198,11 @@ bool ControllerInterfaceBase::is_async() const { return is_async_; } const std::string & ControllerInterfaceBase::get_robot_description() const { return urdf_; } +void ControllerInterfaceBase::wait_for_trigger_update_to_finish() +{ + if (is_async() && async_handler_ && async_handler_->is_running()) + { + async_handler_->wait_for_trigger_cycle_to_finish(); + } +} } // namespace controller_interface diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 273b48b022..6c0b4fde9b 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -22,7 +22,6 @@ #include #include -#include "controller_interface/async_controller.hpp" #include "controller_interface/chainable_controller_interface.hpp" #include "controller_interface/controller_interface.hpp" #include "controller_interface/controller_interface_base.hpp" @@ -628,9 +627,6 @@ class ControllerManager : public rclcpp::Node }; SwitchParams switch_params_; - - std::unordered_map> - async_controller_threads_; }; } // namespace controller_manager diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 72cebaa1e5..a8668f7f1b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -649,13 +649,6 @@ controller_interface::return_type ControllerManager::unload_controller( controller_name.c_str()); return controller_interface::return_type::ERROR; } - if (controller.c->is_async()) - { - RCLCPP_DEBUG( - get_logger(), "Removing controller '%s' from the list of async controllers", - controller_name.c_str()); - async_controller_threads_.erase(controller_name); - } RCLCPP_DEBUG(get_logger(), "Cleanup controller"); controller_chain_spec_cleanup(controller_chain_spec_, controller_name); @@ -798,14 +791,6 @@ controller_interface::return_type ControllerManager::configure_controller( return controller_interface::return_type::ERROR; } - // ASYNCHRONOUS CONTROLLERS: Start background thread for update - if (controller->is_async()) - { - async_controller_threads_.emplace( - controller_name, - std::make_unique(controller, update_rate_)); - } - const auto controller_update_rate = controller->get_update_rate(); const auto cm_update_rate = get_update_rate(); if (controller_update_rate > cm_update_rate) @@ -1393,6 +1378,18 @@ controller_interface::return_type ControllerManager::switch_controller( RCLCPP_DEBUG(get_logger(), " - %s", interface.c_str()); } + // wait for deactivating async controllers to finish their current cycle + for (const auto & controller : deactivate_request_) + { + auto controller_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller)); + if (controller_it != controllers.end()) + { + controller_it->c->wait_for_trigger_update_to_finish(); + } + } + if ( !activate_command_interface_request_.empty() || !deactivate_command_interface_request_.empty()) { @@ -1798,11 +1795,6 @@ void ControllerManager::activate_controllers( resource_manager_->make_controller_exported_state_interfaces_available(controller_name); resource_manager_->make_controller_reference_interfaces_available(controller_name); } - - if (controller->is_async()) - { - async_controller_threads_.at(controller_name)->activate(); - } } } @@ -2343,8 +2335,19 @@ controller_interface::return_type ControllerManager::update( { // TODO(v-lopez) we could cache this information // https://github.com/ros-controls/ros2_control/issues/153 - if (!loaded_controller.c->is_async() && is_controller_active(*loaded_controller.c)) + if (is_controller_active(*loaded_controller.c)) { + if ( + switch_params_.do_switch && loaded_controller.c->is_async() && + std::find( + deactivate_request_.begin(), deactivate_request_.end(), loaded_controller.info.name) != + deactivate_request_.end()) + { + RCLCPP_DEBUG( + get_logger(), "Skipping update for async controller '%s' as it is being deactivated", + loaded_controller.info.name.c_str()); + continue; + } const auto controller_update_rate = loaded_controller.c->get_update_rate(); const bool run_controller_at_cm_rate = (controller_update_rate >= update_rate_); const auto controller_period = @@ -2377,10 +2380,12 @@ controller_interface::return_type ControllerManager::update( const auto controller_actual_period = (time - *loaded_controller.next_update_cycle_time) + controller_period; auto controller_ret = controller_interface::return_type::OK; + bool trigger_status = true; // Catch exceptions thrown by the controller update function try { - controller_ret = loaded_controller.c->update(time, controller_actual_period); + std::tie(trigger_status, controller_ret) = + loaded_controller.c->trigger_update(time, controller_actual_period); } catch (const std::exception & e) { @@ -2601,8 +2606,6 @@ unsigned int ControllerManager::get_update_rate() const { return update_rate_; } void ControllerManager::shutdown_async_controllers_and_components() { - async_controller_threads_.erase( - async_controller_threads_.begin(), async_controller_threads_.end()); resource_manager_->shutdown_async_components(); } diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index ac89239e09..04ae8c02c2 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -61,6 +61,10 @@ controller_interface::InterfaceConfiguration TestController::state_interface_con controller_interface::return_type TestController::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { + if (is_async()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1000 / (2 * get_update_rate()))); + } update_period_ = period; ++internal_counter; diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index a016b20440..8ff844adc2 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -233,6 +233,191 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ(1, test_controller.use_count()); } +TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle) +{ + const auto test_param = GetParam(); + auto test_controller = std::make_shared(); + auto test_controller2 = std::make_shared(); + constexpr char TEST_CONTROLLER2_NAME[] = "test_controller2_name"; + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller2, TEST_CONTROLLER2_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller.use_count()); + + // setup interface to claim from controllers + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + cmd_itfs_cfg.names.push_back(interface); + } + test_controller->set_command_interface_configuration(cmd_itfs_cfg); + + controller_interface::InterfaceConfiguration state_itfs_cfg; + state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + test_controller->set_state_interface_configuration(state_itfs_cfg); + + controller_interface::InterfaceConfiguration cmd_itfs_cfg2; + cmd_itfs_cfg2.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES) + { + cmd_itfs_cfg2.names.push_back(interface); + } + test_controller2->set_command_interface_configuration(cmd_itfs_cfg2); + + controller_interface::InterfaceConfiguration state_itfs_cfg2; + state_itfs_cfg2.type = controller_interface::interface_configuration_type::ALL; + test_controller2->set_state_interface_configuration(state_itfs_cfg2); + + // Check if namespace is set correctly + RCLCPP_INFO( + rclcpp::get_logger("test_controller_manager"), "Controller Manager namespace is '%s'", + cm_->get_namespace()); + EXPECT_STREQ(cm_->get_namespace(), "/"); + RCLCPP_INFO( + rclcpp::get_logger("test_controller_manager"), "Controller 1 namespace is '%s'", + test_controller->get_node()->get_namespace()); + EXPECT_STREQ(test_controller->get_node()->get_namespace(), "/"); + RCLCPP_INFO( + rclcpp::get_logger("test_controller_manager"), "Controller 2 namespace is '%s'", + test_controller2->get_node()->get_namespace()); + EXPECT_STREQ(test_controller2->get_node()->get_namespace(), "/"); + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller->internal_counter) + << "Update should not reach an unconfigured controller"; + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); + + // configure controller + rclcpp::Parameter update_rate_parameter("update_rate", static_cast(20)); + rclcpp::Parameter is_async_parameter("is_async", rclcpp::ParameterValue(true)); + test_controller->get_node()->set_parameter(update_rate_parameter); + test_controller->get_node()->set_parameter(is_async_parameter); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + cm_->configure_controller(TEST_CONTROLLER2_NAME); + } + 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(0u, test_controller2->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 + std::vector start_controllers = {"fake_controller", TEST_CONTROLLER2_NAME}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration(0, 0)); + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is started at the end of update"; + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(test_param.expected_return, switch_future.get()); + } + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_GE(test_controller2->internal_counter, test_param.expected_counter); + + // Start the real test controller, will take effect at the end of the update function + start_controllers = {test_controller::TEST_CONTROLLER_NAME}; + stop_controllers = {}; + switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, test_param.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"; + + 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( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(test_controller->internal_counter, 0u); + std::this_thread::sleep_for( + std::chrono::milliseconds(1000 / (test_controller->get_update_rate()))); + EXPECT_EQ(test_controller->internal_counter, 1u); + size_t last_internal_counter = test_controller->internal_counter; + + // Stop controller, will take effect at the end of the update function + start_controllers = {}; + stop_controllers = {test_controller::TEST_CONTROLLER_NAME}; + switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, test_param.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"; + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(last_internal_counter, test_controller->internal_counter) + << "This shouldn't have updated as this is async and in the controller it is waiting before " + "updating the counter"; + std::this_thread::sleep_for( + std::chrono::milliseconds(1000 / (test_controller->get_update_rate()))); + EXPECT_EQ(last_internal_counter, test_controller->internal_counter) + << "Controller is stopped at the end of update, so it should have done one more update"; + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); + auto unload_future = std::async( + std::launch::async, &controller_manager::ControllerManager::unload_controller, cm_, + test_controller::TEST_CONTROLLER_NAME); + + ASSERT_EQ(std::future_status::timeout, unload_future.wait_for(std::chrono::milliseconds(100))) + << "unload_controller should be blocking until next update cycle"; + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, unload_future.get()); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); + EXPECT_EQ(1, test_controller.use_count()); +} + TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) { auto strictness = GetParam().strictness; From cb91599f8f66aaf39b7485a2f7e131157f633474 Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Fri, 25 Oct 2024 15:18:23 +0200 Subject: [PATCH 2/2] Fix timeout value in std output (#1807) We were using the wrong timeout in the terminal output there. --- .../controller_manager/controller_manager_services.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 102531f8e8..909e681ce6 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -114,7 +114,7 @@ def service_caller( if future.result() is None: node.get_logger().warning( f"Failed getting a result from calling {fully_qualified_service_name} in " - f"{service_timeout}. (Attempt {attempt+1} of {max_attempts}.)" + f"{call_timeout}. (Attempt {attempt+1} of {max_attempts}.)" ) else: return future.result()