Skip to content

Commit 17bb7a2

Browse files
committed
Throw an exception when requesting already claimed exclusive interface
1 parent 848c8ae commit 17bb7a2

File tree

1 file changed

+9
-1
lines changed

1 file changed

+9
-1
lines changed

hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp

Lines changed: 9 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717

1818
#include "hardware_interface/actuator_interface.hpp"
1919
#include "hardware_interface/types/hardware_interface_type_values.hpp"
20+
#include "rclcpp/rclcpp.hpp"
2021
#include "ros2_control_test_assets/test_hardware_interface_constants.hpp"
2122

2223
using hardware_interface::ActuatorInterface;
@@ -124,7 +125,14 @@ class TestActuatorExclusiveInterfaces : public ActuatorInterface
124125
std::find(claimed_joint_copy.begin(), claimed_joint_copy.end(), joint_name) !=
125126
claimed_joint_copy.end())
126127
{
127-
return hardware_interface::return_type::ERROR;
128+
RCLCPP_ERROR_STREAM(
129+
rclcpp::get_logger("TestActuatorExclusiveInterfaces"),
130+
"Joint : " << joint_name
131+
<< " is already claimed. This cannot happen as the hardware "
132+
"interface is exclusive.");
133+
throw std::runtime_error(
134+
"Joint : " + joint_name +
135+
" is already claimed. This cannot happen as the hardware interface is exclusive.");
128136
}
129137
}
130138
}

0 commit comments

Comments
 (0)