Skip to content

Commit

Permalink
[ResourceManager] deactivate hardware from read/write return value (#884
Browse files Browse the repository at this point in the history 
)

* Deactivate hardware from read/write return value
* Added tests to DEACTIVATE return value
* Use constants for defining of special test-values for different HW behaviors

---------

Co-authored-by: Dr. Denis <denis@stoglrobotics.de>
  • Loading branch information
@fmauch @destogl
fmauch and destogl authored Oct 16, 2023
1 parent 19c8ff8 commit bd6911d
Show file tree
Hide file tree
Showing 8 changed files with 225 additions and 21 deletions.
17 changes: 7 additions & 10 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 @@ -22,6 +22,7 @@
#include "controller_manager_test_common.hpp"
#include "hardware_interface/types/lifecycle_state_names.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "ros2_control_test_assets/test_hardware_interface_constants.hpp"
#include "test_controller/test_controller.hpp"

using ::testing::_;
Expand Down Expand Up @@ -162,10 +163,6 @@ class TestControllerManagerWithTestableCM
{}, strictness);
}

// values to set to hardware to simulate failure on read and write
static constexpr double READ_FAIL_VALUE = 28282828.0;
static constexpr double WRITE_FAIL_VALUE = 23232323.0;

static constexpr char TEST_CONTROLLER_ACTUATOR_NAME[] = "test_controller_actuator";
static constexpr char TEST_CONTROLLER_SYSTEM_NAME[] = "test_controller_system";
static constexpr char TEST_BROADCASTER_ALL_NAME[] = "test_broadcaster_all";
Expand Down Expand Up @@ -258,7 +255,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er

// Simulate error in read() on TEST_ACTUATOR_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_actuator->set_first_command_interface_value_to = test_constants::READ_FAIL_VALUE;
{
auto new_counter = test_controller_actuator->internal_counter + 1;
EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD));
Expand Down Expand Up @@ -327,8 +324,8 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er

// 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;
test_controller_actuator->set_first_command_interface_value_to = test_constants::READ_FAIL_VALUE;
test_controller_system->set_first_command_interface_value_to = test_constants::READ_FAIL_VALUE;
{
auto previous_counter_lower = test_controller_actuator->internal_counter + 1;
auto previous_counter_higher = test_controller_system->internal_counter + 1;
Expand Down Expand Up @@ -450,7 +447,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e

// 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;
test_controller_actuator->set_first_command_interface_value_to = test_constants::WRITE_FAIL_VALUE;
{
auto new_counter = test_controller_actuator->internal_counter + 1;
EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD));
Expand Down Expand Up @@ -519,8 +516,8 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e

// 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;
test_controller_actuator->set_first_command_interface_value_to = test_constants::WRITE_FAIL_VALUE;
test_controller_system->set_first_command_interface_value_to = test_constants::WRITE_FAIL_VALUE;
{
auto previous_counter_lower = test_controller_actuator->internal_counter + 1;
auto previous_counter_higher = test_controller_system->internal_counter + 1;
Expand Down
4 changes: 3 additions & 1 deletion hardware_interface/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,7 +101,9 @@ if(BUILD_TESTING)
test/test_components/test_system.cpp)
target_link_libraries(test_components hardware_interface)
ament_target_dependencies(test_components
pluginlib)
pluginlib
ros2_control_test_assets
)
install(TARGETS test_components
DESTINATION lib
)
Expand Down
1 change: 1 addition & 0 deletions hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@ enum class return_type : std::uint8_t
{
OK = 0,
ERROR = 1,
DEACTIVATE = 2,
};

} // namespace hardware_interface
Expand Down
21 changes: 19 additions & 2 deletions hardware_interface/src/resource_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1259,12 +1259,24 @@ HardwareReadWriteStatus ResourceManager::read(
{
for (auto & component : components)
{
if (component.read(time, period) != return_type::OK)
auto ret_val = component.read(time, period);
if (ret_val == return_type::ERROR)
{
read_write_status.ok = false;
read_write_status.failed_hardware_names.push_back(component.get_name());
resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name());
}
else if (ret_val == return_type::DEACTIVATE)
{
resource_storage_->deactivate_hardware(component);
}
// If desired: automatic re-activation. We could add a flag for this...
// else
// {
// using lifecycle_msgs::msg::State;
// rclcpp_lifecycle::State state(State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE);
// set_component_state(component.get_name(), state);
// }
}
};

