Skip to content

Commit f99d2ca

Browse files
shlok-mehndirattashlok-mehndiratta
committed
Add tests for hardware components exception handling
1 parent b99368e commit f99d2ca

5 files changed

Lines changed: 358 additions & 13 deletions

File tree

‎hardware_interface_testing/CMakeLists.txt‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,13 @@ if(BUILD_TESTING)
5555
ros2_control_test_assets::ros2_control_test_assets
5656
${lifecycle_msgs_TARGETS})
5757

58+
ament_add_gmock(test_rm_exception_handling test/test_rm_exception_handling.cpp)
59+
target_link_libraries(test_rm_exception_handling
60+
hardware_interface::hardware_interface
61+
rclcpp_lifecycle::rclcpp_lifecycle
62+
ros2_control_test_assets::ros2_control_test_assets
63+
${lifecycle_msgs_TARGETS})
64+
5865
endif()
5966

6067
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})

‎hardware_interface_testing/test/test_components/test_actuator.cpp‎

Lines changed: 53 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -46,19 +46,45 @@ class TestActuator : public ActuatorInterface
4646
return CallbackReturn::ERROR;
4747
}
4848

49-
/*
50-
* a hardware can optional prove for incorrect info here.
51-
*
52-
* // can only control one joint
53-
* if (get_hardware_info().joints.size() != 1) {return CallbackReturn::ERROR;}
54-
* // can only control in position
55-
* if (get_hardware_info().joints[0].command_interfaces.size() != 1) {return
56-
* CallbackReturn::ERROR;}
57-
* // can only give feedback state for position and velocity
58-
* if (get_hardware_info().joints[0].state_interfaces.size() != 2) {return
59-
* CallbackReturn::ERROR;}
60-
*/
49+
auto it = get_hardware_info().hardware_parameters.find("throw_on_read");
50+
if (it != get_hardware_info().hardware_parameters.end())
51+
{
52+
throw_on_read_ = (it->second == "true");
53+
}
54+
it = get_hardware_info().hardware_parameters.find("throw_on_write");
55+
if (it != get_hardware_info().hardware_parameters.end())
56+
{
57+
throw_on_write_ = (it->second == "true");
58+
}
59+
it = get_hardware_info().hardware_parameters.find("throw_on_configure");
60+
if (it != get_hardware_info().hardware_parameters.end())
61+
{
62+
throw_on_configure_ = (it->second == "true");
63+
}
64+
it = get_hardware_info().hardware_parameters.find("throw_on_activate");
65+
if (it != get_hardware_info().hardware_parameters.end())
66+
{
67+
throw_on_activate_ = (it->second == "true");
68+
}
69+
70+
return CallbackReturn::SUCCESS;
71+
}
6172

73+
CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override
74+
{
75+
if (throw_on_configure_)
76+
{
77+
throw std::runtime_error("Injected exception during on_configure!");
78+
}
79+
return CallbackReturn::SUCCESS;
80+
}
81+
82+
CallbackReturn on_activate(const rclcpp_lifecycle::State & /*previous_state*/) override
83+
{
84+
if (throw_on_activate_)
85+
{
86+
throw std::runtime_error("Injected exception during on_activate!");
87+
}
6288
return CallbackReturn::SUCCESS;
6389
}
6490

@@ -131,10 +157,15 @@ class TestActuator : public ActuatorInterface
131157

132158
return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period) override
133159
{
160+
if (throw_on_read_)
161+
{
162+
throw std::runtime_error("Exception from TestActuator::read() as requested by parameter.");
163+
}
164+
134165
if (get_hardware_info().is_async)
135166
{
136167
std::this_thread::sleep_for(
137-
std::chrono::milliseconds(1000 / (3 * get_hardware_info().rw_rate)));
168+
std::chrono::milliseconds(1000 / (6 * get_hardware_info().rw_rate)));
138169
}
139170
double vel_cmd = 0.0;
140171
std::ignore = velocity_command_->get_value(vel_cmd, false);
@@ -180,6 +211,11 @@ class TestActuator : public ActuatorInterface
180211

