diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 4b7e8d9958..ea5c220be3 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -280,10 +280,11 @@ class ControllerManager : public rclcpp::Node * * \param[in] rt_controller_list controllers in the real-time list. * \param[in] controllers_to_activate names of the controller that have to be activated. + * \param[in] strictness level of strictness for activation. */ void activate_controllers( const std::vector & rt_controller_list, - const std::vector & controllers_to_activate); + const std::vector & controllers_to_activate, int strictness); void list_controllers_srv_cb( const std::shared_ptr request, diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 5bf003cd3f..60db1989db 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1307,6 +1307,10 @@ controller_interface::return_type ControllerManager::configure_controller( resource_manager_->import_controller_reference_interfaces(controller_name, ref_interfaces); resource_manager_->import_controller_exported_state_interfaces( controller_name, state_interfaces); + // make all the exported interfaces of the controller unavailable + controller->set_chained_mode(false); + resource_manager_->make_controller_exported_state_interfaces_unavailable(controller_name); + resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); } // let's update the list of following and preceding controllers @@ -2226,9 +2230,10 @@ void ControllerManager::switch_chained_mode( void ControllerManager::activate_controllers( const std::vector & rt_controller_list, - const std::vector & controllers_to_activate) + const std::vector & controllers_to_activate, int strictness) { std::vector failed_controllers_command_interfaces; + bool is_successful = true; for (const auto & controller_name : controllers_to_activate) { auto found_it = std::find_if( @@ -2295,6 +2300,7 @@ void ControllerManager::activate_controllers( // something went wrong during command interfaces, go skip the controller if (!assignment_successful) { + is_successful = false; continue; } @@ -2333,6 +2339,7 @@ void ControllerManager::activate_controllers( // something went wrong during state interfaces, go skip the controller if (!assignment_successful) { + is_successful = false; continue; } controller->assign_interfaces(std::move(command_loans), std::move(state_loans)); @@ -2365,10 +2372,15 @@ void ControllerManager::activate_controllers( controller->get_node()->get_name(), new_state.label().c_str(), new_state.id(), hardware_interface::lifecycle_state_names::ACTIVE, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + is_successful = false; controller->release_interfaces(); - failed_controllers_command_interfaces.insert( - failed_controllers_command_interfaces.end(), command_interface_names.begin(), - command_interface_names.end()); + ros2_control::add_items(failed_controllers_command_interfaces, command_interface_names); + if (controller->is_chainable()) + { + // make all the exported interfaces of the controller unavailable + resource_manager_->make_controller_exported_state_interfaces_unavailable(controller_name); + resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); + } continue; } @@ -2380,6 +2392,27 @@ void ControllerManager::activate_controllers( resource_manager_->make_controller_reference_interfaces_available(controller_name); } } + if ( + !is_successful && strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + { + RCLCPP_ERROR( + get_logger(), + "At least one controller failed to be activated. Releasing all interfaces and stopping all " + "activated controllers because the switch strictness is set to STRICT."); + // deactivate all controllers that were activated in this switch + deactivate_controllers(rt_controller_list, controllers_to_activate); + if ( + !resource_manager_->prepare_command_mode_switch( + {}, switch_params_.activate_command_interface_request) || + !resource_manager_->perform_command_mode_switch( + {}, switch_params_.activate_command_interface_request)) + { + RCLCPP_ERROR( + get_logger(), + "Error switching back the interfaces in the hardware when the controller activation " + "failed."); + } + } // Now prepare and perform the stop interface switching as this is needed for exclusive // interfaces if ( @@ -2916,7 +2949,8 @@ void ControllerManager::manage_switch() // activate controllers once the switch is fully complete const auto act_start_time = std::chrono::steady_clock::now(); - activate_controllers(rt_controller_list, switch_params_.activate_request); + activate_controllers( + rt_controller_list, switch_params_.activate_request, switch_params_.strictness); execution_time_.activation_time = std::chrono::duration(std::chrono::steady_clock::now() - act_start_time) .count(); @@ -3116,7 +3150,9 @@ controller_interface::return_type ControllerManager::update( deactivate_controllers(rt_controller_list, rt_buffer_.deactivate_controllers_list); if (!rt_buffer_.fallback_controllers_list.empty()) { - activate_controllers(rt_controller_list, rt_buffer_.fallback_controllers_list); + activate_controllers( + rt_controller_list, rt_buffer_.fallback_controllers_list, + controller_manager_msgs::srv::SwitchController::Request::STRICT); } // To publish the activity of the failing controllers and the fallback controllers publish_activity(); diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 64b73b15e9..cdf53d7b00 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -2280,3 +2280,180 @@ TEST_F( test_chainable_controller->get_lifecycle_state().id()); } } + +TEST_F( + TestControllerManagerChainableControllerFailedActivation, + test_chainable_controllers_failed_activation_stops_all_list) +{ + const std::string test_chainable_controller_2_name = "test_chainable_controller_2"; + + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + controller_interface::InterfaceConfiguration itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + // controller 1 + auto test_chainable_controller = + std::make_shared(); + cmd_itfs_cfg.names = {"joint1/position"}; + itfs_cfg.names = {"joint2/velocity"}; + test_chainable_controller->set_command_interface_configuration(cmd_itfs_cfg); + test_chainable_controller->set_state_interface_configuration(itfs_cfg); + test_chainable_controller->set_reference_interface_names({"modified_joint1/position"}); + test_chainable_controller->set_exported_state_interface_names({"modified_joint2/velocity"}); + test_chainable_controller->fail_on_activate = false; + + auto test_chainable_controller_2 = + std::make_shared(); + cmd_itfs_cfg.names = {"joint2/velocity"}; + itfs_cfg.names = {"joint2/velocity"}; + test_chainable_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + test_chainable_controller_2->set_state_interface_configuration(itfs_cfg); + test_chainable_controller_2->set_reference_interface_names({"modified_joint2/position"}); + test_chainable_controller_2->set_exported_state_interface_names({"modified_joint2/velocity"}); + test_chainable_controller_2->fail_on_activate = false; + + auto test_controller = std::make_shared(); + cmd_itfs_cfg.names = { + std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/modified_joint1/position"}; + test_controller->set_command_interface_configuration(cmd_itfs_cfg); + test_controller->set_state_interface_configuration(itfs_cfg); + + cm_->add_controller( + test_chainable_controller, test_chainable_controller::TEST_CONTROLLER_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_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + + EXPECT_EQ(3u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_chainable_controller.use_count()); + EXPECT_EQ(2, test_chainable_controller_2.use_count()); + EXPECT_EQ(2, test_controller.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->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_controller->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller::TEST_CONTROLLER_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_controller::TEST_CONTROLLER_NAME)); + } + + std::vector start_controllers = { + test_chainable_controller::TEST_CONTROLLER_NAME, test_chainable_controller_2_name, + test_controller::TEST_CONTROLLER_NAME}; + std::vector stop_controllers = {}; + { + ControllerManagerRunner cm_runner(this); + + 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::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chainable_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chainable_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller->get_lifecycle_state().id()); + + // Now deactivate all the controllers + auto switch_future_2 = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + stop_controllers, start_controllers, strictness, true, rclcpp::Duration(0, 0)); + ASSERT_EQ(std::future_status::ready, switch_future_2.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ(controller_interface::return_type::OK, switch_future_2.get()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chainable_controller->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_controller->get_lifecycle_state().id()); + } + + // Now, let's make the first controller fail on activation and see that all the controllers are + // set to inactive in strict mode + test_chainable_controller->fail_on_activate = true; + { + ControllerManagerRunner cm_runner(this); + + 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::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_chainable_controller->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_controller->get_lifecycle_state().id()); + } + + // Now, reconfigure and test with BEST_EFFORT strictness and see that the other two controllers + // are active + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller::TEST_CONTROLLER_NAME)); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chainable_controller->get_lifecycle_state().id()); + + const auto best_effort_strictness = + controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, best_effort_strictness, true, rclcpp::Duration(0, 0)); + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_chainable_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chainable_controller_2->get_lifecycle_state().id()); + // test controller needs to be inactive as it depends on the failed controller's interface + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); + } +} diff --git a/controller_manager/test/test_release_interfaces.cpp b/controller_manager/test/test_release_interfaces.cpp index c931502c7b..87646167d4 100644 --- a/controller_manager/test/test_release_interfaces.cpp +++ b/controller_manager/test/test_release_interfaces.cpp @@ -176,7 +176,7 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) abstract_test_controller2.c->get_lifecycle_state().id()); } - { // Test starting both controllers at the same time + { // Test starting both controllers at the same time, with STRICT both should fail RCLCPP_INFO( cm_->get_logger(), "Starting both controllers at the same time (should notify about resource conflict)"); @@ -189,6 +189,27 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) << "switch_controller should be blocking until next update cycle"; ControllerManagerRunner cm_runner(this); EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller1.c->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller2.c->get_lifecycle_state().id()); + } + + { // Test starting both controllers at the same time, with BEST_EFFORT one should activate + RCLCPP_INFO( + cm_->get_logger(), + "Starting both controllers at the same time (should notify about resource conflict)"); + std::vector start_controllers = {controller_name1, controller_name2}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, BEST_EFFORT, 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"; + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, abstract_test_controller1.c->get_lifecycle_state().id()); diff --git a/hardware_interface/include/hardware_interface/helpers.hpp b/hardware_interface/include/hardware_interface/helpers.hpp index 080c833845..85d51fefa4 100644 --- a/hardware_interface/include/hardware_interface/helpers.hpp +++ b/hardware_interface/include/hardware_interface/helpers.hpp @@ -85,6 +85,20 @@ void add_item(std::vector & vector, const T & item) } } +/** + * @brief Add the items from one container to the other, if they are not already in it. + * @param vector The container to add the items to. + * @param items The container to add the items from. + */ +template +void add_items(std::vector & vector, const std::vector & items) +{ + for (const auto & item : items) + { + add_item(vector, item); + } +} + /** * @brief Remove the item from the container if it is in it. * @param container The container to remove the item from.