diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 29d489ad81..913b6072ec 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -105,6 +105,14 @@ if(BUILD_TESTING) target_link_libraries(test_controller_manager_with_namespace.cpp ${PROJECT_NAME} test_controller) ament_target_dependencies(test_controller_manager_with_namespace.cpp ros2_control_test_assets) + ament_add_gmock( + test_controller_manager_hardware_error_handling + test/test_controller_manager_hardware_error_handling.cpp + ) + target_include_directories(test_controller_manager_hardware_error_handling PRIVATE include) + target_link_libraries(test_controller_manager_hardware_error_handling ${PROJECT_NAME} test_controller) + ament_target_dependencies(test_controller_manager_hardware_error_handling ros2_control_test_assets) + ament_add_gmock( test_load_controller test/test_load_controller.cpp diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index a1ae11b4cb..ea1cc0cb13 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -133,17 +133,17 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC controller_interface::return_type configure_controller(const std::string & controller_name); - /// switch_controller Stops some controllers and start others. + /// switch_controller Deactivates some controllers and activates others. /** - * \param[in] start_controllers is a list of controllers to start - * \param[in] stop_controllers is a list of controllers to stop + * \param[in] activate_controllers is a list of controllers to activate. + * \param[in] deactivate_controllers is a list of controllers to deactivate. * \param[in] set level of strictness (BEST_EFFORT or STRICT) * \see Documentation in controller_manager_msgs/SwitchController.srv */ CONTROLLER_MANAGER_PUBLIC controller_interface::return_type switch_controller( - const std::vector & start_controllers, - const std::vector & stop_controllers, int strictness, + const std::vector & activate_controllers, + const std::vector & deactivate_controllers, int strictness, bool activate_asap = kWaitForAllResources, const rclcpp::Duration & timeout = rclcpp::Duration::from_nanoseconds(kInfiniteTimeout)); @@ -205,8 +205,18 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC void manage_switch(); + /// Deactivate chosen controllers from real-time controller list. + /** + * Deactivate controllers with names \p controllers_to_deactivate from list \p rt_controller_list. + * The controller list will be iterated as many times as there are controller names. + * + * \param[in] rt_controller_list controllers in the real-time list. + * \param[in] controllers_to_deactivate names of the controller that have to be deactivated. + */ CONTROLLER_MANAGER_PUBLIC - void deactivate_controllers(); + void deactivate_controllers( + const std::vector & rt_controller_list, + const std::vector controllers_to_deactivate); /** * Switch chained mode for all the controllers with respect to the following cases: @@ -220,11 +230,34 @@ class ControllerManager : public rclcpp::Node void switch_chained_mode( const std::vector & chained_mode_switch_list, bool to_chained_mode); + /// Activate chosen controllers from real-time controller list. + /** + * Activate controllers with names \p controllers_to_activate from list \p rt_controller_list. + * The controller list will be iterated as many times as there are controller names. + * + * \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. + */ CONTROLLER_MANAGER_PUBLIC - void activate_controllers(); + void activate_controllers( + const std::vector & rt_controller_list, + const std::vector controllers_to_activate); + /// Activate chosen controllers from real-time controller list. + /** + * Activate controllers with names \p controllers_to_activate from list \p rt_controller_list. + * The controller list will be iterated as many times as there are controller names. + * + * *NOTE*: There is currently not difference to `activate_controllers` method. + * Check https://github.com/ros-controls/ros2_control/issues/263 for more information. + * + * \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. + */ CONTROLLER_MANAGER_PUBLIC - void activate_controllers_asap(); + void activate_controllers_asap( + const std::vector & rt_controller_list, + const std::vector controllers_to_activate); CONTROLLER_MANAGER_PUBLIC void list_controllers_srv_cb( @@ -315,7 +348,7 @@ class ControllerManager : public rclcpp::Node * * For each controller the whole chain of following controllers is checked. * - * NOTE: The automatically adding of following controller into starting list is not implemented + * NOTE: The automatically adding of following controller into activate list is not implemented * yet. * * \param[in] controllers list with controllers. @@ -339,7 +372,7 @@ class ControllerManager : public rclcpp::Node * - will be deactivated, * - and will not be activated. * - * NOTE: The automatically adding of preceding controllers into stopping list is not implemented + * NOTE: The automatically adding of preceding controllers into deactivate list is not implemented * yet. * * \param[in] controllers list with controllers. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 81792c9e7d..7f3b312424 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1063,6 +1063,45 @@ controller_interface::return_type ControllerManager::switch_controller( { 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 (activate_request_.empty() && deactivate_request_.empty()) @@ -1085,6 +1124,7 @@ controller_interface::return_type ControllerManager::switch_controller( return controller_interface::return_type::ERROR; } } + // start the atomic controller switching switch_params_.strictness = strictness; switch_params_.activate_asap = activate_asap; @@ -1210,7 +1250,10 @@ void ControllerManager::manage_switch() RCLCPP_ERROR(get_logger(), "Error while performing mode switch."); } - deactivate_controllers(); + std::vector & rt_controller_list = + rt_controllers_wrapper_.update_and_get_used_by_rt_list(); + + deactivate_controllers(rt_controller_list, deactivate_request_); switch_chained_mode(to_chained_mode_request_, true); switch_chained_mode(from_chained_mode_request_, false); @@ -1218,23 +1261,23 @@ void ControllerManager::manage_switch() // activate controllers once the switch is fully complete if (!switch_params_.activate_asap) { - activate_controllers(); + activate_controllers(rt_controller_list, activate_request_); } else { // activate controllers as soon as their required joints are done switching - activate_controllers_asap(); + activate_controllers_asap(rt_controller_list, activate_request_); } // TODO(destogl): move here "do_switch = false" } -void ControllerManager::deactivate_controllers() +void ControllerManager::deactivate_controllers( + const std::vector & rt_controller_list, + const std::vector controllers_to_deactivate) { - std::vector & rt_controller_list = - rt_controllers_wrapper_.update_and_get_used_by_rt_list(); - // stop controllers - for (const auto & request : deactivate_request_) + // deactivate controllers + for (const auto & request : controllers_to_deactivate) { auto found_it = std::find_if( rt_controller_list.begin(), rt_controller_list.end(), @@ -1243,7 +1286,7 @@ void ControllerManager::deactivate_controllers() { RCLCPP_ERROR( get_logger(), - "Got request to stop controller '%s' but it is not in the realtime controller list", + "Got request to deactivate controller '%s' but it is not in the realtime controller list", request.c_str()); continue; } @@ -1319,11 +1362,11 @@ void ControllerManager::switch_chained_mode( } } -void ControllerManager::activate_controllers() +void ControllerManager::activate_controllers( + const std::vector & rt_controller_list, + const std::vector controllers_to_activate) { - std::vector & rt_controller_list = - rt_controllers_wrapper_.update_and_get_used_by_rt_list(); - for (const auto & request : activate_request_) + for (const auto & request : controllers_to_activate) { auto found_it = std::find_if( rt_controller_list.begin(), rt_controller_list.end(), @@ -1433,10 +1476,12 @@ void ControllerManager::activate_controllers() switch_params_.do_switch = false; } -void ControllerManager::activate_controllers_asap() +void ControllerManager::activate_controllers_asap( + const std::vector & rt_controller_list, + const std::vector controllers_to_activate) { // https://github.com/ros-controls/ros2_control/issues/263 - activate_controllers(); + activate_controllers(rt_controller_list, controllers_to_activate); } void ControllerManager::list_controllers_srv_cb( @@ -1790,6 +1835,20 @@ void ControllerManager::list_hardware_components_srv_cb( hwi.name = interface; hwi.is_available = resource_manager_->command_interface_is_available(interface); hwi.is_claimed = resource_manager_->command_interface_is_claimed(interface); + // TODO(destogl): Add here mapping to controller that has claimed or + // can be claiming this interface + // Those should be two variables + // if (hwi.is_claimed) + // { + // for (const auto & controller : controllers_that_use_interface(interface)) + // { + // if (is_controller_active(controller)) + // { + // hwi.is_claimed_by = controller; + // } + // } + // } + // hwi.is_used_by = controllers_that_use_interface(interface); component.command_interfaces.push_back(hwi); } @@ -1888,7 +1947,23 @@ std::vector ControllerManager::get_controller_names() void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & period) { - resource_manager_->read(time, period); + auto [ok, failed_hardware_names] = resource_manager_->read(time, period); + + if (!ok) + { + std::vector stop_request = {}; + // Determine controllers to stop + for (const auto & hardware_name : failed_hardware_names) + { + auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name); + stop_request.insert(stop_request.end(), controllers.begin(), controllers.end()); + } + + std::vector & rt_controller_list = + rt_controllers_wrapper_.update_and_get_used_by_rt_list(); + deactivate_controllers(rt_controller_list, stop_request); + // TODO(destogl): do auto-start of broadcasters + } } controller_interface::return_type ControllerManager::update( @@ -1945,7 +2020,23 @@ controller_interface::return_type ControllerManager::update( void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration & period) { - resource_manager_->write(time, period); + auto [ok, failed_hardware_names] = resource_manager_->write(time, period); + + if (!ok) + { + std::vector stop_request = {}; + // Determine controllers to stop + for (const auto & hardware_name : failed_hardware_names) + { + auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name); + stop_request.insert(stop_request.end(), controllers.begin(), controllers.end()); + } + + std::vector & rt_controller_list = + rt_controllers_wrapper_.update_and_get_used_by_rt_list(); + deactivate_controllers(rt_controller_list, stop_request); + // TODO(destogl): do auto-start of broadcasters + } } std::vector & diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 78b3c3250e..78c3fcb06b 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -140,7 +140,6 @@ class ControllerManagerFixture : public ::testing::Test const std::future_status expected_future_status = std::future_status::timeout, const controller_interface::return_type expected_return = controller_interface::return_type::OK) { - // First activation not possible because controller not configured auto switch_future = std::async( std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index b6eac9b689..06f76bd044 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -14,6 +14,7 @@ #include "test_controller.hpp" +#include #include #include @@ -25,6 +26,7 @@ TestController::TestController() : controller_interface::ControllerInterface(), cmd_iface_cfg_{controller_interface::interface_configuration_type::NONE} { + set_first_command_interface_value_to = std::numeric_limits::quiet_NaN(); } controller_interface::InterfaceConfiguration TestController::command_interface_configuration() const @@ -62,12 +64,22 @@ controller_interface::return_type TestController::update( { ++internal_counter; - for (size_t i = 0; i < command_interfaces_.size(); ++i) + // set value to hardware to produce and test different behaviors there + if (!std::isnan(set_first_command_interface_value_to)) { - RCLCPP_INFO( - get_node()->get_logger(), "Setting value of command interface '%s' to %f", - command_interfaces_[i].get_name().c_str(), external_commands_for_testing_[i]); - command_interfaces_[i].set_value(external_commands_for_testing_[i]); + command_interfaces_[0].set_value(set_first_command_interface_value_to); + // reset to be easier to test + set_first_command_interface_value_to = std::numeric_limits::quiet_NaN(); + } + else + { + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { + RCLCPP_INFO( + get_node()->get_logger(), "Setting value of command interface '%s' to %f", + command_interfaces_[i].get_name().c_str(), external_commands_for_testing_[i]); + command_interfaces_[i].set_value(external_commands_for_testing_[i]); + } } return controller_interface::return_type::OK; diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index c88c11050d..f73f592037 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -74,6 +74,9 @@ class TestController : public controller_interface::ControllerInterface controller_interface::InterfaceConfiguration state_iface_cfg_; std::vector external_commands_for_testing_; + // enables external setting of values to command interfaces - used for simulation of hardware + // errors + double set_first_command_interface_value_to; }; } // namespace test_controller diff --git a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp new file mode 100644 index 0000000000..1ec725f8c5 --- /dev/null +++ b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp @@ -0,0 +1,607 @@ +// Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "controller_manager_test_common.hpp" +#include "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "test_controller/test_controller.hpp" + +using ::testing::_; +using ::testing::Return; + +using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES; +using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME; +using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES; +using ros2_control_test_assets::TEST_SENSOR_HARDWARE_COMMAND_INTERFACES; +using ros2_control_test_assets::TEST_SENSOR_HARDWARE_NAME; +using ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES; +using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES; +using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME; +using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_STATE_INTERFACES; + +class TestControllerManagerWithTestableCM; + +class TestableControllerManager : public controller_manager::ControllerManager +{ + friend TestControllerManagerWithTestableCM; + + FRIEND_TEST(TestControllerManagerWithTestableCM, check_cached_controllers_for_hardware); + FRIEND_TEST(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_error); + FRIEND_TEST(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_error); + FRIEND_TEST(TestControllerManagerWithTestableCM, stop_controllers_on_multiple_hardware_error); + +public: + TestableControllerManager( + std::unique_ptr resource_manager, + std::shared_ptr executor, + const std::string & manager_node_name = "controller_manager", + const std::string & namespace_ = "") + : controller_manager::ControllerManager( + std::move(resource_manager), executor, manager_node_name, namespace_) + { + } +}; + +class TestControllerManagerWithTestableCM +: public ControllerManagerFixture, + public testing::WithParamInterface +{ +public: + void SetupAndConfigureControllers(int strictness) + { + test_controller_actuator = std::make_shared(); + cm_->add_controller( + test_controller_actuator, TEST_CONTROLLER_ACTUATOR_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + controller_interface::InterfaceConfiguration test_controller_actuator_cmd_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES}; + controller_interface::InterfaceConfiguration test_controller_actuator_state_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + TEST_ACTUATOR_HARDWARE_STATE_INTERFACES}; + test_controller_actuator->set_command_interface_configuration( + test_controller_actuator_cmd_ifs_cfg); + test_controller_actuator->set_state_interface_configuration( + test_controller_actuator_state_ifs_cfg); + + test_controller_system = std::make_shared(); + cm_->add_controller( + test_controller_system, TEST_CONTROLLER_SYSTEM_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + controller_interface::InterfaceConfiguration test_system_controller_cmd_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES}; + controller_interface::InterfaceConfiguration test_system_controller_state_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + TEST_SYSTEM_HARDWARE_STATE_INTERFACES}; + test_controller_system->set_command_interface_configuration(test_system_controller_cmd_ifs_cfg); + test_controller_system->set_state_interface_configuration(test_system_controller_state_ifs_cfg); + + test_broadcaster_all = std::make_shared(); + cm_->add_controller( + test_broadcaster_all, TEST_BROADCASTER_ALL_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME); + controller_interface::InterfaceConfiguration test_broadcaster_all_cmd_ifs_cfg = { + controller_interface::interface_configuration_type::NONE, {}}; + controller_interface::InterfaceConfiguration test_broadcaster_all_state_ifs_cfg = { + controller_interface::interface_configuration_type::ALL, {}}; + test_broadcaster_all->set_command_interface_configuration(test_broadcaster_all_cmd_ifs_cfg); + test_broadcaster_all->set_state_interface_configuration(test_broadcaster_all_state_ifs_cfg); + + test_broadcaster_sensor = std::make_shared(); + cm_->add_controller( + test_broadcaster_sensor, TEST_BROADCASTER_SENSOR_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + controller_interface::InterfaceConfiguration test_broadcaster_sensor_cmd_ifs_cfg = { + controller_interface::interface_configuration_type::NONE, {}}; + controller_interface::InterfaceConfiguration test_broadcaster_sensor_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + TEST_SENSOR_HARDWARE_STATE_INTERFACES}; + test_broadcaster_sensor->set_command_interface_configuration( + test_broadcaster_sensor_cmd_ifs_cfg); + test_broadcaster_sensor->set_state_interface_configuration(test_broadcaster_sensor_ifs_cfg); + + // check if all controllers are added correctly + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_actuator->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_system->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_broadcaster_all->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_broadcaster_sensor->get_state().id()); + + // configure controllers + cm_->configure_controller(TEST_CONTROLLER_ACTUATOR_NAME); + cm_->configure_controller(TEST_CONTROLLER_SYSTEM_NAME); + cm_->configure_controller(TEST_BROADCASTER_ALL_NAME); + cm_->configure_controller(TEST_BROADCASTER_SENSOR_NAME); + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(0u, test_controller_actuator->internal_counter) << "Controller is not started"; + EXPECT_EQ(0u, test_controller_system->internal_counter) << "Controller is not started"; + EXPECT_EQ(0u, test_broadcaster_all->internal_counter) << "Controller is not started"; + EXPECT_EQ(0u, test_broadcaster_sensor->internal_counter) << "Controller is not started"; + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_actuator->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_broadcaster_sensor->get_state().id()); + + // Start controller, will take effect at the end of the update function + switch_test_controllers( + {TEST_CONTROLLER_ACTUATOR_NAME, TEST_CONTROLLER_SYSTEM_NAME, TEST_BROADCASTER_ALL_NAME, + TEST_BROADCASTER_SENSOR_NAME}, + {}, strictness); + } + + // values to set to hardware to simulate failure on read and write + static constexpr double READ_FAIL_VALUE = 28282828.0; + static constexpr double WRITE_FAIL_VALUE = 23232323.0; + + static constexpr char TEST_CONTROLLER_ACTUATOR_NAME[] = "test_controller_actuator"; + static constexpr char TEST_CONTROLLER_SYSTEM_NAME[] = "test_controller_system"; + static constexpr char TEST_BROADCASTER_ALL_NAME[] = "test_broadcaster_all"; + static constexpr char TEST_BROADCASTER_SENSOR_NAME[] = "test_broadcaster_sensor"; + + std::shared_ptr test_controller_actuator; + std::shared_ptr test_controller_system; + std::shared_ptr test_broadcaster_all; + std::shared_ptr test_broadcaster_sensor; +}; + +TEST_P(TestControllerManagerWithTestableCM, check_cached_controllers_for_hardware) +{ + auto strictness = GetParam().strictness; + SetupAndConfigureControllers(strictness); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_actuator->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + + { + auto controllers = + cm_->resource_manager_->get_cached_controllers_to_hardware(TEST_ACTUATOR_HARDWARE_NAME); + ASSERT_THAT( + controllers, testing::UnorderedElementsAreArray(std::vector( + {TEST_CONTROLLER_ACTUATOR_NAME, TEST_BROADCASTER_ALL_NAME}))); + } + + { + auto controllers = + cm_->resource_manager_->get_cached_controllers_to_hardware(TEST_SYSTEM_HARDWARE_NAME); + ASSERT_THAT( + controllers, testing::UnorderedElementsAreArray(std::vector( + {TEST_CONTROLLER_SYSTEM_NAME, TEST_BROADCASTER_ALL_NAME}))); + } + + { + auto controllers = + cm_->resource_manager_->get_cached_controllers_to_hardware(TEST_SENSOR_HARDWARE_NAME); + ASSERT_THAT( + controllers, testing::UnorderedElementsAreArray(std::vector( + {TEST_BROADCASTER_SENSOR_NAME, TEST_BROADCASTER_ALL_NAME}))); + } +} + +TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_error) +{ + auto strictness = GetParam().strictness; + SetupAndConfigureControllers(strictness); + + rclcpp_lifecycle::State state_active( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + + { + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_GE(test_controller_actuator->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_controller_system->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_broadcaster_all->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_broadcaster_sensor->internal_counter, 1u) + << "Controller is started at the end of update"; + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_actuator->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + + // Execute first time without any errors + { + auto new_counter = test_controller_actuator->internal_counter + 1; + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) << "Execute without errors"; + } + + // Simulate error in read() on TEST_ACTUATOR_HARDWARE_NAME by setting first command interface to + // READ_FAIL_VALUE + test_controller_actuator->set_first_command_interface_value_to = READ_FAIL_VALUE; + { + auto new_counter = test_controller_actuator->internal_counter + 1; + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_controller_system->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + } + + { + auto previous_counter = test_controller_actuator->internal_counter; + auto new_counter = test_controller_system->internal_counter + 1; + + // here happens error in hardware and + // "actuator controller" and "broadcaster all" are deactivated + EXPECT_NO_THROW(cm_->read(TIME, PERIOD)); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_actuator->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) + << "Execute has read error and it is not updated"; + EXPECT_EQ(test_controller_system->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter) + << "Broadcaster for all interfaces is not updated"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + } + + // Recover hardware and activate again all controllers + { + ASSERT_EQ( + cm_->resource_manager_->set_component_state( + ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME, state_active), + hardware_interface::return_type::OK); + auto status_map = cm_->resource_manager_->get_components_status(); + ASSERT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + auto previous_counter_lower = test_controller_actuator->internal_counter; + auto previous_counter_higher = test_controller_system->internal_counter; + + switch_test_controllers( + {TEST_CONTROLLER_ACTUATOR_NAME, TEST_BROADCASTER_ALL_NAME}, {}, strictness); + + EXPECT_GT(test_controller_actuator->internal_counter, previous_counter_lower); + EXPECT_LE(test_controller_actuator->internal_counter, previous_counter_higher); + EXPECT_GT(test_controller_system->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_all->internal_counter, previous_counter_lower); + EXPECT_LE(test_broadcaster_all->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher); + } + + // Simulate error in read() on TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME + // by setting first command interface to READ_FAIL_VALUE + test_controller_actuator->set_first_command_interface_value_to = READ_FAIL_VALUE; + test_controller_system->set_first_command_interface_value_to = READ_FAIL_VALUE; + { + auto previous_counter_lower = test_controller_actuator->internal_counter + 1; + auto previous_counter_higher = test_controller_system->internal_counter + 1; + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + + EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) + << "Execute without errors to write value"; + EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter_lower) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, previous_counter_higher) + << "Execute without errors to write value"; + } + + { + auto previous_counter_lower = test_controller_actuator->internal_counter; + auto previous_counter_higher = test_controller_system->internal_counter; + auto new_counter = test_broadcaster_sensor->internal_counter + 1; + + EXPECT_NO_THROW(cm_->read(TIME, PERIOD)); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_actuator->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) + << "Execute has read error and it is not updated"; + EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher) + << "Execute has read error and it is not updated"; + EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter_lower) + << "Broadcaster for all interfaces is not updated"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + } + + // Recover hardware and activate again all controllers + { + ASSERT_EQ( + cm_->resource_manager_->set_component_state( + ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME, state_active), + hardware_interface::return_type::OK); + ASSERT_EQ( + cm_->resource_manager_->set_component_state( + ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME, state_active), + hardware_interface::return_type::OK); + auto status_map = cm_->resource_manager_->get_components_status(); + ASSERT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + auto previous_counter_lower = test_controller_actuator->internal_counter; + auto previous_counter = test_controller_system->internal_counter; + auto previous_counter_higher = test_broadcaster_sensor->internal_counter; + + switch_test_controllers( + {TEST_CONTROLLER_SYSTEM_NAME, TEST_CONTROLLER_ACTUATOR_NAME, TEST_BROADCASTER_ALL_NAME}, {}, + strictness); + + EXPECT_GT(test_controller_actuator->internal_counter, previous_counter_lower); + EXPECT_LE(test_controller_actuator->internal_counter, previous_counter_higher); + EXPECT_GT(test_controller_system->internal_counter, previous_counter); + EXPECT_LE(test_controller_system->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_all->internal_counter, previous_counter_lower); + EXPECT_LE(test_broadcaster_all->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher); + } +} + +TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_error) +{ + auto strictness = GetParam().strictness; + SetupAndConfigureControllers(strictness); + + rclcpp_lifecycle::State state_active( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + + { + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_GE(test_controller_actuator->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_controller_system->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_broadcaster_all->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_broadcaster_sensor->internal_counter, 1u) + << "Controller is started at the end of update"; + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_actuator->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + + // Execute first time without any errors + { + auto new_counter = test_controller_actuator->internal_counter + 1; + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) << "Execute without errors"; + } + + // Simulate error in write() on TEST_ACTUATOR_HARDWARE_NAME by setting first command interface to + // WRITE_FAIL_VALUE + test_controller_actuator->set_first_command_interface_value_to = WRITE_FAIL_VALUE; + { + auto new_counter = test_controller_actuator->internal_counter + 1; + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_controller_system->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + } + + { + auto previous_counter = test_controller_actuator->internal_counter; + auto new_counter = test_controller_system->internal_counter + 1; + + // here happens error in hardware and + // "actuator controller" and "broadcaster all" are deactivated + EXPECT_NO_THROW(cm_->write(TIME, PERIOD)); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_actuator->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_controller_system->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + } + + // Recover hardware and activate again all controllers + { + ASSERT_EQ( + cm_->resource_manager_->set_component_state( + ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME, state_active), + hardware_interface::return_type::OK); + auto status_map = cm_->resource_manager_->get_components_status(); + ASSERT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + auto previous_counter_lower = test_controller_actuator->internal_counter; + auto previous_counter_higher = test_controller_system->internal_counter; + + switch_test_controllers( + {TEST_CONTROLLER_ACTUATOR_NAME, TEST_BROADCASTER_ALL_NAME}, {}, strictness); + + EXPECT_GT(test_controller_actuator->internal_counter, previous_counter_lower); + EXPECT_LE(test_controller_actuator->internal_counter, previous_counter_higher); + EXPECT_GT(test_controller_system->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_all->internal_counter, previous_counter_lower); + EXPECT_LE(test_broadcaster_all->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher); + } + + // Simulate error in write() on TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME + // by setting first command interface to WRITE_FAIL_VALUE + test_controller_actuator->set_first_command_interface_value_to = WRITE_FAIL_VALUE; + test_controller_system->set_first_command_interface_value_to = WRITE_FAIL_VALUE; + { + auto previous_counter_lower = test_controller_actuator->internal_counter + 1; + auto previous_counter_higher = test_controller_system->internal_counter + 1; + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + + EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) + << "Execute without errors to write value"; + EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter_lower) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, previous_counter_higher) + << "Execute without errors to write value"; + } + + { + auto previous_counter_lower = test_controller_actuator->internal_counter; + auto previous_counter_higher = test_controller_system->internal_counter; + auto new_counter = test_broadcaster_sensor->internal_counter + 1; + + // here happens error in hardware and + // "actuator controller" and "broadcaster all" are deactivated + EXPECT_NO_THROW(cm_->write(TIME, PERIOD)); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_actuator->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) + << "Execute has write error and it is not updated"; + EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher) + << "Execute has write error and it is not updated"; + EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter_lower) + << "Broadcaster for all interfaces is not updated"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + } + + // Recover hardware and activate again all controllers + { + ASSERT_EQ( + cm_->resource_manager_->set_component_state( + ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME, state_active), + hardware_interface::return_type::OK); + ASSERT_EQ( + cm_->resource_manager_->set_component_state( + ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME, state_active), + hardware_interface::return_type::OK); + auto status_map = cm_->resource_manager_->get_components_status(); + ASSERT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + auto previous_counter_lower = test_controller_actuator->internal_counter; + auto previous_counter = test_controller_system->internal_counter; + auto previous_counter_higher = test_broadcaster_sensor->internal_counter; + + switch_test_controllers( + {TEST_CONTROLLER_SYSTEM_NAME, TEST_CONTROLLER_ACTUATOR_NAME, TEST_BROADCASTER_ALL_NAME}, {}, + strictness); + + EXPECT_GT(test_controller_actuator->internal_counter, previous_counter_lower); + EXPECT_LE(test_controller_actuator->internal_counter, previous_counter_higher); + EXPECT_GT(test_controller_system->internal_counter, previous_counter); + EXPECT_LE(test_controller_system->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_all->internal_counter, previous_counter_lower); + EXPECT_LE(test_broadcaster_all->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher); + } +} + +INSTANTIATE_TEST_SUITE_P( + test_strict_best_effort, TestControllerManagerWithTestableCM, + testing::Values(strict, best_effort));