From 123cb10b0171356ce1275d5af5fb776bd63494d1 Mon Sep 17 00:00:00 2001 From: Takashi Sato Date: Thu, 6 Jun 2024 20:07:20 +0900 Subject: [PATCH 01/16] Modify switch_controllers to implement a reset operation by executing deactivate followed by activate when the same controller is specified for both start and stop --- controller_manager/CMakeLists.txt | 21 + .../controller_manager/controller_manager.hpp | 18 +- controller_manager/src/controller_manager.cpp | 955 +++++++++++++++++- .../test_chainable_controller.cpp | 9 + .../test_chainable_controller.hpp | 10 +- .../test/test_controller/test_controller.cpp | 12 + .../test/test_controller/test_controller.hpp | 11 + .../test_controller_with_command.cpp | 74 ++ .../test_controller_with_command.hpp | 86 ++ .../test_controller_with_command.xml | 9 + ...llers_chaining_with_controller_manager.cpp | 445 +++++++- .../test/test_reset_controller.cpp | 280 +++++ 12 files changed, 1872 insertions(+), 58 deletions(-) create mode 100644 controller_manager/test/test_controller_with_command/test_controller_with_command.cpp create mode 100644 controller_manager/test/test_controller_with_command/test_controller_with_command.hpp create mode 100644 controller_manager/test/test_controller_with_command/test_controller_with_command.xml create mode 100644 controller_manager/test/test_reset_controller.cpp diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index c3c0de7739..f76e172bfd 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_reset_controller + test/test_reset_controller.cpp + APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$ + ) + target_link_libraries(test_reset_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..1e4899c122 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -206,6 +206,12 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC void shutdown_async_controllers_and_components(); + CONTROLLER_MANAGER_PUBLIC + std::vector get_ordered_controllers_names() const + { + return ordered_controllers_names_; + } + protected: CONTROLLER_MANAGER_PUBLIC void init_services(); @@ -348,6 +354,9 @@ class ControllerManager : public rclcpp::Node */ void propagate_deactivation_of_chained_mode(const std::vector & controllers); + void propagate_activation_and_deactivation_of_chained_mode( + const std::vector & controllers); + /// Check if all the following controllers will be in active state and in the chained mode /// after controllers' switch. /** @@ -375,7 +384,7 @@ class ControllerManager : public rclcpp::Node */ controller_interface::return_type check_following_controllers_for_activate( const std::vector & controllers, int strictness, - const ControllersListIterator controller_it); + const ControllersListIterator controller_it) const; /// Check if all the preceding controllers will be in inactive state after controllers' switch. /** @@ -398,7 +407,12 @@ class ControllerManager : public rclcpp::Node */ controller_interface::return_type check_preceeding_controllers_for_deactivate( const std::vector & controllers, int strictness, - const ControllersListIterator controller_it); + const ControllersListIterator controller_it) const; + + controller_interface::return_type check_preceding_and_following_controllers_for_switch( + const std::vector & controllers, std::vector & deactivate_request, + std::vector & activate_request, std::vector & to_chained_mode_request, + std::vector & from_chained_mode_request, const int strictness) const; /** * @brief Inserts a controller into an ordered list based on dependencies to compute the diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 3b84fc07e4..c5ce27cfc5 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -14,6 +14,7 @@ #include "controller_manager/controller_manager.hpp" +#include #include #include #include @@ -958,29 +959,251 @@ controller_interface::return_type ControllerManager::switch_controller( return ret; } + const auto show_list = [this](const std::string & tag) + { + RCLCPP_WARN(get_logger(), "[%s]----------------", tag.c_str()); + RCLCPP_WARN(get_logger(), "Activating controllers:"); + for (const auto & controller : activate_request_) + { + RCLCPP_WARN(get_logger(), " - %s", controller.c_str()); + } + RCLCPP_WARN(get_logger(), "Deactivating controllers:"); + for (const auto & controller : deactivate_request_) + { + RCLCPP_WARN(get_logger(), " - %s", controller.c_str()); + } + RCLCPP_WARN(get_logger(), "to chained mode:"); + for (const auto & req : to_chained_mode_request_) + { + RCLCPP_WARN(get_logger(), " - %s", req.c_str()); + } + RCLCPP_WARN(get_logger(), "from chained mode:"); + for (const auto & req : from_chained_mode_request_) + { + RCLCPP_WARN(get_logger(), " - %s", req.c_str()); + } + RCLCPP_WARN(get_logger(), "----------------"); + }; + + RCLCPP_WARN(get_logger(), "STRICTNESS: %d", strictness); + show_list("REQUEST"); + +#if 1 + auto sortAByB = + [](std::vector & A, const std::vector & B, bool reverse = false) + { + // Bの要素の位置を記録するマップを作成 + std::unordered_map indexMap; + for (size_t i = 0; i < B.size(); ++i) + { + indexMap[B[i]] = i; + } + + // Aの要素をBの順序に従ってソート + std::sort( + A.begin(), A.end(), + [&indexMap, reverse](const std::string & a, const std::string & b) + { + if (reverse) + { + return indexMap[a] > indexMap[b]; + } + else + { + return indexMap[a] < indexMap[b]; + } + }); + }; + + // sortAByB(activate_request_, ordered_controllers_names_); + // sortAByB(deactivate_request_, ordered_controllers_names_); + // show_list("SORTED"); +#endif + // lock controllers std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); const std::vector & controllers = rt_controllers_wrapper_.get_updated_list(guard); + // 関数の入出力を明確にするために、メンバ変数の参照をローカル変数に持たせて各ラムダに渡す + auto & activate_request = activate_request_; + auto & deactivate_request = deactivate_request_; + const auto & logger = get_logger(); + + // cleanup switch request first + const auto cleanup_request = + [&controllers, &activate_request, &deactivate_request, &strictness, &logger]() + { + const auto check_following_is_chainable = [&controllers, &logger](const auto & controller) + { + for (const auto & cmd_itf_name : controller->command_interface_configuration().names) + { + 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( + logger, "Checking following controller with name '%s'.", + following_ctrl_it->info.name.c_str()); + + // check if following controller is chainable + if (!following_ctrl_it->c->is_chainable()) + { + RCLCPP_WARN( + 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 false; + } + } + return true; + }; + + for (const auto & controller : controllers) + { + const bool is_active = is_controller_active(*controller.c); + const bool is_inactive = is_controller_inactive(*controller.c); + + // 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); + bool 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(logger, "%s", msg.c_str()); + deactivate_request.clear(); + activate_request.clear(); + return controller_interface::return_type::ERROR; + } + RCLCPP_WARN(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); + } + + // followingが chainable でない場合、activate できない + if ( + (is_active || is_inactive) && in_activate_list && + !check_following_is_chainable(controller.c)) + { + auto conflict_status = handle_conflict( + "Could not activate controller '" + controller.info.name + + "' since it has not chainable following controller"); + if (conflict_status != controller_interface::return_type::OK) + { + return conflict_status; + } + in_activate_list = false; + activate_request.erase(activate_list_it); + } + } + + return controller_interface::return_type::OK; + }; + + if (cleanup_request() != controller_interface::return_type::OK) + { + RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)"); + // reset all lists + clear_requests(); + return controller_interface::return_type::ERROR; + } + show_list("AFTER CLEANUP"); + +LABEL_START: +#if 0 // check deactivate の後に移動 // if a preceding controller is deactivated, all first-level controllers should be switched 'from' // chained mode propagate_deactivation_of_chained_mode(controllers); + show_list("AFTER PROPAGATE"); +#endif + +#if 1 + if ( + check_preceding_and_following_controllers_for_switch( + controllers, deactivate_request, activate_request, to_chained_mode_request_, + from_chained_mode_request_, strictness) != controller_interface::return_type::OK) + { + RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)"); + // reset all lists + clear_requests(); + return controller_interface::return_type::ERROR; + } + show_list("AFTER CHECK FOR SWITCH"); +#else // 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) + for (auto ctrl_it = activate_request_.begin(); ctrl_it != activate_request_.end();) { 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)) + // if controller is neither inactive nor active then do not do any following-controllers + // checks + if (!is_controller_inactive(controller_it->c) && !is_controller_active(controller_it->c)) { RCLCPP_WARN( get_logger(), - "Controller with name '%s' is not inactive so its following " + "Controller with name '%s' is neither inactive and nor active 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; @@ -1005,8 +1228,8 @@ controller_interface::return_type ControllerManager::switch_controller( // 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; + ctrl_it = activate_request_.erase(ctrl_it); + goto LABEL_START; } if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) { @@ -1016,10 +1239,17 @@ controller_interface::return_type ControllerManager::switch_controller( return controller_interface::return_type::ERROR; } } + else + { + // increment iterator only if controller was not removed from the list + ++ctrl_it; + } } + show_list("AFTER CHECK ACTIVATE"); + // 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) + for (auto ctrl_it = deactivate_request_.begin(); ctrl_it != deactivate_request_.end();) { auto controller_it = std::find_if( controllers.begin(), controllers.end(), @@ -1051,8 +1281,8 @@ controller_interface::return_type ControllerManager::switch_controller( { // 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; + ctrl_it = deactivate_request_.erase(ctrl_it); + goto LABEL_START; } if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) { @@ -1062,22 +1292,54 @@ controller_interface::return_type ControllerManager::switch_controller( return controller_interface::return_type::ERROR; } } + else + { + // increment iterator only if controller was not removed from the list + ++ctrl_it; + } } + show_list("AFTER CHECK DEACTIVATE"); +#endif + +#if 0 + propagate_activation_and_deactivation_of_chained_mode(controllers); + show_list("AFTER PROPAGATE"); + +// // if a preceding controller is deactivated, all first-level controllers should be switched +// 'from' +// // chained mode +// propagate_deactivation_of_chained_mode(controllers); +// show_list("AFTER PROPAGATE DEACTIVATION"); + +// // if a preceding controller is activated, all first-level controllers should be switched +// 'to' +// // chained mode +// propagate_activation_of_chained_mode(controllers); +// show_list("AFTER PROPAGATE ACTIVATION"); +#endif + for (const auto & controller : controllers) { - 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 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 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(); + 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(); + +#if 0 const bool is_active = is_controller_active(*controller.c); const bool is_inactive = is_controller_inactive(*controller.c); @@ -1086,11 +1348,24 @@ controller_interface::return_type ControllerManager::switch_controller( { if (is_active && !in_deactivate_list) { - deactivate_request_.push_back(controller.info.name); - activate_request_.push_back(controller.info.name); + if ( + std::find(deactivate_request_.begin(), deactivate_request_.end(), controller.info.name) == + deactivate_request_.end()) + { + deactivate_request_.push_back(controller.info.name); + } + + if ( + std::find(activate_request_.begin(), activate_request_.end(), controller.info.name) == + activate_request_.end()) + { + activate_request_.push_back(controller.info.name); + } } } +#endif +#if 0 // 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); @@ -1156,6 +1431,7 @@ controller_interface::return_type ControllerManager::switch_controller( in_activate_list = false; activate_request_.erase(activate_list_it); } +#endif const auto extract_interfaces_for_controller = [this](const ControllerSpec ctrl, std::vector & request_interface_list) @@ -1226,6 +1502,14 @@ controller_interface::return_type ControllerManager::switch_controller( } } +#if 1 + sortAByB(deactivate_request_, ordered_controllers_names_); + sortAByB(activate_request_, ordered_controllers_names_, true); + show_list("SORTED"); +#endif + + show_list("FINALLY"); + if (activate_request_.empty() && deactivate_request_.empty()) { RCLCPP_INFO(get_logger(), "Empty activate and deactivate list, not requesting switch"); @@ -1267,12 +1551,20 @@ controller_interface::return_type ControllerManager::switch_controller( // wait until switch is finished RCLCPP_DEBUG(get_logger(), "Requested atomic controller switch from realtime loop"); + int timeout_count = 0; while (rclcpp::ok() && switch_params_.do_switch) { if (!rclcpp::ok()) { return controller_interface::return_type::ERROR; } + + if (++timeout_count > 10 * 3000) + { + RCLCPP_ERROR(get_logger(), "Timeout in atomic controller switch"); + return controller_interface::return_type::ERROR; + } + std::this_thread::sleep_for(std::chrono::microseconds(100)); } @@ -1413,13 +1705,25 @@ void ControllerManager::deactivate_controllers( { try { + RCLCPP_WARN( + get_logger(), "Deactivating controller '%s' with state '%s'", controller_name.c_str(), + controller->get_node()->get_current_state().label().c_str()); + const auto new_state = controller->get_node()->deactivate(); + + RCLCPP_WARN(get_logger(), "Release interfaces of controller '%s'", controller_name.c_str()); 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 and will not be activated asap, make the reference + // interfaces unavailable on deactivation + const bool will_be_activated = + std::find(activate_request_.begin(), activate_request_.end(), controller_name) != + activate_request_.end(); + if (controller->is_chainable() && !will_be_activated) { + RCLCPP_WARN( + get_logger(), "Release reference interfaces of controller '%s'", + controller_name.c_str()); resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); } if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) @@ -1510,6 +1814,11 @@ void ControllerManager::activate_controllers( controller_name.c_str()); continue; } + + RCLCPP_WARN( + get_logger(), "Activating controller '%s' with state '%s'", controller_name.c_str(), + found_it->c->get_node()->get_current_state().label().c_str()); + auto controller = found_it->c; // reset the next update cycle time for newly activated controllers *found_it->next_update_cycle_time = @@ -1545,6 +1854,9 @@ void ControllerManager::activate_controllers( } try { + RCLCPP_WARN( + get_logger(), "Claiming command interface '%s' for controller '%s'", + command_interface.c_str(), controller_name.c_str()); command_loans.emplace_back(resource_manager_->claim_command_interface(command_interface)); } catch (const std::exception & e) @@ -2039,7 +2351,8 @@ void ControllerManager::set_hardware_component_state_srv_cb( { rclcpp_lifecycle::State target_state( request->target_state.id, - // the ternary operator is needed because label in State constructor cannot be an empty string + // the ternary operator is needed because label in State constructor cannot be an empty + // string request->target_state.label.empty() ? "-" : request->target_state.label); response->ok = (resource_manager_->set_component_state(request->name, target_state) == @@ -2106,8 +2419,11 @@ void ControllerManager::manage_switch() deactivate_controllers(rt_controller_list, deactivate_request_); - switch_chained_mode(to_chained_mode_request_, true); + // When the same interface is specified for both 'from' and 'to' during a controller reset, 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) @@ -2379,9 +2695,489 @@ void ControllerManager::propagate_deactivation_of_chained_mode( } } +void ControllerManager::propagate_activation_and_deactivation_of_chained_mode( + const std::vector & controllers) +{ + const auto add_following_to_restart_request_if_needed = [&](auto controller_it) + { + const bool is_active = is_controller_active(controller_it->c); + const bool will_be_deactivated = + std::find(deactivate_request_.begin(), deactivate_request_.end(), controller_it->info.name) != + deactivate_request_.end(); + + if (is_active && !will_be_deactivated) + { + deactivate_request_.push_back(controller_it->info.name); + activate_request_.push_back(controller_it->info.name); + // need to traverse again + return false; + }; + + // no need to traverse + return true; + }; + + const auto propagate_deactivation = [&]() + { + 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)); + + // if controller is not active then skip adding following-controllers to "from" chained mode + // request + if (!is_controller_active(controller_it->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_it->info.name.c_str()); + break; + } + + for (const auto & cmd_itf_name : controller_it->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)) + { + continue; + } + + // 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()); + + if (!add_following_to_restart_request_if_needed(following_ctrl_it)) + { + // need to traverse again + return false; + } + } + } + } + + // finish traverse + return true; + }; + + const auto propagate_activation = [&]() + { + for (auto ctrl_it = activate_request_.begin(); ctrl_it != activate_request_.end(); ++ctrl_it) + { + auto controller_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it)); + + const bool controller_will_be_deactivated = + std::find( + deactivate_request_.begin(), deactivate_request_.end(), controller_it->info.name) != + deactivate_request_.end(); + + // if controller is active then skip adding following-controllers to "to" chained mode + // request + if (is_controller_active(controller_it->c) && !controller_will_be_deactivated) + { + RCLCPP_DEBUG( + get_logger(), + "Controller with name '%s' can not be activated since is already active and will not " + "be " + "deactivated. " + "The controller will be removed from the list later." + "Skipping adding following controllers to 'to' chained mode request.", + controller_it->info.name.c_str()); + continue; + } + + for (const auto & cmd_itf_name : controller_it->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)) + { + continue; + } + + // currently iterated "controller" is preceding controller --> add following controller + // with matching interface name to "to" chained mode list (if not already in it) + if ( + std::find( + to_chained_mode_request_.begin(), to_chained_mode_request_.end(), + following_ctrl_it->info.name) == to_chained_mode_request_.end()) + { + to_chained_mode_request_.push_back(following_ctrl_it->info.name); + RCLCPP_DEBUG( + get_logger(), "Adding controller '%s' in 'to chained mode' request.", + following_ctrl_it->info.name.c_str()); + + // if it is a chainable controller and not in chained_mode, make the reference + // interfaces available on preactivation (This is needed when you activate a couple of + // chainable controller altogether) + if (!following_ctrl_it->c->is_in_chained_mode()) + { + resource_manager_->make_controller_reference_interfaces_available( + following_ctrl_it->info.name); + } + + if (!add_following_to_restart_request_if_needed(following_ctrl_it)) + { + // need to traverse again + return false; + } + } + } + } + + // finish traverse + return true; + }; + + while (!propagate_activation() || !propagate_deactivation()); +} + +controller_interface::return_type +ControllerManager::check_preceding_and_following_controllers_for_switch( + const std::vector & controllers, std::vector & deactivate_request, + std::vector & activate_request, std::vector & to_chained_mode_request, + std::vector & from_chained_mode_request, const int strictness) const +{ + const auto logger = get_logger(); + + enum class ConflictStatus + { + NO_CONFLICT, + CONFLICT_WITH_BEST_EFFORT, + CONFLICT_WITH_STRICT, + }; + + const auto handle_conflict = [&]( + const std::string & msg, + const std::string & erase_from_activate = "", + const std::string & erase_from_deactivate = "") -> ConflictStatus + { + if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + { + RCLCPP_ERROR(logger, "%s", msg.c_str()); + deactivate_request.clear(); + activate_request.clear(); + to_chained_mode_request.clear(); + from_chained_mode_request.clear(); + return ConflictStatus::CONFLICT_WITH_STRICT; + } + + RCLCPP_WARN(logger, "%s", msg.c_str()); + if (!erase_from_activate.empty()) + { + auto it = std::find(activate_request.begin(), activate_request.end(), erase_from_activate); + if (it != activate_request.end()) + { + activate_request.erase(it); + } + } + if (!erase_from_deactivate.empty()) + { + auto it = + std::find(deactivate_request.begin(), deactivate_request.end(), erase_from_deactivate); + if (it != deactivate_request.end()) + { + deactivate_request.erase(it); + } + } + + return ConflictStatus::CONFLICT_WITH_BEST_EFFORT; + }; + + // check if there are controllers to (de)activate + if (activate_request.empty() && deactivate_request.empty()) + { + RCLCPP_DEBUG(logger, "No controllers to (de)activate"); + return controller_interface::return_type::OK; + } + + // preceding -> following の順に整合性をチェックする + for (const auto & controller : controllers) + { + const bool is_active = is_controller_active(*controller.c); + const bool is_inactive = is_controller_inactive(*controller.c); + + // skip controllers that are not active nor inactive + if (!is_active && !is_inactive) + { + continue; + } + + // 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_deactivate_list = deactivate_list_it != deactivate_request.end(); + + auto activate_list_it = + std::find(activate_request.begin(), activate_request.end(), controller.info.name); + const bool in_activate_list = activate_list_it != activate_request.end(); + + std::vector> + following_ctrl_refs; + + 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)) + { + // chain に関係しない interface はチェック不要 + continue; + } + + if ( + std::find_if( + following_ctrl_refs.begin(), following_ctrl_refs.end(), [&](const auto & ctrl) + { return ctrl.get().info.name == following_ctrl_it->info.name; }) == + following_ctrl_refs.end()) + { + following_ctrl_refs.push_back(std::cref(*following_ctrl_it)); + } + } + + RCLCPP_WARN(logger, "Checking controller '%s'", controller.info.name.c_str()); + for (const auto following_ctrl_ref : following_ctrl_refs) + { + const auto & following_ctrl = following_ctrl_ref.get(); + RCLCPP_WARN(logger, " - Following controller '%s'", following_ctrl.info.name.c_str()); + } + + for (const auto following_ctrl_ref : following_ctrl_refs) + { + const auto & following_ctrl = following_ctrl_ref.get(); + const bool following_is_active = is_controller_active(following_ctrl.c); + const bool following_is_inactive = is_controller_inactive(following_ctrl.c); + const bool following_in_deactivate_list = + std::find(deactivate_request.begin(), deactivate_request.end(), following_ctrl.info.name) != + deactivate_request.end(); + const bool following_in_activate_list = + std::find(activate_request.begin(), activate_request.end(), following_ctrl.info.name) != + activate_request.end(); + + if ( + !in_activate_list && !in_deactivate_list && !following_in_activate_list && + !following_in_deactivate_list) + { + // preceding, following ともにいずれの request にも含まれていない場合はチェック不要 + RCLCPP_DEBUG( + logger, + " - No need to check preceding '%s' and following '%s' because there are no requests", + controller.info.name.c_str(), following_ctrl.info.name.c_str()); + continue; + } + + // 1. preceding が inactive から activate される場合のエラー判定 + if (is_inactive && in_activate_list) + { + // 以下のいずれかの場合は preceding を activate できないためエラー + // 1-a. following が inactive で activate されない場合 + // 1-b. following が active で deactivateされ、かつ、restart されない場合 + // エラー発生かつstrictがBEST_EFFORTの場合は、precedingをactivateリストから削除 + ConflictStatus conflict_status = ConflictStatus::NO_CONFLICT; + if (following_is_inactive && !following_in_activate_list) + { + conflict_status = handle_conflict( + "The controller with name '" + controller.info.name + + "' cannot be activated because the following controller with name '" + + following_ctrl.info.name + "' is inactive and will not be activated.", + controller.info.name, ""); + } + if (following_is_active && following_in_deactivate_list && !following_in_activate_list) + { + conflict_status = handle_conflict( + "The controller with name '" + controller.info.name + + "' cannot be activated because the following controller with name '" + + following_ctrl.info.name + + "' is active and will be deactivated but will not be activated.", + controller.info.name, ""); + } + + if (conflict_status == ConflictStatus::CONFLICT_WITH_STRICT) + { + // STRICTの場合は即座にエラーを返す + RCLCPP_ERROR( + logger, "Switching controllers failed due to strict conflict: %s", + controller.info.name.c_str()); + return controller_interface::return_type::ERROR; + } + else if (conflict_status == ConflictStatus::CONFLICT_WITH_BEST_EFFORT) + { + // BEST_EFFORTの場合はprecedingをactivateリストから削除したため、次のcontroller に進む + break; + } + } + // 2. preceding が active のまま遷移がない場合のエラー判定 + else if (is_active && !in_deactivate_list && !in_activate_list) + { + // 以下のいずれかの場合は following を deactivate できない (結果として restart もできない) + // ためエラー + // 2-1. following が active で deactivate される場合 + // 2-2. following が active で restart される場合 + // -> 結局のところ、following が activate されるか否かを問わず、deactivate + // される場合はエラー(precedingがactiveの時、followingは必ずactiveなので、followingのactiveチェックは不要) + // エラー発生かつstrictがBEST_EFFORTの場合はfollowingをactivate/deactivateリストから削除 + if (following_in_deactivate_list) + { + const auto conflict_status = handle_conflict( + "The following controller with name '" + following_ctrl.info.name + + "' cannot be deactivated because the preceding controller with name '" + + controller.info.name + "' is active and will not be deactivated and restarted.", + following_ctrl.info.name, following_ctrl.info.name); + + if (conflict_status == ConflictStatus::CONFLICT_WITH_STRICT) + { + // STRICTの場合は即座にエラーを返す + RCLCPP_ERROR( + logger, "Switching controllers failed due to strict conflict: %s", + controller.info.name.c_str()); + return controller_interface::return_type::ERROR; + } + else if (conflict_status == ConflictStatus::CONFLICT_WITH_BEST_EFFORT) + { + // BEST_EFFORTの場合はfollowingをactivate/deactivateリストから削除したため、次のfollowing + // に進む + continue; + } + } + } + // 3. preceding が active で restart される場合のエラー判定 + else if (is_active && in_deactivate_list && in_activate_list) + { + // following が active で deactivate され、かつ、restart しない場合は following を + // deactivate できないためエラー + // エラー発生かつstrictがBEST_EFFORTの場合はfollowingをdeactivateリストから削除 + if (following_is_active && following_in_deactivate_list && !following_in_activate_list) + { + const auto conflict_status = handle_conflict( + "The following controller with name '" + following_ctrl.info.name + + "' cannot be deactivated because the preceding controller with name '" + + controller.info.name + "' is active and will be restarted.", + "", following_ctrl.info.name); + + if (conflict_status == ConflictStatus::CONFLICT_WITH_STRICT) + { + // STRICTの場合は即座にエラーを返す + RCLCPP_ERROR( + logger, "Switching controllers failed due to strict conflict: %s", + controller.info.name.c_str()); + return controller_interface::return_type::ERROR; + } + else if (conflict_status == ConflictStatus::CONFLICT_WITH_BEST_EFFORT) + { + // BEST_EFFORTの場合はfollowingをdeactivateリストから削除したため、次のfollowing に進む + continue; + } + } + } + + RCLCPP_INFO(logger, "--> NO CONFLICT"); + // activate/deactivate request に conflict が検出されなかった場合は、 controller + // の状態に変更に伴う chained_mode 変更の判定に進む + + // preceding のactivate/deactivate に合わせて following の 状態変更をリクエストする + // following が active 状態の場合は、chained_mode の変更時に restart + // が必要となるためそのリクエストも同時に追加する + const auto add_following_to_restart_request_if_needed = [&]() + { + if (following_is_active && !following_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()); + + if ( + std::find( + deactivate_request.begin(), deactivate_request.end(), following_ctrl.info.name) == + deactivate_request.end()) + { + deactivate_request.push_back(following_ctrl.info.name); + } + if ( + std::find(activate_request.begin(), activate_request.end(), following_ctrl.info.name) == + activate_request.end()) + { + activate_request.push_back(following_ctrl.info.name); + } + }; + }; + + const bool preceding_will_be_restart = (is_active && in_deactivate_list && in_activate_list); + const bool following_will_be_restart = + (following_is_active && following_in_deactivate_list && following_in_activate_list); + + if ( + (in_deactivate_list && !preceding_will_be_restart) || + (preceding_will_be_restart && following_will_be_restart)) + { + if ( + std::find( + from_chained_mode_request.begin(), from_chained_mode_request.end(), + following_ctrl.info.name) == from_chained_mode_request.end()) + { + RCLCPP_DEBUG( + logger, "Adding following controller with name '%s' to 'from' chained mode request.", + following_ctrl.info.name.c_str()); + + from_chained_mode_request.push_back(following_ctrl.info.name); + add_following_to_restart_request_if_needed(); + } + } + if ( + (in_activate_list && !preceding_will_be_restart) || + (preceding_will_be_restart && following_will_be_restart)) + { + if ( + std::find( + to_chained_mode_request.begin(), to_chained_mode_request.end(), + following_ctrl.info.name) == to_chained_mode_request.end()) + { + RCLCPP_DEBUG( + logger, "Adding following controller with name '%s' to 'to' chained mode request.", + following_ctrl.info.name.c_str()); + + to_chained_mode_request.push_back(following_ctrl.info.name); + add_following_to_restart_request_if_needed(); + + // 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); + } + } + } + } + } + + return controller_interface::return_type::OK; +} + +#if 0 controller_interface::return_type ControllerManager::check_following_controllers_for_activate( const std::vector & controllers, int strictness, - const ControllersListIterator controller_it) + const ControllersListIterator controller_it) const { // we assume that the controller exists is checked in advance RCLCPP_DEBUG( @@ -2397,10 +3193,10 @@ controller_interface::return_type ControllerManager::check_following_controllers { 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 + // 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 @@ -2419,24 +3215,30 @@ controller_interface::return_type ControllerManager::check_following_controllers return controller_interface::return_type::ERROR; } + const bool following_will_be_deactivated = + std::find( + deactivate_request_.begin(), deactivate_request_.end(), following_ctrl_it->info.name) != + deactivate_request_.end(); + + const bool following_will_be_activated = + std::find(activate_request_.begin(), activate_request_.end(), following_ctrl_it->info.name) != + activate_request_.end(); + if (is_controller_active(following_ctrl_it->c)) { // will following controller be deactivated? - if ( - std::find( - deactivate_request_.begin(), deactivate_request_.end(), following_ctrl_it->info.name) != - deactivate_request_.end()) + if (following_will_be_deactivated && !following_will_be_activated) { RCLCPP_WARN( - get_logger(), "The following controller with name '%s' will be deactivated.", + get_logger(), + "The following controller with name '%s' will be deactivated and will not be " + "activated.", following_ctrl_it->info.name.c_str()); return controller_interface::return_type::ERROR; } } // check if following controller will not be activated - else if ( - std::find(activate_request_.begin(), activate_request_.end(), following_ctrl_it->info.name) == - activate_request_.end()) + else if (!following_will_be_activated) { RCLCPP_WARN( get_logger(), @@ -2458,10 +3260,17 @@ controller_interface::return_type ControllerManager::check_following_controllers // 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 + // // 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()) + +#if 0 + // If the target following_ctrl to be activated is not in chained_mode, add it to + // to_chained_mode_request. Alternatively, if it is already in chained_mode but is subject to + // reset, it needs to be removed from chained_mode once during deactivation and then + // transitioned back to chained_mode, so add it to to_chained_mode_request. + if (following_will_be_activated) { auto found_it = std::find( to_chained_mode_request_.begin(), to_chained_mode_request_.end(), @@ -2469,15 +3278,26 @@ controller_interface::return_type ControllerManager::check_following_controllers 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); + +#if 0 // activate順などでうまく available にしたほうがよいためここは無効化 + // if it is a chainable controller and not in chained_mode, make the reference interfaces + // available on preactivation (This is needed when you activate a couple of chainable + // controller altogether) + if (!following_ctrl_it->c->is_in_chained_mode() && !following_will_be_deactivated) + { + resource_manager_->make_controller_reference_interfaces_available( + following_ctrl_it->info.name); + } +#endif + RCLCPP_DEBUG( get_logger(), "Adding controller '%s' in 'to chained mode' request.", following_ctrl_it->info.name.c_str()); } } +#endif + +#if 0 else { // Check if following controller is in 'from' chained mode list and remove it, if so @@ -2494,17 +3314,23 @@ controller_interface::return_type ControllerManager::check_following_controllers following_ctrl_it->info.name.c_str()); } } +#endif } 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) + const ControllersListIterator controller_it) const { // if not chainable no need for any checks if (!controller_it->c->is_chainable()) { + RCLCPP_DEBUG( + get_logger(), + "Controller with name '%s' is not chainable. " + "No need to do any checks of preceding controllers when stopping it.", + controller_it->info.name.c_str()); return controller_interface::return_type::OK; } @@ -2527,7 +3353,7 @@ controller_interface::return_type ControllerManager::check_preceeding_controller { std::vector preceding_controllers_using_ref_itf; - // TODO(destogl): This data could be cached after configuring controller into a map for faster + // TODO(destogl): This data could be cached after configuring controller into a map for // access here for (auto preceding_ctrl_it = controllers.begin(); preceding_ctrl_it != controllers.end(); ++preceding_ctrl_it) @@ -2543,12 +3369,36 @@ controller_interface::return_type ControllerManager::check_preceeding_controller continue; } - // check if preceding controller will be activated - if ( - is_controller_inactive(preceding_ctrl_it->c) && + const bool this_controller_will_be_reset = + (std::find(activate_request_.begin(), activate_request_.end(), controller_it->info.name) != + activate_request_.end()); + + const bool preceding_will_be_deactivated = + std::find( + deactivate_request_.begin(), deactivate_request_.end(), preceding_ctrl_it->info.name) != + deactivate_request_.end(); + + const bool preceding_will_be_activated = std::find( activate_request_.begin(), activate_request_.end(), preceding_ctrl_it->info.name) != - activate_request_.end()) + activate_request_.end(); + + const bool preceding_will_be_reset = + (preceding_will_be_deactivated && preceding_will_be_activated); + + // if controller and preceding controller will be reset go the next once + if (this_controller_will_be_reset && preceding_will_be_reset) + { + RCLCPP_DEBUG( + get_logger(), + "Controller with name '%s' and preceding controller with name '%s' " + "will be reset.", + controller_it->info.name.c_str(), preceding_ctrl_it->info.name.c_str()); + continue; + } + + // check if preceding controller will be activated + if (is_controller_inactive(preceding_ctrl_it->c) && preceding_will_be_activated) { RCLCPP_WARN( get_logger(), @@ -2558,11 +3408,7 @@ controller_interface::return_type ControllerManager::check_preceeding_controller return controller_interface::return_type::ERROR; } // check if preceding controller will not be deactivated - else if ( - is_controller_active(preceding_ctrl_it->c) && - std::find( - deactivate_request_.begin(), deactivate_request_.end(), preceding_ctrl_it->info.name) == - deactivate_request_.end()) + else if (is_controller_active(preceding_ctrl_it->c) && !preceding_will_be_deactivated) { RCLCPP_WARN( get_logger(), @@ -2571,18 +3417,21 @@ controller_interface::return_type ControllerManager::check_preceeding_controller controller_it->info.name.c_str(), preceding_ctrl_it->info.name.c_str()); return controller_interface::return_type::ERROR; } + // 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 + // // insert to the begin of activate request list to be activated before preceding + // controller // activate_request_.insert(activate_request_.begin(), preceding_ctrl_name); // } } } return controller_interface::return_type::OK; } +#endif 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..c5a02fa6ea --- /dev/null +++ b/controller_manager/test/test_controller_with_command/test_controller_with_command.hpp @@ -0,0 +1,86 @@ +// 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 + +#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_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_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..0cb59f26a2 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 @@ -54,6 +65,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_reset); + FRIEND_TEST( + TestControllerChainingWithControllerManager, test_chained_controllers_reset_error_handling); }; class TestableControllerManager : public controller_manager::ControllerManager @@ -88,6 +102,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_reset); + FRIEND_TEST( + TestControllerChainingWithControllerManager, test_chained_controllers_reset_error_handling); public: TestableControllerManager( @@ -369,6 +386,16 @@ class TestControllerChainingWithControllerManager {}, {controller_name}, test_param.strictness, expected_future_status, expected_return); } + void ResetControllers( + 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 +448,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) + 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); + } + // 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 +565,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; @@ -917,6 +1043,8 @@ TEST_P( // Test Case 6: following controller is deactivated but preceding controller will be activated // --> return error; controllers stay in the same state + RCLCPP_ERROR(cm_->get_logger(), "======================================"); + switch_test_controllers( {DIFF_DRIVE_CONTROLLER}, {PID_LEFT_WHEEL, PID_RIGHT_WHEEL}, test_param.strictness, expected.at(test_param.strictness).future_status, @@ -943,6 +1071,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,8 +1082,11 @@ 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 following controllers + // 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); @@ -964,6 +1098,9 @@ 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 a following controller switch_test_controllers( @@ -977,6 +1114,25 @@ 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 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); + + // 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()); } TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_adding_in_random_order) @@ -1106,6 +1262,291 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add UpdateAllControllerAndCheck(reference, 3u); } +TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_reset) +{ + SetupWithActivationAllControllersAndCheck(); + + // Verify update (internal_counter 2->3) + UpdateAllControllerAndCheck({32.0, 128.0}, 2u); + + { + // Reset the most preceding controller (position tracking controller) + // (internal_counter 3->5) + ResetControllers({POSITION_TRACKING_CONTROLLER}); + + // Following controllers should stay active + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + // Preceding controller should return to active (active -> inactive -> active) + EXPECT_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 reset + 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 reset 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 (reset) + 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 (reset) + + // Verify update after reset most preceding controller + // (internal_counter 5->6) + UpdateAllControllerAndCheck({1024.0, 4096.0}, 5u); + } + + { + // Reset 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 reset process) + ResetControllers( + {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 reset + 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 (reset) + ASSERT_EQ(3u, pid_right_wheel_controller->activate_calls); // +1 (reset) + ASSERT_EQ(3u, diff_drive_controller->activate_calls); // +1 (reset) + ASSERT_EQ(3u, position_tracking_controller->activate_calls); // +1 (reset) + ASSERT_EQ(2u, pid_left_wheel_controller->deactivate_calls); // +1 (reset) + ASSERT_EQ(2u, pid_right_wheel_controller->deactivate_calls); // +1 (reset) + ASSERT_EQ(2u, diff_drive_controller->deactivate_calls); // +1 (reset) + ASSERT_EQ(2u, position_tracking_controller->deactivate_calls); // +1 (reset) + + // Verify update again after reset all controllers + // (internal_counter 8->9->10) + UpdateAllControllerAndCheck({16.0, 64.0}, 8u); + UpdateAllControllerAndCheck({512.0, 2048.0}, 9u); + } + + { + // Reset 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 (reset) + 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 (reset) + ASSERT_EQ(3u, position_tracking_controller->deactivate_calls); // +1 (deactivate) + } + + { + // Reset the preceding controller (diff_drive_controller) and a following controller + // (pid_right_wheel_controller) + ResetControllers({PID_RIGHT_WHEEL, DIFF_DRIVE_CONTROLLER}); + + // Verify that the claimed state of the interface does not change before and after the reset + 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 reset 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 (reset) + ASSERT_EQ(5u, diff_drive_controller->activate_calls); // +1 (reset) + 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 (reset) + ASSERT_EQ(4u, diff_drive_controller->deactivate_calls); // +1 (reset) + ASSERT_EQ(3u, position_tracking_controller->deactivate_calls); // +0 + } + + { + // Reset 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 (reset) + 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 (reset) + ASSERT_EQ(3u, position_tracking_controller->deactivate_calls); // +0 + } +} + +TEST_P( + TestControllerChainingWithControllerManager, + test_chained_controllers_reset_preceding_and_stop_following) +{ + SetupWithActivationAllControllersAndCheck(); + + // Reset 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 reset, 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 reset + 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 + } + else if (test_param.strictness == BEST_EFFORT) + { + // If BEST_EFFORT, since the stop request for the diff_drive_controller is 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 reset + 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 (reset) + 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 (reset) + } +} + +TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_reset_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: reset following controllers but preceding controllers will not be reset + // --> return error; reset will not be executed and controllers stay in the same state as they + // were + { + const auto verify_all_controllers_are_active_and_not_reset = [&]() + { + // 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); + }; + + // Attempt to reset the lowest following controllers (pid_controllers) + ResetControllers({PID_LEFT_WHEEL, PID_RIGHT_WHEEL}, exp.return_type, std::future_status::ready); + verify_all_controllers_are_active_and_not_reset(); + + // Attempt to reset the lowest following controller (pid_right_wheel_controller) + ResetControllers({PID_RIGHT_WHEEL}, exp.return_type, std::future_status::ready); + verify_all_controllers_are_active_and_not_reset(); + + // Attempt to reset the middle following controller (diff_drive_controller) + ResetControllers({DIFF_DRIVE_CONTROLLER}, exp.return_type, std::future_status::ready); + verify_all_controllers_are_active_and_not_reset(); + + // Attempt to reset the all following controllers (pid_controllers and diff_drive_controller) + ResetControllers( + {PID_LEFT_WHEEL, PID_RIGHT_WHEEL, DIFF_DRIVE_CONTROLLER}, exp.return_type, + std::future_status::ready); + verify_all_controllers_are_active_and_not_reset(); + } +} + INSTANTIATE_TEST_SUITE_P( test_strict_best_effort, TestControllerChainingWithControllerManager, - testing::Values(strict, best_effort)); + testing::Values(strict, best_effort), [](const testing::TestParamInfo & info_) + { return info_.param.strictness == STRICT ? "STRICT" : "BEST_EFFORT"; }); diff --git a/controller_manager/test/test_reset_controller.cpp b/controller_manager/test/test_reset_controller.cpp new file mode 100644 index 0000000000..1f1c511680 --- /dev/null +++ b/controller_manager/test/test_reset_controller.cpp @@ -0,0 +1,280 @@ +// 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 + +#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; +// using ::testing::_; +// using ::testing::Return; +const auto CONTROLLER_NAME_1 = "test_controller1"; +const auto CONTROLLER_NAME_2 = "test_controller2"; +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 TestResetController : 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_1, 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_1); + 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_1}, 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 reset 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_1}, strictness, expected_future_status, + expected_interface_status); + } + + void reset_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_1}, strvec{CONTROLLER_NAME_1}, strictness, expected_future_status, + expected_interface_status); + } + + std::shared_ptr test_controller; +}; + +TEST_P(TestResetController, 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 reset to DEACTIVATE_VALUE + ASSERT_EQ(SIMULATE_COMMAND_DEACTIVATE_VALUE, test_controller->simulate_command); + } +} + +TEST_P(TestResetController, can_reset_active_controller) +{ + const auto test_param = GetParam(); + + // Configure controller + configure_and_check_test_controller(); + + { + // Start controller before reset + start_and_check_test_controller(test_param.strictness); + const double OVERRIDE_COMMAND_VALUE = 12121212.0; + + // Override command to check reset behavior + test_controller->simulate_command = OVERRIDE_COMMAND_VALUE; + ASSERT_EQ(OVERRIDE_COMMAND_VALUE, test_controller->simulate_command); + } + + { + // Reset controller + reset_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 reset to ACTIVATE_VALUE + ASSERT_EQ(SIMULATE_COMMAND_ACTIVATE_VALUE, test_controller->simulate_command); + } +} + +TEST_P(TestResetController, reset_inactive_controller) +{ + const auto test_param = GetParam(); + + // Configure controller + configure_and_check_test_controller(); + + // Reset controller without starting it + reset_test_controller( + test_param.strictness, test_param.expected_future_status, test_param.expected_return); + + // STRICT: can not reset 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 reset is executed while inactive, only the start_controller process will be + // effective, resulting in activation + else 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 reset to ACTIVATE_VALUE + ASSERT_EQ(SIMULATE_COMMAND_ACTIVATE_VALUE, test_controller->simulate_command); + } + else + { + FAIL(); + } +} + +TEST_P(TestResetController, can_not_reset_unconfigured_controller) +{ + const auto test_param = GetParam(); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + + // Can not reset unconfigured controller + reset_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(TestResetController, can_not_reset_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 reset finalized controller + reset_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, TestResetController, testing::Values(test_strict, test_best_effort)); From 3989c246233697374dc2a6f37a0e49b823f2b272 Mon Sep 17 00:00:00 2001 From: Takashi Sato Date: Mon, 10 Jun 2024 18:29:06 +0900 Subject: [PATCH 02/16] refactor --- .../controller_manager/controller_manager.hpp | 65 +- controller_manager/src/controller_manager.cpp | 1913 ++++++----------- 2 files changed, 635 insertions(+), 1343 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 1e4899c122..0cf131baf1 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -206,12 +206,6 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC void shutdown_async_controllers_and_components(); - CONTROLLER_MANAGER_PUBLIC - std::vector get_ordered_controllers_names() const - { - return ordered_controllers_names_; - } - protected: CONTROLLER_MANAGER_PUBLIC void init_services(); @@ -346,17 +340,7 @@ 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 propagate_activation_and_deactivation_of_chained_mode( - const std::vector & controllers); - +#if 0 /// Check if all the following controllers will be in active state and in the chained mode /// after controllers' switch. /** @@ -385,34 +369,25 @@ class ControllerManager : public rclcpp::Node controller_interface::return_type check_following_controllers_for_activate( const std::vector & controllers, int strictness, const ControllersListIterator controller_it) 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. - * - * NOTE: The automatically adding of preceding controllers into deactivate list is not implemented - * yet. - * - * \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. - */ - controller_interface::return_type check_preceeding_controllers_for_deactivate( - const std::vector & controllers, int strictness, - const ControllersListIterator controller_it) const; - - controller_interface::return_type check_preceding_and_following_controllers_for_switch( - const std::vector & controllers, std::vector & deactivate_request, - std::vector & activate_request, std::vector & to_chained_mode_request, - std::vector & from_chained_mode_request, const int strictness) const; +#endif + + 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); + + controller_interface::return_type + check_de_activate_request_conflict_and_create_chained_mode_request( + const int strictness, const std::vector & controllers, + std::vector & deactivate_request, std::vector & activate_request, + std::vector & to_chained_mode_request, + std::vector & from_chained_mode_request) const; /** * @brief Inserts a controller into an ordered list based on dependencies to compute the diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index c5ce27cfc5..8ef005d2ba 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -154,6 +154,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) @@ -171,6 +205,68 @@ 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; +} + +std::vector> +list_following_controllers( + const controller_manager::ControllerSpec & controller, const ActivationState & controller_state, + const std::vector & controllers) +{ + // If the controller is neither active nor inactive (in which case it is likely either + // unconfigured or finalized), it is impossible to retrieve the command_interface configuration, + // so return an empty vector + if (!controller_state.is_active && !controller_state.is_inactive) + { + return {}; + } + + std::vector> following_ctrl_refs; + for (const auto & cmd_itf_name : controller.c->command_interface_configuration().names) + { + // controller that 'cmd_tf_name' belongs to + controller_manager::ControllersListIterator following_ctrl_it; + if (!command_interface_is_reference_interface_of_controller( + cmd_itf_name, controllers, following_ctrl_it)) + { + // interfaces that are not related to the chain do not need to be checked + continue; + } + + // check if the controller is already in the list + if ( + std::find_if( + following_ctrl_refs.begin(), following_ctrl_refs.end(), + [&](const auto & ctrl) { return ctrl.get().info.name == following_ctrl_it->info.name; }) == + following_ctrl_refs.end()) + { + following_ctrl_refs.push_back(std::cref(*following_ctrl_it)); + } + } + + return following_ctrl_refs; +} + } // namespace namespace controller_manager @@ -851,42 +947,49 @@ controller_interface::return_type ControllerManager::switch_controller( { switch_params_ = SwitchParams(); - if (!deactivate_request_.empty() || !activate_request_.empty()) - { - RCLCPP_FATAL( - get_logger(), - "The internal deactivate and activate request lists are not empty at the beginning of the " - "switch_controller() call. This should never happen."); - throw std::runtime_error("CM's internal state is not correct. See the FATAL message above."); - } - if ( - !deactivate_command_interface_request_.empty() || !activate_command_interface_request_.empty()) + const auto validate_requests_and_internal_states = [this, &strictness]() { - RCLCPP_FATAL( - get_logger(), - "The internal deactivate and activat 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."); - } - if (!from_chained_mode_request_.empty() || !to_chained_mode_request_.empty()) - { - RCLCPP_FATAL( - get_logger(), - "The internal 'from' and 'to' chained mode requests 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."); - } - if (strictness == 0) - { - RCLCPP_WARN( - get_logger(), - "Controller Manager: to switch controllers you need to specify a " - "strictness level of controller_manager_msgs::SwitchController::STRICT " - "(%d) or ::BEST_EFFORT (%d). Defaulting to ::BEST_EFFORT", - controller_manager_msgs::srv::SwitchController::Request::STRICT, - controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT); - strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; - } + if (!deactivate_request_.empty() || !activate_request_.empty()) + { + RCLCPP_FATAL( + get_logger(), + "The internal deactivate and activate request lists are not empty at the beginning of the " + "switch_controller() call. This should never happen."); + throw std::runtime_error("CM's internal state is not correct. See the FATAL message above."); + } + if ( + !deactivate_command_interface_request_.empty() || + !activate_command_interface_request_.empty()) + { + RCLCPP_FATAL( + get_logger(), + "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."); + } + if (!from_chained_mode_request_.empty() || !to_chained_mode_request_.empty()) + { + RCLCPP_FATAL( + get_logger(), + "The internal 'from' and 'to' chained mode requests 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."); + } + if (strictness == 0) + { + RCLCPP_WARN( + get_logger(), + "Controller Manager: to switch controllers you need to specify a " + "strictness level of controller_manager_msgs::SwitchController::STRICT " + "(%d) or ::BEST_EFFORT (%d). Defaulting to ::BEST_EFFORT", + controller_manager_msgs::srv::SwitchController::Request::STRICT, + controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT); + strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; + } + }; + + validate_requests_and_internal_states(); RCLCPP_DEBUG(get_logger(), "Activating controllers:"); for (const auto & controller : activate_controllers) @@ -946,7 +1049,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; } @@ -954,8 +1057,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; } @@ -988,632 +1090,140 @@ controller_interface::return_type ControllerManager::switch_controller( RCLCPP_WARN(get_logger(), "STRICTNESS: %d", strictness); show_list("REQUEST"); -#if 1 - auto sortAByB = - [](std::vector & A, const std::vector & B, bool reverse = false) - { - // Bの要素の位置を記録するマップを作成 - std::unordered_map indexMap; - for (size_t i = 0; i < B.size(); ++i) - { - indexMap[B[i]] = i; - } - - // Aの要素をBの順序に従ってソート - std::sort( - A.begin(), A.end(), - [&indexMap, reverse](const std::string & a, const std::string & b) - { - if (reverse) - { - return indexMap[a] > indexMap[b]; - } - else - { - return indexMap[a] < indexMap[b]; - } - }); - }; - - // sortAByB(activate_request_, ordered_controllers_names_); - // sortAByB(deactivate_request_, ordered_controllers_names_); - // show_list("SORTED"); -#endif - // lock controllers std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); const std::vector & controllers = rt_controllers_wrapper_.get_updated_list(guard); - // 関数の入出力を明確にするために、メンバ変数の参照をローカル変数に持たせて各ラムダに渡す - auto & activate_request = activate_request_; - auto & deactivate_request = deactivate_request_; - const auto & logger = get_logger(); - - // cleanup switch request first - const auto cleanup_request = - [&controllers, &activate_request, &deactivate_request, &strictness, &logger]() - { - const auto check_following_is_chainable = [&controllers, &logger](const auto & controller) - { - for (const auto & cmd_itf_name : controller->command_interface_configuration().names) - { - 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( - logger, "Checking following controller with name '%s'.", - following_ctrl_it->info.name.c_str()); - - // check if following controller is chainable - if (!following_ctrl_it->c->is_chainable()) - { - RCLCPP_WARN( - 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 false; - } - } - return true; - }; - - for (const auto & controller : controllers) - { - const bool is_active = is_controller_active(*controller.c); - const bool is_inactive = is_controller_inactive(*controller.c); - - // 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); - bool 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(logger, "%s", msg.c_str()); - deactivate_request.clear(); - activate_request.clear(); - return controller_interface::return_type::ERROR; - } - RCLCPP_WARN(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); - } - - // followingが chainable でない場合、activate できない - if ( - (is_active || is_inactive) && in_activate_list && - !check_following_is_chainable(controller.c)) - { - auto conflict_status = handle_conflict( - "Could not activate controller '" + controller.info.name + - "' since it has not chainable following controller"); - if (conflict_status != controller_interface::return_type::OK) - { - return conflict_status; - } - in_activate_list = false; - activate_request.erase(activate_list_it); - } - } - - return controller_interface::return_type::OK; - }; - - if (cleanup_request() != controller_interface::return_type::OK) + // 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). + // Simultaneously, if the (de)activate request necessitates switching the chained mode of the + // following controller, make the corresponding 'from'/'to' chained mode request. + if ( + check_de_activate_request_conflict_and_create_chained_mode_request( + strictness, controllers, deactivate_request_, activate_request_, to_chained_mode_request_, + from_chained_mode_request_) != controller_interface::return_type::OK) { RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)"); // reset all lists clear_requests(); return controller_interface::return_type::ERROR; } - show_list("AFTER CLEANUP"); -LABEL_START: -#if 0 // check deactivate の後に移動 - // if a preceding controller is deactivated, all first-level controllers should be switched 'from' - // chained mode - propagate_deactivation_of_chained_mode(controllers); - show_list("AFTER PROPAGATE"); -#endif + show_list("FINALLY"); -#if 1 - if ( - check_preceding_and_following_controllers_for_switch( - controllers, deactivate_request, activate_request, to_chained_mode_request_, - from_chained_mode_request_, strictness) != controller_interface::return_type::OK) + // check if we need to request switch controllers after all the checks + if (activate_request_.empty() && deactivate_request_.empty()) { - RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)"); - // reset all lists + RCLCPP_INFO(get_logger(), "Empty activate and deactivate list, not requesting switch"); clear_requests(); - return controller_interface::return_type::ERROR; + return controller_interface::return_type::OK; } - show_list("AFTER CHECK FOR SWITCH"); -#else - // 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();) - { - 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; + extract_de_activate_command_interface_request( + controllers, deactivate_request_, activate_request_, deactivate_command_interface_request_, + activate_command_interface_request_); - // if controller is neither inactive nor active then do not do any following-controllers - // checks - if (!is_controller_inactive(controller_it->c) && !is_controller_active(controller_it->c)) - { - RCLCPP_WARN( - get_logger(), - "Controller with name '%s' is neither inactive and nor active 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); - } + 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_) + { + RCLCPP_DEBUG(get_logger(), " - %s", interface.c_str()); + } + RCLCPP_DEBUG(get_logger(), "Release of command interfaces from deactivating controllers:"); + for (const auto & interface : deactivate_command_interface_request_) + { + RCLCPP_DEBUG(get_logger(), " - %s", interface.c_str()); + } - if (status != controller_interface::return_type::OK) + if ( + !activate_command_interface_request_.empty() || !deactivate_command_interface_request_.empty()) + { + if (!resource_manager_->prepare_command_mode_switch( + activate_command_interface_request_, deactivate_command_interface_request_)) { - RCLCPP_WARN( + RCLCPP_ERROR( 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 - ctrl_it = activate_request_.erase(ctrl_it); - goto LABEL_START; - } - 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; - } - } - else - { - // increment iterator only if controller was not removed from the list - ++ctrl_it; + "Could not switch controllers since prepare command mode switch was rejected."); + clear_requests(); + return controller_interface::return_type::ERROR; } } - show_list("AFTER CHECK ACTIVATE"); + // start the atomic controller switching + switch_params_.strictness = strictness; + switch_params_.activate_asap = activate_asap; + switch_params_.init_time = rclcpp::Clock().now(); + switch_params_.timeout = timeout; + switch_params_.do_switch = true; - // check if controllers should be deactivated if used in chained mode - for (auto ctrl_it = deactivate_request_.begin(); ctrl_it != deactivate_request_.end();) + // wait until switch is finished + RCLCPP_DEBUG(get_logger(), "Requested atomic controller switch from realtime loop"); + int timeout_count = 0; + while (rclcpp::ok() && switch_params_.do_switch) { - 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)) + if (!rclcpp::ok()) { - 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; + return controller_interface::return_type::ERROR; } - else + + if (++timeout_count > 10 * 3000) { - status = check_preceeding_controllers_for_deactivate(controllers, strictness, controller_it); + RCLCPP_ERROR(get_logger(), "Timeout in atomic controller switch"); + return controller_interface::return_type::ERROR; } - if (status != controller_interface::return_type::OK) + std::this_thread::sleep_for(std::chrono::microseconds(100)); + } + + // copy the controllers spec from the used to the unused list + std::vector & to = rt_controllers_wrapper_.get_unused_list(guard); + to = controllers; + + // update the claimed interface controller info + for (auto & controller : to) + { + if (is_controller_active(controller.c)) { - 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) + auto command_interface_config = controller.c->command_interface_configuration(); + if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) { - // 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 - ctrl_it = deactivate_request_.erase(ctrl_it); - goto LABEL_START; + controller.info.claimed_interfaces = resource_manager_->available_command_interfaces(); } - if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + if ( + command_interface_config.type == + controller_interface::interface_configuration_type::INDIVIDUAL) { - RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)"); - // reset all lists - clear_requests(); - return controller_interface::return_type::ERROR; + controller.info.claimed_interfaces = command_interface_config.names; } } else { - // increment iterator only if controller was not removed from the list - ++ctrl_it; + controller.info.claimed_interfaces.clear(); } } - show_list("AFTER CHECK DEACTIVATE"); -#endif - -#if 0 - propagate_activation_and_deactivation_of_chained_mode(controllers); - show_list("AFTER PROPAGATE"); - -// // if a preceding controller is deactivated, all first-level controllers should be switched -// 'from' -// // chained mode -// propagate_deactivation_of_chained_mode(controllers); -// show_list("AFTER PROPAGATE DEACTIVATION"); - -// // if a preceding controller is activated, all first-level controllers should be switched -// 'to' -// // chained mode -// propagate_activation_of_chained_mode(controllers); -// show_list("AFTER PROPAGATE ACTIVATION"); -#endif + // switch lists + rt_controllers_wrapper_.switch_updated_list(guard); + // clear unused list + rt_controllers_wrapper_.get_unused_list(guard).clear(); - for (const auto & controller : controllers) - { - // 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(); + clear_requests(); - // 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(); + RCLCPP_DEBUG(get_logger(), "Successfully switched controllers"); + return controller_interface::return_type::OK; +} - 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(); +controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_controller_impl( + const ControllerSpec & controller) +{ + // lock controllers + std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); - 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(); - -#if 0 - 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) - { - if ( - std::find(deactivate_request_.begin(), deactivate_request_.end(), controller.info.name) == - deactivate_request_.end()) - { - deactivate_request_.push_back(controller.info.name); - } - - if ( - std::find(activate_request_.begin(), activate_request_.end(), controller.info.name) == - activate_request_.end()) - { - activate_request_.push_back(controller.info.name); - } - } - } -#endif - -#if 0 - // 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); - } -#endif - - 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_); - } - - // 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) - { - 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); - } - } - -#if 1 - sortAByB(deactivate_request_, ordered_controllers_names_); - sortAByB(activate_request_, ordered_controllers_names_, true); - show_list("SORTED"); -#endif - - show_list("FINALLY"); - - 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; - } - - RCLCPP_DEBUG(get_logger(), "Request for command interfaces from activating controllers:"); - for (const auto & interface : activate_command_interface_request_) - { - RCLCPP_DEBUG(get_logger(), " - %s", interface.c_str()); - } - RCLCPP_DEBUG(get_logger(), "Release of command interfaces from deactivating controllers:"); - for (const auto & interface : deactivate_command_interface_request_) - { - RCLCPP_DEBUG(get_logger(), " - %s", interface.c_str()); - } - - if ( - !activate_command_interface_request_.empty() || !deactivate_command_interface_request_.empty()) - { - if (!resource_manager_->prepare_command_mode_switch( - activate_command_interface_request_, deactivate_command_interface_request_)) - { - RCLCPP_ERROR( - get_logger(), - "Could not switch controllers since prepare command mode switch was rejected."); - clear_requests(); - return controller_interface::return_type::ERROR; - } - } - - // start the atomic controller switching - switch_params_.strictness = strictness; - switch_params_.activate_asap = activate_asap; - switch_params_.init_time = rclcpp::Clock().now(); - switch_params_.timeout = timeout; - switch_params_.do_switch = true; - - // wait until switch is finished - RCLCPP_DEBUG(get_logger(), "Requested atomic controller switch from realtime loop"); - int timeout_count = 0; - while (rclcpp::ok() && switch_params_.do_switch) - { - if (!rclcpp::ok()) - { - return controller_interface::return_type::ERROR; - } - - if (++timeout_count > 10 * 3000) - { - RCLCPP_ERROR(get_logger(), "Timeout in atomic controller switch"); - return controller_interface::return_type::ERROR; - } - - std::this_thread::sleep_for(std::chrono::microseconds(100)); - } - - // copy the controllers spec from the used to the unused list - std::vector & to = rt_controllers_wrapper_.get_unused_list(guard); - to = controllers; - - // update the claimed interface controller info - for (auto & controller : to) - { - if (is_controller_active(controller.c)) - { - auto command_interface_config = controller.c->command_interface_configuration(); - if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) - { - controller.info.claimed_interfaces = resource_manager_->available_command_interfaces(); - } - if ( - command_interface_config.type == - controller_interface::interface_configuration_type::INDIVIDUAL) - { - controller.info.claimed_interfaces = command_interface_config.names; - } - } - else - { - controller.info.claimed_interfaces.clear(); - } - } - - // switch lists - rt_controllers_wrapper_.switch_updated_list(guard); - // clear unused list - rt_controllers_wrapper_.get_unused_list(guard).clear(); - - clear_requests(); - - RCLCPP_DEBUG(get_logger(), "Successfully switched controllers"); - return controller_interface::return_type::OK; -} - -controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_controller_impl( - const ControllerSpec & controller) -{ - // lock controllers - std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); - - std::vector & to = rt_controllers_wrapper_.get_unused_list(guard); - const std::vector & from = rt_controllers_wrapper_.get_updated_list(guard); + std::vector & to = rt_controllers_wrapper_.get_unused_list(guard); + const std::vector & from = rt_controllers_wrapper_.get_updated_list(guard); // Copy all controllers from the 'from' list to the 'to' list to = from; @@ -2646,792 +2256,499 @@ 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); } } -} +}; -void ControllerManager::propagate_activation_and_deactivation_of_chained_mode( - const std::vector & controllers) +void ControllerManager::cache_controller_interfaces_in_activate_request( + const std::vector & controllers, + const std::vector & activate_request) { - const auto add_following_to_restart_request_if_needed = [&](auto controller_it) + auto & resource_manager = *resource_manager_; + + const auto cache_controller_to_hardware = [&resource_manager](const ControllerSpec & controller) { - const bool is_active = is_controller_active(controller_it->c); - const bool will_be_deactivated = - std::find(deactivate_request_.begin(), deactivate_request_.end(), controller_it->info.name) != - deactivate_request_.end(); + std::vector interface_names = {}; - if (is_active && !will_be_deactivated) + auto command_interface_config = controller.c->command_interface_configuration(); + if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) { - deactivate_request_.push_back(controller_it->info.name); - activate_request_.push_back(controller_it->info.name); - // need to traverse again - return false; - }; + 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; + } - // no need to traverse - return true; + interface_names.insert(interface_names.end(), interfaces.begin(), interfaces.end()); + + resource_manager.cache_controller_to_hardware(controller.info.name, interface_names); }; - const auto propagate_deactivation = [&]() + for (const auto & controller : controllers) { - for (auto ctrl_it = deactivate_request_.begin(); ctrl_it != deactivate_request_.end(); - ++ctrl_it) + 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) { - auto controller_it = std::find_if( - controllers.begin(), controllers.end(), - std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it)); + cache_controller_to_hardware(controller); + } + } +}; - // if controller is not active then skip adding following-controllers to "from" chained mode - // request - if (!is_controller_active(controller_it->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_it->info.name.c_str()); - break; - } - - for (const auto & cmd_itf_name : controller_it->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)) - { - continue; - } +enum class ConflictStatus +{ + NO_CONFLICT, + CONFLICT_WITH_BEST_EFFORT, + CONFLICT_WITH_STRICT, +}; - // 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()); - - if (!add_following_to_restart_request_if_needed(following_ctrl_it)) - { - // need to traverse again - return false; - } - } - } +controller_interface::return_type check_consistency_of_controller( + const rclcpp::Logger & logger, hardware_interface::ResourceManager & resource_manager, + const ControllerSpec & controller, std::vector & deactivate_request, + std::vector & activate_request, std::vector & to_chained_mode_request, + std::vector & from_chained_mode_request, const int strictness) +{ + const auto handle_conflict = [&](const std::string & msg) + { + if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + { + RCLCPP_ERROR(logger, "%s", msg.c_str()); + return ConflictStatus::CONFLICT_WITH_STRICT; } - // finish traverse - return true; + RCLCPP_WARN(logger, "%s", msg.c_str()); + return ConflictStatus::CONFLICT_WITH_BEST_EFFORT; }; - const auto propagate_activation = [&]() + 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) { - for (auto ctrl_it = activate_request_.begin(); ctrl_it != activate_request_.end(); ++ctrl_it) + if ( + handle_conflict( + "Could not deactivate controller '" + controller.info.name + "' since it is not active") == + ConflictStatus::CONFLICT_WITH_STRICT) { - auto controller_it = std::find_if( - controllers.begin(), controllers.end(), - std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it)); - - const bool controller_will_be_deactivated = - std::find( - deactivate_request_.begin(), deactivate_request_.end(), controller_it->info.name) != - deactivate_request_.end(); - - // if controller is active then skip adding following-controllers to "to" chained mode - // request - if (is_controller_active(controller_it->c) && !controller_will_be_deactivated) - { - RCLCPP_DEBUG( - get_logger(), - "Controller with name '%s' can not be activated since is already active and will not " - "be " - "deactivated. " - "The controller will be removed from the list later." - "Skipping adding following controllers to 'to' chained mode request.", - controller_it->info.name.c_str()); - continue; - } - - for (const auto & cmd_itf_name : controller_it->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)) - { - continue; - } + return controller_interface::return_type::ERROR; + } + remove_element_from_list(deactivate_request, controller.info.name); + ctrl_state.in_deactivate_list = false; + } - // currently iterated "controller" is preceding controller --> add following controller - // with matching interface name to "to" chained mode list (if not already in it) - if ( - std::find( - to_chained_mode_request_.begin(), to_chained_mode_request_.end(), - following_ctrl_it->info.name) == to_chained_mode_request_.end()) - { - to_chained_mode_request_.push_back(following_ctrl_it->info.name); - RCLCPP_DEBUG( - get_logger(), "Adding controller '%s' in 'to chained mode' request.", - following_ctrl_it->info.name.c_str()); - - // if it is a chainable controller and not in chained_mode, make the reference - // interfaces available on preactivation (This is needed when you activate a couple of - // chainable controller altogether) - if (!following_ctrl_it->c->is_in_chained_mode()) - { - resource_manager_->make_controller_reference_interfaces_available( - following_ctrl_it->info.name); - } - - if (!add_following_to_restart_request_if_needed(following_ctrl_it)) - { - // need to traverse again - return false; - } - } - } + // check for doubled activation + if (ctrl_state.is_active && !ctrl_state.in_deactivate_list && ctrl_state.in_activate_list) + { + if ( + handle_conflict( + "Could not activate controller '" + controller.info.name + + "' since it is already active") == ConflictStatus::CONFLICT_WITH_STRICT) + { + return controller_interface::return_type::ERROR; } + remove_element_from_list(activate_request, controller.info.name); + ctrl_state.in_activate_list = false; + } - // finish traverse - return true; - }; + // 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( + "Could not activate controller '" + controller.info.name + + "' since it is not in inactive state") == ConflictStatus::CONFLICT_WITH_STRICT) + { + return controller_interface::return_type::ERROR; + } + remove_element_from_list(activate_request, controller.info.name); + ctrl_state.in_activate_list = false; + } - while (!propagate_activation() || !propagate_deactivation()); + return controller_interface::return_type::OK; } -controller_interface::return_type -ControllerManager::check_preceding_and_following_controllers_for_switch( - const std::vector & controllers, std::vector & deactivate_request, - std::vector & activate_request, std::vector & to_chained_mode_request, - std::vector & from_chained_mode_request, const int strictness) const +enum class CheckConflictResult { - const auto logger = get_logger(); - - enum class ConflictStatus - { - NO_CONFLICT, - CONFLICT_WITH_BEST_EFFORT, - CONFLICT_WITH_STRICT, - }; + OK, + CheckNextFollowing, + CheckNextPreceding, + Error, +}; - const auto handle_conflict = [&]( - const std::string & msg, - const std::string & erase_from_activate = "", - const std::string & erase_from_deactivate = "") -> ConflictStatus +CheckConflictResult check_conflict_of_preceding_and_following( + const rclcpp::Logger & logger, const int strictness, const ControllerSpec & preceding_ctrl, + const ControllerSpec & following_ctrl, const ActivationState & preceding_state, + const ActivationState & following_state, std::vector & deactivate_request, + std::vector & activate_request, + hardware_interface::ResourceManager & resource_manager) +{ + const auto handle_conflict = [&](const std::string & msg) { if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) { RCLCPP_ERROR(logger, "%s", msg.c_str()); - deactivate_request.clear(); - activate_request.clear(); - to_chained_mode_request.clear(); - from_chained_mode_request.clear(); return ConflictStatus::CONFLICT_WITH_STRICT; } RCLCPP_WARN(logger, "%s", msg.c_str()); - if (!erase_from_activate.empty()) - { - auto it = std::find(activate_request.begin(), activate_request.end(), erase_from_activate); - if (it != activate_request.end()) - { - activate_request.erase(it); - } - } - if (!erase_from_deactivate.empty()) - { - auto it = - std::find(deactivate_request.begin(), deactivate_request.end(), erase_from_deactivate); - if (it != deactivate_request.end()) - { - deactivate_request.erase(it); - } - } - return ConflictStatus::CONFLICT_WITH_BEST_EFFORT; }; - // check if there are controllers to (de)activate - if (activate_request.empty() && deactivate_request.empty()) + // if neither the preceding nor the following is included in any request, no check is + // required, so proceed to the next preceding + if ( + !preceding_state.in_activate_list && !preceding_state.in_deactivate_list && + !following_state.in_activate_list && !following_state.in_deactivate_list) { - RCLCPP_DEBUG(logger, "No controllers to (de)activate"); - return controller_interface::return_type::OK; + RCLCPP_DEBUG( + logger, + " - No need to check preceding '%s' and following '%s' because there are no requests", + preceding_ctrl.info.name.c_str(), following_ctrl.info.name.c_str()); + return CheckConflictResult::CheckNextPreceding; } - // preceding -> following の順に整合性をチェックする - for (const auto & controller : controllers) + // check if following controller is chainable + if (preceding_state.in_activate_list && !following_ctrl.c->is_chainable()) { - const bool is_active = is_controller_active(*controller.c); - const bool is_inactive = is_controller_inactive(*controller.c); - - // skip controllers that are not active nor inactive - if (!is_active && !is_inactive) + const auto conflict_status = handle_conflict( + "The preceding controller with name '" + preceding_ctrl.info.name + + "' cannot be activated because the following controller with name '" + + following_ctrl.info.name + "' is not chainable."); + if (conflict_status == ConflictStatus::CONFLICT_WITH_STRICT) { - continue; + return CheckConflictResult::Error; } - - // 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_deactivate_list = deactivate_list_it != deactivate_request.end(); - - auto activate_list_it = - std::find(activate_request.begin(), activate_request.end(), controller.info.name); - const bool in_activate_list = activate_list_it != activate_request.end(); - - std::vector> - following_ctrl_refs; - - for (const auto & cmd_itf_name : controller.c->command_interface_configuration().names) + else if (conflict_status == ConflictStatus::CONFLICT_WITH_BEST_EFFORT) { - // 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)) - { - // chain に関係しない interface はチェック不要 - continue; - } - - if ( - std::find_if( - following_ctrl_refs.begin(), following_ctrl_refs.end(), [&](const auto & ctrl) - { return ctrl.get().info.name == following_ctrl_it->info.name; }) == - following_ctrl_refs.end()) - { - following_ctrl_refs.push_back(std::cref(*following_ctrl_it)); - } + // BEST_EFFORTの場合はprecedingをactivateリストから削除し、次のpreceding に進む + remove_element_from_list(activate_request, preceding_ctrl.info.name); + return CheckConflictResult::CheckNextPreceding; } + } - RCLCPP_WARN(logger, "Checking controller '%s'", controller.info.name.c_str()); - for (const auto following_ctrl_ref : following_ctrl_refs) + // 1. preceding が inactive から activate される場合のエラー判定 + if (preceding_state.is_inactive && preceding_state.in_activate_list) + { + // 以下のいずれかの場合は preceding を activate できないためエラー + // 1-a. following が inactive で activate されない場合 + // 1-b. following が active で deactivateされ、かつ、restart されない場合 + // エラー発生かつstrictがBEST_EFFORTの場合は、precedingをactivateリストから削除 + ConflictStatus conflict_status = ConflictStatus::NO_CONFLICT; + if (following_state.is_inactive && !following_state.in_activate_list) { - const auto & following_ctrl = following_ctrl_ref.get(); - RCLCPP_WARN(logger, " - Following controller '%s'", following_ctrl.info.name.c_str()); + conflict_status = handle_conflict( + "The controller with name '" + preceding_ctrl.info.name + + "' cannot be activated because the following controller with name '" + + following_ctrl.info.name + "' is inactive and will not be activated."); + } + if ( + following_state.is_active && following_state.in_deactivate_list && + !following_state.in_activate_list) + { + conflict_status = handle_conflict( + "The controller with name '" + preceding_ctrl.info.name + + "' cannot be activated because the following controller with name '" + + following_ctrl.info.name + + "' is active and will be deactivated but will not be activated."); } - for (const auto following_ctrl_ref : following_ctrl_refs) + if (conflict_status == ConflictStatus::CONFLICT_WITH_STRICT) { - const auto & following_ctrl = following_ctrl_ref.get(); - const bool following_is_active = is_controller_active(following_ctrl.c); - const bool following_is_inactive = is_controller_inactive(following_ctrl.c); - const bool following_in_deactivate_list = - std::find(deactivate_request.begin(), deactivate_request.end(), following_ctrl.info.name) != - deactivate_request.end(); - const bool following_in_activate_list = - std::find(activate_request.begin(), activate_request.end(), following_ctrl.info.name) != - activate_request.end(); + return CheckConflictResult::Error; + } + else if (conflict_status == ConflictStatus::CONFLICT_WITH_BEST_EFFORT) + { + // BEST_EFFORTの場合はprecedingをactivateリストから削除し、次のpreceding に進む + remove_element_from_list(activate_request, preceding_ctrl.info.name); + return CheckConflictResult::CheckNextPreceding; + } + } - if ( - !in_activate_list && !in_deactivate_list && !following_in_activate_list && - !following_in_deactivate_list) - { - // preceding, following ともにいずれの request にも含まれていない場合はチェック不要 - RCLCPP_DEBUG( - logger, - " - No need to check preceding '%s' and following '%s' because there are no requests", - controller.info.name.c_str(), following_ctrl.info.name.c_str()); - continue; - } + // 2. preceding が active のまま遷移がない場合のエラー判定 + if ( + preceding_state.is_active && !preceding_state.in_deactivate_list && + !preceding_state.in_activate_list) + { + // 以下のいずれかの場合は following を deactivate できない (結果として restart もできない) + // ためエラー + // 2-1. following が active で deactivate される場合 + // 2-2. following が active で restart される場合 + // -> 結局のところ、following が activate されるか否かを問わず、deactivate + // される場合はエラー(precedingがactiveの時、followingは必ずactiveなので、followingのactiveチェックは不要) + // エラー発生かつstrictがBEST_EFFORTの場合はfollowingをactivate/deactivateリストから削除 + if (following_state.in_deactivate_list) + { + const auto conflict_status = handle_conflict( + "The following controller with name '" + following_ctrl.info.name + + "' cannot be deactivated because the preceding controller with name '" + + preceding_ctrl.info.name + "' is active and will not be deactivated and restarted."); - // 1. preceding が inactive から activate される場合のエラー判定 - if (is_inactive && in_activate_list) + if (conflict_status == ConflictStatus::CONFLICT_WITH_STRICT) { - // 以下のいずれかの場合は preceding を activate できないためエラー - // 1-a. following が inactive で activate されない場合 - // 1-b. following が active で deactivateされ、かつ、restart されない場合 - // エラー発生かつstrictがBEST_EFFORTの場合は、precedingをactivateリストから削除 - ConflictStatus conflict_status = ConflictStatus::NO_CONFLICT; - if (following_is_inactive && !following_in_activate_list) - { - conflict_status = handle_conflict( - "The controller with name '" + controller.info.name + - "' cannot be activated because the following controller with name '" + - following_ctrl.info.name + "' is inactive and will not be activated.", - controller.info.name, ""); - } - if (following_is_active && following_in_deactivate_list && !following_in_activate_list) - { - conflict_status = handle_conflict( - "The controller with name '" + controller.info.name + - "' cannot be activated because the following controller with name '" + - following_ctrl.info.name + - "' is active and will be deactivated but will not be activated.", - controller.info.name, ""); - } - - if (conflict_status == ConflictStatus::CONFLICT_WITH_STRICT) - { - // STRICTの場合は即座にエラーを返す - RCLCPP_ERROR( - logger, "Switching controllers failed due to strict conflict: %s", - controller.info.name.c_str()); - return controller_interface::return_type::ERROR; - } - else if (conflict_status == ConflictStatus::CONFLICT_WITH_BEST_EFFORT) - { - // BEST_EFFORTの場合はprecedingをactivateリストから削除したため、次のcontroller に進む - break; - } + return CheckConflictResult::Error; } - // 2. preceding が active のまま遷移がない場合のエラー判定 - else if (is_active && !in_deactivate_list && !in_activate_list) + else if (conflict_status == ConflictStatus::CONFLICT_WITH_BEST_EFFORT) { - // 以下のいずれかの場合は following を deactivate できない (結果として restart もできない) - // ためエラー - // 2-1. following が active で deactivate される場合 - // 2-2. following が active で restart される場合 - // -> 結局のところ、following が activate されるか否かを問わず、deactivate - // される場合はエラー(precedingがactiveの時、followingは必ずactiveなので、followingのactiveチェックは不要) - // エラー発生かつstrictがBEST_EFFORTの場合はfollowingをactivate/deactivateリストから削除 - if (following_in_deactivate_list) - { - const auto conflict_status = handle_conflict( - "The following controller with name '" + following_ctrl.info.name + - "' cannot be deactivated because the preceding controller with name '" + - controller.info.name + "' is active and will not be deactivated and restarted.", - following_ctrl.info.name, following_ctrl.info.name); - - if (conflict_status == ConflictStatus::CONFLICT_WITH_STRICT) - { - // STRICTの場合は即座にエラーを返す - RCLCPP_ERROR( - logger, "Switching controllers failed due to strict conflict: %s", - controller.info.name.c_str()); - return controller_interface::return_type::ERROR; - } - else if (conflict_status == ConflictStatus::CONFLICT_WITH_BEST_EFFORT) - { - // BEST_EFFORTの場合はfollowingをactivate/deactivateリストから削除したため、次のfollowing - // に進む - continue; - } - } + // BEST_EFFORTの場合はfollowingをactivate/deactivateリストから削除し、次のfollowingに進む + remove_element_from_list(activate_request, following_ctrl.info.name); + remove_element_from_list(deactivate_request, following_ctrl.info.name); + return CheckConflictResult::CheckNextFollowing; } - // 3. preceding が active で restart される場合のエラー判定 - else if (is_active && in_deactivate_list && in_activate_list) - { - // following が active で deactivate され、かつ、restart しない場合は following を - // deactivate できないためエラー - // エラー発生かつstrictがBEST_EFFORTの場合はfollowingをdeactivateリストから削除 - if (following_is_active && following_in_deactivate_list && !following_in_activate_list) - { - const auto conflict_status = handle_conflict( - "The following controller with name '" + following_ctrl.info.name + - "' cannot be deactivated because the preceding controller with name '" + - controller.info.name + "' is active and will be restarted.", - "", following_ctrl.info.name); - - if (conflict_status == ConflictStatus::CONFLICT_WITH_STRICT) - { - // STRICTの場合は即座にエラーを返す - RCLCPP_ERROR( - logger, "Switching controllers failed due to strict conflict: %s", - controller.info.name.c_str()); - return controller_interface::return_type::ERROR; - } - else if (conflict_status == ConflictStatus::CONFLICT_WITH_BEST_EFFORT) - { - // BEST_EFFORTの場合はfollowingをdeactivateリストから削除したため、次のfollowing に進む - continue; - } - } - } - - RCLCPP_INFO(logger, "--> NO CONFLICT"); - // activate/deactivate request に conflict が検出されなかった場合は、 controller - // の状態に変更に伴う chained_mode 変更の判定に進む + } + } - // preceding のactivate/deactivate に合わせて following の 状態変更をリクエストする - // following が active 状態の場合は、chained_mode の変更時に restart - // が必要となるためそのリクエストも同時に追加する - const auto add_following_to_restart_request_if_needed = [&]() - { - if (following_is_active && !following_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()); - - if ( - std::find( - deactivate_request.begin(), deactivate_request.end(), following_ctrl.info.name) == - deactivate_request.end()) - { - deactivate_request.push_back(following_ctrl.info.name); - } - if ( - std::find(activate_request.begin(), activate_request.end(), following_ctrl.info.name) == - activate_request.end()) - { - activate_request.push_back(following_ctrl.info.name); - } - }; - }; - - const bool preceding_will_be_restart = (is_active && in_deactivate_list && in_activate_list); - const bool following_will_be_restart = - (following_is_active && following_in_deactivate_list && following_in_activate_list); + // 3. preceding が active で restart される場合のエラー判定 + if ( + preceding_state.is_active && preceding_state.in_deactivate_list && + preceding_state.in_activate_list) + { + // following が active で deactivate され、かつ、restart しない場合は following を + // deactivate できないためエラー + // エラー発生かつstrictがBEST_EFFORTの場合はfollowingをdeactivateリストから削除 + if ( + following_state.is_active && following_state.in_deactivate_list && + !following_state.in_activate_list) + { + const auto conflict_status = handle_conflict( + "The following controller with name '" + following_ctrl.info.name + + "' cannot be deactivated because the preceding controller with name '" + + preceding_ctrl.info.name + "' is active and will be restarted."); - if ( - (in_deactivate_list && !preceding_will_be_restart) || - (preceding_will_be_restart && following_will_be_restart)) + if (conflict_status == ConflictStatus::CONFLICT_WITH_STRICT) { - if ( - std::find( - from_chained_mode_request.begin(), from_chained_mode_request.end(), - following_ctrl.info.name) == from_chained_mode_request.end()) - { - RCLCPP_DEBUG( - logger, "Adding following controller with name '%s' to 'from' chained mode request.", - following_ctrl.info.name.c_str()); - - from_chained_mode_request.push_back(following_ctrl.info.name); - add_following_to_restart_request_if_needed(); - } + return CheckConflictResult::Error; } - if ( - (in_activate_list && !preceding_will_be_restart) || - (preceding_will_be_restart && following_will_be_restart)) + else if (conflict_status == ConflictStatus::CONFLICT_WITH_BEST_EFFORT) { - if ( - std::find( - to_chained_mode_request.begin(), to_chained_mode_request.end(), - following_ctrl.info.name) == to_chained_mode_request.end()) - { - RCLCPP_DEBUG( - logger, "Adding following controller with name '%s' to 'to' chained mode request.", - following_ctrl.info.name.c_str()); - - to_chained_mode_request.push_back(following_ctrl.info.name); - add_following_to_restart_request_if_needed(); - - // 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); - } - } + // BEST_EFFORTの場合はfollowingをdeactivateリストから削除し、次のfollowing に進む + remove_element_from_list(deactivate_request, following_ctrl.info.name); + return CheckConflictResult::CheckNextFollowing; } } } - return controller_interface::return_type::OK; + return CheckConflictResult::OK; } -#if 0 -controller_interface::return_type ControllerManager::check_following_controllers_for_activate( - const std::vector & controllers, int strictness, - const ControllersListIterator controller_it) const +void propagate_chained_mode( + const rclcpp::Logger & logger, const ControllerSpec & preceding_ctrl, + const ControllerSpec & following_ctrl, const ActivationState & preceding_state, + const ActivationState & following_state, std::vector & deactivate_request, + std::vector & activate_request, std::vector & to_chained_mode_request, + std::vector & from_chained_mode_request, + hardware_interface::ResourceManager & resource_manager) { - // 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()); - - for (const auto & cmd_itf_name : controller_it->c->command_interface_configuration().names) + 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) { - 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()); - - // check if following controller is chainable - if (!following_ctrl_it->c->is_chainable()) - { - 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; - } - - const bool following_will_be_deactivated = - std::find( - deactivate_request_.begin(), deactivate_request_.end(), following_ctrl_it->info.name) != - deactivate_request_.end(); + add_element_to_list(from_chained_mode_request, following_ctrl.info.name); + in_from_chained_mode_list = true; + } - const bool following_will_be_activated = - std::find(activate_request_.begin(), activate_request_.end(), following_ctrl_it->info.name) != - activate_request_.end(); + // 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 (is_controller_active(following_ctrl_it->c)) + // 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()) { - // will following controller be deactivated? - if (following_will_be_deactivated && !following_will_be_activated) - { - RCLCPP_WARN( - get_logger(), - "The following controller with name '%s' will be deactivated and will not be " - "activated.", - following_ctrl_it->info.name.c_str()); - return controller_interface::return_type::ERROR; - } - } - // check if following controller will not be activated - else if (!following_will_be_activated) - { - 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; + resource_manager.make_controller_reference_interfaces_available(following_ctrl.info.name); } + } - // 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) + // 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) { - return controller_interface::return_type::ERROR; - } - - // TODO(destogl): this should be discussed how to it the best - just a placeholder for now - // else if (strictness == - // 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 0 - // If the target following_ctrl to be activated is not in chained_mode, add it to - // to_chained_mode_request. Alternatively, if it is already in chained_mode but is subject to - // reset, it needs to be removed from chained_mode once during deactivation and then - // transitioned back to chained_mode, so add it to to_chained_mode_request. - if (following_will_be_activated) - { - 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 0 // activate順などでうまく available にしたほうがよいためここは無効化 - // if it is a chainable controller and not in chained_mode, make the reference interfaces - // available on preactivation (This is needed when you activate a couple of chainable - // controller altogether) - if (!following_ctrl_it->c->is_in_chained_mode() && !following_will_be_deactivated) - { - resource_manager_->make_controller_reference_interfaces_available( - following_ctrl_it->info.name); - } -#endif - - RCLCPP_DEBUG( - get_logger(), "Adding controller '%s' in 'to chained mode' request.", - following_ctrl_it->info.name.c_str()); - } - } -#endif + RCLCPP_DEBUG( + logger, + "Adding following controller with name '%s' to deactivate and activate request for " + "restart.", + following_ctrl.info.name.c_str()); -#if 0 - else - { - // 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()); - } - } -#endif + add_element_to_list(deactivate_request, following_ctrl.info.name); + add_element_to_list(activate_request, following_ctrl.info.name); + }; } - 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) const +controller_interface::return_type +ControllerManager::check_de_activate_request_conflict_and_create_chained_mode_request( + const int strictness, const std::vector & controllers, + std::vector & deactivate_request, std::vector & activate_request, + std::vector & to_chained_mode_request, + std::vector & from_chained_mode_request) const { - // if not chainable no need for any checks - if (!controller_it->c->is_chainable()) - { - RCLCPP_DEBUG( - get_logger(), - "Controller with name '%s' is not chainable. " - "No need to do any checks of preceding controllers when stopping it.", - controller_it->info.name.c_str()); - return controller_interface::return_type::OK; - } + const auto logger = get_logger(); - if (!controller_it->c->is_in_chained_mode()) + // check if there are controllers to (de)activate + if (activate_request.empty() && deactivate_request.empty()) { - 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()); + RCLCPP_DEBUG(logger, "No controllers to (de)activate"); return controller_interface::return_type::OK; } - RCLCPP_DEBUG( - get_logger(), "Checking preceding controller of following controller with name '%s'.", - controller_it->info.name.c_str()); - - for (const auto & ref_itf_name : - resource_manager_->get_controller_reference_interface_names(controller_it->info.name)) + // Check for consistency 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; - - // TODO(destogl): This data could be cached after configuring controller into a map for - // access here - for (auto preceding_ctrl_it = controllers.begin(); preceding_ctrl_it != controllers.end(); - ++preceding_ctrl_it) + // check for consistency of (de)activate request for the controller + if ( + check_consistency_of_controller( + logger, *resource_manager_, controller, deactivate_request, activate_request, + to_chained_mode_request, from_chained_mode_request, + strictness) != controller_interface::return_type::OK) { - const auto preceding_ctrl_cmd_itfs = - preceding_ctrl_it->c->command_interface_configuration().names; + return controller_interface::return_type::ERROR; + } - // 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()) - { - continue; - } + const auto controller_state = + get_activation_state(controller, deactivate_request, activate_request); - const bool this_controller_will_be_reset = - (std::find(activate_request_.begin(), activate_request_.end(), controller_it->info.name) != - activate_request_.end()); + // get following controllers references for the controller + const auto following_ctrl_refs = + list_following_controllers(controller, controller_state, controllers); - const bool preceding_will_be_deactivated = - std::find( - deactivate_request_.begin(), deactivate_request_.end(), preceding_ctrl_it->info.name) != - deactivate_request_.end(); + // if there are no following controllers, this controller is not a preceding controller, so skip + // the following checks + if (following_ctrl_refs.empty()) + { + continue; + } - const bool preceding_will_be_activated = - std::find( - activate_request_.begin(), activate_request_.end(), preceding_ctrl_it->info.name) != - activate_request_.end(); + // this controller is a preceding controller + const auto & preceding_ctrl = controller; + const auto & preceding_state = controller_state; - const bool preceding_will_be_reset = - (preceding_will_be_deactivated && preceding_will_be_activated); + // check consistency of the preceding and each following controller + for (const auto & following_ctrl_ref : following_ctrl_refs) + { + const auto & following_ctrl = following_ctrl_ref.get(); - // if controller and preceding controller will be reset go the next once - if (this_controller_will_be_reset && preceding_will_be_reset) - { - RCLCPP_DEBUG( - get_logger(), - "Controller with name '%s' and preceding controller with name '%s' " - "will be reset.", - controller_it->info.name.c_str(), preceding_ctrl_it->info.name.c_str()); - continue; - } + const auto following_state = + get_activation_state(following_ctrl, deactivate_request, activate_request); - // check if preceding controller will be activated - if (is_controller_inactive(preceding_ctrl_it->c) && preceding_will_be_activated) - { - 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; - } - // check if preceding controller will not be deactivated - else if (is_controller_active(preceding_ctrl_it->c) && !preceding_will_be_deactivated) + const auto check_result = check_conflict_of_preceding_and_following( + logger, strictness, preceding_ctrl, following_ctrl, preceding_state, following_state, + deactivate_request, activate_request, *resource_manager_); + + // based on the check result, select the next action + switch (check_result) { - 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; + case CheckConflictResult::OK: + // activate/deactivate request に conflict が検出されなかった場合は、 controller + // の状態に変更に伴う chained_mode 変更の判定に進む + propagate_chained_mode( + logger, preceding_ctrl, following_ctrl, preceding_state, following_state, + deactivate_request, activate_request, to_chained_mode_request, + from_chained_mode_request, *resource_manager_); + break; + case CheckConflictResult::CheckNextFollowing: + continue; + case CheckConflictResult::CheckNextPreceding: + // TODO: ここは本来 for loop を break したいが、これだと switch を break + // するだけなのでバグ + break; + case CheckConflictResult::Error: + return controller_interface::return_type::ERROR; + default: + RCLCPP_ERROR(logger, "Unknown check result"); + return controller_interface::return_type::ERROR; } - - // TODO(destogl): this should be discussed how to it the best - just a placeholder for now - // else if ( - // strictness == - // 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); - // } } } + + // To ensure that the appropriate (de)activation are performed, sort deactivate requests in + // the order of preceding to following, and sort activate requests in the reverse order of + // following to preceding + sort_list_by_another(deactivate_request, ordered_controllers_names_, false); + sort_list_by_another(activate_request, ordered_controllers_names_, true); + return controller_interface::return_type::OK; } -#endif void ControllerManager::controller_activity_diagnostic_callback( diagnostic_updater::DiagnosticStatusWrapper & stat) From f9aa9ca091968ede35b4374d06dc0163ad724707 Mon Sep 17 00:00:00 2001 From: Takashi Sato Date: Tue, 11 Jun 2024 18:59:02 +0900 Subject: [PATCH 03/16] fix a bug and add test --- controller_manager/src/controller_manager.cpp | 197 +++++++++--------- ...llers_chaining_with_controller_manager.cpp | 78 +++---- 2 files changed, 135 insertions(+), 140 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 8ef005d2ba..d5d212afd4 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2363,22 +2363,19 @@ enum class ConflictStatus CONFLICT_WITH_STRICT, }; -controller_interface::return_type check_consistency_of_controller( - const rclcpp::Logger & logger, hardware_interface::ResourceManager & resource_manager, - const ControllerSpec & controller, std::vector & deactivate_request, - std::vector & activate_request, std::vector & to_chained_mode_request, - std::vector & from_chained_mode_request, const int strictness) +controller_interface::return_type check_de_activate_request_conflict_for_controller( + const rclcpp::Logger & logger, const int strictness, const ControllerSpec & controller, + std::vector & deactivate_request, std::vector & activate_request) { - const auto handle_conflict = [&](const std::string & msg) + const auto handle_conflict = [&logger, strictness](const std::string & msg) { if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) { RCLCPP_ERROR(logger, "%s", msg.c_str()); - return ConflictStatus::CONFLICT_WITH_STRICT; + return false; } - RCLCPP_WARN(logger, "%s", msg.c_str()); - return ConflictStatus::CONFLICT_WITH_BEST_EFFORT; + return true; }; auto ctrl_state = get_activation_state(controller, deactivate_request, activate_request); @@ -2386,10 +2383,8 @@ controller_interface::return_type check_consistency_of_controller( // check for double stop if (!ctrl_state.is_active && ctrl_state.in_deactivate_list) { - if ( - handle_conflict( - "Could not deactivate controller '" + controller.info.name + "' since it is not active") == - ConflictStatus::CONFLICT_WITH_STRICT) + if (!handle_conflict( + "Could not deactivate controller '" + controller.info.name + "' since it is not active")) { return controller_interface::return_type::ERROR; } @@ -2400,10 +2395,9 @@ controller_interface::return_type check_consistency_of_controller( // check for doubled activation if (ctrl_state.is_active && !ctrl_state.in_deactivate_list && ctrl_state.in_activate_list) { - if ( - handle_conflict( - "Could not activate controller '" + controller.info.name + - "' since it is already active") == ConflictStatus::CONFLICT_WITH_STRICT) + if (!handle_conflict( + "Could not activate controller '" + controller.info.name + + "' since it is already active")) { return controller_interface::return_type::ERROR; } @@ -2414,10 +2408,9 @@ controller_interface::return_type check_consistency_of_controller( // 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( - "Could not activate controller '" + controller.info.name + - "' since it is not in inactive state") == ConflictStatus::CONFLICT_WITH_STRICT) + if (!handle_conflict( + "Could not activate controller '" + controller.info.name + + "' since it is not in inactive state")) { return controller_interface::return_type::ERROR; } @@ -2440,23 +2433,25 @@ CheckConflictResult check_conflict_of_preceding_and_following( const rclcpp::Logger & logger, const int strictness, const ControllerSpec & preceding_ctrl, const ControllerSpec & following_ctrl, const ActivationState & preceding_state, const ActivationState & following_state, std::vector & deactivate_request, - std::vector & activate_request, - hardware_interface::ResourceManager & resource_manager) + std::vector & activate_request) { - const auto handle_conflict = [&](const std::string & msg) + const auto handle_conflict = [&logger, strictness](const std::string & msg) { if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) { RCLCPP_ERROR(logger, "%s", msg.c_str()); - return ConflictStatus::CONFLICT_WITH_STRICT; + return false; } - RCLCPP_WARN(logger, "%s", msg.c_str()); - return ConflictStatus::CONFLICT_WITH_BEST_EFFORT; + return true; }; + RCLCPP_ERROR( + logger, "Checking conflict between '%s' and '%s'", preceding_ctrl.info.name.c_str(), + following_ctrl.info.name.c_str()); + // if neither the preceding nor the following is included in any request, no check is - // required, so proceed to the next preceding + // required, so proceed to the next following if ( !preceding_state.in_activate_list && !preceding_state.in_deactivate_list && !following_state.in_activate_list && !following_state.in_deactivate_list) @@ -2465,39 +2460,38 @@ CheckConflictResult check_conflict_of_preceding_and_following( logger, " - No need to check preceding '%s' and following '%s' because there are no requests", preceding_ctrl.info.name.c_str(), following_ctrl.info.name.c_str()); - return CheckConflictResult::CheckNextPreceding; + return CheckConflictResult::CheckNextFollowing; } - // check if following controller is chainable + // if the preceding will be activated, check if the following is chainable if (preceding_state.in_activate_list && !following_ctrl.c->is_chainable()) { - const auto conflict_status = handle_conflict( - "The preceding controller with name '" + preceding_ctrl.info.name + - "' cannot be activated because the following controller with name '" + - following_ctrl.info.name + "' is not chainable."); - if (conflict_status == ConflictStatus::CONFLICT_WITH_STRICT) + if (!handle_conflict( + "The preceding controller with name '" + preceding_ctrl.info.name + + "' cannot be activated because the following controller with name '" + + following_ctrl.info.name + "' is not chainable.")) { return CheckConflictResult::Error; } - else if (conflict_status == ConflictStatus::CONFLICT_WITH_BEST_EFFORT) - { - // BEST_EFFORTの場合はprecedingをactivateリストから削除し、次のpreceding に進む - remove_element_from_list(activate_request, preceding_ctrl.info.name); - return CheckConflictResult::CheckNextPreceding; - } + + // if BEST_EFFORT, remove the preceding controller from the activate list and proceed to the + // next preceding + remove_element_from_list(activate_request, preceding_ctrl.info.name); + return CheckConflictResult::CheckNextPreceding; } - // 1. preceding が inactive から activate される場合のエラー判定 + // check for conflict when preceding will be activated from inactive if (preceding_state.is_inactive && preceding_state.in_activate_list) { // 以下のいずれかの場合は preceding を activate できないためエラー // 1-a. following が inactive で activate されない場合 // 1-b. following が active で deactivateされ、かつ、restart されない場合 // エラー発生かつstrictがBEST_EFFORTの場合は、precedingをactivateリストから削除 - ConflictStatus conflict_status = ConflictStatus::NO_CONFLICT; + + std::optional found_conflict = std::nullopt; if (following_state.is_inactive && !following_state.in_activate_list) { - conflict_status = handle_conflict( + found_conflict = handle_conflict( "The controller with name '" + preceding_ctrl.info.name + "' cannot be activated because the following controller with name '" + following_ctrl.info.name + "' is inactive and will not be activated."); @@ -2506,26 +2500,28 @@ CheckConflictResult check_conflict_of_preceding_and_following( following_state.is_active && following_state.in_deactivate_list && !following_state.in_activate_list) { - conflict_status = handle_conflict( + found_conflict = handle_conflict( "The controller with name '" + preceding_ctrl.info.name + "' cannot be activated because the following controller with name '" + following_ctrl.info.name + "' is active and will be deactivated but will not be activated."); } - if (conflict_status == ConflictStatus::CONFLICT_WITH_STRICT) + if (found_conflict.has_value()) { - return CheckConflictResult::Error; - } - else if (conflict_status == ConflictStatus::CONFLICT_WITH_BEST_EFFORT) - { - // BEST_EFFORTの場合はprecedingをactivateリストから削除し、次のpreceding に進む + if (!found_conflict.value()) + { + return CheckConflictResult::Error; + } + + // if BEST_EFFORT, remove the preceding controller from the activate list and proceed to the + // next preceding remove_element_from_list(activate_request, preceding_ctrl.info.name); return CheckConflictResult::CheckNextPreceding; } } - // 2. preceding が active のまま遷移がない場合のエラー判定 + // check for conflict when preceding is active and will not be transitioned if ( preceding_state.is_active && !preceding_state.in_deactivate_list && !preceding_state.in_activate_list) @@ -2539,26 +2535,23 @@ CheckConflictResult check_conflict_of_preceding_and_following( // エラー発生かつstrictがBEST_EFFORTの場合はfollowingをactivate/deactivateリストから削除 if (following_state.in_deactivate_list) { - const auto conflict_status = handle_conflict( - "The following controller with name '" + following_ctrl.info.name + - "' cannot be deactivated because the preceding controller with name '" + - preceding_ctrl.info.name + "' is active and will not be deactivated and restarted."); - - if (conflict_status == ConflictStatus::CONFLICT_WITH_STRICT) + if (!handle_conflict( + "The following controller with name '" + following_ctrl.info.name + + "' cannot be deactivated because the preceding controller with name '" + + preceding_ctrl.info.name + "' is active and will not be deactivated and restarted.")) { return CheckConflictResult::Error; } - else if (conflict_status == ConflictStatus::CONFLICT_WITH_BEST_EFFORT) - { - // BEST_EFFORTの場合はfollowingをactivate/deactivateリストから削除し、次のfollowingに進む - remove_element_from_list(activate_request, following_ctrl.info.name); - remove_element_from_list(deactivate_request, following_ctrl.info.name); - return CheckConflictResult::CheckNextFollowing; - } + + // if BEST_EFFORT, remove the following controller from the activate and deactivate + // list and proceed to the next following + remove_element_from_list(activate_request, following_ctrl.info.name); + remove_element_from_list(deactivate_request, following_ctrl.info.name); + return CheckConflictResult::CheckNextFollowing; } } - // 3. preceding が active で restart される場合のエラー判定 + // check for conflict when preceding will be restarted from active if ( preceding_state.is_active && preceding_state.in_deactivate_list && preceding_state.in_activate_list) @@ -2570,24 +2563,22 @@ CheckConflictResult check_conflict_of_preceding_and_following( following_state.is_active && following_state.in_deactivate_list && !following_state.in_activate_list) { - const auto conflict_status = handle_conflict( - "The following controller with name '" + following_ctrl.info.name + - "' cannot be deactivated because the preceding controller with name '" + - preceding_ctrl.info.name + "' is active and will be restarted."); - - if (conflict_status == ConflictStatus::CONFLICT_WITH_STRICT) + if (!handle_conflict( + "The following controller with name '" + following_ctrl.info.name + + "' cannot be deactivated because the preceding controller with name '" + + preceding_ctrl.info.name + "' is active and will be restarted.")) { return CheckConflictResult::Error; } - else if (conflict_status == ConflictStatus::CONFLICT_WITH_BEST_EFFORT) - { - // BEST_EFFORTの場合はfollowingをdeactivateリストから削除し、次のfollowing に進む - remove_element_from_list(deactivate_request, following_ctrl.info.name); - return CheckConflictResult::CheckNextFollowing; - } + + // if BEST_EFFORT, remove the following controller from the deactivate list and proceed to + // the next following + remove_element_from_list(deactivate_request, following_ctrl.info.name); + return CheckConflictResult::CheckNextFollowing; } } + // No conflict occurs in other conditions, so return OK return CheckConflictResult::OK; } @@ -2675,12 +2666,11 @@ ControllerManager::check_de_activate_request_conflict_and_create_chained_mode_re // subsequent processes will not function properly. for (const auto & controller : controllers) { - // check for consistency of (de)activate request for the controller + // check for conflict of (de)activate request for the controller if ( - check_consistency_of_controller( - logger, *resource_manager_, controller, deactivate_request, activate_request, - to_chained_mode_request, from_chained_mode_request, - strictness) != controller_interface::return_type::OK) + check_de_activate_request_conflict_for_controller( + logger, strictness, controller, deactivate_request, activate_request) != + controller_interface::return_type::OK) { return controller_interface::return_type::ERROR; } @@ -2713,30 +2703,29 @@ ControllerManager::check_de_activate_request_conflict_and_create_chained_mode_re const auto check_result = check_conflict_of_preceding_and_following( logger, strictness, preceding_ctrl, following_ctrl, preceding_state, following_state, - deactivate_request, activate_request, *resource_manager_); + deactivate_request, activate_request); // based on the check result, select the next action - switch (check_result) + if (check_result == CheckConflictResult::OK) { - case CheckConflictResult::OK: - // activate/deactivate request に conflict が検出されなかった場合は、 controller - // の状態に変更に伴う chained_mode 変更の判定に進む - propagate_chained_mode( - logger, preceding_ctrl, following_ctrl, preceding_state, following_state, - deactivate_request, activate_request, to_chained_mode_request, - from_chained_mode_request, *resource_manager_); - break; - case CheckConflictResult::CheckNextFollowing: - continue; - case CheckConflictResult::CheckNextPreceding: - // TODO: ここは本来 for loop を break したいが、これだと switch を break - // するだけなのでバグ - break; - case CheckConflictResult::Error: - return controller_interface::return_type::ERROR; - default: - RCLCPP_ERROR(logger, "Unknown check result"); - return controller_interface::return_type::ERROR; + // if no conflict is detected in the activate/deactivate request, proceed to + // determine the chained_mode request + propagate_chained_mode( + logger, preceding_ctrl, following_ctrl, preceding_state, following_state, + deactivate_request, activate_request, to_chained_mode_request, from_chained_mode_request, + *resource_manager_); + } + else if (check_result == CheckConflictResult::CheckNextFollowing) + { + continue; + } + else if (check_result == CheckConflictResult::CheckNextPreceding) + { + break; + } + else + { + return controller_interface::return_type::ERROR; } } } 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 0cb59f26a2..d283d05306 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -1042,9 +1042,6 @@ TEST_P( // Test Case 6: following controller is deactivated but preceding controller will be activated // --> return error; controllers stay in the same state - - RCLCPP_ERROR(cm_->get_logger(), "======================================"); - switch_test_controllers( {DIFF_DRIVE_CONTROLLER}, {PID_LEFT_WHEEL, PID_RIGHT_WHEEL}, test_param.strictness, expected.at(test_param.strictness).future_status, @@ -1062,6 +1059,21 @@ TEST_P( // Test Case 7: 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, @@ -1086,53 +1098,47 @@ TEST_P( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, position_tracking_controller->get_state().id()); + // Attempt to deactivate one of the lowest following controller + switch_test_controllers( + {}, {PID_LEFT_WHEEL}, test_param.strictness, std::future_status::ready, + expected.at(test_param.strictness).return_type); + verify_all_controllers_are_still_be_active(); + + // Attempt to deactivate another lowest following controller + switch_test_controllers( + {}, {PID_RIGHT_WHEEL}, test_param.strictness, std::future_status::ready, + expected.at(test_param.strictness).return_type); + verify_all_controllers_are_still_be_active(); + // Attempt to deactivate the lowest following controllers switch_test_controllers( {}, {PID_LEFT_WHEEL, PID_RIGHT_WHEEL}, test_param.strictness, std::future_status::ready, expected.at(test_param.strictness).return_type); + verify_all_controllers_are_still_be_active(); - // 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()); + // 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()); - ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - position_tracking_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); - - // 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()); + verify_all_controllers_are_still_be_active(); } TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_adding_in_random_order) From ed0a1871f75f8e2bf21a4d65e1d7d251ab572c65 Mon Sep 17 00:00:00 2001 From: Takashi Sato Date: Tue, 11 Jun 2024 20:53:24 +0900 Subject: [PATCH 04/16] move internal function to anonymous namespace. add comment --- .../controller_manager/controller_manager.hpp | 65 +- controller_manager/src/controller_manager.cpp | 770 +++++++++--------- 2 files changed, 414 insertions(+), 421 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 0cf131baf1..ee2e00efdc 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -340,37 +340,6 @@ class ControllerManager : public rclcpp::Node */ void clear_requests(); -#if 0 - /// 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. - * - * For each controller the whole chain of following controllers is checked. - * - * 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. - * - * \returns return_type::OK if all following controllers 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) const; -#endif - void extract_de_activate_command_interface_request( const std::vector & controllers, const std::vector & deactivate_request, @@ -382,12 +351,42 @@ class ControllerManager : public rclcpp::Node const std::vector & controllers, const std::vector & activate_request); + /** + * @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). Simultaneously, if the (de)activate request necessitates switching the + * chained mode of the following controller, make the corresponding 'from'/'to' chained mode + * request. + * + * 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. + * + * \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] controllers list 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. Additionally, 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 the + * strictness value is equal to "BEST_EFFORT", conflicting controllers found will be erased from + * the list. Additionally, 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. + * + * \returns return_type::OK if all (de)activate requests pass the checks, otherwise + * return_type::ERROR. + */ controller_interface::return_type check_de_activate_request_conflict_and_create_chained_mode_request( const int strictness, const std::vector & controllers, std::vector & deactivate_request, std::vector & activate_request, - std::vector & to_chained_mode_request, - std::vector & from_chained_mode_request) const; + 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 diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index d5d212afd4..1d701dd0a5 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -267,6 +267,289 @@ list_following_controllers( return following_ctrl_refs; } +controller_interface::return_type check_de_activate_request_conflict_for_controller( + const rclcpp::Logger & logger, const int strictness, + const controller_manager::ControllerSpec & controller, + std::vector & deactivate_request, std::vector & activate_request) +{ + const auto handle_conflict = [&logger, strictness](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; + }; + + 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( + "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 doubled activation + if (ctrl_state.is_active && !ctrl_state.in_deactivate_list && ctrl_state.in_activate_list) + { + if (!handle_conflict( + "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( + "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; +} + +enum class CheckConflictResult +{ + OK, + CheckNextFollowing, + CheckNextPreceding, + Error, +}; + +CheckConflictResult check_conflict_between_preceding_and_following( + const rclcpp::Logger & logger, const int strictness, + const controller_manager::ControllerSpec & preceding_ctrl, + const controller_manager::ControllerSpec & following_ctrl, + const ActivationState & preceding_state, const ActivationState & following_state, + std::vector & deactivate_request, std::vector & activate_request) +{ + const auto handle_conflict = [&logger, strictness](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; + }; + + RCLCPP_ERROR( + logger, "Checking conflict between '%s' and '%s'", preceding_ctrl.info.name.c_str(), + following_ctrl.info.name.c_str()); + + // if neither the preceding nor the following is included in any request, no check is + // required, so proceed to the next following + if ( + !preceding_state.in_activate_list && !preceding_state.in_deactivate_list && + !following_state.in_activate_list && !following_state.in_deactivate_list) + { + RCLCPP_DEBUG( + logger, + " - No need to check preceding '%s' and following '%s' because there are no requests", + preceding_ctrl.info.name.c_str(), following_ctrl.info.name.c_str()); + return CheckConflictResult::CheckNextFollowing; + } + + // if the preceding will be activated, check if the following is chainable + if (preceding_state.in_activate_list && !following_ctrl.c->is_chainable()) + { + if (!handle_conflict( + "The preceding controller with name '" + preceding_ctrl.info.name + + "' cannot be activated because the following controller with name '" + + following_ctrl.info.name + "' is not chainable.")) + { + return CheckConflictResult::Error; + } + + // if BEST_EFFORT, remove the preceding controller from the activate list and proceed to the + // next preceding + remove_element_from_list(activate_request, preceding_ctrl.info.name); + return CheckConflictResult::CheckNextPreceding; + } + + // check for conflict when preceding will be activated from inactive + if (preceding_state.is_inactive && preceding_state.in_activate_list) + { + // In the following cases, preceding cannot be activated, resulting in an error: + // - the following is inactive and will not be activated + // - the following is active and will be deactivated and will not be restarted + std::optional found_conflict = std::nullopt; + if (following_state.is_inactive && !following_state.in_activate_list) + { + found_conflict = handle_conflict( + "The controller with name '" + preceding_ctrl.info.name + + "' cannot be activated because the following controller with name '" + + following_ctrl.info.name + "' is inactive and will not be activated."); + } + if ( + following_state.is_active && following_state.in_deactivate_list && + !following_state.in_activate_list) + { + found_conflict = handle_conflict( + "The controller with name '" + preceding_ctrl.info.name + + "' cannot be activated because the following controller with name '" + + following_ctrl.info.name + + "' is active and will be deactivated but will not be activated."); + } + + if (found_conflict.has_value()) + { + if (!found_conflict.value()) + { + return CheckConflictResult::Error; + } + + // if BEST_EFFORT, remove the preceding controller from the activate list and proceed to the + // next preceding + remove_element_from_list(activate_request, preceding_ctrl.info.name); + return CheckConflictResult::CheckNextPreceding; + } + } + + // check for conflict when preceding is active and will not be transitioned + if ( + preceding_state.is_active && !preceding_state.in_deactivate_list && + !preceding_state.in_activate_list) + { + // In the following cases, following cannot be deactivated (and thus cannot be restarted), + // resulting in an error: + // - the following is active and will be deactivated + // - the following is active and will be restarted + // In the end, an error occurs if following wll be deactivated, regardless of whether + // following will be restarted or not. + // (Note: Since preceding is active, following is always active, so there is no need to check if + // following is active.) + if (following_state.in_deactivate_list) + { + if (!handle_conflict( + "The following controller with name '" + following_ctrl.info.name + + "' cannot be deactivated because the preceding controller with name '" + + preceding_ctrl.info.name + "' is active and will not be deactivated and restarted.")) + { + return CheckConflictResult::Error; + } + + // if BEST_EFFORT, remove the following controller from the activate and deactivate + // list and proceed to the next following + remove_element_from_list(activate_request, following_ctrl.info.name); + remove_element_from_list(deactivate_request, following_ctrl.info.name); + return CheckConflictResult::CheckNextFollowing; + } + } + + // check for conflict when preceding will be restarted from active + if ( + preceding_state.is_active && preceding_state.in_deactivate_list && + preceding_state.in_activate_list) + { + // In the following cases, following cannot be deactivated, resulting in an error: + // - the following is active and will be deactivated but will not be restarted + if ( + following_state.is_active && following_state.in_deactivate_list && + !following_state.in_activate_list) + { + if (!handle_conflict( + "The following controller with name '" + following_ctrl.info.name + + "' cannot be deactivated because the preceding controller with name '" + + preceding_ctrl.info.name + "' is active and will be restarted.")) + { + return CheckConflictResult::Error; + } + + // if BEST_EFFORT, remove the following controller from the deactivate list and proceed to + // the next following + remove_element_from_list(deactivate_request, following_ctrl.info.name); + return CheckConflictResult::CheckNextFollowing; + } + } + + // No conflict occurs in other conditions, so return OK + return CheckConflictResult::OK; +} + +void propagate_chained_mode( + const rclcpp::Logger & logger, const controller_manager::ControllerSpec & preceding_ctrl, + 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 @@ -1102,8 +1385,8 @@ controller_interface::return_type ControllerManager::switch_controller( // following controller, make the corresponding 'from'/'to' chained mode request. if ( check_de_activate_request_conflict_and_create_chained_mode_request( - strictness, controllers, deactivate_request_, activate_request_, to_chained_mode_request_, - from_chained_mode_request_) != controller_interface::return_type::OK) + strictness, controllers, deactivate_request_, activate_request_, from_chained_mode_request_, + to_chained_mode_request_) != controller_interface::return_type::OK) { RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)"); // reset all lists @@ -2230,438 +2513,147 @@ void ControllerManager::RTControllerListWrapper::wait_until_rt_not_using( { while (used_by_realtime_controllers_index_ == index) { - if (!rclcpp::ok()) - { - throw std::runtime_error("rclcpp interrupted"); - } - std::this_thread::sleep_for(sleep_period); - } -} - -std::pair ControllerManager::split_command_interface( - const std::string & command_interface) -{ - auto index = command_interface.find('/'); - auto prefix = command_interface.substr(0, index); - auto interface_type = command_interface.substr(index + 1, command_interface.size() - 1); - return {prefix, interface_type}; -} - -unsigned int ControllerManager::get_update_rate() const { return update_rate_; } - -void ControllerManager::shutdown_async_controllers_and_components() -{ - async_controller_threads_.erase( - async_controller_threads_.begin(), async_controller_threads_.end()); - resource_manager_->shutdown_async_components(); -} - -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) - { - 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 (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); - } - } -}; - -void ControllerManager::cache_controller_interfaces_in_activate_request( - const std::vector & controllers, - const std::vector & activate_request) -{ - auto & resource_manager = *resource_manager_; - - const auto cache_controller_to_hardware = [&resource_manager](const ControllerSpec & controller) - { - 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); - }; - - 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) - { - cache_controller_to_hardware(controller); - } - } -}; - -enum class ConflictStatus -{ - NO_CONFLICT, - CONFLICT_WITH_BEST_EFFORT, - CONFLICT_WITH_STRICT, -}; - -controller_interface::return_type check_de_activate_request_conflict_for_controller( - const rclcpp::Logger & logger, const int strictness, const ControllerSpec & controller, - std::vector & deactivate_request, std::vector & activate_request) -{ - const auto handle_conflict = [&logger, strictness](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; - }; - - 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( - "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 doubled activation - if (ctrl_state.is_active && !ctrl_state.in_deactivate_list && ctrl_state.in_activate_list) - { - if (!handle_conflict( - "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( - "Could not activate controller '" + controller.info.name + - "' since it is not in inactive state")) + if (!rclcpp::ok()) { - return controller_interface::return_type::ERROR; + throw std::runtime_error("rclcpp interrupted"); } - remove_element_from_list(activate_request, controller.info.name); - ctrl_state.in_activate_list = false; + std::this_thread::sleep_for(sleep_period); } +} - return controller_interface::return_type::OK; +std::pair ControllerManager::split_command_interface( + const std::string & command_interface) +{ + auto index = command_interface.find('/'); + auto prefix = command_interface.substr(0, index); + auto interface_type = command_interface.substr(index + 1, command_interface.size() - 1); + return {prefix, interface_type}; } -enum class CheckConflictResult +unsigned int ControllerManager::get_update_rate() const { return update_rate_; } + +void ControllerManager::shutdown_async_controllers_and_components() { - OK, - CheckNextFollowing, - CheckNextPreceding, - Error, -}; + async_controller_threads_.erase( + async_controller_threads_.begin(), async_controller_threads_.end()); + resource_manager_->shutdown_async_components(); +} -CheckConflictResult check_conflict_of_preceding_and_following( - const rclcpp::Logger & logger, const int strictness, const ControllerSpec & preceding_ctrl, - const ControllerSpec & following_ctrl, const ActivationState & preceding_state, - const ActivationState & following_state, std::vector & deactivate_request, - std::vector & activate_request) +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 handle_conflict = [&logger, strictness](const std::string & msg) + const auto & resource_manager = *resource_manager_; + + const auto extract_interfaces_for_controller = + [&resource_manager]( + const ControllerSpec & controller, std::vector & request_interface_list) { - if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + 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) { - RCLCPP_ERROR(logger, "%s", msg.c_str()); - return false; + command_interface_names = resource_manager.available_command_interfaces(); } - RCLCPP_WARN(logger, "%s", msg.c_str()); - return true; + 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()); }; - RCLCPP_ERROR( - logger, "Checking conflict between '%s' and '%s'", preceding_ctrl.info.name.c_str(), - following_ctrl.info.name.c_str()); - - // if neither the preceding nor the following is included in any request, no check is - // required, so proceed to the next following - if ( - !preceding_state.in_activate_list && !preceding_state.in_deactivate_list && - !following_state.in_activate_list && !following_state.in_deactivate_list) + for (const auto & controller : controllers) { - RCLCPP_DEBUG( - logger, - " - No need to check preceding '%s' and following '%s' because there are no requests", - preceding_ctrl.info.name.c_str(), following_ctrl.info.name.c_str()); - return CheckConflictResult::CheckNextFollowing; - } + 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 the preceding will be activated, check if the following is chainable - if (preceding_state.in_activate_list && !following_ctrl.c->is_chainable()) - { - if (!handle_conflict( - "The preceding controller with name '" + preceding_ctrl.info.name + - "' cannot be activated because the following controller with name '" + - following_ctrl.info.name + "' is not chainable.")) + if (in_activate_list) { - return CheckConflictResult::Error; + extract_interfaces_for_controller(controller, activate_command_interface_request); + } + if (in_deactivate_list) + { + extract_interfaces_for_controller(controller, deactivate_command_interface_request); } - - // if BEST_EFFORT, remove the preceding controller from the activate list and proceed to the - // next preceding - remove_element_from_list(activate_request, preceding_ctrl.info.name); - return CheckConflictResult::CheckNextPreceding; } +}; - // check for conflict when preceding will be activated from inactive - if (preceding_state.is_inactive && preceding_state.in_activate_list) +void ControllerManager::cache_controller_interfaces_in_activate_request( + const std::vector & controllers, + const std::vector & activate_request) +{ + auto & resource_manager = *resource_manager_; + + const auto cache_controller_to_hardware = [&resource_manager](const ControllerSpec & controller) { - // 以下のいずれかの場合は preceding を activate できないためエラー - // 1-a. following が inactive で activate されない場合 - // 1-b. following が active で deactivateされ、かつ、restart されない場合 - // エラー発生かつstrictがBEST_EFFORTの場合は、precedingをactivateリストから削除 + std::vector interface_names = {}; - std::optional found_conflict = std::nullopt; - if (following_state.is_inactive && !following_state.in_activate_list) + auto command_interface_config = controller.c->command_interface_configuration(); + if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) { - found_conflict = handle_conflict( - "The controller with name '" + preceding_ctrl.info.name + - "' cannot be activated because the following controller with name '" + - following_ctrl.info.name + "' is inactive and will not be activated."); + interface_names = resource_manager.available_command_interfaces(); } if ( - following_state.is_active && following_state.in_deactivate_list && - !following_state.in_activate_list) - { - found_conflict = handle_conflict( - "The controller with name '" + preceding_ctrl.info.name + - "' cannot be activated because the following controller with name '" + - following_ctrl.info.name + - "' is active and will be deactivated but will not be activated."); - } - - if (found_conflict.has_value()) + command_interface_config.type == + controller_interface::interface_configuration_type::INDIVIDUAL) { - if (!found_conflict.value()) - { - return CheckConflictResult::Error; - } - - // if BEST_EFFORT, remove the preceding controller from the activate list and proceed to the - // next preceding - remove_element_from_list(activate_request, preceding_ctrl.info.name); - return CheckConflictResult::CheckNextPreceding; + interface_names = command_interface_config.names; } - } - // check for conflict when preceding is active and will not be transitioned - if ( - preceding_state.is_active && !preceding_state.in_deactivate_list && - !preceding_state.in_activate_list) - { - // 以下のいずれかの場合は following を deactivate できない (結果として restart もできない) - // ためエラー - // 2-1. following が active で deactivate される場合 - // 2-2. following が active で restart される場合 - // -> 結局のところ、following が activate されるか否かを問わず、deactivate - // される場合はエラー(precedingがactiveの時、followingは必ずactiveなので、followingのactiveチェックは不要) - // エラー発生かつstrictがBEST_EFFORTの場合はfollowingをactivate/deactivateリストから削除 - if (following_state.in_deactivate_list) + std::vector interfaces = {}; + auto state_interface_config = controller.c->state_interface_configuration(); + if (state_interface_config.type == controller_interface::interface_configuration_type::ALL) { - if (!handle_conflict( - "The following controller with name '" + following_ctrl.info.name + - "' cannot be deactivated because the preceding controller with name '" + - preceding_ctrl.info.name + "' is active and will not be deactivated and restarted.")) - { - return CheckConflictResult::Error; - } - - // if BEST_EFFORT, remove the following controller from the activate and deactivate - // list and proceed to the next following - remove_element_from_list(activate_request, following_ctrl.info.name); - remove_element_from_list(deactivate_request, following_ctrl.info.name); - return CheckConflictResult::CheckNextFollowing; + interfaces = resource_manager.available_state_interfaces(); } - } - - // check for conflict when preceding will be restarted from active - if ( - preceding_state.is_active && preceding_state.in_deactivate_list && - preceding_state.in_activate_list) - { - // following が active で deactivate され、かつ、restart しない場合は following を - // deactivate できないためエラー - // エラー発生かつstrictがBEST_EFFORTの場合はfollowingをdeactivateリストから削除 if ( - following_state.is_active && following_state.in_deactivate_list && - !following_state.in_activate_list) + state_interface_config.type == controller_interface::interface_configuration_type::INDIVIDUAL) { - if (!handle_conflict( - "The following controller with name '" + following_ctrl.info.name + - "' cannot be deactivated because the preceding controller with name '" + - preceding_ctrl.info.name + "' is active and will be restarted.")) - { - return CheckConflictResult::Error; - } - - // if BEST_EFFORT, remove the following controller from the deactivate list and proceed to - // the next following - remove_element_from_list(deactivate_request, following_ctrl.info.name); - return CheckConflictResult::CheckNextFollowing; + interfaces = state_interface_config.names; } - } - - // No conflict occurs in other conditions, so return OK - return CheckConflictResult::OK; -} - -void propagate_chained_mode( - const rclcpp::Logger & logger, const ControllerSpec & preceding_ctrl, - const ControllerSpec & following_ctrl, const ActivationState & preceding_state, - const ActivationState & following_state, std::vector & deactivate_request, - std::vector & activate_request, std::vector & to_chained_mode_request, - std::vector & from_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; + interface_names.insert(interface_names.end(), interfaces.begin(), interfaces.end()); - // 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; - } + resource_manager.cache_controller_to_hardware(controller.info.name, interface_names); + }; - // 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) + for (const auto & controller : controllers) { - add_element_to_list(to_chained_mode_request, following_ctrl.info.name); - in_to_chained_mode_list = true; + const bool in_activate_list = is_element_in_list(activate_request, controller.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) - if (!following_ctrl.c->is_in_chained_mode()) + // 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) { - resource_manager.make_controller_reference_interfaces_available(following_ctrl.info.name); + cache_controller_to_hardware(controller); } } - - // 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); - }; - } -} +}; controller_interface::return_type ControllerManager::check_de_activate_request_conflict_and_create_chained_mode_request( - const int strictness, const std::vector & controllers, + const int strictness, const std::vector & controllers, std::vector & deactivate_request, std::vector & activate_request, - std::vector & to_chained_mode_request, - std::vector & from_chained_mode_request) const + std::vector & from_chained_mode_request, + std::vector & to_chained_mode_request) const { - const auto logger = get_logger(); - // check if there are controllers to (de)activate if (activate_request.empty() && deactivate_request.empty()) { - RCLCPP_DEBUG(logger, "No controllers to (de)activate"); + RCLCPP_DEBUG(get_logger(), "No controllers to (de)activate"); return controller_interface::return_type::OK; } - // Check for consistency in the order of "preceding" to "following" + // 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) @@ -2669,7 +2661,7 @@ ControllerManager::check_de_activate_request_conflict_and_create_chained_mode_re // check for conflict of (de)activate request for the controller if ( check_de_activate_request_conflict_for_controller( - logger, strictness, controller, deactivate_request, activate_request) != + get_logger(), strictness, controller, deactivate_request, activate_request) != controller_interface::return_type::OK) { return controller_interface::return_type::ERROR; @@ -2693,7 +2685,7 @@ ControllerManager::check_de_activate_request_conflict_and_create_chained_mode_re const auto & preceding_ctrl = controller; const auto & preceding_state = controller_state; - // check consistency of the preceding and each following controller + // check for conflict of the preceding and each following controller for (const auto & following_ctrl_ref : following_ctrl_refs) { const auto & following_ctrl = following_ctrl_ref.get(); @@ -2701,8 +2693,8 @@ ControllerManager::check_de_activate_request_conflict_and_create_chained_mode_re const auto following_state = get_activation_state(following_ctrl, deactivate_request, activate_request); - const auto check_result = check_conflict_of_preceding_and_following( - logger, strictness, preceding_ctrl, following_ctrl, preceding_state, following_state, + const auto check_result = check_conflict_between_preceding_and_following( + get_logger(), strictness, preceding_ctrl, following_ctrl, preceding_state, following_state, deactivate_request, activate_request); // based on the check result, select the next action @@ -2711,16 +2703,18 @@ ControllerManager::check_de_activate_request_conflict_and_create_chained_mode_re // if no conflict is detected in the activate/deactivate request, proceed to // determine the chained_mode request propagate_chained_mode( - logger, preceding_ctrl, following_ctrl, preceding_state, following_state, - deactivate_request, activate_request, to_chained_mode_request, from_chained_mode_request, + get_logger(), preceding_ctrl, following_ctrl, preceding_state, following_state, + deactivate_request, activate_request, from_chained_mode_request, to_chained_mode_request, *resource_manager_); } else if (check_result == CheckConflictResult::CheckNextFollowing) { + // proceed to check the next following controller continue; } else if (check_result == CheckConflictResult::CheckNextPreceding) { + // proceed to check the next preceding controller break; } else From bf9f2fc6bb1e1e0865ce54edc8584f1e7c4e88ad Mon Sep 17 00:00:00 2001 From: Takashi Sato Date: Tue, 11 Jun 2024 21:21:41 +0900 Subject: [PATCH 05/16] remove debug code and add comment --- controller_manager/src/controller_manager.cpp | 57 ++++--------------- 1 file changed, 12 insertions(+), 45 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 1d701dd0a5..c879bf8fc4 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -486,7 +486,7 @@ CheckConflictResult check_conflict_between_preceding_and_following( return CheckConflictResult::OK; } -void propagate_chained_mode( +void update_chained_mode_request_of_following( const rclcpp::Logger & logger, const controller_manager::ControllerSpec & preceding_ctrl, const controller_manager::ControllerSpec & following_ctrl, const ActivationState & preceding_state, const ActivationState & following_state, @@ -1230,7 +1230,7 @@ controller_interface::return_type ControllerManager::switch_controller( { switch_params_ = SwitchParams(); - const auto validate_requests_and_internal_states = [this, &strictness]() + const auto check_request_and_internal_state_before_switch = [this, &strictness]() { if (!deactivate_request_.empty() || !activate_request_.empty()) { @@ -1272,7 +1272,7 @@ controller_interface::return_type ControllerManager::switch_controller( } }; - validate_requests_and_internal_states(); + check_request_and_internal_state_before_switch(); RCLCPP_DEBUG(get_logger(), "Activating controllers:"); for (const auto & controller : activate_controllers) @@ -1344,35 +1344,6 @@ controller_interface::return_type ControllerManager::switch_controller( return ret; } - const auto show_list = [this](const std::string & tag) - { - RCLCPP_WARN(get_logger(), "[%s]----------------", tag.c_str()); - RCLCPP_WARN(get_logger(), "Activating controllers:"); - for (const auto & controller : activate_request_) - { - RCLCPP_WARN(get_logger(), " - %s", controller.c_str()); - } - RCLCPP_WARN(get_logger(), "Deactivating controllers:"); - for (const auto & controller : deactivate_request_) - { - RCLCPP_WARN(get_logger(), " - %s", controller.c_str()); - } - RCLCPP_WARN(get_logger(), "to chained mode:"); - for (const auto & req : to_chained_mode_request_) - { - RCLCPP_WARN(get_logger(), " - %s", req.c_str()); - } - RCLCPP_WARN(get_logger(), "from chained mode:"); - for (const auto & req : from_chained_mode_request_) - { - RCLCPP_WARN(get_logger(), " - %s", req.c_str()); - } - RCLCPP_WARN(get_logger(), "----------------"); - }; - - RCLCPP_WARN(get_logger(), "STRICTNESS: %d", strictness); - show_list("REQUEST"); - // lock controllers std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); @@ -1394,8 +1365,6 @@ controller_interface::return_type ControllerManager::switch_controller( return controller_interface::return_type::ERROR; } - show_list("FINALLY"); - // check if we need to request switch controllers after all the checks if (activate_request_.empty() && deactivate_request_.empty()) { @@ -1404,10 +1373,13 @@ controller_interface::return_type ControllerManager::switch_controller( 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:"); @@ -1444,20 +1416,12 @@ controller_interface::return_type ControllerManager::switch_controller( // wait until switch is finished RCLCPP_DEBUG(get_logger(), "Requested atomic controller switch from realtime loop"); - int timeout_count = 0; while (rclcpp::ok() && switch_params_.do_switch) { if (!rclcpp::ok()) { return controller_interface::return_type::ERROR; } - - if (++timeout_count > 10 * 3000) - { - RCLCPP_ERROR(get_logger(), "Timeout in atomic controller switch"); - return controller_interface::return_type::ERROR; - } - std::this_thread::sleep_for(std::chrono::microseconds(100)); } @@ -2700,9 +2664,12 @@ ControllerManager::check_de_activate_request_conflict_and_create_chained_mode_re // based on the check result, select the next action if (check_result == CheckConflictResult::OK) { - // if no conflict is detected in the activate/deactivate request, proceed to - // determine the chained_mode request - propagate_chained_mode( + // If no conflicts are found in the (de)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(), preceding_ctrl, following_ctrl, preceding_state, following_state, deactivate_request, activate_request, from_chained_mode_request, to_chained_mode_request, *resource_manager_); From 8c41a0c54a5f28fbcd3684a08717641f6f0ba523 Mon Sep 17 00:00:00 2001 From: Takashi Sato Date: Wed, 12 Jun 2024 17:41:28 +0900 Subject: [PATCH 06/16] fix logic and add test --- controller_manager/src/controller_manager.cpp | 507 +++++++++++++++++- ...llers_chaining_with_controller_manager.cpp | 172 +++++- 2 files changed, 673 insertions(+), 6 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index c879bf8fc4..3e76c41082 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -18,10 +18,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" @@ -230,13 +232,13 @@ ActivationState get_activation_state( std::vector> list_following_controllers( - const controller_manager::ControllerSpec & controller, const ActivationState & controller_state, - const std::vector & controllers) + const controller_manager::ControllerSpec & controller, const bool is_active, + const bool is_inactive, const std::vector & controllers) { // If the controller is neither active nor inactive (in which case it is likely either // unconfigured or finalized), it is impossible to retrieve the command_interface configuration, // so return an empty vector - if (!controller_state.is_active && !controller_state.is_inactive) + if (!is_active && !is_inactive) { return {}; } @@ -267,6 +269,249 @@ list_following_controllers( return following_ctrl_refs; } +std::optional find_controller_by_name( + const std::vector & controllers, const std::string & name) +{ + auto it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, name)); + if (it == controllers.end()) + { + return std::nullopt; + } + return it; +} + +using ControllersMap = std::unordered_map; +using ControllerChainSpecsMap = + std::unordered_map; + +ControllersMap create_controllers_map( + const std::vector & controllers) +{ + ControllersMap controllers_map; + for (const auto & controller : controllers) + { + controllers_map.insert({controller.info.name, controller}); + } + return controllers_map; +} + +#if 0 +using ControllerRef = std::reference_wrapper; +using ControllerRefs = std::vector; + +class ChainableController +{ +public: + ChainableController(const controller_manager::ControllerSpec & spec_) : spec(spec_) {} + ChainableController() = delete; + + const controller_manager::ControllerSpec & spec; + std::vector> following_controllers; + std::vector> preceding_controllers; +}; + +using ControllerChainMap = std::unordered_map>; + +ControllerChainMap create_controller_chain_map( + const std::vector & controllers) +{ + ControllerChainMap chainable_controllers_map; + for (const auto & controller : controllers) + { + const bool is_active = is_controller_active(controller.c); + const bool is_inactive = is_controller_inactive(controller.c); + + // If the controller is neither active nor inactive (in which case it is likely either + // unconfigured or finalized), it is impossible to retrieve the command_interface configuration, + // so return an empty vector + if (!is_active && !is_inactive) + { + continue; + } + + for (const auto & cmd_itf_name : controller.c->command_interface_configuration().names) + { + // controller that 'cmd_tf_name' belongs to + controller_manager::ControllersListIterator following_ctrl_it; + if (!command_interface_is_reference_interface_of_controller( + cmd_itf_name, controllers, following_ctrl_it)) + { + // interfaces that are not related to the chain do not need to be checked + continue; + } + + auto preceding_it = chainable_controllers_map.find(controller.info.name); + if (preceding_it == chainable_controllers_map.end()) + { + preceding_it = + chainable_controllers_map + .emplace(controller.info.name, std::make_shared(controller)) + .first; + } + const auto & preceding = preceding_it->second; + + auto following_it = chainable_controllers_map.find(following_ctrl_it->info.name); + if (following_it == chainable_controllers_map.end()) + { + following_it = + chainable_controllers_map + .emplace(controller.info.name, std::make_shared(controller)) + .first; + } + const auto & following = following_it->second; + + if ( + std::find_if( + preceding->following_controllers.begin(), preceding->following_controllers.end(), + [&](const std::weak_ptr & ctrl) { + return ctrl.lock()->spec.info.name == following->spec.info.name; + }) == preceding->following_controllers.end()) + { + preceding->following_controllers.push_back(following); + } + + if ( + std::find_if( + following->preceding_controllers.begin(), following->preceding_controllers.end(), + [&](const std::weak_ptr & ctrl) { + return ctrl.lock()->spec.info.name == preceding->spec.info.name; + }) == following->preceding_controllers.end()) + { + following->preceding_controllers.push_back(preceding); + } + } + } + + return chainable_controllers_map; +} +#endif + +bool will_become_active(const ActivationState & state) +{ + return (state.is_inactive && state.in_activate_list) || + (state.is_active && (!state.in_deactivate_list || state.in_activate_list)); +} + +bool will_become_inactive(const ActivationState & state) +{ + return !state.in_activate_list && + (state.is_inactive || (state.is_active && state.in_deactivate_list)); +} + +bool will_be_restarted(const ActivationState & state) +{ + return state.is_active && state.in_deactivate_list && state.in_activate_list; +} + +bool can_activate( + const rclcpp::Logger & logger, const std::string & controller_name, + const ControllersMap & controllers_map, const 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); + + if (will_become_inactive(following_state)) + { + 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 (!can_activate( + logger, following_controller_name, controllers_map, chain_specs_map, deactivate_request, + activate_request)) + { + return false; + } + } + return true; +} + +bool can_deactivate( + const rclcpp::Logger & logger, const std::string & controller_name, + const ControllersMap & controllers_map, const 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); + + if (will_become_active(preceding_state)) + { + 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; + } + + if (!can_deactivate( + logger, preceding_controller_name, controllers_map, chain_specs_map, deactivate_request, + activate_request)) + { + return false; + } + } + return true; +} + +bool can_restart( + const rclcpp::Logger & logger, const std::string & controller_name, + const ControllersMap & controllers_map, const 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); + + if ( + !will_be_restarted(preceding_state) && + (preceding_state.is_active && !preceding_state.in_deactivate_list)) + { + RCLCPP_WARN( + logger, + "Controller '%s' cannot be restarted because preceding controller with name '%s' will " + "be not restarted and become active after switch", + controller_name.c_str(), preceding_controller_name.c_str()); + return false; + } + + if (!can_restart( + logger, preceding_controller_name, controllers_map, chain_specs_map, deactivate_request, + activate_request)) + { + return false; + } + } + return true; +} + controller_interface::return_type check_de_activate_request_conflict_for_controller( const rclcpp::Logger & logger, const int strictness, const controller_manager::ControllerSpec & controller, @@ -1228,6 +1473,34 @@ controller_interface::return_type ControllerManager::switch_controller( const std::vector & deactivate_controllers, int strictness, bool activate_asap, const rclcpp::Duration & timeout) { + const auto show_list = [this](const std::string & tag) + { + RCLCPP_WARN(get_logger(), "[%s]----------------", tag.c_str()); + RCLCPP_WARN(get_logger(), "Activating controllers:"); + for (const auto & controller : activate_request_) + { + RCLCPP_WARN(get_logger(), " - %s", controller.c_str()); + } + RCLCPP_WARN(get_logger(), "Deactivating controllers:"); + for (const auto & controller : deactivate_request_) + { + RCLCPP_WARN(get_logger(), " - %s", controller.c_str()); + } + RCLCPP_WARN(get_logger(), "to chained mode:"); + for (const auto & req : to_chained_mode_request_) + { + RCLCPP_WARN(get_logger(), " - %s", req.c_str()); + } + RCLCPP_WARN(get_logger(), "from chained mode:"); + for (const auto & req : from_chained_mode_request_) + { + RCLCPP_WARN(get_logger(), " - %s", req.c_str()); + } + RCLCPP_WARN(get_logger(), "----------------"); + }; + + RCLCPP_WARN(get_logger(), "STRICTNESS: %d", strictness); + switch_params_ = SwitchParams(); const auto check_request_and_internal_state_before_switch = [this, &strictness]() @@ -1344,6 +1617,8 @@ controller_interface::return_type ControllerManager::switch_controller( return ret; } + show_list("REQUEST"); + // lock controllers std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); @@ -1365,6 +1640,8 @@ controller_interface::return_type ControllerManager::switch_controller( return controller_interface::return_type::ERROR; } + show_list("AFTER CHECK"); + // check if we need to request switch controllers after all the checks if (activate_request_.empty() && deactivate_request_.empty()) { @@ -1416,8 +1693,15 @@ controller_interface::return_type ControllerManager::switch_controller( // wait until switch is finished RCLCPP_DEBUG(get_logger(), "Requested atomic controller switch from realtime loop"); + int timeout_count = 0; while (rclcpp::ok() && switch_params_.do_switch) { + if (++timeout_count > 3000) + { + RCLCPP_ERROR(get_logger(), "Timeout while waiting for atomic controller switch to finish"); + clear_requests(); + return controller_interface::return_type::ERROR; + } if (!rclcpp::ok()) { return controller_interface::return_type::ERROR; @@ -2617,6 +2901,222 @@ ControllerManager::check_de_activate_request_conflict_and_create_chained_mode_re return controller_interface::return_type::OK; } +#if 1 + const auto controllers_map = create_controllers_map(controllers); + + // RCLCPP_DEBUG(get_logger(), "============== CHAIN SPEC ==================="); + // for (const auto & [controller_name, chain] : controller_chain_spec_) + // { + // RCLCPP_DEBUG(get_logger(), "-'%s'", controller_name.c_str()); + // RCLCPP_DEBUG(get_logger(), " [preceding]"); + // for (const auto & preceding : chain.preceding_controllers) + // { + // RCLCPP_DEBUG(get_logger(), "\t- '%s'", preceding.c_str()); + // } + // RCLCPP_DEBUG(get_logger(), " [following]"); + // for (const auto & following : chain.following_controllers) + // { + // RCLCPP_DEBUG(get_logger(), "\t- '%s'", following.c_str()); + // } + // } + // RCLCPP_DEBUG(get_logger(), "============================================"); + // return controller_interface::return_type::ERROR; + + // 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) + { + // check for conflict of (de)activate request for the controller + if ( + check_de_activate_request_conflict_for_controller( + get_logger(), strictness, controller, deactivate_request, activate_request) != + controller_interface::return_type::OK) + { + return controller_interface::return_type::ERROR; + } + } + +#if 1 + // 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) + { + const auto controller_state = + get_activation_state(controller, deactivate_request, activate_request); + + if (controller_state.in_deactivate_list && controller_state.in_activate_list) + { + if (!can_restart( + get_logger(), controller.info.name, controllers_map, controller_chain_spec_, + deactivate_request, activate_request)) + { + if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + { + RCLCPP_ERROR( + get_logger(), "Cannot restart controller '%s' due to conflict", + controller.info.name.c_str()); + return controller_interface::return_type::ERROR; + } + else + { + RCLCPP_WARN( + get_logger(), "Cannot restart controller '%s' due to conflict", + controller.info.name.c_str()); + remove_element_from_list(activate_request, controller.info.name); + } + } + else + { + continue; + } + } + + if (controller_state.in_deactivate_list) + { + if (!can_deactivate( + get_logger(), controller.info.name, controllers_map, controller_chain_spec_, + deactivate_request, activate_request)) + { + if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + { + RCLCPP_ERROR( + get_logger(), "Cannot deactivate controller '%s' due to conflict", + controller.info.name.c_str()); + return controller_interface::return_type::ERROR; + } + else + { + RCLCPP_WARN( + get_logger(), "Cannot deactivate controller '%s' due to conflict", + controller.info.name.c_str()); + remove_element_from_list(deactivate_request, controller.info.name); + } + } + } + if (controller_state.in_activate_list) + { + if (!can_activate( + get_logger(), controller.info.name, controllers_map, controller_chain_spec_, + deactivate_request, activate_request)) + { + if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + { + RCLCPP_ERROR( + get_logger(), "Cannot activate controller '%s' due to conflict", + controller.info.name.c_str()); + return controller_interface::return_type::ERROR; + } + else + { + RCLCPP_WARN( + get_logger(), "Cannot activate controller '%s' due to conflict", + controller.info.name.c_str()); + remove_element_from_list(activate_request, controller.info.name); + } + } + } + } + +#else + sort_list_by_another(deactivate_request, ordered_controllers_names_, false); + sort_list_by_another(activate_request, ordered_controllers_names_, true); + + for (auto ctrl_it = deactivate_request.cbegin(); ctrl_it != deactivate_request.cend();) + { + if (can_deactivate( + get_logger(), *ctrl_it, controllers_map, controller_chain_spec_, deactivate_request, + activate_request)) + { + ++ctrl_it; + continue; + } + + if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + { + RCLCPP_ERROR( + get_logger(), "Cannot deactivate controller '%s' due to conflict", ctrl_it->c_str()); + return controller_interface::return_type::ERROR; + } + else + { + RCLCPP_WARN( + get_logger(), "Cannot deactivate controller '%s' due to conflict", ctrl_it->c_str()); + ctrl_it = deactivate_request.erase(ctrl_it); + } + } + + for (auto ctrl_it = activate_request.crbegin(); ctrl_it != activate_request.crend();) + { + if (can_activate( + get_logger(), *ctrl_it, controllers_map, controller_chain_spec_, deactivate_request, + activate_request)) + { + ++ctrl_it; + continue; + } + + if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + { + RCLCPP_ERROR( + get_logger(), "Cannot activate controller '%s' due to conflict", ctrl_it->c_str()); + return controller_interface::return_type::ERROR; + } + else + { + RCLCPP_WARN( + get_logger(), "Cannot activate controller '%s' due to conflict", ctrl_it->c_str()); + ctrl_it = std::vector::reverse_iterator( + activate_request.erase(std::next(ctrl_it).base())); + } + } +#endif + + // 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) + { + const auto controller_state = + get_activation_state(controller, deactivate_request, activate_request); + + // get following controllers references for the controller + const auto following_ctrl_refs = list_following_controllers( + controller, controller_state.is_active, controller_state.is_inactive, controllers); + + // if there are no following controllers, this controller is not a preceding controller, so + // skip the following checks + if (following_ctrl_refs.empty()) + { + continue; + } + + // this controller is a preceding controller + const auto & preceding_ctrl = controller; + const auto & preceding_state = controller_state; + + // check for conflict of the preceding and each following controller + for (const auto & following_ctrl_ref : following_ctrl_refs) + { + const auto & following_ctrl = following_ctrl_ref.get(); + + const auto following_state = + get_activation_state(following_ctrl, deactivate_request, activate_request); + + // If no conflicts are found in the (de)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(), preceding_ctrl, following_ctrl, preceding_state, following_state, + deactivate_request, activate_request, from_chained_mode_request, to_chained_mode_request, + *resource_manager_); + } + } + +#else // 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. @@ -2690,6 +3190,7 @@ ControllerManager::check_de_activate_request_conflict_and_create_chained_mode_re } } } +#endif // To ensure that the appropriate (de)activation are performed, sort deactivate requests in // the order of preceding to following, and sort activate requests in the reverse order of 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 d283d05306..1bd2e9e579 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -68,6 +68,8 @@ class TestableTestChainableController : public test_chainable_controller::TestCh FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers_reset); FRIEND_TEST( TestControllerChainingWithControllerManager, test_chained_controllers_reset_error_handling); + FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers_ex); + FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers_ex2); }; class TestableControllerManager : public controller_manager::ControllerManager @@ -105,6 +107,8 @@ class TestableControllerManager : public controller_manager::ControllerManager FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers_reset); FRIEND_TEST( TestControllerChainingWithControllerManager, test_chained_controllers_reset_error_handling); + FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers_ex); + FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers_ex2); public: TestableControllerManager( @@ -1311,6 +1315,12 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res ASSERT_EQ(1u, diff_drive_controller->deactivate_calls); // +0 ASSERT_EQ(1u, position_tracking_controller->deactivate_calls); // +1 (reset) + // 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()); + ASSERT_FALSE(position_tracking_controller->is_in_chained_mode()); + // Verify update after reset most preceding controller // (internal_counter 5->6) UpdateAllControllerAndCheck({1024.0, 4096.0}, 5u); @@ -1341,6 +1351,12 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res ASSERT_EQ(2u, diff_drive_controller->deactivate_calls); // +1 (reset) ASSERT_EQ(2u, position_tracking_controller->deactivate_calls); // +1 (reset) + // 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()); + ASSERT_FALSE(position_tracking_controller->is_in_chained_mode()); + // Verify update again after reset all controllers // (internal_counter 8->9->10) UpdateAllControllerAndCheck({16.0, 64.0}, 8u); @@ -1373,6 +1389,12 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res ASSERT_EQ(2u, pid_right_wheel_controller->deactivate_calls); // +0 ASSERT_EQ(3u, diff_drive_controller->deactivate_calls); // +1 (reset) 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()); + ASSERT_FALSE(position_tracking_controller->is_in_chained_mode()); } { @@ -1397,6 +1419,12 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res ASSERT_EQ(3u, pid_right_wheel_controller->deactivate_calls); // +1 (reset) ASSERT_EQ(4u, diff_drive_controller->deactivate_calls); // +1 (reset) 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()); + ASSERT_FALSE(position_tracking_controller->is_in_chained_mode()); } { @@ -1424,6 +1452,12 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res ASSERT_EQ(3u, pid_right_wheel_controller->deactivate_calls); // +0 ASSERT_EQ(5u, diff_drive_controller->deactivate_calls); // +1 (reset) 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()); + ASSERT_FALSE(position_tracking_controller->is_in_chained_mode()); } } @@ -1459,10 +1493,16 @@ TEST_P( 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()); + ASSERT_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 accepted, and the + // 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}, @@ -1485,6 +1525,12 @@ TEST_P( 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 (reset) + + // 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()); + ASSERT_FALSE(position_tracking_controller->is_in_chained_mode()); } } @@ -1530,13 +1576,24 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res 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()); + ASSERT_FALSE(position_tracking_controller->is_in_chained_mode()); }; + verify_all_controllers_are_active_and_not_reset(); // Attempt to reset the lowest following controllers (pid_controllers) ResetControllers({PID_LEFT_WHEEL, PID_RIGHT_WHEEL}, exp.return_type, std::future_status::ready); verify_all_controllers_are_active_and_not_reset(); - // Attempt to reset the lowest following controller (pid_right_wheel_controller) + // Attempt to reset the one of the lowest following controller (pid_left_wheel_controller) + ResetControllers({PID_LEFT_WHEEL}, exp.return_type, std::future_status::ready); + verify_all_controllers_are_active_and_not_reset(); + + // Attempt to reset another lowest following controller (pid_right_wheel_controller) ResetControllers({PID_RIGHT_WHEEL}, exp.return_type, std::future_status::ready); verify_all_controllers_are_active_and_not_reset(); @@ -1544,14 +1601,123 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res ResetControllers({DIFF_DRIVE_CONTROLLER}, exp.return_type, std::future_status::ready); verify_all_controllers_are_active_and_not_reset(); + // Attempt to reset the middle following and one of the lowest following controller + ResetControllers( + {DIFF_DRIVE_CONTROLLER, PID_LEFT_WHEEL}, exp.return_type, std::future_status::ready); + verify_all_controllers_are_active_and_not_reset(); + + // Attempt to reset the middle following and another lowest following controller + ResetControllers( + {DIFF_DRIVE_CONTROLLER, PID_LEFT_WHEEL}, exp.return_type, std::future_status::ready); + verify_all_controllers_are_active_and_not_reset(); + // Attempt to reset the all following controllers (pid_controllers and diff_drive_controller) ResetControllers( - {PID_LEFT_WHEEL, PID_RIGHT_WHEEL, DIFF_DRIVE_CONTROLLER}, exp.return_type, + {DIFF_DRIVE_CONTROLLER, PID_LEFT_WHEEL, PID_RIGHT_WHEEL}, exp.return_type, std::future_status::ready); verify_all_controllers_are_active_and_not_reset(); } } +TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_ex) +{ + 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); + + // 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_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}}}; + const auto & exp = expected.at(test_param.strictness); + + switch_test_controllers( + {POSITION_TRACKING_CONTROLLER, DIFF_DRIVE_CONTROLLER}, {}, test_param.strictness, + std::future_status::ready, exp.return_type); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + 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()); + + switch_test_controllers( + {POSITION_TRACKING_CONTROLLER, DIFF_DRIVE_CONTROLLER, PID_LEFT_WHEEL}, {}, + test_param.strictness, exp.future_status, exp.return_type); + + 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()); + + 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()); + ASSERT_FALSE(position_tracking_controller->is_in_chained_mode()); + + DeactivateController(PID_LEFT_WHEEL, exp.return_type, exp.future_status); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + pid_left_wheel_controller->get_state().id()); + + switch_test_controllers( + {POSITION_TRACKING_CONTROLLER, DIFF_DRIVE_CONTROLLER, PID_RIGHT_WHEEL}, {}, + test_param.strictness, exp.future_status, exp.return_type); + + 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()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + position_tracking_controller->get_state().id()); + + 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()); + ASSERT_FALSE(position_tracking_controller->is_in_chained_mode()); +} + INSTANTIATE_TEST_SUITE_P( test_strict_best_effort, TestControllerChainingWithControllerManager, testing::Values(strict, best_effort), [](const testing::TestParamInfo & info_) From 731999632a600ecd5b839a2c4f23f10af5278ef7 Mon Sep 17 00:00:00 2001 From: Takashi Sato Date: Thu, 13 Jun 2024 12:06:48 +0900 Subject: [PATCH 07/16] renamed from reset to restart --- controller_manager/CMakeLists.txt | 6 +- controller_manager/src/controller_manager.cpp | 2 +- ...llers_chaining_with_controller_manager.cpp | 147 +++++++++--------- ...roller.cpp => test_restart_controller.cpp} | 51 +++--- 4 files changed, 104 insertions(+), 102 deletions(-) rename controller_manager/test/{test_reset_controller.cpp => test_restart_controller.cpp} (85%) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index f76e172bfd..957ad0d33a 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -134,11 +134,11 @@ if(BUILD_TESTING) ros2_control_test_assets::ros2_control_test_assets ) - ament_add_gmock(test_reset_controller - test/test_reset_controller.cpp + ament_add_gmock(test_restart_controller + test/test_restart_controller.cpp APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$ ) - target_link_libraries(test_reset_controller + target_link_libraries(test_restart_controller controller_manager test_controller_with_command ros2_control_test_assets::ros2_control_test_assets diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 3e76c41082..ac6a32f83f 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2560,7 +2560,7 @@ void ControllerManager::manage_switch() deactivate_controllers(rt_controller_list, deactivate_request_); - // When the same interface is specified for both 'from' and 'to' during a controller reset, it + // When the same interface is specified for both 'from' and 'to' during a controller restart, 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); 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 1bd2e9e579..ca894a40b9 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -65,9 +65,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_reset); + FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers_restart); FRIEND_TEST( - TestControllerChainingWithControllerManager, test_chained_controllers_reset_error_handling); + TestControllerChainingWithControllerManager, test_chained_controllers_restart_error_handling); FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers_ex); FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers_ex2); }; @@ -104,9 +104,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_reset); + FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers_restart); FRIEND_TEST( - TestControllerChainingWithControllerManager, test_chained_controllers_reset_error_handling); + TestControllerChainingWithControllerManager, test_chained_controllers_restart_error_handling); FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers_ex); FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers_ex2); @@ -390,7 +390,7 @@ class TestControllerChainingWithControllerManager {}, {controller_name}, test_param.strictness, expected_future_status, expected_return); } - void ResetControllers( + 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) @@ -1272,7 +1272,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add UpdateAllControllerAndCheck(reference, 3u); } -TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_reset) +TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_restart) { SetupWithActivationAllControllersAndCheck(); @@ -1280,9 +1280,9 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res UpdateAllControllerAndCheck({32.0, 128.0}, 2u); { - // Reset the most preceding controller (position tracking controller) + // Restart the most preceding controller (position tracking controller) // (internal_counter 3->5) - ResetControllers({POSITION_TRACKING_CONTROLLER}); + RestartControllers({POSITION_TRACKING_CONTROLLER}); // Following controllers should stay active EXPECT_EQ( @@ -1298,22 +1298,22 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res 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 reset + // 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 reset controller has its activate and deactivate calls incremented. + // 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 (reset) + 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 (reset) + 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()); @@ -1321,20 +1321,20 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(position_tracking_controller->is_in_chained_mode()); - // Verify update after reset most preceding controller + // Verify update after restart most preceding controller // (internal_counter 5->6) UpdateAllControllerAndCheck({1024.0, 4096.0}, 5u); } { - // Reset all controllers + // 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 reset process) - ResetControllers( + // 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 reset + // 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, @@ -1342,14 +1342,14 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res 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 (reset) - ASSERT_EQ(3u, pid_right_wheel_controller->activate_calls); // +1 (reset) - ASSERT_EQ(3u, diff_drive_controller->activate_calls); // +1 (reset) - ASSERT_EQ(3u, position_tracking_controller->activate_calls); // +1 (reset) - ASSERT_EQ(2u, pid_left_wheel_controller->deactivate_calls); // +1 (reset) - ASSERT_EQ(2u, pid_right_wheel_controller->deactivate_calls); // +1 (reset) - ASSERT_EQ(2u, diff_drive_controller->deactivate_calls); // +1 (reset) - ASSERT_EQ(2u, position_tracking_controller->deactivate_calls); // +1 (reset) + 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()); @@ -1357,14 +1357,14 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(position_tracking_controller->is_in_chained_mode()); - // Verify update again after reset all controllers + // Verify update again after restart all controllers // (internal_counter 8->9->10) UpdateAllControllerAndCheck({16.0, 64.0}, 8u); UpdateAllControllerAndCheck({512.0, 2048.0}, 9u); } { - // Reset middle preceding/following controller (diff_drive_controller) and stop the most + // 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}, @@ -1383,11 +1383,11 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res // 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 (reset) + 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 (reset) + 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 @@ -1398,11 +1398,11 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res } { - // Reset the preceding controller (diff_drive_controller) and a following controller + // Restart the preceding controller (diff_drive_controller) and a following controller // (pid_right_wheel_controller) - ResetControllers({PID_RIGHT_WHEEL, DIFF_DRIVE_CONTROLLER}); + RestartControllers({PID_RIGHT_WHEEL, DIFF_DRIVE_CONTROLLER}); - // Verify that the claimed state of the interface does not change before and after the reset + // 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, @@ -1410,14 +1410,14 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res concat_vector( POSITION_CONTROLLER_NOT_CLAIMED_INTERFACES, POSITION_CONTROLLER_CLAIMED_INTERFACES)); - // Verify that only the reset controller has its activate and deactivate calls incremented. + // 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 (reset) - ASSERT_EQ(5u, diff_drive_controller->activate_calls); // +1 (reset) + 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 (reset) - ASSERT_EQ(4u, diff_drive_controller->deactivate_calls); // +1 (reset) + 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 @@ -1428,7 +1428,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res } { - // Reset middle preceding/following controller (diff_drive_controller) and start the most + // 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}, @@ -1446,11 +1446,11 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res // 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 (reset) + 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 (reset) + 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 @@ -1463,21 +1463,21 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res TEST_P( TestControllerChainingWithControllerManager, - test_chained_controllers_reset_preceding_and_stop_following) + test_chained_controllers_restart_preceding_and_stop_following) { SetupWithActivationAllControllersAndCheck(); - // Reset the most preceding controller (position_tracking_controller) and stop the middle + // 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 reset, can not stop the + // 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 reset + // 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, @@ -1508,7 +1508,7 @@ TEST_P( {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 reset + // 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, @@ -1520,11 +1520,11 @@ TEST_P( 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 (reset) + 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 (reset) + 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()); @@ -1534,7 +1534,7 @@ TEST_P( } } -TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_reset_error_handling) +TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_restart_error_handling) { SetupWithActivationAllControllersAndCheck(); @@ -1548,11 +1548,11 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE}}}; const auto & exp = expected.at(test_param.strictness); - // Test Case: reset following controllers but preceding controllers will not be reset - // --> return error; reset will not be executed and controllers stay in the same state as they + // Test Case: 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_reset = [&]() + const auto verify_all_controllers_are_active_and_not_restart = [&]() { // All controllers should still be active ASSERT_EQ( @@ -1583,39 +1583,40 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(position_tracking_controller->is_in_chained_mode()); }; - verify_all_controllers_are_active_and_not_reset(); + verify_all_controllers_are_active_and_not_restart(); - // Attempt to reset the lowest following controllers (pid_controllers) - ResetControllers({PID_LEFT_WHEEL, PID_RIGHT_WHEEL}, exp.return_type, std::future_status::ready); - verify_all_controllers_are_active_and_not_reset(); + // 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 reset the one of the lowest following controller (pid_left_wheel_controller) - ResetControllers({PID_LEFT_WHEEL}, exp.return_type, std::future_status::ready); - verify_all_controllers_are_active_and_not_reset(); + // 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 reset another lowest following controller (pid_right_wheel_controller) - ResetControllers({PID_RIGHT_WHEEL}, exp.return_type, std::future_status::ready); - verify_all_controllers_are_active_and_not_reset(); + // 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 reset the middle following controller (diff_drive_controller) - ResetControllers({DIFF_DRIVE_CONTROLLER}, exp.return_type, std::future_status::ready); - verify_all_controllers_are_active_and_not_reset(); + // 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 reset the middle following and one of the lowest following controller - ResetControllers( + // 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_reset(); + verify_all_controllers_are_active_and_not_restart(); - // Attempt to reset the middle following and another lowest following controller - ResetControllers( + // 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_reset(); + verify_all_controllers_are_active_and_not_restart(); - // Attempt to reset the all following controllers (pid_controllers and diff_drive_controller) - ResetControllers( + // 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_reset(); + verify_all_controllers_are_active_and_not_restart(); } } diff --git a/controller_manager/test/test_reset_controller.cpp b/controller_manager/test/test_restart_controller.cpp similarity index 85% rename from controller_manager/test/test_reset_controller.cpp rename to controller_manager/test/test_restart_controller.cpp index 1f1c511680..d68207a190 100644 --- a/controller_manager/test/test_reset_controller.cpp +++ b/controller_manager/test/test_restart_controller.cpp @@ -49,8 +49,9 @@ TestRestControllerStrictness test_best_effort{ BEST_EFFORT, std::future_status::timeout, controller_interface::return_type::OK}; } // namespace -class TestResetController : public ControllerManagerFixture, - public testing::WithParamInterface +class TestRestartController +: public ControllerManagerFixture, + public testing::WithParamInterface { public: void SetUp() override @@ -111,7 +112,7 @@ class TestResetController : public ControllerManagerFixtureactivate_calls); ASSERT_EQ(0u, test_controller->deactivate_calls); - // Controller command should be reset to ACTIVATE_VALUE + // Controller command should be restart to ACTIVATE_VALUE ASSERT_EQ(SIMULATE_COMMAND_ACTIVATE_VALUE, test_controller->simulate_command); } @@ -126,7 +127,7 @@ class TestResetController : public ControllerManagerFixture test_controller; }; -TEST_P(TestResetController, starting_and_stopping_a_controller) +TEST_P(TestRestartController, starting_and_stopping_a_controller) { const auto test_param = GetParam(); @@ -164,12 +165,12 @@ TEST_P(TestResetController, starting_and_stopping_a_controller) ASSERT_EQ(1u, test_controller->activate_calls); ASSERT_EQ(1u, test_controller->deactivate_calls); - // Controller command should be reset to DEACTIVATE_VALUE + // Controller command should be restart to DEACTIVATE_VALUE ASSERT_EQ(SIMULATE_COMMAND_DEACTIVATE_VALUE, test_controller->simulate_command); } } -TEST_P(TestResetController, can_reset_active_controller) +TEST_P(TestRestartController, can_restart_active_controller) { const auto test_param = GetParam(); @@ -177,18 +178,18 @@ TEST_P(TestResetController, can_reset_active_controller) configure_and_check_test_controller(); { - // Start controller before reset + // Start controller before restart start_and_check_test_controller(test_param.strictness); const double OVERRIDE_COMMAND_VALUE = 12121212.0; - // Override command to check reset behavior + // Override command to check restart behavior test_controller->simulate_command = OVERRIDE_COMMAND_VALUE; ASSERT_EQ(OVERRIDE_COMMAND_VALUE, test_controller->simulate_command); } { - // Reset controller - reset_test_controller(test_param.strictness); + // 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()); @@ -197,23 +198,23 @@ TEST_P(TestResetController, can_reset_active_controller) ASSERT_EQ(2u, test_controller->activate_calls); ASSERT_EQ(1u, test_controller->deactivate_calls); - // Controller command should be reset to ACTIVATE_VALUE + // Controller command should be restart to ACTIVATE_VALUE ASSERT_EQ(SIMULATE_COMMAND_ACTIVATE_VALUE, test_controller->simulate_command); } } -TEST_P(TestResetController, reset_inactive_controller) +TEST_P(TestRestartController, restart_inactive_controller) { const auto test_param = GetParam(); // Configure controller configure_and_check_test_controller(); - // Reset controller without starting it - reset_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 reset inactive controller + // STRICT: can not restart inactive controller if (test_param.strictness == STRICT) { // State should not be changed @@ -224,7 +225,7 @@ TEST_P(TestResetController, reset_inactive_controller) ASSERT_EQ(0u, test_controller->activate_calls); ASSERT_EQ(0u, test_controller->deactivate_calls); } - // BEST_EFFORT: If reset is executed while inactive, only the start_controller process will be + // BEST_EFFORT: If restart is executed while inactive, only the start_controller process will be // effective, resulting in activation else if (test_param.strictness == BEST_EFFORT) { @@ -235,7 +236,7 @@ TEST_P(TestResetController, reset_inactive_controller) ASSERT_EQ(1u, test_controller->activate_calls); ASSERT_EQ(0u, test_controller->deactivate_calls); - // Controller command should be reset to ACTIVATE_VALUE + // Controller command should be restart to ACTIVATE_VALUE ASSERT_EQ(SIMULATE_COMMAND_ACTIVATE_VALUE, test_controller->simulate_command); } else @@ -244,21 +245,21 @@ TEST_P(TestResetController, reset_inactive_controller) } } -TEST_P(TestResetController, can_not_reset_unconfigured_controller) +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 reset unconfigured controller - reset_test_controller( + // 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(TestResetController, can_not_reset_finalized_controller) +TEST_P(TestRestartController, can_not_restart_finalized_controller) { const auto test_param = GetParam(); @@ -270,11 +271,11 @@ TEST_P(TestResetController, can_not_reset_finalized_controller) test_controller->get_node()->shutdown().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); - // Can not reset finalized controller - reset_test_controller( + // 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, TestResetController, testing::Values(test_strict, test_best_effort)); + test_strict_best_effort, TestRestartController, testing::Values(test_strict, test_best_effort)); From df79a64747c665433c1e08b710a24cf980f55b66 Mon Sep 17 00:00:00 2001 From: Takashi Sato Date: Thu, 13 Jun 2024 12:51:25 +0900 Subject: [PATCH 08/16] refactor and cleanup code --- .../controller_manager/controller_manager.hpp | 50 +- .../controller_manager/controller_spec.hpp | 5 + controller_manager/src/controller_manager.cpp | 926 +++++------------- ...llers_chaining_with_controller_manager.cpp | 260 ++--- .../test/test_restart_controller.cpp | 23 +- 5 files changed, 410 insertions(+), 854 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index ee2e00efdc..174db79092 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -354,9 +354,7 @@ class ControllerManager : public rclcpp::Node /** * @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). Simultaneously, if the (de)activate request necessitates switching the - * chained mode of the following controller, make the corresponding 'from'/'to' chained mode - * request. + * request (BEST_EFFORT). * * 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. @@ -365,25 +363,47 @@ class ControllerManager : public rclcpp::Node * controllers will be automatically added to the activate request list if they are not in the * deactivate request. * \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. Additionally, if a following controller requiring a restart due to chained mode is - * found, it will be automatically added to the list. + * 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. Additionally, if a following controller requiring a restart due to chained mode is - * found, it will be automatically added to the list. + * the list. + * + * \returns return_type::OK if all (de)activate requests pass the checks, otherwise + * return_type::ERROR. + */ + 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; + + /** + * @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 input argument controllers must be sorted in the order of preceding and following. + * + * \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 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. - * - * \returns return_type::OK if all (de)activate requests pass the checks, otherwise - * return_type::ERROR. */ - controller_interface::return_type - check_de_activate_request_conflict_and_create_chained_mode_request( - const int strictness, const std::vector & controllers, + 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; @@ -393,8 +413,8 @@ class ControllerManager : public rclcpp::Node * 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..715dd4c51d 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -46,5 +46,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 ac6a32f83f..c700132415 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -14,7 +14,6 @@ #include "controller_manager/controller_manager.hpp" -#include #include #include #include @@ -86,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" @@ -230,66 +240,10 @@ ActivationState get_activation_state( return state; } -std::vector> -list_following_controllers( - const controller_manager::ControllerSpec & controller, const bool is_active, - const bool is_inactive, const std::vector & controllers) -{ - // If the controller is neither active nor inactive (in which case it is likely either - // unconfigured or finalized), it is impossible to retrieve the command_interface configuration, - // so return an empty vector - if (!is_active && !is_inactive) - { - return {}; - } - - std::vector> following_ctrl_refs; - for (const auto & cmd_itf_name : controller.c->command_interface_configuration().names) - { - // controller that 'cmd_tf_name' belongs to - controller_manager::ControllersListIterator following_ctrl_it; - if (!command_interface_is_reference_interface_of_controller( - cmd_itf_name, controllers, following_ctrl_it)) - { - // interfaces that are not related to the chain do not need to be checked - continue; - } - - // check if the controller is already in the list - if ( - std::find_if( - following_ctrl_refs.begin(), following_ctrl_refs.end(), - [&](const auto & ctrl) { return ctrl.get().info.name == following_ctrl_it->info.name; }) == - following_ctrl_refs.end()) - { - following_ctrl_refs.push_back(std::cref(*following_ctrl_it)); - } - } - - return following_ctrl_refs; -} - -std::optional find_controller_by_name( - const std::vector & controllers, const std::string & name) -{ - auto it = std::find_if( - controllers.begin(), controllers.end(), - std::bind(controller_name_compare, std::placeholders::_1, name)); - if (it == controllers.end()) - { - return std::nullopt; - } - return it; -} - -using ControllersMap = std::unordered_map; -using ControllerChainSpecsMap = - std::unordered_map; - -ControllersMap create_controllers_map( +controller_manager::ControllersMap create_controllers_map( const std::vector & controllers) { - ControllersMap controllers_map; + controller_manager::ControllersMap controllers_map; for (const auto & controller : controllers) { controllers_map.insert({controller.info.name, controller}); @@ -297,117 +251,10 @@ ControllersMap create_controllers_map( return controllers_map; } -#if 0 -using ControllerRef = std::reference_wrapper; -using ControllerRefs = std::vector; - -class ChainableController -{ -public: - ChainableController(const controller_manager::ControllerSpec & spec_) : spec(spec_) {} - ChainableController() = delete; - - const controller_manager::ControllerSpec & spec; - std::vector> following_controllers; - std::vector> preceding_controllers; -}; - -using ControllerChainMap = std::unordered_map>; - -ControllerChainMap create_controller_chain_map( - const std::vector & controllers) -{ - ControllerChainMap chainable_controllers_map; - for (const auto & controller : controllers) - { - const bool is_active = is_controller_active(controller.c); - const bool is_inactive = is_controller_inactive(controller.c); - - // If the controller is neither active nor inactive (in which case it is likely either - // unconfigured or finalized), it is impossible to retrieve the command_interface configuration, - // so return an empty vector - if (!is_active && !is_inactive) - { - continue; - } - - for (const auto & cmd_itf_name : controller.c->command_interface_configuration().names) - { - // controller that 'cmd_tf_name' belongs to - controller_manager::ControllersListIterator following_ctrl_it; - if (!command_interface_is_reference_interface_of_controller( - cmd_itf_name, controllers, following_ctrl_it)) - { - // interfaces that are not related to the chain do not need to be checked - continue; - } - - auto preceding_it = chainable_controllers_map.find(controller.info.name); - if (preceding_it == chainable_controllers_map.end()) - { - preceding_it = - chainable_controllers_map - .emplace(controller.info.name, std::make_shared(controller)) - .first; - } - const auto & preceding = preceding_it->second; - - auto following_it = chainable_controllers_map.find(following_ctrl_it->info.name); - if (following_it == chainable_controllers_map.end()) - { - following_it = - chainable_controllers_map - .emplace(controller.info.name, std::make_shared(controller)) - .first; - } - const auto & following = following_it->second; - - if ( - std::find_if( - preceding->following_controllers.begin(), preceding->following_controllers.end(), - [&](const std::weak_ptr & ctrl) { - return ctrl.lock()->spec.info.name == following->spec.info.name; - }) == preceding->following_controllers.end()) - { - preceding->following_controllers.push_back(following); - } - - if ( - std::find_if( - following->preceding_controllers.begin(), following->preceding_controllers.end(), - [&](const std::weak_ptr & ctrl) { - return ctrl.lock()->spec.info.name == preceding->spec.info.name; - }) == following->preceding_controllers.end()) - { - following->preceding_controllers.push_back(preceding); - } - } - } - - return chainable_controllers_map; -} -#endif - -bool will_become_active(const ActivationState & state) -{ - return (state.is_inactive && state.in_activate_list) || - (state.is_active && (!state.in_deactivate_list || state.in_activate_list)); -} - -bool will_become_inactive(const ActivationState & state) -{ - return !state.in_activate_list && - (state.is_inactive || (state.is_active && state.in_deactivate_list)); -} - -bool will_be_restarted(const ActivationState & state) -{ - return state.is_active && state.in_deactivate_list && state.in_activate_list; -} - -bool can_activate( +bool can_controller_activate( const rclcpp::Logger & logger, const std::string & controller_name, - const ControllersMap & controllers_map, const ControllerChainSpecsMap & chain_specs_map, + const controller_manager::ControllersMap & controllers_map, + const controller_manager::ControllerChainSpecsMap & chain_specs_map, const std::vector & deactivate_request, const std::vector & activate_request) { @@ -420,7 +267,15 @@ bool can_activate( const auto following_state = get_activation_state(following_controller, deactivate_request, activate_request); - if (will_become_inactive(following_state)) + 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, @@ -430,7 +285,19 @@ bool can_activate( return false; } - if (!can_activate( + // 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)) { @@ -440,9 +307,10 @@ bool can_activate( return true; } -bool can_deactivate( +bool can_controller_deactivate( const rclcpp::Logger & logger, const std::string & controller_name, - const ControllersMap & controllers_map, const ControllerChainSpecsMap & chain_specs_map, + const controller_manager::ControllersMap & controllers_map, + const controller_manager::ControllerChainSpecsMap & chain_specs_map, const std::vector & deactivate_request, const std::vector & activate_request) { @@ -455,7 +323,15 @@ bool can_deactivate( const auto preceding_state = get_activation_state(preceding_controller, deactivate_request, activate_request); - if (will_become_active(preceding_state)) + 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, @@ -465,7 +341,8 @@ bool can_deactivate( return false; } - if (!can_deactivate( + // recursively check the next preceding controller + if (!can_controller_deactivate( logger, preceding_controller_name, controllers_map, chain_specs_map, deactivate_request, activate_request)) { @@ -475,9 +352,10 @@ bool can_deactivate( return true; } -bool can_restart( +bool can_controller_restart( const rclcpp::Logger & logger, const std::string & controller_name, - const ControllersMap & controllers_map, const ControllerChainSpecsMap & chain_specs_map, + const controller_manager::ControllersMap & controllers_map, + const controller_manager::ControllerChainSpecsMap & chain_specs_map, const std::vector & deactivate_request, const std::vector & activate_request) { @@ -490,50 +368,49 @@ bool can_restart( const auto preceding_state = get_activation_state(preceding_controller, deactivate_request, activate_request); - if ( - !will_be_restarted(preceding_state) && - (preceding_state.is_active && !preceding_state.in_deactivate_list)) + 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 " - "be not restarted and become active after switch", + "not be restarted or stay active during switch", controller_name.c_str(), preceding_controller_name.c_str()); return false; } - if (!can_restart( + // 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_de_activate_request_conflict_for_controller( +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) { - const auto handle_conflict = [&logger, strictness](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; - }; - 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; @@ -542,12 +419,13 @@ controller_interface::return_type check_de_activate_request_conflict_for_control ctrl_state.in_deactivate_list = false; } - // check for doubled activation + // 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")) + "' since it is already active")) { return controller_interface::return_type::ERROR; } @@ -559,8 +437,9 @@ controller_interface::return_type check_de_activate_request_conflict_for_control 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")) + "' since it is not in inactive state")) { return controller_interface::return_type::ERROR; } @@ -571,166 +450,6 @@ controller_interface::return_type check_de_activate_request_conflict_for_control return controller_interface::return_type::OK; } -enum class CheckConflictResult -{ - OK, - CheckNextFollowing, - CheckNextPreceding, - Error, -}; - -CheckConflictResult check_conflict_between_preceding_and_following( - const rclcpp::Logger & logger, const int strictness, - const controller_manager::ControllerSpec & preceding_ctrl, - const controller_manager::ControllerSpec & following_ctrl, - const ActivationState & preceding_state, const ActivationState & following_state, - std::vector & deactivate_request, std::vector & activate_request) -{ - const auto handle_conflict = [&logger, strictness](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; - }; - - RCLCPP_ERROR( - logger, "Checking conflict between '%s' and '%s'", preceding_ctrl.info.name.c_str(), - following_ctrl.info.name.c_str()); - - // if neither the preceding nor the following is included in any request, no check is - // required, so proceed to the next following - if ( - !preceding_state.in_activate_list && !preceding_state.in_deactivate_list && - !following_state.in_activate_list && !following_state.in_deactivate_list) - { - RCLCPP_DEBUG( - logger, - " - No need to check preceding '%s' and following '%s' because there are no requests", - preceding_ctrl.info.name.c_str(), following_ctrl.info.name.c_str()); - return CheckConflictResult::CheckNextFollowing; - } - - // if the preceding will be activated, check if the following is chainable - if (preceding_state.in_activate_list && !following_ctrl.c->is_chainable()) - { - if (!handle_conflict( - "The preceding controller with name '" + preceding_ctrl.info.name + - "' cannot be activated because the following controller with name '" + - following_ctrl.info.name + "' is not chainable.")) - { - return CheckConflictResult::Error; - } - - // if BEST_EFFORT, remove the preceding controller from the activate list and proceed to the - // next preceding - remove_element_from_list(activate_request, preceding_ctrl.info.name); - return CheckConflictResult::CheckNextPreceding; - } - - // check for conflict when preceding will be activated from inactive - if (preceding_state.is_inactive && preceding_state.in_activate_list) - { - // In the following cases, preceding cannot be activated, resulting in an error: - // - the following is inactive and will not be activated - // - the following is active and will be deactivated and will not be restarted - std::optional found_conflict = std::nullopt; - if (following_state.is_inactive && !following_state.in_activate_list) - { - found_conflict = handle_conflict( - "The controller with name '" + preceding_ctrl.info.name + - "' cannot be activated because the following controller with name '" + - following_ctrl.info.name + "' is inactive and will not be activated."); - } - if ( - following_state.is_active && following_state.in_deactivate_list && - !following_state.in_activate_list) - { - found_conflict = handle_conflict( - "The controller with name '" + preceding_ctrl.info.name + - "' cannot be activated because the following controller with name '" + - following_ctrl.info.name + - "' is active and will be deactivated but will not be activated."); - } - - if (found_conflict.has_value()) - { - if (!found_conflict.value()) - { - return CheckConflictResult::Error; - } - - // if BEST_EFFORT, remove the preceding controller from the activate list and proceed to the - // next preceding - remove_element_from_list(activate_request, preceding_ctrl.info.name); - return CheckConflictResult::CheckNextPreceding; - } - } - - // check for conflict when preceding is active and will not be transitioned - if ( - preceding_state.is_active && !preceding_state.in_deactivate_list && - !preceding_state.in_activate_list) - { - // In the following cases, following cannot be deactivated (and thus cannot be restarted), - // resulting in an error: - // - the following is active and will be deactivated - // - the following is active and will be restarted - // In the end, an error occurs if following wll be deactivated, regardless of whether - // following will be restarted or not. - // (Note: Since preceding is active, following is always active, so there is no need to check if - // following is active.) - if (following_state.in_deactivate_list) - { - if (!handle_conflict( - "The following controller with name '" + following_ctrl.info.name + - "' cannot be deactivated because the preceding controller with name '" + - preceding_ctrl.info.name + "' is active and will not be deactivated and restarted.")) - { - return CheckConflictResult::Error; - } - - // if BEST_EFFORT, remove the following controller from the activate and deactivate - // list and proceed to the next following - remove_element_from_list(activate_request, following_ctrl.info.name); - remove_element_from_list(deactivate_request, following_ctrl.info.name); - return CheckConflictResult::CheckNextFollowing; - } - } - - // check for conflict when preceding will be restarted from active - if ( - preceding_state.is_active && preceding_state.in_deactivate_list && - preceding_state.in_activate_list) - { - // In the following cases, following cannot be deactivated, resulting in an error: - // - the following is active and will be deactivated but will not be restarted - if ( - following_state.is_active && following_state.in_deactivate_list && - !following_state.in_activate_list) - { - if (!handle_conflict( - "The following controller with name '" + following_ctrl.info.name + - "' cannot be deactivated because the preceding controller with name '" + - preceding_ctrl.info.name + "' is active and will be restarted.")) - { - return CheckConflictResult::Error; - } - - // if BEST_EFFORT, remove the following controller from the deactivate list and proceed to - // the next following - remove_element_from_list(deactivate_request, following_ctrl.info.name); - return CheckConflictResult::CheckNextFollowing; - } - } - - // No conflict occurs in other conditions, so return OK - return CheckConflictResult::OK; -} - void update_chained_mode_request_of_following( const rclcpp::Logger & logger, const controller_manager::ControllerSpec & preceding_ctrl, const controller_manager::ControllerSpec & following_ctrl, @@ -1473,79 +1192,45 @@ controller_interface::return_type ControllerManager::switch_controller( const std::vector & deactivate_controllers, int strictness, bool activate_asap, const rclcpp::Duration & timeout) { - const auto show_list = [this](const std::string & tag) - { - RCLCPP_WARN(get_logger(), "[%s]----------------", tag.c_str()); - RCLCPP_WARN(get_logger(), "Activating controllers:"); - for (const auto & controller : activate_request_) - { - RCLCPP_WARN(get_logger(), " - %s", controller.c_str()); - } - RCLCPP_WARN(get_logger(), "Deactivating controllers:"); - for (const auto & controller : deactivate_request_) - { - RCLCPP_WARN(get_logger(), " - %s", controller.c_str()); - } - RCLCPP_WARN(get_logger(), "to chained mode:"); - for (const auto & req : to_chained_mode_request_) - { - RCLCPP_WARN(get_logger(), " - %s", req.c_str()); - } - RCLCPP_WARN(get_logger(), "from chained mode:"); - for (const auto & req : from_chained_mode_request_) - { - RCLCPP_WARN(get_logger(), " - %s", req.c_str()); - } - RCLCPP_WARN(get_logger(), "----------------"); - }; - - RCLCPP_WARN(get_logger(), "STRICTNESS: %d", strictness); - switch_params_ = SwitchParams(); - const auto check_request_and_internal_state_before_switch = [this, &strictness]() + // check (de)activate request and internal state before switch + if (!deactivate_request_.empty() || !activate_request_.empty()) { - if (!deactivate_request_.empty() || !activate_request_.empty()) - { - RCLCPP_FATAL( - get_logger(), - "The internal deactivate and activate request lists are not empty at the beginning of the " - "switch_controller() call. This should never happen."); - throw std::runtime_error("CM's internal state is not correct. See the FATAL message above."); - } - if ( - !deactivate_command_interface_request_.empty() || - !activate_command_interface_request_.empty()) - { - RCLCPP_FATAL( - get_logger(), - "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."); - } - if (!from_chained_mode_request_.empty() || !to_chained_mode_request_.empty()) - { - RCLCPP_FATAL( - get_logger(), - "The internal 'from' and 'to' chained mode requests 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."); - } - if (strictness == 0) - { - RCLCPP_WARN( - get_logger(), - "Controller Manager: to switch controllers you need to specify a " - "strictness level of controller_manager_msgs::SwitchController::STRICT " - "(%d) or ::BEST_EFFORT (%d). Defaulting to ::BEST_EFFORT", - controller_manager_msgs::srv::SwitchController::Request::STRICT, - controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT); - strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; - } - }; - - check_request_and_internal_state_before_switch(); + RCLCPP_FATAL( + get_logger(), + "The internal deactivate and activate request lists are not empty at the beginning of the " + "switch_controller() call. This should never happen."); + throw std::runtime_error("CM's internal state is not correct. See the FATAL message above."); + } + if ( + !deactivate_command_interface_request_.empty() || !activate_command_interface_request_.empty()) + { + RCLCPP_FATAL( + get_logger(), + "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."); + } + if (!from_chained_mode_request_.empty() || !to_chained_mode_request_.empty()) + { + RCLCPP_FATAL( + get_logger(), + "The internal 'from' and 'to' chained mode requests 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."); + } + if (strictness == 0) + { + RCLCPP_WARN( + get_logger(), + "Controller Manager: to switch controllers you need to specify a " + "strictness level of controller_manager_msgs::SwitchController::STRICT " + "(%d) or ::BEST_EFFORT (%d). Defaulting to ::BEST_EFFORT", + controller_manager_msgs::srv::SwitchController::Request::STRICT, + controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT); + strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; + } RCLCPP_DEBUG(get_logger(), "Activating controllers:"); for (const auto & controller : activate_controllers) @@ -1617,22 +1302,21 @@ controller_interface::return_type ControllerManager::switch_controller( return ret; } - show_list("REQUEST"); - // lock controllers std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); const std::vector & controllers = rt_controllers_wrapper_.get_updated_list(guard); + // create a map of controllers for easy access for the following processing + const auto controllers_map = create_controllers_map(controllers); + // 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). - // Simultaneously, if the (de)activate request necessitates switching the chained mode of the - // following controller, make the corresponding 'from'/'to' chained mode request. + // either raising an error (STRICT) or removing the conflicting controller from the (de)activate + // request (BEST_EFFORT). if ( - check_de_activate_request_conflict_and_create_chained_mode_request( - strictness, controllers, deactivate_request_, activate_request_, from_chained_mode_request_, - to_chained_mode_request_) != controller_interface::return_type::OK) + check_controllers_for_de_activate( + strictness, controllers, controllers_map, deactivate_request_, activate_request_) != + controller_interface::return_type::OK) { RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)"); // reset all lists @@ -1640,12 +1324,32 @@ controller_interface::return_type ControllerManager::switch_controller( return controller_interface::return_type::ERROR; } - show_list("AFTER CHECK"); + // if the (de)activate request necessitates switching the chained mode of the + // following controller, make 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_); + + // In manage_switch, deactivation needs to be performed in the order of preceding, following, and + // activation needs to be performed in the reverse order of following, preceding. Therefore, the + // (de)activate requests are sorted in advance here. + sort_list_by_another(deactivate_request_, ordered_controllers_names_, false); + sort_list_by_another(activate_request_, ordered_controllers_names_, true); // check if we need to request switch controllers after all the checks if (activate_request_.empty() && deactivate_request_.empty()) { RCLCPP_INFO(get_logger(), "Empty activate and deactivate list, not requesting switch"); + + if (!from_chained_mode_request_.empty() || !to_chained_mode_request_.empty()) + { + 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!)"); + } + clear_requests(); return controller_interface::return_type::OK; } @@ -1693,15 +1397,8 @@ controller_interface::return_type ControllerManager::switch_controller( // wait until switch is finished RCLCPP_DEBUG(get_logger(), "Requested atomic controller switch from realtime loop"); - int timeout_count = 0; while (rclcpp::ok() && switch_params_.do_switch) { - if (++timeout_count > 3000) - { - RCLCPP_ERROR(get_logger(), "Timeout while waiting for atomic controller switch to finish"); - clear_requests(); - return controller_interface::return_type::ERROR; - } if (!rclcpp::ok()) { return controller_interface::return_type::ERROR; @@ -1846,25 +1543,19 @@ void ControllerManager::deactivate_controllers( { try { - RCLCPP_WARN( - get_logger(), "Deactivating controller '%s' with state '%s'", controller_name.c_str(), - controller->get_node()->get_current_state().label().c_str()); - const auto new_state = controller->get_node()->deactivate(); - - RCLCPP_WARN(get_logger(), "Release interfaces of controller '%s'", controller_name.c_str()); controller->release_interfaces(); - // if it is a chainable controller and will not be activated asap, make the reference - // interfaces unavailable on deactivation - const bool will_be_activated = - std::find(activate_request_.begin(), activate_request_.end(), controller_name) != - activate_request_.end(); - if (controller->is_chainable() && !will_be_activated) + // If it is a chainable controller, make the reference interfaces unavailable on + // deactivation. + // However, if it will be activated asap for a 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) { - RCLCPP_WARN( - get_logger(), "Release reference interfaces of controller '%s'", - controller_name.c_str()); resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); } if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) @@ -1956,10 +1647,6 @@ void ControllerManager::activate_controllers( continue; } - RCLCPP_WARN( - get_logger(), "Activating controller '%s' with state '%s'", controller_name.c_str(), - found_it->c->get_node()->get_current_state().label().c_str()); - auto controller = found_it->c; // reset the next update cycle time for newly activated controllers *found_it->next_update_cycle_time = @@ -1995,9 +1682,6 @@ void ControllerManager::activate_controllers( } try { - RCLCPP_WARN( - get_logger(), "Claiming command interface '%s' for controller '%s'", - command_interface.c_str(), controller_name.c_str()); command_loans.emplace_back(resource_manager_->claim_command_interface(command_interface)); } catch (const std::exception & e) @@ -2492,8 +2176,7 @@ void ControllerManager::set_hardware_component_state_srv_cb( { rclcpp_lifecycle::State target_state( request->target_state.id, - // the ternary operator is needed because label in State constructor cannot be an empty - // string + // the ternary operator is needed because label in State constructor cannot be an empty string request->target_state.label.empty() ? "-" : request->target_state.label); response->ok = (resource_manager_->set_component_state(request->name, target_state) == @@ -2560,9 +2243,13 @@ void ControllerManager::manage_switch() deactivate_controllers(rt_controller_list, deactivate_request_); - // When the same interface is specified for both 'from' and 'to' during a controller restart, it + // 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. + + // When the same controller is specified for both 'from' and 'to' during a restart, it is + // necessary to switch in the sequence of 'from' then 'to' to temporarily disable and then + // re-enable the chained mode. switch_chained_mode(from_chained_mode_request_, false); switch_chained_mode(to_chained_mode_request_, true); @@ -2887,12 +2574,10 @@ void ControllerManager::cache_controller_interfaces_in_activate_request( } }; -controller_interface::return_type -ControllerManager::check_de_activate_request_conflict_and_create_chained_mode_request( +controller_interface::return_type ControllerManager::check_controllers_for_de_activate( const int strictness, const std::vector & controllers, - std::vector & deactivate_request, std::vector & activate_request, - std::vector & from_chained_mode_request, - std::vector & to_chained_mode_request) const + const controller_manager::ControllersMap & controllers_map, + std::vector & deactivate_request, std::vector & activate_request) const { // check if there are controllers to (de)activate if (activate_request.empty() && deactivate_request.empty()) @@ -2901,35 +2586,11 @@ ControllerManager::check_de_activate_request_conflict_and_create_chained_mode_re return controller_interface::return_type::OK; } -#if 1 - const auto controllers_map = create_controllers_map(controllers); - - // RCLCPP_DEBUG(get_logger(), "============== CHAIN SPEC ==================="); - // for (const auto & [controller_name, chain] : controller_chain_spec_) - // { - // RCLCPP_DEBUG(get_logger(), "-'%s'", controller_name.c_str()); - // RCLCPP_DEBUG(get_logger(), " [preceding]"); - // for (const auto & preceding : chain.preceding_controllers) - // { - // RCLCPP_DEBUG(get_logger(), "\t- '%s'", preceding.c_str()); - // } - // RCLCPP_DEBUG(get_logger(), " [following]"); - // for (const auto & following : chain.following_controllers) - // { - // RCLCPP_DEBUG(get_logger(), "\t- '%s'", following.c_str()); - // } - // } - // RCLCPP_DEBUG(get_logger(), "============================================"); - // return controller_interface::return_type::ERROR; - - // 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. + // check for conflicts of (de)activate requests for controllers for (const auto & controller : controllers) { - // check for conflict of (de)activate request for the controller if ( - check_de_activate_request_conflict_for_controller( + check_controller_for_de_activate( get_logger(), strictness, controller, deactivate_request, activate_request) != controller_interface::return_type::OK) { @@ -2937,7 +2598,14 @@ ControllerManager::check_de_activate_request_conflict_and_create_chained_mode_re } } -#if 1 + // 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) + // 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. @@ -2946,259 +2614,123 @@ ControllerManager::check_de_activate_request_conflict_and_create_chained_mode_re const auto controller_state = get_activation_state(controller, deactivate_request, activate_request); - if (controller_state.in_deactivate_list && controller_state.in_activate_list) + if (controller_state.in_deactivate_list) { - if (!can_restart( - get_logger(), controller.info.name, controllers_map, controller_chain_spec_, - deactivate_request, activate_request)) + // 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) { - if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + if (can_controller_restart( + get_logger(), controller.info.name, controllers_map, controller_chain_spec_, + deactivate_request, activate_request)) { - RCLCPP_ERROR( - get_logger(), "Cannot restart controller '%s' due to conflict", - controller.info.name.c_str()); - return controller_interface::return_type::ERROR; + // if no conflict is found in the restart request, further checks are unnecessary, so + // proceed to the next controller. + continue; } - else + + // 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")) { - RCLCPP_WARN( - get_logger(), "Cannot restart controller '%s' due to conflict", - controller.info.name.c_str()); - remove_element_from_list(activate_request, controller.info.name); + return controller_interface::return_type::ERROR; } + + remove_element_from_list(activate_request, controller.info.name); } - else - { - continue; - } - } - if (controller_state.in_deactivate_list) - { - if (!can_deactivate( + // 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)) { - if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + if (!handle_conflict( + strictness, get_logger(), + "The deactivation request for controller '" + controller.info.name + + "' was rejected due to conflict")) { - RCLCPP_ERROR( - get_logger(), "Cannot deactivate controller '%s' due to conflict", - controller.info.name.c_str()); return controller_interface::return_type::ERROR; } - else - { - RCLCPP_WARN( - get_logger(), "Cannot deactivate controller '%s' due to conflict", - controller.info.name.c_str()); - remove_element_from_list(deactivate_request, controller.info.name); - } + + remove_element_from_list(deactivate_request, controller.info.name); } } - if (controller_state.in_activate_list) + else if (controller_state.in_activate_list) { - if (!can_activate( + // 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)) { - if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + if (!handle_conflict( + strictness, get_logger(), + "The activation request for controller '" + controller.info.name + + "' was rejected due to conflict")) { - RCLCPP_ERROR( - get_logger(), "Cannot activate controller '%s' due to conflict", - controller.info.name.c_str()); return controller_interface::return_type::ERROR; } - else - { - RCLCPP_WARN( - get_logger(), "Cannot activate controller '%s' due to conflict", - controller.info.name.c_str()); - remove_element_from_list(activate_request, controller.info.name); - } - } - } - } -#else - sort_list_by_another(deactivate_request, ordered_controllers_names_, false); - sort_list_by_another(activate_request, ordered_controllers_names_, true); - - for (auto ctrl_it = deactivate_request.cbegin(); ctrl_it != deactivate_request.cend();) - { - if (can_deactivate( - get_logger(), *ctrl_it, controllers_map, controller_chain_spec_, deactivate_request, - activate_request)) - { - ++ctrl_it; - continue; - } - - if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) - { - RCLCPP_ERROR( - get_logger(), "Cannot deactivate controller '%s' due to conflict", ctrl_it->c_str()); - return controller_interface::return_type::ERROR; + remove_element_from_list(activate_request, controller.info.name); + } } else { - RCLCPP_WARN( - get_logger(), "Cannot deactivate controller '%s' due to conflict", ctrl_it->c_str()); - ctrl_it = deactivate_request.erase(ctrl_it); + // this controller does not have any (de)activate request --> do nothing } } - for (auto ctrl_it = activate_request.crbegin(); ctrl_it != activate_request.crend();) - { - if (can_activate( - get_logger(), *ctrl_it, controllers_map, controller_chain_spec_, deactivate_request, - activate_request)) - { - ++ctrl_it; - continue; - } - - if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) - { - RCLCPP_ERROR( - get_logger(), "Cannot activate controller '%s' due to conflict", ctrl_it->c_str()); - return controller_interface::return_type::ERROR; - } - else - { - RCLCPP_WARN( - get_logger(), "Cannot activate controller '%s' due to conflict", ctrl_it->c_str()); - ctrl_it = std::vector::reverse_iterator( - activate_request.erase(std::next(ctrl_it).base())); - } - } -#endif + return controller_interface::return_type::OK; +} - // Check for conflict in the order of "preceding" to "following" +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 controller_state = - get_activation_state(controller, deactivate_request, activate_request); - - // get following controllers references for the controller - const auto following_ctrl_refs = list_following_controllers( - controller, controller_state.is_active, controller_state.is_inactive, 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 (following_ctrl_refs.empty()) + if (chain_spec.following_controllers.empty()) { continue; } // this controller is a preceding controller const auto & preceding_ctrl = controller; - const auto & preceding_state = controller_state; + 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_ctrl_ref : following_ctrl_refs) + for (const auto & following_controller_name : chain_spec.following_controllers) { - const auto & following_ctrl = following_ctrl_ref.get(); + const auto & following_ctrl = controllers_map.at(following_controller_name); const auto following_state = get_activation_state(following_ctrl, deactivate_request, activate_request); - // If no conflicts are found in the (de)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. + // 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(), preceding_ctrl, following_ctrl, preceding_state, following_state, deactivate_request, activate_request, from_chained_mode_request, to_chained_mode_request, *resource_manager_); } } - -#else - // 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) - { - // check for conflict of (de)activate request for the controller - if ( - check_de_activate_request_conflict_for_controller( - get_logger(), strictness, controller, deactivate_request, activate_request) != - controller_interface::return_type::OK) - { - return controller_interface::return_type::ERROR; - } - - const auto controller_state = - get_activation_state(controller, deactivate_request, activate_request); - - // get following controllers references for the controller - const auto following_ctrl_refs = - list_following_controllers(controller, controller_state, controllers); - - // if there are no following controllers, this controller is not a preceding controller, so skip - // the following checks - if (following_ctrl_refs.empty()) - { - continue; - } - - // this controller is a preceding controller - const auto & preceding_ctrl = controller; - const auto & preceding_state = controller_state; - - // check for conflict of the preceding and each following controller - for (const auto & following_ctrl_ref : following_ctrl_refs) - { - const auto & following_ctrl = following_ctrl_ref.get(); - - const auto following_state = - get_activation_state(following_ctrl, deactivate_request, activate_request); - - const auto check_result = check_conflict_between_preceding_and_following( - get_logger(), strictness, preceding_ctrl, following_ctrl, preceding_state, following_state, - deactivate_request, activate_request); - - // based on the check result, select the next action - if (check_result == CheckConflictResult::OK) - { - // If no conflicts are found in the (de)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(), preceding_ctrl, following_ctrl, preceding_state, following_state, - deactivate_request, activate_request, from_chained_mode_request, to_chained_mode_request, - *resource_manager_); - } - else if (check_result == CheckConflictResult::CheckNextFollowing) - { - // proceed to check the next following controller - continue; - } - else if (check_result == CheckConflictResult::CheckNextPreceding) - { - // proceed to check the next preceding controller - break; - } - else - { - return controller_interface::return_type::ERROR; - } - } - } -#endif - - // To ensure that the appropriate (de)activation are performed, sort deactivate requests in - // the order of preceding to following, and sort activate requests in the reverse order of - // following to preceding - sort_list_by_another(deactivate_request, ordered_controllers_names_, false); - sort_list_by_another(activate_request, ordered_controllers_names_, true); - - return controller_interface::return_type::OK; } void ControllerManager::controller_activity_diagnostic_callback( 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 ca894a40b9..f7b504811a 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -68,8 +68,6 @@ class TestableTestChainableController : public test_chainable_controller::TestCh FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers_restart); FRIEND_TEST( TestControllerChainingWithControllerManager, test_chained_controllers_restart_error_handling); - FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers_ex); - FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers_ex2); }; class TestableControllerManager : public controller_manager::ControllerManager @@ -107,8 +105,6 @@ class TestableControllerManager : public controller_manager::ControllerManager FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers_restart); FRIEND_TEST( TestControllerChainingWithControllerManager, test_chained_controllers_restart_error_handling); - FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers_ex); - FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers_ex2); public: TestableControllerManager( @@ -539,14 +535,14 @@ class TestControllerChainingWithControllerManager // 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) - 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); + 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 @@ -877,6 +873,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, @@ -905,13 +926,97 @@ 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()); + + // Test Case 3: Trying to activate a preceding controllers and one of the following controller + // --> return error; preceding controllers are not activated, + // BUT following controller IS activated + static std::unordered_map expected_case3 = { + {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}}}; + + // Deactivate following controllers before next test + switch_test_controllers( + {}, {PID_LEFT_WHEEL, PID_RIGHT_WHEEL}, test_param.strictness, std::future_status::timeout, + controller_interface::return_type::OK); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + pid_left_wheel_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + pid_right_wheel_controller->get_state().id()); + + // 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_case3.at(test_param.strictness).future_status, + expected_case3.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_case3.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_case3.at(test_param.strictness).return_type, + expected_case3.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_case3.at(test_param.strictness).future_status, + expected_case3.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_case3.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 4: 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(); @@ -1046,6 +1151,7 @@ TEST_P( // Test Case 6: following controller is deactivated but preceding controller will be activated // --> return error; controllers stay in the same state + switch_test_controllers( {DIFF_DRIVE_CONTROLLER}, {PID_LEFT_WHEEL, PID_RIGHT_WHEEL}, test_param.strictness, expected.at(test_param.strictness).future_status, @@ -1285,16 +1391,16 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res RestartControllers({POSITION_TRACKING_CONTROLLER}); // Following controllers should stay active - EXPECT_EQ( + ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); - EXPECT_EQ( + ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); - EXPECT_EQ( + ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); // Preceding controller should return to active (active -> inactive -> active) - EXPECT_EQ( + ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, position_tracking_controller->get_state().id()); @@ -1319,7 +1425,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res 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()); - ASSERT_FALSE(position_tracking_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) @@ -1355,7 +1461,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res 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()); - ASSERT_FALSE(position_tracking_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) @@ -1394,7 +1500,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res 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()); - ASSERT_FALSE(position_tracking_controller->is_in_chained_mode()); + EXPECT_FALSE(position_tracking_controller->is_in_chained_mode()); } { @@ -1424,7 +1530,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res 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()); - ASSERT_FALSE(position_tracking_controller->is_in_chained_mode()); + EXPECT_FALSE(position_tracking_controller->is_in_chained_mode()); } { @@ -1457,7 +1563,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res 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()); - ASSERT_FALSE(position_tracking_controller->is_in_chained_mode()); + EXPECT_FALSE(position_tracking_controller->is_in_chained_mode()); } } @@ -1498,7 +1604,7 @@ TEST_P( 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()); - ASSERT_FALSE(position_tracking_controller->is_in_chained_mode()); + EXPECT_FALSE(position_tracking_controller->is_in_chained_mode()); } else if (test_param.strictness == BEST_EFFORT) { @@ -1530,7 +1636,7 @@ TEST_P( 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()); - ASSERT_FALSE(position_tracking_controller->is_in_chained_mode()); + EXPECT_FALSE(position_tracking_controller->is_in_chained_mode()); } } @@ -1548,7 +1654,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE}}}; const auto & exp = expected.at(test_param.strictness); - // Test Case: restart following controllers but preceding controllers will not be restart + // Test Case 8: 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 { @@ -1581,7 +1687,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res 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()); - ASSERT_FALSE(position_tracking_controller->is_in_chained_mode()); + EXPECT_FALSE(position_tracking_controller->is_in_chained_mode()); }; verify_all_controllers_are_active_and_not_restart(); @@ -1620,106 +1726,6 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res } } -TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_ex) -{ - 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); - - // 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_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}}}; - const auto & exp = expected.at(test_param.strictness); - - switch_test_controllers( - {POSITION_TRACKING_CONTROLLER, DIFF_DRIVE_CONTROLLER}, {}, test_param.strictness, - std::future_status::ready, exp.return_type); - - ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - 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()); - - switch_test_controllers( - {POSITION_TRACKING_CONTROLLER, DIFF_DRIVE_CONTROLLER, PID_LEFT_WHEEL}, {}, - test_param.strictness, exp.future_status, exp.return_type); - - 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()); - - 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()); - ASSERT_FALSE(position_tracking_controller->is_in_chained_mode()); - - DeactivateController(PID_LEFT_WHEEL, exp.return_type, exp.future_status); - ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - pid_left_wheel_controller->get_state().id()); - - switch_test_controllers( - {POSITION_TRACKING_CONTROLLER, DIFF_DRIVE_CONTROLLER, PID_RIGHT_WHEEL}, {}, - test_param.strictness, exp.future_status, exp.return_type); - - 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()); - ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - position_tracking_controller->get_state().id()); - - 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()); - ASSERT_FALSE(position_tracking_controller->is_in_chained_mode()); -} - INSTANTIATE_TEST_SUITE_P( test_strict_best_effort, TestControllerChainingWithControllerManager, - testing::Values(strict, best_effort), [](const testing::TestParamInfo & info_) - { return info_.param.strictness == STRICT ? "STRICT" : "BEST_EFFORT"; }); + testing::Values(strict, best_effort)); diff --git a/controller_manager/test/test_restart_controller.cpp b/controller_manager/test/test_restart_controller.cpp index d68207a190..529d7ad33a 100644 --- a/controller_manager/test/test_restart_controller.cpp +++ b/controller_manager/test/test_restart_controller.cpp @@ -16,7 +16,6 @@ #include #include #include -#include #include #include "controller_interface/controller_interface.hpp" @@ -29,10 +28,7 @@ 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; -// using ::testing::_; -// using ::testing::Return; -const auto CONTROLLER_NAME_1 = "test_controller1"; -const auto CONTROLLER_NAME_2 = "test_controller2"; +const auto CONTROLLER_NAME = "test_controller1"; using strvec = std::vector; namespace @@ -64,7 +60,7 @@ class TestRestartController { // load controller test_controller = std::make_shared(); - cm_->add_controller(test_controller, CONTROLLER_NAME_1, TEST_CONTROLLER_CLASS_NAME); + 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()); @@ -75,7 +71,7 @@ class TestRestartController void configure_and_check_test_controller() { // configure controller - cm_->configure_controller(CONTROLLER_NAME_1); + cm_->configure_controller(CONTROLLER_NAME); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); @@ -92,7 +88,7 @@ class TestRestartController controller_interface::return_type::OK) { switch_test_controllers( - strvec{CONTROLLER_NAME_1}, strvec{}, strictness, expected_future_status, + strvec{CONTROLLER_NAME}, strvec{}, strictness, expected_future_status, expected_interface_status); } @@ -123,7 +119,7 @@ class TestRestartController controller_interface::return_type::OK) { switch_test_controllers( - strvec{}, strvec{CONTROLLER_NAME_1}, strictness, expected_future_status, + strvec{}, strvec{CONTROLLER_NAME}, strictness, expected_future_status, expected_interface_status); } @@ -134,7 +130,7 @@ class TestRestartController controller_interface::return_type::OK) { switch_test_controllers( - strvec{CONTROLLER_NAME_1}, strvec{CONTROLLER_NAME_1}, strictness, expected_future_status, + strvec{CONTROLLER_NAME}, strvec{CONTROLLER_NAME}, strictness, expected_future_status, expected_interface_status); } @@ -225,9 +221,10 @@ TEST_P(TestRestartController, restart_inactive_controller) 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 - else if (test_param.strictness == BEST_EFFORT) + if (test_param.strictness == BEST_EFFORT) { // State changed to active ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); @@ -239,10 +236,6 @@ TEST_P(TestRestartController, restart_inactive_controller) // Controller command should be restart to ACTIVATE_VALUE ASSERT_EQ(SIMULATE_COMMAND_ACTIVATE_VALUE, test_controller->simulate_command); } - else - { - FAIL(); - } } TEST_P(TestRestartController, can_not_restart_unconfigured_controller) From 7c370a0ac3d176ce4fd2f38d83ba6b91a1b255f3 Mon Sep 17 00:00:00 2001 From: Takashi Sato Date: Thu, 13 Jun 2024 17:30:14 +0900 Subject: [PATCH 09/16] fix the points indicated by pre-commit run --- .../include/controller_manager/controller_spec.hpp | 1 + .../test_controller_with_command.hpp | 5 ++--- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 715dd4c51d..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" 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 index c5a02fa6ea..d1992b8340 100644 --- 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 @@ -15,8 +15,7 @@ #ifndef TEST_CONTROLLER_WITH_COMMAND__TEST_CONTROLLER_WITH_COMMAND_HPP_ #define TEST_CONTROLLER_WITH_COMMAND__TEST_CONTROLLER_WITH_COMMAND_HPP_ -#include -#include +#include #include "controller_interface/visibility_control.h" #include "controller_manager/controller_manager.hpp" @@ -83,4 +82,4 @@ class TestControllerWithCommand : public controller_interface::ControllerInterfa } // namespace test_controller_with_command -#endif // TEST_CONTROLLER_WITH_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_HPP_ +#endif // TEST_CONTROLLER_WITH_COMMAND__TEST_CONTROLLER_WITH_COMMAND_HPP_ From 704990e927fb274c0a4dbd03ec1217d5e2677100 Mon Sep 17 00:00:00 2001 From: Takashi Sato Date: Fri, 14 Jun 2024 17:01:51 +0900 Subject: [PATCH 10/16] remove unused input arg --- controller_manager/src/controller_manager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index c700132415..d94327bdae 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -451,7 +451,7 @@ controller_interface::return_type check_controller_for_de_activate( } void update_chained_mode_request_of_following( - const rclcpp::Logger & logger, const controller_manager::ControllerSpec & preceding_ctrl, + 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, @@ -2726,7 +2726,7 @@ void ControllerManager::create_from_to_chained_mode_request( // 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(), preceding_ctrl, following_ctrl, preceding_state, following_state, + get_logger(), following_ctrl, preceding_state, following_state, deactivate_request, activate_request, from_chained_mode_request, to_chained_mode_request, *resource_manager_); } From 0a956c1d26c8a9df38f8c96b83bb481f4b3f3eae Mon Sep 17 00:00:00 2001 From: Takashi Sato Date: Fri, 14 Jun 2024 17:07:42 +0900 Subject: [PATCH 11/16] add dummy lambda definition to maintain ABI compatibility --- controller_manager/src/controller_manager.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index d94327bdae..6097774dfe 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -451,8 +451,7 @@ controller_interface::return_type check_controller_for_de_activate( } void update_chained_mode_request_of_following( - const rclcpp::Logger & logger, - const controller_manager::ControllerSpec & following_ctrl, + 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, @@ -1192,6 +1191,10 @@ 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 @@ -2726,9 +2729,8 @@ void ControllerManager::create_from_to_chained_mode_request( // 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_); + get_logger(), following_ctrl, preceding_state, following_state, deactivate_request, + activate_request, from_chained_mode_request, to_chained_mode_request, *resource_manager_); } } } From 3721ad64f3d79afd1c18eacb11d2fa8465bbb76e Mon Sep 17 00:00:00 2001 From: Takashi Sato Date: Fri, 14 Jun 2024 21:08:29 +0900 Subject: [PATCH 12/16] modify comment --- controller_manager/src/controller_manager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 6097774dfe..0e0cd12dbf 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1328,7 +1328,7 @@ controller_interface::return_type ControllerManager::switch_controller( } // if the (de)activate request necessitates switching the chained mode of the - // following controller, make the corresponding 'from'/'to' chained mode + // following controller, create the corresponding 'from'/'to' chained mode // request. create_from_to_chained_mode_request( controllers, controllers_map, deactivate_request_, activate_request_, @@ -2520,7 +2520,7 @@ void ControllerManager::extract_de_activate_command_interface_request( extract_interfaces_for_controller(controller, deactivate_command_interface_request); } } -}; +} void ControllerManager::cache_controller_interfaces_in_activate_request( const std::vector & controllers, From 5cce35d3dfe73daf4e3f87e7babf9691561dd9c9 Mon Sep 17 00:00:00 2001 From: Takashi Sato Date: Sun, 23 Jun 2024 08:58:09 +0900 Subject: [PATCH 13/16] enhance chainig test --- ...llers_chaining_with_controller_manager.cpp | 198 ++++++++++-------- 1 file changed, 115 insertions(+), 83 deletions(-) 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 f7b504811a..1ed1aab30b 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -60,6 +60,9 @@ class TestableTestChainableController : public test_chainable_controller::TestCh FRIEND_TEST( TestControllerChainingWithControllerManager, test_chained_controllers_activation_switching_error_handling); + FRIEND_TEST( + TestControllerChainingWithControllerManager, + test_chained_controllers_activation_switching_error_handling2); FRIEND_TEST( TestControllerChainingWithControllerManager, test_chained_controllers_deactivation_error_handling); @@ -97,6 +100,9 @@ class TestableControllerManager : public controller_manager::ControllerManager FRIEND_TEST( TestControllerChainingWithControllerManager, test_chained_controllers_activation_switching_error_handling); + FRIEND_TEST( + TestControllerChainingWithControllerManager, + test_chained_controllers_activation_switching_error_handling2); FRIEND_TEST( TestControllerChainingWithControllerManager, test_chained_controllers_deactivation_error_handling); @@ -927,10 +933,51 @@ TEST_P( ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); - // Test Case 3: Trying to activate a preceding controllers and one of the following controller - // --> return error; preceding controllers are not activated, + // 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_case3 = { + 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}}, @@ -938,29 +985,81 @@ TEST_P( {controller_interface::return_type::OK, std::future_status::timeout, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE}}}; - // Deactivate following controllers before next test + // Attempt to activate preceding controller (diff-drive controller) and + // one of the following controller (pid_left_wheel_controller) switch_test_controllers( - {}, {PID_LEFT_WHEEL, PID_RIGHT_WHEEL}, test_param.strictness, std::future_status::timeout, - controller_interface::return_type::OK); + {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_right_wheel_controller->get_state().id()); + 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_case3.at(test_param.strictness).future_status, - expected_case3.at(test_param.strictness).return_type); + 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_case3.at(test_param.strictness).state, pid_left_wheel_controller->get_state().id()); + 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()); @@ -978,8 +1077,8 @@ TEST_P( // Deactivate the following controller (pid_left_wheel_controller) before next test DeactivateController( - PID_LEFT_WHEEL, expected_case3.at(test_param.strictness).return_type, - expected_case3.at(test_param.strictness).future_status); + 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()); @@ -988,8 +1087,8 @@ TEST_P( // another following controller (pid_right_wheel_controller) switch_test_controllers( {POSITION_TRACKING_CONTROLLER, DIFF_DRIVE_CONTROLLER, PID_RIGHT_WHEEL}, {}, - test_param.strictness, expected_case3.at(test_param.strictness).future_status, - expected_case3.at(test_param.strictness).return_type); + 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) @@ -997,8 +1096,7 @@ TEST_P( EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, pid_left_wheel_controller->get_state().id()); - ASSERT_EQ( - expected_case3.at(test_param.strictness).state, pid_right_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( @@ -1012,72 +1110,6 @@ TEST_P( EXPECT_FALSE(position_tracking_controller->is_in_chained_mode()); } -TEST_P( - TestControllerChainingWithControllerManager, - test_chained_controllers_activation_switching_error_handling) -{ - // Test Case 4: 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(); - - // 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); - - // Activate the following controller and the preceding controllers - 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); - - // Verify that the other preceding controller is deactivated (diff_drive_controller_two) - EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - diff_drive_controller_two->get_state().id()); - - // Deactivate the first preceding controller (diff_drive_controller) and - // activate the other preceding controller (diff_drive_controller_two) - switch_test_controllers( - {DIFF_DRIVE_CONTROLLER_TWO}, {DIFF_DRIVE_CONTROLLER}, test_param.strictness, - std::future_status::timeout, controller_interface::return_type::OK); - - // Following controllers should stay active - EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); - EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); - // The original preceding controller (diff_drive_controller) should be inactive while - // the other preceding controller should be active (diff_drive_controller_two) - ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); - ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller_two->get_state().id()); -} - TEST_P( TestControllerChainingWithControllerManager, test_chained_controllers_deactivation_error_handling) { From 70d417e832d9a81c7512eb8d3f23e64a4d8c8e5e Mon Sep 17 00:00:00 2001 From: Takashi Sato Date: Wed, 26 Jun 2024 12:26:33 +0900 Subject: [PATCH 14/16] restored the test case that had been accidentally deleted --- ...llers_chaining_with_controller_manager.cpp | 74 ++++++++++++++++++- 1 file changed, 70 insertions(+), 4 deletions(-) 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 1ed1aab30b..5c58e77165 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -59,10 +59,10 @@ class TestableTestChainableController : public test_chainable_controller::TestCh test_chained_controllers_activation_error_handling); FRIEND_TEST( TestControllerChainingWithControllerManager, - test_chained_controllers_activation_switching_error_handling); + test_chained_controllers_activation_error_handling2); FRIEND_TEST( TestControllerChainingWithControllerManager, - test_chained_controllers_activation_switching_error_handling2); + test_chained_controllers_activation_switching_error_handling); FRIEND_TEST( TestControllerChainingWithControllerManager, test_chained_controllers_deactivation_error_handling); @@ -99,10 +99,10 @@ class TestableControllerManager : public controller_manager::ControllerManager test_chained_controllers_activation_error_handling); FRIEND_TEST( TestControllerChainingWithControllerManager, - test_chained_controllers_activation_switching_error_handling); + test_chained_controllers_activation_error_handling2); FRIEND_TEST( TestControllerChainingWithControllerManager, - test_chained_controllers_activation_switching_error_handling2); + test_chained_controllers_activation_switching_error_handling); FRIEND_TEST( TestControllerChainingWithControllerManager, test_chained_controllers_deactivation_error_handling); @@ -1110,6 +1110,72 @@ TEST_P( 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. + // Example: Need two diff drive controllers, one should be deactivated, + // and the other should be activated. Following controller should stay in activated state. + 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); + + // Activate the following controller and the preceding controllers + 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); + + // Verify that the other preceding controller is deactivated (diff_drive_controller_two) + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller_two->get_state().id()); + + // Deactivate the first preceding controller (diff_drive_controller) and + // activate the other preceding controller (diff_drive_controller_two) + switch_test_controllers( + {DIFF_DRIVE_CONTROLLER_TWO}, {DIFF_DRIVE_CONTROLLER}, test_param.strictness, + std::future_status::timeout, controller_interface::return_type::OK); + + // Following controllers should stay active + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); + // The original preceding controller (diff_drive_controller) should be inactive while + // the other preceding controller should be active (diff_drive_controller_two) + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller_two->get_state().id()); +} + TEST_P( TestControllerChainingWithControllerManager, test_chained_controllers_deactivation_error_handling) { From b9dab554181ef0e143591cd2cb3420dee8c20464 Mon Sep 17 00:00:00 2001 From: Takashi Sato Date: Wed, 26 Jun 2024 12:32:11 +0900 Subject: [PATCH 15/16] fix test case numbers --- ...st_controllers_chaining_with_controller_manager.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) 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 5c58e77165..b4638d0163 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -1114,7 +1114,7 @@ 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(); @@ -1217,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 @@ -1247,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( @@ -1264,7 +1264,7 @@ TEST_P( ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); - // Test Case 7: following controller deactivation but preceding controller is active + // Test Case 8: following controller deactivation but preceding controller is active // --> return error; controllers stay in the same state as they were const auto verify_all_controllers_are_still_be_active = [&]() @@ -1752,7 +1752,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_res lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE}}}; const auto & exp = expected.at(test_param.strictness); - // Test Case 8: restart following controllers but preceding controllers will not be restart + // 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 { From 1865ab794cbc29a05b34a934a4d1891e71ddc9f5 Mon Sep 17 00:00:00 2001 From: Takashi Sato Date: Wed, 26 Jun 2024 14:40:00 +0900 Subject: [PATCH 16/16] removed unnecessary sorting of (de)activate request --- controller_manager/src/controller_manager.cpp | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 0e0cd12dbf..a4e306c5f3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1334,12 +1334,6 @@ controller_interface::return_type ControllerManager::switch_controller( controllers, controllers_map, deactivate_request_, activate_request_, from_chained_mode_request_, to_chained_mode_request_); - // In manage_switch, deactivation needs to be performed in the order of preceding, following, and - // activation needs to be performed in the reverse order of following, preceding. Therefore, the - // (de)activate requests are sorted in advance here. - sort_list_by_another(deactivate_request_, ordered_controllers_names_, false); - sort_list_by_another(activate_request_, ordered_controllers_names_, true); - // check if we need to request switch controllers after all the checks if (activate_request_.empty() && deactivate_request_.empty()) { @@ -1551,7 +1545,7 @@ void ControllerManager::deactivate_controllers( // If it is a chainable controller, make the reference interfaces unavailable on // deactivation. - // However, if it will be activated asap for a restart due to subsequent processes in + // 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 && @@ -2249,10 +2243,6 @@ void ControllerManager::manage_switch() // 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. - - // When the same controller is specified for both 'from' and 'to' during a restart, it is - // necessary to switch in the sequence of 'from' then 'to' to temporarily disable and then - // re-enable the chained mode. switch_chained_mode(from_chained_mode_request_, false); switch_chained_mode(to_chained_mode_request_, true);