From c312bd2e93c9d507c02225cd9b13f7cdb5fcf7fa Mon Sep 17 00:00:00 2001 From: shlok-mehndiratta Date: Thu, 16 Apr 2026 02:55:03 +0530 Subject: [PATCH 1/6] Add tests for hardware components exception handling --- hardware_interface_testing/CMakeLists.txt | 7 + .../test/test_components/test_actuator.cpp | 68 ++- .../test/test_components/test_sensor.cpp | 44 ++ .../test/test_components/test_system.cpp | 56 +++ .../test/test_rm_exception_handling.cpp | 426 ++++++++++++++++++ 5 files changed, 588 insertions(+), 13 deletions(-) create mode 100644 hardware_interface_testing/test/test_rm_exception_handling.cpp diff --git a/hardware_interface_testing/CMakeLists.txt b/hardware_interface_testing/CMakeLists.txt index fabef98ae6..f51a8aa6ab 100644 --- a/hardware_interface_testing/CMakeLists.txt +++ b/hardware_interface_testing/CMakeLists.txt @@ -55,6 +55,13 @@ 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 + 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..9c2999f261 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include #include #include "hardware_interface/actuator_interface.hpp" @@ -46,19 +48,45 @@ class TestActuator : public ActuatorInterface return CallbackReturn::ERROR; } - /* - * a hardware can optional prove for incorrect info here. - * - * // can only control one joint - * if (get_hardware_info().joints.size() != 1) {return CallbackReturn::ERROR;} - * // can only control in position - * if (get_hardware_info().joints[0].command_interfaces.size() != 1) {return - * CallbackReturn::ERROR;} - * // can only give feedback state for position and velocity - * if (get_hardware_info().joints[0].state_interfaces.size() != 2) {return - * CallbackReturn::ERROR;} - */ + auto it = get_hardware_info().hardware_parameters.find("throw_on_read"); + if (it != get_hardware_info().hardware_parameters.end()) + { + throw_on_read_ = (it->second == "true"); + } + it = get_hardware_info().hardware_parameters.find("throw_on_write"); + if (it != get_hardware_info().hardware_parameters.end()) + { + throw_on_write_ = (it->second == "true"); + } + it = get_hardware_info().hardware_parameters.find("throw_on_configure"); + if (it != get_hardware_info().hardware_parameters.end()) + { + throw_on_configure_ = (it->second == "true"); + } + it = get_hardware_info().hardware_parameters.find("throw_on_activate"); + if (it != get_hardware_info().hardware_parameters.end()) + { + throw_on_activate_ = (it->second == "true"); + } + + return CallbackReturn::SUCCESS; + } + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (throw_on_configure_) + { + throw std::runtime_error("Injected exception during on_configure!"); + } + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_activate(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (throw_on_activate_) + { + throw std::runtime_error("Injected exception during on_activate!"); + } return CallbackReturn::SUCCESS; } @@ -131,10 +159,15 @@ class TestActuator : public ActuatorInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period) override { + if (throw_on_read_) + { + throw std::runtime_error("Exception from TestActuator::read() as requested by parameter."); + } + if (get_hardware_info().is_async) { std::this_thread::sleep_for( - std::chrono::milliseconds(1000 / (3 * get_hardware_info().rw_rate))); + std::chrono::milliseconds(1000 / (6 * get_hardware_info().rw_rate))); } double vel_cmd = 0.0; std::ignore = velocity_command_->get_value(vel_cmd, false); @@ -180,6 +213,11 @@ class TestActuator : public ActuatorInterface return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { + if (throw_on_write_) + { + throw std::runtime_error("Exception from TestActuator::write() as requested by parameter."); + } + if (get_hardware_info().is_async) { std::this_thread::sleep_for( @@ -212,6 +250,10 @@ class TestActuator : public ActuatorInterface } private: + bool throw_on_read_ = false; + bool throw_on_write_ = false; + bool throw_on_configure_ = false; + bool throw_on_activate_ = false; StateInterface::SharedPtr position_state_; StateInterface::SharedPtr velocity_state_; CommandInterface::SharedPtr velocity_command_; diff --git a/hardware_interface_testing/test/test_components/test_sensor.cpp b/hardware_interface_testing/test/test_components/test_sensor.cpp index a4cfb5131f..05ddae2eb2 100644 --- a/hardware_interface_testing/test/test_components/test_sensor.cpp +++ b/hardware_interface_testing/test/test_components/test_sensor.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include #include #include "hardware_interface/sensor_interface.hpp" @@ -43,6 +45,41 @@ class TestSensor : public SensorInterface get_hardware_info().name.c_str(), get_hardware_info().hardware_plugin_name.c_str()); return CallbackReturn::ERROR; } + + auto it = get_hardware_info().hardware_parameters.find("throw_on_read"); + if (it != get_hardware_info().hardware_parameters.end()) + { + throw_on_read_ = (it->second == "true"); + } + it = get_hardware_info().hardware_parameters.find("throw_on_configure"); + if (it != get_hardware_info().hardware_parameters.end()) + { + throw_on_configure_ = (it->second == "true"); + } + it = get_hardware_info().hardware_parameters.find("throw_on_activate"); + if (it != get_hardware_info().hardware_parameters.end()) + { + throw_on_activate_ = (it->second == "true"); + } + + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (throw_on_configure_) + { + throw std::runtime_error("Injected exception during on_configure!"); + } + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_activate(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (throw_on_activate_) + { + throw std::runtime_error("Injected exception during on_activate!"); + } return CallbackReturn::SUCCESS; } @@ -59,10 +96,17 @@ class TestSensor : public SensorInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { + if (throw_on_read_) + { + throw std::runtime_error("Exception from TestSensor::read() as requested by parameter."); + } return return_type::OK; } private: + bool throw_on_read_ = false; + bool throw_on_configure_ = false; + bool throw_on_activate_ = false; StateInterface::SharedPtr velocity_state_; }; diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index a10a2939e1..dc81e48e02 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include +#include +#include #include #include "hardware_interface/system_interface.hpp" @@ -63,6 +65,46 @@ class TestSystem : public SystemInterface get_hardware_info().name.c_str(), get_hardware_info().hardware_plugin_name.c_str()); return CallbackReturn::ERROR; } + + auto it = get_hardware_info().hardware_parameters.find("throw_on_read"); + if (it != get_hardware_info().hardware_parameters.end()) + { + throw_on_read_ = (it->second == "true"); + } + it = get_hardware_info().hardware_parameters.find("throw_on_write"); + if (it != get_hardware_info().hardware_parameters.end()) + { + throw_on_write_ = (it->second == "true"); + } + it = get_hardware_info().hardware_parameters.find("throw_on_configure"); + if (it != get_hardware_info().hardware_parameters.end()) + { + throw_on_configure_ = (it->second == "true"); + } + it = get_hardware_info().hardware_parameters.find("throw_on_activate"); + if (it != get_hardware_info().hardware_parameters.end()) + { + throw_on_activate_ = (it->second == "true"); + } + + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (throw_on_configure_) + { + throw std::runtime_error("Injected exception during on_configure!"); + } + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_activate(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (throw_on_activate_) + { + throw std::runtime_error("Injected exception during on_activate!"); + } return CallbackReturn::SUCCESS; } @@ -131,6 +173,11 @@ class TestSystem : public SystemInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { + if (throw_on_read_) + { + throw std::runtime_error("Exception from TestSystem::read() as requested by parameter."); + } + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); if (get_hardware_info().is_async) { @@ -171,6 +218,11 @@ class TestSystem : public SystemInterface return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { + if (throw_on_write_) + { + throw std::runtime_error("Exception from TestSystem::write() as requested by parameter."); + } + verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); if (get_hardware_info().is_async) { @@ -204,6 +256,10 @@ class TestSystem : public SystemInterface } private: + bool throw_on_read_ = false; + bool throw_on_write_ = false; + bool throw_on_configure_ = false; + bool throw_on_activate_ = false; std::array velocity_command_; std::array position_state_; std::array velocity_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..31d0afc95e --- /dev/null +++ b/hardware_interface_testing/test/test_rm_exception_handling.cpp @@ -0,0 +1,426 @@ +// 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 +#include + +#include "hardware_interface/resource_manager.hpp" +#include "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/rclcpp.hpp" + +using hardware_interface::CommandInterface; +using hardware_interface::HardwareInfo; +using hardware_interface::ResourceManager; +using hardware_interface::return_type; +using hardware_interface::StateInterface; + +class ResourceManagerExceptionTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) + { + rclcpp::init(0, nullptr); + } + node_ = std::make_shared("ResourceManagerExceptionTestNode"); + } + + std::shared_ptr node_; + + std::string get_exception_robot_urdf(const std::string & param_name) + { + std::string urdf = R"( + + + + + + + + + + + + + + + + + + + + test_system + true + + + + + + + + + + + + + + + + +)"; + return urdf; + } +}; + +TEST_F(ResourceManagerExceptionTest, catch_read_exception) +{ + std::string urdf = get_exception_robot_urdf("throw_on_read"); + + // Use the 5-argument constructor with node interfaces to ensure parsing success + ResourceManager rm( + urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); + rclcpp_lifecycle::State active_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + + EXPECT_EQ(rm.set_component_state("ExceptionSystem", inactive_state), return_type::OK); + EXPECT_EQ(rm.set_component_state("ExceptionSystem", active_state), return_type::OK); + + // Read should throw, be caught, and transition component to UNCONFIGURED + auto [result, failed_hardware] = + rm.read(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); + + EXPECT_EQ(result, return_type::ERROR); + ASSERT_EQ(failed_hardware.size(), 1u); + EXPECT_EQ(failed_hardware[0], "ExceptionSystem"); + + auto status = rm.get_components_status(); + EXPECT_EQ( + status["ExceptionSystem"].state.label(), + hardware_interface::lifecycle_state_names::UNCONFIGURED); +} + +TEST_F(ResourceManagerExceptionTest, catch_write_exception) +{ + std::string urdf = get_exception_robot_urdf("throw_on_write"); + ResourceManager rm( + urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); + rclcpp_lifecycle::State active_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + + EXPECT_EQ(rm.set_component_state("ExceptionSystem", inactive_state), return_type::OK); + EXPECT_EQ(rm.set_component_state("ExceptionSystem", active_state), return_type::OK); + + // Write should throw, be caught, and transition component to UNCONFIGURED + auto [result, failed_hardware] = + rm.write(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); + + EXPECT_EQ(result, return_type::ERROR); + + auto status = rm.get_components_status(); + EXPECT_EQ( + status["ExceptionSystem"].state.label(), + hardware_interface::lifecycle_state_names::UNCONFIGURED); +} + +TEST_F(ResourceManagerExceptionTest, catch_configure_exception) +{ + std::string urdf = get_exception_robot_urdf("throw_on_configure"); + ResourceManager rm( + urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); + + // on_configure throws, caught by RM → component stays UNCONFIGURED + EXPECT_EQ(rm.set_component_state("ExceptionSystem", inactive_state), return_type::ERROR); + + auto status = rm.get_components_status(); + // After configure exception the state is not advanced — stays UNCONFIGURED + EXPECT_EQ( + status["ExceptionSystem"].state.label(), + hardware_interface::lifecycle_state_names::UNCONFIGURED); +} + +TEST_F(ResourceManagerExceptionTest, catch_activate_exception) +{ + std::string urdf = get_exception_robot_urdf("throw_on_activate"); + ResourceManager rm( + urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); + rclcpp_lifecycle::State active_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + + EXPECT_EQ(rm.set_component_state("ExceptionSystem", inactive_state), return_type::OK); + + // on_activate throws, caught by activate_hardware in RM. The exception propagates before + // HardwareComponent::activate() can record the new state, so the component stays INACTIVE. + EXPECT_EQ(rm.set_component_state("ExceptionSystem", active_state), return_type::ERROR); + + auto status = rm.get_components_status(); + EXPECT_EQ( + status["ExceptionSystem"].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); +} + +class ResourceManagerActuatorExceptionTest : public ResourceManagerExceptionTest +{ +protected: + std::string get_exception_actuator_urdf(const std::string & param_name) + { + std::string urdf = R"( + + + + + + + + + + + + + test_actuator + true + + + + + + + + +)"; + return urdf; + } +}; + +TEST_F(ResourceManagerActuatorExceptionTest, catch_read_exception) +{ + std::string urdf = get_exception_actuator_urdf("throw_on_read"); + ResourceManager rm( + urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); + rclcpp_lifecycle::State active_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + + EXPECT_EQ(rm.set_component_state("ExceptionActuator", inactive_state), return_type::OK); + EXPECT_EQ(rm.set_component_state("ExceptionActuator", active_state), return_type::OK); + + auto [result, failed_hardware] = + rm.read(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); + + EXPECT_EQ(result, return_type::ERROR); + ASSERT_EQ(failed_hardware.size(), 1u); + EXPECT_EQ(failed_hardware[0], "ExceptionActuator"); + + auto status = rm.get_components_status(); + EXPECT_EQ( + status["ExceptionActuator"].state.label(), + hardware_interface::lifecycle_state_names::UNCONFIGURED); +} + +TEST_F(ResourceManagerActuatorExceptionTest, catch_write_exception) +{ + std::string urdf = get_exception_actuator_urdf("throw_on_write"); + ResourceManager rm( + urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); + rclcpp_lifecycle::State active_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + + EXPECT_EQ(rm.set_component_state("ExceptionActuator", inactive_state), return_type::OK); + EXPECT_EQ(rm.set_component_state("ExceptionActuator", active_state), return_type::OK); + + auto [result, failed_hardware] = + rm.write(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); + + EXPECT_EQ(result, return_type::ERROR); + + auto status = rm.get_components_status(); + EXPECT_EQ( + status["ExceptionActuator"].state.label(), + hardware_interface::lifecycle_state_names::UNCONFIGURED); +} + +TEST_F(ResourceManagerActuatorExceptionTest, catch_configure_exception) +{ + std::string urdf = get_exception_actuator_urdf("throw_on_configure"); + ResourceManager rm( + urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); + + EXPECT_EQ(rm.set_component_state("ExceptionActuator", inactive_state), return_type::ERROR); + + auto status = rm.get_components_status(); + EXPECT_EQ( + status["ExceptionActuator"].state.label(), + hardware_interface::lifecycle_state_names::UNCONFIGURED); +} + +TEST_F(ResourceManagerActuatorExceptionTest, catch_activate_exception) +{ + std::string urdf = get_exception_actuator_urdf("throw_on_activate"); + ResourceManager rm( + urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); + rclcpp_lifecycle::State active_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + + EXPECT_EQ(rm.set_component_state("ExceptionActuator", inactive_state), return_type::OK); + EXPECT_EQ(rm.set_component_state("ExceptionActuator", active_state), return_type::ERROR); + + auto status = rm.get_components_status(); + EXPECT_EQ( + status["ExceptionActuator"].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); +} + +class ResourceManagerSensorExceptionTest : public ResourceManagerExceptionTest +{ +protected: + std::string get_exception_sensor_urdf(const std::string & param_name) + { + std::string urdf = R"( + + + + + + test_sensor + true + + + + + + +)"; + return urdf; + } +}; + +TEST_F(ResourceManagerSensorExceptionTest, catch_read_exception) +{ + std::string urdf = get_exception_sensor_urdf("throw_on_read"); + ResourceManager rm( + urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); + rclcpp_lifecycle::State active_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + + EXPECT_EQ(rm.set_component_state("ExceptionSensor", inactive_state), return_type::OK); + EXPECT_EQ(rm.set_component_state("ExceptionSensor", active_state), return_type::OK); + + auto [result, failed_hardware] = + rm.read(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); + + EXPECT_EQ(result, return_type::ERROR); + ASSERT_EQ(failed_hardware.size(), 1u); + EXPECT_EQ(failed_hardware[0], "ExceptionSensor"); + + auto status = rm.get_components_status(); + EXPECT_EQ( + status["ExceptionSensor"].state.label(), + hardware_interface::lifecycle_state_names::UNCONFIGURED); +} + +TEST_F(ResourceManagerSensorExceptionTest, catch_configure_exception) +{ + std::string urdf = get_exception_sensor_urdf("throw_on_configure"); + ResourceManager rm( + urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); + + EXPECT_EQ(rm.set_component_state("ExceptionSensor", inactive_state), return_type::ERROR); + + auto status = rm.get_components_status(); + EXPECT_EQ( + status["ExceptionSensor"].state.label(), + hardware_interface::lifecycle_state_names::UNCONFIGURED); +} + +TEST_F(ResourceManagerSensorExceptionTest, catch_activate_exception) +{ + std::string urdf = get_exception_sensor_urdf("throw_on_activate"); + ResourceManager rm( + urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); + rclcpp_lifecycle::State active_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + + EXPECT_EQ(rm.set_component_state("ExceptionSensor", inactive_state), return_type::OK); + EXPECT_EQ(rm.set_component_state("ExceptionSensor", active_state), return_type::ERROR); + + auto status = rm.get_components_status(); + EXPECT_EQ( + status["ExceptionSensor"].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} From 03e30d40c9b74b5e9584b6ca9d1fc8f7c21a2c56 Mon Sep 17 00:00:00 2001 From: shlok-mehndiratta Date: Thu, 16 Apr 2026 04:55:51 +0530 Subject: [PATCH 2/6] Hardening: Add nominal test case and fix Jazzy compatibility --- .../test/test_rm_exception_handling.cpp | 96 ++++++++++++++++++- 1 file changed, 94 insertions(+), 2 deletions(-) diff --git a/hardware_interface_testing/test/test_rm_exception_handling.cpp b/hardware_interface_testing/test/test_rm_exception_handling.cpp index 31d0afc95e..56a735bd61 100644 --- a/hardware_interface_testing/test/test_rm_exception_handling.cpp +++ b/hardware_interface_testing/test/test_rm_exception_handling.cpp @@ -416,11 +416,103 @@ TEST_F(ResourceManagerSensorExceptionTest, catch_activate_exception) status["ExceptionSensor"].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); } +// --- Nominal Usage Tests to reach 100% coverage and verify non-interference --- + +class ResourceManagerNominalAllComponentsTest : public ResourceManagerExceptionTest +{ +protected: + std::string get_nominal_robot_all_components_urdf() + { + return R"( + + + + + + + + + + + + + + + + + + test_system + + + + + + + + + + test_actuator + + + + + + + test_sensor + + + +)"; + } +}; + +TEST_F(ResourceManagerNominalAllComponentsTest, validate_nominal_behavior) +{ + std::string urdf = get_nominal_robot_all_components_urdf(); + ResourceManager rm( + urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); + rclcpp_lifecycle::State active_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + + // Configure and Activate all + EXPECT_EQ(rm.set_component_state("NominalSystem", inactive_state), return_type::OK); + EXPECT_EQ(rm.set_component_state("NominalSystem", active_state), return_type::OK); + EXPECT_EQ(rm.set_component_state("NominalActuator", inactive_state), return_type::OK); + EXPECT_EQ(rm.set_component_state("NominalActuator", active_state), return_type::OK); + EXPECT_EQ(rm.set_component_state("NominalSensor", inactive_state), return_type::OK); + EXPECT_EQ(rm.set_component_state("NominalSensor", active_state), return_type::OK); + + // Perform read/write cycle + { + auto [result, failed_hardware] = + rm.read(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); + EXPECT_EQ(result, return_type::OK); + } + { + auto [result, failed_hardware] = + rm.write(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); + EXPECT_EQ(result, return_type::OK); + } + + // Verify all are ACTIVE + auto status = rm.get_components_status(); + EXPECT_EQ( + status["NominalSystem"].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); + EXPECT_EQ( + status["NominalActuator"].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); + EXPECT_EQ( + status["NominalSensor"].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); - rclcpp::init(argc, argv); + // Remove global rclcpp::init to satisfy branch coverage in SetUp() int result = RUN_ALL_TESTS(); - rclcpp::shutdown(); return result; } From 3f35ee8d0deb95f7516fd10ece80c753de2091dd Mon Sep 17 00:00:00 2001 From: shlok-mehndiratta Date: Thu, 16 Apr 2026 10:36:54 +0530 Subject: [PATCH 3/6] Fix CI: Restore rclcpp initialization in main and ensure cross-distro compatibility --- hardware_interface_testing/test/test_rm_exception_handling.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/hardware_interface_testing/test/test_rm_exception_handling.cpp b/hardware_interface_testing/test/test_rm_exception_handling.cpp index 56a735bd61..d662bd2508 100644 --- a/hardware_interface_testing/test/test_rm_exception_handling.cpp +++ b/hardware_interface_testing/test/test_rm_exception_handling.cpp @@ -512,7 +512,8 @@ TEST_F(ResourceManagerNominalAllComponentsTest, validate_nominal_behavior) int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); - // Remove global rclcpp::init to satisfy branch coverage in SetUp() + rclcpp::init(argc, argv); int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); return result; } From 702e69dd88d8f73e60372f6f0234e3d2c128e889 Mon Sep 17 00:00:00 2001 From: shlok-mehndiratta Date: Thu, 16 Apr 2026 13:19:43 +0530 Subject: [PATCH 4/6] Refine exception tests: Simplify nominal case and fix Humble compatibility --- .../test/test_rm_exception_handling.cpp | 169 ++++++------------ 1 file changed, 59 insertions(+), 110 deletions(-) diff --git a/hardware_interface_testing/test/test_rm_exception_handling.cpp b/hardware_interface_testing/test/test_rm_exception_handling.cpp index d662bd2508..de94e7b7c3 100644 --- a/hardware_interface_testing/test/test_rm_exception_handling.cpp +++ b/hardware_interface_testing/test/test_rm_exception_handling.cpp @@ -23,6 +23,7 @@ #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/version.h" using hardware_interface::CommandInterface; using hardware_interface::HardwareInfo; @@ -30,6 +31,17 @@ using hardware_interface::ResourceManager; using hardware_interface::return_type; using hardware_interface::StateInterface; +// Distribution-aware status check macros. +// On Humble, HardwareReadWriteStatus.result is a bool (ok). +// On Jazzy/Rolling, HardwareReadWriteStatus.result is a return_type enum. +#if RCLCPP_VERSION_MAJOR >= 28 // Jazzy and Rolling +#define EXPECT_STATUS_OK(status) EXPECT_EQ(status, hardware_interface::return_type::OK) +#define EXPECT_STATUS_ERROR(status) EXPECT_EQ(status, hardware_interface::return_type::ERROR) +#else +#define EXPECT_STATUS_OK(status) EXPECT_TRUE(status) +#define EXPECT_STATUS_ERROR(status) EXPECT_FALSE(status) +#endif + class ResourceManagerExceptionTest : public ::testing::Test { protected: @@ -44,8 +56,14 @@ class ResourceManagerExceptionTest : public ::testing::Test std::shared_ptr node_; - std::string get_exception_robot_urdf(const std::string & param_name) + std::string get_exception_robot_urdf(const std::string & param_name, bool include_param = true) { + std::string param_tag = ""; + if (include_param) + { + param_tag = "true"; + } + std::string urdf = R"( @@ -67,8 +85,9 @@ class ResourceManagerExceptionTest : public ::testing::Test test_system - true + 100 + )" + param_tag + + R"( @@ -93,8 +112,6 @@ class ResourceManagerExceptionTest : public ::testing::Test TEST_F(ResourceManagerExceptionTest, catch_read_exception) { std::string urdf = get_exception_robot_urdf("throw_on_read"); - - // Use the 5-argument constructor with node interfaces to ensure parsing success ResourceManager rm( urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); @@ -108,11 +125,10 @@ TEST_F(ResourceManagerExceptionTest, catch_read_exception) EXPECT_EQ(rm.set_component_state("ExceptionSystem", inactive_state), return_type::OK); EXPECT_EQ(rm.set_component_state("ExceptionSystem", active_state), return_type::OK); - // Read should throw, be caught, and transition component to UNCONFIGURED auto [result, failed_hardware] = rm.read(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); - EXPECT_EQ(result, return_type::ERROR); + EXPECT_STATUS_ERROR(result); ASSERT_EQ(failed_hardware.size(), 1u); EXPECT_EQ(failed_hardware[0], "ExceptionSystem"); @@ -138,11 +154,10 @@ TEST_F(ResourceManagerExceptionTest, catch_write_exception) EXPECT_EQ(rm.set_component_state("ExceptionSystem", inactive_state), return_type::OK); EXPECT_EQ(rm.set_component_state("ExceptionSystem", active_state), return_type::OK); - // Write should throw, be caught, and transition component to UNCONFIGURED auto [result, failed_hardware] = rm.write(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); - EXPECT_EQ(result, return_type::ERROR); + EXPECT_STATUS_ERROR(result); auto status = rm.get_components_status(); EXPECT_EQ( @@ -160,11 +175,9 @@ TEST_F(ResourceManagerExceptionTest, catch_configure_exception) lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE); - // on_configure throws, caught by RM → component stays UNCONFIGURED EXPECT_EQ(rm.set_component_state("ExceptionSystem", inactive_state), return_type::ERROR); auto status = rm.get_components_status(); - // After configure exception the state is not advanced — stays UNCONFIGURED EXPECT_EQ( status["ExceptionSystem"].state.label(), hardware_interface::lifecycle_state_names::UNCONFIGURED); @@ -184,9 +197,6 @@ TEST_F(ResourceManagerExceptionTest, catch_activate_exception) hardware_interface::lifecycle_state_names::ACTIVE); EXPECT_EQ(rm.set_component_state("ExceptionSystem", inactive_state), return_type::OK); - - // on_activate throws, caught by activate_hardware in RM. The exception propagates before - // HardwareComponent::activate() can record the new state, so the component stays INACTIVE. EXPECT_EQ(rm.set_component_state("ExceptionSystem", active_state), return_type::ERROR); auto status = rm.get_components_status(); @@ -194,6 +204,36 @@ TEST_F(ResourceManagerExceptionTest, catch_activate_exception) status["ExceptionSystem"].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); } +TEST_F(ResourceManagerExceptionTest, nominal_system_usage) +{ + // Test 100% coverage by omitting injection parameters + std::string urdf = get_exception_robot_urdf("", false); + ResourceManager rm( + urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); + rclcpp_lifecycle::State active_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + + EXPECT_EQ(rm.set_component_state("ExceptionSystem", inactive_state), return_type::OK); + EXPECT_EQ(rm.set_component_state("ExceptionSystem", active_state), return_type::OK); + + auto [result_read, failed_read] = + rm.read(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); + EXPECT_STATUS_OK(result_read); + + auto [result_write, failed_write] = + rm.write(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); + EXPECT_STATUS_OK(result_write); + + auto status = rm.get_components_status(); + EXPECT_EQ( + status["ExceptionSystem"].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); +} + class ResourceManagerActuatorExceptionTest : public ResourceManagerExceptionTest { protected: @@ -213,6 +253,7 @@ class ResourceManagerActuatorExceptionTest : public ResourceManagerExceptionTest test_actuator + 100 true @@ -247,7 +288,7 @@ TEST_F(ResourceManagerActuatorExceptionTest, catch_read_exception) auto [result, failed_hardware] = rm.read(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); - EXPECT_EQ(result, return_type::ERROR); + EXPECT_STATUS_ERROR(result); ASSERT_EQ(failed_hardware.size(), 1u); EXPECT_EQ(failed_hardware[0], "ExceptionActuator"); @@ -276,7 +317,7 @@ TEST_F(ResourceManagerActuatorExceptionTest, catch_write_exception) auto [result, failed_hardware] = rm.write(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); - EXPECT_EQ(result, return_type::ERROR); + EXPECT_STATUS_ERROR(result); auto status = rm.get_components_status(); EXPECT_EQ( @@ -335,6 +376,7 @@ class ResourceManagerSensorExceptionTest : public ResourceManagerExceptionTest test_sensor + 100 true @@ -367,7 +409,7 @@ TEST_F(ResourceManagerSensorExceptionTest, catch_read_exception) auto [result, failed_hardware] = rm.read(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); - EXPECT_EQ(result, return_type::ERROR); + EXPECT_STATUS_ERROR(result); ASSERT_EQ(failed_hardware.size(), 1u); EXPECT_EQ(failed_hardware[0], "ExceptionSensor"); @@ -416,99 +458,6 @@ TEST_F(ResourceManagerSensorExceptionTest, catch_activate_exception) status["ExceptionSensor"].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); } -// --- Nominal Usage Tests to reach 100% coverage and verify non-interference --- - -class ResourceManagerNominalAllComponentsTest : public ResourceManagerExceptionTest -{ -protected: - std::string get_nominal_robot_all_components_urdf() - { - return R"( - - - - - - - - - - - - - - - - - - test_system - - - - - - - - - - test_actuator - - - - - - - test_sensor - - - -)"; - } -}; - -TEST_F(ResourceManagerNominalAllComponentsTest, validate_nominal_behavior) -{ - std::string urdf = get_nominal_robot_all_components_urdf(); - ResourceManager rm( - urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); - - rclcpp_lifecycle::State inactive_state( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - hardware_interface::lifecycle_state_names::INACTIVE); - rclcpp_lifecycle::State active_state( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - hardware_interface::lifecycle_state_names::ACTIVE); - - // Configure and Activate all - EXPECT_EQ(rm.set_component_state("NominalSystem", inactive_state), return_type::OK); - EXPECT_EQ(rm.set_component_state("NominalSystem", active_state), return_type::OK); - EXPECT_EQ(rm.set_component_state("NominalActuator", inactive_state), return_type::OK); - EXPECT_EQ(rm.set_component_state("NominalActuator", active_state), return_type::OK); - EXPECT_EQ(rm.set_component_state("NominalSensor", inactive_state), return_type::OK); - EXPECT_EQ(rm.set_component_state("NominalSensor", active_state), return_type::OK); - - // Perform read/write cycle - { - auto [result, failed_hardware] = - rm.read(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); - EXPECT_EQ(result, return_type::OK); - } - { - auto [result, failed_hardware] = - rm.write(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); - EXPECT_EQ(result, return_type::OK); - } - - // Verify all are ACTIVE - auto status = rm.get_components_status(); - EXPECT_EQ( - status["NominalSystem"].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); - EXPECT_EQ( - status["NominalActuator"].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); - EXPECT_EQ( - status["NominalSensor"].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); -} - int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); From 3bbc2e14dc1ae6363de76713be7ae6abd6fe59be Mon Sep 17 00:00:00 2001 From: shlok-mehndiratta Date: Thu, 16 Apr 2026 15:41:32 +0530 Subject: [PATCH 5/6] [testing] Final hardening and coverage fixes for exception suite --- .../test/test_rm_exception_handling.cpp | 173 ++++++++++++++---- 1 file changed, 142 insertions(+), 31 deletions(-) diff --git a/hardware_interface_testing/test/test_rm_exception_handling.cpp b/hardware_interface_testing/test/test_rm_exception_handling.cpp index de94e7b7c3..03a1bd5a28 100644 --- a/hardware_interface_testing/test/test_rm_exception_handling.cpp +++ b/hardware_interface_testing/test/test_rm_exception_handling.cpp @@ -23,7 +23,6 @@ #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/rclcpp.hpp" -#include "rclcpp/version.h" using hardware_interface::CommandInterface; using hardware_interface::HardwareInfo; @@ -31,26 +30,11 @@ using hardware_interface::ResourceManager; using hardware_interface::return_type; using hardware_interface::StateInterface; -// Distribution-aware status check macros. -// On Humble, HardwareReadWriteStatus.result is a bool (ok). -// On Jazzy/Rolling, HardwareReadWriteStatus.result is a return_type enum. -#if RCLCPP_VERSION_MAJOR >= 28 // Jazzy and Rolling -#define EXPECT_STATUS_OK(status) EXPECT_EQ(status, hardware_interface::return_type::OK) -#define EXPECT_STATUS_ERROR(status) EXPECT_EQ(status, hardware_interface::return_type::ERROR) -#else -#define EXPECT_STATUS_OK(status) EXPECT_TRUE(status) -#define EXPECT_STATUS_ERROR(status) EXPECT_FALSE(status) -#endif - class ResourceManagerExceptionTest : public ::testing::Test { protected: void SetUp() override { - if (!rclcpp::ok()) - { - rclcpp::init(0, nullptr); - } node_ = std::make_shared("ResourceManagerExceptionTestNode"); } @@ -128,7 +112,7 @@ TEST_F(ResourceManagerExceptionTest, catch_read_exception) auto [result, failed_hardware] = rm.read(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); - EXPECT_STATUS_ERROR(result); + EXPECT_EQ(result, return_type::ERROR); ASSERT_EQ(failed_hardware.size(), 1u); EXPECT_EQ(failed_hardware[0], "ExceptionSystem"); @@ -157,7 +141,7 @@ TEST_F(ResourceManagerExceptionTest, catch_write_exception) auto [result, failed_hardware] = rm.write(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); - EXPECT_STATUS_ERROR(result); + EXPECT_EQ(result, return_type::ERROR); auto status = rm.get_components_status(); EXPECT_EQ( @@ -206,8 +190,33 @@ TEST_F(ResourceManagerExceptionTest, catch_activate_exception) TEST_F(ResourceManagerExceptionTest, nominal_system_usage) { - // Test 100% coverage by omitting injection parameters - std::string urdf = get_exception_robot_urdf("", false); + // exercises the 'Param Missing' branches and the is_async=true branch + std::string urdf = R"( + + + + + + + + + + + + + test_system + 100 + + + + + + + + + + +)"; ResourceManager rm( urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); @@ -218,20 +227,51 @@ TEST_F(ResourceManagerExceptionTest, nominal_system_usage) lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); - EXPECT_EQ(rm.set_component_state("ExceptionSystem", inactive_state), return_type::OK); - EXPECT_EQ(rm.set_component_state("ExceptionSystem", active_state), return_type::OK); + EXPECT_EQ(rm.set_component_state("NominalSystem", inactive_state), return_type::OK); + EXPECT_EQ(rm.set_component_state("NominalSystem", active_state), return_type::OK); auto [result_read, failed_read] = rm.read(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); - EXPECT_STATUS_OK(result_read); - - auto [result_write, failed_write] = - rm.write(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); - EXPECT_STATUS_OK(result_write); + EXPECT_EQ(result_read, return_type::OK); auto status = rm.get_components_status(); EXPECT_EQ( - status["ExceptionSystem"].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); + status["NominalSystem"].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); +} + +TEST_F(ResourceManagerExceptionTest, initialization_failure) +{ + // Cover the CallbackReturn::ERROR line by omitting update_rate + std::string urdf = R"( + + + + + + + + + + + + + test_system + + + + + + + + +)"; + // RM construction will fail to initialize the component due to missing update_rate + ResourceManager rm( + urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 0); + + auto status = rm.get_components_status(); + // It stays 'unknown' because on_init failed in the constructor + EXPECT_EQ(status["FailSystem"].state.label(), "unknown"); } class ResourceManagerActuatorExceptionTest : public ResourceManagerExceptionTest @@ -288,7 +328,7 @@ TEST_F(ResourceManagerActuatorExceptionTest, catch_read_exception) auto [result, failed_hardware] = rm.read(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); - EXPECT_STATUS_ERROR(result); + EXPECT_EQ(result, return_type::ERROR); ASSERT_EQ(failed_hardware.size(), 1u); EXPECT_EQ(failed_hardware[0], "ExceptionActuator"); @@ -317,7 +357,7 @@ TEST_F(ResourceManagerActuatorExceptionTest, catch_write_exception) auto [result, failed_hardware] = rm.write(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); - EXPECT_STATUS_ERROR(result); + EXPECT_EQ(result, return_type::ERROR); auto status = rm.get_components_status(); EXPECT_EQ( @@ -364,6 +404,46 @@ TEST_F(ResourceManagerActuatorExceptionTest, catch_activate_exception) status["ExceptionActuator"].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); } +TEST_F(ResourceManagerActuatorExceptionTest, nominal_actuator_usage) +{ + // exercises the 'Param Missing' branches for Actuator + std::string urdf = R"( + + + + + + + + + + + + + test_actuator + 100 + + + + + + + + +)"; + ResourceManager rm( + urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); + EXPECT_EQ(rm.set_component_state("NominalActuator", inactive_state), return_type::OK); + + auto status = rm.get_components_status(); + EXPECT_EQ( + status["NominalActuator"].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); +} + class ResourceManagerSensorExceptionTest : public ResourceManagerExceptionTest { protected: @@ -409,7 +489,7 @@ TEST_F(ResourceManagerSensorExceptionTest, catch_read_exception) auto [result, failed_hardware] = rm.read(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); - EXPECT_STATUS_ERROR(result); + EXPECT_EQ(result, return_type::ERROR); ASSERT_EQ(failed_hardware.size(), 1u); EXPECT_EQ(failed_hardware[0], "ExceptionSensor"); @@ -458,6 +538,37 @@ TEST_F(ResourceManagerSensorExceptionTest, catch_activate_exception) status["ExceptionSensor"].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); } +TEST_F(ResourceManagerSensorExceptionTest, nominal_sensor_usage) +{ + // exercises the 'Param Missing' branches for Sensor + std::string urdf = R"( + + + + + + test_sensor + 100 + + + + + + +)"; + ResourceManager rm( + urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + + rclcpp_lifecycle::State inactive_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); + EXPECT_EQ(rm.set_component_state("NominalSensor", inactive_state), return_type::OK); + + auto status = rm.get_components_status(); + EXPECT_EQ( + status["NominalSensor"].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); From 45ccf52f4c0d7457b6876d6067ecc1a00c541d52 Mon Sep 17 00:00:00 2001 From: shlok-mehndiratta Date: Sun, 19 Apr 2026 13:28:48 +0530 Subject: [PATCH 6/6] Hardening exception handling in ResourceManager and adding comprehensive test suite (#1530) --- hardware_interface/src/resource_manager.cpp | 42 +- hardware_interface_testing/CMakeLists.txt | 2 + .../test/test_components/test_actuator.cpp | 131 +- .../test/test_components/test_sensor.cpp | 93 +- .../test/test_components/test_system.cpp | 122 +- .../test/test_rm_exception_handling.cpp | 1099 ++++++++++------- 6 files changed, 924 insertions(+), 565 deletions(-) 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 f51a8aa6ab..04742a757a 100644 --- a/hardware_interface_testing/CMakeLists.txt +++ b/hardware_interface_testing/CMakeLists.txt @@ -58,6 +58,8 @@ if(BUILD_TESTING) 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}) diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index 9c2999f261..c48a6669ff 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -12,11 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include #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" @@ -27,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 { @@ -35,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; @@ -48,50 +71,61 @@ class TestActuator : public ActuatorInterface return CallbackReturn::ERROR; } - auto it = get_hardware_info().hardware_parameters.find("throw_on_read"); - if (it != get_hardware_info().hardware_parameters.end()) - { - throw_on_read_ = (it->second == "true"); - } - it = get_hardware_info().hardware_parameters.find("throw_on_write"); - if (it != get_hardware_info().hardware_parameters.end()) - { - throw_on_write_ = (it->second == "true"); - } - it = get_hardware_info().hardware_parameters.find("throw_on_configure"); - if (it != get_hardware_info().hardware_parameters.end()) - { - throw_on_configure_ = (it->second == "true"); - } - it = get_hardware_info().hardware_parameters.find("throw_on_activate"); - if (it != get_hardware_info().hardware_parameters.end()) - { - throw_on_activate_ = (it->second == "true"); - } + /* + * a hardware can optional prove for incorrect info here. + * + * // can only control one joint + * if (get_hardware_info().joints.size() != 1) {return CallbackReturn::ERROR;} + * // can only control in position + * if (get_hardware_info().joints[0].command_interfaces.size() != 1) {return + * CallbackReturn::ERROR;} + * // can only give feedback state for position and velocity + * if (get_hardware_info().joints[0].state_interfaces.size() != 2) {return + * CallbackReturn::ERROR;} + */ return CallbackReturn::SUCCESS; } - CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override { - if (throw_on_configure_) - { - throw std::runtime_error("Injected exception during on_configure!"); - } - return CallbackReturn::SUCCESS; + check_injected_failure("on_configure"); + return ActuatorInterface::on_configure(previous_state); } - CallbackReturn on_activate(const rclcpp_lifecycle::State & /*previous_state*/) override + CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override { - if (throw_on_activate_) - { - throw std::runtime_error("Injected exception during on_activate!"); - } - return CallbackReturn::SUCCESS; + 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); @@ -110,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); @@ -129,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); @@ -139,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 ( @@ -159,15 +196,11 @@ class TestActuator : public ActuatorInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period) override { - if (throw_on_read_) - { - throw std::runtime_error("Exception from TestActuator::read() as requested by parameter."); - } - + check_injected_failure("read"); if (get_hardware_info().is_async) { std::this_thread::sleep_for( - std::chrono::milliseconds(1000 / (6 * get_hardware_info().rw_rate))); + std::chrono::milliseconds(1000 / (3 * get_hardware_info().rw_rate))); } double vel_cmd = 0.0; std::ignore = velocity_command_->get_value(vel_cmd, false); @@ -213,11 +246,7 @@ class TestActuator : public ActuatorInterface return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { - if (throw_on_write_) - { - throw std::runtime_error("Exception from TestActuator::write() as requested by parameter."); - } - + check_injected_failure("write"); if (get_hardware_info().is_async) { std::this_thread::sleep_for( @@ -250,10 +279,6 @@ class TestActuator : public ActuatorInterface } private: - bool throw_on_read_ = false; - bool throw_on_write_ = false; - bool throw_on_configure_ = false; - bool throw_on_activate_ = false; StateInterface::SharedPtr position_state_; StateInterface::SharedPtr velocity_state_; CommandInterface::SharedPtr velocity_command_; diff --git a/hardware_interface_testing/test/test_components/test_sensor.cpp b/hardware_interface_testing/test/test_components/test_sensor.cpp index 05ddae2eb2..e10172ef76 100644 --- a/hardware_interface_testing/test/test_components/test_sensor.cpp +++ b/hardware_interface_testing/test/test_components/test_sensor.cpp @@ -12,10 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include #include +#include "hardware_interface/lexical_casts.hpp" #include "hardware_interface/sensor_interface.hpp" #include "rclcpp/logging.hpp" @@ -25,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 { @@ -32,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) { @@ -45,46 +68,48 @@ class TestSensor : public SensorInterface get_hardware_info().name.c_str(), get_hardware_info().hardware_plugin_name.c_str()); return CallbackReturn::ERROR; } + return CallbackReturn::SUCCESS; + } - auto it = get_hardware_info().hardware_parameters.find("throw_on_read"); - if (it != get_hardware_info().hardware_parameters.end()) - { - throw_on_read_ = (it->second == "true"); - } - it = get_hardware_info().hardware_parameters.find("throw_on_configure"); - if (it != get_hardware_info().hardware_parameters.end()) - { - throw_on_configure_ = (it->second == "true"); - } - it = get_hardware_info().hardware_parameters.find("throw_on_activate"); - if (it != get_hardware_info().hardware_parameters.end()) - { - throw_on_activate_ = (it->second == "true"); - } + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override + { + check_injected_failure("on_configure"); + return SensorInterface::on_configure(previous_state); + } - return CallbackReturn::SUCCESS; + CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override + { + check_injected_failure("on_cleanup"); + return SensorInterface::on_cleanup(previous_state); } - CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override { - if (throw_on_configure_) - { - throw std::runtime_error("Injected exception during on_configure!"); - } - return CallbackReturn::SUCCESS; + check_injected_failure("on_activate"); + return SensorInterface::on_activate(previous_state); } - CallbackReturn on_activate(const rclcpp_lifecycle::State & /*previous_state*/) override + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override { - if (throw_on_activate_) - { - throw std::runtime_error("Injected exception during on_activate!"); - } - return CallbackReturn::SUCCESS; + 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); @@ -96,17 +121,11 @@ class TestSensor : public SensorInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { - if (throw_on_read_) - { - throw std::runtime_error("Exception from TestSensor::read() as requested by parameter."); - } + check_injected_failure("read"); return return_type::OK; } private: - bool throw_on_read_ = false; - bool throw_on_configure_ = false; - bool throw_on_activate_ = false; StateInterface::SharedPtr velocity_state_; }; diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index dc81e48e02..6e6c93cf1f 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -13,12 +13,10 @@ // limitations under the License. #include -#include -#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" @@ -42,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 { @@ -51,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") { @@ -65,51 +85,48 @@ class TestSystem : public SystemInterface get_hardware_info().name.c_str(), get_hardware_info().hardware_plugin_name.c_str()); return CallbackReturn::ERROR; } + return CallbackReturn::SUCCESS; + } - auto it = get_hardware_info().hardware_parameters.find("throw_on_read"); - if (it != get_hardware_info().hardware_parameters.end()) - { - throw_on_read_ = (it->second == "true"); - } - it = get_hardware_info().hardware_parameters.find("throw_on_write"); - if (it != get_hardware_info().hardware_parameters.end()) - { - throw_on_write_ = (it->second == "true"); - } - it = get_hardware_info().hardware_parameters.find("throw_on_configure"); - if (it != get_hardware_info().hardware_parameters.end()) - { - throw_on_configure_ = (it->second == "true"); - } - it = get_hardware_info().hardware_parameters.find("throw_on_activate"); - if (it != get_hardware_info().hardware_parameters.end()) - { - throw_on_activate_ = (it->second == "true"); - } + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override + { + check_injected_failure("on_configure"); + return SystemInterface::on_configure(previous_state); + } - return CallbackReturn::SUCCESS; + CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override + { + check_injected_failure("on_cleanup"); + return SystemInterface::on_cleanup(previous_state); } - CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override { - if (throw_on_configure_) - { - throw std::runtime_error("Injected exception during on_configure!"); - } - return CallbackReturn::SUCCESS; + check_injected_failure("on_activate"); + return SystemInterface::on_activate(previous_state); } - CallbackReturn on_activate(const rclcpp_lifecycle::State & /*previous_state*/) override + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override { - if (throw_on_activate_) - { - throw std::runtime_error("Injected exception during on_activate!"); - } - return CallbackReturn::SUCCESS; + 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; @@ -143,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; @@ -173,11 +191,7 @@ class TestSystem : public SystemInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { - if (throw_on_read_) - { - throw std::runtime_error("Exception from TestSystem::read() as requested by parameter."); - } - + check_injected_failure("read"); verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); if (get_hardware_info().is_async) { @@ -218,11 +232,7 @@ class TestSystem : public SystemInterface return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { - if (throw_on_write_) - { - throw std::runtime_error("Exception from TestSystem::write() as requested by parameter."); - } - + check_injected_failure("write"); verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id()); if (get_hardware_info().is_async) { @@ -255,11 +265,23 @@ 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: - bool throw_on_read_ = false; - bool throw_on_write_ = false; - bool throw_on_configure_ = false; - bool throw_on_activate_ = false; std::array velocity_command_; std::array position_state_; std::array velocity_state_; diff --git a/hardware_interface_testing/test/test_rm_exception_handling.cpp b/hardware_interface_testing/test/test_rm_exception_handling.cpp index 03a1bd5a28..7f7adde39c 100644 --- a/hardware_interface_testing/test/test_rm_exception_handling.cpp +++ b/hardware_interface_testing/test/test_rm_exception_handling.cpp @@ -13,567 +13,840 @@ // limitations under the License. #include -#include #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::CommandInterface; -using hardware_interface::HardwareInfo; using hardware_interface::ResourceManager; +using hardware_interface::ResourceManagerParams; using hardware_interface::return_type; -using hardware_interface::StateInterface; - -class ResourceManagerExceptionTest : public ::testing::Test -{ -protected: - void SetUp() override - { - node_ = std::make_shared("ResourceManagerExceptionTestNode"); - } +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 + + + + + + + + +)"; - std::shared_ptr node_; +// 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 + + + + + +)"; - std::string get_exception_robot_urdf(const std::string & param_name, bool include_param = true) - { - std::string param_tag = ""; - if (include_param) - { - param_tag = "true"; - } - - std::string urdf = R"( - - - - - - - - - - - - - - - - - - +// 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 - 100 - )" + param_tag + - R"( - + - + - + + + + + - )"; - return urdf; + +// --------------------------------------------------------------------------- +// 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_; }; -TEST_F(ResourceManagerExceptionTest, catch_read_exception) +// --------------------------------------------------------------------------- +// 1. on_init exception — Actuator +// Covers: ResourceStorage::initialize_hardware catch(std::exception) and catch(...) +// --------------------------------------------------------------------------- +TEST_F(TestResourceManagerExceptionHandling, actuator_init_standard_exception) { - std::string urdf = get_exception_robot_urdf("throw_on_read"); - ResourceManager rm( - urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + params_.robot_description = + make_urdf(ACTUATOR_HW, " true"); - rclcpp_lifecycle::State inactive_state( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - hardware_interface::lifecycle_state_names::INACTIVE); - rclcpp_lifecycle::State active_state( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - hardware_interface::lifecycle_state_names::ACTIVE); + // handle_exceptions = true => absorb, no throw at RM level + params_.handle_exceptions = true; + EXPECT_NO_THROW({ ResourceManager rm(params_, true); }); - EXPECT_EQ(rm.set_component_state("ExceptionSystem", inactive_state), return_type::OK); - EXPECT_EQ(rm.set_component_state("ExceptionSystem", active_state), return_type::OK); + // handle_exceptions = false => propagate + params_.handle_exceptions = false; + EXPECT_THROW({ ResourceManager rm(params_, true); }, std::runtime_error); +} - auto [result, failed_hardware] = - rm.read(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); +TEST_F(TestResourceManagerExceptionHandling, actuator_init_unknown_exception) +{ + params_.robot_description = + make_urdf(ACTUATOR_HW, " true"); - EXPECT_EQ(result, return_type::ERROR); - ASSERT_EQ(failed_hardware.size(), 1u); - EXPECT_EQ(failed_hardware[0], "ExceptionSystem"); + params_.handle_exceptions = true; + EXPECT_NO_THROW({ ResourceManager rm(params_, true); }); - auto status = rm.get_components_status(); - EXPECT_EQ( - status["ExceptionSystem"].state.label(), - hardware_interface::lifecycle_state_names::UNCONFIGURED); + 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); }); } -TEST_F(ResourceManagerExceptionTest, catch_write_exception) +// --------------------------------------------------------------------------- +// 2. on_init exception — Sensor +// Covers: the sensor-branch of initialize_hardware +// --------------------------------------------------------------------------- +TEST_F(TestResourceManagerExceptionHandling, sensor_init_standard_exception) { - std::string urdf = get_exception_robot_urdf("throw_on_write"); - ResourceManager rm( - urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + params_.robot_description = + make_urdf(SENSOR_HW, " true"); - rclcpp_lifecycle::State inactive_state( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - hardware_interface::lifecycle_state_names::INACTIVE); - rclcpp_lifecycle::State active_state( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - hardware_interface::lifecycle_state_names::ACTIVE); + params_.handle_exceptions = true; + EXPECT_NO_THROW({ ResourceManager rm(params_, true); }); - EXPECT_EQ(rm.set_component_state("ExceptionSystem", inactive_state), return_type::OK); - EXPECT_EQ(rm.set_component_state("ExceptionSystem", active_state), return_type::OK); + params_.handle_exceptions = false; + EXPECT_THROW({ ResourceManager rm(params_, true); }, std::runtime_error); +} - auto [result, failed_hardware] = - rm.write(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); +// --------------------------------------------------------------------------- +// 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"); - EXPECT_EQ(result, return_type::ERROR); + params_.handle_exceptions = true; + EXPECT_NO_THROW({ ResourceManager rm(params_, true); }); - auto status = rm.get_components_status(); - EXPECT_EQ( - status["ExceptionSystem"].state.label(), - hardware_interface::lifecycle_state_names::UNCONFIGURED); + params_.handle_exceptions = false; + EXPECT_THROW({ ResourceManager rm(params_, true); }, std::runtime_error); } -TEST_F(ResourceManagerExceptionTest, catch_configure_exception) +// --------------------------------------------------------------------------- +// 4. export_state_interfaces exception +// Covers: import_state_interfaces catch(std::exception) — newly added block +// --------------------------------------------------------------------------- +TEST_F(TestResourceManagerExceptionHandling, system_export_state_interfaces_exception) { - std::string urdf = get_exception_robot_urdf("throw_on_configure"); - ResourceManager rm( - urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + params_.robot_description = + make_urdf(SYSTEM_HW, " true"); - rclcpp_lifecycle::State inactive_state( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - hardware_interface::lifecycle_state_names::INACTIVE); + params_.handle_exceptions = true; + EXPECT_NO_THROW({ ResourceManager rm(params_, true); }); - EXPECT_EQ(rm.set_component_state("ExceptionSystem", inactive_state), return_type::ERROR); + params_.handle_exceptions = false; + EXPECT_THROW({ ResourceManager rm(params_, true); }, std::runtime_error); +} - auto status = rm.get_components_status(); +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( - status["ExceptionSystem"].state.label(), - hardware_interface::lifecycle_state_names::UNCONFIGURED); + 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(ResourceManagerExceptionTest, catch_activate_exception) +TEST_F(TestResourceManagerExceptionHandling, system_activate_exception) { - std::string urdf = get_exception_robot_urdf("throw_on_activate"); - ResourceManager rm( - urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + 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, - hardware_interface::lifecycle_state_names::INACTIVE); + 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, - hardware_interface::lifecycle_state_names::ACTIVE); + 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); +} - EXPECT_EQ(rm.set_component_state("ExceptionSystem", inactive_state), return_type::OK); - EXPECT_EQ(rm.set_component_state("ExceptionSystem", active_state), return_type::ERROR); +TEST_F(TestResourceManagerExceptionHandling, system_deactivate_exception) +{ + params_.robot_description = + make_urdf(SYSTEM_HW, " true"); - auto status = rm.get_components_status(); - EXPECT_EQ( - status["ExceptionSystem"].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); -} - -TEST_F(ResourceManagerExceptionTest, nominal_system_usage) -{ - // exercises the 'Param Missing' branches and the is_async=true branch - std::string urdf = R"( - - - - - - - - - - - - - test_system - 100 - - - - - - - - - - -)"; - ResourceManager rm( - urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + // 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, - hardware_interface::lifecycle_state_names::INACTIVE); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); rclcpp_lifecycle::State active_state( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - hardware_interface::lifecycle_state_names::ACTIVE); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); - EXPECT_EQ(rm.set_component_state("NominalSystem", inactive_state), return_type::OK); - EXPECT_EQ(rm.set_component_state("NominalSystem", active_state), return_type::OK); + rm_handled.set_component_state("TestSystemHardware", inactive_state); + rm_handled.set_component_state("TestSystemHardware", active_state); - auto [result_read, failed_read] = - rm.read(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); - EXPECT_EQ(result_read, return_type::OK); - - auto status = rm.get_components_status(); + // Deactivate back to INACTIVE EXPECT_EQ( - status["NominalSystem"].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); -} - -TEST_F(ResourceManagerExceptionTest, initialization_failure) -{ - // Cover the CallbackReturn::ERROR line by omitting update_rate - std::string urdf = R"( - - - - - - - - - - - - - test_system - - - - - - - - -)"; - // RM construction will fail to initialize the component due to missing update_rate - ResourceManager rm( - urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 0); - - auto status = rm.get_components_status(); - // It stays 'unknown' because on_init failed in the constructor - EXPECT_EQ(status["FailSystem"].state.label(), "unknown"); + 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); } -class ResourceManagerActuatorExceptionTest : public ResourceManagerExceptionTest +// --------------------------------------------------------------------------- +// 7. read / write exceptions +// Covers: read_hardware and write_hardware catch blocks in ResourceManager +// --------------------------------------------------------------------------- +TEST_F(TestResourceManagerExceptionHandling, system_read_exception) { -protected: - std::string get_exception_actuator_urdf(const std::string & param_name) - { - std::string urdf = R"( - - - - - - - - - - - - - test_actuator - 100 - true - - - - - - - - -)"; - return urdf; - } -}; + 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); -TEST_F(ResourceManagerActuatorExceptionTest, catch_read_exception) + 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) { - std::string urdf = get_exception_actuator_urdf("throw_on_read"); - ResourceManager rm( - urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + 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, - hardware_interface::lifecycle_state_names::INACTIVE); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); rclcpp_lifecycle::State active_state( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - hardware_interface::lifecycle_state_names::ACTIVE); + 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.set_component_state("ExceptionActuator", inactive_state), return_type::OK); - EXPECT_EQ(rm.set_component_state("ExceptionActuator", active_state), return_type::OK); + EXPECT_NO_THROW( + rm_handled.write(rclcpp::Time(0, 0, RCL_STEADY_TIME), rclcpp::Duration::from_seconds(0.01))); - auto [result, failed_hardware] = - rm.read(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); + 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_EQ(result, return_type::ERROR); - ASSERT_EQ(failed_hardware.size(), 1u); - EXPECT_EQ(failed_hardware[0], "ExceptionActuator"); + EXPECT_THROW( + rm_throwing.write(rclcpp::Time(0, 0, RCL_STEADY_TIME), rclcpp::Duration::from_seconds(0.01)), + std::runtime_error); +} - auto status = rm.get_components_status(); +// --------------------------------------------------------------------------- +// 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( - status["ExceptionActuator"].state.label(), - hardware_interface::lifecycle_state_names::UNCONFIGURED); + 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(ResourceManagerActuatorExceptionTest, catch_write_exception) +TEST_F(TestResourceManagerExceptionHandling, system_activate_unknown_exception) { - std::string urdf = get_exception_actuator_urdf("throw_on_write"); - ResourceManager rm( - urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + 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, - hardware_interface::lifecycle_state_names::INACTIVE); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); rclcpp_lifecycle::State active_state( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - hardware_interface::lifecycle_state_names::ACTIVE); + 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)); +} - EXPECT_EQ(rm.set_component_state("ExceptionActuator", inactive_state), return_type::OK); - EXPECT_EQ(rm.set_component_state("ExceptionActuator", active_state), return_type::OK); +TEST_F(TestResourceManagerExceptionHandling, system_deactivate_unknown_exception) +{ + params_.robot_description = + make_urdf(SYSTEM_HW, " true"); - auto [result, failed_hardware] = - rm.write(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); + 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)); +} - EXPECT_EQ(result, return_type::ERROR); +// --------------------------------------------------------------------------- +// 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"); - auto status = rm.get_components_status(); + 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( - status["ExceptionActuator"].state.label(), - hardware_interface::lifecycle_state_names::UNCONFIGURED); + 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(ResourceManagerActuatorExceptionTest, catch_configure_exception) +TEST_F(TestResourceManagerExceptionHandling, system_cleanup_unknown_exception) { - std::string urdf = get_exception_actuator_urdf("throw_on_configure"); - ResourceManager rm( - urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + params_.robot_description = + make_urdf(SYSTEM_HW, " true"); rclcpp_lifecycle::State inactive_state( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - hardware_interface::lifecycle_state_names::INACTIVE); + 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); - EXPECT_EQ(rm.set_component_state("ExceptionActuator", inactive_state), return_type::ERROR); - - auto status = rm.get_components_status(); + params_.handle_exceptions = true; + ResourceManager rm_handled(params_, true); + rm_handled.set_component_state("TestSystemHardware", inactive_state); EXPECT_EQ( - status["ExceptionActuator"].state.label(), - hardware_interface::lifecycle_state_names::UNCONFIGURED); + 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)); } -TEST_F(ResourceManagerActuatorExceptionTest, catch_activate_exception) +// --------------------------------------------------------------------------- +// 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) { - std::string urdf = get_exception_actuator_urdf("throw_on_activate"); - ResourceManager rm( - urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + params_.robot_description = + make_urdf(SYSTEM_HW, " true"); - rclcpp_lifecycle::State inactive_state( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - hardware_interface::lifecycle_state_names::INACTIVE); - rclcpp_lifecycle::State active_state( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - hardware_interface::lifecycle_state_names::ACTIVE); + // 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"); - EXPECT_EQ(rm.set_component_state("ExceptionActuator", inactive_state), return_type::OK); - EXPECT_EQ(rm.set_component_state("ExceptionActuator", active_state), return_type::ERROR); + rclcpp_lifecycle::State finalized_state( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED); - auto status = rm.get_components_status(); + params_.handle_exceptions = true; + ResourceManager rm_handled(params_, true); EXPECT_EQ( - status["ExceptionActuator"].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); -} - -TEST_F(ResourceManagerActuatorExceptionTest, nominal_actuator_usage) -{ - // exercises the 'Param Missing' branches for Actuator - std::string urdf = R"( - - - - - - - - - - - + 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"( + - test_actuator - 100 + this_plugin_does_not_exist - + + + + + + + + + + + + + - )"; - ResourceManager rm( - urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); - rclcpp_lifecycle::State inactive_state( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - hardware_interface::lifecycle_state_names::INACTIVE); - EXPECT_EQ(rm.set_component_state("NominalActuator", inactive_state), return_type::OK); + params_.robot_description = std::string(ros2_control_test_assets::urdf_head) + BAD_PLUGIN_HW + + ros2_control_test_assets::urdf_tail; - auto status = rm.get_components_status(); - EXPECT_EQ( - status["NominalActuator"].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); + // 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); } -class ResourceManagerSensorExceptionTest : public ResourceManagerExceptionTest +// --------------------------------------------------------------------------- +// 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) { -protected: - std::string get_exception_sensor_urdf(const std::string & param_name) - { - std::string urdf = R"( - - - - - - test_sensor - 100 - true - - - - - - -)"; - return urdf; - } -}; + params_.robot_description = + make_urdf(SYSTEM_HW, " true"); -TEST_F(ResourceManagerSensorExceptionTest, catch_read_exception) + 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) { - std::string urdf = get_exception_sensor_urdf("throw_on_read"); - ResourceManager rm( - urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + params_.robot_description = make_urdf( + SYSTEM_HW, " true"); rclcpp_lifecycle::State inactive_state( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - hardware_interface::lifecycle_state_names::INACTIVE); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); rclcpp_lifecycle::State active_state( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - hardware_interface::lifecycle_state_names::ACTIVE); + 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"}, {})); +} - EXPECT_EQ(rm.set_component_state("ExceptionSensor", inactive_state), return_type::OK); - EXPECT_EQ(rm.set_component_state("ExceptionSensor", active_state), return_type::OK); +// --------------------------------------------------------------------------- +// 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"); - auto [result, failed_hardware] = - rm.read(node_->get_clock()->now(), rclcpp::Duration(std::chrono::milliseconds(10))); + 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); +} - EXPECT_EQ(result, return_type::ERROR); - ASSERT_EQ(failed_hardware.size(), 1u); - EXPECT_EQ(failed_hardware[0], "ExceptionSensor"); +TEST_F(TestResourceManagerExceptionHandling, perform_command_mode_switch_unknown_exception) +{ + params_.robot_description = make_urdf( + SYSTEM_HW, " true"); - auto status = rm.get_components_status(); - EXPECT_EQ( - status["ExceptionSensor"].state.label(), - hardware_interface::lifecycle_state_names::UNCONFIGURED); + 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(ResourceManagerSensorExceptionTest, catch_configure_exception) +TEST_F(TestResourceManagerExceptionHandling, system_write_unknown_exception) { - std::string urdf = get_exception_sensor_urdf("throw_on_configure"); - ResourceManager rm( - urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + params_.robot_description = + make_urdf(SYSTEM_HW, " true"); rclcpp_lifecycle::State inactive_state( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - hardware_interface::lifecycle_state_names::INACTIVE); + 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))); +} - EXPECT_EQ(rm.set_component_state("ExceptionSensor", inactive_state), return_type::ERROR); +// --------------------------------------------------------------------------- +// 16. Actuator read / write exceptions +// --------------------------------------------------------------------------- +TEST_F(TestResourceManagerExceptionHandling, actuator_read_write_exception) +{ + params_.robot_description = make_urdf( + ACTUATOR_HW, + " true\n" + " true"); - auto status = rm.get_components_status(); - EXPECT_EQ( - status["ExceptionSensor"].state.label(), - hardware_interface::lifecycle_state_names::UNCONFIGURED); + 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(ResourceManagerSensorExceptionTest, catch_activate_exception) +TEST_F(TestResourceManagerExceptionHandling, actuator_read_write_unknown_exception) { - std::string urdf = get_exception_sensor_urdf("throw_on_activate"); - ResourceManager rm( - urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + params_.robot_description = make_urdf( + ACTUATOR_HW, + " true\n" + " true"); rclcpp_lifecycle::State inactive_state( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - hardware_interface::lifecycle_state_names::INACTIVE); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); rclcpp_lifecycle::State active_state( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - hardware_interface::lifecycle_state_names::ACTIVE); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); - EXPECT_EQ(rm.set_component_state("ExceptionSensor", inactive_state), return_type::OK); - EXPECT_EQ(rm.set_component_state("ExceptionSensor", active_state), return_type::ERROR); + params_.handle_exceptions = false; + ResourceManager rm(params_, true); + rm.set_component_state("TestActuatorHardware", inactive_state); + rm.set_component_state("TestActuatorHardware", active_state); - auto status = rm.get_components_status(); - EXPECT_EQ( - status["ExceptionSensor"].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); + 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))); } -TEST_F(ResourceManagerSensorExceptionTest, nominal_sensor_usage) +// --------------------------------------------------------------------------- +// 17. Sensor read exception +// --------------------------------------------------------------------------- +TEST_F(TestResourceManagerExceptionHandling, sensor_read_exception) { - // exercises the 'Param Missing' branches for Sensor - std::string urdf = R"( - - - - - - test_sensor - 100 - - - - - - -)"; - ResourceManager rm( - urdf, node_->get_node_clock_interface(), node_->get_node_logging_interface(), true, 100); + params_.robot_description = + make_urdf(SENSOR_HW, " true"); rclcpp_lifecycle::State inactive_state( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - hardware_interface::lifecycle_state_names::INACTIVE); - EXPECT_EQ(rm.set_component_state("NominalSensor", inactive_state), return_type::OK); + 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); - auto status = rm.get_components_status(); - EXPECT_EQ( - status["NominalSensor"].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); + 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); } -int main(int argc, char ** argv) +TEST_F(TestResourceManagerExceptionHandling, sensor_read_unknown_exception) { - testing::InitGoogleTest(&argc, argv); - rclcpp::init(argc, argv); - int result = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return result; + 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))); }