From 8c34ab6b383ea53c53c05fe460f9c7c743a96fa1 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 12 Jan 2024 18:40:56 +0100 Subject: [PATCH] Add additional checks for non existing and not available interfaces. (#1218) --- hardware_interface/CMakeLists.txt | 4 + .../hardware_interface/resource_manager.hpp | 6 +- hardware_interface/src/resource_manager.cpp | 135 ++++-- .../mock_components/test_generic_system.cpp | 81 ++-- .../test/test_components/test_actuator.cpp | 16 + .../test_system_with_command_modes.cpp | 3 + .../test/test_resource_manager.cpp | 121 +----- .../test/test_resource_manager.hpp | 80 ++++ ...esource_manager_prepare_perform_switch.cpp | 389 ++++++++++++++++++ 9 files changed, 646 insertions(+), 189 deletions(-) create mode 100644 hardware_interface/test/test_resource_manager.hpp create mode 100644 hardware_interface/test/test_resource_manager_prepare_perform_switch.cpp diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 510d970c84..35823b3ce9 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -114,6 +114,10 @@ if(BUILD_TESTING) target_link_libraries(test_resource_manager hardware_interface) ament_target_dependencies(test_resource_manager ros2_control_test_assets) + ament_add_gmock(test_resource_manager_prepare_perform_switch test/test_resource_manager_prepare_perform_switch.cpp) + target_link_libraries(test_resource_manager_prepare_perform_switch hardware_interface) + ament_target_dependencies(test_resource_manager_prepare_perform_switch ros2_control_test_assets) + ament_add_gmock(test_generic_system test/mock_components/test_generic_system.cpp) target_include_directories(test_generic_system PRIVATE include) target_link_libraries(test_generic_system hardware_interface) diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 92bde14817..146b903f46 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -331,7 +331,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * by default * \param[in] start_interfaces vector of string identifiers for the command interfaces starting. * \param[in] stop_interfaces vector of string identifiers for the command interfaces stopping. - * \return true if switch can be prepared, false if a component rejects switch request. + * \return true if switch can be prepared; false if a component rejects switch request, and if + * at least one of the input interfaces are not existing or not available (i.e., component is not + * in ACTIVE or INACTIVE state). */ bool prepare_command_mode_switch( const std::vector & start_interfaces, @@ -344,6 +346,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * \note this is intended for mode-switching when a hardware interface needs to change * control mode depending on which command interface is claimed. * \note this is for realtime switching of the command interface. + * \note it is assumed that `prepare_command_mode_switch` is called just before this method + * with the same input arguments. * \param[in] start_interfaces vector of string identifiers for the command interfaces starting. * \param[in] stop_interfaces vector of string identifiers for the command interfacs stopping. * \return true if switch is performed, false if a component rejects switching. diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 5c5d5af3e6..0cbb620c76 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -287,6 +287,7 @@ class ResourceStorage if (result) { + remove_all_hardware_interfaces_from_available_list(hardware.get_name()); async_component_threads_.erase(hardware.get_name()); // TODO(destogl): change this - deimport all things if there is there are interfaces there // deimport_non_movement_command_interfaces(hardware); @@ -1072,6 +1073,12 @@ bool ResourceManager::prepare_command_mode_switch( const std::vector & start_interfaces, const std::vector & stop_interfaces) { + // When only broadcaster is activated then this lists are empty + if (start_interfaces.empty() && stop_interfaces.empty()) + { + return true; + } + auto interfaces_to_string = [&]() { std::stringstream ss; @@ -1090,27 +1097,85 @@ bool ResourceManager::prepare_command_mode_switch( return ss.str(); }; - for (auto & component : resource_storage_->actuators_) + // Check if interface exists + std::stringstream ss_not_existing; + ss_not_existing << "Not existing: " << std::endl << "[" << std::endl; + auto check_exist = [&](const std::vector & list_to_check) { - if (return_type::OK != component.prepare_command_mode_switch(start_interfaces, stop_interfaces)) + bool all_exist = true; + for (const auto & interface : list_to_check) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Component '%s' did not accept new command resource combination: \n %s", - component.get_name().c_str(), interfaces_to_string().c_str()); - return false; + if (!command_interface_exists(interface)) + { + all_exist = false; + ss_not_existing << " " << interface << std::endl; + } } + return all_exist; + }; + if (!(check_exist(start_interfaces) && check_exist(stop_interfaces))) + { + ss_not_existing << "]" << std::endl; + RCUTILS_LOG_ERROR_NAMED( + "resource_manager", "Not acceptable command interfaces combination: \n%s%s", + interfaces_to_string().c_str(), ss_not_existing.str().c_str()); + return false; } - for (auto & component : resource_storage_->systems_) + + // Check if interfaces are available + std::stringstream ss_not_available; + ss_not_available << "Not available: " << std::endl << "[" << std::endl; + auto check_available = [&](const std::vector & list_to_check) { - if (return_type::OK != component.prepare_command_mode_switch(start_interfaces, stop_interfaces)) + bool all_available = true; + for (const auto & interface : list_to_check) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Component '%s' did not accept new command resource combination: \n %s", - component.get_name().c_str(), interfaces_to_string().c_str()); - return false; + if (!command_interface_is_available(interface)) + { + all_available = false; + ss_not_available << " " << interface << std::endl; + } } + return all_available; + }; + if (!(check_available(start_interfaces) && check_available(stop_interfaces))) + { + ss_not_available << "]" << std::endl; + RCUTILS_LOG_ERROR_NAMED( + "resource_manager", "Not acceptable command interfaces combination: \n%s%s", + interfaces_to_string().c_str(), ss_not_available.str().c_str()); + return false; } - return true; + + auto call_prepare_mode_switch = + [&start_interfaces, &stop_interfaces, &interfaces_to_string](auto & components) + { + bool ret = true; + for (auto & component : components) + { + if ( + component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + if ( + return_type::OK != + component.prepare_command_mode_switch(start_interfaces, stop_interfaces)) + { + RCUTILS_LOG_ERROR_NAMED( + "resource_manager", + "Component '%s' did not accept command interfaces combination: \n%s", + component.get_name().c_str(), interfaces_to_string().c_str()); + ret = false; + } + } + } + return ret; + }; + + const bool actuators_result = call_prepare_mode_switch(resource_storage_->actuators_); + const bool systems_result = call_prepare_mode_switch(resource_storage_->systems_); + + return actuators_result && systems_result; } // CM API: Called in "update"-thread @@ -1118,27 +1183,39 @@ bool ResourceManager::perform_command_mode_switch( const std::vector & start_interfaces, const std::vector & stop_interfaces) { - for (auto & component : resource_storage_->actuators_) + // When only broadcaster is activated then this lists are empty + if (start_interfaces.empty() && stop_interfaces.empty()) { - if (return_type::OK != component.perform_command_mode_switch(start_interfaces, stop_interfaces)) - { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Component '%s' could not perform switch", - component.get_name().c_str()); - return false; - } + return true; } - for (auto & component : resource_storage_->systems_) + + auto call_perform_mode_switch = [&start_interfaces, &stop_interfaces](auto & components) { - if (return_type::OK != component.perform_command_mode_switch(start_interfaces, stop_interfaces)) + bool ret = true; + for (auto & component : components) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Component '%s' could not perform switch", - component.get_name().c_str()); - return false; + if ( + component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + if ( + return_type::OK != + component.perform_command_mode_switch(start_interfaces, stop_interfaces)) + { + RCUTILS_LOG_ERROR_NAMED( + "resource_manager", "Component '%s' could not perform switch", + component.get_name().c_str()); + ret = false; + } + } } - } - return true; + return ret; + }; + + const bool actuators_result = call_perform_mode_switch(resource_storage_->actuators_); + const bool systems_result = call_perform_mode_switch(resource_storage_->systems_); + + return actuators_result && systems_result; } // CM API: Called in "callback/slow"-thread diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 57b5796d0d..0641cfda57 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -51,7 +51,7 @@ class TestGenericSystem : public ::testing::Test { hardware_system_2dof_ = R"( - + mock_components/GenericSystem @@ -72,7 +72,7 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_asymetric_ = R"( - + mock_components/GenericSystem @@ -93,7 +93,7 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_standard_interfaces_ = R"( - + mock_components/GenericSystem @@ -118,7 +118,7 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_with_other_interface_ = R"( - + mock_components/GenericSystem @@ -153,7 +153,7 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_with_sensor_ = R"( - + mock_components/GenericSystem @@ -181,7 +181,7 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_with_sensor_mock_command_ = R"( - + mock_components/GenericSystem true @@ -210,7 +210,7 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_with_sensor_mock_command_True_ = R"( - + mock_components/GenericSystem True @@ -239,7 +239,7 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_with_mimic_joint_ = R"( - + mock_components/GenericSystem @@ -264,7 +264,7 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_standard_interfaces_with_offset_ = R"( - + mock_components/GenericSystem -3 @@ -290,7 +290,7 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_missing_ = R"( - + mock_components/GenericSystem -3 @@ -321,7 +321,7 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_ = R"( - + mock_components/GenericSystem -3 @@ -352,7 +352,7 @@ class TestGenericSystem : public ::testing::Test valid_urdf_ros2_control_system_robot_with_gpio_ = R"( - + mock_components/GenericSystem 2 @@ -389,7 +389,7 @@ class TestGenericSystem : public ::testing::Test valid_urdf_ros2_control_system_robot_with_gpio_mock_command_ = R"( - + mock_components/GenericSystem true @@ -425,7 +425,7 @@ class TestGenericSystem : public ::testing::Test valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_ = R"( - + mock_components/GenericSystem True @@ -461,7 +461,7 @@ class TestGenericSystem : public ::testing::Test sensor_with_initial_value_ = R"( - + mock_components/GenericSystem @@ -481,7 +481,7 @@ class TestGenericSystem : public ::testing::Test gpio_with_initial_value_ = R"( - + mock_components/GenericSystem @@ -495,7 +495,7 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_standard_interfaces_with_different_control_modes_ = R"( - + mock_components/GenericSystem true @@ -528,7 +528,7 @@ class TestGenericSystem : public ::testing::Test valid_hardware_system_2dof_standard_interfaces_with_different_control_modes_ = R"( - + mock_components/GenericSystem true @@ -568,7 +568,7 @@ class TestGenericSystem : public ::testing::Test disabled_commands_ = R"( - + mock_components/GenericSystem True @@ -689,7 +689,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_symetric_interfaces) ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); // Activate components to get all interfaces available - activate_components(rm, {"HardwareSystem2dof"}); + activate_components(rm, {"MockHardwareSystem"}); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -720,7 +720,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_asymetric_interfaces) ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); // Activate components to get all interfaces available - activate_components(rm, {"HardwareSystem2dofAsymetric"}); + activate_components(rm, {"MockHardwareSystem"}); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -866,7 +866,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality) auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_standard_interfaces_ + ros2_control_test_assets::urdf_tail; - generic_system_functional_test(urdf, {"HardwareSystem2dofStandardInterfaces"}); + generic_system_functional_test(urdf, {"MockHardwareSystem"}); } TEST_F(TestGenericSystem, generic_system_2dof_other_interfaces) @@ -875,7 +875,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_other_interfaces) ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); // Activate components to get all interfaces available - activate_components(rm, {"HardwareSystem2dofWithOtherInterface"}); + activate_components(rm, {"MockHardwareSystem"}); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -958,7 +958,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor) ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); // Activate components to get all interfaces available - activate_components(rm, {"HardwareSystem2dofWithSensor"}); + activate_components(rm, {"MockHardwareSystem"}); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -1180,7 +1180,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command) auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_sensor_mock_command_ + ros2_control_test_assets::urdf_tail; - test_generic_system_with_mock_sensor_commands(urdf, "HardwareSystem2dofWithSensorMockCommand"); + test_generic_system_with_mock_sensor_commands(urdf, "MockHardwareSystem"); } TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command_True) @@ -1189,8 +1189,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command_True) hardware_system_2dof_with_sensor_mock_command_True_ + ros2_control_test_assets::urdf_tail; - test_generic_system_with_mock_sensor_commands( - urdf, "HardwareSystem2dofWithSensorMockCommandTrue"); + test_generic_system_with_mock_sensor_commands(urdf, "MockHardwareSystem"); } void TestGenericSystem::test_generic_system_with_mimic_joint( @@ -1265,7 +1264,7 @@ TEST_F(TestGenericSystem, hardware_system_2dof_with_mimic_joint) auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_mimic_joint_ + ros2_control_test_assets::urdf_tail; - test_generic_system_with_mimic_joint(urdf, "HardwareSystem2dofWithMimicJoint"); + test_generic_system_with_mimic_joint(urdf, "MockHardwareSystem"); } TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset) @@ -1274,7 +1273,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset) hardware_system_2dof_standard_interfaces_with_offset_ + ros2_control_test_assets::urdf_tail; - generic_system_functional_test(urdf, "HardwareSystem2dofStandardInterfacesWithOffset", -3); + generic_system_functional_test(urdf, "MockHardwareSystem", -3); } TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_interface_missing) @@ -1284,8 +1283,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i ros2_control_test_assets::urdf_tail; // custom interface is missing so offset will not be applied - generic_system_functional_test( - urdf, "HardwareSystem2dofStandardInterfacesWithCustomInterfaceForOffsetMissing", 0.0); + generic_system_functional_test(urdf, "MockHardwareSystem", 0.0); } TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_interface) @@ -1298,8 +1296,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i TestableResourceManager rm(urdf); - const std::string hardware_name = - "HardwareSystem2dofStandardInterfacesWithCustomInterfaceForOffset"; + const std::string hardware_name = "MockHardwareSystem"; // check is hardware is configured auto status_map = rm.get_components_status(); @@ -1412,7 +1409,7 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio) valid_urdf_ros2_control_system_robot_with_gpio_ + ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); - const std::string hardware_name = "ValidURDFros2controlSystemRobotWithGPIO"; + const std::string hardware_name = "MockHardwareSystem"; // check is hardware is started auto status_map = rm.get_components_status(); @@ -1617,8 +1614,7 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_co valid_urdf_ros2_control_system_robot_with_gpio_mock_command_ + ros2_control_test_assets::urdf_tail; - test_generic_system_with_mock_gpio_commands( - urdf, "ValidURDFros2controlSystemRobotWithGPIOMockCommand"); + test_generic_system_with_mock_gpio_commands(urdf, "MockHardwareSystem"); } TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True) @@ -1627,8 +1623,7 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_co valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_ + ros2_control_test_assets::urdf_tail; - test_generic_system_with_mock_gpio_commands( - urdf, "ValidURDFros2controlSystemRobotWithGPIOMockCommandTrue"); + test_generic_system_with_mock_gpio_commands(urdf, "MockHardwareSystem"); } TEST_F(TestGenericSystem, sensor_with_initial_value) @@ -1637,7 +1632,7 @@ TEST_F(TestGenericSystem, sensor_with_initial_value) ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); // Activate components to get all interfaces available - activate_components(rm, {"SensorWithInitialValue"}); + activate_components(rm, {"MockHardwareSystem"}); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -1665,7 +1660,7 @@ TEST_F(TestGenericSystem, gpio_with_initial_value) ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); // Activate components to get all interfaces available - activate_components(rm, {"GPIOWithInitialValue"}); + activate_components(rm, {"MockHardwareSystem"}); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -1686,7 +1681,7 @@ TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) TestableResourceManager rm(urdf); // Activate components to get all interfaces available - activate_components(rm, {"HardwareSystem2dofStandardInterfacesWithDifferentControlModes"}); + activate_components(rm, {"MockHardwareSystem"}); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -1880,7 +1875,7 @@ TEST_F(TestGenericSystem, disabled_commands_flag_is_active) ros2_control_test_assets::urdf_head + disabled_commands_ + ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); // Activate components to get all interfaces available - activate_components(rm, {"DisabledCommands"}); + activate_components(rm, {"MockHardwareSystem"}); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -1927,6 +1922,8 @@ TEST_F(TestGenericSystem, prepare_command_mode_switch_works_with_all_example_tag { TestableResourceManager rm( ros2_control_test_assets::urdf_head + urdf + ros2_control_test_assets::urdf_tail); + rclcpp_lifecycle::State state(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); + rm.set_component_state("MockHardwareSystem", state); auto start_interfaces = rm.command_interface_keys(); std::vector stop_interfaces; return rm.prepare_command_mode_switch(start_interfaces, stop_interfaces); diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp index 2f606b74cb..41f27e201b 100644 --- a/hardware_interface/test/test_components/test_actuator.cpp +++ b/hardware_interface/test/test_components/test_actuator.cpp @@ -74,6 +74,22 @@ class TestActuator : public ActuatorInterface return command_interfaces; } + hardware_interface::return_type prepare_command_mode_switch( + const std::vector & /*start_interfaces*/, + const std::vector & /*stop_interfaces*/) override + { + position_state_ += 1.0; + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type perform_command_mode_switch( + const std::vector & /*start_interfaces*/, + const std::vector & /*stop_interfaces*/) override + { + position_state_ += 100.0; + return hardware_interface::return_type::OK; + } + return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on read diff --git a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp index 846508b1e2..a594d3b70a 100644 --- a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp +++ b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp @@ -120,6 +120,8 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface const std::vector & start_interfaces, const std::vector & stop_interfaces) override { + acceleration_state_[0] += 1.0; + // Starting interfaces start_modes_.clear(); stop_modes_.clear(); @@ -166,6 +168,7 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface const std::vector & start_interfaces, const std::vector & /*stop_interfaces*/) override { + acceleration_state_[0] += 100.0; // Test of failure in perform command mode switch // Fail if given an empty list. // This should never occur in a real system as the same start_interfaces list is sent to both diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index 84519c20fa..4bb9e0c5fe 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -14,7 +14,7 @@ // Authors: Karsten Knese, Denis Stogl -#include +#include "test_resource_manager.hpp" #include #include @@ -23,7 +23,6 @@ #include #include "hardware_interface/actuator_interface.hpp" -#include "hardware_interface/resource_manager.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -46,59 +45,6 @@ using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_PLUGIN_NAME; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_STATE_INTERFACES; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_TYPE; -class ResourceManagerTest : public ::testing::Test -{ -public: - static void SetUpTestCase() {} - - void SetUp() {} -}; - -// Forward declaration -namespace hardware_interface -{ -class ResourceStorage; -} - -class TestableResourceManager : public hardware_interface::ResourceManager -{ -public: - friend ResourceManagerTest; - - FRIEND_TEST(ResourceManagerTest, initialization_with_urdf_manual_validation); - FRIEND_TEST(ResourceManagerTest, post_initialization_add_components); - FRIEND_TEST(ResourceManagerTest, managing_controllers_reference_interfaces); - FRIEND_TEST(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle); - FRIEND_TEST(ResourceManagerTest, test_unitilizable_hardware_no_validation); - - TestableResourceManager() : hardware_interface::ResourceManager() {} - - TestableResourceManager( - const std::string & urdf, bool validate_interfaces = true, bool activate_all = false) - : hardware_interface::ResourceManager(urdf, validate_interfaces, activate_all) - { - } -}; - -std::vector set_components_state( - TestableResourceManager & rm, const std::vector & components, const uint8_t state_id, - const std::string & state_name) -{ - auto int_components = components; - if (int_components.empty()) - { - int_components = {"TestActuatorHardware", "TestSensorHardware", "TestSystemHardware"}; - } - std::vector results; - for (const auto & component : int_components) - { - rclcpp_lifecycle::State state(state_id, state_name); - const auto result = rm.set_component_state(component, state); - results.push_back(result); - } - return results; -} - auto configure_components = [](TestableResourceManager & rm, const std::vector & components = {}) { @@ -445,68 +391,9 @@ TEST_F(ResourceManagerTest, default_prepare_perform_switch) // Activate components to get all interfaces available activate_components(rm); - EXPECT_TRUE(rm.prepare_command_mode_switch({""}, {""})); - EXPECT_TRUE(rm.perform_command_mode_switch({""}, {""})); -} - -const auto hardware_resources_command_modes = - R"( - - - test_hardware_components/TestSystemCommandModes - - - - - - - - - - - - - - -)"; -const auto command_mode_urdf = std::string(ros2_control_test_assets::urdf_head) + - std::string(hardware_resources_command_modes) + - std::string(ros2_control_test_assets::urdf_tail); - -TEST_F(ResourceManagerTest, custom_prepare_perform_switch) -{ - TestableResourceManager rm(command_mode_urdf); - // Scenarios defined by example criteria - std::vector empty_keys = {}; - std::vector irrelevant_keys = {"elbow_joint/position", "should_joint/position"}; - std::vector illegal_single_key = {"joint1/position"}; - std::vector legal_keys_position = {"joint1/position", "joint2/position"}; - std::vector legal_keys_velocity = {"joint1/velocity", "joint2/velocity"}; - // Default behavior for empty key lists - EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, empty_keys)); - - // Default behavior when given irrelevant keys - EXPECT_TRUE(rm.prepare_command_mode_switch(irrelevant_keys, irrelevant_keys)); - EXPECT_TRUE(rm.prepare_command_mode_switch(irrelevant_keys, empty_keys)); - EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, irrelevant_keys)); - - // The test hardware interface has a criteria that both joints must change mode - EXPECT_FALSE(rm.prepare_command_mode_switch(illegal_single_key, illegal_single_key)); - EXPECT_FALSE(rm.prepare_command_mode_switch(illegal_single_key, empty_keys)); - EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, illegal_single_key)); - - // Test legal start keys - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_position, legal_keys_position)); - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_velocity, legal_keys_velocity)); - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_position, empty_keys)); - EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, legal_keys_position)); - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_velocity, empty_keys)); - EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, legal_keys_velocity)); - - // Test rejection from perform_command_mode_switch, test hardware rejects empty start sets - EXPECT_TRUE(rm.perform_command_mode_switch(legal_keys_position, legal_keys_position)); - EXPECT_FALSE(rm.perform_command_mode_switch(empty_keys, empty_keys)); - EXPECT_FALSE(rm.perform_command_mode_switch(empty_keys, legal_keys_position)); + // Default behavior for empty key lists, e.g., when a Broadcaster is activated + EXPECT_TRUE(rm.prepare_command_mode_switch({}, {})); + EXPECT_TRUE(rm.perform_command_mode_switch({}, {})); } TEST_F(ResourceManagerTest, resource_status) diff --git a/hardware_interface/test/test_resource_manager.hpp b/hardware_interface/test/test_resource_manager.hpp new file mode 100644 index 0000000000..46a487f2ef --- /dev/null +++ b/hardware_interface/test/test_resource_manager.hpp @@ -0,0 +1,80 @@ +// Copyright 2023 ros2_control Developers +// +// 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. + +// Authors: Dr. Denis + +#ifndef TEST_RESOURCE_MANAGER_HPP_ +#define TEST_RESOURCE_MANAGER_HPP_ + +#include + +#include +#include + +#include "hardware_interface/resource_manager.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" + +class ResourceManagerTest : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() {} +}; + +// Forward declaration +namespace hardware_interface +{ +class ResourceStorage; +} + +class TestableResourceManager : public hardware_interface::ResourceManager +{ +public: + friend ResourceManagerTest; + + FRIEND_TEST(ResourceManagerTest, initialization_with_urdf_manual_validation); + FRIEND_TEST(ResourceManagerTest, post_initialization_add_components); + FRIEND_TEST(ResourceManagerTest, managing_controllers_reference_interfaces); + FRIEND_TEST(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle); + FRIEND_TEST(ResourceManagerTest, test_unitilizable_hardware_no_validation); + + TestableResourceManager() : hardware_interface::ResourceManager() {} + + TestableResourceManager( + const std::string & urdf, bool validate_interfaces = true, bool activate_all = false) + : hardware_interface::ResourceManager(urdf, validate_interfaces, activate_all) + { + } +}; + +std::vector set_components_state( + TestableResourceManager & rm, const std::vector & components, const uint8_t state_id, + const std::string & state_name) +{ + auto int_components = components; + if (int_components.empty()) + { + int_components = {"TestActuatorHardware", "TestSensorHardware", "TestSystemHardware"}; + } + std::vector results; + for (const auto & component : int_components) + { + rclcpp_lifecycle::State state(state_id, state_name); + const auto result = rm.set_component_state(component, state); + results.push_back(result); + } + return results; +} +#endif // TEST_RESOURCE_MANAGER_HPP_ diff --git a/hardware_interface/test/test_resource_manager_prepare_perform_switch.cpp b/hardware_interface/test/test_resource_manager_prepare_perform_switch.cpp new file mode 100644 index 0000000000..8f6ba8f99a --- /dev/null +++ b/hardware_interface/test/test_resource_manager_prepare_perform_switch.cpp @@ -0,0 +1,389 @@ +// Copyright 2023 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. + +// Authors: Dr. Denis + +#include "test_resource_manager.hpp" + +#include +#include + +#include "hardware_interface/loaned_state_interface.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +const auto hardware_resources_command_modes = + R"( + + + test_hardware_components/TestSystemCommandModes + + + + + + + + + + + + + + + + + test_actuator + + + + + + + + + )"; +const auto command_mode_urdf = std::string(ros2_control_test_assets::urdf_head) + + std::string(hardware_resources_command_modes) + + std::string(ros2_control_test_assets::urdf_tail); + +class ResourceManagerPreparePerformTest : public ResourceManagerTest +{ +public: + void SetUp() + { + ResourceManagerTest::SetUp(); + + rm_ = std::make_unique(command_mode_urdf); + ASSERT_EQ(1u, rm_->actuator_components_size()); + ASSERT_EQ(1u, rm_->system_components_size()); + + // empty call can be done at any time and it doesn't propagates to HW components - always true + ASSERT_TRUE(rm_->perform_command_mode_switch(empty_keys, empty_keys)); + // empty call can be done at any time and it doesn't propagates to HW components - always true + ASSERT_TRUE(rm_->perform_command_mode_switch(empty_keys, empty_keys)); + + // Set both HW to ACTIVE to claim interfaces. There should stay persistent because we are not + // cleaning them for now, so this is a good way to keep the access and "f* the system" + set_components_state( + *rm_, {"TestSystemCommandModes"}, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); + set_components_state( + *rm_, {"TestActuatorHardware"}, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); + // State to get feedback how many times "prepare_for_switch" is called + claimed_system_acceleration_state_ = std::make_unique( + rm_->claim_state_interface("joint1/acceleration")); + claimed_actuator_position_state_ = std::make_unique( + rm_->claim_state_interface("joint3/position")); + } + + void preconfigure_components( + const uint8_t system_state_id, const std::string syste_state_name, + const uint8_t actuator_state_id, const std::string actuator_state_name) + { + set_components_state(*rm_, {"TestSystemCommandModes"}, system_state_id, syste_state_name); + set_components_state(*rm_, {"TestActuatorHardware"}, actuator_state_id, actuator_state_name); + + auto status_map = rm_->get_components_status(); + EXPECT_EQ(status_map["TestSystemCommandModes"].state.id(), system_state_id); + EXPECT_EQ(status_map["TestActuatorHardware"].state.id(), actuator_state_id); + } + + std::unique_ptr rm_; + + std::unique_ptr claimed_system_acceleration_state_; + std::unique_ptr claimed_actuator_position_state_; + + // Scenarios defined by example criteria + std::vector empty_keys = {}; + std::vector non_existing_keys = {"elbow_joint/position", "should_joint/position"}; + std::vector legal_keys_system = {"joint1/position", "joint2/position"}; + std::vector legal_keys_actuator = {"joint3/position"}; +}; + +// System : ACTIVE +// Actuator: UNCONFIGURED +TEST_F( + ResourceManagerPreparePerformTest, + when_system_active_and_actuator_unconfigured_expect_system_passing) +{ + preconfigure_components( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active", + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, "unconfigured"); + + // Default behavior for empty key lists, e.g., when a Broadcaster is activated + EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, empty_keys)); + // empty call can be done at any time and it doesn't propagates to HW components - always true + EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, empty_keys)); + + // When TestSystemCommandModes is ACTIVE expect OK + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 1.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 101.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 102.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 202.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 203.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 303.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + + // When TestActuatorHardware is UNCONFIGURED expect OK + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 303.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 403.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 403.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 503.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + + EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 503.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 603.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); +}; + +// System : ACTIVE +// Actuator: INACTIVE +TEST_F( + ResourceManagerPreparePerformTest, when_system_active_and_actuator_inactive_expect_both_passing) +{ + preconfigure_components( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active", + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, "inactive"); + + // When TestSystemCommandModes is ACTIVE expect OK + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 1.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 1.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 101.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 101.0); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 102.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 102.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 202.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 202.0); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 203.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 203.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 303.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 303.0); + + // When TestActuatorHardware is INACTIVE expect OK + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 304.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 304.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 404.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 404.0); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 405.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 405.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 505.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 505.0); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 506.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 506.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 606.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 606.0); +}; + +// System : INACTIVE +// Actuator: ACTIVE +TEST_F( + ResourceManagerPreparePerformTest, when_system_inactive_and_actuator_active_expect_both_passing) +{ + preconfigure_components( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, "inactive", + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); + + // When TestSystemCommandModes is INACTIVE expect OK + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 1.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 1.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 101.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 101.0); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 102.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 102.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 202.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 202.0); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 203.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 203.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 303.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 303.0); + + // When TestActuatorHardware is ACTIVE expect OK + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 304.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 304.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 404.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 404.0); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 405.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 405.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 505.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 505.0); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 506.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 506.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 606.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 606.0); +}; + +// System : UNCONFIGURED +// Actuator: ACTIVE +TEST_F( + ResourceManagerPreparePerformTest, + when_system_unconfigured_and_actuator_active_expect_actuator_passing) +{ + preconfigure_components( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, "unconfigured", + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); + + // When TestSystemCommandModes is UNCONFIGURED expect error + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 100.0); + + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 100.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 200.0); + + EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 200.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 300.0); + + // When TestActuatorHardware is INACTIVE expect OK + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 301.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 401.0); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 402.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 502.0); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 503.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 603.0); +}; + +// System : UNCONFIGURED +// Actuator: FINALIZED +TEST_F( + ResourceManagerPreparePerformTest, + when_system_unconfigured_and_actuator_finalized_expect_none_passing) +{ + preconfigure_components( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, "unconfigured", + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, "finalized"); + + // When TestSystemCommandModes is UNCONFIGURED expect error + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + + EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + + // When TestActuatorHardware is INACTIVE expect OK + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + + EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); +};