181212
return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
182213
{
214+
if (throw_on_write_)
215+
{
216+
throw std::runtime_error("Exception from TestActuator::write() as requested by parameter.");
217+
}
218+
183219
if (get_hardware_info().is_async)
184220
{
185221
std::this_thread::sleep_for(
@@ -212,6 +248,10 @@ class TestActuator : public ActuatorInterface
212248
}
213249

214250
private:
251+
bool throw_on_read_ = false;
252+
bool throw_on_write_ = false;
253+
bool throw_on_configure_ = false;
254+
bool throw_on_activate_ = false;
215255
StateInterface::SharedPtr position_state_;
216256
StateInterface::SharedPtr velocity_state_;
217257
CommandInterface::SharedPtr velocity_command_;

‎hardware_interface_testing/test/test_components/test_sensor.cpp‎

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,41 @@ class TestSensor : public SensorInterface
4343
get_hardware_info().name.c_str(), get_hardware_info().hardware_plugin_name.c_str());
4444
return CallbackReturn::ERROR;
4545
}
46+
47+
auto it = get_hardware_info().hardware_parameters.find("throw_on_read");
48+
if (it != get_hardware_info().hardware_parameters.end())
49+
{
50+
throw_on_read_ = (it->second == "true");
51+
}
52+
it = get_hardware_info().hardware_parameters.find("throw_on_configure");
53+
if (it != get_hardware_info().hardware_parameters.end())
54+
{
55+
throw_on_configure_ = (it->second == "true");
56+
}
57+
it = get_hardware_info().hardware_parameters.find("throw_on_activate");
58+
if (it != get_hardware_info().hardware_parameters.end())
59+
{
60+
throw_on_activate_ = (it->second == "true");
61+
}
62+
63+
return CallbackReturn::SUCCESS;
64+
}
65+
66+
CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override
67+
{
68+
if (throw_on_configure_)
69+
{
70+
throw std::runtime_error("Injected exception during on_configure!");
71+
}
72+
return CallbackReturn::SUCCESS;
73+
}
74+
75+
CallbackReturn on_activate(const rclcpp_lifecycle::State & /*previous_state*/) override
76+
{
77+
if (throw_on_activate_)
78+
{
79+
throw std::runtime_error("Injected exception during on_activate!");
80+
}
4681
return CallbackReturn::SUCCESS;
4782
}
4883

@@ -59,10 +94,17 @@ class TestSensor : public SensorInterface
5994

6095
return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
6196
{
97+
if (throw_on_read_)
98+
{
99+
throw std::runtime_error("Exception from TestSensor::read() as requested by parameter.");
100+
}
62101
return return_type::OK;
63102
}
64103

65104
private:
105+
bool throw_on_read_ = false;
106+
bool throw_on_configure_ = false;
107+
bool throw_on_activate_ = false;
66108
StateInterface::SharedPtr velocity_state_;
67109
};
68110

‎hardware_interface_testing/test/test_components/test_system.cpp‎

Lines changed: 54 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -63,6 +63,46 @@ class TestSystem : public SystemInterface
6363
get_hardware_info().name.c_str(), get_hardware_info().hardware_plugin_name.c_str());
6464
return CallbackReturn::ERROR;
6565
}
66+
67+
auto it = get_hardware_info().hardware_parameters.find("throw_on_read");
68+
if (it != get_hardware_info().hardware_parameters.end())
69+
{
70+
throw_on_read_ = (it->second == "true");
71+
}
72+
it = get_hardware_info().hardware_parameters.find("throw_on_write");
73+
if (it != get_hardware_info().hardware_parameters.end())
74+
{
75+
throw_on_write_ = (it->second == "true");
76+
}
77+
it = get_hardware_info().hardware_parameters.find("throw_on_configure");
78+
if (it != get_hardware_info().hardware_parameters.end())
79+
{
80+
throw_on_configure_ = (it->second == "true");
81+
}
82+
it = get_hardware_info().hardware_parameters.find("throw_on_activate");
83+
if (it != get_hardware_info().hardware_parameters.end())
84+
{
85+
throw_on_activate_ = (it->second == "true");
86+
}
87+
88+
return CallbackReturn::SUCCESS;
89+
}
90+
91+
CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override
92+
{
93+
if (throw_on_configure_)
94+
{
95+
throw std::runtime_error("Injected exception during on_configure!");
96+
}
97+
return CallbackReturn::SUCCESS;
98+
}
99+
100+
CallbackReturn on_activate(const rclcpp_lifecycle::State & /*previous_state*/) override
101+
{
102+
if (throw_on_activate_)
103+
{
104+
throw std::runtime_error("Injected exception during on_activate!");
105+
}
66106
return CallbackReturn::SUCCESS;
67107
}
68108

@@ -131,6 +171,11 @@ class TestSystem : public SystemInterface
131171

132172
return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
133173
{
174+
if (throw_on_read_)
175+
{
176+
throw std::runtime_error("Exception from TestSystem::read() as requested by parameter.");
177+
}
178+
134179
verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id());
135180
if (get_hardware_info().is_async)
136181
{
@@ -171,6 +216,11 @@ class TestSystem : public SystemInterface
171216

172217
return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
173218
{
219+
if (throw_on_write_)
220+
{
221+
throw std::runtime_error("Exception from TestSystem::write() as requested by parameter.");
222+
}
223+
174224
verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id());
175225
if (get_hardware_info().is_async)
176226
{
@@ -204,6 +254,10 @@ class TestSystem : public SystemInterface
204254
}
205255

206256
private:
257+
bool throw_on_read_ = false;
258+
bool throw_on_write_ = false;
259+
bool throw_on_configure_ = false;
260+
bool throw_on_activate_ = false;
207261
std::array<CommandInterface::SharedPtr, 2> velocity_command_;
208262
std::array<StateInterface::SharedPtr, 2> position_state_;
209263
std::array<StateInterface::SharedPtr, 2> velocity_state_;

0 commit comments

Comments
 (0)