diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index e96e767948..b5566b72a7 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -409,13 +409,14 @@ Hardware and Controller Errors ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ If the hardware during it's ``read`` or ``write`` method returns ``return_type::ERROR``, the controller manager will stop all controllers that are using the hardware's command and state interfaces. -Likewise, if a controller returns ``return_type::ERROR`` from its ``update`` method, the controller manager will deactivate the respective controller. In future, the controller manager will try to start any fallback controllers if available. +Likewise, if a controller returns ``return_type::ERROR`` from its ``update`` method, the controller manager will deactivate the respective controller (or) the entire controller chain it is part of, then the controller manager will try to start any available fallback controllers. Factors that affect Determinism ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ When run under the conditions determined in the above section, the determinism is assured up to the limitations of the hardware and the real-time kernel. However, there are some situations that can affect determinism: -* When a controller fails to activate, the controller_manager will call the methods ``prepare_command_mode_switch`` and ``perform_command_mode_switch`` to stop the started interfaces. These calls can cause jitter in the main control loop. +* When a controller fails to activate in the realtime loop, the controller_manager will call the methods ``prepare_command_mode_switch`` and ``perform_command_mode_switch`` to stop the started interfaces. These calls can cause jitter in the main control loop. +* If a controller does not complete a successful update cycle in the realtime loop (for example, returns ``return_type::ERROR``), the controller manager will deactivate that controller (or) the entire controller chain it is part of. It will then invoke ``prepare_command_mode_switch`` and ``perform_command_mode_switch`` to stop the interfaces used by the affected controller(s). These actions can introduce jitter into the main control loop. Support for Asynchronous Updates ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index ea5c220be3..686c25933f 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -488,6 +488,14 @@ class ControllerManager : public rclcpp::Node const std::string & ctrl_name, std::vector::iterator controller_iterator, bool append_to_controller); + /** + * @brief Build the controller chain topology information based on the provided controllers. + * This method constructs a directed graph representing the dependencies between controllers. + * It analyzes the relationships between controllers, such as which controllers depend on others, + * and builds a directed graph to represent these dependencies. + */ + void build_controllers_topology_info(const std::vector & controllers); + /** * @brief Method to publish the state of the controller manager. * The state includes the list of controllers and the list of hardware interfaces along with diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index cb7a31cfe9..cc3db440ee 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -48,6 +48,7 @@ struct ControllerSpec hardware_interface::ControllerInfo info; controller_interface::ControllerInterfaceBaseSharedPtr c; std::shared_ptr last_update_cycle_time; + std::vector controllers_chain_group = {}; std::shared_ptr execution_time_statistics; std::shared_ptr periodicity_statistics; }; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 60db1989db..f10cb9c893 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -357,6 +357,52 @@ void get_controller_list_command_interfaces( } } +void get_full_chain_spec( + const std::string & controller_name, + const std::unordered_map & chain_spec, + std::vector & full_chain_list) +{ + auto it = chain_spec.find(controller_name); + if (it == chain_spec.end()) + { + RCLCPP_WARN( + rclcpp::get_logger("ControllerManager::utils"), + "Controller '%s' not found in the controller chain specification.", controller_name.c_str()); + return; + } + ros2_control::add_item(full_chain_list, controller_name); + for (const auto & following_controller : it->second.following_controllers) + { + if (!ros2_control::has_item(full_chain_list, following_controller)) + { + get_full_chain_spec(following_controller, chain_spec, full_chain_list); + } + } + for (const auto & preceding_controller : it->second.preceding_controllers) + { + if (!ros2_control::has_item(full_chain_list, preceding_controller)) + { + get_full_chain_spec(preceding_controller, chain_spec, full_chain_list); + } + } +} + +void build_controller_full_chain_map_cache( + const std::unordered_map & + controller_chain_spec, + std::unordered_map> & controller_full_chain_info_cache) +{ + controller_full_chain_info_cache.clear(); + for (const auto & [controller_name, chain_spec] : controller_chain_spec) + { + controller_full_chain_info_cache[controller_name] = {}; + // add the controller itself to the list + ros2_control::add_item(controller_full_chain_info_cache[controller_name], controller_name); + get_full_chain_spec( + controller_name, controller_chain_spec, controller_full_chain_info_cache[controller_name]); + } +} + void register_controller_manager_statistics( const std::string & name, const libstatistics_collector::moving_average_statistics::StatisticData * variable) @@ -1346,33 +1392,10 @@ controller_interface::return_type ControllerManager::configure_controller( return controller_interface::return_type::ERROR; } - for (const auto & cmd_itf : cmd_itfs) - { - controller_manager::ControllersListIterator ctrl_it; - if (is_interface_a_chained_interface(cmd_itf, controllers, ctrl_it)) - { - ros2_control::add_item( - controller_chain_spec_[controller_name].following_controllers, ctrl_it->info.name); - ros2_control::add_item( - controller_chain_spec_[ctrl_it->info.name].preceding_controllers, controller_name); - ros2_control::add_item( - controller_chained_reference_interfaces_cache_[ctrl_it->info.name], controller_name); - } - } - // This is needed when we start exporting the state interfaces from the controllers - for (const auto & state_itf : state_itfs) - { - controller_manager::ControllersListIterator ctrl_it; - if (is_interface_a_chained_interface(state_itf, controllers, ctrl_it)) - { - ros2_control::add_item( - controller_chain_spec_[controller_name].preceding_controllers, ctrl_it->info.name); - ros2_control::add_item( - controller_chain_spec_[ctrl_it->info.name].following_controllers, controller_name); - ros2_control::add_item( - controller_chained_state_interfaces_cache_[ctrl_it->info.name], controller_name); - } - } + build_controllers_topology_info(controllers); + + std::unordered_map> controller_full_chain_info_cache; + build_controller_full_chain_map_cache(controller_chain_spec_, controller_full_chain_info_cache); // Now let's reorder the controllers // lock controllers @@ -1407,6 +1430,23 @@ controller_interface::return_type ControllerManager::configure_controller( } } + // Update the controllers chain groups in the ControllerSpec + for (const auto & [ctrl_name, full_chain_info] : controller_full_chain_info_cache) + { + RCLCPP_DEBUG( + get_logger(), "%s", + fmt::format( + "The controller '{}' is in chain with: [{}]", ctrl_name, fmt::join(full_chain_info, ", ")) + .c_str()); + auto controller_it = std::find_if( + new_list.begin(), new_list.end(), + std::bind(controller_name_compare, std::placeholders::_1, ctrl_name)); + if (controller_it != new_list.end()) + { + controller_it->controllers_chain_group = full_chain_info; + } + } + to = new_list; RCLCPP_DEBUG(get_logger(), "Reordered controllers list is:"); for (const auto & ctrl : to) @@ -2154,6 +2194,7 @@ void ControllerManager::deactivate_controllers( // deactivation if (controller->is_chainable()) { + controller->set_chained_mode(false); resource_manager_->make_controller_exported_state_interfaces_unavailable(controller_name); resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); } @@ -3096,7 +3137,16 @@ controller_interface::return_type ControllerManager::update( if (controller_ret != controller_interface::return_type::OK) { - rt_buffer_.deactivate_controllers_list.push_back(loaded_controller.info.name); + const std::vector & controller_chain = + loaded_controller.controllers_chain_group; + RCLCPP_INFO_EXPRESSION( + get_logger(), controller_chain.size() > 1, + "Controller '%s' is part of a chain of %lu controllers that will be deactivated.", + loaded_controller.info.name.c_str(), controller_chain.size()); + for (const auto & chained_controller : controller_chain) + { + ros2_control::add_item(rt_buffer_.deactivate_controllers_list, chained_controller); + } ret = controller_ret; } } @@ -4398,6 +4448,81 @@ void ControllerManager::update_list_with_controller_chain( } } +void ControllerManager::build_controllers_topology_info( + const std::vector & controllers) +{ + std::for_each( + controller_chain_spec_.begin(), controller_chain_spec_.end(), + [](auto & pair) + { + pair.second.following_controllers.clear(); + pair.second.preceding_controllers.clear(); + }); + std::for_each( + controller_chained_reference_interfaces_cache_.begin(), + controller_chained_reference_interfaces_cache_.end(), [](auto & pair) { pair.second.clear(); }); + std::for_each( + controller_chained_state_interfaces_cache_.begin(), + controller_chained_state_interfaces_cache_.end(), [](auto & pair) { pair.second.clear(); }); + for (const auto & controller : controllers) + { + if (is_controller_unconfigured(*controller.c)) + { + RCLCPP_DEBUG( + get_logger(), "Controller '%s' is unconfigured, skipping chain building.", + controller.info.name.c_str()); + continue; + } + const auto cmd_itfs = controller.c->command_interface_configuration().names; + const auto state_itfs = controller.c->state_interface_configuration().names; + + for (const auto & cmd_itf : cmd_itfs) + { + controller_manager::ControllersListIterator ctrl_it; + if (is_interface_a_chained_interface(cmd_itf, controllers, ctrl_it)) + { + ros2_control::add_item( + controller_chain_spec_[controller.info.name].following_controllers, ctrl_it->info.name); + ros2_control::add_item( + controller_chain_spec_[ctrl_it->info.name].preceding_controllers, controller.info.name); + ros2_control::add_item( + controller_chained_reference_interfaces_cache_[ctrl_it->info.name], controller.info.name); + } + } + // This is needed when we start exporting the state interfaces from the controllers + for (const auto & state_itf : state_itfs) + { + controller_manager::ControllersListIterator ctrl_it; + if (is_interface_a_chained_interface(state_itf, controllers, ctrl_it)) + { + ros2_control::add_item( + controller_chain_spec_[controller.info.name].preceding_controllers, ctrl_it->info.name); + ros2_control::add_item( + controller_chain_spec_[ctrl_it->info.name].following_controllers, controller.info.name); + ros2_control::add_item( + controller_chained_state_interfaces_cache_[ctrl_it->info.name], controller.info.name); + } + } + } + for (const auto & [controller_name, controller_chain] : controller_chain_spec_) + { + RCLCPP_DEBUG( + get_logger(), "Controller '%s' has %ld following controllers and %ld preceding controllers.", + controller_name.c_str(), controller_chain.following_controllers.size(), + controller_chain.preceding_controllers.size()); + RCLCPP_DEBUG_EXPRESSION( + get_logger(), !controller_chain.following_controllers.empty(), "%s", + fmt::format( + "\tFollowing controllers are : {}", fmt::join(controller_chain.following_controllers, ", ")) + .c_str()); + RCLCPP_DEBUG_EXPRESSION( + get_logger(), !controller_chain.preceding_controllers.empty(), "%s", + fmt::format( + "\tPreceding controllers are : {}", fmt::join(controller_chain.preceding_controllers, ", ")) + .c_str()); + } +} + rclcpp::NodeOptions ControllerManager::determine_controller_node_options( const ControllerSpec & controller) const { 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 2f3143bab5..f377830418 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -129,7 +129,7 @@ controller_interface::return_type TestChainableController::update_and_write_comm state_interfaces_values_[i] = state_interfaces_[i].get_optional().value(); } - return controller_interface::return_type::OK; + return update_return_value; } CallbackReturn TestChainableController::on_init() { return CallbackReturn::SUCCESS; } 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 8733d6c878..02f49dbaeb 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp @@ -81,6 +81,7 @@ class TestChainableController : public controller_interface::ChainableController size_t internal_counter; bool fail_on_activate = false; + controller_interface::return_type update_return_value = controller_interface::return_type::OK; 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_manager.cpp b/controller_manager/test/test_controller_manager.cpp index cdf53d7b00..77072b9b1e 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -2182,6 +2182,586 @@ TEST_F( } } +class TestControllerManagerControllerChainFailedUpdateCycle +: public ControllerManagerFixture +{ +}; + +TEST_F( + TestControllerManagerControllerChainFailedUpdateCycle, + test_failing_update_cycle_in_a_controller_chain) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + controller_interface::InterfaceConfiguration state_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + // Controller names + const std::string test_chainable_controller_1_name = "test_chainable_controller_1"; + const std::string test_chainable_controller_2_name = "test_chainable_controller_2"; + const std::string test_chainable_controller_3_name = "test_chainable_controller_3"; + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + + // chain controller 1 + auto test_chainable_controller_1 = + std::make_shared(); + cmd_itfs_cfg.names = {"joint1/position"}; + state_itfs_cfg.names = {"joint2/velocity"}; + test_chainable_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + test_chainable_controller_1->set_state_interface_configuration(state_itfs_cfg); + test_chainable_controller_1->set_reference_interface_names({"modified_joint1/position"}); + test_chainable_controller_1->set_exported_state_interface_names({"modified_joint2/velocity"}); + + // chain controller 2 + auto test_chainable_controller_2 = + std::make_shared(); + cmd_itfs_cfg.names = {test_chainable_controller_1_name + "/modified_joint1/position"}; + state_itfs_cfg.names = {"joint2/velocity"}; + test_chainable_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + test_chainable_controller_2->set_state_interface_configuration(state_itfs_cfg); + test_chainable_controller_2->set_reference_interface_names({"modified_joint1/position"}); + test_chainable_controller_2->set_exported_state_interface_names({"modified_joint2/velocity"}); + + // chain controller 3 + auto test_chainable_controller_3 = + std::make_shared(); + cmd_itfs_cfg.names = {test_chainable_controller_2_name + "/modified_joint1/position"}; + state_itfs_cfg.names = {"joint2/velocity"}; + test_chainable_controller_3->set_command_interface_configuration(cmd_itfs_cfg); + test_chainable_controller_3->set_state_interface_configuration(state_itfs_cfg); + test_chainable_controller_3->set_reference_interface_names({"modified_joint1/position"}); + test_chainable_controller_3->set_exported_state_interface_names({"modified_joint2/velocity"}); + + // controller 1 + auto test_controller_1 = std::make_shared(); + cmd_itfs_cfg.names = {test_chainable_controller_3_name + "/modified_joint1/position"}; + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_1->set_state_interface_configuration(state_itfs_cfg); + + // controller 2 + auto test_controller_2 = std::make_shared(); + cmd_itfs_cfg.names = {"joint2/velocity"}; + state_itfs_cfg.names = {"joint2/velocity"}; + test_controller_2->set_state_interface_configuration(state_itfs_cfg); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + + { + cm_->add_controller( + test_chainable_controller_1, test_chainable_controller_1_name, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chainable_controller_2, test_chainable_controller_2_name, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chainable_controller_3, test_chainable_controller_3_name, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_1, test_controller_1_name, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_2, test_controller_2_name, test_controller::TEST_CONTROLLER_CLASS_NAME); + } + + EXPECT_EQ(5u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_chainable_controller_1.use_count()); + EXPECT_EQ(2, test_chainable_controller_2.use_count()); + EXPECT_EQ(2, test_chainable_controller_3.use_count()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_chainable_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_chainable_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_chainable_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller_1_name)); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller_2_name)); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller_3_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_2_name)); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chainable_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chainable_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chainable_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + std::vector start_controllers = { + test_controller_1_name, test_controller_2_name, test_chainable_controller_1_name, + test_chainable_controller_2_name, test_chainable_controller_3_name}; + auto activate_all_controllers = [&]() + { + std::vector stop_controllers = {}; + // Start controller, will take effect at the end of the update function + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + }; + activate_all_controllers(); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chainable_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chainable_controller_2->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chainable_controller_3->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); + + auto run_10_update_cycles = [&]() + { + // Let's emulate 10 update cycles and it should be working + for (int i = 0; i < 10; ++i) + { + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + } + }; + + auto check_for_inactive_chain = [&]() + { + ASSERT_EQ( + controller_interface::return_type::ERROR, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chainable_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chainable_controller_2->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chainable_controller_3->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); + }; + + start_controllers = { + test_controller_1_name, test_chainable_controller_1_name, test_chainable_controller_2_name, + test_chainable_controller_3_name}; + run_10_update_cycles(); + test_controller_1->set_external_commands_for_testing({std::numeric_limits::quiet_NaN()}); + check_for_inactive_chain(); + + test_controller_1->set_external_commands_for_testing({0.0}); + activate_all_controllers(); + run_10_update_cycles(); + test_chainable_controller_1->update_return_value = controller_interface::return_type::ERROR; + check_for_inactive_chain(); + + test_chainable_controller_1->update_return_value = controller_interface::return_type::OK; + activate_all_controllers(); + run_10_update_cycles(); + test_chainable_controller_2->update_return_value = controller_interface::return_type::ERROR; + check_for_inactive_chain(); + + test_chainable_controller_2->update_return_value = controller_interface::return_type::OK; + activate_all_controllers(); + run_10_update_cycles(); + test_chainable_controller_3->update_return_value = controller_interface::return_type::ERROR; + check_for_inactive_chain(); +} + +TEST_F( + TestControllerManagerControllerChainFailedUpdateCycle, + test_failing_update_cycle_in_a_complex_controller_chain) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + controller_interface::InterfaceConfiguration state_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + // Controller names + const std::string test_chainable_controller_1_name = "test_chainable_controller_1"; + const std::string test_chainable_controller_2_name = "test_chainable_controller_2"; + const std::string test_chainable_controller_3_name = "test_chainable_controller_3"; + const std::string test_chainable_controller_4_name = "test_chainable_controller_4"; + const std::string test_chainable_controller_5_name = "test_chainable_controller_5"; + const std::string test_chainable_controller_6_name = "test_chainable_controller_6"; + const std::string test_chainable_controller_7_name = "test_chainable_controller_7"; + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + + // chain controller 1 + auto test_chainable_controller_1 = + std::make_shared(); + cmd_itfs_cfg.names = {"joint1/position"}; + state_itfs_cfg.names = {"joint2/velocity"}; + test_chainable_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + test_chainable_controller_1->set_state_interface_configuration(state_itfs_cfg); + test_chainable_controller_1->set_reference_interface_names({"modified_joint1/position"}); + test_chainable_controller_1->set_exported_state_interface_names({"modified_joint2/velocity"}); + + // chain controller 2 + auto test_chainable_controller_2 = + std::make_shared(); + cmd_itfs_cfg.names = {"joint1/max_velocity"}; + state_itfs_cfg.names = {"joint2/velocity"}; + test_chainable_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + test_chainable_controller_2->set_state_interface_configuration(state_itfs_cfg); + test_chainable_controller_2->set_reference_interface_names({"modified_joint1/max_velocity"}); + test_chainable_controller_2->set_exported_state_interface_names({"modified_joint2/velocity"}); + + // chain controller 3 + auto test_chainable_controller_3 = + std::make_shared(); + cmd_itfs_cfg.names = {"joint2/velocity"}; + state_itfs_cfg.names = {"joint2/velocity"}; + test_chainable_controller_3->set_command_interface_configuration(cmd_itfs_cfg); + test_chainable_controller_3->set_state_interface_configuration(state_itfs_cfg); + test_chainable_controller_3->set_reference_interface_names({"modified_joint2/velocity"}); + test_chainable_controller_3->set_exported_state_interface_names({"modified_joint2/velocity"}); + + // chain controller 4 + auto test_chainable_controller_4 = + std::make_shared(); + cmd_itfs_cfg.names = {"joint3/velocity"}; + state_itfs_cfg.names = {"joint3/velocity"}; + test_chainable_controller_4->set_command_interface_configuration(cmd_itfs_cfg); + test_chainable_controller_4->set_state_interface_configuration(state_itfs_cfg); + test_chainable_controller_4->set_reference_interface_names({"modified_joint3/velocity"}); + test_chainable_controller_4->set_exported_state_interface_names({"modified_joint3/velocity"}); + + // chain controller 5 + auto test_chainable_controller_5 = + std::make_shared(); + cmd_itfs_cfg.names = { + test_chainable_controller_1_name + "/modified_joint1/position", + test_chainable_controller_2_name + "/modified_joint1/max_velocity"}; + state_itfs_cfg.names = {"joint2/velocity", "joint3/velocity"}; + test_chainable_controller_5->set_command_interface_configuration(cmd_itfs_cfg); + test_chainable_controller_5->set_state_interface_configuration(state_itfs_cfg); + test_chainable_controller_5->set_reference_interface_names({"modified_joint1/position"}); + test_chainable_controller_5->set_exported_state_interface_names({"modified_joint1/velocity"}); + + // chain controller 6 + auto test_chainable_controller_6 = + std::make_shared(); + cmd_itfs_cfg.names = { + test_chainable_controller_3_name + "/modified_joint2/velocity", + test_chainable_controller_4_name + "/modified_joint3/velocity"}; + state_itfs_cfg.names = { + "joint2/velocity", "joint3/velocity", + test_chainable_controller_7_name + "/joint2/max_acceleration"}; + test_chainable_controller_6->set_command_interface_configuration(cmd_itfs_cfg); + test_chainable_controller_6->set_state_interface_configuration(state_itfs_cfg); + test_chainable_controller_6->set_reference_interface_names({"modified_joint2/velocity"}); + test_chainable_controller_6->set_exported_state_interface_names({"modified_joint2/velocity"}); + + // chain controller 7 + auto test_chainable_controller_7 = + std::make_shared(); + cmd_itfs_cfg.names = {"joint2/max_acceleration"}; + state_itfs_cfg.names = {"joint2/velocity"}; + test_chainable_controller_7->set_command_interface_configuration(cmd_itfs_cfg); + test_chainable_controller_7->set_state_interface_configuration(state_itfs_cfg); + test_chainable_controller_7->set_reference_interface_names({"modified_joint2/max_acceleration"}); + test_chainable_controller_7->set_exported_state_interface_names({"joint2/max_acceleration"}); + + // controller 1 + auto test_controller_1 = std::make_shared(); + cmd_itfs_cfg.names = {test_chainable_controller_5_name + "/modified_joint1/position"}; + // this is to create a closed-loop instance in the control design + state_itfs_cfg.names = {test_chainable_controller_1_name + "/modified_joint2/velocity"}; + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_1->set_state_interface_configuration(state_itfs_cfg); + + // controller 2 + auto test_controller_2 = std::make_shared(); + cmd_itfs_cfg.names = {test_chainable_controller_6_name + "/modified_joint2/velocity"}; + state_itfs_cfg.names = {"joint2/velocity"}; + test_controller_2->set_state_interface_configuration(state_itfs_cfg); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + + { + cm_->add_controller( + test_chainable_controller_2, test_chainable_controller_2_name, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chainable_controller_1, test_chainable_controller_1_name, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chainable_controller_6, test_chainable_controller_6_name, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chainable_controller_3, test_chainable_controller_3_name, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_1, test_controller_1_name, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chainable_controller_4, test_chainable_controller_4_name, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_2, test_controller_2_name, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chainable_controller_5, test_chainable_controller_5_name, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chainable_controller_7, test_chainable_controller_7_name, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + } + + EXPECT_EQ(9u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_chainable_controller_1.use_count()); + EXPECT_EQ(2, test_chainable_controller_2.use_count()); + EXPECT_EQ(2, test_chainable_controller_3.use_count()); + EXPECT_EQ(2, test_chainable_controller_4.use_count()); + EXPECT_EQ(2, test_chainable_controller_5.use_count()); + EXPECT_EQ(2, test_chainable_controller_6.use_count()); + EXPECT_EQ(2, test_chainable_controller_7.use_count()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + auto check_controllers_state = [&](const std::vector state) + { + ASSERT_EQ(state[0], test_chainable_controller_1->get_lifecycle_state().id()); + ASSERT_EQ(state[1], test_chainable_controller_2->get_lifecycle_state().id()); + ASSERT_EQ(state[2], test_chainable_controller_3->get_lifecycle_state().id()); + ASSERT_EQ(state[3], test_chainable_controller_4->get_lifecycle_state().id()); + ASSERT_EQ(state[4], test_chainable_controller_5->get_lifecycle_state().id()); + ASSERT_EQ(state[5], test_chainable_controller_6->get_lifecycle_state().id()); + ASSERT_EQ(state[6], test_chainable_controller_7->get_lifecycle_state().id()); + ASSERT_EQ(state[7], test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ(state[8], test_controller_2->get_lifecycle_state().id()); + }; + check_controllers_state( + std::vector(9, lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller_6_name)); + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller_1_name)); + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller_7_name)); + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller_2_name)); + ASSERT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller_5_name)); + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller_3_name)); + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller_4_name)); + ASSERT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_2_name)); + } + + check_controllers_state(std::vector(9, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)); + + std::vector start_controllers = { + test_controller_1_name, test_controller_2_name, + test_chainable_controller_1_name, test_chainable_controller_2_name, + test_chainable_controller_3_name, test_chainable_controller_4_name, + test_chainable_controller_5_name, test_chainable_controller_6_name, + test_chainable_controller_7_name}; + auto activate_controllers = [&]() + { + std::vector stop_controllers = {}; + // Start controller, will take effect at the end of the update function + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + }; + activate_controllers(); + + check_controllers_state(std::vector(9, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)); + + auto run_5_update_cycles = [&]() + { + // Let's emulate 5 update cycles and it should be working + for (int i = 0; i < 5; ++i) + { + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + } + }; + + auto check_for_inactive_chain_1 = [&]() + { + ASSERT_EQ( + controller_interface::return_type::ERROR, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + check_controllers_state( + {lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE}); + }; + + start_controllers = { + test_controller_1_name, test_chainable_controller_1_name, test_chainable_controller_2_name, + test_chainable_controller_5_name}; + run_5_update_cycles(); + test_controller_1->set_external_commands_for_testing({std::numeric_limits::quiet_NaN()}); + check_for_inactive_chain_1(); + test_controller_1->set_external_commands_for_testing({0.0}); + + activate_controllers(); + check_controllers_state(std::vector(9, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)); + run_5_update_cycles(); + test_chainable_controller_1->update_return_value = controller_interface::return_type::ERROR; + check_for_inactive_chain_1(); + test_chainable_controller_1->update_return_value = controller_interface::return_type::OK; + + activate_controllers(); + check_controllers_state(std::vector(9, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)); + run_5_update_cycles(); + test_chainable_controller_2->update_return_value = controller_interface::return_type::ERROR; + check_for_inactive_chain_1(); + test_chainable_controller_2->update_return_value = controller_interface::return_type::OK; + + activate_controllers(); + check_controllers_state(std::vector(9, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)); + run_5_update_cycles(); + test_chainable_controller_5->update_return_value = controller_interface::return_type::ERROR; + check_for_inactive_chain_1(); + test_chainable_controller_5->update_return_value = controller_interface::return_type::OK; + + activate_controllers(); + run_5_update_cycles(); + check_controllers_state(std::vector(9, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)); + + // Now let's check the 2nd chain + auto check_for_inactive_chain_2 = [&]() + { + ASSERT_EQ( + controller_interface::return_type::ERROR, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + check_controllers_state( + {lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE}); + }; + start_controllers = { + test_controller_2_name, test_chainable_controller_3_name, test_chainable_controller_4_name, + test_chainable_controller_6_name, test_chainable_controller_7_name}; + + run_5_update_cycles(); + test_controller_2->set_external_commands_for_testing({std::numeric_limits::quiet_NaN()}); + check_for_inactive_chain_2(); + test_controller_2->set_external_commands_for_testing({0.0}); + + activate_controllers(); + check_controllers_state(std::vector(9, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)); + run_5_update_cycles(); + test_chainable_controller_3->update_return_value = controller_interface::return_type::ERROR; + check_for_inactive_chain_2(); + test_chainable_controller_3->update_return_value = controller_interface::return_type::OK; + + activate_controllers(); + check_controllers_state(std::vector(9, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)); + run_5_update_cycles(); + test_chainable_controller_4->update_return_value = controller_interface::return_type::ERROR; + check_for_inactive_chain_2(); + test_chainable_controller_4->update_return_value = controller_interface::return_type::OK; + + activate_controllers(); + check_controllers_state(std::vector(9, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)); + run_5_update_cycles(); + test_chainable_controller_6->update_return_value = controller_interface::return_type::ERROR; + check_for_inactive_chain_2(); + test_chainable_controller_6->update_return_value = controller_interface::return_type::OK; +} + class TestControllerManagerChainableControllerFailedActivation : public ControllerManagerFixture { diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 903280545f..70bc473e0a 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -8,6 +8,7 @@ This list summarizes important changes between Kilted Kaiju (previous) and Lyric controller_interface ******************** * The new ``MagneticFieldSensor`` semantic component provides an interface for reading data from magnetometers. `(#2627 `__) +* The controller_manager will now deactivate the entire controller chain if a controller in the chain fails during the update cycle. `(#2681 `__) controller_manager ******************