diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index c3c0de7739..957ad0d33a 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -72,6 +72,17 @@ if(BUILD_TESTING) DESTINATION lib ) + add_library(test_controller_with_command SHARED + test/test_controller_with_command/test_controller_with_command.cpp) + target_link_libraries(test_controller_with_command PUBLIC controller_manager) + target_compile_definitions(test_controller_with_command PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") + pluginlib_export_plugin_description_file( + controller_interface test/test_controller_with_command/test_controller_with_command.xml) + install( + TARGETS test_controller_with_command + DESTINATION lib + ) + add_library(test_chainable_controller SHARED test/test_chainable_controller/test_chainable_controller.cpp ) @@ -123,6 +134,16 @@ if(BUILD_TESTING) ros2_control_test_assets::ros2_control_test_assets ) + ament_add_gmock(test_restart_controller + test/test_restart_controller.cpp + APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$ + ) + target_link_libraries(test_restart_controller + controller_manager + test_controller_with_command + ros2_control_test_assets::ros2_control_test_assets + ) + ament_add_gmock(test_controllers_chaining_with_controller_manager test/test_controllers_chaining_with_controller_manager.cpp ) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 845fa2dee0..6144a81701 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -329,6 +329,13 @@ class ControllerManager : public rclcpp::Node std::unique_ptr resource_manager_; private: + enum class CheckDeActivateRequestResult + { + OK, + ERROR, + RETRY + }; + std::vector get_controller_names(); std::pair split_command_interface( const std::string & command_interface); @@ -339,6 +346,7 @@ class ControllerManager : public rclcpp::Node * and "control loop" threads. */ void clear_requests(); + void clear_chained_mode_requests(); /** * If a controller is deactivated all following controllers (if any exist) should be switched @@ -377,6 +385,23 @@ class ControllerManager : public rclcpp::Node const std::vector & controllers, int strictness, const ControllersListIterator controller_it); + /** + * Check if all activate requests are valid. + * Perform a detailed check internally by calling check_following_controllers_for_activate. + * + * \param[in] controllers list with controllers. + * \param[in] strictness if value is equal "MANIPULATE_CONTROLLERS_CHAIN" then all following + * controllers will be automatically added to the activate request list if they are not in the + * deactivate request. + * + * \returns If all activate requests pass the check, return CheckDeActivateRequestResult::OK. If + * an error occurs during the check, the processing will differ based on the value of strictness: + * if BEST_EFFORT, erase the target controller from the activate_request and return + * CheckDeActivateRequestResult::RETRY; if STRICT, return CheckDeActivateRequestResult::ERROR. + */ + CheckDeActivateRequestResult check_activate_requests( + const std::vector & controllers, int strictness); + /// Check if all the preceding controllers will be in inactive state after controllers' switch. /** * Check that all preceding controllers of the @controller_it @@ -396,10 +421,27 @@ class ControllerManager : public rclcpp::Node * \returns return_type::OK if all preceding controllers pass the checks, otherwise * return_type::ERROR. */ - controller_interface::return_type check_preceeding_controllers_for_deactivate( + controller_interface::return_type check_preceding_controllers_for_deactivate( const std::vector & controllers, int strictness, const ControllersListIterator controller_it); + /** + * Check if all deactivate requests are valid. + * Perform a detailed check internally by calling check_preceding_controllers_for_deactivate. + * + * \param[in] controllers list with controllers. + * \param[in] strictness if value is equal "MANIPULATE_CONTROLLERS_CHAIN" then all following + * controllers will be automatically added to the activate request list if they are not in the + * deactivate request. + * + * \returns If all deactivate requests pass the check, return CheckDeActivateRequestResult::OK. If + * an error occurs during the check, the processing will differ based on the value of strictness: + * if BEST_EFFORT, erase the target controller from the deactivate_request and return + * CheckDeActivateRequestResult::RETRY; if STRICT, return CheckDeActivateRequestResult::ERROR. + */ + CheckDeActivateRequestResult check_deactivate_requests( + const std::vector & controllers, int strictness); + /** * @brief Inserts a controller into an ordered list based on dependencies to compute the * controller chain. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 3b84fc07e4..1fc883430b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -831,16 +831,21 @@ void ControllerManager::clear_requests() { deactivate_request_.clear(); activate_request_.clear(); - // Set these interfaces as unavailable when clearing requests to avoid leaving them in available - // state without the controller being in active state + clear_chained_mode_requests(); + activate_command_interface_request_.clear(); + deactivate_command_interface_request_.clear(); +} + +void ControllerManager::clear_chained_mode_requests() +{ + // Set these interfaces as unavailable when clearing requests to avoid leaving them in + // available state without the controller being in active state for (const auto & controller_name : to_chained_mode_request_) { resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); } to_chained_mode_request_.clear(); from_chained_mode_request_.clear(); - activate_command_interface_request_.clear(); - deactivate_command_interface_request_.clear(); } controller_interface::return_type ControllerManager::switch_controller( @@ -963,105 +968,48 @@ controller_interface::return_type ControllerManager::switch_controller( const std::vector & controllers = rt_controllers_wrapper_.get_updated_list(guard); - // if a preceding controller is deactivated, all first-level controllers should be switched 'from' - // chained mode - propagate_deactivation_of_chained_mode(controllers); - - // check if controllers should be switched 'to' chained mode when controllers are activated - for (auto ctrl_it = activate_request_.begin(); ctrl_it != activate_request_.end(); ++ctrl_it) + const auto check_de_activate_request_and_create_chained_mode_request = + [this, &strictness, &controllers]() -> CheckDeActivateRequestResult { - auto controller_it = std::find_if( - controllers.begin(), controllers.end(), - std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it)); - controller_interface::return_type status = controller_interface::return_type::OK; + // if a preceding controller is deactivated, all first-level controllers should be switched + // 'from' chained mode + propagate_deactivation_of_chained_mode(controllers); - // if controller is not inactive then do not do any following-controllers checks - if (!is_controller_inactive(controller_it->c)) + // we need to check the requests in the order of deactivate and activate to consider the case + // where the controller will be restarted is included in the (de)activate request + if (const auto result = check_deactivate_requests(controllers, strictness); + result != CheckDeActivateRequestResult::OK) { - RCLCPP_WARN( - get_logger(), - "Controller with name '%s' is not inactive so its following " - "controllers do not have to be checked, because it cannot be activated.", - controller_it->info.name.c_str()); - status = controller_interface::return_type::ERROR; + return result; } - else + if (const auto result = check_activate_requests(controllers, strictness); + result != CheckDeActivateRequestResult::OK) { - status = check_following_controllers_for_activate(controllers, strictness, controller_it); + return result; } - if (status != controller_interface::return_type::OK) - { - RCLCPP_WARN( - get_logger(), - "Could not activate controller with name '%s'. Check above warnings for more details. " - "Check the state of the controllers and their required interfaces using " - "`ros2 control list_controllers -v` CLI to get more information.", - (*ctrl_it).c_str()); - if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT) - { - // TODO(destogl): automatic manipulation of the chain: - // || strictness == - // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN); - // remove controller that can not be activated from the activation request and step-back - // iterator to correctly step to the next element in the list in the loop - activate_request_.erase(ctrl_it); - --ctrl_it; - } - if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) - { - RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)"); - // reset all lists - clear_requests(); - return controller_interface::return_type::ERROR; - } - } - } + return CheckDeActivateRequestResult::OK; + }; - // check if controllers should be deactivated if used in chained mode - for (auto ctrl_it = deactivate_request_.begin(); ctrl_it != deactivate_request_.end(); ++ctrl_it) + // Validate the (de)activate request and create from/to chained mode requests as needed. + // If the strictness value is STRICT, return an error for any invalid request. + // If the strictness value is BEST_EFFORT, remove any controllers that cannot be + // (de)activated from the request and proceed. However, any changes to the + // (de)activate request will affect the outcome of the check and creation process, + // so retry from the beginning. + while (true) { - auto controller_it = std::find_if( - controllers.begin(), controllers.end(), - std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it)); - controller_interface::return_type status = controller_interface::return_type::OK; - - // if controller is not active then skip preceding-controllers checks - if (!is_controller_active(controller_it->c)) - { - RCLCPP_WARN( - get_logger(), "Controller with name '%s' can not be deactivated since it is not active.", - controller_it->info.name.c_str()); - status = controller_interface::return_type::ERROR; - } - else + const auto result = check_de_activate_request_and_create_chained_mode_request(); + if (result == CheckDeActivateRequestResult::RETRY) { - status = check_preceeding_controllers_for_deactivate(controllers, strictness, controller_it); + continue; } - - if (status != controller_interface::return_type::OK) + if (result == CheckDeActivateRequestResult::ERROR) { - RCLCPP_WARN( - get_logger(), - "Could not deactivate controller with name '%s'. Check above warnings for more details. " - "Check the state of the controllers and their required interfaces using " - "`ros2 control list_controllers -v` CLI to get more information.", - (*ctrl_it).c_str()); - if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT) - { - // remove controller that can not be activated from the activation request and step-back - // iterator to correctly step to the next element in the list in the loop - deactivate_request_.erase(ctrl_it); - --ctrl_it; - } - if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) - { - RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)"); - // reset all lists - clear_requests(); - return controller_interface::return_type::ERROR; - } + return controller_interface::return_type::ERROR; } + // if result == CheckDeActivateRequestResult::OK -> break the loop + break; } for (const auto & controller : controllers) @@ -1416,9 +1364,16 @@ void ControllerManager::deactivate_controllers( const auto new_state = controller->get_node()->deactivate(); controller->release_interfaces(); - // if it is a chainable controller, make the reference interfaces unavailable on - // deactivation - if (controller->is_chainable()) + // If it is a chainable controller, make the reference interfaces unavailable on + // deactivation. + // However, if it will be activated asap for restart due to subsequent processes in + // manage_switch, the reference_interface needs to remain available to ensure the success of + // the switch_chained_mode process, so this case is an exception. + const bool will_be_restarted_asap = + switch_params_.do_switch && switch_params_.activate_asap && + (std::find(activate_request_.begin(), activate_request_.end(), controller_name) != + activate_request_.end()); + if (controller->is_chainable() && !will_be_restarted_asap) { resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); } @@ -2106,8 +2061,11 @@ void ControllerManager::manage_switch() deactivate_controllers(rt_controller_list, deactivate_request_); - switch_chained_mode(to_chained_mode_request_, true); + // When the same controller is specified for both 'from' and 'to' for restarting a controller, it + // is necessary to switch in the order 'from' then 'to', in order to disable the chained mode + // once and then enable it again. switch_chained_mode(from_chained_mode_request_, false); + switch_chained_mode(to_chained_mode_request_, true); // activate controllers once the switch is fully complete if (!switch_params_.activate_asap) @@ -2419,24 +2377,28 @@ controller_interface::return_type ControllerManager::check_following_controllers return controller_interface::return_type::ERROR; } + const bool following_will_be_activated = + std::find(activate_request_.begin(), activate_request_.end(), following_ctrl_it->info.name) != + activate_request_.end(); + const bool following_will_be_deactivated = + std::find( + deactivate_request_.begin(), deactivate_request_.end(), following_ctrl_it->info.name) != + deactivate_request_.end(); + if (is_controller_active(following_ctrl_it->c)) { - // will following controller be deactivated? - if ( - std::find( - deactivate_request_.begin(), deactivate_request_.end(), following_ctrl_it->info.name) != - deactivate_request_.end()) + // will following controller be deactivated and will not be restarted? + if (following_will_be_deactivated && !following_will_be_activated) { RCLCPP_WARN( - get_logger(), "The following controller with name '%s' will be deactivated.", + get_logger(), + "The following controller with name '%s' will be deactivated and will not be restarted.", following_ctrl_it->info.name.c_str()); return controller_interface::return_type::ERROR; } } // check if following controller will not be activated - else if ( - std::find(activate_request_.begin(), activate_request_.end(), following_ctrl_it->info.name) == - activate_request_.end()) + else if (!following_will_be_activated) { RCLCPP_WARN( get_logger(), @@ -2496,9 +2458,71 @@ controller_interface::return_type ControllerManager::check_following_controllers } } return controller_interface::return_type::OK; -}; +} + +ControllerManager::CheckDeActivateRequestResult ControllerManager::check_activate_requests( + const std::vector & controllers, int strictness) +{ + // check if controllers should be switched 'to' chained mode when controllers are activated + for (auto ctrl_it = activate_request_.begin(); ctrl_it != activate_request_.end(); ++ctrl_it) + { + auto controller_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it)); + controller_interface::return_type status = controller_interface::return_type::OK; -controller_interface::return_type ControllerManager::check_preceeding_controllers_for_deactivate( + // if controller is not inactive then do not do any following-controllers checks + if ( + !is_controller_inactive(controller_it->c) && + std::find(deactivate_request_.begin(), deactivate_request_.end(), *ctrl_it) == + deactivate_request_.end()) + { + RCLCPP_WARN( + get_logger(), + "Controller with name '%s' is not inactive so its following " + "controllers do not have to be checked, because it cannot be activated.", + controller_it->info.name.c_str()); + status = controller_interface::return_type::ERROR; + } + else + { + status = check_following_controllers_for_activate(controllers, strictness, controller_it); + } + + if (status != controller_interface::return_type::OK) + { + RCLCPP_WARN( + get_logger(), + "Could not activate controller with name '%s'. Check above warnings for more details. " + "Check the state of the controllers and their required interfaces using " + "`ros2 control list_controllers -v` CLI to get more information.", + (*ctrl_it).c_str()); + if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT) + { + // TODO(destogl): automatic manipulation of the chain: + // || strictness == + // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN); + // remove controller that can not be activated from the activation request and step-back + // iterator to correctly step to the next element in the list in the loop + activate_request_.erase(ctrl_it); + // reset chained mode request lists and will retry the creation of the request + clear_chained_mode_requests(); + return CheckDeActivateRequestResult::RETRY; + } + if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + { + RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)"); + // reset all lists + clear_requests(); + return CheckDeActivateRequestResult::ERROR; + } + } + } + + return CheckDeActivateRequestResult::OK; +} + +controller_interface::return_type ControllerManager::check_preceding_controllers_for_deactivate( const std::vector & controllers, int /*strictness*/, const ControllersListIterator controller_it) { @@ -2543,12 +2567,22 @@ controller_interface::return_type ControllerManager::check_preceeding_controller continue; } - // check if preceding controller will be activated - if ( - is_controller_inactive(preceding_ctrl_it->c) && + const bool following_will_be_activated = + std::find(activate_request_.begin(), activate_request_.end(), controller_it->info.name) != + activate_request_.end(); + const bool preceding_will_be_deactivated = + std::find( + deactivate_request_.begin(), deactivate_request_.end(), preceding_ctrl_it->info.name) != + deactivate_request_.end(); + const bool preceding_will_be_activated = std::find( activate_request_.begin(), activate_request_.end(), preceding_ctrl_it->info.name) != - activate_request_.end()) + activate_request_.end(); + const bool preceding_will_be_restarted = + preceding_will_be_deactivated && preceding_will_be_activated; + + // check if preceding controller will be activated + if (is_controller_inactive(preceding_ctrl_it->c) && preceding_will_be_activated) { RCLCPP_WARN( get_logger(), @@ -2558,11 +2592,7 @@ controller_interface::return_type ControllerManager::check_preceeding_controller return controller_interface::return_type::ERROR; } // check if preceding controller will not be deactivated - else if ( - is_controller_active(preceding_ctrl_it->c) && - std::find( - deactivate_request_.begin(), deactivate_request_.end(), preceding_ctrl_it->info.name) == - deactivate_request_.end()) + else if (is_controller_active(preceding_ctrl_it->c) && !preceding_will_be_deactivated) { RCLCPP_WARN( get_logger(), @@ -2571,6 +2601,19 @@ controller_interface::return_type ControllerManager::check_preceeding_controller controller_it->info.name.c_str(), preceding_ctrl_it->info.name.c_str()); return controller_interface::return_type::ERROR; } + // check if only following controller will not be restarted despite preceding will not be + // restarted + else if ( + is_controller_active(preceding_ctrl_it->c) && !following_will_be_activated && + preceding_will_be_restarted) + { + RCLCPP_WARN( + get_logger(), + "Could not deactivate and restart controller with name '%s' because " + "preceding controller with name '%s' is active and will not be restarted.", + controller_it->info.name.c_str(), preceding_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } // TODO(destogl): this should be discussed how to it the best - just a placeholder for now // else if ( // strictness == @@ -2584,6 +2627,60 @@ controller_interface::return_type ControllerManager::check_preceeding_controller return controller_interface::return_type::OK; } +ControllerManager::CheckDeActivateRequestResult ControllerManager::check_deactivate_requests( + const std::vector & controllers, int strictness) +{ + // check if controllers should be deactivated if used in chained mode + for (auto ctrl_it = deactivate_request_.begin(); ctrl_it != deactivate_request_.end(); ++ctrl_it) + { + auto controller_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it)); + controller_interface::return_type status = controller_interface::return_type::OK; + + // if controller is not active then skip preceding-controllers checks + if (!is_controller_active(controller_it->c)) + { + RCLCPP_WARN( + get_logger(), "Controller with name '%s' can not be deactivated since it is not active.", + controller_it->info.name.c_str()); + status = controller_interface::return_type::ERROR; + } + else + { + status = check_preceding_controllers_for_deactivate(controllers, strictness, controller_it); + } + + if (status != controller_interface::return_type::OK) + { + RCLCPP_WARN( + get_logger(), + "Could not deactivate controller with name '%s'. Check above warnings for more details. " + "Check the state of the controllers and their required interfaces using " + "`ros2 control list_controllers -v` CLI to get more information.", + (*ctrl_it).c_str()); + if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT) + { + // remove controller that can not be activated from the activation request and step-back + // iterator to correctly step to the next element in the list in the loop + deactivate_request_.erase(ctrl_it); + // reset chained mode request lists and will retry the creation of the request + clear_chained_mode_requests(); + return CheckDeActivateRequestResult::RETRY; + } + if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + { + RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)"); + // reset all lists + clear_requests(); + return CheckDeActivateRequestResult::ERROR; + } + } + } + + return CheckDeActivateRequestResult::OK; +} + void ControllerManager::controller_activity_diagnostic_callback( diagnostic_updater::DiagnosticStatusWrapper & stat) { diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index d21957a0b4..2c35a8d071 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -134,6 +134,8 @@ CallbackReturn TestChainableController::on_configure( CallbackReturn TestChainableController::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { + ++activate_calls; + if (!is_in_chained_mode()) { auto msg = rt_command_ptr_.readFromRT(); @@ -143,6 +145,13 @@ CallbackReturn TestChainableController::on_activate( return CallbackReturn::SUCCESS; } +CallbackReturn TestChainableController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + ++deactivate_calls; + return CallbackReturn::SUCCESS; +} + CallbackReturn TestChainableController::on_cleanup( const rclcpp_lifecycle::State & /*previous_state*/) { diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp index 5925ed8d11..3a56b87cf3 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp @@ -57,6 +57,9 @@ class TestChainableController : public controller_interface::ChainableController CONTROLLER_MANAGER_PUBLIC CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + CONTROLLER_MANAGER_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + CONTROLLER_MANAGER_PUBLIC CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; @@ -80,7 +83,12 @@ class TestChainableController : public controller_interface::ChainableController CONTROLLER_MANAGER_PUBLIC void set_reference_interface_names(const std::vector & reference_interface_names); - size_t internal_counter; + size_t internal_counter = 0; + + // Variable where we store when activate or deactivate was called + size_t activate_calls = 0; + size_t deactivate_calls = 0; + controller_interface::InterfaceConfiguration cmd_iface_cfg_; controller_interface::InterfaceConfiguration state_iface_cfg_; std::vector reference_interface_names_; diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 8f120bbd47..76c3b749ae 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -101,6 +101,18 @@ CallbackReturn TestController::on_configure(const rclcpp_lifecycle::State & /*pr return CallbackReturn::SUCCESS; } +CallbackReturn TestController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) +{ + ++activate_calls; + return CallbackReturn::SUCCESS; +} + +CallbackReturn TestController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) +{ + ++deactivate_calls; + return CallbackReturn::SUCCESS; +} + CallbackReturn TestController::on_cleanup(const rclcpp_lifecycle::State & /*previous_state*/) { if (simulate_cleanup_failure) diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index 368aeae4a8..97c9cd7c0e 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -56,6 +56,12 @@ class TestController : public controller_interface::ControllerInterface CONTROLLER_MANAGER_PUBLIC CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + CONTROLLER_MANAGER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + CONTROLLER_MANAGER_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + CONTROLLER_MANAGER_PUBLIC CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; @@ -69,6 +75,11 @@ class TestController : public controller_interface::ControllerInterface const std::string & getRobotDescription() const; unsigned int internal_counter = 0; + + // Variable where we store when activate or deactivate was called + size_t activate_calls = 0; + size_t deactivate_calls = 0; + bool simulate_cleanup_failure = false; // Variable where we store when cleanup was called, pointer because the controller // is usually destroyed after cleanup diff --git a/controller_manager/test/test_controller_with_command/test_controller_with_command.cpp b/controller_manager/test/test_controller_with_command/test_controller_with_command.cpp new file mode 100644 index 0000000000..1a1eb788d6 --- /dev/null +++ b/controller_manager/test/test_controller_with_command/test_controller_with_command.cpp @@ -0,0 +1,74 @@ +// Copyright 2024 Tokyo Robotics Inc. +// +// 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. + +#include "test_controller_with_command.hpp" + +#include +#include + +#include "lifecycle_msgs/msg/transition.hpp" + +namespace test_controller_with_command +{ +TestControllerWithCommand::TestControllerWithCommand() : controller_interface::ControllerInterface() +{ +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +TestControllerWithCommand::on_init() +{ + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type TestControllerWithCommand::update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + return controller_interface::return_type::OK; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +TestControllerWithCommand::on_configure(const rclcpp_lifecycle::State & /*previous_state&*/) +{ + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +TestControllerWithCommand::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) +{ + simulate_command = SIMULATE_COMMAND_ACTIVATE_VALUE; + ++activate_calls; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +TestControllerWithCommand::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) +{ + simulate_command = SIMULATE_COMMAND_DEACTIVATE_VALUE; + ++deactivate_calls; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +TestControllerWithCommand::on_cleanup(const rclcpp_lifecycle::State & /*previous_state*/) +{ + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +} // namespace test_controller_with_command + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + test_controller_with_command::TestControllerWithCommand, + controller_interface::ControllerInterface) diff --git a/controller_manager/test/test_controller_with_command/test_controller_with_command.hpp b/controller_manager/test/test_controller_with_command/test_controller_with_command.hpp new file mode 100644 index 0000000000..d1992b8340 --- /dev/null +++ b/controller_manager/test/test_controller_with_command/test_controller_with_command.hpp @@ -0,0 +1,85 @@ +// Copyright 2024 Tokyo Robotics Inc. +// +// 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 TEST_CONTROLLER_WITH_COMMAND__TEST_CONTROLLER_WITH_COMMAND_HPP_ +#define TEST_CONTROLLER_WITH_COMMAND__TEST_CONTROLLER_WITH_COMMAND_HPP_ + +#include + +#include "controller_interface/visibility_control.h" +#include "controller_manager/controller_manager.hpp" + +namespace test_controller_with_command +{ +// Corresponds to the name listed within the pluginlib xml +constexpr char TEST_CONTROLLER_CLASS_NAME[] = "controller_manager/test_controller_with_command"; +// Corresponds to the command interface to claim +constexpr char TEST_CONTROLLER_COMMAND_INTERFACE[] = "joint/position"; + +constexpr double SIMULATE_COMMAND_DEACTIVATE_VALUE = 25252525.0; +constexpr double SIMULATE_COMMAND_ACTIVATE_VALUE = 27272727.0; + +class TestControllerWithCommand : public controller_interface::ControllerInterface +{ +public: + CONTROLLER_MANAGER_PUBLIC + TestControllerWithCommand(); + + CONTROLLER_MANAGER_PUBLIC + virtual ~TestControllerWithCommand() = default; + + controller_interface::InterfaceConfiguration command_interface_configuration() const override + { + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; + } + + controller_interface::InterfaceConfiguration state_interface_configuration() const override + { + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; + } + + CONTROLLER_MANAGER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + CONTROLLER_MANAGER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override; + + CONTROLLER_MANAGER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + CONTROLLER_MANAGER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + CONTROLLER_MANAGER_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + CONTROLLER_MANAGER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + // Variable where we store when activate or deactivate was called + size_t activate_calls = 0; + size_t deactivate_calls = 0; + + // Simulate command value for test + double simulate_command = std::numeric_limits::quiet_NaN(); +}; + +} // namespace test_controller_with_command + +#endif // TEST_CONTROLLER_WITH_COMMAND__TEST_CONTROLLER_WITH_COMMAND_HPP_ diff --git a/controller_manager/test/test_controller_with_command/test_controller_with_command.xml b/controller_manager/test/test_controller_with_command/test_controller_with_command.xml new file mode 100644 index 0000000000..e28cfbd0b9 --- /dev/null +++ b/controller_manager/test/test_controller_with_command/test_controller_with_command.xml @@ -0,0 +1,9 @@ + + + + + Controller used for testing + + + + diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index eadca39756..c7edc0eca7 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -28,6 +28,17 @@ #include "test_chainable_controller/test_chainable_controller.hpp" #include "test_controller/test_controller.hpp" +namespace +{ +template +std::vector concat_vector(const std::vector & first, const Args &... args) +{ + std::vector result = first; + (result.insert(result.end(), args.begin(), args.end()), ...); + return result; +} +} // namespace + // The tests in this file are implementing example of chained-control for DiffDrive robot example: // https://github.com/ros-controls/roadmap/blob/9f32e215a84347aee0b519cb24d081f23bbbb224/design_drafts/cascade_control.md#motivation-purpose-and-use // The controller have the names as stated in figure, but they are simply forwarding values without @@ -46,6 +57,9 @@ class TestableTestChainableController : public test_chainable_controller::TestCh FRIEND_TEST( TestControllerChainingWithControllerManager, test_chained_controllers_activation_error_handling); + FRIEND_TEST( + TestControllerChainingWithControllerManager, + test_chained_controllers_activation_error_handling2); FRIEND_TEST( TestControllerChainingWithControllerManager, test_chained_controllers_activation_switching_error_handling); @@ -54,6 +68,9 @@ class TestableTestChainableController : public test_chainable_controller::TestCh test_chained_controllers_deactivation_error_handling); FRIEND_TEST( TestControllerChainingWithControllerManager, test_chained_controllers_adding_in_random_order); + FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers_restart); + FRIEND_TEST( + TestControllerChainingWithControllerManager, test_chained_controllers_restart_error_handling); }; class TestableControllerManager : public controller_manager::ControllerManager @@ -80,6 +97,9 @@ class TestableControllerManager : public controller_manager::ControllerManager FRIEND_TEST( TestControllerChainingWithControllerManager, test_chained_controllers_activation_error_handling); + FRIEND_TEST( + TestControllerChainingWithControllerManager, + test_chained_controllers_activation_error_handling2); FRIEND_TEST( TestControllerChainingWithControllerManager, test_chained_controllers_activation_switching_error_handling); @@ -88,6 +108,9 @@ class TestableControllerManager : public controller_manager::ControllerManager test_chained_controllers_deactivation_error_handling); FRIEND_TEST( TestControllerChainingWithControllerManager, test_chained_controllers_adding_in_random_order); + FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers_restart); + FRIEND_TEST( + TestControllerChainingWithControllerManager, test_chained_controllers_restart_error_handling); public: TestableControllerManager( @@ -369,6 +392,16 @@ class TestControllerChainingWithControllerManager {}, {controller_name}, test_param.strictness, expected_future_status, expected_return); } + void RestartControllers( + const std::vector & controller_names, + const controller_interface::return_type expected_return = controller_interface::return_type::OK, + const std::future_status expected_future_status = std::future_status::timeout) + { + switch_test_controllers( + {controller_names}, {controller_names}, test_param.strictness, expected_future_status, + expected_return); + } + void UpdateAllControllerAndCheck( const std::vector & reference, size_t exp_internal_counter_pos_ctrl) { @@ -417,6 +450,91 @@ class TestControllerChainingWithControllerManager ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); } + void check_command_interfaces_claimed( + const std::vector & claimed_interfaces, + const std::vector & not_claimed_interfaces) + { + for (const auto & interface : claimed_interfaces) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)) + << "command interface: " << interface << " does not exist"; + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)) + << "command interface: " << interface << " is not available"; + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_claimed(interface)) + << "command interface: " << interface << " is not claimed"; + } + for (const auto & interface : not_claimed_interfaces) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)) + << "command interface: " << interface << " does not exist"; + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)) + << "command interface: " << interface << " is not available"; + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)) + << "command interface: " << interface << " is claimed"; + } + }; + + void SetupWithActivationAllControllersAndCheck() + { + SetupControllers(); + + // add all controllers - CONTROLLERS HAVE TO ADDED IN EXECUTION ORDER + cm_->add_controller( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + diff_drive_controller_two, DIFF_DRIVE_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + pid_left_wheel_controller, PID_LEFT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + pid_right_wheel_controller, PID_RIGHT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + + CheckIfControllersAreAddedCorrectly(); + + ConfigureAndCheckControllers(); + + // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers + cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); + + // Activate all controller for this test + ActivateAndCheckController( + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 1u); + ActivateAndCheckController( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 1u); + ActivateAndCheckController( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 1u); + ActivateAndCheckController( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + POSITION_CONTROLLER_CLAIMED_INTERFACES, 1u); + + // Verify that the claimed state of the command interface is correct + check_command_interfaces_claimed( + concat_vector( + DIFF_DRIVE_CLAIMED_INTERFACES, PID_LEFT_WHEEL_CLAIMED_INTERFACES, + PID_RIGHT_WHEEL_CLAIMED_INTERFACES), + POSITION_CONTROLLER_NOT_CLAIMED_INTERFACES); + + // Verify that activate and deactivate calls + // (All controllers are activated once at the beginning, + // and all following controllers are activated once more for switching to chained mode + // and deactivated for switching to chained mode when the preceding controller is + // activated) + EXPECT_EQ(2u, pid_left_wheel_controller->activate_calls); + EXPECT_EQ(2u, pid_right_wheel_controller->activate_calls); + EXPECT_EQ(2u, diff_drive_controller->activate_calls); + EXPECT_EQ(1u, position_tracking_controller->activate_calls); + EXPECT_EQ(1u, pid_left_wheel_controller->deactivate_calls); + EXPECT_EQ(1u, pid_right_wheel_controller->deactivate_calls); + EXPECT_EQ(1u, diff_drive_controller->deactivate_calls); + EXPECT_EQ(0u, position_tracking_controller->deactivate_calls); + } + // check data propagation through controllers and if individual controllers are working double chained_ctrl_calculation(double reference, double state) { return reference - state; } double hardware_calculation(double command) { return command / 2.0; } @@ -441,6 +559,8 @@ class TestControllerChainingWithControllerManager "pid_left_wheel_controller/velocity", "pid_right_wheel_controller/velocity"}; const std::vector POSITION_CONTROLLER_CLAIMED_INTERFACES = { "diff_drive_controller/vel_x", "diff_drive_controller/vel_y"}; + const std::vector POSITION_CONTROLLER_NOT_CLAIMED_INTERFACES = { + "diff_drive_controller/rot_z"}; // controllers objects std::shared_ptr pid_left_wheel_controller; @@ -746,6 +866,31 @@ TEST_P( // Check if the controller activated (Should not be activated) ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + // Attempt to activate the most preceding controller (position tracking controller) and the + // middle preceding/following controller (diff-drive controller) + + switch_test_controllers( + {POSITION_TRACKING_CONTROLLER, DIFF_DRIVE_CONTROLLER}, {}, test_param.strictness, + std::future_status::ready, expected.at(test_param.strictness).return_type); + + // Check if the controllers are activated (Should not be activated) + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + pid_left_wheel_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + pid_right_wheel_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + position_tracking_controller->get_state().id()); + + // Check if the controllers are not in chained mode + ASSERT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + EXPECT_FALSE(position_tracking_controller->is_in_chained_mode()); // Test Case 2: Try to activate a preceding controller the same time when trying to // deactivate a following controller (using switch_controller function) @@ -775,13 +920,192 @@ TEST_P( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + + // Check if the controllers are not in chained mode + ASSERT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); +} + +TEST_P( + TestControllerChainingWithControllerManager, test_chained_controllers_activation_error_handling2) +{ + SetupControllers(); + + // add all controllers - CONTROLLERS HAVE TO ADDED IN EXECUTION ORDER + cm_->add_controller( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + diff_drive_controller_two, DIFF_DRIVE_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + pid_left_wheel_controller, PID_LEFT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + pid_right_wheel_controller, PID_RIGHT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + + CheckIfControllersAreAddedCorrectly(); + + ConfigureAndCheckControllers(); + + // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers + cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); + rclcpp::get_logger("ControllerManager::utils").set_level(rclcpp::Logger::Level::Debug); + + // at beginning controllers are not in chained mode + EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + + // Test Case 3: Trying to activate a preceding controller and one of the following controller + // --> return error; preceding controller are not activated, + // BUT following controller IS activated + static std::unordered_map expected = { + {controller_manager_msgs::srv::SwitchController::Request::STRICT, + {controller_interface::return_type::ERROR, std::future_status::ready, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE}}, + {controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT, + {controller_interface::return_type::OK, std::future_status::timeout, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE}}}; + + // Attempt to activate preceding controllers (position tracking and diff-drive controller) and + // one of the following controller (pid_left_wheel_controller) + switch_test_controllers( + {DIFF_DRIVE_CONTROLLER, PID_LEFT_WHEEL}, {}, test_param.strictness, + expected.at(test_param.strictness).future_status, + expected.at(test_param.strictness).return_type); + + // Preceding controller should stay deactivated and following controller + // should be activated (if BEST_EFFORT) + // If STRICT, preceding controller and following controller should stay deactivated + ASSERT_EQ(expected.at(test_param.strictness).state, pid_left_wheel_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + pid_right_wheel_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + position_tracking_controller->get_state().id()); + + // Check if the controllers are not in chained mode + ASSERT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + + // Deactivate following controller before next test + switch_test_controllers( + {}, {PID_LEFT_WHEEL}, test_param.strictness, expected.at(test_param.strictness).future_status, + expected.at(test_param.strictness).return_type); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + pid_left_wheel_controller->get_state().id()); + + // Attempt to activate preceding controller (diff-drive controller) and + // another following controller (pid_right_wheel_controller) + switch_test_controllers( + {DIFF_DRIVE_CONTROLLER, PID_RIGHT_WHEEL}, {}, test_param.strictness, + expected.at(test_param.strictness).future_status, + expected.at(test_param.strictness).return_type); + + // Preceding controller should stay deactivated and one of the following controller + // should be activated (if BEST_EFFORT) + // If STRICT, preceding controller and following controller should stay deactivated + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + pid_left_wheel_controller->get_state().id()); + ASSERT_EQ(expected.at(test_param.strictness).state, pid_right_wheel_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + + // Check if the controllers are not in chained mode + ASSERT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + + // Deactivate following controller before next test + switch_test_controllers( + {}, {PID_RIGHT_WHEEL}, test_param.strictness, expected.at(test_param.strictness).future_status, + expected.at(test_param.strictness).return_type); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + pid_left_wheel_controller->get_state().id()); + + // Test Case 4: Trying to activate preceding controllers and one of the following controller + // --> return error; preceding controllers are not activated, + // BUT following controller IS activated + + // Attempt to activate preceding controllers (position tracking and diff-drive controller) and + // one of the following controller (pid_left_wheel_controller) + switch_test_controllers( + {POSITION_TRACKING_CONTROLLER, DIFF_DRIVE_CONTROLLER, PID_LEFT_WHEEL}, {}, + test_param.strictness, expected.at(test_param.strictness).future_status, + expected.at(test_param.strictness).return_type); + + // Preceding controllers should stay deactivated and following controller + // should be activated (if BEST_EFFORT) + // If STRICT, preceding controllers and following controller should stay deactivated + ASSERT_EQ(expected.at(test_param.strictness).state, pid_left_wheel_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + pid_right_wheel_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + position_tracking_controller->get_state().id()); + + // Check if the controllers are not in chained mode + ASSERT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + EXPECT_FALSE(position_tracking_controller->is_in_chained_mode()); + + // Deactivate the following controller (pid_left_wheel_controller) before next test + DeactivateController( + PID_LEFT_WHEEL, expected.at(test_param.strictness).return_type, + expected.at(test_param.strictness).future_status); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + pid_left_wheel_controller->get_state().id()); + + // Attempt to activate preceding controllers (position tracking and diff-drive controller) and + // another following controller (pid_right_wheel_controller) + switch_test_controllers( + {POSITION_TRACKING_CONTROLLER, DIFF_DRIVE_CONTROLLER, PID_RIGHT_WHEEL}, {}, + test_param.strictness, expected.at(test_param.strictness).future_status, + expected.at(test_param.strictness).return_type); + + // Preceding controllers should stay deactivated and following controller + // should be activated (if BEST_EFFORT) + // If STRICT, preceding controllers and following controller should stay deactivated + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + pid_left_wheel_controller->get_state().id()); + ASSERT_EQ(expected.at(test_param.strictness).state, pid_right_wheel_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + position_tracking_controller->get_state().id()); + + // Check if the controllers are not in chained mode + ASSERT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + EXPECT_FALSE(position_tracking_controller->is_in_chained_mode()); } TEST_P( TestControllerChainingWithControllerManager, test_chained_controllers_activation_switching_error_handling) { - // Test Case 3: In terms of current implementation. + // Test Case 5: In terms of current implementation. // Example: Need two diff drive controllers, one should be deactivated, // and the other should be activated. Following controller should stay in activated state. SetupControllers(); @@ -884,7 +1208,7 @@ TEST_P( ActivateAndCheckController( pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 1u); - // Test Case 5: Deactivating a preceding controller that is not active --> return error; + // Test Case 6: Deactivating a preceding controller that is not active --> return error; // all controller stay in the same state // There is different error and timeout behavior depending on strictness @@ -914,7 +1238,7 @@ TEST_P( ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); - // Test Case 6: following controller is deactivated but preceding controller will be activated + // Test Case 7: following controller is deactivated but preceding controller will be activated // --> return error; controllers stay in the same state switch_test_controllers( @@ -931,7 +1255,7 @@ TEST_P( ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); - // Test Case 7: following controller deactivation but preceding controller is active + // Test Case 8: following controller deactivation but preceding controller is active // --> return error; controllers stay in the same state as they were // Activate all controllers for this test @@ -977,6 +1301,83 @@ TEST_P( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + + // Test Case 9: middle preceding/following controller deactivation but the most preceding and the + // lowest following controllers are active + // --> return error; controllers stay in the same state as they were + + const auto verify_all_controllers_are_still_be_active = [&]() + { + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + position_tracking_controller->get_state().id()); + }; + + // Activate all controllers for this test + ActivateController( + POSITION_TRACKING_CONTROLLER, controller_interface::return_type::OK, + std::future_status::timeout); + + // Expect all controllers to be active + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + position_tracking_controller->get_state().id()); + + // Attempt to deactivate one of the lowest following controller + switch_test_controllers( + {}, {PID_LEFT_WHEEL}, test_param.strictness, std::future_status::ready, + expected.at(test_param.strictness).return_type); + verify_all_controllers_are_still_be_active(); + + // Attempt to deactivate another lowest following controller + switch_test_controllers( + {}, {PID_RIGHT_WHEEL}, test_param.strictness, std::future_status::ready, + expected.at(test_param.strictness).return_type); + verify_all_controllers_are_still_be_active(); + + // Attempt to deactivate the lowest following controllers + switch_test_controllers( + {}, {PID_LEFT_WHEEL, PID_RIGHT_WHEEL}, test_param.strictness, std::future_status::ready, + expected.at(test_param.strictness).return_type); + verify_all_controllers_are_still_be_active(); + + // Attempt to deactivate the middle preceding/following controller + switch_test_controllers( + {}, {DIFF_DRIVE_CONTROLLER}, test_param.strictness, std::future_status::ready, + expected.at(test_param.strictness).return_type); + verify_all_controllers_are_still_be_active(); + + // Attempt to deactivate the middle following and one of the lowest following controller + switch_test_controllers( + {}, {DIFF_DRIVE_CONTROLLER, PID_LEFT_WHEEL}, test_param.strictness, std::future_status::ready, + expected.at(test_param.strictness).return_type); + verify_all_controllers_are_still_be_active(); + + // Attempt to deactivate the middle following and another lowest following controller + switch_test_controllers( + {}, {DIFF_DRIVE_CONTROLLER, PID_LEFT_WHEEL}, test_param.strictness, std::future_status::ready, + expected.at(test_param.strictness).return_type); + verify_all_controllers_are_still_be_active(); + + // Attempt to deactivate all following controllers + switch_test_controllers( + {}, {PID_LEFT_WHEEL, PID_RIGHT_WHEEL, DIFF_DRIVE_CONTROLLER}, test_param.strictness, + std::future_status::ready, expected.at(test_param.strictness).return_type); + verify_all_controllers_are_still_be_active(); } TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_adding_in_random_order) @@ -1106,6 +1507,485 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add UpdateAllControllerAndCheck(reference, 3u); } +TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_restart) +{ + SetupWithActivationAllControllersAndCheck(); + + // Verify update (internal_counter 2->3) + UpdateAllControllerAndCheck({32.0, 128.0}, 2u); + + { + // Restart the most preceding controller (position tracking controller) + // (internal_counter 3->5) + RestartControllers({POSITION_TRACKING_CONTROLLER}); + + // Following controllers should stay active + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + // Preceding controller should return to active (active -> inactive -> active) + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + position_tracking_controller->get_state().id()); + + // Verify that the claimed state of the interface does not change before and after the restart + check_command_interfaces_claimed( + concat_vector( + POSITION_CONTROLLER_CLAIMED_INTERFACES, DIFF_DRIVE_CLAIMED_INTERFACES, + PID_LEFT_WHEEL_CLAIMED_INTERFACES, PID_RIGHT_WHEEL_CLAIMED_INTERFACES), + POSITION_CONTROLLER_NOT_CLAIMED_INTERFACES); + + // Verify that only the restart controller has its activate and deactivate calls incremented. + ASSERT_EQ(2u, pid_left_wheel_controller->activate_calls); // +0 + ASSERT_EQ(2u, pid_right_wheel_controller->activate_calls); // +0 + ASSERT_EQ(2u, diff_drive_controller->activate_calls); // +0 + ASSERT_EQ(2u, position_tracking_controller->activate_calls); // +1 (restart) + ASSERT_EQ(1u, pid_left_wheel_controller->deactivate_calls); // +0 + ASSERT_EQ(1u, pid_right_wheel_controller->deactivate_calls); // +0 + ASSERT_EQ(1u, diff_drive_controller->deactivate_calls); // +0 + ASSERT_EQ(1u, position_tracking_controller->deactivate_calls); // +1 (restart) + + // Verify that the controllers are still in the same chained mode + ASSERT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); + EXPECT_FALSE(position_tracking_controller->is_in_chained_mode()); + + // Verify update after restart most preceding controller + // (internal_counter 5->6) + UpdateAllControllerAndCheck({1024.0, 4096.0}, 5u); + } + + { + // Restart all controllers + // (internal_counter 6->8) + // Note : It is also important that the state where the internal_counter values + // between controllers are offset by +2 does not change due to the restart process) + RestartControllers( + {POSITION_TRACKING_CONTROLLER, DIFF_DRIVE_CONTROLLER, PID_LEFT_WHEEL, PID_RIGHT_WHEEL}); + + // Verify that the claimed state of the interface does not change before and after the restart + check_command_interfaces_claimed( + concat_vector( + POSITION_CONTROLLER_CLAIMED_INTERFACES, DIFF_DRIVE_CLAIMED_INTERFACES, + PID_LEFT_WHEEL_CLAIMED_INTERFACES, PID_RIGHT_WHEEL_CLAIMED_INTERFACES), + POSITION_CONTROLLER_NOT_CLAIMED_INTERFACES); + + // Verify that the activate and deactivate calls of all controllers are incremented + ASSERT_EQ(3u, pid_left_wheel_controller->activate_calls); // +1 (restart) + ASSERT_EQ(3u, pid_right_wheel_controller->activate_calls); // +1 (restart) + ASSERT_EQ(3u, diff_drive_controller->activate_calls); // +1 (restart) + ASSERT_EQ(3u, position_tracking_controller->activate_calls); // +1 (restart) + ASSERT_EQ(2u, pid_left_wheel_controller->deactivate_calls); // +1 (restart) + ASSERT_EQ(2u, pid_right_wheel_controller->deactivate_calls); // +1 (restart) + ASSERT_EQ(2u, diff_drive_controller->deactivate_calls); // +1 (restart) + ASSERT_EQ(2u, position_tracking_controller->deactivate_calls); // +1 (restart) + + // Verify that the controllers are still in the same chained mode + ASSERT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); + EXPECT_FALSE(position_tracking_controller->is_in_chained_mode()); + + // Verify update again after restart all controllers + // (internal_counter 8->9->10) + UpdateAllControllerAndCheck({16.0, 64.0}, 8u); + UpdateAllControllerAndCheck({512.0, 2048.0}, 9u); + } + + { + // Restart middle preceding/following controller (diff_drive_controller) and stop the most + // preceding controller (position_tracking_controller) simultaneously + switch_test_controllers( + {DIFF_DRIVE_CONTROLLER}, {DIFF_DRIVE_CONTROLLER, POSITION_TRACKING_CONTROLLER}, + test_param.strictness, std::future_status::timeout, controller_interface::return_type::OK); + + // Verify that the reference_interface is not claimed because diff_drive_controller is no + // longer the following controller + check_command_interfaces_claimed( + concat_vector( + DIFF_DRIVE_CLAIMED_INTERFACES, PID_LEFT_WHEEL_CLAIMED_INTERFACES, + PID_RIGHT_WHEEL_CLAIMED_INTERFACES), + concat_vector( + POSITION_CONTROLLER_NOT_CLAIMED_INTERFACES, POSITION_CONTROLLER_CLAIMED_INTERFACES)); + + // Verify that the activate and deactivate calls for diff_drive_controller are incremented, + // and only the deactivate count for position_tracking_controller is incremented. + ASSERT_EQ(3u, pid_left_wheel_controller->activate_calls); // +0 + ASSERT_EQ(3u, pid_right_wheel_controller->activate_calls); // +0 + ASSERT_EQ(4u, diff_drive_controller->activate_calls); // +1 (restart) + ASSERT_EQ(3u, position_tracking_controller->activate_calls); // +0 + ASSERT_EQ(2u, pid_left_wheel_controller->deactivate_calls); // +0 + ASSERT_EQ(2u, pid_right_wheel_controller->deactivate_calls); // +0 + ASSERT_EQ(3u, diff_drive_controller->deactivate_calls); // +1 (restart) + ASSERT_EQ(3u, position_tracking_controller->deactivate_calls); // +1 (deactivate) + + // Verify that the diff_drive_controller is no longer in chained mode + ASSERT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + EXPECT_FALSE(position_tracking_controller->is_in_chained_mode()); + } + + { + // Restart the preceding controller (diff_drive_controller) and a following controller + // (pid_right_wheel_controller) + RestartControllers({PID_RIGHT_WHEEL, DIFF_DRIVE_CONTROLLER}); + + // Verify that the claimed state of the interface does not change before and after the restart + check_command_interfaces_claimed( + concat_vector( + DIFF_DRIVE_CLAIMED_INTERFACES, PID_LEFT_WHEEL_CLAIMED_INTERFACES, + PID_RIGHT_WHEEL_CLAIMED_INTERFACES), + concat_vector( + POSITION_CONTROLLER_NOT_CLAIMED_INTERFACES, POSITION_CONTROLLER_CLAIMED_INTERFACES)); + + // Verify that only the restart controller has its activate and deactivate calls incremented. + ASSERT_EQ(3u, pid_left_wheel_controller->activate_calls); // +0 + ASSERT_EQ(4u, pid_right_wheel_controller->activate_calls); // +1 (restart) + ASSERT_EQ(5u, diff_drive_controller->activate_calls); // +1 (restart) + ASSERT_EQ(3u, position_tracking_controller->activate_calls); // +0 + ASSERT_EQ(2u, pid_left_wheel_controller->deactivate_calls); // +0 + ASSERT_EQ(3u, pid_right_wheel_controller->deactivate_calls); // +1 (restart) + ASSERT_EQ(4u, diff_drive_controller->deactivate_calls); // +1 (restart) + ASSERT_EQ(3u, position_tracking_controller->deactivate_calls); // +0 + + // Verify that the controllers are still in the same chained mode + ASSERT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + EXPECT_FALSE(position_tracking_controller->is_in_chained_mode()); + } + + { + // Restart middle preceding/following controller (diff_drive_controller) and start the most + // preceding controller (position_tracking_controller) simultaneously + switch_test_controllers( + {POSITION_TRACKING_CONTROLLER, DIFF_DRIVE_CONTROLLER}, {DIFF_DRIVE_CONTROLLER}, + test_param.strictness, std::future_status::timeout, controller_interface::return_type::OK); + + // Verify that the reference_interface is claimed because diff_drive_controller becomes the + // following controller + check_command_interfaces_claimed( + concat_vector( + POSITION_CONTROLLER_CLAIMED_INTERFACES, DIFF_DRIVE_CLAIMED_INTERFACES, + PID_LEFT_WHEEL_CLAIMED_INTERFACES, PID_RIGHT_WHEEL_CLAIMED_INTERFACES), + POSITION_CONTROLLER_NOT_CLAIMED_INTERFACES); + + // Verify that the activate and deactivate calls for diff_drive_controller are incremented, + // and only the deactivate count for position_tracking_controller is incremented. + ASSERT_EQ(3u, pid_left_wheel_controller->activate_calls); // +0 + ASSERT_EQ(4u, pid_right_wheel_controller->activate_calls); // +0 + ASSERT_EQ(6u, diff_drive_controller->activate_calls); // +1 (restart) + ASSERT_EQ(4u, position_tracking_controller->activate_calls); // +1 (activate) + ASSERT_EQ(2u, pid_left_wheel_controller->deactivate_calls); // +0 + ASSERT_EQ(3u, pid_right_wheel_controller->deactivate_calls); // +0 + ASSERT_EQ(5u, diff_drive_controller->deactivate_calls); // +1 (restart) + ASSERT_EQ(3u, position_tracking_controller->deactivate_calls); // +0 + + // Verify that the diff_drive_controller is in chained mode + ASSERT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); + EXPECT_FALSE(position_tracking_controller->is_in_chained_mode()); + } +} + +TEST_P( + TestControllerChainingWithControllerManager, + test_chained_controllers_restart_preceding_and_stop_following) +{ + SetupWithActivationAllControllersAndCheck(); + + // Restart the most preceding controller (position_tracking_controller) and stop the middle + // following controller (diff_drive_controller) simultaneously + if (test_param.strictness == STRICT) + { + // If STRICT, since the preceding controller is reactivated by restart, can not stop the + // following controller and should be return error + switch_test_controllers( + {POSITION_TRACKING_CONTROLLER}, {POSITION_TRACKING_CONTROLLER, DIFF_DRIVE_CONTROLLER}, STRICT, + std::future_status::ready, controller_interface::return_type::ERROR); + + // Verify that the claimed state of the interface does not change before and after the restart + check_command_interfaces_claimed( + concat_vector( + POSITION_CONTROLLER_CLAIMED_INTERFACES, DIFF_DRIVE_CLAIMED_INTERFACES, + PID_LEFT_WHEEL_CLAIMED_INTERFACES, PID_RIGHT_WHEEL_CLAIMED_INTERFACES), + POSITION_CONTROLLER_NOT_CLAIMED_INTERFACES); + + // Verify activate and deactivate calls are not changed + ASSERT_EQ(2u, pid_left_wheel_controller->activate_calls); // +0 + ASSERT_EQ(2u, pid_right_wheel_controller->activate_calls); // +0 + ASSERT_EQ(2u, diff_drive_controller->activate_calls); // +0 + ASSERT_EQ(1u, position_tracking_controller->activate_calls); // +0 + ASSERT_EQ(1u, pid_left_wheel_controller->deactivate_calls); // +0 + ASSERT_EQ(1u, pid_right_wheel_controller->deactivate_calls); // +0 + ASSERT_EQ(1u, diff_drive_controller->deactivate_calls); // +0 + ASSERT_EQ(0u, position_tracking_controller->deactivate_calls); // +0 + + // Verify that the controllers are still in the same chained mode + ASSERT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); + EXPECT_FALSE(position_tracking_controller->is_in_chained_mode()); + } + else if (test_param.strictness == BEST_EFFORT) + { + // If BEST_EFFORT, since the stop request for the diff_drive_controller is not accepted, and the + // restart request for position_tracking_controller is accepted, and an OK response is returned + switch_test_controllers( + {POSITION_TRACKING_CONTROLLER}, {POSITION_TRACKING_CONTROLLER, DIFF_DRIVE_CONTROLLER}, + BEST_EFFORT, std::future_status::timeout, controller_interface::return_type::OK); + + // Verify that the claimed state of the interface does not change before and after the restart + check_command_interfaces_claimed( + concat_vector( + POSITION_CONTROLLER_CLAIMED_INTERFACES, DIFF_DRIVE_CLAIMED_INTERFACES, + PID_LEFT_WHEEL_CLAIMED_INTERFACES, PID_RIGHT_WHEEL_CLAIMED_INTERFACES), + POSITION_CONTROLLER_NOT_CLAIMED_INTERFACES); + + // Verify that only the restart position_tracking_controller has its activate and deactivate + // calls incremented. + ASSERT_EQ(2u, pid_left_wheel_controller->activate_calls); // +0 + ASSERT_EQ(2u, pid_right_wheel_controller->activate_calls); // +0 + ASSERT_EQ(2u, diff_drive_controller->activate_calls); // +0 + ASSERT_EQ(2u, position_tracking_controller->activate_calls); // +1 (restart) + ASSERT_EQ(1u, pid_left_wheel_controller->deactivate_calls); // +0 + ASSERT_EQ(1u, pid_right_wheel_controller->deactivate_calls); // +0 + ASSERT_EQ(1u, diff_drive_controller->deactivate_calls); // +0 + ASSERT_EQ(1u, position_tracking_controller->deactivate_calls); // +1 (restart) + + // Verify that the controllers are still in the same chained mode + ASSERT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); + EXPECT_FALSE(position_tracking_controller->is_in_chained_mode()); + } +} + +TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_restart_error_handling) +{ + SetupWithActivationAllControllersAndCheck(); + + // There is different error and timeout behavior depending on strictness + static std::unordered_map expected = { + {controller_manager_msgs::srv::SwitchController::Request::STRICT, + {controller_interface::return_type::ERROR, std::future_status::ready, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE}}, + {controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT, + {controller_interface::return_type::OK, std::future_status::timeout, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE}}}; + const auto & exp = expected.at(test_param.strictness); + + // Test Case 9: restart following controllers but preceding controllers will not be restart + // --> return error; restart will not be executed and controllers stay in the same state as they + // were + { + const auto verify_all_controllers_are_active_and_not_restart = [&]() + { + // All controllers should still be active + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + position_tracking_controller->get_state().id()); + + // All controllers should not have their activate and deactivate calls incremented + ASSERT_EQ(2u, pid_left_wheel_controller->activate_calls); + ASSERT_EQ(2u, pid_right_wheel_controller->activate_calls); + ASSERT_EQ(2u, diff_drive_controller->activate_calls); + ASSERT_EQ(1u, position_tracking_controller->activate_calls); + ASSERT_EQ(1u, pid_left_wheel_controller->deactivate_calls); + ASSERT_EQ(1u, pid_right_wheel_controller->deactivate_calls); + ASSERT_EQ(1u, diff_drive_controller->deactivate_calls); + ASSERT_EQ(0u, position_tracking_controller->deactivate_calls); + + // All controllers chained mode should not be changed + ASSERT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); + EXPECT_FALSE(position_tracking_controller->is_in_chained_mode()); + }; + verify_all_controllers_are_active_and_not_restart(); + + // Attempt to restart the lowest following controllers (pid_controllers) + RestartControllers( + {PID_LEFT_WHEEL, PID_RIGHT_WHEEL}, exp.return_type, std::future_status::ready); + verify_all_controllers_are_active_and_not_restart(); + + // Attempt to restart the one of the lowest following controller (pid_left_wheel_controller) + RestartControllers({PID_LEFT_WHEEL}, exp.return_type, std::future_status::ready); + verify_all_controllers_are_active_and_not_restart(); + + // Attempt to restart another lowest following controller (pid_right_wheel_controller) + RestartControllers({PID_RIGHT_WHEEL}, exp.return_type, std::future_status::ready); + verify_all_controllers_are_active_and_not_restart(); + + // Attempt to restart the middle following controller (diff_drive_controller) + RestartControllers({DIFF_DRIVE_CONTROLLER}, exp.return_type, std::future_status::ready); + verify_all_controllers_are_active_and_not_restart(); + + // Attempt to restart the middle following and one of the lowest following controller + RestartControllers( + {DIFF_DRIVE_CONTROLLER, PID_LEFT_WHEEL}, exp.return_type, std::future_status::ready); + verify_all_controllers_are_active_and_not_restart(); + + // Attempt to restart the middle following and another lowest following controller + RestartControllers( + {DIFF_DRIVE_CONTROLLER, PID_LEFT_WHEEL}, exp.return_type, std::future_status::ready); + verify_all_controllers_are_active_and_not_restart(); + + // Attempt to restart the all following controllers (pid_controllers and diff_drive_controller) + RestartControllers( + {DIFF_DRIVE_CONTROLLER, PID_LEFT_WHEEL, PID_RIGHT_WHEEL}, exp.return_type, + std::future_status::ready); + verify_all_controllers_are_active_and_not_restart(); + } +} + +TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_restart_random_order) +{ + SetupWithActivationAllControllersAndCheck(); + + { + // Restart all controllers + RestartControllers( + {PID_RIGHT_WHEEL, DIFF_DRIVE_CONTROLLER, POSITION_TRACKING_CONTROLLER, PID_LEFT_WHEEL}); + + // Verify that the claimed state of the interface does not change before and after the restart + check_command_interfaces_claimed( + concat_vector( + POSITION_CONTROLLER_CLAIMED_INTERFACES, DIFF_DRIVE_CLAIMED_INTERFACES, + PID_LEFT_WHEEL_CLAIMED_INTERFACES, PID_RIGHT_WHEEL_CLAIMED_INTERFACES), + POSITION_CONTROLLER_NOT_CLAIMED_INTERFACES); + + // Verify that the activate and deactivate calls of all controllers are incremented + EXPECT_EQ(3u, pid_left_wheel_controller->activate_calls); + EXPECT_EQ(3u, pid_right_wheel_controller->activate_calls); + EXPECT_EQ(3u, diff_drive_controller->activate_calls); + EXPECT_EQ(2u, position_tracking_controller->activate_calls); + EXPECT_EQ(2u, pid_left_wheel_controller->deactivate_calls); + EXPECT_EQ(2u, pid_right_wheel_controller->deactivate_calls); + EXPECT_EQ(2u, diff_drive_controller->deactivate_calls); + EXPECT_EQ(1u, position_tracking_controller->deactivate_calls); + + // Verify that the controllers are still in the same chained mode + ASSERT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); + EXPECT_FALSE(position_tracking_controller->is_in_chained_mode()); + } + + { + // Restart middle preceding/following controller (diff_drive_controller) and stop the most + // preceding controller (position_tracking_controller) simultaneously + switch_test_controllers( + {DIFF_DRIVE_CONTROLLER}, {POSITION_TRACKING_CONTROLLER, DIFF_DRIVE_CONTROLLER}, + test_param.strictness, std::future_status::timeout, controller_interface::return_type::OK); + + // Verify that the reference_interface is not claimed because diff_drive_controller is no + // longer the following controller + check_command_interfaces_claimed( + concat_vector( + DIFF_DRIVE_CLAIMED_INTERFACES, PID_LEFT_WHEEL_CLAIMED_INTERFACES, + PID_RIGHT_WHEEL_CLAIMED_INTERFACES), + concat_vector( + POSITION_CONTROLLER_NOT_CLAIMED_INTERFACES, POSITION_CONTROLLER_CLAIMED_INTERFACES)); + + // Verify that the activate and deactivate calls for diff_drive_controller are incremented, + // and only the deactivate count for position_tracking_controller is incremented. + ASSERT_EQ(3u, pid_left_wheel_controller->activate_calls); // +0 + ASSERT_EQ(3u, pid_right_wheel_controller->activate_calls); // +0 + ASSERT_EQ(4u, diff_drive_controller->activate_calls); // +1 (restart) + ASSERT_EQ(2u, position_tracking_controller->activate_calls); // +0 + ASSERT_EQ(2u, pid_left_wheel_controller->deactivate_calls); // +0 + ASSERT_EQ(2u, pid_right_wheel_controller->deactivate_calls); // +0 + ASSERT_EQ(3u, diff_drive_controller->deactivate_calls); // +1 (restart) + ASSERT_EQ(2u, position_tracking_controller->deactivate_calls); // +1 (deactivate) + + // Verify that the diff_drive_controller is no longer in chained mode + ASSERT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + EXPECT_FALSE(position_tracking_controller->is_in_chained_mode()); + } + + { + // Restart the preceding controller (diff_drive_controller) and a following controller + // (pid_right_wheel_controller) + RestartControllers({PID_RIGHT_WHEEL, DIFF_DRIVE_CONTROLLER}); + + // Verify that the claimed state of the interface does not change before and after the restart + check_command_interfaces_claimed( + concat_vector( + DIFF_DRIVE_CLAIMED_INTERFACES, PID_LEFT_WHEEL_CLAIMED_INTERFACES, + PID_RIGHT_WHEEL_CLAIMED_INTERFACES), + concat_vector( + POSITION_CONTROLLER_NOT_CLAIMED_INTERFACES, POSITION_CONTROLLER_CLAIMED_INTERFACES)); + + // Verify that only the restart controller has its activate and deactivate calls incremented. + ASSERT_EQ(3u, pid_left_wheel_controller->activate_calls); // +0 + ASSERT_EQ(4u, pid_right_wheel_controller->activate_calls); // +1 (restart) + ASSERT_EQ(5u, diff_drive_controller->activate_calls); // +1 (restart) + ASSERT_EQ(2u, position_tracking_controller->activate_calls); // +0 + ASSERT_EQ(2u, pid_left_wheel_controller->deactivate_calls); // +0 + ASSERT_EQ(3u, pid_right_wheel_controller->deactivate_calls); // +1 (restart) + ASSERT_EQ(4u, diff_drive_controller->deactivate_calls); // +1 (restart) + ASSERT_EQ(2u, position_tracking_controller->deactivate_calls); // +0 + + // Verify that the controllers are still in the same chained mode + ASSERT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + EXPECT_FALSE(position_tracking_controller->is_in_chained_mode()); + } + + { + // Restart middle preceding/following controller (diff_drive_controller) and start the most + // preceding controller (position_tracking_controller) simultaneously + switch_test_controllers( + {DIFF_DRIVE_CONTROLLER, POSITION_TRACKING_CONTROLLER}, {DIFF_DRIVE_CONTROLLER}, + test_param.strictness, std::future_status::timeout, controller_interface::return_type::OK); + + // Verify that the reference_interface is claimed because diff_drive_controller becomes the + // following controller + check_command_interfaces_claimed( + concat_vector( + POSITION_CONTROLLER_CLAIMED_INTERFACES, DIFF_DRIVE_CLAIMED_INTERFACES, + PID_LEFT_WHEEL_CLAIMED_INTERFACES, PID_RIGHT_WHEEL_CLAIMED_INTERFACES), + POSITION_CONTROLLER_NOT_CLAIMED_INTERFACES); + + // Verify that the activate and deactivate calls for diff_drive_controller are incremented, + // and only the deactivate count for position_tracking_controller is incremented. + ASSERT_EQ(3u, pid_left_wheel_controller->activate_calls); // +0 + ASSERT_EQ(4u, pid_right_wheel_controller->activate_calls); // +0 + ASSERT_EQ(6u, diff_drive_controller->activate_calls); // +1 (restart) + ASSERT_EQ(3u, position_tracking_controller->activate_calls); // +1 (activate) + ASSERT_EQ(2u, pid_left_wheel_controller->deactivate_calls); // +0 + ASSERT_EQ(3u, pid_right_wheel_controller->deactivate_calls); // +0 + ASSERT_EQ(5u, diff_drive_controller->deactivate_calls); // +1 (restart) + ASSERT_EQ(2u, position_tracking_controller->deactivate_calls); // +0 + + // Verify that the diff_drive_controller is in chained mode + ASSERT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); + EXPECT_FALSE(position_tracking_controller->is_in_chained_mode()); + } +} + INSTANTIATE_TEST_SUITE_P( test_strict_best_effort, TestControllerChainingWithControllerManager, testing::Values(strict, best_effort)); diff --git a/controller_manager/test/test_restart_controller.cpp b/controller_manager/test/test_restart_controller.cpp new file mode 100644 index 0000000000..529d7ad33a --- /dev/null +++ b/controller_manager/test/test_restart_controller.cpp @@ -0,0 +1,274 @@ +// Copyright 2024 Tokyo Robotics Inc. +// +// 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. + +#include +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "controller_manager/controller_manager.hpp" +#include "controller_manager_test_common.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "test_controller_with_command/test_controller_with_command.hpp" + +using test_controller_with_command::SIMULATE_COMMAND_ACTIVATE_VALUE; +using test_controller_with_command::SIMULATE_COMMAND_DEACTIVATE_VALUE; +using test_controller_with_command::TEST_CONTROLLER_CLASS_NAME; +using test_controller_with_command::TestControllerWithCommand; +const auto CONTROLLER_NAME = "test_controller1"; +using strvec = std::vector; + +namespace +{ +struct TestRestControllerStrictness +{ + int strictness = STRICT; + std::future_status expected_future_status; + controller_interface::return_type expected_return; +}; +TestRestControllerStrictness test_strict{ + STRICT, std::future_status::ready, controller_interface::return_type::ERROR}; +TestRestControllerStrictness test_best_effort{ + BEST_EFFORT, std::future_status::timeout, controller_interface::return_type::OK}; +} // namespace + +class TestRestartController +: public ControllerManagerFixture, + public testing::WithParamInterface +{ +public: + void SetUp() override + { + SetupController(); + run_updater_ = false; + } + + void SetupController() + { + // load controller + test_controller = std::make_shared(); + cm_->add_controller(test_controller, CONTROLLER_NAME, TEST_CONTROLLER_CLASS_NAME); + + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2u, test_controller.use_count()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + } + + void configure_and_check_test_controller() + { + // configure controller + cm_->configure_controller(CONTROLLER_NAME); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + + // validate initial states + EXPECT_EQ(0u, test_controller->activate_calls); + EXPECT_EQ(0u, test_controller->deactivate_calls); + EXPECT_TRUE(std::isnan(test_controller->simulate_command)); + } + + void start_test_controller( + const int strictness, + const std::future_status expected_future_status = std::future_status::timeout, + const controller_interface::return_type expected_interface_status = + controller_interface::return_type::OK) + { + switch_test_controllers( + strvec{CONTROLLER_NAME}, strvec{}, strictness, expected_future_status, + expected_interface_status); + } + + void start_and_check_test_controller( + const int strictness, + const std::future_status expected_future_status = std::future_status::timeout, + const controller_interface::return_type expected_interface_status = + controller_interface::return_type::OK) + { + // Start controller + start_test_controller(strictness, expected_future_status, expected_interface_status); + + // State should be active + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); + + // Only activate counter should be incremented + ASSERT_EQ(1u, test_controller->activate_calls); + ASSERT_EQ(0u, test_controller->deactivate_calls); + + // Controller command should be restart to ACTIVATE_VALUE + ASSERT_EQ(SIMULATE_COMMAND_ACTIVATE_VALUE, test_controller->simulate_command); + } + + void stop_test_controller( + const int strictness, + const std::future_status expected_future_status = std::future_status::timeout, + const controller_interface::return_type expected_interface_status = + controller_interface::return_type::OK) + { + switch_test_controllers( + strvec{}, strvec{CONTROLLER_NAME}, strictness, expected_future_status, + expected_interface_status); + } + + void restart_test_controller( + const int strictness, + const std::future_status expected_future_status = std::future_status::timeout, + const controller_interface::return_type expected_interface_status = + controller_interface::return_type::OK) + { + switch_test_controllers( + strvec{CONTROLLER_NAME}, strvec{CONTROLLER_NAME}, strictness, expected_future_status, + expected_interface_status); + } + + std::shared_ptr test_controller; +}; + +TEST_P(TestRestartController, starting_and_stopping_a_controller) +{ + const auto test_param = GetParam(); + + // Configure controller + configure_and_check_test_controller(); + + { + // Start controller + start_and_check_test_controller(test_param.strictness); + } + + { + // Stop controller + stop_test_controller(test_param.strictness); + + // State should be inactive + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + + // Only deactivate counter should be incremented + ASSERT_EQ(1u, test_controller->activate_calls); + ASSERT_EQ(1u, test_controller->deactivate_calls); + + // Controller command should be restart to DEACTIVATE_VALUE + ASSERT_EQ(SIMULATE_COMMAND_DEACTIVATE_VALUE, test_controller->simulate_command); + } +} + +TEST_P(TestRestartController, can_restart_active_controller) +{ + const auto test_param = GetParam(); + + // Configure controller + configure_and_check_test_controller(); + + { + // Start controller before restart + start_and_check_test_controller(test_param.strictness); + const double OVERRIDE_COMMAND_VALUE = 12121212.0; + + // Override command to check restart behavior + test_controller->simulate_command = OVERRIDE_COMMAND_VALUE; + ASSERT_EQ(OVERRIDE_COMMAND_VALUE, test_controller->simulate_command); + } + + { + // Restart controller + restart_test_controller(test_param.strictness); + + // State should be return active immediately (active -> inactive -> active) + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); + + // Both activate counter and deactivate counter are incremented + ASSERT_EQ(2u, test_controller->activate_calls); + ASSERT_EQ(1u, test_controller->deactivate_calls); + + // Controller command should be restart to ACTIVATE_VALUE + ASSERT_EQ(SIMULATE_COMMAND_ACTIVATE_VALUE, test_controller->simulate_command); + } +} + +TEST_P(TestRestartController, restart_inactive_controller) +{ + const auto test_param = GetParam(); + + // Configure controller + configure_and_check_test_controller(); + + // Restart controller without starting it + restart_test_controller( + test_param.strictness, test_param.expected_future_status, test_param.expected_return); + + // STRICT: can not restart inactive controller + if (test_param.strictness == STRICT) + { + // State should not be changed + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + + // Both activate counter and deactivate counter should not be incremented + ASSERT_EQ(0u, test_controller->activate_calls); + ASSERT_EQ(0u, test_controller->deactivate_calls); + } + + // BEST_EFFORT: If restart is executed while inactive, only the start_controller process will be + // effective, resulting in activation + if (test_param.strictness == BEST_EFFORT) + { + // State changed to active + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); + + // Only activate counter and deactivate counter should not be incremented + ASSERT_EQ(1u, test_controller->activate_calls); + ASSERT_EQ(0u, test_controller->deactivate_calls); + + // Controller command should be restart to ACTIVATE_VALUE + ASSERT_EQ(SIMULATE_COMMAND_ACTIVATE_VALUE, test_controller->simulate_command); + } +} + +TEST_P(TestRestartController, can_not_restart_unconfigured_controller) +{ + const auto test_param = GetParam(); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + + // Can not restart unconfigured controller + restart_test_controller( + test_param.strictness, std::future_status::ready, test_param.expected_return); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); +} + +TEST_P(TestRestartController, can_not_restart_finalized_controller) +{ + const auto test_param = GetParam(); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + + // Shutdown controller on purpose for testing + ASSERT_EQ( + test_controller->get_node()->shutdown().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + + // Can not restart finalized controller + restart_test_controller( + test_param.strictness, std::future_status::ready, test_param.expected_return); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, test_controller->get_state().id()); +} + +INSTANTIATE_TEST_SUITE_P( + test_strict_best_effort, TestRestartController, testing::Values(test_strict, test_best_effort));