Expand All @@ -1287,12 +1299,17 @@ HardwareReadWriteStatus ResourceManager::write(
{
for (auto & component : components)
{
if (component.write(time, period) != return_type::OK)
auto ret_val = component.write(time, period);
if (ret_val == return_type::ERROR)
{
read_write_status.ok = false;
read_write_status.failed_hardware_names.push_back(component.get_name());
resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name());
}
else if (ret_val == return_type::DEACTIVATE)
{
resource_storage_->deactivate_hardware(component);
}
}
};

Expand Down
15 changes: 13 additions & 2 deletions hardware_interface/test/test_components/test_actuator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
#include <vector>

#include "hardware_interface/actuator_interface.hpp"
#include "ros2_control_test_assets/test_hardware_interface_constants.hpp"

using hardware_interface::ActuatorInterface;
using hardware_interface::CommandInterface;
Expand Down Expand Up @@ -76,12 +77,17 @@ class TestActuator : public ActuatorInterface
return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
{
// simulate error on read
if (velocity_command_ == 28282828.0)
if (velocity_command_ == test_constants::READ_FAIL_VALUE)
{
// reset value to get out from error on the next call - simplifies CM tests
velocity_command_ = 0.0;
return return_type::ERROR;
}
// simulate deactivate on read
if (velocity_command_ == test_constants::READ_DEACTIVATE_VALUE)
{
return return_type::DEACTIVATE;
}
// The next line is for the testing purposes. We need value to be changed to be sure that
// the feedback from hardware to controllers in the chain is working as it should.
// This makes value checks clearer and confirms there is no "state = command" line or some
Expand All @@ -93,12 +99,17 @@ class TestActuator : public ActuatorInterface
return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
{
// simulate error on write
if (velocity_command_ == 23232323.0)
if (velocity_command_ == test_constants::WRITE_FAIL_VALUE)
{
// reset value to get out from error on the next call - simplifies CM tests
velocity_command_ = 0.0;
return return_type::ERROR;
}
// simulate deactivate on write
if (velocity_command_ == test_constants::WRITE_DEACTIVATE_VALUE)
{
return return_type::DEACTIVATE;
}
return return_type::OK;
}

Expand Down
15 changes: 13 additions & 2 deletions hardware_interface/test/test_components/test_system.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@

#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "ros2_control_test_assets/test_hardware_interface_constants.hpp"

using hardware_interface::CommandInterface;
using hardware_interface::return_type;
Expand Down Expand Up @@ -81,24 +82,34 @@ class TestSystem : public SystemInterface
return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
{
// simulate error on read
if (velocity_command_[0] == 28282828)
if (velocity_command_[0] == test_constants::READ_FAIL_VALUE)
{
// reset value to get out from error on the next call - simplifies CM tests
velocity_command_[0] = 0.0;
return return_type::ERROR;
}
// simulate deactivate on read
if (velocity_command_[0] == test_constants::READ_DEACTIVATE_VALUE)
{
return return_type::DEACTIVATE;
}
return return_type::OK;
}

return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
{
// simulate error on write
if (velocity_command_[0] == 23232323)
if (velocity_command_[0] == test_constants::WRITE_FAIL_VALUE)
{
// reset value to get out from error on the next call - simplifies CM tests
velocity_command_[0] = 0.0;
return return_type::ERROR;
}
// simulate deactivate on write
if (velocity_command_[0] == test_constants::WRITE_DEACTIVATE_VALUE)
{
return return_type::DEACTIVATE;
}
return return_type::OK;
}

Expand Down
145 changes: 141 additions & 4 deletions hardware_interface/test/test_resource_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "ros2_control_test_assets/descriptions.hpp"
#include "ros2_control_test_assets/test_hardware_interface_constants.hpp"

using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES;
using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME;
Expand Down Expand Up @@ -1538,6 +1539,122 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest
}
}

void check_read_or_write_deactivate(
FunctionT method_that_deactivates, FunctionT other_method, const double deactivate_value)
{
// define state to set components to
rclcpp_lifecycle::State state_active(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
hardware_interface::lifecycle_state_names::ACTIVE);

// deactivate for TEST_ACTUATOR_HARDWARE_NAME
claimed_itfs[0].set_value(deactivate_value);
claimed_itfs[1].set_value(deactivate_value - 10.0);
{
// deactivate on error
auto [ok, failed_hardware_names] = method_that_deactivates(time, duration);
EXPECT_TRUE(ok);
EXPECT_TRUE(failed_hardware_names.empty());
auto status_map = rm->get_components_status();
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
check_if_interface_available(true, true);

// reactivate
rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_active);
status_map = rm->get_components_status();
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
check_if_interface_available(true, true);
}
// write is sill OK
{
auto [ok, failed_hardware_names] = other_method(time, duration);
EXPECT_TRUE(ok);
EXPECT_TRUE(failed_hardware_names.empty());
check_if_interface_available(true, true);
}

