Skip to content

Commit 64c9e2a

Browse files
taDachstaDachs
authored
[ResourceManager] adds test for uninitialized hardware (#1243)
1 parent 0dfbd3d commit 64c9e2a

File tree

6 files changed

+139
-0
lines changed

6 files changed

+139
-0
lines changed

‎hardware_interface/test/test_components/test_actuator.cpp

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -120,5 +120,15 @@ class TestActuator : public ActuatorInterface
120120
double max_velocity_command_ = 0.0;
121121
};
122122

123+
class TestUnitilizableActuator : public TestActuator
124+
{
125+
CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override
126+
{
127+
ActuatorInterface::on_init(info);
128+
return CallbackReturn::ERROR;
129+
}
130+
};
131+
123132
#include "pluginlib/class_list_macros.hpp" // NOLINT
124133
PLUGINLIB_EXPORT_CLASS(TestActuator, hardware_interface::ActuatorInterface)
134+
PLUGINLIB_EXPORT_CLASS(TestUnitilizableActuator, hardware_interface::ActuatorInterface)

‎hardware_interface/test/test_components/test_components.xml

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,4 +17,22 @@
1717
Test System
1818
</description>
1919
</class>
20+
21+
<class name="test_unitilizable_actuator" type="TestUnitilizableActuator" base_class_type="hardware_interface::ActuatorInterface">
22+
<description>
23+
Test Unitilizable Actuator
24+
</description>
25+
</class>
26+
27+
<class name="test_unitilizable_sensor" type="TestUnitilizableSensor" base_class_type="hardware_interface::SensorInterface">
28+
<description>
29+
Test Unitilizable Sensor
30+
</description>
31+
</class>
32+
33+
<class name="test_unitilizable_system" type="TestUnitilizableSystem" base_class_type="hardware_interface::SystemInterface">
34+
<description>
35+
Test Unitilizable System
36+
</description>
37+
</class>
2038
</library>

‎hardware_interface/test/test_components/test_sensor.cpp

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -55,5 +55,15 @@ class TestSensor : public SensorInterface
5555
double velocity_state_ = 0.0;
5656
};
5757

58+
class TestUnitilizableSensor : public TestSensor
59+
{
60+
CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override
61+
{
62+
SensorInterface::on_init(info);
63+
return CallbackReturn::ERROR;
64+
}
65+
};
66+
5867
#include "pluginlib/class_list_macros.hpp" // NOLINT
5968
PLUGINLIB_EXPORT_CLASS(TestSensor, hardware_interface::SensorInterface)
69+
PLUGINLIB_EXPORT_CLASS(TestUnitilizableSensor, hardware_interface::SensorInterface)

‎hardware_interface/test/test_components/test_system.cpp

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -123,5 +123,15 @@ class TestSystem : public SystemInterface
123123
double configuration_command_ = 0.0;
124124
};
125125

126+
class TestUnitilizableSystem : public TestSystem
127+
{
128+
CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override
129+
{
130+
SystemInterface::on_init(info);
131+
return CallbackReturn::ERROR;
132+
}
133+
};
134+
126135
#include "pluginlib/class_list_macros.hpp" // NOLINT
127136
PLUGINLIB_EXPORT_CLASS(TestSystem, hardware_interface::SystemInterface)
137+
PLUGINLIB_EXPORT_CLASS(TestUnitilizableSystem, hardware_interface::SystemInterface)

‎hardware_interface/test/test_resource_manager.cpp

Lines changed: 40 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -69,6 +69,7 @@ class TestableResourceManager : public hardware_interface::ResourceManager
6969
FRIEND_TEST(ResourceManagerTest, post_initialization_add_components);
7070
FRIEND_TEST(ResourceManagerTest, managing_controllers_reference_interfaces);
7171
FRIEND_TEST(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle);
72+
FRIEND_TEST(ResourceManagerTest, test_unitilizable_hardware_no_validation);
7273

7374
TestableResourceManager() : hardware_interface::ResourceManager() {}
7475

@@ -154,6 +155,45 @@ TEST_F(ResourceManagerTest, post_initialization_with_urdf)
154155
ASSERT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf));
155156
}
156157

