Skip to content

Commit c312bd2

Browse files
shlok-mehndirattashlok-mehndiratta
committed
Add tests for hardware components exception handling
1 parent 0870f0e commit c312bd2

5 files changed

Lines changed: 588 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: 55 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,8 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15+
#include <memory>
16+
#include <stdexcept>
1517
#include <vector>
1618

1719
#include "hardware_interface/actuator_interface.hpp"
@@ -46,19 +48,45 @@ class TestActuator : public ActuatorInterface
4648
return CallbackReturn::ERROR;
4749
}
4850

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-
*/
51+
auto it = get_hardware_info().hardware_parameters.find("throw_on_read");
52+
if (it != get_hardware_info().hardware_parameters.end())
53+
{
54+
throw_on_read_ = (it->second == "true");
55+
}
56+
it = get_hardware_info().hardware_parameters.find("throw_on_write");
57+
if (it != get_hardware_info().hardware_parameters.end())
58+
{
59+
throw_on_write_ = (it->second == "true");
60+
}
61+
it = get_hardware_info().hardware_parameters.find("throw_on_configure");
62+
if (it != get_hardware_info().hardware_parameters.end())
63+
{
64+
throw_on_configure_ = (it->second == "true");
65+
}
66+
it = get_hardware_info().hardware_parameters.find("throw_on_activate");
67+
if (it != get_hardware_info().hardware_parameters.end())
68+
{
69+
throw_on_activate_ = (it->second == "true");
70+
}
71+
72+
return CallbackReturn::SUCCESS;
73+
}
6174

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

@@ -131,10 +159,15 @@ class TestActuator : public ActuatorInterface
131159

132160
return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period) override
133161
{
162+
if (throw_on_read_)
163+
{
164+
throw std::runtime_error("Exception from TestActuator::read() as requested by parameter.");
165+
}
166+
134167
if (get_hardware_info().is_async)
135168
{
136169
std::this_thread::sleep_for(
137-
std::chrono::milliseconds(1000 / (3 * get_hardware_info().rw_rate)));
170+
std::chrono::milliseconds(1000 / (6 * get_hardware_info().rw_rate)));
138171
}
139172
double vel_cmd = 0.0;
140173
std::ignore = velocity_command_->get_value(vel_cmd, false);
@@ -180,6 +213,11 @@ class TestActuator : public ActuatorInterface
180213

181214
return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
182215
{
216+
if (throw_on_write_)
217+
{
218+
throw std::runtime_error("Exception from TestActuator::write() as requested by parameter.");
219+
}
220+
183221
if (get_hardware_info().is_async)
184222
{
185223
std::this_thread::sleep_for(
@@ -212,6 +250,10 @@ class TestActuator : public ActuatorInterface
212250
}
213251

214252
private:
253+
bool throw_on_read_ = false;
254+
bool throw_on_write_ = false;
255+
bool throw_on_configure_ = false;
256+
bool throw_on_activate_ = false;
215257
StateInterface::SharedPtr position_state_;
216258
StateInterface::SharedPtr velocity_state_;
217259
CommandInterface::SharedPtr velocity_command_;

‎hardware_interface_testing/test/test_components/test_sensor.cpp‎

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,8 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15+
#include <memory>
16+
#include <stdexcept>
1517
#include <vector>
1618

1719
#include "hardware_interface/sensor_interface.hpp"
@@ -43,6 +45,41 @@ class TestSensor : public SensorInterface
4345
get_hardware_info().name.c_str(), get_hardware_info().hardware_plugin_name.c_str());
4446
return CallbackReturn::ERROR;
4547
}
48+
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_configure");
55+
if (it != get_hardware_info().hardware_parameters.end())
56+
{
57+
throw_on_configure_ = (it->second == "true");
58+
}
59+
it = get_hardware_info().hardware_parameters.find("throw_on_activate");
60+
if (it != get_hardware_info().hardware_parameters.end())
61+
{
62+
throw_on_activate_ = (it->second == "true");
63+
}
64+
65+
return CallbackReturn::SUCCESS;
66+
}
67+
68+
CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override
69+
{
70+
if (throw_on_configure_)
71+
{
72+
throw std::runtime_error("Injected exception during on_configure!");
73+
}
74+
return CallbackReturn::SUCCESS;
75+
}
76+
77+
CallbackReturn on_activate(const rclcpp_lifecycle::State & /*previous_state*/) override
78+
{
79+
if (throw_on_activate_)
80+
{
81+
throw std::runtime_error("Injected exception during on_activate!");
82+
}
4683
return CallbackReturn::SUCCESS;
4784
}
4885