// deactivate for TEST_SYSTEM_HARDWARE_NAME
claimed_itfs[0].set_value(deactivate_value - 10.0);
claimed_itfs[1].set_value(deactivate_value);
{
// deactivate on flag
auto [ok, failed_hardware_names] = method_that_deactivates(time, duration);
EXPECT_TRUE(ok);
EXPECT_TRUE(failed_hardware_names.empty());
auto status_map = rm->get_components_status();
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
check_if_interface_available(true, true);
// re-activate
rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_active);
status_map = rm->get_components_status();
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
check_if_interface_available(true, true);
}
// write is sill OK
{
auto [ok, failed_hardware_names] = other_method(time, duration);
EXPECT_TRUE(ok);
EXPECT_TRUE(failed_hardware_names.empty());
check_if_interface_available(true, true);
}

// deactivate both, TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME
claimed_itfs[0].set_value(deactivate_value);
claimed_itfs[1].set_value(deactivate_value);
{
// deactivate on flag
auto [ok, failed_hardware_names] = method_that_deactivates(time, duration);
EXPECT_TRUE(ok);
EXPECT_TRUE(failed_hardware_names.empty());
auto status_map = rm->get_components_status();
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
check_if_interface_available(true, true);
// re-activate
rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_active);
rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_active);
status_map = rm->get_components_status();
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
check_if_interface_available(true, true);
}
// write is sill OK
{
auto [ok, failed_hardware_names] = other_method(time, duration);
EXPECT_TRUE(ok);
EXPECT_TRUE(failed_hardware_names.empty());
check_if_interface_available(true, true);
}
}

public:
std::shared_ptr<TestableResourceManager> rm;
std::vector<hardware_interface::LoanedCommandInterface> claimed_itfs;
Expand All @@ -1546,8 +1663,6 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest
const rclcpp::Duration duration = rclcpp::Duration::from_seconds(0.01);

// values to set to hardware to simulate failure on read and write
static constexpr double READ_FAIL_VALUE = 28282828.0;
static constexpr double WRITE_FAIL_VALUE = 23232323.0;
};

TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_read)
Expand All @@ -1558,7 +1673,7 @@ TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_read)
// check read methods failures
check_read_or_write_failure(
std::bind(&TestableResourceManager::read, rm, _1, _2),
std::bind(&TestableResourceManager::write, rm, _1, _2), READ_FAIL_VALUE);
std::bind(&TestableResourceManager::write, rm, _1, _2), test_constants::READ_FAIL_VALUE);
}

TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_write)
Expand All @@ -1569,7 +1684,29 @@ TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_write)
// check write methods failures
check_read_or_write_failure(
std::bind(&TestableResourceManager::write, rm, _1, _2),
std::bind(&TestableResourceManager::read, rm, _1, _2), WRITE_FAIL_VALUE);
std::bind(&TestableResourceManager::read, rm, _1, _2), test_constants::WRITE_FAIL_VALUE);
}

TEST_F(ResourceManagerTestReadWriteError, handle_deactivate_on_hardware_read)
{
setup_resource_manager_and_do_initial_checks();

using namespace std::placeholders;
// check read methods failures
check_read_or_write_deactivate(
std::bind(&TestableResourceManager::read, rm, _1, _2),
std::bind(&TestableResourceManager::write, rm, _1, _2), test_constants::READ_DEACTIVATE_VALUE);
}

TEST_F(ResourceManagerTestReadWriteError, handle_deactivate_on_hardware_write)
{
setup_resource_manager_and_do_initial_checks();

using namespace std::placeholders;
// check write methods failures
check_read_or_write_deactivate(
std::bind(&TestableResourceManager::write, rm, _1, _2),
std::bind(&TestableResourceManager::read, rm, _1, _2), test_constants::WRITE_DEACTIVATE_VALUE);
}

TEST_F(ResourceManagerTest, test_caching_of_controllers_to_hardware)
Expand Down
Loading

0 comments on commit bd6911d

Please sign in to comment.