From a1ad5237c1ee0df7e82e9339b8fb566f265c19be Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 9 Aug 2024 14:03:12 +0200 Subject: [PATCH] refactor SwitchParams to fix the incosistencies in the spawner tests (#1638) --- .../controller_manager/controller_manager.hpp | 23 +++++++++--- controller_manager/src/controller_manager.cpp | 37 ++++++++++++++----- .../test/test_controller_manager_srvs.cpp | 31 +++++++++++++++- 3 files changed, 74 insertions(+), 17 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 2f326ff302..810936554e 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -583,14 +583,25 @@ class ControllerManager : public rclcpp::Node struct SwitchParams { - bool do_switch = {false}; - bool started = {false}; - rclcpp::Time init_time = {rclcpp::Time::max()}; + void reset() + { + do_switch = false; + started = false; + strictness = 0; + activate_asap = false; + } + + bool do_switch; + bool started; // Switch options - int strictness = {0}; - bool activate_asap = {false}; - rclcpp::Duration timeout = rclcpp::Duration{0, 0}; + int strictness; + bool activate_asap; + std::chrono::nanoseconds timeout; + + // conditional variable and mutex to wait for the switch to complete + std::condition_variable cv; + std::mutex mutex; }; SwitchParams switch_params_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 87414ce4ec..25add2045e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -849,6 +849,7 @@ controller_interface::return_type ControllerManager::configure_controller( void ControllerManager::clear_requests() { + switch_params_.do_switch = false; deactivate_request_.clear(); activate_request_.clear(); // Set these interfaces as unavailable when clearing requests to avoid leaving them in available @@ -878,7 +879,8 @@ controller_interface::return_type ControllerManager::switch_controller( return controller_interface::return_type::ERROR; } - switch_params_ = SwitchParams(); + // reset the switch param internal variables + switch_params_.reset(); if (!deactivate_request_.empty() || !activate_request_.empty()) { @@ -1299,19 +1301,27 @@ controller_interface::return_type ControllerManager::switch_controller( // start the atomic controller switching switch_params_.strictness = strictness; switch_params_.activate_asap = activate_asap; - switch_params_.init_time = rclcpp::Clock().now(); - switch_params_.timeout = timeout; + if (timeout == rclcpp::Duration{0, 0}) + { + RCLCPP_INFO_ONCE(get_logger(), "Switch controller timeout is set to 0, using default 1s!"); + switch_params_.timeout = std::chrono::nanoseconds(1'000'000'000); + } + else + { + switch_params_.timeout = timeout.to_chrono(); + } switch_params_.do_switch = true; - // wait until switch is finished RCLCPP_DEBUG(get_logger(), "Requested atomic controller switch from realtime loop"); - while (rclcpp::ok() && switch_params_.do_switch) + std::unique_lock switch_params_guard(switch_params_.mutex, std::defer_lock); + if (!switch_params_.cv.wait_for( + switch_params_guard, switch_params_.timeout, [this] { return !switch_params_.do_switch; })) { - if (!rclcpp::ok()) - { - return controller_interface::return_type::ERROR; - } - std::this_thread::sleep_for(std::chrono::microseconds(100)); + RCLCPP_ERROR( + get_logger(), "Switch controller timed out after %f seconds!", + static_cast(switch_params_.timeout.count()) / 1e9); + clear_requests(); + return controller_interface::return_type::ERROR; } // copy the controllers spec from the used to the unused list @@ -2147,6 +2157,12 @@ void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & void ControllerManager::manage_switch() { + std::unique_lock guard(switch_params_.mutex, std::try_to_lock); + if (!guard.owns_lock()) + { + RCLCPP_DEBUG(get_logger(), "Unable to lock switch mutex. Retrying in next cycle."); + return; + } // Ask hardware interfaces to change mode if (!resource_manager_->perform_command_mode_switch( activate_command_interface_request_, deactivate_command_interface_request_)) @@ -2175,6 +2191,7 @@ void ControllerManager::manage_switch() // All controllers switched --> switching done switch_params_.do_switch = false; + switch_params_.cv.notify_all(); } controller_interface::return_type ControllerManager::update( diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index af04cc5406..e53ad10578 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -168,9 +168,38 @@ TEST_F(TestControllerManagerSrvs, list_controllers_srv) "joint1/velocity", "joint2/acceleration", "joint2/position", "joint2/velocity", "joint3/acceleration", "joint3/position", "joint3/velocity", "sensor1/velocity")); + // Switch with a very low timeout 1 ns and it should fail as there is no enough time to switch + ASSERT_EQ( + controller_interface::return_type::ERROR, + cm_->switch_controller( + {}, {test_controller::TEST_CONTROLLER_NAME}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 1))); + + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ(1u, result->controller.size()); + ASSERT_EQ("active", result->controller[0].state); + ASSERT_THAT( + result->controller[0].claimed_interfaces, + UnorderedElementsAre( + "joint2/velocity", "joint3/velocity", "joint2/max_acceleration", "configuration/max_tcp_jerk", + "joint1/position", "joint1/max_velocity")); + ASSERT_THAT( + result->controller[0].required_command_interfaces, + UnorderedElementsAre( + "configuration/max_tcp_jerk", "joint1/max_velocity", "joint1/position", + "joint2/max_acceleration", "joint2/velocity", "joint3/velocity")); + ASSERT_THAT( + result->controller[0].required_state_interfaces, + UnorderedElementsAre( + "configuration/max_tcp_jerk", "joint1/position", "joint1/some_unlisted_interface", + "joint1/velocity", "joint2/acceleration", "joint2/position", "joint2/velocity", + "joint3/acceleration", "joint3/position", "joint3/velocity", "sensor1/velocity")); + + // Try again with higher timeout cm_->switch_controller( {}, {test_controller::TEST_CONTROLLER_NAME}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(3, 0)); result = call_service_and_wait(*client, request, srv_executor); ASSERT_EQ(1u, result->controller.size());