158+
TEST_F(ResourceManagerTest, test_unitilizable_hardware_validation)
159+
{
160+
// If the the hardware can not be initialized and load_urdf tried to validate the interfaces a
161+
// runtime exception is thrown
162+
TestableResourceManager rm;
163+
ASSERT_THROW(
164+
rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, true),
165+
std::runtime_error);
166+
}
167+
168+
TEST_F(ResourceManagerTest, test_unitilizable_hardware_no_validation)
169+
{
170+
// If the the hardware can not be initialized and load_urdf didn't try to validate the interfaces,
171+
// the interface should not show up
172+
TestableResourceManager rm;
173+
EXPECT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, false));
174+
175+
// test actuator
176+
EXPECT_FALSE(rm.state_interface_exists("joint1/position"));
177+
EXPECT_FALSE(rm.state_interface_exists("joint1/velocity"));
178+
EXPECT_FALSE(rm.command_interface_exists("joint1/position"));
179+
EXPECT_FALSE(rm.command_interface_exists("joint1/max_velocity"));
180+
181+
// test sensor
182+
EXPECT_FALSE(rm.state_interface_exists("sensor1/velocity"));
183+
184+
// test system
185+
EXPECT_FALSE(rm.state_interface_exists("joint2/position"));
186+
EXPECT_FALSE(rm.state_interface_exists("joint2/velocity"));
187+
EXPECT_FALSE(rm.state_interface_exists("joint2/acceleration"));
188+
EXPECT_FALSE(rm.command_interface_exists("joint2/velocity"));
189+
EXPECT_FALSE(rm.command_interface_exists("joint2/max_acceleration"));
190+
EXPECT_FALSE(rm.state_interface_exists("joint3/position"));
191+
EXPECT_FALSE(rm.state_interface_exists("joint3/velocity"));
192+
EXPECT_FALSE(rm.state_interface_exists("joint3/acceleration"));
193+
EXPECT_FALSE(rm.command_interface_exists("joint3/velocity"));
194+
EXPECT_FALSE(rm.command_interface_exists("joint3/max_acceleration"));
195+
}
196+
157197
TEST_F(ResourceManagerTest, initialization_with_urdf_manual_validation)
158198
{
159199
// we validate the results manually

‎ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp

Lines changed: 51 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -176,6 +176,55 @@ const auto hardware_resources =
176176
</ros2_control>
177177
)";
178178

179+
const auto unitilizable_hardware_resources =
180+
R"(
181+
<ros2_control name="TestUnitilizableActuatorHardware" type="actuator">
182+
<hardware>
183+
<plugin>test_unitilizable_actuator</plugin>
184+
</hardware>
185+
<joint name="joint1">
186+
<command_interface name="position"/>
187+
<state_interface name="position"/>
188+
<state_interface name="velocity"/>
189+
<command_interface name="max_velocity" />
190+
</joint>
191+
</ros2_control>
192+
<ros2_control name="TestUnitilizableSensorHardware" type="sensor">
193+
<hardware>
194+
<plugin>test_unitilizable_sensor</plugin>
195+
<param name="example_param_write_for_sec">2</param>
196+
<param name="example_param_read_for_sec">2</param>
197+
</hardware>
198+
<sensor name="sensor1">
199+
<state_interface name="velocity"/>
200+
</sensor>
201+
</ros2_control>
202+
<ros2_control name="TestUnitilizableSystemHardware" type="system">
203+
<hardware>
204+
<plugin>test_unitilizable_system</plugin>
205+
<param name="example_param_write_for_sec">2</param>
206+
<param name="example_param_read_for_sec">2</param>
207+
</hardware>
208+
<joint name="joint2">
209+
<command_interface name="velocity"/>
210+
<state_interface name="position"/>
211+
<state_interface name="velocity"/>
212+
<state_interface name="acceleration"/>
213+
<command_interface name="max_acceleration" />
214+
</joint>
215+
<joint name="joint3">
216+
<command_interface name="velocity"/>
217+
<state_interface name="position"/>
218+
<state_interface name="velocity"/>
219+
<state_interface name="acceleration"/>
220+
</joint>
221+
<joint name="configuration">
222+
<command_interface name="max_tcp_jerk"/>
223+
<state_interface name="max_tcp_jerk"/>
224+
</joint>
225+
</ros2_control>
226+
)";
227+
179228
const auto hardware_resources_missing_state_keys =
180229
R"(
181230
<ros2_control name="TestActuatorHardware" type="actuator">
@@ -406,6 +455,8 @@ const auto diffbot_urdf =
406455

407456
const auto minimal_robot_urdf =
408457
std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail);
458+
const auto minimal_unitilizable_robot_urdf =
459+
std::string(urdf_head) + std::string(unitilizable_hardware_resources) + std::string(urdf_tail);
409460

410461
const auto minimal_robot_missing_state_keys_urdf =
411462
std::string(urdf_head) + std::string(hardware_resources_missing_state_keys) +

0 commit comments

Comments
 (0)