From c575b4e4ebdaa944b1b18cb7ad8b54b365440b4d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Tue, 12 Dec 2023 17:39:03 +0100 Subject: [PATCH 01/10] Add additional checks for non existing and not available interfaces. --- hardware_interface/src/resource_manager.cpp | 62 ++++++- .../test/test_resource_manager.cpp | 163 +++++++++++++++++- 2 files changed, 218 insertions(+), 7 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 5c5d5af3e6..4162ffa253 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,12 +1097,63 @@ bool ResourceManager::prepare_command_mode_switch( return ss.str(); }; + // 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) + { + bool all_exist = true; + for (const auto & interface : list_to_check) + { + 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; + } + + // 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) + { + bool all_available = true; + for (const auto & interface : list_to_check) + { + 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; + } + + // Check now if component allows the given interface combination for (auto & component : resource_storage_->actuators_) { if (return_type::OK != component.prepare_command_mode_switch(start_interfaces, stop_interfaces)) { RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Component '%s' did not accept new command resource combination: \n %s", + "resource_manager", "Component '%s' did not accept command interfaces combination: \n%s", component.get_name().c_str(), interfaces_to_string().c_str()); return false; } @@ -1105,7 +1163,7 @@ bool ResourceManager::prepare_command_mode_switch( if (return_type::OK != component.prepare_command_mode_switch(start_interfaces, stop_interfaces)) { RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Component '%s' did not accept new command resource combination: \n %s", + "resource_manager", "Component '%s' did not accept command interfaces combination: \n%s", component.get_name().c_str(), interfaces_to_string().c_str()); return false; } diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index 8246cc207d..1a24feca81 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -405,8 +405,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({""}, {""})); + // 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({}, {})); } const auto hardware_resources_command_modes = @@ -428,22 +429,174 @@ const auto hardware_resources_command_modes = + + + 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); +TEST_F(ResourceManagerTest, expect_prepare_perform_switch_to_work_only_when_hw_inactive_or_active) +{ + // Scenarios defined by example criteria + std::vector empty_keys = {}; + std::vector non_existing_keys = {"elbow_joint/position", "should_joint/position"}; + std::vector legal_keys_actuator = {"joint3/position"}; + std::vector legal_keys_system = {"joint1/position", "joint2/position"}; + + TestableResourceManager rm(command_mode_urdf); + EXPECT_EQ(1u, rm.actuator_components_size()); + EXPECT_EQ(1u, rm.system_components_size()); + + set_components_state( + rm, {"TestSystemCommandModes"}, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); + + { + auto status_map = rm.get_components_status(); + EXPECT_EQ( + status_map["TestSystemCommandModes"].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map["TestActuatorHardware"].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_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)); + + // When non existing keys are provided + EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, non_existing_keys)); + EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, empty_keys)); + EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, non_existing_keys)); + + // When TestActuatorHardware is UNCONFIGURED expect failure + EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + + // When TestSystemCommandModes is ACTIVE expect OK + EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, legal_keys_system)); + + set_components_state( + rm, {"TestActuatorHardware"}, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, "inactive"); + + { + auto status_map = rm.get_components_status(); + EXPECT_EQ( + status_map["TestSystemCommandModes"].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map["TestActuatorHardware"].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + } + + // Default behavior for empty key lists, e.g., when a Broadcaster is activated + EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, empty_keys)); + + // When non existing keys are provided + EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, non_existing_keys)); + EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, empty_keys)); + EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, non_existing_keys)); + + // When TestActuatorHardware is INACTIVE expect failure + EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + + // When TestSystemCommandModes is FINALIZED expect OK + EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, legal_keys_system)); + + set_components_state( + rm, {"TestSystemCommandModes"}, lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + "finalized"); + + { + auto status_map = rm.get_components_status(); + EXPECT_EQ( + status_map["TestSystemCommandModes"].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map["TestActuatorHardware"].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + } + + // Default behavior for empty key lists, e.g., when a Broadcaster is activated + EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, empty_keys)); + + // When non existing keys are provided + EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, non_existing_keys)); + EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, empty_keys)); + EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, non_existing_keys)); + + // When TestActuatorHardware is INACTIVE expect failure + EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + + // When TestSystemCommandModes is UNCONFIGURED expect OK + EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, legal_keys_system)); + + set_components_state( + rm, {"TestActuatorHardware"}, lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, "finalized"); + + { + auto status_map = rm.get_components_status(); + EXPECT_EQ( + status_map["TestSystemCommandModes"].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map["TestActuatorHardware"].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + } + + // Default behavior for empty key lists, e.g., when a Broadcaster is activated + EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, empty_keys)); + + // When non existing keys are provided + EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, non_existing_keys)); + EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, empty_keys)); + EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, non_existing_keys)); + + // When TestActuatorHardware is FINALIZED expect failure + EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + + // When TestSystemCommandModes is UNCONFIGURED expect OK + EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, legal_keys_system)); +} + TEST_F(ResourceManagerTest, custom_prepare_perform_switch) { TestableResourceManager rm(command_mode_urdf); + set_components_state( + rm, {"TestActuatorHardware"}, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, "inactive"); + set_components_state( + rm, {"TestSystemCommandModes"}, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); + // Scenarios defined by example criteria std::vector empty_keys = {}; - std::vector irrelevant_keys = {"elbow_joint/position", "should_joint/position"}; + std::vector irrelevant_keys = {"joint3/position", "joint3/max_velocity"}; 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)); From f9766e2d56c562d093cd82c33f2aa6d4f03db58f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Tue, 12 Dec 2023 18:49:15 +0100 Subject: [PATCH 02/10] Protect components from being called only in inactive and active states. --- hardware_interface/src/resource_manager.cpp | 34 +- .../mock_components/test_generic_system.cpp | 81 +++-- .../test/test_components/test_actuator.cpp | 8 + .../test_system_with_command_modes.cpp | 2 + .../test/test_resource_manager.cpp | 309 +++++++++++++----- 5 files changed, 298 insertions(+), 136 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 4162ffa253..542d4eff1f 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1147,25 +1147,39 @@ bool ResourceManager::prepare_command_mode_switch( return false; } + using lifecycle_msgs::msg::State; + // Check now if component allows the given interface combination for (auto & component : resource_storage_->actuators_) { - if (return_type::OK != component.prepare_command_mode_switch(start_interfaces, stop_interfaces)) + if ( + component.get_state().id() == State::PRIMARY_STATE_INACTIVE || + component.get_state().id() == State::PRIMARY_STATE_ACTIVE) { - 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()); - return false; + 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()); + return false; + } } } for (auto & component : resource_storage_->systems_) { - if (return_type::OK != component.prepare_command_mode_switch(start_interfaces, stop_interfaces)) + if ( + component.get_state().id() == State::PRIMARY_STATE_INACTIVE || + component.get_state().id() == State::PRIMARY_STATE_ACTIVE) { - 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()); - return false; + 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()); + return false; + } } } return true; 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 7cf55b50d3..60760cf8d9 100644 --- a/hardware_interface/test/test_components/test_actuator.cpp +++ b/hardware_interface/test/test_components/test_actuator.cpp @@ -74,6 +74,14 @@ 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; + } + 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..5bf06c9348 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(); diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index 1a24feca81..e22d3f382c 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -457,10 +457,28 @@ TEST_F(ResourceManagerTest, expect_prepare_perform_switch_to_work_only_when_hw_i EXPECT_EQ(1u, rm.actuator_components_size()); EXPECT_EQ(1u, rm.system_components_size()); + // 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 + auto claimed_system_acceleration_state = rm.claim_state_interface("joint1/acceleration"); + auto claimed_actuator_position_state = rm.claim_state_interface("joint3/position"); + // System : ACTIVE + // Actuator: UNCONFIGURED { + set_components_state( + rm, {"TestSystemCommandModes"}, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); + set_components_state( + rm, {"TestActuatorHardware"}, lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + "unconfigured"); + + double ACTUATOR_CALL_COUNTER_START_VALUE = 0.0; + double SYSTEM_CALL_COUNTER_START_VALUE = 0.0; + auto status_map = rm.get_components_status(); EXPECT_EQ( status_map["TestSystemCommandModes"].state.id(), @@ -468,30 +486,49 @@ TEST_F(ResourceManagerTest, expect_prepare_perform_switch_to_work_only_when_hw_i EXPECT_EQ( status_map["TestActuatorHardware"].state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_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)); - - // When non existing keys are provided - EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, non_existing_keys)); - EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, empty_keys)); - EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, non_existing_keys)); - - // When TestActuatorHardware is UNCONFIGURED expect failure - EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - // When TestSystemCommandModes is ACTIVE expect OK - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, legal_keys_system)); + // Default behavior for empty key lists, e.g., when a Broadcaster is activated + EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, empty_keys)); + + // When non existing keys are provided + EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, non_existing_keys)); + EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, empty_keys)); + EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, non_existing_keys)); + + // When TestActuatorHardware is UNCONFIGURED expect failure (not available) + EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 0.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); + EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 0.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); + EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 0.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); + + // When TestSystemCommandModes is ACTIVE expect OK + EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 0.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 1.0); + EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 0.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 2.0); + EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 0.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 3.0); + } + + // System : ACTIVE + // Actuator: INACTIVE + { + set_components_state( + rm, {"TestSystemCommandModes"}, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); + set_components_state( + rm, {"TestActuatorHardware"}, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, "inactive"); + + double ACTUATOR_CALL_COUNTER_START_VALUE = 0.0; + double SYSTEM_CALL_COUNTER_START_VALUE = 3.0; - set_components_state( - rm, {"TestActuatorHardware"}, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, "inactive"); - - { auto status_map = rm.get_components_status(); EXPECT_EQ( status_map["TestSystemCommandModes"].state.id(), @@ -499,62 +536,154 @@ TEST_F(ResourceManagerTest, expect_prepare_perform_switch_to_work_only_when_hw_i EXPECT_EQ( status_map["TestActuatorHardware"].state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - } - - // Default behavior for empty key lists, e.g., when a Broadcaster is activated - EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, empty_keys)); - // When non existing keys are provided - EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, non_existing_keys)); - EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, empty_keys)); - EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, non_existing_keys)); + // Default behavior for empty key lists, e.g., when a Broadcaster is activated + EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, empty_keys)); + + // When non existing keys are provided + EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, non_existing_keys)); + EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, empty_keys)); + EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, non_existing_keys)); + + // When TestActuatorHardware is INACTIVE expect OK + EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + // called first time because it was UNCONFIGURED + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 1.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 1.0); + EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 2.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 2.0); + EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 3.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 3.0); + + // When TestSystemCommandModes is ACTIVE expect OK + EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 4.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 4.0); + EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 5.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 5.0); + EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 6.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 6.0); + } + + // System : INACTIVE + // Actuator: ACTIVE + { + set_components_state( + rm, {"TestSystemCommandModes"}, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + "inactive"); + set_components_state( + rm, {"TestActuatorHardware"}, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); + + double ACTUATOR_CALL_COUNTER_START_VALUE = 6.0; + double SYSTEM_CALL_COUNTER_START_VALUE = 9.0; - // When TestActuatorHardware is INACTIVE expect failure - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - - // When TestSystemCommandModes is FINALIZED expect OK - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, legal_keys_system)); + auto status_map = rm.get_components_status(); + EXPECT_EQ( + status_map["TestSystemCommandModes"].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map["TestActuatorHardware"].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - set_components_state( - rm, {"TestSystemCommandModes"}, lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - "finalized"); + // Default behavior for empty key lists, e.g., when a Broadcaster is activated + EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, empty_keys)); + + // When non existing keys are provided + EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, non_existing_keys)); + EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, empty_keys)); + EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, non_existing_keys)); + + // When TestActuatorHardware is ACTIVE expect OK + EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 1.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 1.0); + EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 2.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 2.0); + EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 3.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 3.0); + + // When TestSystemCommandModes is INACTIVE expect OK + EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 4.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 4.0); + EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 5.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 5.0); + EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 6.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 6.0); + } + + // System : UNCONFIGURED + // Actuator: ACTIVE + { + set_components_state( + rm, {"TestSystemCommandModes"}, lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + "unconfigured"); + set_components_state( + rm, {"TestActuatorHardware"}, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); + + double SYSTEM_CALL_COUNTER_START_VALUE = 15.0; + double ACTUATOR_CALL_COUNTER_START_VALUE = 12.0; - { auto status_map = rm.get_components_status(); EXPECT_EQ( status_map["TestSystemCommandModes"].state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); EXPECT_EQ( status_map["TestActuatorHardware"].state.id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - } - - // Default behavior for empty key lists, e.g., when a Broadcaster is activated - EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, empty_keys)); - - // When non existing keys are provided - EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, non_existing_keys)); - EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, empty_keys)); - EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, non_existing_keys)); - - // When TestActuatorHardware is INACTIVE expect failure - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - - // When TestSystemCommandModes is UNCONFIGURED expect OK - EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, legal_keys_system)); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - set_components_state( - rm, {"TestActuatorHardware"}, lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, "finalized"); + // Default behavior for empty key lists, e.g., when a Broadcaster is activated + EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, empty_keys)); + + // When non existing keys are provided + EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, non_existing_keys)); + EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, empty_keys)); + EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, non_existing_keys)); + + // When TestActuatorHardware is ACTIVE expect OK + EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 1.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); + EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 2.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); + EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 3.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); + + // When TestSystemCommandModes is UNCONFIGURED expect failure (not available) + EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 3.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); + EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 3.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); + EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 3.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); + } + + // System : UNCONFIGURED + // Actuator: FINALIZED + { + set_components_state( + rm, {"TestSystemCommandModes"}, lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + "unconfigured"); + set_components_state( + rm, {"TestActuatorHardware"}, lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, + "finalized"); + + double ACTUATOR_CALL_COUNTER_START_VALUE = 15.0; + double SYSTEM_CALL_COUNTER_START_VALUE = 15.0; - { auto status_map = rm.get_components_status(); EXPECT_EQ( status_map["TestSystemCommandModes"].state.id(), @@ -562,25 +691,37 @@ TEST_F(ResourceManagerTest, expect_prepare_perform_switch_to_work_only_when_hw_i EXPECT_EQ( status_map["TestActuatorHardware"].state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); - } - // Default behavior for empty key lists, e.g., when a Broadcaster is activated - EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, empty_keys)); - - // When non existing keys are provided - EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, non_existing_keys)); - EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, empty_keys)); - EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, non_existing_keys)); - - // When TestActuatorHardware is FINALIZED expect failure - EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - - // When TestSystemCommandModes is UNCONFIGURED expect OK - EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, legal_keys_system)); + // Default behavior for empty key lists, e.g., when a Broadcaster is activated + EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, empty_keys)); + + // When non existing keys are provided + EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, non_existing_keys)); + EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, empty_keys)); + EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, non_existing_keys)); + + // When TestActuatorHardware is FINALIZED expect failure + EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 0.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); + EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 0.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); + EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 0.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); + + // When TestSystemCommandModes is UNCONFIGURED expect failure + EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 0.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); + EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 0.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); + EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 0.0); + EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); + } } TEST_F(ResourceManagerTest, custom_prepare_perform_switch) From 485b25fc40a09c35938ff6b012d9ae8c5f0fcaca Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Wed, 13 Dec 2023 16:41:50 +0100 Subject: [PATCH 03/10] Make reference interfaces of chainable controllers available in inactive state. --- controller_manager/src/controller_manager.cpp | 11 +++++++++++ ...llers_chaining_with_controller_manager.cpp | 19 +++++++------------ hardware_interface/src/resource_manager.cpp | 4 ++-- 3 files changed, 20 insertions(+), 14 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 2e703f4219..51f49331f7 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -643,6 +643,11 @@ controller_interface::return_type ControllerManager::unload_controller( RCLCPP_DEBUG(get_logger(), "Cleanup controller"); // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for // cleaning-up controllers? + // Controller will not be removed so make its reference interface available + if (controller.c->is_chainable()) + { + resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); + } controller.c->get_node()->cleanup(); executor_->remove_node(controller.c->get_node()->get_node_base_interface()); to.erase(found_it); @@ -799,6 +804,12 @@ controller_interface::return_type ControllerManager::configure_controller( // clear unused list rt_controllers_wrapper_.get_unused_list(guard).clear(); + // Controller will not be in INACTIVE state so make its reference interface available + if (controller->is_chainable()) + { + resource_manager_->make_controller_reference_interfaces_available(controller_name); + } + return controller_interface::return_type::OK; } diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index 0ebddd85e0..74533c2835 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -211,7 +211,7 @@ class TestControllerChainingWithControllerManager for (const auto & interface : {"pid_left_wheel_controller/velocity"}) { EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); - EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } @@ -223,7 +223,7 @@ class TestControllerChainingWithControllerManager for (const auto & interface : {"pid_right_wheel_controller/velocity"}) { EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); - EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } @@ -236,7 +236,7 @@ class TestControllerChainingWithControllerManager "diff_drive_controller/rot_z"}) { EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); - EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } @@ -250,7 +250,7 @@ class TestControllerChainingWithControllerManager "diff_drive_controller/rot_z"}) { EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); - EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } @@ -290,6 +290,7 @@ class TestControllerChainingWithControllerManager size_t expected_internal_counter, const controller_interface::return_type expected_return, bool deactivated, bool claimed_interfaces_from_hw = false) { + (void)claimed_interfaces_from_hw; for (const auto & interface : claimed_command_itfs) { EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); @@ -297,18 +298,12 @@ class TestControllerChainingWithControllerManager if ((expected_return == controller_interface::return_type::OK) != deactivated) { EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); EXPECT_TRUE(cm_->resource_manager_->command_interface_is_claimed(interface)); } else { - if (claimed_interfaces_from_hw) - { - EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); - } - else - { - EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); - } + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } } diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 542d4eff1f..8fe5cb3841 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1108,7 +1108,7 @@ bool ResourceManager::prepare_command_mode_switch( if (!command_interface_exists(interface)) { all_exist = false; - ss_not_existing << " " << interface << std::endl; + ss_not_existing << " " << interface << std::endl; } } return all_exist; @@ -1133,7 +1133,7 @@ bool ResourceManager::prepare_command_mode_switch( if (!command_interface_is_available(interface)) { all_available = false; - ss_not_available << " " << interface << std::endl; + ss_not_available << " " << interface << std::endl; } } return all_available; From d033cf8959d480fc1ab7bf423e51f33e9f80c26b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Sun, 17 Dec 2023 19:49:24 +0100 Subject: [PATCH 04/10] Revert "Make reference interfaces of chainable controllers available in inactive state." This reverts commit 485b25fc40a09c35938ff6b012d9ae8c5f0fcaca. --- controller_manager/src/controller_manager.cpp | 11 ----------- ...llers_chaining_with_controller_manager.cpp | 19 ++++++++++++------- hardware_interface/src/resource_manager.cpp | 4 ++-- 3 files changed, 14 insertions(+), 20 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 51f49331f7..2e703f4219 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -643,11 +643,6 @@ controller_interface::return_type ControllerManager::unload_controller( RCLCPP_DEBUG(get_logger(), "Cleanup controller"); // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for // cleaning-up controllers? - // Controller will not be removed so make its reference interface available - if (controller.c->is_chainable()) - { - resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); - } controller.c->get_node()->cleanup(); executor_->remove_node(controller.c->get_node()->get_node_base_interface()); to.erase(found_it); @@ -804,12 +799,6 @@ controller_interface::return_type ControllerManager::configure_controller( // clear unused list rt_controllers_wrapper_.get_unused_list(guard).clear(); - // Controller will not be in INACTIVE state so make its reference interface available - if (controller->is_chainable()) - { - resource_manager_->make_controller_reference_interfaces_available(controller_name); - } - return controller_interface::return_type::OK; } diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index 74533c2835..0ebddd85e0 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -211,7 +211,7 @@ class TestControllerChainingWithControllerManager for (const auto & interface : {"pid_left_wheel_controller/velocity"}) { EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); - EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } @@ -223,7 +223,7 @@ class TestControllerChainingWithControllerManager for (const auto & interface : {"pid_right_wheel_controller/velocity"}) { EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); - EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } @@ -236,7 +236,7 @@ class TestControllerChainingWithControllerManager "diff_drive_controller/rot_z"}) { EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); - EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } @@ -250,7 +250,7 @@ class TestControllerChainingWithControllerManager "diff_drive_controller/rot_z"}) { EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); - EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } @@ -290,7 +290,6 @@ class TestControllerChainingWithControllerManager size_t expected_internal_counter, const controller_interface::return_type expected_return, bool deactivated, bool claimed_interfaces_from_hw = false) { - (void)claimed_interfaces_from_hw; for (const auto & interface : claimed_command_itfs) { EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); @@ -298,12 +297,18 @@ class TestControllerChainingWithControllerManager if ((expected_return == controller_interface::return_type::OK) != deactivated) { EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); - EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); EXPECT_TRUE(cm_->resource_manager_->command_interface_is_claimed(interface)); } else { - EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); + if (claimed_interfaces_from_hw) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); + } + else + { + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); + } EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } } diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 8fe5cb3841..542d4eff1f 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1108,7 +1108,7 @@ bool ResourceManager::prepare_command_mode_switch( if (!command_interface_exists(interface)) { all_exist = false; - ss_not_existing << " " << interface << std::endl; + ss_not_existing << " " << interface << std::endl; } } return all_exist; @@ -1133,7 +1133,7 @@ bool ResourceManager::prepare_command_mode_switch( if (!command_interface_is_available(interface)) { all_available = false; - ss_not_available << " " << interface << std::endl; + ss_not_available << " " << interface << std::endl; } } return all_available; From 44e3b72d2ddf82598b66cc077d9a9ecb39db83ef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Sun, 17 Dec 2023 15:06:16 +0100 Subject: [PATCH 05/10] [RM] Separate tests for prepare and perform switch because they are long. --- hardware_interface/CMakeLists.txt | 4 + .../test/test_resource_manager.cpp | 408 +----------------- .../test/test_resource_manager.hpp | 79 ++++ ...esource_manager_prepare_perform_switch.cpp | 389 +++++++++++++++++ 4 files changed, 473 insertions(+), 407 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 bcd03a0a16..f0c9327582 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -113,6 +113,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/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index e22d3f382c..7b39935e75 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,58 +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); - - 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 = {}) { @@ -410,359 +357,6 @@ TEST_F(ResourceManagerTest, default_prepare_perform_switch) EXPECT_TRUE(rm.perform_command_mode_switch({}, {})); } -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); - -TEST_F(ResourceManagerTest, expect_prepare_perform_switch_to_work_only_when_hw_inactive_or_active) -{ - // Scenarios defined by example criteria - std::vector empty_keys = {}; - std::vector non_existing_keys = {"elbow_joint/position", "should_joint/position"}; - std::vector legal_keys_actuator = {"joint3/position"}; - std::vector legal_keys_system = {"joint1/position", "joint2/position"}; - - TestableResourceManager rm(command_mode_urdf); - EXPECT_EQ(1u, rm.actuator_components_size()); - EXPECT_EQ(1u, rm.system_components_size()); - - // 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 - auto claimed_system_acceleration_state = rm.claim_state_interface("joint1/acceleration"); - auto claimed_actuator_position_state = rm.claim_state_interface("joint3/position"); - - // System : ACTIVE - // Actuator: UNCONFIGURED - { - set_components_state( - rm, {"TestSystemCommandModes"}, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); - set_components_state( - rm, {"TestActuatorHardware"}, lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - "unconfigured"); - - double ACTUATOR_CALL_COUNTER_START_VALUE = 0.0; - double SYSTEM_CALL_COUNTER_START_VALUE = 0.0; - - auto status_map = rm.get_components_status(); - EXPECT_EQ( - status_map["TestSystemCommandModes"].state.id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - EXPECT_EQ( - status_map["TestActuatorHardware"].state.id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_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)); - - // When non existing keys are provided - EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, non_existing_keys)); - EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, empty_keys)); - EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, non_existing_keys)); - - // When TestActuatorHardware is UNCONFIGURED expect failure (not available) - EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 0.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); - EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 0.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); - EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 0.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); - - // When TestSystemCommandModes is ACTIVE expect OK - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 0.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 1.0); - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 0.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 2.0); - EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 0.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 3.0); - } - - // System : ACTIVE - // Actuator: INACTIVE - { - set_components_state( - rm, {"TestSystemCommandModes"}, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); - set_components_state( - rm, {"TestActuatorHardware"}, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, "inactive"); - - double ACTUATOR_CALL_COUNTER_START_VALUE = 0.0; - double SYSTEM_CALL_COUNTER_START_VALUE = 3.0; - - auto status_map = rm.get_components_status(); - EXPECT_EQ( - status_map["TestSystemCommandModes"].state.id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - EXPECT_EQ( - status_map["TestActuatorHardware"].state.id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - - // Default behavior for empty key lists, e.g., when a Broadcaster is activated - EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, empty_keys)); - - // When non existing keys are provided - EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, non_existing_keys)); - EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, empty_keys)); - EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, non_existing_keys)); - - // When TestActuatorHardware is INACTIVE expect OK - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - // called first time because it was UNCONFIGURED - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 1.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 1.0); - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 2.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 2.0); - EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 3.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 3.0); - - // When TestSystemCommandModes is ACTIVE expect OK - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 4.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 4.0); - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 5.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 5.0); - EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 6.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 6.0); - } - - // System : INACTIVE - // Actuator: ACTIVE - { - set_components_state( - rm, {"TestSystemCommandModes"}, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - "inactive"); - set_components_state( - rm, {"TestActuatorHardware"}, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); - - double ACTUATOR_CALL_COUNTER_START_VALUE = 6.0; - double SYSTEM_CALL_COUNTER_START_VALUE = 9.0; - - auto status_map = rm.get_components_status(); - EXPECT_EQ( - status_map["TestSystemCommandModes"].state.id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - EXPECT_EQ( - status_map["TestActuatorHardware"].state.id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - - // Default behavior for empty key lists, e.g., when a Broadcaster is activated - EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, empty_keys)); - - // When non existing keys are provided - EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, non_existing_keys)); - EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, empty_keys)); - EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, non_existing_keys)); - - // When TestActuatorHardware is ACTIVE expect OK - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 1.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 1.0); - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 2.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 2.0); - EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 3.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 3.0); - - // When TestSystemCommandModes is INACTIVE expect OK - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 4.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 4.0); - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 5.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 5.0); - EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 6.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 6.0); - } - - // System : UNCONFIGURED - // Actuator: ACTIVE - { - set_components_state( - rm, {"TestSystemCommandModes"}, lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - "unconfigured"); - set_components_state( - rm, {"TestActuatorHardware"}, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); - - double SYSTEM_CALL_COUNTER_START_VALUE = 15.0; - double ACTUATOR_CALL_COUNTER_START_VALUE = 12.0; - - auto status_map = rm.get_components_status(); - EXPECT_EQ( - status_map["TestSystemCommandModes"].state.id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - EXPECT_EQ( - status_map["TestActuatorHardware"].state.id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - - // Default behavior for empty key lists, e.g., when a Broadcaster is activated - EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, empty_keys)); - - // When non existing keys are provided - EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, non_existing_keys)); - EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, empty_keys)); - EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, non_existing_keys)); - - // When TestActuatorHardware is ACTIVE expect OK - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 1.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 2.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); - EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 3.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); - - // When TestSystemCommandModes is UNCONFIGURED expect failure (not available) - EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 3.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); - EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 3.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); - EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 3.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); - } - - // System : UNCONFIGURED - // Actuator: FINALIZED - { - set_components_state( - rm, {"TestSystemCommandModes"}, lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - "unconfigured"); - set_components_state( - rm, {"TestActuatorHardware"}, lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, - "finalized"); - - double ACTUATOR_CALL_COUNTER_START_VALUE = 15.0; - double SYSTEM_CALL_COUNTER_START_VALUE = 15.0; - - auto status_map = rm.get_components_status(); - EXPECT_EQ( - status_map["TestSystemCommandModes"].state.id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - EXPECT_EQ( - status_map["TestActuatorHardware"].state.id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); - - // Default behavior for empty key lists, e.g., when a Broadcaster is activated - EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, empty_keys)); - - // When non existing keys are provided - EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, non_existing_keys)); - EXPECT_FALSE(rm.prepare_command_mode_switch(non_existing_keys, empty_keys)); - EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, non_existing_keys)); - - // When TestActuatorHardware is FINALIZED expect failure - EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 0.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); - EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 0.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); - EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 0.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); - - // When TestSystemCommandModes is UNCONFIGURED expect failure - EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 0.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); - EXPECT_FALSE(rm.prepare_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 0.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); - EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_actuator_position_state.get_value(), ACTUATOR_CALL_COUNTER_START_VALUE + 0.0); - EXPECT_EQ(claimed_system_acceleration_state.get_value(), SYSTEM_CALL_COUNTER_START_VALUE + 0.0); - } -} - -TEST_F(ResourceManagerTest, custom_prepare_perform_switch) -{ - TestableResourceManager rm(command_mode_urdf); - set_components_state( - rm, {"TestActuatorHardware"}, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, "inactive"); - set_components_state( - rm, {"TestSystemCommandModes"}, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); - - // Scenarios defined by example criteria - std::vector empty_keys = {}; - std::vector irrelevant_keys = {"joint3/position", "joint3/max_velocity"}; - 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 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)); -} - TEST_F(ResourceManagerTest, resource_status) { TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); diff --git a/hardware_interface/test/test_resource_manager.hpp b/hardware_interface/test/test_resource_manager.hpp new file mode 100644 index 0000000000..ecb37d68d2 --- /dev/null +++ b/hardware_interface/test/test_resource_manager.hpp @@ -0,0 +1,79 @@ +// 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); + + 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); +}; From 0a6b107df1e12100f0144d9e5ef62a8f58ea1d57 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Sun, 17 Dec 2023 15:07:27 +0100 Subject: [PATCH 06/10] Optimize prepre/perform switch in RM and add additional documentation about it. --- .../hardware_interface/resource_manager.hpp | 6 +- hardware_interface/src/resource_manager.cpp | 97 ++++++++++--------- .../test/test_components/test_actuator.cpp | 8 ++ .../test_system_with_command_modes.cpp | 1 + 4 files changed, 65 insertions(+), 47 deletions(-) diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 92bde14817..cac4c3fbde 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 is is assumed that `prepare_command_mode_switch` is called just before this methods + * 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 542d4eff1f..cae7824466 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1147,42 +1147,35 @@ bool ResourceManager::prepare_command_mode_switch( return false; } - using lifecycle_msgs::msg::State; - - // Check now if component allows the given interface combination - for (auto & component : resource_storage_->actuators_) - { - if ( - component.get_state().id() == State::PRIMARY_STATE_INACTIVE || - component.get_state().id() == 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()); - return false; - } - } - } - for (auto & component : resource_storage_->systems_) + auto call_method_on_components = + [&start_interfaces, &stop_interfaces, &interfaces_to_string](auto & components) { - if ( - component.get_state().id() == State::PRIMARY_STATE_INACTIVE || - component.get_state().id() == State::PRIMARY_STATE_ACTIVE) + bool ret = true; + for (auto & component : components) { if ( - return_type::OK != component.prepare_command_mode_switch(start_interfaces, stop_interfaces)) + component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - 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()); - return false; + 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 true; + return ret; + }; + + const bool actuators_result = call_method_on_components(resource_storage_->actuators_); + const bool systems_result = call_method_on_components(resource_storage_->systems_); + + return actuators_result && systems_result; } // CM API: Called in "update"-thread @@ -1190,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_method_on_components = [&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_method_on_components(resource_storage_->actuators_); + const bool systems_result = call_method_on_components(resource_storage_->systems_); + + return actuators_result && systems_result; } // CM API: Called in "callback/slow"-thread diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp index 60760cf8d9..7c83cc8441 100644 --- a/hardware_interface/test/test_components/test_actuator.cpp +++ b/hardware_interface/test/test_components/test_actuator.cpp @@ -82,6 +82,14 @@ class TestActuator : public ActuatorInterface 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 5bf06c9348..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 @@ -168,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 From 3162b29e04012a3197907b32c99a58098177f522 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 3 Jan 2024 19:19:01 +0100 Subject: [PATCH 07/10] Make if nicer and more efficient Co-authored-by: Sai Kishor Kothakota --- hardware_interface/src/resource_manager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index cae7824466..cbcc4f1e72 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1113,7 +1113,7 @@ bool ResourceManager::prepare_command_mode_switch( } return all_exist; }; - if (!check_exist(start_interfaces) || !check_exist(stop_interfaces)) + if (!(check_exist(start_interfaces) && check_exist(stop_interfaces))) { ss_not_existing << "]" << std::endl; RCUTILS_LOG_ERROR_NAMED( @@ -1138,7 +1138,7 @@ bool ResourceManager::prepare_command_mode_switch( } return all_available; }; - if (!check_available(start_interfaces) || !check_available(stop_interfaces)) + if (!(check_available(start_interfaces) && check_available(stop_interfaces))) { ss_not_available << "]" << std::endl; RCUTILS_LOG_ERROR_NAMED( From 46559ce591026c09e533be1ba55cc285cf809a69 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 8 Jan 2024 19:59:14 +0100 Subject: [PATCH 08/10] Update hardware_interface/test/test_resource_manager.hpp --- hardware_interface/test/test_resource_manager.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/hardware_interface/test/test_resource_manager.hpp b/hardware_interface/test/test_resource_manager.hpp index ecb37d68d2..46a487f2ef 100644 --- a/hardware_interface/test/test_resource_manager.hpp +++ b/hardware_interface/test/test_resource_manager.hpp @@ -48,6 +48,7 @@ class TestableResourceManager : public hardware_interface::ResourceManager 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() {} From 77884b990190fa68b9469ff2d57d18c7606d86f1 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 8 Jan 2024 20:00:56 +0100 Subject: [PATCH 09/10] Fix some typos Co-authored-by: Bence Magyar --- .../include/hardware_interface/resource_manager.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index cac4c3fbde..146b903f46 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -346,7 +346,7 @@ 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 is is assumed that `prepare_command_mode_switch` is called just before this methods + * \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. From 80ef781bf527fcd1f9d32af357d8da2e4d4c4e98 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 9 Jan 2024 17:43:45 +0100 Subject: [PATCH 10/10] Rename lambda method for clarity. Co-authored-by: Bence Magyar Co-authored-by: Sai Kishor Kothakota --- hardware_interface/src/resource_manager.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index cbcc4f1e72..0cbb620c76 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1147,7 +1147,7 @@ bool ResourceManager::prepare_command_mode_switch( return false; } - auto call_method_on_components = + auto call_prepare_mode_switch = [&start_interfaces, &stop_interfaces, &interfaces_to_string](auto & components) { bool ret = true; @@ -1172,8 +1172,8 @@ bool ResourceManager::prepare_command_mode_switch( return ret; }; - const bool actuators_result = call_method_on_components(resource_storage_->actuators_); - const bool systems_result = call_method_on_components(resource_storage_->systems_); + 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; } @@ -1189,7 +1189,7 @@ bool ResourceManager::perform_command_mode_switch( return true; } - auto call_method_on_components = [&start_interfaces, &stop_interfaces](auto & components) + auto call_perform_mode_switch = [&start_interfaces, &stop_interfaces](auto & components) { bool ret = true; for (auto & component : components) @@ -1212,8 +1212,8 @@ bool ResourceManager::perform_command_mode_switch( return ret; }; - const bool actuators_result = call_method_on_components(resource_storage_->actuators_); - const bool systems_result = call_method_on_components(resource_storage_->systems_); + 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; }