[hardware_interface_testing] Add tests for hardware components exception handling

hardware_interface/src/resource_manager.cpp

@@ -680,17 +680,34 @@ class ResourceStorage
   template <class HardwareT>
   void import_state_interfaces(HardwareT & hardware)
   {
-    auto interfaces = hardware.export_state_interfaces();
-    const auto interface_names = add_state_interfaces(interfaces);
-    hardware_info_map_[hardware.get_name()].state_interfaces = interface_names;
-
-    RCLCPP_WARN_EXPRESSION(
-      get_logger(), interface_names.empty(),
-      "Importing state interfaces for the hardware '%s' returned no state interfaces.",
-      hardware.get_name().c_str());
-    hardware_info_map_[hardware.get_name()].state_interfaces = interface_names;
-    available_state_interfaces_.reserve(
-      available_state_interfaces_.capacity() + interface_names.size());
+    try
+    {
+      auto interfaces = hardware.export_state_interfaces();
+      const auto interface_names = add_state_interfaces(interfaces);
+      hardware_info_map_[hardware.get_name()].state_interfaces = interface_names;
+
+      RCLCPP_WARN_EXPRESSION(
+        get_logger(), interface_names.empty(),
+        "Importing state interfaces for the hardware '%s' returned no state interfaces.",
+        hardware.get_name().c_str());
+    }
+    catch (const std::exception & ex)
+    {
+      RCLCPP_ERROR(
+        get_logger(),
+        "Exception of type : %s occurred while importing state interfaces for the hardware '%s' "
+        ": %s",
+        typeid(ex).name(), hardware.get_name().c_str(), ex.what());
+      handle_exception_ ? void() : throw;
+    }
+    catch (...)
+    {
+      RCLCPP_ERROR(
+        get_logger(),
+        "Unknown exception occurred while importing state interfaces for the hardware '%s'",
+        hardware.get_name().c_str());
+      handle_exception_ ? void() : throw;
+    }
   }
 
   void insert_command_interface(const CommandInterface::SharedPtr command_interface)
