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..174db79092 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -340,73 +340,81 @@ class ControllerManager : public rclcpp::Node */ void clear_requests(); - /** - * If a controller is deactivated all following controllers (if any exist) should be switched - * 'from' the chained mode. - * - * \param[in] controllers list with controllers. - */ - void propagate_deactivation_of_chained_mode(const std::vector & controllers); + void extract_de_activate_command_interface_request( + const std::vector & controllers, + const std::vector & deactivate_request, + const std::vector & activate_request, + std::vector & deactivate_command_interface_request, + std::vector & activate_command_interface_request) const; + + void cache_controller_interfaces_in_activate_request( + const std::vector & controllers, + const std::vector & activate_request); - /// Check if all the following controllers will be in active state and in the chained mode - /// after controllers' switch. /** - * Check recursively that all following controllers of the @controller_it - * - are already active, - * - will not be deactivated, - * - or will be activated. - * The following controllers are added to the request to switch in the chained mode or removed - * from the request to switch from the chained mode. + * @brief Check for conflicts with the (de)activate request. If conflicts are detected, handle + * them by either raising an error (STRICT) or removing the conflicting controller from the + * request (BEST_EFFORT). * - * For each controller the whole chain of following controllers is checked. + * NOTE: The input argument controllers must be sorted in the order of preceding and following. + * NOTE: strictness type "MANIPULATE_CONTROLLERS_CHAIN" is not implemented yet. * - * NOTE: The automatically adding of following controller into activate list is not implemented - * yet. - * - * \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. - * \param[in] controller_it iterator to the controller for which the following controllers are - * checked. + * \param[in] controllers list with controllers. + * \param[in] controllers_map map with controllers. + * \param[in,out] deactivate_request A list of controller names requested for deactivation. If the + * strictness value is equal to "BEST_EFFORT", conflicting controllers found will be erased from + * the list. + * \param[in,out] activate_request A list of controller names requested for activation. If the + * strictness value is equal to "BEST_EFFORT", conflicting controllers found will be erased from + * the list. * - * \returns return_type::OK if all following controllers pass the checks, otherwise + * \returns return_type::OK if all (de)activate requests pass the checks, otherwise * return_type::ERROR. */ - controller_interface::return_type check_following_controllers_for_activate( - const std::vector & controllers, int strictness, - const ControllersListIterator controller_it); + controller_interface::return_type check_controllers_for_de_activate( + const int strictness, const std::vector & controllers, + const controller_manager::ControllersMap & controllers_map, + std::vector & deactivate_request, + std::vector & activate_request) const; - /// Check if all the preceding controllers will be in inactive state after controllers' switch. /** - * Check that all preceding controllers of the @controller_it - * - are inactive, - * - will be deactivated, - * - and will not be activated. + * @brief Determine if a change chained mode is necessary for the following controllers based on + * the request, and add them to the chained mode request if necessary. Additionally, if only + * preceding performs the activation state transition, a restart of the following controller + * is necessary to switch the chained mode, so add them to the (de)activate request. * - * NOTE: The automatically adding of preceding controllers into deactivate list is not implemented - * yet. + * NOTE: The input argument controllers must be sorted in the order of preceding and following. * * \param[in] controllers list with controllers. - * \param[in] strictness if value is equal "MANIPULATE_CONTROLLERS_CHAIN" then all preceding - * controllers will be automatically added to the deactivate request list. - * \param[in] controller_it iterator to the controller for which the preceding controllers are - * checked. - * - * \returns return_type::OK if all preceding controllers pass the checks, otherwise - * return_type::ERROR. + * \param[in] controllers_map map with controllers. + * \param[in,out] deactivate_request A list of controller names requested for deactivation. If a + * following controller requiring a restart due to chained mode is found, it will be automatically + * added to the list. + * \param[in,out] activate_request A list of controller names requested for activation. If a + * following controller requiring a restart due to chained mode is found, it will be automatically + * added to the list. + * \param[out] from_chained_mode_request A list containing the names of following controllers that + * require switching from chained mode to non-chained mode, based on the requested (de)activation. + * \param[out] to_chained_mode_request A list containing the names of following controllers that + * require switching from non-chained mode to chained mode, based on the requested (de)activation. */ - controller_interface::return_type check_preceeding_controllers_for_deactivate( - const std::vector & controllers, int strictness, - const ControllersListIterator controller_it); + void create_from_to_chained_mode_request( + const std::vector & controllers, + const controller_manager::ControllersMap & controllers_map, + std::vector & deactivate_request, std::vector & activate_request, + std::vector & from_chained_mode_request, + std::vector & to_chained_mode_request) const; /** * @brief Inserts a controller into an ordered list based on dependencies to compute the * controller chain. * * This method computes the controller chain by inserting the provided controller name into an - * ordered list of controllers based on dependencies. It ensures that controllers are inserted in - * the correct order so that dependencies are satisfied. + * ordered list of controllers based on dependencies. It ensures that controllers are inserted + * in the correct order so that dependencies are satisfied. * * @param ctrl_name The name of the controller to be inserted into the chain. * @param controller_iterator An iterator pointing to the position in the ordered list where the diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index d13e9c56bd..6168ebe998 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include "controller_interface/controller_interface.hpp" #include "hardware_interface/controller_info.hpp" @@ -46,5 +47,10 @@ struct ControllerChainSpec std::vector following_controllers; std::vector preceding_controllers; }; + +using ControllersMap = std::unordered_map; +using ControllerChainSpecsMap = + std::unordered_map; + } // namespace controller_manager #endif // CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 3b84fc07e4..a4e306c5f3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -17,10 +17,12 @@ #include #include #include +#include #include #include #include "controller_interface/controller_interface_base.hpp" +#include "controller_manager/controller_spec.hpp" #include "controller_manager_msgs/msg/hardware_component_state.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" @@ -83,6 +85,17 @@ bool controller_name_compare(const controller_manager::ControllerSpec & a, const return a.info.name == name; } +bool handle_conflict(const int strictness, const rclcpp::Logger & logger, const std::string & msg) +{ + if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + { + RCLCPP_ERROR(logger, "%s", msg.c_str()); + return false; + } + RCLCPP_WARN(logger, "%s", msg.c_str()); + return true; +}; + /// Checks if a command interface belongs to a controller based on its prefix. /** * A command interface can be provided by a controller in which case is called "reference" @@ -153,6 +166,40 @@ void remove_element_from_list(std::vector & list, const T & element) } } +template +bool is_element_in_list(const std::vector & list, const T & element) +{ + return std::find(list.begin(), list.end(), element) != list.end(); +} + +template +void sort_list_by_another( + std::vector & sorting_list, const std::vector & ordered_list, + const bool reverse_order = false) +{ + // Create index map of the ordered list + std::unordered_map index_map; + for (size_t i = 0; i < ordered_list.size(); ++i) + { + index_map[ordered_list[i]] = i; + } + + // sort the sorting list based on the index map order + std::sort( + sorting_list.begin(), sorting_list.end(), + [&index_map, reverse_order](const std::string & a, const std::string & b) + { + if (reverse_order) + { + return index_map[a] > index_map[b]; + } + else + { + return index_map[a] < index_map[b]; + } + }); +} + void controller_chain_spec_cleanup( std::unordered_map & ctrl_chain_spec, const std::string & controller) @@ -170,6 +217,302 @@ void controller_chain_spec_cleanup( ctrl_chain_spec.erase(controller); } +struct ActivationState +{ + bool is_active; + bool is_inactive; + bool in_deactivate_list; + bool in_activate_list; +}; + +ActivationState get_activation_state( + const controller_manager::ControllerSpec & controller, + const std::vector & deactivate_request, + const std::vector & activate_request) +{ + ActivationState state; + + state.is_active = is_controller_active(controller.c); + state.is_inactive = is_controller_inactive(controller.c); + state.in_deactivate_list = is_element_in_list(deactivate_request, controller.info.name); + state.in_activate_list = is_element_in_list(activate_request, controller.info.name); + + return state; +} + +controller_manager::ControllersMap create_controllers_map( + const std::vector & controllers) +{ + controller_manager::ControllersMap controllers_map; + for (const auto & controller : controllers) + { + controllers_map.insert({controller.info.name, controller}); + } + return controllers_map; +} + +bool can_controller_activate( + const rclcpp::Logger & logger, const std::string & controller_name, + const controller_manager::ControllersMap & controllers_map, + const controller_manager::ControllerChainSpecsMap & chain_specs_map, + const std::vector & deactivate_request, + const std::vector & activate_request) +{ + const auto & chain_spec = chain_specs_map.at(controller_name); + + for (const auto & following_controller_name : chain_spec.following_controllers) + { + const auto & following_controller = controllers_map.at(following_controller_name); + + const auto following_state = + get_activation_state(following_controller, deactivate_request, activate_request); + + const bool following_will_become_inactive = + !following_state.in_activate_list && + (following_state.is_inactive || + (following_state.is_active && following_state.in_deactivate_list)); + + // if the following controller will become inactive after the switch controller, + // whether as a result of any state transition or without a transition, it is not possible to + // activate the preceding controller + if (following_will_become_inactive) + { + RCLCPP_WARN( + logger, + "Controller '%s' cannot be activated because following controller with name '%s' will " + "become inactive after switch", + controller_name.c_str(), following_controller_name.c_str()); + return false; + } + + // if the following controller is not chainable, it is not possible to activate the preceding + if (!following_controller.c->is_chainable()) + { + RCLCPP_WARN( + logger, + "Controller '%s' cannot be activated because following controller with name '%s' is not " + "chainable", + controller_name.c_str(), following_controller_name.c_str()); + return false; + } + + // recursively check the next following controller + if (!can_controller_activate( + logger, following_controller_name, controllers_map, chain_specs_map, deactivate_request, + activate_request)) + { + return false; + } + } + return true; +} + +bool can_controller_deactivate( + const rclcpp::Logger & logger, const std::string & controller_name, + const controller_manager::ControllersMap & controllers_map, + const controller_manager::ControllerChainSpecsMap & chain_specs_map, + const std::vector & deactivate_request, + const std::vector & activate_request) +{ + const auto & chain_spec = chain_specs_map.at(controller_name); + + for (const auto & preceding_controller_name : chain_spec.preceding_controllers) + { + const auto & preceding_controller = controllers_map.at(preceding_controller_name); + + const auto preceding_state = + get_activation_state(preceding_controller, deactivate_request, activate_request); + + const bool preceding_will_become_active = + (preceding_state.is_inactive && preceding_state.in_activate_list) || + (preceding_state.is_active && + (!preceding_state.in_deactivate_list || preceding_state.in_activate_list)); + + // if the preceding controller will become active after the switch controller, + // whether as a result of any state transition or without a transition, it is not possible to + // deactivate the following controller + if (preceding_will_become_active) + { + RCLCPP_WARN( + logger, + "Controller '%s' cannot be deactivated because preceding controller with name '%s' will " + "become active after switch", + controller_name.c_str(), preceding_controller_name.c_str()); + return false; + } + + // recursively check the next preceding controller + if (!can_controller_deactivate( + logger, preceding_controller_name, controllers_map, chain_specs_map, deactivate_request, + activate_request)) + { + return false; + } + } + return true; +} + +bool can_controller_restart( + const rclcpp::Logger & logger, const std::string & controller_name, + const controller_manager::ControllersMap & controllers_map, + const controller_manager::ControllerChainSpecsMap & chain_specs_map, + const std::vector & deactivate_request, + const std::vector & activate_request) +{ + const auto & chain_spec = chain_specs_map.at(controller_name); + + for (const auto & preceding_controller_name : chain_spec.preceding_controllers) + { + const auto & preceding_controller = controllers_map.at(preceding_controller_name); + + const auto preceding_state = + get_activation_state(preceding_controller, deactivate_request, activate_request); + + const bool preceding_will_be_restarted = preceding_state.is_active && + preceding_state.in_deactivate_list && + preceding_state.in_activate_list; + + const bool preceding_stay_active = + (preceding_state.is_active && !preceding_state.in_deactivate_list); + + // if the preceding will not be restarted or stay active without changing its state, the + // following cannot be restarted + if (!preceding_will_be_restarted && preceding_stay_active) + { + RCLCPP_WARN( + logger, + "Controller '%s' cannot be restarted because preceding controller with name '%s' will " + "not be restarted or stay active during switch", + controller_name.c_str(), preceding_controller_name.c_str()); + return false; + } + + // recursively check the next preceding controller + if (!can_controller_restart( + logger, preceding_controller_name, controllers_map, chain_specs_map, deactivate_request, + activate_request)) + { + return false; + } + } + + return true; +} + +controller_interface::return_type check_controller_for_de_activate( + const rclcpp::Logger & logger, const int strictness, + const controller_manager::ControllerSpec & controller, + std::vector & deactivate_request, std::vector & activate_request) +{ + auto ctrl_state = get_activation_state(controller, deactivate_request, activate_request); + + // check for double stop + if (!ctrl_state.is_active && ctrl_state.in_deactivate_list) + { + if (!handle_conflict( + strictness, logger, + "Could not deactivate controller '" + controller.info.name + "' since it is not active")) + { + return controller_interface::return_type::ERROR; + } + remove_element_from_list(deactivate_request, controller.info.name); + ctrl_state.in_deactivate_list = false; + } + + // check for double activation + if (ctrl_state.is_active && !ctrl_state.in_deactivate_list && ctrl_state.in_activate_list) + { + if (!handle_conflict( + strictness, logger, + "Could not activate controller '" + controller.info.name + + "' since it is already active")) + { + return controller_interface::return_type::ERROR; + } + remove_element_from_list(activate_request, controller.info.name); + ctrl_state.in_activate_list = false; + } + + // check for illegal activation of an unconfigured/finalized controller + if (!ctrl_state.is_inactive && !ctrl_state.in_deactivate_list && ctrl_state.in_activate_list) + { + if (!handle_conflict( + strictness, logger, + "Could not activate controller '" + controller.info.name + + "' since it is not in inactive state")) + { + return controller_interface::return_type::ERROR; + } + remove_element_from_list(activate_request, controller.info.name); + ctrl_state.in_activate_list = false; + } + + return controller_interface::return_type::OK; +} + +void update_chained_mode_request_of_following( + const rclcpp::Logger & logger, const controller_manager::ControllerSpec & following_ctrl, + const ActivationState & preceding_state, const ActivationState & following_state, + std::vector & deactivate_request, std::vector & activate_request, + std::vector & from_chained_mode_request, + std::vector & to_chained_mode_request, + hardware_interface::ResourceManager & resource_manager) +{ + bool in_to_chained_mode_list = false; + bool in_from_chained_mode_list = false; + + const bool preceding_will_be_restarted = + preceding_state.in_deactivate_list && preceding_state.in_activate_list; + const bool following_will_be_restarted = + following_state.in_deactivate_list && following_state.in_activate_list; + const bool both_will_be_restarted = preceding_will_be_restarted && following_will_be_restarted; + + // if the preceding will be deactivated and will not be restarted, or if both the + // preceding and following will be restarted, add the following to the 'from' chained + // mode request + if ( + (preceding_state.in_deactivate_list && !preceding_will_be_restarted) || both_will_be_restarted) + { + add_element_to_list(from_chained_mode_request, following_ctrl.info.name); + in_from_chained_mode_list = true; + } + + // if the preceding will be activated and will not be restarted, or if both the + // preceding and following will be restarted, add the following to the 'to' chained + // mode request + if ((preceding_state.in_activate_list && !preceding_will_be_restarted) || both_will_be_restarted) + { + add_element_to_list(to_chained_mode_request, following_ctrl.info.name); + in_to_chained_mode_list = true; + + // if it is a chainable controller, make the reference interfaces available on + // preactivation (This is needed when you activate a couple of chainable controller + // altogether) + if (!following_ctrl.c->is_in_chained_mode()) + { + resource_manager.make_controller_reference_interfaces_available(following_ctrl.info.name); + } + } + + // if a change to the chained_mode has been requested and the following is active and will not + // be deactivated, the following will require a restart, so it should be added to + // the request + if (in_from_chained_mode_list || in_to_chained_mode_list) + { + if (following_state.is_active && !following_state.in_deactivate_list) + { + RCLCPP_DEBUG( + logger, + "Adding following controller with name '%s' to deactivate and activate request for " + "restart.", + following_ctrl.info.name.c_str()); + + add_element_to_list(deactivate_request, following_ctrl.info.name); + add_element_to_list(activate_request, following_ctrl.info.name); + }; + } +} + } // namespace namespace controller_manager @@ -848,8 +1191,13 @@ controller_interface::return_type ControllerManager::switch_controller( const std::vector & deactivate_controllers, int strictness, bool activate_asap, const rclcpp::Duration & timeout) { + // NOTE: without the following lambda that captures "strictness", ABI compatibility will be lost + auto dummy_lambda_to_maintain_ABI_compatibility = [&strictness]() {}; + (void)(dummy_lambda_to_maintain_ABI_compatibility); + switch_params_ = SwitchParams(); + // check (de)activate request and internal state before switch if (!deactivate_request_.empty() || !activate_request_.empty()) { RCLCPP_FATAL( @@ -863,7 +1211,7 @@ controller_interface::return_type ControllerManager::switch_controller( { RCLCPP_FATAL( get_logger(), - "The internal deactivate and activat requests command interface lists are not empty at the " + "The internal deactivate and activate requests command interface lists are not empty at the " "switch_controller() call. This should never happen."); throw std::runtime_error("CM's internal state is not correct. See the FATAL message above."); } @@ -945,7 +1293,7 @@ controller_interface::return_type ControllerManager::switch_controller( auto ret = list_controllers(deactivate_controllers, deactivate_request_, "deactivate"); if (ret != controller_interface::return_type::OK) { - deactivate_request_.clear(); + clear_requests(); return ret; } @@ -953,8 +1301,7 @@ controller_interface::return_type ControllerManager::switch_controller( ret = list_controllers(activate_controllers, activate_request_, "activate"); if (ret != controller_interface::return_type::OK) { - deactivate_request_.clear(); - activate_request_.clear(); + clear_requests(); return ret; } @@ -963,276 +1310,56 @@ 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); + // create a map of controllers for easy access for the following processing + const auto controllers_map = create_controllers_map(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) + // Check for conflicts with the (de)activate request. If conflicts are detected, handle them by + // either raising an error (STRICT) or removing the conflicting controller from the (de)activate + // request (BEST_EFFORT). + if ( + check_controllers_for_de_activate( + strictness, controllers, controllers_map, deactivate_request_, activate_request_) != + controller_interface::return_type::OK) { - 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 inactive then do not do any following-controllers checks - if (!is_controller_inactive(controller_it->c)) - { - 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); - --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; - } - } + RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)"); + // reset all lists + clear_requests(); + return controller_interface::return_type::ERROR; } - // 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_preceeding_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); - --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; - } - } - } + // if the (de)activate request necessitates switching the chained mode of the + // following controller, create the corresponding 'from'/'to' chained mode + // request. + create_from_to_chained_mode_request( + controllers, controllers_map, deactivate_request_, activate_request_, + from_chained_mode_request_, to_chained_mode_request_); - for (const auto & controller : controllers) + // check if we need to request switch controllers after all the checks + if (activate_request_.empty() && deactivate_request_.empty()) { - auto to_chained_mode_list_it = std::find( - to_chained_mode_request_.begin(), to_chained_mode_request_.end(), controller.info.name); - bool in_to_chained_mode_list = to_chained_mode_list_it != to_chained_mode_request_.end(); - - auto from_chained_mode_list_it = std::find( - from_chained_mode_request_.begin(), from_chained_mode_request_.end(), controller.info.name); - bool in_from_chained_mode_list = from_chained_mode_list_it != from_chained_mode_request_.end(); - - auto deactivate_list_it = - std::find(deactivate_request_.begin(), deactivate_request_.end(), controller.info.name); - bool in_deactivate_list = deactivate_list_it != deactivate_request_.end(); - - const bool is_active = is_controller_active(*controller.c); - const bool is_inactive = is_controller_inactive(*controller.c); - - // restart controllers that need to switch their 'chained mode' - add to (de)activate lists - if (in_to_chained_mode_list || in_from_chained_mode_list) - { - if (is_active && !in_deactivate_list) - { - deactivate_request_.push_back(controller.info.name); - activate_request_.push_back(controller.info.name); - } - } - - // get pointers to places in deactivate and activate lists ((de)activate lists have changed) - deactivate_list_it = - std::find(deactivate_request_.begin(), deactivate_request_.end(), controller.info.name); - in_deactivate_list = deactivate_list_it != deactivate_request_.end(); - - auto activate_list_it = - std::find(activate_request_.begin(), activate_request_.end(), controller.info.name); - bool in_activate_list = activate_list_it != activate_request_.end(); - - auto handle_conflict = [&](const std::string & msg) - { - if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) - { - RCLCPP_ERROR(get_logger(), "%s", msg.c_str()); - deactivate_request_.clear(); - deactivate_command_interface_request_.clear(); - activate_request_.clear(); - activate_command_interface_request_.clear(); - to_chained_mode_request_.clear(); - from_chained_mode_request_.clear(); - return controller_interface::return_type::ERROR; - } - RCLCPP_WARN(get_logger(), "%s", msg.c_str()); - return controller_interface::return_type::OK; - }; - - // check for double stop - if (!is_active && in_deactivate_list) - { - auto conflict_status = handle_conflict( - "Could not deactivate controller '" + controller.info.name + "' since it is not active"); - if (conflict_status != controller_interface::return_type::OK) - { - return conflict_status; - } - in_deactivate_list = false; - deactivate_request_.erase(deactivate_list_it); - } - - // check for doubled activation - if (is_active && !in_deactivate_list && in_activate_list) - { - auto conflict_status = handle_conflict( - "Could not activate controller '" + controller.info.name + "' since it is already active"); - if (conflict_status != controller_interface::return_type::OK) - { - return conflict_status; - } - in_activate_list = false; - activate_request_.erase(activate_list_it); - } - - // check for illegal activation of an unconfigured/finalized controller - if (!is_inactive && !in_deactivate_list && in_activate_list) - { - auto conflict_status = handle_conflict( - "Could not activate controller '" + controller.info.name + - "' since it is not in inactive state"); - if (conflict_status != controller_interface::return_type::OK) - { - return conflict_status; - } - in_activate_list = false; - activate_request_.erase(activate_list_it); - } - - const auto extract_interfaces_for_controller = - [this](const ControllerSpec ctrl, std::vector & request_interface_list) - { - auto command_interface_config = ctrl.c->command_interface_configuration(); - std::vector command_interface_names = {}; - if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) - { - command_interface_names = resource_manager_->available_command_interfaces(); - } - if ( - command_interface_config.type == - controller_interface::interface_configuration_type::INDIVIDUAL) - { - command_interface_names = command_interface_config.names; - } - request_interface_list.insert( - request_interface_list.end(), command_interface_names.begin(), - command_interface_names.end()); - }; - - if (in_activate_list) - { - extract_interfaces_for_controller(controller, activate_command_interface_request_); - } - if (in_deactivate_list) - { - extract_interfaces_for_controller(controller, deactivate_command_interface_request_); - } + RCLCPP_INFO(get_logger(), "Empty activate and deactivate list, not requesting switch"); - // cache mapping between hardware and controllers for stopping when read/write error happens - // TODO(destogl): This caching approach is suboptimal because the cache can fast become - // outdated. Keeping it up to date is not easy because of stopping controllers from multiple - // threads maybe we should not at all cache this but always search for the related controllers - // to a hardware when error in hardware happens - if (in_activate_list) + if (!from_chained_mode_request_.empty() || !to_chained_mode_request_.empty()) { - std::vector interface_names = {}; - - auto command_interface_config = controller.c->command_interface_configuration(); - if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) - { - interface_names = resource_manager_->available_command_interfaces(); - } - if ( - command_interface_config.type == - controller_interface::interface_configuration_type::INDIVIDUAL) - { - interface_names = command_interface_config.names; - } - - std::vector interfaces = {}; - auto state_interface_config = controller.c->state_interface_configuration(); - if (state_interface_config.type == controller_interface::interface_configuration_type::ALL) - { - interfaces = resource_manager_->available_state_interfaces(); - } - if ( - state_interface_config.type == - controller_interface::interface_configuration_type::INDIVIDUAL) - { - interfaces = state_interface_config.names; - } - - interface_names.insert(interface_names.end(), interfaces.begin(), interfaces.end()); - - resource_manager_->cache_controller_to_hardware(controller.info.name, interface_names); + RCLCPP_FATAL( + get_logger(), + "Despite the (de)activate request being empty, there " + "exists a non-empty from/to chained mode request. (This should never happen!)"); } - } - if (activate_request_.empty() && deactivate_request_.empty()) - { - RCLCPP_INFO(get_logger(), "Empty activate and deactivate list, not requesting switch"); clear_requests(); return controller_interface::return_type::OK; } + // extract the command interfaces that need to be (de)activated + extract_de_activate_command_interface_request( + controllers, deactivate_request_, activate_request_, deactivate_command_interface_request_, + activate_command_interface_request_); + + // cache the mapping of state and command interfaces of the controllers that are being + // activated to the resource manager + cache_controller_interfaces_in_activate_request(controllers, activate_request_); + RCLCPP_DEBUG(get_logger(), "Request for command interfaces from activating controllers:"); for (const auto & interface : activate_command_interface_request_) { @@ -1416,9 +1543,15 @@ 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 && + is_element_in_list(activate_request_, controller_name); + if (controller->is_chainable() && !will_be_restarted_asap) { resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); } @@ -1510,6 +1643,7 @@ void ControllerManager::activate_controllers( controller_name.c_str()); continue; } + auto controller = found_it->c; // reset the next update cycle time for newly activated controllers *found_it->next_update_cycle_time = @@ -2106,8 +2240,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) @@ -2330,260 +2467,264 @@ void ControllerManager::shutdown_async_controllers_and_components() resource_manager_->shutdown_async_components(); } -void ControllerManager::propagate_deactivation_of_chained_mode( - const std::vector & controllers) +void ControllerManager::extract_de_activate_command_interface_request( + const std::vector & controllers, + const std::vector & deactivate_request, + const std::vector & activate_request, + std::vector & deactivate_command_interface_request, + std::vector & activate_command_interface_request) const { + const auto & resource_manager = *resource_manager_; + + const auto extract_interfaces_for_controller = + [&resource_manager]( + const ControllerSpec & controller, std::vector & request_interface_list) + { + auto command_interface_config = controller.c->command_interface_configuration(); + std::vector command_interface_names = {}; + if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) + { + command_interface_names = resource_manager.available_command_interfaces(); + } + if ( + command_interface_config.type == + controller_interface::interface_configuration_type::INDIVIDUAL) + { + command_interface_names = command_interface_config.names; + } + request_interface_list.insert( + request_interface_list.end(), command_interface_names.begin(), command_interface_names.end()); + }; + for (const auto & controller : controllers) { - // get pointers to places in deactivate and activate lists ((de)activate lists have changed) - auto deactivate_list_it = - std::find(deactivate_request_.begin(), deactivate_request_.end(), controller.info.name); + const bool in_activate_list = is_element_in_list(activate_request, controller.info.name); + const bool in_deactivate_list = is_element_in_list(deactivate_request, controller.info.name); - if (deactivate_list_it != deactivate_request_.end()) + if (in_activate_list) { - // if controller is not active then skip adding following-controllers to "from" chained mode - // request - if (!is_controller_active(controller.c)) - { - RCLCPP_DEBUG( - get_logger(), - "Controller with name '%s' can not be deactivated since is not active. " - "The controller will be removed from the list later." - "Skipping adding following controllers to 'from' chained mode request.", - controller.info.name.c_str()); - break; - } - - for (const auto & cmd_itf_name : controller.c->command_interface_configuration().names) - { - // controller that 'cmd_tf_name' belongs to - ControllersListIterator following_ctrl_it; - if (command_interface_is_reference_interface_of_controller( - cmd_itf_name, controllers, following_ctrl_it)) - { - // currently iterated "controller" is preceding controller --> add following controller - // with matching interface name to "from" chained mode list (if not already in it) - if ( - std::find( - from_chained_mode_request_.begin(), from_chained_mode_request_.end(), - following_ctrl_it->info.name) == from_chained_mode_request_.end()) - { - from_chained_mode_request_.push_back(following_ctrl_it->info.name); - RCLCPP_DEBUG( - get_logger(), "Adding controller '%s' in 'from chained mode' request.", - following_ctrl_it->info.name.c_str()); - } - } - } + extract_interfaces_for_controller(controller, activate_command_interface_request); + } + if (in_deactivate_list) + { + extract_interfaces_for_controller(controller, deactivate_command_interface_request); } } } -controller_interface::return_type ControllerManager::check_following_controllers_for_activate( - const std::vector & controllers, int strictness, - const ControllersListIterator controller_it) +void ControllerManager::cache_controller_interfaces_in_activate_request( + const std::vector & controllers, + const std::vector & activate_request) { - // we assume that the controller exists is checked in advance - RCLCPP_DEBUG( - get_logger(), "Checking following controllers of preceding controller with name '%s'.", - controller_it->info.name.c_str()); + auto & resource_manager = *resource_manager_; - for (const auto & cmd_itf_name : controller_it->c->command_interface_configuration().names) + const auto cache_controller_to_hardware = [&resource_manager](const ControllerSpec & controller) { - ControllersListIterator following_ctrl_it; - // Check if interface if reference interface and following controller exist. - if (!command_interface_is_reference_interface_of_controller( - cmd_itf_name, controllers, following_ctrl_it)) - { - continue; - } - // TODO(destogl): performance of this code could be optimized by adding additional lists with - // controllers that cache if the check has failed and has succeeded. Then the following would be - // done only once per controller, otherwise in complex scenarios the same controller is checked - // multiple times - - // check that all following controllers exits, are either: activated, will be activated, or - // will not be deactivated - RCLCPP_DEBUG( - get_logger(), "Checking following controller with name '%s'.", - following_ctrl_it->info.name.c_str()); + std::vector interface_names = {}; - // check if following controller is chainable - if (!following_ctrl_it->c->is_chainable()) + auto command_interface_config = controller.c->command_interface_configuration(); + if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) { - RCLCPP_WARN( - get_logger(), - "No reference interface '%s' exist, since the following controller with name '%s' " - "is not chainable.", - cmd_itf_name.c_str(), following_ctrl_it->info.name.c_str()); - return controller_interface::return_type::ERROR; + interface_names = resource_manager.available_command_interfaces(); } - - if (is_controller_active(following_ctrl_it->c)) + if ( + command_interface_config.type == + controller_interface::interface_configuration_type::INDIVIDUAL) { - // will following controller be deactivated? - if ( - std::find( - deactivate_request_.begin(), deactivate_request_.end(), following_ctrl_it->info.name) != - deactivate_request_.end()) - { - RCLCPP_WARN( - get_logger(), "The following controller with name '%s' will be deactivated.", - following_ctrl_it->info.name.c_str()); - return controller_interface::return_type::ERROR; - } + interface_names = command_interface_config.names; } - // 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()) + + std::vector interfaces = {}; + auto state_interface_config = controller.c->state_interface_configuration(); + if (state_interface_config.type == controller_interface::interface_configuration_type::ALL) { - RCLCPP_WARN( - get_logger(), - "The following controller with name '%s' is not active and will not be activated.", - following_ctrl_it->info.name.c_str()); - return controller_interface::return_type::ERROR; + interfaces = resource_manager.available_state_interfaces(); } - - // Trigger recursion to check all the following controllers only if they are OK, add this - // controller update chained mode requests if ( - check_following_controllers_for_activate(controllers, strictness, following_ctrl_it) == - controller_interface::return_type::ERROR) + state_interface_config.type == controller_interface::interface_configuration_type::INDIVIDUAL) { - return controller_interface::return_type::ERROR; + interfaces = state_interface_config.names; } - // TODO(destogl): this should be discussed how to it the best - just a placeholder for now - // else if (strictness == - // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN) - // { - // // insert to the begin of activate request list to be activated before preceding controller - // activate_request_.insert(activate_request_.begin(), following_ctrl_name); - // } - if (!following_ctrl_it->c->is_in_chained_mode()) - { - auto found_it = std::find( - to_chained_mode_request_.begin(), to_chained_mode_request_.end(), - following_ctrl_it->info.name); - if (found_it == to_chained_mode_request_.end()) - { - to_chained_mode_request_.push_back(following_ctrl_it->info.name); - // if it is a chainable controller, make the reference interfaces available on preactivation - // (This is needed when you activate a couple of chainable controller altogether) - resource_manager_->make_controller_reference_interfaces_available( - following_ctrl_it->info.name); - RCLCPP_DEBUG( - get_logger(), "Adding controller '%s' in 'to chained mode' request.", - following_ctrl_it->info.name.c_str()); - } - } - else + interface_names.insert(interface_names.end(), interfaces.begin(), interfaces.end()); + + resource_manager.cache_controller_to_hardware(controller.info.name, interface_names); + }; + + for (const auto & controller : controllers) + { + const bool in_activate_list = is_element_in_list(activate_request, controller.info.name); + + // cache mapping between hardware and controllers for stopping when read/write error happens + // TODO(destogl): This caching approach is suboptimal because the cache can fast become + // outdated. Keeping it up to date is not easy because of stopping controllers from multiple + // threads maybe we should not at all cache this but always search for the related controllers + // to a hardware when error in hardware happens + if (in_activate_list) { - // Check if following controller is in 'from' chained mode list and remove it, if so - auto found_it = std::find( - from_chained_mode_request_.begin(), from_chained_mode_request_.end(), - following_ctrl_it->info.name); - if (found_it != from_chained_mode_request_.end()) - { - from_chained_mode_request_.erase(found_it); - RCLCPP_DEBUG( - get_logger(), - "Removing controller '%s' in 'from chained mode' request because it " - "should stay in chained mode.", - following_ctrl_it->info.name.c_str()); - } + cache_controller_to_hardware(controller); } } - return controller_interface::return_type::OK; }; -controller_interface::return_type ControllerManager::check_preceeding_controllers_for_deactivate( - const std::vector & controllers, int /*strictness*/, - const ControllersListIterator controller_it) +controller_interface::return_type ControllerManager::check_controllers_for_de_activate( + const int strictness, const std::vector & controllers, + const controller_manager::ControllersMap & controllers_map, + std::vector & deactivate_request, std::vector & activate_request) const { - // if not chainable no need for any checks - if (!controller_it->c->is_chainable()) + // check if there are controllers to (de)activate + if (activate_request.empty() && deactivate_request.empty()) { + RCLCPP_DEBUG(get_logger(), "No controllers to (de)activate"); return controller_interface::return_type::OK; } - if (!controller_it->c->is_in_chained_mode()) + // check for conflicts of (de)activate requests for controllers + for (const auto & controller : controllers) { - RCLCPP_DEBUG( - get_logger(), - "Controller with name '%s' is chainable but not in chained mode. " - "No need to do any checks of preceding controllers when stopping it.", - controller_it->info.name.c_str()); - return controller_interface::return_type::OK; + if ( + check_controller_for_de_activate( + get_logger(), strictness, controller, deactivate_request, activate_request) != + controller_interface::return_type::OK) + { + return controller_interface::return_type::ERROR; + } } - RCLCPP_DEBUG( - get_logger(), "Checking preceding controller of following controller with name '%s'.", - controller_it->info.name.c_str()); + // NOTE: If the value of strictness is BEST_EFFORT, the following invalid requests will be removed + // by the above function (check_controller_for_de_activate), and their existence will not be + // considered in subsequent processing: + // - Deactivate a controller that is already inactive: (double stop) + // - Activate a controller that is already active: (double activation) + // - Executing (de)activate from a state other than active/inactive (unconfigured/finalized): + // (illegal activation) - for (const auto & ref_itf_name : - resource_manager_->get_controller_reference_interface_names(controller_it->info.name)) + // Check for conflict in the order of "preceding" to "following" + // Note: If the order of controllers is not sorted in the sequence of preceding -> following, the + // subsequent processes will not function properly. + for (const auto & controller : controllers) { - std::vector preceding_controllers_using_ref_itf; + const auto controller_state = + get_activation_state(controller, deactivate_request, activate_request); - // TODO(destogl): This data could be cached after configuring controller into a map for faster - // access here - for (auto preceding_ctrl_it = controllers.begin(); preceding_ctrl_it != controllers.end(); - ++preceding_ctrl_it) + if (controller_state.in_deactivate_list) { - const auto preceding_ctrl_cmd_itfs = - preceding_ctrl_it->c->command_interface_configuration().names; - - // if controller is not preceding go the next one - if ( - std::find(preceding_ctrl_cmd_itfs.begin(), preceding_ctrl_cmd_itfs.end(), ref_itf_name) == - preceding_ctrl_cmd_itfs.end()) + // if the controller is included in both the deactivate list and the activate list, check if a + // restart is possible. + if (controller_state.in_activate_list) { - continue; + if (can_controller_restart( + get_logger(), controller.info.name, controllers_map, controller_chain_spec_, + deactivate_request, activate_request)) + { + // if no conflict is found in the restart request, further checks are unnecessary, so + // proceed to the next controller. + continue; + } + + // if a conflict is found, erase the activate request, and then check if only deactivation + // is possible. (if BEST_EFFORT) + if (!handle_conflict( + strictness, get_logger(), + "The restart request for controller '" + controller.info.name + + "' was rejected due to conflict")) + { + return controller_interface::return_type::ERROR; + } + + remove_element_from_list(activate_request, controller.info.name); } - // check if preceding controller will be activated - if ( - is_controller_inactive(preceding_ctrl_it->c) && - std::find( - activate_request_.begin(), activate_request_.end(), preceding_ctrl_it->info.name) != - activate_request_.end()) + // if it is included only in the deactivate list, check for conflicts in the deactivate + // request for the controller. + if (!can_controller_deactivate( + get_logger(), controller.info.name, controllers_map, controller_chain_spec_, + deactivate_request, activate_request)) { - RCLCPP_WARN( - get_logger(), - "Could not deactivate controller with name '%s' because " - "preceding controller with name '%s' will be activated. ", - controller_it->info.name.c_str(), preceding_ctrl_it->info.name.c_str()); - return controller_interface::return_type::ERROR; + if (!handle_conflict( + strictness, get_logger(), + "The deactivation request for controller '" + controller.info.name + + "' was rejected due to conflict")) + { + return controller_interface::return_type::ERROR; + } + + remove_element_from_list(deactivate_request, controller.info.name); } - // 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 (controller_state.in_activate_list) + { + // if it is included only in the activate list, check for conflicts in the activate request + // for the controller. + if (!can_controller_activate( + get_logger(), controller.info.name, controllers_map, controller_chain_spec_, + deactivate_request, activate_request)) { - RCLCPP_WARN( - get_logger(), - "Could not deactivate controller with name '%s' because " - "preceding controller with name '%s' is active and will not be deactivated.", - controller_it->info.name.c_str(), preceding_ctrl_it->info.name.c_str()); - return controller_interface::return_type::ERROR; + if (!handle_conflict( + strictness, get_logger(), + "The activation request for controller '" + controller.info.name + + "' was rejected due to conflict")) + { + return controller_interface::return_type::ERROR; + } + + remove_element_from_list(activate_request, controller.info.name); } - // TODO(destogl): this should be discussed how to it the best - just a placeholder for now - // else if ( - // strictness == - // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN) - // { - // // insert to the begin of activate request list to be activated before preceding controller - // activate_request_.insert(activate_request_.begin(), preceding_ctrl_name); - // } + } + else + { + // this controller does not have any (de)activate request --> do nothing } } + return controller_interface::return_type::OK; } +void ControllerManager::create_from_to_chained_mode_request( + const std::vector & controllers, + const controller_manager::ControllersMap & controllers_map, + std::vector & deactivate_request, std::vector & activate_request, + std::vector & from_chained_mode_request, + std::vector & to_chained_mode_request) const +{ + // Check all preceding and following controller + // Note: If the order of controllers is not sorted in the sequence of preceding -> following, + // the subsequent processes will not function properly. + for (const auto & controller : controllers) + { + const auto & chain_spec = controller_chain_spec_.at(controller.info.name); + + // if there are no following controllers, this controller is not a preceding controller, so + // skip the following checks + if (chain_spec.following_controllers.empty()) + { + continue; + } + + // this controller is a preceding controller + const auto & preceding_ctrl = controller; + const auto preceding_state = + get_activation_state(preceding_ctrl, deactivate_request, activate_request); + + // check for conflict of the preceding and each following controller + for (const auto & following_controller_name : chain_spec.following_controllers) + { + const auto & following_ctrl = controllers_map.at(following_controller_name); + + const auto following_state = + get_activation_state(following_ctrl, deactivate_request, activate_request); + + // determine if a change chained mode is necessary for the following controllers based on the + // request, and add them to the chained mode request if necessary. Additionally, if only + // preceding performs the activation state transition, a restart of the following controller + // is necessary to switch the chained mode, so add them to the (de)activate request. + update_chained_mode_request_of_following( + get_logger(), following_ctrl, preceding_state, following_state, deactivate_request, + activate_request, from_chained_mode_request, to_chained_mode_request, *resource_manager_); + } + } +} + 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..b4638d0163 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) { @@ -421,6 +454,103 @@ class TestControllerChainingWithControllerManager double chained_ctrl_calculation(double reference, double state) { return reference - state; } double hardware_calculation(double command) { return command / 2.0; } + 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(); + + SetToChainedModeAndMakeReferenceInterfacesAvailable( + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_REFERENCE_INTERFACES); + SetToChainedModeAndMakeReferenceInterfacesAvailable( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_REFERENCE_INTERFACES); + SetToChainedModeAndMakeReferenceInterfacesAvailable( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_REFERENCE_INTERFACES); + + EXPECT_THROW( + cm_->resource_manager_->make_controller_reference_interfaces_available( + POSITION_TRACKING_CONTROLLER), + std::out_of_range); + + // 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); + } + // set controllers' names static constexpr char PID_LEFT_WHEEL[] = "pid_left_wheel_controller"; static constexpr char PID_RIGHT_WHEEL[] = "pid_right_wheel_controller"; @@ -441,6 +571,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; @@ -747,6 +879,31 @@ TEST_P( 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) // --> return error; preceding controller is not activated, @@ -775,13 +932,189 @@ 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 is 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 controller (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 one of the 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()); + + // 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 +1217,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 +1247,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,9 +1264,24 @@ 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 + 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( PID_LEFT_WHEEL, expected.at(test_param.strictness).return_type, @@ -943,6 +1291,9 @@ TEST_P( expected.at(test_param.strictness).future_status); ActivateController( DIFF_DRIVE_CONTROLLER, controller_interface::return_type::OK, std::future_status::timeout); + ActivateController( + POSITION_TRACKING_CONTROLLER, controller_interface::return_type::OK, + std::future_status::timeout); // Expect all controllers to be active ASSERT_EQ( @@ -951,32 +1302,51 @@ 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()); + 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 following controllers + // 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(); - // 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()); + // Attempt to deactivate the middle 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 a following controller + // Attempt to deactivate the middle following and one of the lowest following controller switch_test_controllers( - {}, {PID_RIGHT_WHEEL}, test_param.strictness, std::future_status::ready, + {}, {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(); - // 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()); + // 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 +1476,354 @@ 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(); + } +} + 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));