diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 916dd14661..3995c8dea1 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -680,17 +680,34 @@ class ResourceStorage template 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(params.clock, params.logger)), params_(params) +: resource_storage_(std::make_unique(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); diff --git a/hardware_interface_testing/CMakeLists.txt b/hardware_interface_testing/CMakeLists.txt index fabef98ae6..04742a757a 100644 --- a/hardware_interface_testing/CMakeLists.txt +++ b/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}) diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index 13d1881f64..c48a6669ff 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -15,6 +15,7 @@ #include #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 on_export_state_interfaces() override { + check_injected_failure("export_state_interfaces"); std::vector state_interfaces; position_state_ = std::make_shared( 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 on_export_command_interfaces() override { + check_injected_failure("export_command_interfaces"); std::vector command_interfaces; velocity_command_ = std::make_shared( get_hardware_info().joints[0].name, get_hardware_info().joints[0].command_interfaces[0].name); @@ -101,9 +164,10 @@ class TestActuator : public ActuatorInterface } hardware_interface::return_type prepare_command_mode_switch( - const std::vector & /*start_interfaces*/, - const std::vector & /*stop_interfaces*/) override + const std::vector & start_interfaces, + const std::vector & 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); @@ -111,9 +175,10 @@ class TestActuator : public ActuatorInterface } hardware_interface::return_type perform_command_mode_switch( - const std::vector & /*start_interfaces*/, - const std::vector & /*stop_interfaces*/) override + const std::vector & start_interfaces, + const std::vector & 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( diff --git a/hardware_interface_testing/test/test_components/test_sensor.cpp b/hardware_interface_testing/test/test_components/test_sensor.cpp index a4cfb5131f..e10172ef76 100644 --- a/hardware_interface_testing/test/test_components/test_sensor.cpp +++ b/hardware_interface_testing/test/test_components/test_sensor.cpp @@ -14,6 +14,7 @@ #include +#include "hardware_interface/lexical_casts.hpp" #include "hardware_interface/sensor_interface.hpp" #include "rclcpp/logging.hpp" @@ -23,6 +24,28 @@ 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 { @@ -30,6 +53,8 @@ class TestSensor : public SensorInterface { 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 on_export_state_interfaces() override { + check_injected_failure("export_state_interfaces"); std::vector state_interfaces; velocity_state_ = std::make_shared( 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; } diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index a10a2939e1..6e6c93cf1f 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -15,8 +15,8 @@ #include #include +#include "hardware_interface/lexical_casts.hpp" #include "hardware_interface/system_interface.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/logging.hpp" #include "ros2_control_test_assets/test_hardware_interface_constants.hpp" @@ -40,6 +40,26 @@ void verify_internal_lifecycle_id(uint8_t expected_id, uint8_t actual_id) class TestSystem : public SystemInterface { + 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 TestSystem::" + method_name); + } + + if ( + params.count(unknown_throw_key) && + hardware_interface::parse_bool(params.at(unknown_throw_key))) + { + throw 42; // Throw an unknown type (int) + } + } + CallbackReturn on_init( const hardware_interface::HardwareComponentInterfaceParams & params) override { @@ -49,6 +69,8 @@ class TestSystem : public SystemInterface } verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); + check_injected_failure("on_init"); + // Simulating initialization error if (get_hardware_info().joints[0].state_interfaces[1].name == "does_not_exist") { @@ -66,8 +88,45 @@ class TestSystem : public SystemInterface return CallbackReturn::SUCCESS; } + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override + { + check_injected_failure("on_configure"); + return SystemInterface::on_configure(previous_state); + } + + CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override + { + check_injected_failure("on_cleanup"); + return SystemInterface::on_cleanup(previous_state); + } + + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override + { + check_injected_failure("on_activate"); + return SystemInterface::on_activate(previous_state); + } + + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override + { + check_injected_failure("on_deactivate"); + return SystemInterface::on_deactivate(previous_state); + } + + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override + { + check_injected_failure("on_shutdown"); + return SystemInterface::on_shutdown(previous_state); + } + + CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override + { + check_injected_failure("on_error"); + return SystemInterface::on_error(previous_state); + } + std::vector on_export_state_interfaces() override { + check_injected_failure("export_state_interfaces"); verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); const auto info = get_hardware_info(); std::vector state_interfaces; @@ -101,6 +160,7 @@ class TestSystem : public SystemInterface std::vector on_export_command_interfaces() override { + check_injected_failure("export_command_interfaces"); verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); const auto info = get_hardware_info(); std::vector command_interfaces; @@ -131,6 +191,7 @@ class TestSystem : public SystemInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { + check_injected_failure("read"); verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); if (get_hardware_info().is_async) { @@ -171,6 +232,7 @@ class TestSystem : public SystemInterface return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { + check_injected_failure("write"); verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); if (get_hardware_info().is_async) { @@ -203,6 +265,22 @@ class TestSystem : public SystemInterface return return_type::OK; } + return_type prepare_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) override + { + check_injected_failure("prepare_command_mode_switch"); + return SystemInterface::prepare_command_mode_switch(start_interfaces, stop_interfaces); + } + + return_type perform_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) override + { + check_injected_failure("perform_command_mode_switch"); + return SystemInterface::perform_command_mode_switch(start_interfaces, stop_interfaces); + } + private: std::array velocity_command_; std::array position_state_; diff --git a/hardware_interface_testing/test/test_rm_exception_handling.cpp b/hardware_interface_testing/test/test_rm_exception_handling.cpp new file mode 100644 index 0000000000..7f7adde39c --- /dev/null +++ b/hardware_interface_testing/test/test_rm_exception_handling.cpp @@ -0,0 +1,852 @@ +// Copyright 2024 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "hardware_interface/resource_manager.hpp" +#include "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp/rclcpp.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +using hardware_interface::ResourceManager; +using hardware_interface::ResourceManagerParams; +using hardware_interface::return_type; +namespace lifecycle_state_names = hardware_interface::lifecycle_state_names; + +// --------------------------------------------------------------------------- +// URDF snippets — these match exactly what TestActuator/Sensor/System expect +// --------------------------------------------------------------------------- + +// A single TestActuator component with the joints/interfaces it requires. +// TestActuator::on_init checks joints[0].state_interfaces[1].name != "does_not_exist" +// and needs at least position + velocity state interfaces and position + max_velocity command. +static const char * const ACTUATOR_HW = + R"( + + + test_actuator + + + + + + + + +)"; + +// A single TestSensor component. +// TestSensor::on_init checks sensors[0].state_interfaces.size() != 2 (only 1 is OK). +static const char * const SENSOR_HW = + R"( + + + test_sensor + + + + + +)"; + +// A single TestSystem component with the minimal structure it requires. +// TestSystem::on_init checks joints[0].state_interfaces[1].name != "does_not_exist". +// on_export_command_interfaces accesses joints[0].command_interfaces[1] (max_acceleration). +static const char * const SYSTEM_HW = + R"( + + + test_system + + + + + + + + + + + + + + + + + + + +)"; + +// --------------------------------------------------------------------------- +// Helper: build a URDF by injecting extra tags into a hw snippet +// --------------------------------------------------------------------------- +static std::string make_urdf(const std::string & hw_snippet, const std::string & extra_params = "") +{ + // If no extra params, just use the snippet as-is + if (extra_params.empty()) + { + return std::string(ros2_control_test_assets::urdf_head) + hw_snippet + + ros2_control_test_assets::urdf_tail; + } + + // Inject extra params after the ... line + std::string snippet = hw_snippet; + const std::string plugin_close = ""; + auto pos = snippet.find(plugin_close); + if (pos != std::string::npos) + { + snippet.insert(pos + plugin_close.size(), "\n" + extra_params); + } + return std::string(ros2_control_test_assets::urdf_head) + snippet + + ros2_control_test_assets::urdf_tail; +} + +// --------------------------------------------------------------------------- +// Test Fixture +// --------------------------------------------------------------------------- +class TestResourceManagerExceptionHandling : public ::testing::Test +{ +protected: + void SetUp() override + { + // Build a default valid robot description (no failures expected) + params_.robot_description = std::string(ros2_control_test_assets::urdf_head) + ACTUATOR_HW + + SENSOR_HW + SYSTEM_HW + ros2_control_test_assets::urdf_tail; + params_.clock = std::make_shared(RCL_STEADY_TIME); + params_.handle_exceptions = true; // default: swallow exceptions + } + + ResourceManagerParams params_; +}; + +// --------------------------------------------------------------------------- +// 1. on_init exception — Actuator +// Covers: ResourceStorage::initialize_hardware catch(std::exception) and catch(...) +// --------------------------------------------------------------------------- +TEST_F(TestResourceManagerExceptionHandling, actuator_init_standard_exception) +{ + params_.robot_description = + make_urdf(ACTUATOR_HW, " true"); + + // handle_exceptions = true => absorb, no throw at RM level + params_.handle_exceptions = true; + EXPECT_NO_THROW({ ResourceManager rm(params_, true); }); + + // handle_exceptions = false => propagate + params_.handle_exceptions = false; + EXPECT_THROW({ ResourceManager rm(params_, true); }, std::runtime_error); +} + +TEST_F(TestResourceManagerExceptionHandling, actuator_init_unknown_exception) +{ + params_.robot_description = + make_urdf(ACTUATOR_HW, " true"); + + params_.handle_exceptions = true; + EXPECT_NO_THROW({ ResourceManager rm(params_, true); }); + + params_.handle_exceptions = false; + // RM's catch(...) re-throws — EXPECT_ANY_THROW since it is not std::exception + EXPECT_ANY_THROW({ ResourceManager rm(params_, true); }); +} + +// --------------------------------------------------------------------------- +// 2. on_init exception — Sensor +// Covers: the sensor-branch of initialize_hardware +// --------------------------------------------------------------------------- +TEST_F(TestResourceManagerExceptionHandling, sensor_init_standard_exception) +{ + params_.robot_description = + make_urdf(SENSOR_HW, " true"); + + params_.handle_exceptions = true; + EXPECT_NO_THROW({ ResourceManager rm(params_, true); }); + + params_.handle_exceptions = false; + EXPECT_THROW({ ResourceManager rm(params_, true); }, std::runtime_error); +} + +// --------------------------------------------------------------------------- +// 3. on_init exception — System +// Covers: the system-branch of initialize_hardware +// --------------------------------------------------------------------------- +TEST_F(TestResourceManagerExceptionHandling, system_init_standard_exception) +{ + params_.robot_description = + make_urdf(SYSTEM_HW, " true"); + + params_.handle_exceptions = true; + EXPECT_NO_THROW({ ResourceManager rm(params_, true); }); + + params_.handle_exceptions = false; + EXPECT_THROW({ ResourceManager rm(params_, true); }, std::runtime_error); +} + +// --------------------------------------------------------------------------- +// 4. export_state_interfaces exception +// Covers: import_state_interfaces catch(std::exception) — newly added block +// --------------------------------------------------------------------------- +TEST_F(TestResourceManagerExceptionHandling, system_export_state_interfaces_exception) +{ + params_.robot_description = + make_urdf(SYSTEM_HW, " true"); + + params_.handle_exceptions = true; + EXPECT_NO_THROW({ ResourceManager rm(params_, true); }); + + params_.handle_exceptions = false; + EXPECT_THROW({ ResourceManager rm(params_, true); }, std::runtime_error); +} + +TEST_F(TestResourceManagerExceptionHandling, system_export_state_interfaces_unknown_exception) +{ + params_.robot_description = make_urdf( + SYSTEM_HW, " true"); + + params_.handle_exceptions = true; + EXPECT_NO_THROW({ ResourceManager rm(params_, true); }); + + params_.handle_exceptions = false; + EXPECT_ANY_THROW({ ResourceManager rm(params_, true); }); +} + +// --------------------------------------------------------------------------- +// 5. export_command_interfaces exception +// Covers: import_command_interfaces catch(std::exception) and catch(...) +// --------------------------------------------------------------------------- +TEST_F(TestResourceManagerExceptionHandling, system_export_command_interfaces_exception) +{ + params_.robot_description = + make_urdf(SYSTEM_HW, " true"); + + params_.handle_exceptions = true; + EXPECT_NO_THROW({ ResourceManager rm(params_, true); }); + + params_.handle_exceptions = false; + EXPECT_THROW({ ResourceManager rm(params_, true); }, std::runtime_error); +} + +TEST_F(TestResourceManagerExceptionHandling, system_export_command_interfaces_unknown_exception) +{ + params_.robot_description = make_urdf( + SYSTEM_HW, " true"); + + params_.handle_exceptions = true; + EXPECT_NO_THROW({ ResourceManager rm(params_, true); }); + + params_.handle_exceptions = false; + EXPECT_ANY_THROW({ ResourceManager rm(params_, true); }); +} + +// --------------------------------------------------------------------------- +// 6. Lifecycle transition exceptions: configure, activate, deactivate +// Covers: configure_hardware, activate_hardware, deactivate_hardware catch blocks +// --------------------------------------------------------------------------- +TEST_F(TestResourceManagerExceptionHandling, system_configure_exception) +{ + params_.robot_description = + make_urdf(SYSTEM_HW, " true"); + + params_.handle_exceptions = true; + ResourceManager rm_handled(params_, true); + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); + // set_component_state should return ERROR (not throw) when handle_exceptions=true + EXPECT_EQ( + rm_handled.set_component_state("TestSystemHardware", inactive_state), return_type::ERROR); + + params_.handle_exceptions = false; + ResourceManager rm_throwing(params_, true); + EXPECT_THROW( + rm_throwing.set_component_state("TestSystemHardware", inactive_state), std::runtime_error); +} + +TEST_F(TestResourceManagerExceptionHandling, system_activate_exception) +{ + params_.robot_description = + make_urdf(SYSTEM_HW, " true"); + + params_.handle_exceptions = true; + ResourceManager rm_handled(params_, true); + + // Need to configure first before activating + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); + rm_handled.set_component_state("TestSystemHardware", inactive_state); + + rclcpp_lifecycle::State active_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); + EXPECT_EQ(rm_handled.set_component_state("TestSystemHardware", active_state), return_type::ERROR); + + params_.handle_exceptions = false; + ResourceManager rm_throwing(params_, true); + rm_throwing.set_component_state("TestSystemHardware", inactive_state); + EXPECT_THROW( + rm_throwing.set_component_state("TestSystemHardware", active_state), std::runtime_error); +} + +TEST_F(TestResourceManagerExceptionHandling, system_deactivate_exception) +{ + params_.robot_description = + make_urdf(SYSTEM_HW, " true"); + + // First boot the component to ACTIVE, then try to deactivate — that should throw + params_.handle_exceptions = true; + ResourceManager rm_handled(params_, true); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); + rclcpp_lifecycle::State active_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); + + rm_handled.set_component_state("TestSystemHardware", inactive_state); + rm_handled.set_component_state("TestSystemHardware", active_state); + + // Deactivate back to INACTIVE + EXPECT_EQ( + rm_handled.set_component_state("TestSystemHardware", inactive_state), return_type::ERROR); + + params_.handle_exceptions = false; + ResourceManager rm_throwing(params_, true); + rm_throwing.set_component_state("TestSystemHardware", inactive_state); + rm_throwing.set_component_state("TestSystemHardware", active_state); + EXPECT_THROW( + rm_throwing.set_component_state("TestSystemHardware", inactive_state), std::runtime_error); +} + +// --------------------------------------------------------------------------- +// 7. read / write exceptions +// Covers: read_hardware and write_hardware catch blocks in ResourceManager +// --------------------------------------------------------------------------- +TEST_F(TestResourceManagerExceptionHandling, system_read_exception) +{ + params_.robot_description = + make_urdf(SYSTEM_HW, " true"); + + // handle_exceptions = true: read() should return ERROR but not throw + params_.handle_exceptions = true; + ResourceManager rm_handled(params_, true); + + rclcpp_lifecycle::State active_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); + rm_handled.set_component_state("TestSystemHardware", inactive_state); + rm_handled.set_component_state("TestSystemHardware", active_state); + + EXPECT_NO_THROW( + rm_handled.read(rclcpp::Time(0, 0, RCL_STEADY_TIME), rclcpp::Duration::from_seconds(0.01))); + + // handle_exceptions = false: read() should throw + params_.handle_exceptions = false; + ResourceManager rm_throwing(params_, true); + rm_throwing.set_component_state("TestSystemHardware", inactive_state); + rm_throwing.set_component_state("TestSystemHardware", active_state); + + EXPECT_THROW( + rm_throwing.read(rclcpp::Time(0, 0, RCL_STEADY_TIME), rclcpp::Duration::from_seconds(0.01)), + std::runtime_error); +} + +TEST_F(TestResourceManagerExceptionHandling, system_write_exception) +{ + params_.robot_description = + make_urdf(SYSTEM_HW, " true"); + + params_.handle_exceptions = true; + ResourceManager rm_handled(params_, true); + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); + rclcpp_lifecycle::State active_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); + rm_handled.set_component_state("TestSystemHardware", inactive_state); + rm_handled.set_component_state("TestSystemHardware", active_state); + + EXPECT_NO_THROW( + rm_handled.write(rclcpp::Time(0, 0, RCL_STEADY_TIME), rclcpp::Duration::from_seconds(0.01))); + + params_.handle_exceptions = false; + ResourceManager rm_throwing(params_, true); + rm_throwing.set_component_state("TestSystemHardware", inactive_state); + rm_throwing.set_component_state("TestSystemHardware", active_state); + + EXPECT_THROW( + rm_throwing.write(rclcpp::Time(0, 0, RCL_STEADY_TIME), rclcpp::Duration::from_seconds(0.01)), + std::runtime_error); +} + +// --------------------------------------------------------------------------- +// 9. Unknown exceptions for configure / activate / deactivate +// Covers: configure_hardware catch(...), activate_hardware catch(...), +// deactivate_hardware catch(...) +// --------------------------------------------------------------------------- +TEST_F(TestResourceManagerExceptionHandling, system_configure_unknown_exception) +{ + params_.robot_description = + make_urdf(SYSTEM_HW, " true"); + + params_.handle_exceptions = true; + ResourceManager rm_handled(params_, true); + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); + EXPECT_EQ( + rm_handled.set_component_state("TestSystemHardware", inactive_state), return_type::ERROR); + + params_.handle_exceptions = false; + ResourceManager rm_throwing(params_, true); + EXPECT_ANY_THROW(rm_throwing.set_component_state("TestSystemHardware", inactive_state)); +} + +TEST_F(TestResourceManagerExceptionHandling, system_activate_unknown_exception) +{ + params_.robot_description = + make_urdf(SYSTEM_HW, " true"); + + params_.handle_exceptions = true; + ResourceManager rm_handled(params_, true); + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); + rclcpp_lifecycle::State active_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); + rm_handled.set_component_state("TestSystemHardware", inactive_state); + EXPECT_EQ(rm_handled.set_component_state("TestSystemHardware", active_state), return_type::ERROR); + + params_.handle_exceptions = false; + ResourceManager rm_throwing(params_, true); + rm_throwing.set_component_state("TestSystemHardware", inactive_state); + EXPECT_ANY_THROW(rm_throwing.set_component_state("TestSystemHardware", active_state)); +} + +TEST_F(TestResourceManagerExceptionHandling, system_deactivate_unknown_exception) +{ + params_.robot_description = + make_urdf(SYSTEM_HW, " true"); + + params_.handle_exceptions = true; + ResourceManager rm_handled(params_, true); + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); + rclcpp_lifecycle::State active_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); + rm_handled.set_component_state("TestSystemHardware", inactive_state); + rm_handled.set_component_state("TestSystemHardware", active_state); + EXPECT_EQ( + rm_handled.set_component_state("TestSystemHardware", inactive_state), return_type::ERROR); + + params_.handle_exceptions = false; + ResourceManager rm_throwing(params_, true); + rm_throwing.set_component_state("TestSystemHardware", inactive_state); + rm_throwing.set_component_state("TestSystemHardware", active_state); + EXPECT_ANY_THROW(rm_throwing.set_component_state("TestSystemHardware", inactive_state)); +} + +// --------------------------------------------------------------------------- +// 10. cleanup_hardware exceptions (requires INACTIVE state first) +// Covers: cleanup_hardware catch(std::exception) and catch(...) +// State transition: UNCONFIGURED -> configure -> INACTIVE +// INACTIVE -> cleanup -> UNCONFIGURED +// --------------------------------------------------------------------------- +TEST_F(TestResourceManagerExceptionHandling, system_cleanup_standard_exception) +{ + params_.robot_description = + make_urdf(SYSTEM_HW, " true"); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); + rclcpp_lifecycle::State unconfigured_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED); + + // handle_exceptions = true: cleanup() throws, RM catches and returns ERROR + params_.handle_exceptions = true; + ResourceManager rm_handled(params_, true); + rm_handled.set_component_state("TestSystemHardware", inactive_state); + EXPECT_EQ( + rm_handled.set_component_state("TestSystemHardware", unconfigured_state), return_type::ERROR); + + // handle_exceptions = false: cleanup() throws, RM re-throws it + params_.handle_exceptions = false; + ResourceManager rm_throwing(params_, true); + rm_throwing.set_component_state("TestSystemHardware", inactive_state); + EXPECT_THROW( + rm_throwing.set_component_state("TestSystemHardware", unconfigured_state), std::runtime_error); +} + +TEST_F(TestResourceManagerExceptionHandling, system_cleanup_unknown_exception) +{ + params_.robot_description = + make_urdf(SYSTEM_HW, " true"); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); + rclcpp_lifecycle::State unconfigured_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED); + + params_.handle_exceptions = true; + ResourceManager rm_handled(params_, true); + rm_handled.set_component_state("TestSystemHardware", inactive_state); + EXPECT_EQ( + rm_handled.set_component_state("TestSystemHardware", unconfigured_state), return_type::ERROR); + + params_.handle_exceptions = false; + ResourceManager rm_throwing(params_, true); + rm_throwing.set_component_state("TestSystemHardware", inactive_state); + EXPECT_ANY_THROW(rm_throwing.set_component_state("TestSystemHardware", unconfigured_state)); +} + +// --------------------------------------------------------------------------- +// 11. shutdown_hardware exceptions +// Covers: shutdown_hardware catch(std::exception) and catch(...) +// State transition: UNCONFIGURED -> FINALIZED (calls shutdown_hardware) +// --------------------------------------------------------------------------- +TEST_F(TestResourceManagerExceptionHandling, system_shutdown_standard_exception) +{ + params_.robot_description = + make_urdf(SYSTEM_HW, " true"); + + // After loading, hardware is in UNCONFIGURED. Moving to FINALIZED calls shutdown. + rclcpp_lifecycle::State finalized_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED); + + params_.handle_exceptions = true; + ResourceManager rm_handled(params_, true); + EXPECT_EQ( + rm_handled.set_component_state("TestSystemHardware", finalized_state), return_type::ERROR); + + params_.handle_exceptions = false; + ResourceManager rm_throwing(params_, true); + EXPECT_THROW( + rm_throwing.set_component_state("TestSystemHardware", finalized_state), std::runtime_error); +} + +TEST_F(TestResourceManagerExceptionHandling, system_shutdown_unknown_exception) +{ + params_.robot_description = + make_urdf(SYSTEM_HW, " true"); + + rclcpp_lifecycle::State finalized_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED); + + params_.handle_exceptions = true; + ResourceManager rm_handled(params_, true); + EXPECT_EQ( + rm_handled.set_component_state("TestSystemHardware", finalized_state), return_type::ERROR); + + params_.handle_exceptions = false; + ResourceManager rm_throwing(params_, true); + EXPECT_ANY_THROW(rm_throwing.set_component_state("TestSystemHardware", finalized_state)); +} + +// --------------------------------------------------------------------------- +// 12. load_hardware pluginlib exception +// Covers: load_hardware catch(pluginlib::PluginlibException) +// Triggered naturally by an invalid plugin name in the URDF — no mock needed. +// --------------------------------------------------------------------------- +TEST_F(TestResourceManagerExceptionHandling, load_hardware_pluginlib_exception) +{ + // A deliberately invalid plugin name triggers PluginlibException during dlopen. + const char * const BAD_PLUGIN_HW = + R"( + + + this_plugin_does_not_exist + + + + + + + + + + + + + + + + + + + +)"; + + params_.robot_description = std::string(ros2_control_test_assets::urdf_head) + BAD_PLUGIN_HW + + ros2_control_test_assets::urdf_tail; + + // handle_exceptions = true: error logged, no throw — component simply not loaded + params_.handle_exceptions = true; + EXPECT_NO_THROW({ ResourceManager rm(params_, true); }); + + // handle_exceptions = false: PluginlibException propagates from load_hardware + params_.handle_exceptions = false; + EXPECT_THROW({ ResourceManager rm(params_, true); }, pluginlib::PluginlibException); +} + +// --------------------------------------------------------------------------- +// 13. prepare_command_mode_switch exceptions +// Covers: call_prepare_mode_switch lambda catch(std::exception) and catch(...) +// Component must be in INACTIVE or ACTIVE before calling prepare_command_mode_switch. +// --------------------------------------------------------------------------- +TEST_F(TestResourceManagerExceptionHandling, prepare_command_mode_switch_standard_exception) +{ + params_.robot_description = + make_urdf(SYSTEM_HW, " true"); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); + rclcpp_lifecycle::State active_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); + + // handle_exceptions = true: exception caught, returns false + params_.handle_exceptions = true; + ResourceManager rm_handled(params_, true); + rm_handled.set_component_state("TestSystemHardware", inactive_state); + rm_handled.set_component_state("TestSystemHardware", active_state); + EXPECT_FALSE(rm_handled.prepare_command_mode_switch({"joint2/velocity"}, {})); + + // handle_exceptions = false: exception re-thrown + params_.handle_exceptions = false; + ResourceManager rm_throwing(params_, true); + rm_throwing.set_component_state("TestSystemHardware", inactive_state); + rm_throwing.set_component_state("TestSystemHardware", active_state); + EXPECT_THROW( + rm_throwing.prepare_command_mode_switch({"joint2/velocity"}, {}), std::runtime_error); +} + +TEST_F(TestResourceManagerExceptionHandling, prepare_command_mode_switch_unknown_exception) +{ + params_.robot_description = make_urdf( + SYSTEM_HW, " true"); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); + rclcpp_lifecycle::State active_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); + + params_.handle_exceptions = true; + ResourceManager rm_handled(params_, true); + rm_handled.set_component_state("TestSystemHardware", inactive_state); + rm_handled.set_component_state("TestSystemHardware", active_state); + EXPECT_FALSE(rm_handled.prepare_command_mode_switch({"joint2/velocity"}, {})); + + params_.handle_exceptions = false; + ResourceManager rm_throwing(params_, true); + rm_throwing.set_component_state("TestSystemHardware", inactive_state); + rm_throwing.set_component_state("TestSystemHardware", active_state); + EXPECT_ANY_THROW(rm_throwing.prepare_command_mode_switch({"joint2/velocity"}, {})); +} + +// --------------------------------------------------------------------------- +// 14. perform_command_mode_switch exceptions +// Covers: call_perform_mode_switch lambda catch(std::exception) and catch(...) +// --------------------------------------------------------------------------- +TEST_F(TestResourceManagerExceptionHandling, perform_command_mode_switch_standard_exception) +{ + params_.robot_description = + make_urdf(SYSTEM_HW, " true"); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); + rclcpp_lifecycle::State active_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); + + params_.handle_exceptions = true; + ResourceManager rm_handled(params_, true); + rm_handled.set_component_state("TestSystemHardware", inactive_state); + rm_handled.set_component_state("TestSystemHardware", active_state); + EXPECT_FALSE(rm_handled.perform_command_mode_switch({"joint2/velocity"}, {})); + + params_.handle_exceptions = false; + ResourceManager rm_throwing(params_, true); + rm_throwing.set_component_state("TestSystemHardware", inactive_state); + rm_throwing.set_component_state("TestSystemHardware", active_state); + EXPECT_THROW( + rm_throwing.perform_command_mode_switch({"joint2/velocity"}, {}), std::runtime_error); +} + +TEST_F(TestResourceManagerExceptionHandling, perform_command_mode_switch_unknown_exception) +{ + params_.robot_description = make_urdf( + SYSTEM_HW, " true"); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); + rclcpp_lifecycle::State active_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); + + params_.handle_exceptions = true; + ResourceManager rm_handled(params_, true); + rm_handled.set_component_state("TestSystemHardware", inactive_state); + rm_handled.set_component_state("TestSystemHardware", active_state); + EXPECT_FALSE(rm_handled.perform_command_mode_switch({"joint2/velocity"}, {})); + + params_.handle_exceptions = false; + ResourceManager rm_throwing(params_, true); + rm_throwing.set_component_state("TestSystemHardware", inactive_state); + rm_throwing.set_component_state("TestSystemHardware", active_state); + EXPECT_ANY_THROW(rm_throwing.perform_command_mode_switch({"joint2/velocity"}, {})); +} + +// --------------------------------------------------------------------------- +// 15. Unknown exceptions during read / write +// Covers: read catch(...) and write catch(...) +// --------------------------------------------------------------------------- +TEST_F(TestResourceManagerExceptionHandling, system_read_unknown_exception) +{ + params_.robot_description = + make_urdf(SYSTEM_HW, " true"); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); + rclcpp_lifecycle::State active_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); + + params_.handle_exceptions = true; + ResourceManager rm_handled(params_, true); + rm_handled.set_component_state("TestSystemHardware", inactive_state); + rm_handled.set_component_state("TestSystemHardware", active_state); + EXPECT_NO_THROW( + rm_handled.read(rclcpp::Time(0, 0, RCL_STEADY_TIME), rclcpp::Duration::from_seconds(0.01))); + + params_.handle_exceptions = false; + ResourceManager rm_throwing(params_, true); + rm_throwing.set_component_state("TestSystemHardware", inactive_state); + rm_throwing.set_component_state("TestSystemHardware", active_state); + EXPECT_ANY_THROW( + rm_throwing.read(rclcpp::Time(0, 0, RCL_STEADY_TIME), rclcpp::Duration::from_seconds(0.01))); +} + +TEST_F(TestResourceManagerExceptionHandling, system_write_unknown_exception) +{ + params_.robot_description = + make_urdf(SYSTEM_HW, " true"); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); + rclcpp_lifecycle::State active_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); + + params_.handle_exceptions = true; + ResourceManager rm_handled(params_, true); + rm_handled.set_component_state("TestSystemHardware", inactive_state); + rm_handled.set_component_state("TestSystemHardware", active_state); + EXPECT_NO_THROW( + rm_handled.write(rclcpp::Time(0, 0, RCL_STEADY_TIME), rclcpp::Duration::from_seconds(0.01))); + + params_.handle_exceptions = false; + ResourceManager rm_throwing(params_, true); + rm_throwing.set_component_state("TestSystemHardware", inactive_state); + rm_throwing.set_component_state("TestSystemHardware", active_state); + EXPECT_ANY_THROW( + rm_throwing.write(rclcpp::Time(0, 0, RCL_STEADY_TIME), rclcpp::Duration::from_seconds(0.01))); +} + +// --------------------------------------------------------------------------- +// 16. Actuator read / write exceptions +// --------------------------------------------------------------------------- +TEST_F(TestResourceManagerExceptionHandling, actuator_read_write_exception) +{ + params_.robot_description = make_urdf( + ACTUATOR_HW, + " true\n" + " true"); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); + rclcpp_lifecycle::State active_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); + + params_.handle_exceptions = false; + ResourceManager rm(params_, true); + rm.set_component_state("TestActuatorHardware", inactive_state); + rm.set_component_state("TestActuatorHardware", active_state); + + EXPECT_THROW( + rm.read(rclcpp::Time(0, 0, RCL_STEADY_TIME), rclcpp::Duration::from_seconds(0.01)), + std::runtime_error); + EXPECT_THROW( + rm.write(rclcpp::Time(0, 0, RCL_STEADY_TIME), rclcpp::Duration::from_seconds(0.01)), + std::runtime_error); +} + +TEST_F(TestResourceManagerExceptionHandling, actuator_read_write_unknown_exception) +{ + params_.robot_description = make_urdf( + ACTUATOR_HW, + " true\n" + " true"); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); + rclcpp_lifecycle::State active_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); + + params_.handle_exceptions = false; + ResourceManager rm(params_, true); + rm.set_component_state("TestActuatorHardware", inactive_state); + rm.set_component_state("TestActuatorHardware", active_state); + + EXPECT_ANY_THROW( + rm.read(rclcpp::Time(0, 0, RCL_STEADY_TIME), rclcpp::Duration::from_seconds(0.01))); + EXPECT_ANY_THROW( + rm.write(rclcpp::Time(0, 0, RCL_STEADY_TIME), rclcpp::Duration::from_seconds(0.01))); +} + +// --------------------------------------------------------------------------- +// 17. Sensor read exception +// --------------------------------------------------------------------------- +TEST_F(TestResourceManagerExceptionHandling, sensor_read_exception) +{ + params_.robot_description = + make_urdf(SENSOR_HW, " true"); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); + rclcpp_lifecycle::State active_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); + + params_.handle_exceptions = false; + ResourceManager rm(params_, true); + rm.set_component_state("TestSensorHardware", inactive_state); + rm.set_component_state("TestSensorHardware", active_state); + + EXPECT_THROW( + rm.read(rclcpp::Time(0, 0, RCL_STEADY_TIME), rclcpp::Duration::from_seconds(0.01)), + std::runtime_error); +} + +TEST_F(TestResourceManagerExceptionHandling, sensor_read_unknown_exception) +{ + params_.robot_description = + make_urdf(SENSOR_HW, " true"); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); + rclcpp_lifecycle::State active_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); + + params_.handle_exceptions = false; + ResourceManager rm(params_, true); + rm.set_component_state("TestSensorHardware", inactive_state); + rm.set_component_state("TestSensorHardware", active_state); + + EXPECT_ANY_THROW( + rm.read(rclcpp::Time(0, 0, RCL_STEADY_TIME), rclcpp::Duration::from_seconds(0.01))); +}