[ResourceManager] adds test for uninitialized hardware (#1243)

ros-controls · Jan 4, 2024 · 64c9e2a · 64c9e2a
1 parent 0dfbd3d
commit 64c9e2a
Show file tree

Hide file tree

Showing 6 changed files with 139 additions and 0 deletions.
diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp
@@ -120,5 +120,15 @@ class TestActuator : public ActuatorInterface
   double max_velocity_command_ = 0.0;
 };
 
+class TestUnitilizableActuator : public TestActuator
+{
+  CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override
+  {
+    ActuatorInterface::on_init(info);
+    return CallbackReturn::ERROR;
+  }
+};
+
 #include "pluginlib/class_list_macros.hpp"  // NOLINT
 PLUGINLIB_EXPORT_CLASS(TestActuator, hardware_interface::ActuatorInterface)
+PLUGINLIB_EXPORT_CLASS(TestUnitilizableActuator, hardware_interface::ActuatorInterface)
diff --git a/hardware_interface/test/test_components/test_components.xml b/hardware_interface/test/test_components/test_components.xml
@@ -17,4 +17,22 @@
       Test System
     </description>
   </class>
+
+  <class name="test_unitilizable_actuator" type="TestUnitilizableActuator" base_class_type="hardware_interface::ActuatorInterface">
+    <description>
+      Test Unitilizable Actuator
+    </description>
+  </class>
+
+  <class name="test_unitilizable_sensor" type="TestUnitilizableSensor" base_class_type="hardware_interface::SensorInterface">
+    <description>
+      Test Unitilizable Sensor
+    </description>
+  </class>
+
+  <class name="test_unitilizable_system" type="TestUnitilizableSystem" base_class_type="hardware_interface::SystemInterface">
+    <description>
+      Test Unitilizable System
+    </description>
+  </class>
 </library>
diff --git a/hardware_interface/test/test_components/test_sensor.cpp b/hardware_interface/test/test_components/test_sensor.cpp
@@ -55,5 +55,15 @@ class TestSensor : public SensorInterface
   double velocity_state_ = 0.0;
 };
 
+class TestUnitilizableSensor : public TestSensor
+{
+  CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override
+  {
+    SensorInterface::on_init(info);
+    return CallbackReturn::ERROR;
+  }
+};
+
 #include "pluginlib/class_list_macros.hpp"  // NOLINT
 PLUGINLIB_EXPORT_CLASS(TestSensor, hardware_interface::SensorInterface)
+PLUGINLIB_EXPORT_CLASS(TestUnitilizableSensor, hardware_interface::SensorInterface)
diff --git a/hardware_interface/test/test_components/test_system.cpp b/hardware_interface/test/test_components/test_system.cpp
@@ -123,5 +123,15 @@ class TestSystem : public SystemInterface
   double configuration_command_ = 0.0;
 };
 
+class TestUnitilizableSystem : public TestSystem
+{
+  CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override
+  {
+    SystemInterface::on_init(info);
+    return CallbackReturn::ERROR;
+  }
+};
+
 #include "pluginlib/class_list_macros.hpp"  // NOLINT
 PLUGINLIB_EXPORT_CLASS(TestSystem, hardware_interface::SystemInterface)
+PLUGINLIB_EXPORT_CLASS(TestUnitilizableSystem, hardware_interface::SystemInterface)
diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp
@@ -69,6 +69,7 @@ class TestableResourceManager : public hardware_interface::ResourceManager
   FRIEND_TEST(ResourceManagerTest, post_initialization_add_components);
   FRIEND_TEST(ResourceManagerTest, managing_controllers_reference_interfaces);
   FRIEND_TEST(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle);
+  FRIEND_TEST(ResourceManagerTest, test_unitilizable_hardware_no_validation);
 
   TestableResourceManager() : hardware_interface::ResourceManager() {}
 
@@ -154,6 +155,45 @@ TEST_F(ResourceManagerTest, post_initialization_with_urdf)
   ASSERT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf));
 }
 
