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..f12ab9e96b --- /dev/null +++ b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp @@ -0,0 +1,604 @@ +// 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 "ros2_control_test_assets/test_hardware_interface_constants.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); + } + + 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 = test_constants::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 = test_constants::READ_FAIL_VALUE; + test_controller_system->set_first_command_interface_value_to = test_constants::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 = test_constants::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 = test_constants::WRITE_FAIL_VALUE; + test_controller_system->set_first_command_interface_value_to = test_constants::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)); diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 2fb31641b0..73e1f8ec22 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -143,7 +143,9 @@ if(BUILD_TESTING) test/test_components/test_system.cpp) target_link_libraries(test_components ${PROJECT_NAME}) ament_target_dependencies(test_components - pluginlib) + pluginlib + ros2_control_test_assets + ) install(TARGETS test_components DESTINATION lib ) diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp index 5c3ea22ca0..d39dee2c64 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp @@ -23,6 +23,7 @@ enum class return_type : std::uint8_t { OK = 0, ERROR = 1, + DEACTIVATE = 2, }; } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 98f2727613..56ef2af514 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1089,6 +1089,7 @@ void ResourceManager::read(const rclcpp::Time & time, const rclcpp::Duration & p { for (auto & component : resource_storage_->actuators_) { +<<<<<<< HEAD component.read(time, period); } for (auto & component : resource_storage_->sensors_) @@ -1099,6 +1100,68 @@ void ResourceManager::read(const rclcpp::Time & time, const rclcpp::Duration & p { component.read(time, period); } +======= + for (auto & component : components) + { + auto ret_val = component.read(time, period); + if (ret_val == return_type::ERROR) + { + read_write_status.ok = false; + read_write_status.failed_hardware_names.push_back(component.get_name()); + resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name()); + } + else if (ret_val == return_type::DEACTIVATE) + { + resource_storage_->deactivate_hardware(component); + } + // If desired: automatic re-activation. We could add a flag for this... + // else + // { + // using lifecycle_msgs::msg::State; + // rclcpp_lifecycle::State state(State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); + // set_component_state(component.get_name(), state); + // } + } + }; + + read_components(resource_storage_->actuators_); + read_components(resource_storage_->sensors_); + read_components(resource_storage_->systems_); + + return read_write_status; +} + +// CM API: Called in "update"-thread +HardwareReadWriteStatus ResourceManager::write( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + std::lock_guard guard(resources_lock_); + read_write_status.ok = true; + read_write_status.failed_hardware_names.clear(); + + auto write_components = [&](auto & components) + { + for (auto & component : components) + { + auto ret_val = component.write(time, period); + if (ret_val == return_type::ERROR) + { + read_write_status.ok = false; + read_write_status.failed_hardware_names.push_back(component.get_name()); + resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name()); + } + else if (ret_val == return_type::DEACTIVATE) + { + resource_storage_->deactivate_hardware(component); + } + } + }; + + write_components(resource_storage_->actuators_); + write_components(resource_storage_->systems_); + + return read_write_status; +>>>>>>> bd6911d ([ResourceManager] deactivate hardware from read/write return value (#884)) } void ResourceManager::write(const rclcpp::Time & time, const rclcpp::Duration & period) diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp index 47dd9f0d32..098ed530a3 100644 --- a/hardware_interface/test/test_components/test_actuator.cpp +++ b/hardware_interface/test/test_components/test_actuator.cpp @@ -16,6 +16,7 @@ #include #include "hardware_interface/actuator_interface.hpp" +#include "ros2_control_test_assets/test_hardware_interface_constants.hpp" using hardware_interface::ActuatorInterface; using hardware_interface::CommandInterface; @@ -75,6 +76,21 @@ class TestActuator : public ActuatorInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { +<<<<<<< HEAD +======= + // simulate error on read + if (velocity_command_ == test_constants::READ_FAIL_VALUE) + { + // reset value to get out from error on the next call - simplifies CM tests + velocity_command_ = 0.0; + return return_type::ERROR; + } + // simulate deactivate on read + if (velocity_command_ == test_constants::READ_DEACTIVATE_VALUE) + { + return return_type::DEACTIVATE; + } +>>>>>>> bd6911d ([ResourceManager] deactivate hardware from read/write return value (#884)) // The next line is for the testing purposes. We need value to be changed to be sure that // the feedback from hardware to controllers in the chain is working as it should. // This makes value checks clearer and confirms there is no "state = command" line or some @@ -85,6 +101,21 @@ class TestActuator : public ActuatorInterface return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { +<<<<<<< HEAD +======= + // simulate error on write + if (velocity_command_ == test_constants::WRITE_FAIL_VALUE) + { + // reset value to get out from error on the next call - simplifies CM tests + velocity_command_ = 0.0; + return return_type::ERROR; + } + // simulate deactivate on write + if (velocity_command_ == test_constants::WRITE_DEACTIVATE_VALUE) + { + return return_type::DEACTIVATE; + } +>>>>>>> bd6911d ([ResourceManager] deactivate hardware from read/write return value (#884)) return return_type::OK; } diff --git a/hardware_interface/test/test_components/test_system.cpp b/hardware_interface/test/test_components/test_system.cpp index 73659ca506..65bb57b2da 100644 --- a/hardware_interface/test/test_components/test_system.cpp +++ b/hardware_interface/test/test_components/test_system.cpp @@ -18,6 +18,7 @@ #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "ros2_control_test_assets/test_hardware_interface_constants.hpp" using hardware_interface::CommandInterface; using hardware_interface::return_type; @@ -80,11 +81,41 @@ class TestSystem : public SystemInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { +<<<<<<< HEAD +======= + // simulate error on read + if (velocity_command_[0] == test_constants::READ_FAIL_VALUE) + { + // reset value to get out from error on the next call - simplifies CM tests + velocity_command_[0] = 0.0; + return return_type::ERROR; + } + // simulate deactivate on read + if (velocity_command_[0] == test_constants::READ_DEACTIVATE_VALUE) + { + return return_type::DEACTIVATE; + } +>>>>>>> bd6911d ([ResourceManager] deactivate hardware from read/write return value (#884)) return return_type::OK; } return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { +<<<<<<< HEAD +======= + // simulate error on write + if (velocity_command_[0] == test_constants::WRITE_FAIL_VALUE) + { + // reset value to get out from error on the next call - simplifies CM tests + velocity_command_[0] = 0.0; + return return_type::ERROR; + } + // simulate deactivate on write + if (velocity_command_[0] == test_constants::WRITE_DEACTIVATE_VALUE) + { + return return_type::DEACTIVATE; + } +>>>>>>> bd6911d ([ResourceManager] deactivate hardware from read/write return value (#884)) return return_type::OK; } diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index eca8bbf40a..22c89a2c16 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -28,6 +28,7 @@ #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp_lifecycle/state.hpp" #include "ros2_control_test_assets/descriptions.hpp" +#include "ros2_control_test_assets/test_hardware_interface_constants.hpp" using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_CLASS_TYPE; using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES; @@ -1331,3 +1332,402 @@ TEST_F(TestResourceManager, managing_controllers_reference_interfaces) EXPECT_THROW( rm.make_controller_reference_interfaces_unavailable("unknown_controller"), std::out_of_range); } +<<<<<<< HEAD +======= + +class ResourceManagerTestReadWriteError : public ResourceManagerTest +{ +public: + void setup_resource_manager_and_do_initial_checks() + { + rm = std::make_shared( + ros2_control_test_assets::minimal_robot_urdf, false); + activate_components(*rm); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + claimed_itfs.push_back( + rm->claim_command_interface(TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES[0])); + claimed_itfs.push_back(rm->claim_command_interface(TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES[0])); + + check_if_interface_available(true, true); + // with default values read and write should run without any problems + { + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + } + { + auto [ok, failed_hardware_names] = rm->write(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + } + check_if_interface_available(true, true); + } + + // check if all interfaces are available + void check_if_interface_available(const bool actuator_interfaces, const bool system_interfaces) + { + for (const auto & interface : TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + EXPECT_EQ(rm->command_interface_is_available(interface), actuator_interfaces); + } + for (const auto & interface : TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + EXPECT_EQ(rm->state_interface_is_available(interface), actuator_interfaces); + } + for (const auto & interface : TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES) + { + EXPECT_EQ(rm->command_interface_is_available(interface), system_interfaces); + } + for (const auto & interface : TEST_SYSTEM_HARDWARE_STATE_INTERFACES) + { + EXPECT_EQ(rm->state_interface_is_available(interface), system_interfaces); + } + }; + + using FunctionT = + std::function; + + void check_read_or_write_failure( + FunctionT method_that_fails, FunctionT other_method, const double fail_value) + { + // define state to set components to + rclcpp_lifecycle::State state_active( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + + // read failure for TEST_ACTUATOR_HARDWARE_NAME + claimed_itfs[0].set_value(fail_value); + claimed_itfs[1].set_value(fail_value - 10.0); + { + auto [ok, failed_hardware_names] = method_that_fails(time, duration); + EXPECT_FALSE(ok); + EXPECT_FALSE(failed_hardware_names.empty()); + ASSERT_THAT( + failed_hardware_names, + testing::ElementsAreArray(std::vector({TEST_ACTUATOR_HARDWARE_NAME}))); + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + check_if_interface_available(false, true); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_active); + status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + check_if_interface_available(true, true); + } + // write is sill OK + { + auto [ok, failed_hardware_names] = other_method(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + check_if_interface_available(true, true); + } + + // read failure for TEST_SYSTEM_HARDWARE_NAME + claimed_itfs[0].set_value(fail_value - 10.0); + claimed_itfs[1].set_value(fail_value); + { + auto [ok, failed_hardware_names] = method_that_fails(time, duration); + EXPECT_FALSE(ok); + EXPECT_FALSE(failed_hardware_names.empty()); + ASSERT_THAT( + failed_hardware_names, + testing::ElementsAreArray(std::vector({TEST_SYSTEM_HARDWARE_NAME}))); + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + check_if_interface_available(true, false); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_active); + status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + check_if_interface_available(true, true); + } + // write is sill OK + { + auto [ok, failed_hardware_names] = other_method(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + check_if_interface_available(true, true); + } + + // read failure for both, TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME + claimed_itfs[0].set_value(fail_value); + claimed_itfs[1].set_value(fail_value); + { + auto [ok, failed_hardware_names] = method_that_fails(time, duration); + EXPECT_FALSE(ok); + EXPECT_FALSE(failed_hardware_names.empty()); + ASSERT_THAT( + failed_hardware_names, testing::ElementsAreArray(std::vector( + {TEST_ACTUATOR_HARDWARE_NAME, TEST_SYSTEM_HARDWARE_NAME}))); + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + check_if_interface_available(false, false); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_active); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_active); + status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + check_if_interface_available(true, true); + } + // write is sill OK + { + auto [ok, failed_hardware_names] = other_method(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + check_if_interface_available(true, true); + } + } + + void check_read_or_write_deactivate( + FunctionT method_that_deactivates, FunctionT other_method, const double deactivate_value) + { + // define state to set components to + rclcpp_lifecycle::State state_active( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + + // deactivate for TEST_ACTUATOR_HARDWARE_NAME + claimed_itfs[0].set_value(deactivate_value); + claimed_itfs[1].set_value(deactivate_value - 10.0); + { + // deactivate on error + auto [ok, failed_hardware_names] = method_that_deactivates(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + check_if_interface_available(true, true); + + // reactivate + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_active); + status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + check_if_interface_available(true, true); + } + // write is sill OK + { + auto [ok, failed_hardware_names] = other_method(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + check_if_interface_available(true, true); + } + + // deactivate for TEST_SYSTEM_HARDWARE_NAME + claimed_itfs[0].set_value(deactivate_value - 10.0); + claimed_itfs[1].set_value(deactivate_value); + { + // deactivate on flag + auto [ok, failed_hardware_names] = method_that_deactivates(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + check_if_interface_available(true, true); + // re-activate + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_active); + status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + check_if_interface_available(true, true); + } + // write is sill OK + { + auto [ok, failed_hardware_names] = other_method(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + check_if_interface_available(true, true); + } + + // deactivate both, TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME + claimed_itfs[0].set_value(deactivate_value); + claimed_itfs[1].set_value(deactivate_value); + { + // deactivate on flag + auto [ok, failed_hardware_names] = method_that_deactivates(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + check_if_interface_available(true, true); + // re-activate + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_active); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_active); + status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + check_if_interface_available(true, true); + } + // write is sill OK + { + auto [ok, failed_hardware_names] = other_method(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + check_if_interface_available(true, true); + } + } + +public: + std::shared_ptr rm; + std::vector claimed_itfs; + + const rclcpp::Time time = rclcpp::Time(0); + const rclcpp::Duration duration = rclcpp::Duration::from_seconds(0.01); + + // values to set to hardware to simulate failure on read and write +}; + +TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_read) +{ + setup_resource_manager_and_do_initial_checks(); + + using namespace std::placeholders; + // check read methods failures + check_read_or_write_failure( + std::bind(&TestableResourceManager::read, rm, _1, _2), + std::bind(&TestableResourceManager::write, rm, _1, _2), test_constants::READ_FAIL_VALUE); +} + +TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_write) +{ + setup_resource_manager_and_do_initial_checks(); + + using namespace std::placeholders; + // check write methods failures + check_read_or_write_failure( + std::bind(&TestableResourceManager::write, rm, _1, _2), + std::bind(&TestableResourceManager::read, rm, _1, _2), test_constants::WRITE_FAIL_VALUE); +} + +TEST_F(ResourceManagerTestReadWriteError, handle_deactivate_on_hardware_read) +{ + setup_resource_manager_and_do_initial_checks(); + + using namespace std::placeholders; + // check read methods failures + check_read_or_write_deactivate( + std::bind(&TestableResourceManager::read, rm, _1, _2), + std::bind(&TestableResourceManager::write, rm, _1, _2), test_constants::READ_DEACTIVATE_VALUE); +} + +TEST_F(ResourceManagerTestReadWriteError, handle_deactivate_on_hardware_write) +{ + setup_resource_manager_and_do_initial_checks(); + + using namespace std::placeholders; + // check write methods failures + check_read_or_write_deactivate( + std::bind(&TestableResourceManager::write, rm, _1, _2), + std::bind(&TestableResourceManager::read, rm, _1, _2), test_constants::WRITE_DEACTIVATE_VALUE); +} + +TEST_F(ResourceManagerTest, test_caching_of_controllers_to_hardware) +{ + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf, false); + activate_components(rm); + + static const std::string TEST_CONTROLLER_ACTUATOR_NAME = "test_controller_actuator"; + static const std::string TEST_CONTROLLER_SYSTEM_NAME = "test_controller_system"; + static const std::string TEST_BROADCASTER_ALL_NAME = "test_broadcaster_all"; + static const std::string TEST_BROADCASTER_SENSOR_NAME = "test_broadcaster_sensor"; + + rm.cache_controller_to_hardware( + TEST_CONTROLLER_ACTUATOR_NAME, TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES); + rm.cache_controller_to_hardware( + TEST_BROADCASTER_ALL_NAME, TEST_ACTUATOR_HARDWARE_STATE_INTERFACES); + + rm.cache_controller_to_hardware( + TEST_CONTROLLER_SYSTEM_NAME, TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES); + rm.cache_controller_to_hardware(TEST_BROADCASTER_ALL_NAME, TEST_SYSTEM_HARDWARE_STATE_INTERFACES); + + rm.cache_controller_to_hardware( + TEST_BROADCASTER_SENSOR_NAME, TEST_SENSOR_HARDWARE_STATE_INTERFACES); + rm.cache_controller_to_hardware(TEST_BROADCASTER_ALL_NAME, TEST_SENSOR_HARDWARE_STATE_INTERFACES); + + { + auto controllers = rm.get_cached_controllers_to_hardware(TEST_ACTUATOR_HARDWARE_NAME); + ASSERT_THAT( + controllers, testing::ElementsAreArray(std::vector( + {TEST_CONTROLLER_ACTUATOR_NAME, TEST_BROADCASTER_ALL_NAME}))); + } + + { + auto controllers = rm.get_cached_controllers_to_hardware(TEST_SYSTEM_HARDWARE_NAME); + ASSERT_THAT( + controllers, testing::ElementsAreArray(std::vector( + {TEST_CONTROLLER_SYSTEM_NAME, TEST_BROADCASTER_ALL_NAME}))); + } + + { + auto controllers = rm.get_cached_controllers_to_hardware(TEST_SENSOR_HARDWARE_NAME); + ASSERT_THAT( + controllers, testing::ElementsAreArray(std::vector( + {TEST_BROADCASTER_SENSOR_NAME, TEST_BROADCASTER_ALL_NAME}))); + } +} +>>>>>>> bd6911d ([ResourceManager] deactivate hardware from read/write return value (#884)) diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/test_hardware_interface_constants.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/test_hardware_interface_constants.hpp new file mode 100644 index 0000000000..eb2bbf24c7 --- /dev/null +++ b/ros2_control_test_assets/include/ros2_control_test_assets/test_hardware_interface_constants.hpp @@ -0,0 +1,28 @@ +// Copyright (c) 2023, FZI Forschungszentrum Informatik +// +// 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. +// +/// \author: Felix Exner + +#ifndef ROS2_CONTROL_TEST_ASSETS__TEST_HARDWARE_INTERFACE_CONSTANTS_HPP_ +#define ROS2_CONTROL_TEST_ASSETS__TEST_HARDWARE_INTERFACE_CONSTANTS_HPP_ +namespace test_constants +{ +/// Constants defining special values used inside tests to trigger things like deactivate or errors +/// on read/write. +constexpr double READ_FAIL_VALUE = 28282828.0; +constexpr double WRITE_FAIL_VALUE = 23232323.0; +constexpr double READ_DEACTIVATE_VALUE = 29292929.0; +constexpr double WRITE_DEACTIVATE_VALUE = 24242424.0; +} // namespace test_constants +#endif // ROS2_CONTROL_TEST_ASSETS__TEST_HARDWARE_INTERFACE_CONSTANTS_HPP_