@@ -59,10 +96,17 @@ class TestSensor : public SensorInterface
5996

6097
return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
6198
{
99+
if (throw_on_read_)
100+
{
101+
throw std::runtime_error("Exception from TestSensor::read() as requested by parameter.");
102+
}
62103
return return_type::OK;
63104
}
64105

65106
private:
107+
bool throw_on_read_ = false;
108+
bool throw_on_configure_ = false;
109+
bool throw_on_activate_ = false;
66110
StateInterface::SharedPtr velocity_state_;
67111
};
68112

‎hardware_interface_testing/test/test_components/test_system.cpp‎

Lines changed: 56 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,8 @@
1313
// limitations under the License.
1414

1515
#include <array>
16+
#include <memory>
17+
#include <stdexcept>
1618
#include <vector>
1719

1820
#include "hardware_interface/system_interface.hpp"
@@ -63,6 +65,46 @@ class TestSystem : public SystemInterface
6365
get_hardware_info().name.c_str(), get_hardware_info().hardware_plugin_name.c_str());
6466
return CallbackReturn::ERROR;
6567
}
68+
69+
auto it = get_hardware_info().hardware_parameters.find("throw_on_read");
70+
if (it != get_hardware_info().hardware_parameters.end())
71+
{
72+
throw_on_read_ = (it->second == "true");
73+
}
74+
it = get_hardware_info().hardware_parameters.find("throw_on_write");
75+
if (it != get_hardware_info().hardware_parameters.end())
76+
{
77+
throw_on_write_ = (it->second == "true");
78+
}
79+
it = get_hardware_info().hardware_parameters.find("throw_on_configure");
80+
if (it != get_hardware_info().hardware_parameters.end())
81+
{
82+
throw_on_configure_ = (it->second == "true");
83+
}
84+
it = get_hardware_info().hardware_parameters.find("throw_on_activate");
85+
if (it != get_hardware_info().hardware_parameters.end())
86+
{
87+
throw_on_activate_ = (it->second == "true");
88+
}
89+
90+
return CallbackReturn::SUCCESS;
91+
}
92+
93+
CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override
94+
{
95+
if (throw_on_configure_)
96+
{
97+
throw std::runtime_error("Injected exception during on_configure!");
98+
}
99+
return CallbackReturn::SUCCESS;
100+
}
101+
102+
CallbackReturn on_activate(const rclcpp_lifecycle::State & /*previous_state*/) override
103+
{
104+
if (throw_on_activate_)
105+
{
106+
throw std::runtime_error("Injected exception during on_activate!");
107+
}
66108
return CallbackReturn::SUCCESS;
67109
}
68110

@@ -131,6 +173,11 @@ class TestSystem : public SystemInterface
131173

132174
return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
133175
{
176+
if (throw_on_read_)
177+
{
178+
throw std::runtime_error("Exception from TestSystem::read() as requested by parameter.");
179+
}
180+
134181
verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id());
135182
if (get_hardware_info().is_async)
136183
{
@@ -171,6 +218,11 @@ class TestSystem : public SystemInterface
171218

172219
return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
173220
{
221+
if (throw_on_write_)
222+
{
223+
throw std::runtime_error("Exception from TestSystem::write() as requested by parameter.");
224+
}
225+
174226
verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id());
175227
if (get_hardware_info().is_async)
176228
{
@@ -204,6 +256,10 @@ class TestSystem : public SystemInterface
204256
}
205257

206258
private:
259+
bool throw_on_read_ = false;
260+
bool throw_on_write_ = false;
261+
bool throw_on_configure_ = false;
262+
bool throw_on_activate_ = false;
207263
std::array<CommandInterface::SharedPtr, 2> velocity_command_;
208264
std::array<StateInterface::SharedPtr, 2> position_state_;
209265
std::array<StateInterface::SharedPtr, 2> velocity_state_;

0 commit comments

Comments
 (0)