Skip to content

Commit bd6911d

Browse files
fmauchdestogl
fmauch
and
destogl
authored
[ResourceManager] deactivate hardware from read/write return value (ros-controls#884)
* 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 <[email protected]>
1 parent 19c8ff8 commit bd6911d

File tree

8 files changed

+225
-21
lines changed

8 files changed

+225
-21
lines changed

‎controller_manager/test/test_controller_manager_hardware_error_handling.cpp

Lines changed: 7 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@
2222
#include "controller_manager_test_common.hpp"
2323
#include "hardware_interface/types/lifecycle_state_names.hpp"
2424
#include "lifecycle_msgs/msg/state.hpp"
25+
#include "ros2_control_test_assets/test_hardware_interface_constants.hpp"
2526
#include "test_controller/test_controller.hpp"
2627

2728
using ::testing::_;
@@ -162,10 +163,6 @@ class TestControllerManagerWithTestableCM
162163
{}, strictness);
163164
}
164165

165-
// values to set to hardware to simulate failure on read and write
166-
static constexpr double READ_FAIL_VALUE = 28282828.0;
167-
static constexpr double WRITE_FAIL_VALUE = 23232323.0;
168-
169166
static constexpr char TEST_CONTROLLER_ACTUATOR_NAME[] = "test_controller_actuator";
170167
static constexpr char TEST_CONTROLLER_SYSTEM_NAME[] = "test_controller_system";
171168
static constexpr char TEST_BROADCASTER_ALL_NAME[] = "test_broadcaster_all";
@@ -258,7 +255,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er
258255

259256
// Simulate error in read() on TEST_ACTUATOR_HARDWARE_NAME by setting first command interface to
260257
// READ_FAIL_VALUE
261-
test_controller_actuator->set_first_command_interface_value_to = READ_FAIL_VALUE;
258+
test_controller_actuator->set_first_command_interface_value_to = test_constants::READ_FAIL_VALUE;
262259
{
263260
auto new_counter = test_controller_actuator->internal_counter + 1;
264261
EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD));
@@ -327,8 +324,8 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er
327324

328325
// Simulate error in read() on TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME
329326
// by setting first command interface to READ_FAIL_VALUE
330-
test_controller_actuator->set_first_command_interface_value_to = READ_FAIL_VALUE;
331-
test_controller_system->set_first_command_interface_value_to = READ_FAIL_VALUE;
327+
test_controller_actuator->set_first_command_interface_value_to = test_constants::READ_FAIL_VALUE;
328+
test_controller_system->set_first_command_interface_value_to = test_constants::READ_FAIL_VALUE;
332329
{
333330
auto previous_counter_lower = test_controller_actuator->internal_counter + 1;
334331
auto previous_counter_higher = test_controller_system->internal_counter + 1;
@@ -450,7 +447,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e
450447

451448
// Simulate error in write() on TEST_ACTUATOR_HARDWARE_NAME by setting first command interface to
452449
// WRITE_FAIL_VALUE
453-
test_controller_actuator->set_first_command_interface_value_to = WRITE_FAIL_VALUE;
450+
test_controller_actuator->set_first_command_interface_value_to = test_constants::WRITE_FAIL_VALUE;
454451
{
455452
auto new_counter = test_controller_actuator->internal_counter + 1;
456453
EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD));
@@ -519,8 +516,8 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e
519516

520517
// Simulate error in write() on TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME
521518
// by setting first command interface to WRITE_FAIL_VALUE
522-
test_controller_actuator->set_first_command_interface_value_to = WRITE_FAIL_VALUE;
523-
test_controller_system->set_first_command_interface_value_to = WRITE_FAIL_VALUE;
519+
test_controller_actuator->set_first_command_interface_value_to = test_constants::WRITE_FAIL_VALUE;
520+
test_controller_system->set_first_command_interface_value_to = test_constants::WRITE_FAIL_VALUE;
524521
{
525522
auto previous_counter_lower = test_controller_actuator->internal_counter + 1;
526523
auto previous_counter_higher = test_controller_system->internal_counter + 1;

‎hardware_interface/CMakeLists.txt

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -101,7 +101,9 @@ if(BUILD_TESTING)
101101
test/test_components/test_system.cpp)
102102
target_link_libraries(test_components hardware_interface)
103103
ament_target_dependencies(test_components
104-
pluginlib)
104+
pluginlib
105+
ros2_control_test_assets
106+
)
105107
install(TARGETS test_components
106108
DESTINATION lib
107109
)

