Skip to content

Commit

Permalink
Finalized tests for deactivating controllers when write failes.
Browse files Browse the repository at this point in the history
  • Loading branch information
@destogl
destogl committed Jun 29, 2022
1 parent 55d8a20 commit 00eb08b
Showing 1 changed file with 288 additions and 15 deletions.
303 changes: 288 additions & 15 deletions controller_manager/test/test_controller_manager_hardware_error_handling.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,8 +44,8 @@ class TestableControllerManager : public controller_manager::ControllerManager
friend TestControllerManagerWithTestableCM;

FRIEND_TEST(TestControllerManagerWithTestableCM, check_cached_controllers_for_hardware);
FRIEND_TEST(TestControllerManagerWithTestableCM, stop_controllers_on_read_hardware_error);
FRIEND_TEST(TestControllerManagerWithTestableCM, stop_controllers_on_write_hardware_error);
FRIEND_TEST(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_error);
FRIEND_TEST(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_error);
FRIEND_TEST(TestControllerManagerWithTestableCM, stop_controllers_on_multiple_hardware_error);

public:
Expand Down Expand Up @@ -216,11 +216,15 @@ TEST_P(TestControllerManagerWithTestableCM, check_cached_controllers_for_hardwar
}
}

TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_read_hardware_error)
TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_error)
{
auto strictness = GetParam().strictness;
SetupAndConfigureControllers(strictness);

rclcpp_lifecycle::State state_active(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
hardware_interface::lifecycle_state_names::ACTIVE);

{
EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD));
EXPECT_GE(test_controller_actuator->internal_counter, 1u)
Expand Down Expand Up @@ -274,10 +278,6 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_read_hardware_er

// here happens error in hardware and
// "actuator controller" and "broadcaster all" are deactivated
auto [ok, failed_hardware_names] = cm_->resource_manager_->read(TIME, PERIOD);

EXPECT_FALSE(ok);

EXPECT_NO_THROW(cm_->read(TIME, PERIOD));
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
Expand All @@ -301,19 +301,208 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_read_hardware_er
}

// Recover hardware and activate again all controllers
{
ASSERT_EQ(
cm_->resource_manager_->set_component_state(
ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME, state_active),
hardware_interface::return_type::OK);
auto status_map = cm_->resource_manager_->get_components_status();
ASSERT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);

auto previous_counter_lower = test_controller_actuator->internal_counter;
auto previous_counter_higher = test_controller_system->internal_counter;

switch_test_controllers(
{TEST_CONTROLLER_ACTUATOR_NAME, TEST_BROADCASTER_ALL_NAME}, {}, strictness);

EXPECT_GT(test_controller_actuator->internal_counter, previous_counter_lower);
EXPECT_LE(test_controller_actuator->internal_counter, previous_counter_higher);
EXPECT_GT(test_controller_system->internal_counter, previous_counter_higher);
EXPECT_GT(test_broadcaster_all->internal_counter, previous_counter_lower);
EXPECT_LE(test_broadcaster_all->internal_counter, previous_counter_higher);
EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher);
}

// Simulate error in read() on TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME
// by setting first command interface to READ_FAIL_VALUE
test_controller_actuator->set_first_command_interface_value_to = READ_FAIL_VALUE;
test_controller_system->set_first_command_interface_value_to = READ_FAIL_VALUE;
{
auto previous_counter_lower = test_controller_actuator->internal_counter + 1;
auto previous_counter_higher = test_controller_system->internal_counter + 1;

EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD));

EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower)
<< "Execute without errors to write value";
EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher)
<< "Execute without errors to write value";
EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter_lower)
<< "Execute without errors to write value";
EXPECT_EQ(test_broadcaster_sensor->internal_counter, previous_counter_higher)
<< "Execute without errors to write value";
}

{
auto previous_counter_lower = test_controller_actuator->internal_counter;
auto previous_counter_higher = test_controller_system->internal_counter;
auto new_counter = test_broadcaster_sensor->internal_counter + 1;

EXPECT_NO_THROW(cm_->read(TIME, PERIOD));
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
test_controller_actuator->get_state().id());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id());

EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD));
EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower)
<< "Execute has read error and it is not updated";
EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher)
<< "Execute has read error and it is not updated";
EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter_lower)
<< "Broadcaster for all interfaces is not updated";
EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter)
<< "Execute without errors to write value";
}

// Recover hardware and activate again all controllers
{
ASSERT_EQ(
cm_->resource_manager_->set_component_state(
ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME, state_active),
hardware_interface::return_type::OK);
ASSERT_EQ(
cm_->resource_manager_->set_component_state(
ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME, state_active),
hardware_interface::return_type::OK);
auto status_map = cm_->resource_manager_->get_components_status();
ASSERT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
ASSERT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);

auto previous_counter_lower = test_controller_actuator->internal_counter;
auto previous_counter = test_controller_system->internal_counter;
auto previous_counter_higher = test_broadcaster_sensor->internal_counter;

switch_test_controllers(
{TEST_CONTROLLER_SYSTEM_NAME, TEST_CONTROLLER_ACTUATOR_NAME, TEST_BROADCASTER_ALL_NAME}, {},
strictness);

EXPECT_GT(test_controller_actuator->internal_counter, previous_counter_lower);
EXPECT_LE(test_controller_actuator->internal_counter, previous_counter_higher);
EXPECT_GT(test_controller_system->internal_counter, previous_counter);
EXPECT_LE(test_controller_system->internal_counter, previous_counter_higher);
EXPECT_GT(test_broadcaster_all->internal_counter, previous_counter_lower);
EXPECT_LE(test_broadcaster_all->internal_counter, previous_counter_higher);
EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher);
}
}

TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_error)
{
auto strictness = GetParam().strictness;
SetupAndConfigureControllers(strictness);

rclcpp_lifecycle::State state_active(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
hardware_interface::lifecycle_state_names::ACTIVE);
ASSERT_EQ(
cm_->resource_manager_->set_component_state(
ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME, state_active),
hardware_interface::return_type::OK);
auto status_map = cm_->resource_manager_->get_components_status();
ASSERT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);

{
EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD));
EXPECT_GE(test_controller_actuator->internal_counter, 1u)
<< "Controller is started at the end of update";
EXPECT_GE(test_controller_system->internal_counter, 1u)
<< "Controller is started at the end of update";
EXPECT_GE(test_broadcaster_all->internal_counter, 1u)
<< "Controller is started at the end of update";
EXPECT_GE(test_broadcaster_sensor->internal_counter, 1u)
<< "Controller is started at the end of update";
}

EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_actuator->get_state().id());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id());

// Execute first time without any errors
{
auto new_counter = test_controller_actuator->internal_counter + 1;
EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD));
EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors";
EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Execute without errors";
EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors";
EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) << "Execute without errors";
}

// Simulate error in write() on TEST_ACTUATOR_HARDWARE_NAME by setting first command interface to
// WRITE_FAIL_VALUE
test_controller_actuator->set_first_command_interface_value_to = WRITE_FAIL_VALUE;
{
auto new_counter = test_controller_actuator->internal_counter + 1;
EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD));
EXPECT_EQ(test_controller_actuator->internal_counter, new_counter)
<< "Execute without errors to write value";
EXPECT_EQ(test_controller_system->internal_counter, new_counter)
<< "Execute without errors to write value";
EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter)
<< "Execute without errors to write value";
EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter)
<< "Execute without errors to write value";
}

{
auto previous_counter = test_controller_actuator->internal_counter;
auto new_counter = test_controller_system->internal_counter + 1;

// here happens error in hardware and
// "actuator controller" and "broadcaster all" are deactivated
EXPECT_NO_THROW(cm_->write(TIME, PERIOD));
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
test_controller_actuator->get_state().id());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id());

EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD));
EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter)
<< "Execute without errors to write value";
EXPECT_EQ(test_controller_system->internal_counter, new_counter)
<< "Execute without errors to write value";
EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter)
<< "Execute without errors to write value";
EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter)
<< "Execute without errors to write value";
}

// Recover hardware and activate again all controllers
{
ASSERT_EQ(
cm_->resource_manager_->set_component_state(
ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME, state_active),
hardware_interface::return_type::OK);
auto status_map = cm_->resource_manager_->get_components_status();
ASSERT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);

auto previous_counter_lower = test_controller_actuator->internal_counter;
auto previous_counter_higher = test_controller_system->internal_counter;

Expand All @@ -327,6 +516,90 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_read_hardware_er
EXPECT_LE(test_broadcaster_all->internal_counter, previous_counter_higher);
EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher);
}

// Simulate error in write() on TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME
// by setting first command interface to WRITE_FAIL_VALUE
test_controller_actuator->set_first_command_interface_value_to = WRITE_FAIL_VALUE;
test_controller_system->set_first_command_interface_value_to = WRITE_FAIL_VALUE;
{
auto previous_counter_lower = test_controller_actuator->internal_counter + 1;
auto previous_counter_higher = test_controller_system->internal_counter + 1;

EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD));

EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower)
<< "Execute without errors to write value";
EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher)
<< "Execute without errors to write value";
EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter_lower)
<< "Execute without errors to write value";
EXPECT_EQ(test_broadcaster_sensor->internal_counter, previous_counter_higher)
<< "Execute without errors to write value";
}

{
auto previous_counter_lower = test_controller_actuator->internal_counter;
auto previous_counter_higher = test_controller_system->internal_counter;
auto new_counter = test_broadcaster_sensor->internal_counter + 1;

// here happens error in hardware and
// "actuator controller" and "broadcaster all" are deactivated
EXPECT_NO_THROW(cm_->write(TIME, PERIOD));
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
test_controller_actuator->get_state().id());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id());

EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD));
EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower)
<< "Execute has write error and it is not updated";
EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher)
<< "Execute has write error and it is not updated";
EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter_lower)
<< "Broadcaster for all interfaces is not updated";
EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter)
<< "Execute without errors to write value";
}

// Recover hardware and activate again all controllers
{
ASSERT_EQ(
cm_->resource_manager_->set_component_state(
ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME, state_active),
hardware_interface::return_type::OK);
ASSERT_EQ(
cm_->resource_manager_->set_component_state(
ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME, state_active),
hardware_interface::return_type::OK);
auto status_map = cm_->resource_manager_->get_components_status();
ASSERT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
ASSERT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);

auto previous_counter_lower = test_controller_actuator->internal_counter;
auto previous_counter = test_controller_system->internal_counter;
auto previous_counter_higher = test_broadcaster_sensor->internal_counter;

switch_test_controllers(
{TEST_CONTROLLER_SYSTEM_NAME, TEST_CONTROLLER_ACTUATOR_NAME, TEST_BROADCASTER_ALL_NAME}, {},
strictness);

EXPECT_GT(test_controller_actuator->internal_counter, previous_counter_lower);
EXPECT_LE(test_controller_actuator->internal_counter, previous_counter_higher);
EXPECT_GT(test_controller_system->internal_counter, previous_counter);
EXPECT_LE(test_controller_system->internal_counter, previous_counter_higher);
EXPECT_GT(test_broadcaster_all->internal_counter, previous_counter_lower);
EXPECT_LE(test_broadcaster_all->internal_counter, previous_counter_higher);
EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher);
}
}

INSTANTIATE_TEST_SUITE_P(
Expand Down

0 comments on commit 00eb08b

Please sign in to comment.