+TEST_F(ResourceManagerTest, test_unitilizable_hardware_validation)
+{
+  // If the the hardware can not be initialized and load_urdf tried to validate the interfaces a
+  // runtime exception is thrown
+  TestableResourceManager rm;
+  ASSERT_THROW(
+    rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, true),
+    std::runtime_error);
+}
+
+TEST_F(ResourceManagerTest, test_unitilizable_hardware_no_validation)
+{
+  // If the the hardware can not be initialized and load_urdf didn't try to validate the interfaces,
+  // the interface should not show up
+  TestableResourceManager rm;
+  EXPECT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, false));
+
+  // test actuator
+  EXPECT_FALSE(rm.state_interface_exists("joint1/position"));
+  EXPECT_FALSE(rm.state_interface_exists("joint1/velocity"));
+  EXPECT_FALSE(rm.command_interface_exists("joint1/position"));
+  EXPECT_FALSE(rm.command_interface_exists("joint1/max_velocity"));
+
+  // test sensor
+  EXPECT_FALSE(rm.state_interface_exists("sensor1/velocity"));
+
+  // test system
+  EXPECT_FALSE(rm.state_interface_exists("joint2/position"));
+  EXPECT_FALSE(rm.state_interface_exists("joint2/velocity"));
+  EXPECT_FALSE(rm.state_interface_exists("joint2/acceleration"));
+  EXPECT_FALSE(rm.command_interface_exists("joint2/velocity"));
+  EXPECT_FALSE(rm.command_interface_exists("joint2/max_acceleration"));
+  EXPECT_FALSE(rm.state_interface_exists("joint3/position"));
+  EXPECT_FALSE(rm.state_interface_exists("joint3/velocity"));
+  EXPECT_FALSE(rm.state_interface_exists("joint3/acceleration"));
+  EXPECT_FALSE(rm.command_interface_exists("joint3/velocity"));
+  EXPECT_FALSE(rm.command_interface_exists("joint3/max_acceleration"));
+}
+
 TEST_F(ResourceManagerTest, initialization_with_urdf_manual_validation)
 {
   // we validate the results manually

diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp
@@ -176,6 +176,55 @@ const auto hardware_resources =
   </ros2_control>
 )";
 
+const auto unitilizable_hardware_resources =
+  R"(
+  <ros2_control name="TestUnitilizableActuatorHardware" type="actuator">
+    <hardware>
+      <plugin>test_unitilizable_actuator</plugin>
+    </hardware>
+    <joint name="joint1">
+      <command_interface name="position"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <command_interface name="max_velocity" />
+    </joint>
+  </ros2_control>
+  <ros2_control name="TestUnitilizableSensorHardware" type="sensor">
+    <hardware>
+      <plugin>test_unitilizable_sensor</plugin>
+      <param name="example_param_write_for_sec">2</param>
+      <param name="example_param_read_for_sec">2</param>
+    </hardware>
+    <sensor name="sensor1">
+      <state_interface name="velocity"/>
+    </sensor>
+  </ros2_control>
+  <ros2_control name="TestUnitilizableSystemHardware" type="system">
+    <hardware>
+      <plugin>test_unitilizable_system</plugin>
+      <param name="example_param_write_for_sec">2</param>
+      <param name="example_param_read_for_sec">2</param>
+    </hardware>
+    <joint name="joint2">
+      <command_interface name="velocity"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="acceleration"/>
+      <command_interface name="max_acceleration" />
+    </joint>
+    <joint name="joint3">
+      <command_interface name="velocity"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="acceleration"/>
+    </joint>
+    <joint name="configuration">
+      <command_interface name="max_tcp_jerk"/>
+      <state_interface name="max_tcp_jerk"/>
+    </joint>
+  </ros2_control>
+)";
+
 const auto hardware_resources_missing_state_keys =
   R"(
   <ros2_control name="TestActuatorHardware" type="actuator">
@@ -406,6 +455,8 @@ const auto diffbot_urdf =
 
 const auto minimal_robot_urdf =
   std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail);
+const auto minimal_unitilizable_robot_urdf =
+  std::string(urdf_head) + std::string(unitilizable_hardware_resources) + std::string(urdf_tail);
 
 const auto minimal_robot_missing_state_keys_urdf =
   std::string(urdf_head) + std::string(hardware_resources_missing_state_keys) +