‎hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@ enum class return_type : std::uint8_t
2323
{
2424
OK = 0,
2525
ERROR = 1,
26+
DEACTIVATE = 2,
2627
};
2728

2829
} // namespace hardware_interface

‎hardware_interface/src/resource_manager.cpp

Lines changed: 19 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1259,12 +1259,24 @@ HardwareReadWriteStatus ResourceManager::read(
12591259
{
12601260
for (auto & component : components)
12611261
{
1262-
if (component.read(time, period) != return_type::OK)
1262+
auto ret_val = component.read(time, period);
1263+
if (ret_val == return_type::ERROR)
12631264
{
12641265
read_write_status.ok = false;
12651266
read_write_status.failed_hardware_names.push_back(component.get_name());
12661267
resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name());
12671268
}
1269+
else if (ret_val == return_type::DEACTIVATE)
1270+
{
1271+
resource_storage_->deactivate_hardware(component);
1272+
}
1273+
// If desired: automatic re-activation. We could add a flag for this...
1274+
// else
1275+
// {
1276+
// using lifecycle_msgs::msg::State;
1277+
// rclcpp_lifecycle::State state(State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE);
1278+
// set_component_state(component.get_name(), state);
1279+
// }
12681280
}
12691281
};
12701282

@@ -1287,12 +1299,17 @@ HardwareReadWriteStatus ResourceManager::write(
12871299
{
12881300
for (auto & component : components)
12891301
{
1290-
if (component.write(time, period) != return_type::OK)
1302+
auto ret_val = component.write(time, period);
1303+
if (ret_val == return_type::ERROR)
12911304
{
12921305
read_write_status.ok = false;
12931306
read_write_status.failed_hardware_names.push_back(component.get_name());
12941307
resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name());
12951308
}
1309+
else if (ret_val == return_type::DEACTIVATE)
1310+
{
1311+
resource_storage_->deactivate_hardware(component);
1312+
}
12961313
}
12971314
};
12981315

‎hardware_interface/test/test_components/test_actuator.cpp

Lines changed: 13 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616
#include <vector>
1717

1818
#include "hardware_interface/actuator_interface.hpp"
19+
#include "ros2_control_test_assets/test_hardware_interface_constants.hpp"
1920

2021
using hardware_interface::ActuatorInterface;
2122
using hardware_interface::CommandInterface;
@@ -76,12 +77,17 @@ class TestActuator : public ActuatorInterface
7677
return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
7778
{
7879
// simulate error on read
79-
if (velocity_command_ == 28282828.0)
80+
if (velocity_command_ == test_constants::READ_FAIL_VALUE)
8081
{
8182
// reset value to get out from error on the next call - simplifies CM tests
8283
velocity_command_ = 0.0;
8384
return return_type::ERROR;
8485
}
86+
// simulate deactivate on read
87+
if (velocity_command_ == test_constants::READ_DEACTIVATE_VALUE)
88+
{
89+
return return_type::DEACTIVATE;
90+
}
8591
// The next line is for the testing purposes. We need value to be changed to be sure that
8692
// the feedback from hardware to controllers in the chain is working as it should.
8793
// This makes value checks clearer and confirms there is no "state = command" line or some
@@ -93,12 +99,17 @@ class TestActuator : public ActuatorInterface
9399
return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
94100
{
95101
// simulate error on write
96-
if (velocity_command_ == 23232323.0)
102+
if (velocity_command_ == test_constants::WRITE_FAIL_VALUE)
97103
{
98104
// reset value to get out from error on the next call - simplifies CM tests
99105
velocity_command_ = 0.0;
100106
return return_type::ERROR;
101107
}
108+
// simulate deactivate on write
109+
if (velocity_command_ == test_constants::WRITE_DEACTIVATE_VALUE)
110+
{
111+
return return_type::DEACTIVATE;
112+
}
102113
return return_type::OK;
103114
}
104115

‎hardware_interface/test/test_components/test_system.cpp

Lines changed: 13 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818

1919
#include "hardware_interface/system_interface.hpp"
2020
#include "hardware_interface/types/hardware_interface_type_values.hpp"
21+
#include "ros2_control_test_assets/test_hardware_interface_constants.hpp"
2122

2223
using hardware_interface::CommandInterface;
2324
using hardware_interface::return_type;
@@ -81,24 +82,34 @@ class TestSystem : public SystemInterface
8182
return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
8283
{
8384
// simulate error on read
84-
if (velocity_command_[0] == 28282828)
85+
if (velocity_command_[0] == test_constants::READ_FAIL_VALUE)
8586
{
8687
// reset value to get out from error on the next call - simplifies CM tests
8788
velocity_command_[0] = 0.0;
8889
return return_type::ERROR;
8990
}
91+
// simulate deactivate on read
92+
if (velocity_command_[0] == test_constants::READ_DEACTIVATE_VALUE)
93+
{
94+
return return_type::DEACTIVATE;
95+
}
9096
return return_type::OK;
9197
}
9298

9399
return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
94100
{
95101
// simulate error on write
96-
if (velocity_command_[0] == 23232323)
102+
if (velocity_command_[0] == test_constants::WRITE_FAIL_VALUE)
97103
{
98104
// reset value to get out from error on the next call - simplifies CM tests
99105
velocity_command_[0] = 0.0;
100106
return return_type::ERROR;
101107
}
108+
// simulate deactivate on write
109+
if (velocity_command_[0] == test_constants::WRITE_DEACTIVATE_VALUE)
110+
{
111+
return return_type::DEACTIVATE;
112+
}
102113
return return_type::OK;
103114
}
104115

‎hardware_interface/test/test_resource_manager.cpp

Lines changed: 141 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@
2828
#include "lifecycle_msgs/msg/state.hpp"
2929
#include "rclcpp_lifecycle/state.hpp"
3030
#include "ros2_control_test_assets/descriptions.hpp"
31+
#include "ros2_control_test_assets/test_hardware_interface_constants.hpp"
3132

3233
using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES;
3334
using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME;
@@ -1538,6 +1539,122 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest
15381539
}
15391540
}
15401541

