Skip to content

Commit

Permalink
Protect components from being called only in inactive and active states.
Browse files Browse the repository at this point in the history
  • Loading branch information
@destogl
destogl committed Dec 17, 2023
1 parent c575b4e commit f9766e2
Show file tree
Hide file tree
Showing 5 changed files with 298 additions and 136 deletions.
34 changes: 24 additions & 10 deletions hardware_interface/src/resource_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
81 changes: 39 additions & 42 deletions hardware_interface/test/mock_components/test_generic_system.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@ class TestGenericSystem : public ::testing::Test
{
hardware_system_2dof_ =
R"(
<ros2_control name="HardwareSystem2dof" type="system">
<ros2_control name="MockHardwareSystem" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
</hardware>
Expand All @@ -72,7 +72,7 @@ class TestGenericSystem : public ::testing::Test

hardware_system_2dof_asymetric_ =
R"(
<ros2_control name="HardwareSystem2dofAsymetric" type="system">
<ros2_control name="MockHardwareSystem" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
</hardware>
Expand All @@ -93,7 +93,7 @@ class TestGenericSystem : public ::testing::Test

hardware_system_2dof_standard_interfaces_ =
R"(
<ros2_control name="HardwareSystem2dofStandardInterfaces" type="system">
<ros2_control name="MockHardwareSystem" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
</hardware>
Expand All @@ -118,7 +118,7 @@ class TestGenericSystem : public ::testing::Test

hardware_system_2dof_with_other_interface_ =
R"(
<ros2_control name="HardwareSystem2dofWithOtherInterface" type="system">
<ros2_control name="MockHardwareSystem" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
</hardware>
Expand Down Expand Up @@ -153,7 +153,7 @@ class TestGenericSystem : public ::testing::Test

hardware_system_2dof_with_sensor_ =
R"(
<ros2_control name="HardwareSystem2dofWithSensor" type="system">
<ros2_control name="MockHardwareSystem" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
</hardware>
Expand Down Expand Up @@ -181,7 +181,7 @@ class TestGenericSystem : public ::testing::Test

hardware_system_2dof_with_sensor_mock_command_ =
R"(
<ros2_control name="HardwareSystem2dofWithSensorMockCommand" type="system">
<ros2_control name="MockHardwareSystem" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
<param name="mock_sensor_commands">true</param>
Expand Down Expand Up @@ -210,7 +210,7 @@ class TestGenericSystem : public ::testing::Test

hardware_system_2dof_with_sensor_mock_command_True_ =
R"(
<ros2_control name="HardwareSystem2dofWithSensorMockCommandTrue" type="system">
<ros2_control name="MockHardwareSystem" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
<param name="mock_sensor_commands">True</param>
Expand Down Expand Up @@ -239,7 +239,7 @@ class TestGenericSystem : public ::testing::Test

hardware_system_2dof_with_mimic_joint_ =
R"(
<ros2_control name="HardwareSystem2dofWithMimicJoint" type="system">
<ros2_control name="MockHardwareSystem" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
</hardware>
Expand All @@ -264,7 +264,7 @@ class TestGenericSystem : public ::testing::Test

hardware_system_2dof_standard_interfaces_with_offset_ =
R"(
<ros2_control name="HardwareSystem2dofStandardInterfacesWithOffset" type="system">
<ros2_control name="MockHardwareSystem" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
<param name="position_state_following_offset">-3</param>
Expand All @@ -290,7 +290,7 @@ class TestGenericSystem : public ::testing::Test

hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_missing_ =
R"(
<ros2_control name="HardwareSystem2dofStandardInterfacesWithCustomInterfaceForOffsetMissing" type="system">
<ros2_control name="MockHardwareSystem" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
<param name="position_state_following_offset">-3</param>
Expand Down Expand Up @@ -321,7 +321,7 @@ class TestGenericSystem : public ::testing::Test

hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_ =
R"(
<ros2_control name="HardwareSystem2dofStandardInterfacesWithCustomInterfaceForOffset" type="system">
<ros2_control name="MockHardwareSystem" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
<param name="position_state_following_offset">-3</param>
Expand Down Expand Up @@ -352,7 +352,7 @@ class TestGenericSystem : public ::testing::Test

valid_urdf_ros2_control_system_robot_with_gpio_ =
R"(
<ros2_control name="ValidURDFros2controlSystemRobotWithGPIO" type="system">
<ros2_control name="MockHardwareSystem" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
<param name="example_param_write_for_sec">2</param>
Expand Down Expand Up @@ -389,7 +389,7 @@ class TestGenericSystem : public ::testing::Test

valid_urdf_ros2_control_system_robot_with_gpio_mock_command_ =
R"(
<ros2_control name="ValidURDFros2controlSystemRobotWithGPIOMockCommand" type="system">
<ros2_control name="MockHardwareSystem" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
<param name="mock_gpio_commands">true</param>
Expand Down Expand Up @@ -425,7 +425,7 @@ class TestGenericSystem : public ::testing::Test

valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_ =
R"(
<ros2_control name="ValidURDFros2controlSystemRobotWithGPIOMockCommandTrue" type="system">
<ros2_control name="MockHardwareSystem" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
<param name="mock_gpio_commands">True</param>
Expand Down Expand Up @@ -461,7 +461,7 @@ class TestGenericSystem : public ::testing::Test

sensor_with_initial_value_ =
R"(
<ros2_control name="SensorWithInitialValue" type="system">
<ros2_control name="MockHardwareSystem" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
</hardware>
Expand All @@ -481,7 +481,7 @@ class TestGenericSystem : public ::testing::Test

gpio_with_initial_value_ =
R"(
<ros2_control name="GPIOWithInitialValue" type="system">
<ros2_control name="MockHardwareSystem" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
</hardware>
Expand All @@ -495,7 +495,7 @@ class TestGenericSystem : public ::testing::Test

hardware_system_2dof_standard_interfaces_with_different_control_modes_ =
R"(
<ros2_control name="HardwareSystem2dofStandardInterfacesWithDifferentControlModes" type="system">
<ros2_control name="MockHardwareSystem" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
<param name="calculate_dynamics">true</param>
Expand Down Expand Up @@ -528,7 +528,7 @@ class TestGenericSystem : public ::testing::Test

valid_hardware_system_2dof_standard_interfaces_with_different_control_modes_ =
R"(
<ros2_control name="HardwareSystem2dofStandardInterfacesWithDifferentControlModes" type="system">
<ros2_control name="MockHardwareSystem" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
<param name="calculate_dynamics">true</param>
Expand Down Expand Up @@ -568,7 +568,7 @@ class TestGenericSystem : public ::testing::Test

disabled_commands_ =
R"(
<ros2_control name="DisabledCommands" type="system">
<ros2_control name="MockHardwareSystem" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
<param name="disable_commands">True</param>
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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)
Expand All @@ -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());
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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)
Expand All @@ -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(
Expand Down Expand Up @@ -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)
Expand All @@ -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)
Expand All @@ -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)
Expand All @@ -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();
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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)
Expand All @@ -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)
Expand All @@ -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());
Expand Down Expand Up @@ -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());
Expand All @@ -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());
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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<std::string> stop_interfaces;
return rm.prepare_command_mode_switch(start_interfaces, stop_interfaces);
Expand Down
Loading

0 comments on commit f9766e2

Please sign in to comment.