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)