1542+
void check_read_or_write_deactivate(
1543+
FunctionT method_that_deactivates, FunctionT other_method, const double deactivate_value)
1544+
{
1545+
// define state to set components to
1546+
rclcpp_lifecycle::State state_active(
1547+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
1548+
hardware_interface::lifecycle_state_names::ACTIVE);
1549+
1550+
// deactivate for TEST_ACTUATOR_HARDWARE_NAME
1551+
claimed_itfs[0].set_value(deactivate_value);
1552+
claimed_itfs[1].set_value(deactivate_value - 10.0);
1553+
{
1554+
// deactivate on error
1555+
auto [ok, failed_hardware_names] = method_that_deactivates(time, duration);
1556+
EXPECT_TRUE(ok);
1557+
EXPECT_TRUE(failed_hardware_names.empty());
1558+
auto status_map = rm->get_components_status();
1559+
EXPECT_EQ(
1560+
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
1561+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
1562+
EXPECT_EQ(
1563+
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
1564+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
1565+
check_if_interface_available(true, true);
1566+
1567+
// reactivate
1568+
rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_active);
1569+
status_map = rm->get_components_status();
1570+
EXPECT_EQ(
1571+
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
1572+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
1573+
EXPECT_EQ(
1574+
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
1575+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
1576+
check_if_interface_available(true, true);
1577+
}
1578+
// write is sill OK
1579+
{
1580+
auto [ok, failed_hardware_names] = other_method(time, duration);
1581+
EXPECT_TRUE(ok);
1582+
EXPECT_TRUE(failed_hardware_names.empty());
1583+
check_if_interface_available(true, true);
1584+
}
1585+
1586+
// deactivate for TEST_SYSTEM_HARDWARE_NAME
1587+
claimed_itfs[0].set_value(deactivate_value - 10.0);
1588+
claimed_itfs[1].set_value(deactivate_value);
1589+
{
1590+
// deactivate on flag
1591+
auto [ok, failed_hardware_names] = method_that_deactivates(time, duration);
1592+
EXPECT_TRUE(ok);
1593+
EXPECT_TRUE(failed_hardware_names.empty());
1594+
auto status_map = rm->get_components_status();
1595+
EXPECT_EQ(
1596+
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
1597+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
1598+
EXPECT_EQ(
1599+
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
1600+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
1601+
check_if_interface_available(true, true);
1602+
// re-activate
1603+
rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_active);
1604+
status_map = rm->get_components_status();
1605+
EXPECT_EQ(
1606+
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
1607+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
1608+
EXPECT_EQ(
1609+
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
1610+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
1611+
check_if_interface_available(true, true);
1612+
}
1613+
// write is sill OK
1614+
{
1615+
auto [ok, failed_hardware_names] = other_method(time, duration);
1616+
EXPECT_TRUE(ok);
1617+
EXPECT_TRUE(failed_hardware_names.empty());
1618+
check_if_interface_available(true, true);
1619+
}
1620+
1621+
// deactivate both, TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME
1622+
claimed_itfs[0].set_value(deactivate_value);
1623+
claimed_itfs[1].set_value(deactivate_value);
1624+
{
1625+
// deactivate on flag
1626+
auto [ok, failed_hardware_names] = method_that_deactivates(time, duration);
1627+
EXPECT_TRUE(ok);
1628+
EXPECT_TRUE(failed_hardware_names.empty());
1629+
auto status_map = rm->get_components_status();
1630+
EXPECT_EQ(
1631+
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
1632+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
1633+
EXPECT_EQ(
1634+
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
1635+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
1636+
check_if_interface_available(true, true);
1637+
// re-activate
1638+
rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_active);
1639+
rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_active);
1640+
status_map = rm->get_components_status();
1641+
EXPECT_EQ(
1642+
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
1643+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
1644+
EXPECT_EQ(
1645+
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
1646+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
1647+
check_if_interface_available(true, true);
1648+
}
1649+
// write is sill OK
1650+
{
1651+
auto [ok, failed_hardware_names] = other_method(time, duration);
1652+
EXPECT_TRUE(ok);
1653+
EXPECT_TRUE(failed_hardware_names.empty());
1654+
check_if_interface_available(true, true);
1655+
}
1656+
}
1657+
15411658
public:
15421659
std::shared_ptr<TestableResourceManager> rm;
15431660
std::vector<hardware_interface::LoanedCommandInterface> claimed_itfs;
@@ -1546,8 +1663,6 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest
15461663
const rclcpp::Duration duration = rclcpp::Duration::from_seconds(0.01);
15471664

15481665
// values to set to hardware to simulate failure on read and write
1549-
static constexpr double READ_FAIL_VALUE = 28282828.0;
1550-
static constexpr double WRITE_FAIL_VALUE = 23232323.0;
15511666
};
15521667

15531668
TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_read)
@@ -1558,7 +1673,7 @@ TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_read)
15581673
// check read methods failures
15591674
check_read_or_write_failure(
15601675
std::bind(&TestableResourceManager::read, rm, _1, _2),
1561-
std::bind(&TestableResourceManager::write, rm, _1, _2), READ_FAIL_VALUE);
1676+
std::bind(&TestableResourceManager::write, rm, _1, _2), test_constants::READ_FAIL_VALUE);
15621677
}
15631678

15641679
TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_write)
@@ -1569,7 +1684,29 @@ TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_write)
15691684
// check write methods failures
15701685
check_read_or_write_failure(
15711686
std::bind(&TestableResourceManager::write, rm, _1, _2),
1572-
std::bind(&TestableResourceManager::read, rm, _1, _2), WRITE_FAIL_VALUE);
1687+
std::bind(&TestableResourceManager::read, rm, _1, _2), test_constants::WRITE_FAIL_VALUE);
1688+
}
1689+
1690+
TEST_F(ResourceManagerTestReadWriteError, handle_deactivate_on_hardware_read)
1691+
{
1692+
setup_resource_manager_and_do_initial_checks();
1693+
1694+
using namespace std::placeholders;
1695+
// check read methods failures
1696+
check_read_or_write_deactivate(
1697+
std::bind(&TestableResourceManager::read, rm, _1, _2),
1698+
std::bind(&TestableResourceManager::write, rm, _1, _2), test_constants::READ_DEACTIVATE_VALUE);
1699+
}
1700+
1701+
TEST_F(ResourceManagerTestReadWriteError, handle_deactivate_on_hardware_write)
1702+
{
1703+
setup_resource_manager_and_do_initial_checks();
1704+
1705+
using namespace std::placeholders;
1706+
// check write methods failures
1707+
check_read_or_write_deactivate(
1708+
std::bind(&TestableResourceManager::write, rm, _1, _2),
1709+
std::bind(&TestableResourceManager::read, rm, _1, _2), test_constants::WRITE_DEACTIVATE_VALUE);
15731710
}
15741711

15751712
TEST_F(ResourceManagerTest, test_caching_of_controllers_to_hardware)

0 commit comments

Comments
 (0)