@@ -1455,7 +1472,7 @@ ResourceManager::ResourceManager(
 
 ResourceManager::ResourceManager(
   const hardware_interface::ResourceManagerParams & params, bool load)
-: resource_storage_(std::make_unique<ResourceStorage>(params.clock, params.logger)), params_(params)
+: resource_storage_(std::make_unique<ResourceStorage>(params)), params_(params)
 {
   RCLCPP_WARN_EXPRESSION(
     params.logger, params.allow_controller_activation_with_inactive_hardware,
@@ -1515,6 +1532,7 @@ bool ResourceManager::load_and_initialize_components(
   params_.robot_description = params.robot_description;
   params_.update_rate = params.update_rate;
   params_.handle_exceptions = params.handle_exceptions;
+  resource_storage_->handle_exception_ = params.handle_exceptions;
 
   auto hardware_info =
     hardware_interface::parse_control_resources_from_urdf(params.robot_description);

hardware_interface_testing/CMakeLists.txt

@@ -55,6 +55,15 @@ if(BUILD_TESTING)
                         ros2_control_test_assets::ros2_control_test_assets
                         ${lifecycle_msgs_TARGETS})
 
+  ament_add_gmock(test_rm_exception_handling test/test_rm_exception_handling.cpp)
+  target_link_libraries(test_rm_exception_handling
+                        hardware_interface::hardware_interface
+                        pluginlib::pluginlib
+                        rclcpp::rclcpp
+                        rclcpp_lifecycle::rclcpp_lifecycle
+                        ros2_control_test_assets::ros2_control_test_assets
+                        ${lifecycle_msgs_TARGETS})
+
 endif()
 
 ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})

hardware_interface_testing/test/test_components/test_actuator.cpp

@@ -15,6 +15,7 @@
 #include <vector>
 
 #include "hardware_interface/actuator_interface.hpp"
+#include "hardware_interface/lexical_casts.hpp"
 #include "rclcpp/logging.hpp"
 #include "ros2_control_test_assets/test_hardware_interface_constants.hpp"
 
@@ -25,6 +26,28 @@ using hardware_interface::StateInterface;
 
 class TestActuator : public ActuatorInterface
 {
+  void check_injected_failure(const std::string & method_name) const
+  {
+    const auto & info = get_hardware_info();
+    const auto & params = info.hardware_parameters;
+    const std::string throw_key = "throw_on_" + method_name;
+    const std::string unknown_throw_key = "throw_unknown_on_" + method_name;
+
+    if (params.count(throw_key) && hardware_interface::parse_bool(params.at(throw_key)))
+    {
+      throw std::runtime_error("Standard exception from TestActuator::" + method_name);
+    }
+
+    if (
+      params.count(unknown_throw_key) &&
+      hardware_interface::parse_bool(params.at(unknown_throw_key)))
+    {
+      RCLCPP_DEBUG(
+        get_logger(), "Injecting unknown exception for TestActuator::%s", method_name.c_str());
+      throw 42;  // Throw an unknown type (int)
+    }
+  }
+
   CallbackReturn on_init(
     const hardware_interface::HardwareComponentInterfaceParams & params) override
   {
@@ -33,6 +56,8 @@ class TestActuator : public ActuatorInterface
       return CallbackReturn::ERROR;
     }
 
+    check_injected_failure("on_init");
+
     if (get_hardware_info().joints[0].state_interfaces[1].name == "does_not_exist")
     {
       return CallbackReturn::ERROR;
@@ -62,8 +87,45 @@ class TestActuator : public ActuatorInterface
     return CallbackReturn::SUCCESS;
   }
 
+  CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override
+  {
+    check_injected_failure("on_configure");
+    return ActuatorInterface::on_configure(previous_state);
+  }
+
+  CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override
+  {
+    check_injected_failure("on_cleanup");
+    return ActuatorInterface::on_cleanup(previous_state);
+  }
+
+  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override
+  {
+    check_injected_failure("on_activate");
+    return ActuatorInterface::on_activate(previous_state);
+  }
+
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override
+  {
+    check_injected_failure("on_deactivate");
+    return ActuatorInterface::on_deactivate(previous_state);
+  }
+
+  CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override
+  {
+    check_injected_failure("on_shutdown");
+    return ActuatorInterface::on_shutdown(previous_state);
+  }
+
+  CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override
+  {
+    check_injected_failure("on_error");
+    return ActuatorInterface::on_error(previous_state);
+  }
+
   std::vector<StateInterface::ConstSharedPtr> on_export_state_interfaces() override
   {
+    check_injected_failure("export_state_interfaces");
     std::vector<StateInterface::ConstSharedPtr> state_interfaces;
     position_state_ = std::make_shared<StateInterface>(
       get_hardware_info().joints[0].name, get_hardware_info().joints[0].state_interfaces[0].name);
@@ -82,6 +144,7 @@ class TestActuator : public ActuatorInterface
 
   std::vector<CommandInterface::SharedPtr> on_export_command_interfaces() override
   {
+    check_injected_failure("export_command_interfaces");
     std::vector<CommandInterface::SharedPtr> command_interfaces;
     velocity_command_ = std::make_shared<CommandInterface>(
       get_hardware_info().joints[0].name, get_hardware_info().joints[0].command_interfaces[0].name);
@@ -101,19 +164,21 @@ class TestActuator : public ActuatorInterface
   }
 
   hardware_interface::return_type prepare_command_mode_switch(
-    const std::vector<std::string> & /*start_interfaces*/,
-    const std::vector<std::string> & /*stop_interfaces*/) override
+    const std::vector<std::string> & start_interfaces,
+    const std::vector<std::string> & stop_interfaces) override
   {
+    check_injected_failure("prepare_command_mode_switch");
     double pos = 0.0;
     std::ignore = position_state_->get_value(pos, false);
     std::ignore = position_state_->set_value(pos + 0.001, false);
     return hardware_interface::return_type::OK;
   }
 
   hardware_interface::return_type perform_command_mode_switch(
-    const std::vector<std::string> & /*start_interfaces*/,
-    const std::vector<std::string> & /*stop_interfaces*/) override
+    const std::vector<std::string> & start_interfaces,
+    const std::vector<std::string> & stop_interfaces) override
   {
+    check_injected_failure("perform_command_mode_switch");
     if (get_hardware_info().hardware_parameters.count("fail_on_perform_mode_switch"))
     {
       if (
@@ -131,6 +196,7 @@ class TestActuator : public ActuatorInterface
 
   return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period) override
   {
+    check_injected_failure("read");
     if (get_hardware_info().is_async)
     {
       std::this_thread::sleep_for(
@@ -180,6 +246,7 @@ class TestActuator : public ActuatorInterface
 
   return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
   {
+    check_injected_failure("write");
     if (get_hardware_info().is_async)
     {
       std::this_thread::sleep_for(

hardware_interface_testing/test/test_components/test_sensor.cpp

@@ -14,6 +14,7 @@
 
 #include <vector>
 
+#include "hardware_interface/lexical_casts.hpp"
 #include "hardware_interface/sensor_interface.hpp"
 #include "rclcpp/logging.hpp"
 
@@ -23,13 +24,37 @@ using hardware_interface::StateInterface;
 
 class TestSensor : public SensorInterface
 {
+  void check_injected_failure(const std::string & method_name) const
+  {
+    const auto & info = get_hardware_info();
+    const auto & params = info.hardware_parameters;
+    const std::string throw_key = "throw_on_" + method_name;
+    const std::string unknown_throw_key = "throw_unknown_on_" + method_name;
+
+    if (params.count(throw_key) && hardware_interface::parse_bool(params.at(throw_key)))
+    {
+      throw std::runtime_error("Standard exception from TestSensor::" + method_name);
+    }
+
+    if (
+      params.count(unknown_throw_key) &&
+      hardware_interface::parse_bool(params.at(unknown_throw_key)))
+    {
+      RCLCPP_DEBUG(
+        get_logger(), "Injecting unknown exception for TestSensor::%s", method_name.c_str());
+      throw 42;  // Throw an unknown type (int)
+    }
+  }
+
   CallbackReturn on_init(
     const hardware_interface::HardwareComponentInterfaceParams & params) override
   {
     if (SensorInterface::on_init(params) != CallbackReturn::SUCCESS)
     {
       return CallbackReturn::ERROR;
     }
+
+    check_injected_failure("on_init");
     // can only give feedback state for velocity
     if (get_hardware_info().sensors[0].state_interfaces.size() == 2)
     {
@@ -46,8 +71,45 @@ class TestSensor : public SensorInterface
     return CallbackReturn::SUCCESS;
   }
 
+  CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override
+  {
+    check_injected_failure("on_configure");
+    return SensorInterface::on_configure(previous_state);
+  }
+
+  CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override
+  {
+    check_injected_failure("on_cleanup");
+    return SensorInterface::on_cleanup(previous_state);
+  }
+
+  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override
+  {
+    check_injected_failure("on_activate");
+    return SensorInterface::on_activate(previous_state);
+  }
+
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override
+  {
+    check_injected_failure("on_deactivate");
+    return SensorInterface::on_deactivate(previous_state);
+  }
+
+  CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override
+  {
+    check_injected_failure("on_shutdown");
+    return SensorInterface::on_shutdown(previous_state);
+  }
+
+  CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override
+  {
+    check_injected_failure("on_error");
+    return SensorInterface::on_error(previous_state);
+  }
+
   std::vector<StateInterface::ConstSharedPtr> on_export_state_interfaces() override
   {
+    check_injected_failure("export_state_interfaces");
     std::vector<StateInterface::ConstSharedPtr> state_interfaces;
     velocity_state_ = std::make_shared<StateInterface>(
       get_hardware_info().sensors[0].name, get_hardware_info().sensors[0].state_interfaces[0].name);
@@ -59,6 +121,7 @@ class TestSensor : public SensorInterface
 
   return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
   {
+    check_injected_failure("read");
     return return_type::OK;
   }