From 8163ddce598059a254bdb896fc009a79d7b71daf Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 24 Nov 2025 12:54:13 +0100 Subject: [PATCH] Add `handle_exceptions` parameter to controller manager (#2807) --- controller_manager/src/controller_manager.cpp | 17 ++ .../src/controller_manager_parameters.yaml | 7 + .../test/controller_manager_test_common.hpp | 9 +- .../test/test_controller/test_controller.cpp | 13 +- .../test/test_controller/test_controller.hpp | 3 + .../test/test_controller_manager.cpp | 261 ++++++++++++++++++ ...roller_manager_hardware_error_handling.cpp | 5 +- .../test_controller_manager_urdf_passing.cpp | 5 +- ...llers_chaining_with_controller_manager.cpp | 5 +- doc/debugging.rst | 15 + doc/release_notes.rst | 1 + .../hardware_interface/resource_manager.hpp | 1 + .../types/resource_manager_params.hpp | 7 + hardware_interface/src/resource_manager.cpp | 57 +++- 14 files changed, 387 insertions(+), 19 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index c651a30020..9fcb86baaf 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -977,6 +977,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c get_logger(), "Caught exception of type : %s while loading the controller '%s' of plugin type '%s':\n%s", typeid(e).name(), controller_name.c_str(), controller_type.c_str(), e.what()); + params_->handle_exceptions ? void() : throw; return nullptr; } catch (...) @@ -1187,6 +1188,7 @@ controller_interface::return_type ControllerManager::cleanup_controller( RCLCPP_ERROR( get_logger(), "Caught exception while cleaning-up the controller '%s'", controller.info.name.c_str()); + params_->handle_exceptions ? void() : throw; return controller_interface::return_type::ERROR; } return controller_interface::return_type::OK; @@ -1211,12 +1213,14 @@ void ControllerManager::shutdown_controller( get_logger(), "Caught exception of type : %s while shutdown the controller '%s' before unloading: %s", typeid(e).name(), controller.info.name.c_str(), e.what()); + params_->handle_exceptions ? void() : throw; } catch (...) { RCLCPP_ERROR( get_logger(), "Failed to shutdown the controller '%s' before unloading", controller.info.name.c_str()); + params_->handle_exceptions ? void() : throw; } } @@ -1287,6 +1291,7 @@ controller_interface::return_type ControllerManager::configure_controller( RCLCPP_ERROR( get_logger(), "Caught exception of type : %s while configuring controller '%s': %s", typeid(e).name(), controller_name.c_str(), e.what()); + params_->handle_exceptions ? void() : throw; return controller_interface::return_type::ERROR; } catch (...) @@ -1294,6 +1299,7 @@ controller_interface::return_type ControllerManager::configure_controller( RCLCPP_ERROR( get_logger(), "Caught unknown exception while configuring controller '%s'", controller_name.c_str()); + params_->handle_exceptions ? void() : throw; return controller_interface::return_type::ERROR; } @@ -1348,6 +1354,7 @@ controller_interface::return_type ControllerManager::configure_controller( RCLCPP_FATAL( get_logger(), "Export of the state or reference interfaces failed with following error: %s", e.what()); + params_->handle_exceptions ? void() : throw; return controller_interface::return_type::ERROR; } resource_manager_->import_controller_reference_interfaces(controller_name, ref_interfaces); @@ -2133,6 +2140,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co RCLCPP_ERROR( get_logger(), "Caught exception of type : %s while initializing controller '%s': %s", typeid(e).name(), controller.info.name.c_str(), e.what()); + params_->handle_exceptions ? void() : throw; return nullptr; } catch (...) @@ -2141,6 +2149,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co RCLCPP_ERROR( get_logger(), "Caught unknown exception while initializing controller '%s'", controller.info.name.c_str()); + params_->handle_exceptions ? void() : throw; return nullptr; } @@ -2210,6 +2219,7 @@ void ControllerManager::deactivate_controllers( RCLCPP_ERROR( get_logger(), "Caught exception of type : %s while deactivating the controller '%s': %s", typeid(e).name(), controller_name.c_str(), e.what()); + params_->handle_exceptions ? void() : throw; continue; } catch (...) @@ -2217,6 +2227,7 @@ void ControllerManager::deactivate_controllers( RCLCPP_ERROR( get_logger(), "Caught unknown exception while deactivating the controller '%s'", controller_name.c_str()); + params_->handle_exceptions ? void() : throw; continue; } } @@ -2333,6 +2344,7 @@ void ControllerManager::activate_controllers( "Caught exception of type : %s while claiming the command interfaces. Can't activate " "controller '%s': %s", typeid(e).name(), controller_name.c_str(), e.what()); + params_->handle_exceptions ? void() : throw; command_loans.clear(); assignment_successful = false; break; @@ -2374,6 +2386,7 @@ void ControllerManager::activate_controllers( "controller '%s': %s", typeid(e).name(), controller_name.c_str(), e.what()); assignment_successful = false; + params_->handle_exceptions ? void() : throw; break; } } @@ -2397,12 +2410,14 @@ void ControllerManager::activate_controllers( RCLCPP_ERROR( get_logger(), "Caught exception of type : %s while activating the controller '%s': %s", typeid(e).name(), controller_name.c_str(), e.what()); + params_->handle_exceptions ? void() : throw; } catch (...) { RCLCPP_ERROR( get_logger(), "Caught unknown exception while activating the controller '%s'", controller_name.c_str()); + params_->handle_exceptions ? void() : throw; } if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { @@ -3127,6 +3142,7 @@ controller_interface::return_type ControllerManager::update( RCLCPP_ERROR( get_logger(), "Caught exception of type : %s while updating controller '%s': %s", typeid(e).name(), loaded_controller.info.name.c_str(), e.what()); + params_->handle_exceptions ? void() : throw; controller_ret = controller_interface::return_type::ERROR; } catch (...) @@ -3134,6 +3150,7 @@ controller_interface::return_type ControllerManager::update( RCLCPP_ERROR( get_logger(), "Caught unknown exception while updating controller '%s'", loaded_controller.info.name.c_str()); + params_->handle_exceptions ? void() : throw; controller_ret = controller_interface::return_type::ERROR; } diff --git a/controller_manager/src/controller_manager_parameters.yaml b/controller_manager/src/controller_manager_parameters.yaml index 7d2693a34c..eea2a0cbf9 100644 --- a/controller_manager/src/controller_manager_parameters.yaml +++ b/controller_manager/src/controller_manager_parameters.yaml @@ -13,6 +13,13 @@ controller_manager: description: "If true, the controller manager will enforce command limits defined in the robot description. If false, no limits will be enforced. If true, when the command is outside the limits, the command is clamped to be within the limits depending on the type of configured joint limits defined in the robot description. If the command is within the limits, the command is passed through without any changes.", } + handle_exceptions: { + type: bool, + default_value: true, + read_only: true, + description: "If true, the controller manager will catch exceptions thrown during the different operations of controllers and hardware components. If false, exceptions will propagate up and will cause the controller manager to crash.", + } + hardware_components_initial_state: unconfigured: { type: string_array, diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 5a6d53da1c..decf83a95e 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -57,14 +57,19 @@ class ControllerManagerFixture : public ::testing::Test public: explicit ControllerManagerFixture( const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf, - const std::string & cm_namespace = "") + const std::string & cm_namespace = "", std::vector cm_parameters = {}) : robot_description_(robot_description) { executor_ = std::make_shared(); + rclcpp::NodeOptions cm_node_options = controller_manager::get_cm_node_options(); + if (!cm_parameters.empty()) + { + cm_node_options.parameter_overrides(cm_parameters); + } cm_ = std::make_shared( std::make_unique( rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface()), - executor_, TEST_CM_NAME, cm_namespace); + executor_, TEST_CM_NAME, cm_namespace, cm_node_options); // We want to be able to not pass robot description immediately if (!robot_description_.empty()) { diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 31a0c8174a..7f1c848554 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -61,6 +61,10 @@ controller_interface::InterfaceConfiguration TestController::state_interface_con controller_interface::return_type TestController::update( const rclcpp::Time & time, const rclcpp::Duration & period) { + if (throw_on_update) + { + throw std::runtime_error("Exception from TestController::update() as requested."); + } if (time.get_clock_type() != RCL_ROS_TIME) { throw std::runtime_error("ROS Time is required for the controller to operate."); @@ -101,7 +105,14 @@ controller_interface::return_type TestController::update( return controller_interface::return_type::OK; } -CallbackReturn TestController::on_init() { return CallbackReturn::SUCCESS; } +CallbackReturn TestController::on_init() +{ + if (throw_on_initialize) + { + throw std::runtime_error("Exception from TestController::on_init() as requested."); + } + return CallbackReturn::SUCCESS; +} CallbackReturn TestController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index aa15796323..edff2a58f6 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -83,6 +83,9 @@ class TestController : public controller_interface::ControllerInterface // errors double set_first_command_interface_value_to; rclcpp::Duration update_period_ = rclcpp::Duration::from_seconds(0.); + + bool throw_on_initialize = false; + bool throw_on_update = false; }; } // namespace test_controller diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 77072b9b1e..0807ac9b69 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -3037,3 +3037,264 @@ TEST_F( test_controller->get_lifecycle_state().id()); } } + +class TestControllerManagerWithHandlingExceptions +: public ControllerManagerFixture +{ +}; + +TEST_F(TestControllerManagerWithHandlingExceptions, controller_lifecycle_on_exceptions) +{ + auto test_controller = std::make_shared(); + + test_controller->throw_on_initialize = true; + + EXPECT_NO_THROW(cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME)); + EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); + + test_controller->throw_on_initialize = false; + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller.use_count()); + + // setup interface to claim from controllers + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + cmd_itfs_cfg.names.push_back(interface); + } + test_controller->set_command_interface_configuration(cmd_itfs_cfg); + + controller_interface::InterfaceConfiguration state_itfs_cfg; + state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + test_controller->set_state_interface_configuration(state_itfs_cfg); + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller->internal_counter) + << "Update should not reach an unconfigured controller"; + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); + + // configure controller + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME)); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); + + std::vector start_controllers = {test_controller::TEST_CONTROLLER_NAME}; + std::vector stop_controllers = {}; + { + ControllerManagerRunner cm_runner(this); + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); + + test_controller->throw_on_update = true; + EXPECT_NO_THROW(cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); + + test_controller->throw_on_update = false; + { + ControllerManagerRunner cm_runner(this); + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + { + ControllerManagerRunner cm_runner(this); + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + stop_controllers, start_controllers, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); +} + +class TestControllerManagerNotHandlingExceptions +: public ControllerManagerFixture +{ +public: + TestControllerManagerNotHandlingExceptions() + : ControllerManagerFixture( + ros2_control_test_assets::minimal_robot_urdf, "", + {rclcpp::Parameter("handle_exceptions", false)}) + { + } +}; + +TEST_F(TestControllerManagerNotHandlingExceptions, controller_lifecycle_on_exceptions) +{ + auto test_controller = std::make_shared(); + + test_controller->throw_on_initialize = true; + + EXPECT_THROW( + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME), + std::runtime_error); + EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); + + test_controller->throw_on_initialize = false; + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller.use_count()); + + // setup interface to claim from controllers + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + cmd_itfs_cfg.names.push_back(interface); + } + test_controller->set_command_interface_configuration(cmd_itfs_cfg); + + controller_interface::InterfaceConfiguration state_itfs_cfg; + state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + test_controller->set_state_interface_configuration(state_itfs_cfg); + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller->internal_counter) + << "Update should not reach an unconfigured controller"; + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); + + // configure controller + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME)); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); + + std::vector start_controllers = {test_controller::TEST_CONTROLLER_NAME}; + std::vector stop_controllers = {}; + { + ControllerManagerRunner cm_runner(this); + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); + + test_controller->throw_on_update = true; + EXPECT_THROW(cm_->update(time_, rclcpp::Duration::from_seconds(0.01)), std::runtime_error); + + test_controller->throw_on_update = false; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); + + { + ControllerManagerRunner cm_runner(this); + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + stop_controllers, start_controllers, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); +} diff --git a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp index e1bcc55895..7895559633 100644 --- a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp +++ b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp @@ -55,9 +55,10 @@ class TestableControllerManager : public controller_manager::ControllerManager std::unique_ptr resource_manager, std::shared_ptr executor, const std::string & manager_node_name = "controller_manager", - const std::string & node_namespace = "") + const std::string & node_namespace = "", + const rclcpp::NodeOptions & node_options = controller_manager::get_cm_node_options()) : controller_manager::ControllerManager( - std::move(resource_manager), executor, manager_node_name, node_namespace) + std::move(resource_manager), executor, manager_node_name, node_namespace, node_options) { } }; diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp index eeb6dff5f3..ed63a8c6de 100644 --- a/controller_manager/test/test_controller_manager_urdf_passing.cpp +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -46,9 +46,10 @@ class TestableControllerManager : public controller_manager::ControllerManager std::unique_ptr resource_manager, std::shared_ptr executor, const std::string & manager_node_name = "controller_manager", - const std::string & node_namespace = "") + const std::string & node_namespace = "", + const rclcpp::NodeOptions & node_options = controller_manager::get_cm_node_options()) : controller_manager::ControllerManager( - std::move(resource_manager), executor, manager_node_name, node_namespace) + std::move(resource_manager), executor, manager_node_name, node_namespace, node_options) { } }; diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index 4ba1223419..ba36446ac9 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -92,9 +92,10 @@ class TestableControllerManager : public controller_manager::ControllerManager std::unique_ptr resource_manager, std::shared_ptr executor, const std::string & manager_node_name = "controller_manager", - const std::string & node_namespace = "") + const std::string & node_namespace = "", + const rclcpp::NodeOptions & node_options = controller_manager::get_cm_node_options()) : controller_manager::ControllerManager( - std::move(resource_manager), executor, manager_node_name, node_namespace) + std::move(resource_manager), executor, manager_node_name, node_namespace, node_options) { } }; diff --git a/doc/debugging.rst b/doc/debugging.rst index acf6c646ff..219cef1d69 100644 --- a/doc/debugging.rst +++ b/doc/debugging.rst @@ -52,6 +52,21 @@ How-To ld.add_action(controller_manager) +Catching Exceptions +************************ + +* The controller manager by default catches most of the exceptions thrown by controllers and hardware components to avoid crashing the whole system. However, it does print the exception type and message to the console. + This can make debugging difficult as the debugger might not catch the exception and also when the exception message is not clear enough to identify the root cause. + This behaviour can be disabled by setting the parameter ``handle_exceptions`` to ``false`` in the controller manager node, this way the exceptions will propagate up to the controller manager and can be caught by the debugger (or) crash by printing the stacktrace during normal execution. + + Example controller manager config file: + + .. code-block:: yaml + + controller_manager: + ros__parameters: + update_rate: 1000 + handle_exceptions: false Additional notes ***************** diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 966acb474e..db3a1077fb 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -103,6 +103,7 @@ controller_manager * New parameters ``overruns.manage`` and ``overruns.print_warnings`` were added to control the behavior of the controller manager/ros2_control_node when overruns occur (`#2546 `_). * The ``bcolors`` class now respects the ``RCUTILS_COLORIZED_OUTPUT`` environment variable to automatically disable colors in non-TTY and CI environments. +* A new parameter ``handle_exceptions`` is added to the controller manager to control whether exceptions thrown by controllers during update are caught and handled internally or propagated. (`#2807 `__) hardware_interface ****************** diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index e24f504fec..2d73c9a7e8 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -665,6 +665,7 @@ class ResourceManager std::unordered_map claimed_command_interface_map_; std::unique_ptr resource_storage_; + hardware_interface::ResourceManagerParams params_; // Structure to store read and write status so it is not initialized in the real-time loop HardwareReadWriteStatus read_write_status; diff --git a/hardware_interface/include/hardware_interface/types/resource_manager_params.hpp b/hardware_interface/include/hardware_interface/types/resource_manager_params.hpp index 5abb695c10..8b96580200 100644 --- a/hardware_interface/include/hardware_interface/types/resource_manager_params.hpp +++ b/hardware_interface/include/hardware_interface/types/resource_manager_params.hpp @@ -96,6 +96,13 @@ struct ResourceManagerParams * or for other timing considerations. */ unsigned int update_rate = 100; + + /** + * @brief If true, the ResourceManager will catch exceptions thrown during the different + * operations of controllers and hardware components. If false, exceptions will propagate up + * and will cause the ResourceManager to crash. + */ + bool handle_exceptions = true; }; } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index ad87268c13..bc37dbbc6c 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -151,6 +151,12 @@ class ResourceStorage } } + explicit ResourceStorage(const hardware_interface::ResourceManagerParams & rm_param) + : ResourceStorage(rm_param.clock, rm_param.logger) + { + handle_exception_ = rm_param.handle_exceptions; + } + template [[nodiscard]] bool load_hardware( const HardwareInfo & hardware_info, pluginlib::ClassLoader & loader, @@ -204,18 +210,21 @@ class ResourceStorage RCLCPP_ERROR( get_logger(), "Caught exception of type : %s while loading hardware: %s", typeid(ex).name(), ex.what()); + handle_exception_ ? void() : throw; } catch (const std::exception & ex) { RCLCPP_ERROR( get_logger(), "Exception of type : %s occurred while loading hardware '%s': %s", typeid(ex).name(), hardware_info.name.c_str(), ex.what()); + handle_exception_ ? void() : throw; } catch (...) { RCLCPP_ERROR( get_logger(), "Unknown exception occurred while loading hardware '%s'", hardware_info.name.c_str()); + handle_exception_ ? void() : throw; } return is_loaded; } @@ -257,12 +266,14 @@ class ResourceStorage RCLCPP_ERROR( get_logger(), "Exception of type : %s occurred while initializing hardware '%s': %s", typeid(ex).name(), component_params.hardware_info.name.c_str(), ex.what()); + handle_exception_ ? void() : throw; } catch (...) { RCLCPP_ERROR( get_logger(), "Unknown exception occurred while initializing hardware '%s'", component_params.hardware_info.name.c_str()); + handle_exception_ ? void() : throw; } return result; @@ -283,12 +294,14 @@ class ResourceStorage RCLCPP_ERROR( get_logger(), "Exception of type : %s occurred while configuring 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 configuring hardware '%s'", hardware.get_name().c_str()); + handle_exception_ ? void() : throw; } if (result) @@ -423,12 +436,14 @@ class ResourceStorage RCLCPP_ERROR( get_logger(), "Exception of type : %s occurred while cleaning up 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 cleaning up hardware '%s'", hardware.get_name().c_str()); + handle_exception_ ? void() : throw; } if (result) @@ -457,12 +472,14 @@ class ResourceStorage RCLCPP_ERROR( get_logger(), "Exception of type : %s occurred while shutting down 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 shutting down hardware '%s'", hardware.get_name().c_str()); + handle_exception_ ? void() : throw; } if (result) @@ -495,12 +512,14 @@ class ResourceStorage RCLCPP_ERROR( get_logger(), "Exception of type : %s occurred while activating 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 activating hardware '%s'", hardware.get_name().c_str()); + handle_exception_ ? void() : throw; } return result; } @@ -520,12 +539,14 @@ class ResourceStorage RCLCPP_ERROR( get_logger(), "Exception of type : %s occurred while deactivating 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 deactivating hardware '%s'", hardware.get_name().c_str()); + handle_exception_ ? void() : throw; } if (result) @@ -709,6 +730,7 @@ class ResourceStorage "Exception of type : %s occurred while importing command interfaces for the hardware '%s' " ": %s", typeid(ex).name(), hardware.get_name().c_str(), ex.what()); + handle_exception_ ? void() : throw; } catch (...) { @@ -716,6 +738,7 @@ class ResourceStorage get_logger(), "Unknown exception occurred while importing command interfaces for the hardware '%s'", hardware.get_name().c_str()); + handle_exception_ ? void() : throw; } } @@ -906,6 +929,7 @@ class ResourceStorage { RCLCPP_WARN( get_logger(), "Exception occurred while importing state interfaces: %s", e.what()); + handle_exception_ ? void() : throw; } } available_state_interfaces_.reserve( @@ -1302,6 +1326,7 @@ class ResourceStorage rclcpp::Logger rm_logger_; rclcpp::Executor::WeakPtr executor_; std::string node_namespace_; + bool handle_exception_ = true; std::vector actuators_; std::vector sensors_; @@ -1392,7 +1417,7 @@ ResourceManager::ResourceManager( ResourceManager::ResourceManager( const hardware_interface::ResourceManagerParams & params, bool load) -: resource_storage_(std::make_unique(params.clock, params.logger)) +: resource_storage_(std::make_unique(params.clock, params.logger)), params_(params) { RCLCPP_WARN_EXPRESSION( params.logger, params.allow_controller_activation_with_inactive_hardware, @@ -1449,6 +1474,8 @@ bool ResourceManager::load_and_initialize_components( resource_storage_->robot_description_ = urdf; resource_storage_->cm_update_rate_ = update_rate; + params_.robot_description = urdf; + params_.update_rate = update_rate; auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); // Set the update rate for all hardware components @@ -2038,7 +2065,8 @@ bool ResourceManager::prepare_command_mode_switch( auto call_prepare_mode_switch = [&start_interfaces, &stop_interfaces, &hardware_info_map, logger = get_logger(), allow_controller_activation_with_inactive_hardware = - allow_controller_activation_with_inactive_hardware_]( + allow_controller_activation_with_inactive_hardware_, + handle_exceptions = params_.handle_exceptions]( auto & components, auto & start_interfaces_buffer, auto & stop_interfaces_buffer) { bool ret = true; @@ -2093,6 +2121,7 @@ bool ResourceManager::prepare_command_mode_switch( typeid(e).name(), component.get_name().c_str(), interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str(), e.what()); + handle_exceptions ? void() : throw; ret = false; } catch (...) @@ -2103,6 +2132,7 @@ bool ResourceManager::prepare_command_mode_switch( "the interfaces: \n %s", component.get_name().c_str(), interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str()); + handle_exceptions ? void() : throw; ret = false; } } @@ -2142,7 +2172,8 @@ bool ResourceManager::perform_command_mode_switch( auto call_perform_mode_switch = [&start_interfaces, &stop_interfaces, &hardware_info_map, logger = get_logger(), allow_controller_activation_with_inactive_hardware = - allow_controller_activation_with_inactive_hardware_]( + allow_controller_activation_with_inactive_hardware_, + handle_exceptions = params_.handle_exceptions]( auto & components, auto & start_interfaces_buffer, auto & stop_interfaces_buffer) { bool ret = true; @@ -2197,6 +2228,7 @@ bool ResourceManager::perform_command_mode_switch( typeid(e).name(), component.get_name().c_str(), interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str(), e.what()); + handle_exceptions ? void() : throw; ret = false; } catch (...) @@ -2208,6 +2240,7 @@ bool ResourceManager::perform_command_mode_switch( "the interfaces: \n %s", component.get_name().c_str(), interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str()); + handle_exceptions ? void() : throw; ret = false; } } @@ -2366,7 +2399,7 @@ HardwareReadWriteStatus ResourceManager::read( { return read_write_status; } - auto read_components = [&](auto & components) + auto read_components = [&](auto & components, bool handle_exceptions) { for (auto & component : components) { @@ -2423,6 +2456,7 @@ HardwareReadWriteStatus ResourceManager::read( RCLCPP_ERROR( get_logger(), "Exception of type : %s thrown during read of the component '%s': %s", typeid(e).name(), component_name.c_str(), e.what()); + handle_exceptions ? void() : throw; ret_val = return_type::ERROR; } catch (...) @@ -2430,6 +2464,7 @@ HardwareReadWriteStatus ResourceManager::read( RCLCPP_ERROR( get_logger(), "Unknown exception thrown during read of the component '%s'", component_name.c_str()); + handle_exceptions ? void() : throw; ret_val = return_type::ERROR; } RCLCPP_WARN_EXPRESSION( @@ -2445,9 +2480,9 @@ HardwareReadWriteStatus ResourceManager::read( } }; - read_components(resource_storage_->actuators_); - read_components(resource_storage_->sensors_); - read_components(resource_storage_->systems_); + read_components(resource_storage_->actuators_, params_.handle_exceptions); + read_components(resource_storage_->sensors_, params_.handle_exceptions); + read_components(resource_storage_->systems_, params_.handle_exceptions); return read_write_status; } @@ -2465,7 +2500,7 @@ HardwareReadWriteStatus ResourceManager::write( { return read_write_status; } - auto write_components = [&](auto & components) + auto write_components = [&](auto & components, bool handle_exceptions) { for (auto & component : components) { @@ -2523,6 +2558,7 @@ HardwareReadWriteStatus ResourceManager::write( RCLCPP_ERROR( get_logger(), "Exception of type : %s thrown during write of the component '%s': %s", typeid(e).name(), component_name.c_str(), e.what()); + handle_exceptions ? void() : throw; ret_val = return_type::ERROR; } catch (...) @@ -2530,6 +2566,7 @@ HardwareReadWriteStatus ResourceManager::write( RCLCPP_ERROR( get_logger(), "Unknown exception thrown during write of the component '%s'", component_name.c_str()); + handle_exceptions ? void() : throw; ret_val = return_type::ERROR; } if (ret_val == return_type::ERROR) @@ -2553,8 +2590,8 @@ HardwareReadWriteStatus ResourceManager::write( } }; - write_components(resource_storage_->actuators_); - write_components(resource_storage_->systems_); + write_components(resource_storage_->actuators_, params_.handle_exceptions); + write_components(resource_storage_->systems_, params_.handle_exceptions); return read_write_status; }