From 650787445e3cfb189cfa3b1313d2e3ff0d8d2ba0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 25 Apr 2024 23:17:26 +0200 Subject: [PATCH 1/7] catch exceptions during node cleanup --- controller_manager/src/controller_manager.cpp | 23 +++++++++++++++---- 1 file changed, 19 insertions(+), 4 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index e3729b90f8..1a0d8db460 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -562,11 +562,26 @@ controller_interface::return_type ControllerManager::unload_controller( get_logger(), "Controller '%s' is cleaned-up before unloading!", controller_name.c_str()); // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for // cleaning-up controllers? - const auto new_state = controller.c->get_node()->cleanup(); - if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + try { - RCLCPP_WARN( - get_logger(), "Failed to clean-up the controller '%s' before unloading!", + const auto new_state = controller.c->get_node()->cleanup(); + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + RCLCPP_WARN( + get_logger(), "Failed to clean-up the controller '%s' before unloading!", + controller_name.c_str()); + } + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_logger(), "Failed to clean-up the controller '%s' before unloading: %s", + controller_name.c_str(), e.what()); + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Failed to clean-up the controller '%s' before unloading", controller_name.c_str()); } } From 63eb75e1d58b7dc79d2432cbb68503b4444c897b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 25 Apr 2024 23:18:06 +0200 Subject: [PATCH 2/7] catch exceptions during node initialization --- controller_manager/src/controller_manager.cpp | 31 ++++++++++++++++--- 1 file changed, 26 insertions(+), 5 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 1a0d8db460..ab44809b98 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1301,14 +1301,35 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co } const rclcpp::NodeOptions controller_node_options = determine_controller_node_options(controller); - if ( - controller.c->init( - controller.info.name, robot_description_, get_update_rate(), get_namespace(), - controller_node_options) == controller_interface::return_type::ERROR) + // Catch whatever exception the controller might throw + try + { + if ( + controller.c->init( + controller.info.name, robot_description_, get_update_rate(), get_namespace(), + controller_node_options) == controller_interface::return_type::ERROR) + { + to.clear(); + RCLCPP_ERROR( + get_logger(), "Could not initialize the controller named '%s'", + controller.info.name.c_str()); + return nullptr; + } + } + catch (const std::exception & e) { to.clear(); RCLCPP_ERROR( - get_logger(), "Could not initialize the controller named '%s'", controller.info.name.c_str()); + get_logger(), "Caught exception while initializing controller '%s': %s", + controller.info.name.c_str(), e.what()); + return nullptr; + } + catch (...) + { + to.clear(); + RCLCPP_ERROR( + get_logger(), "Caught unknown exception while initializing controller '%s'", + controller.info.name.c_str()); return nullptr; } From 083b829d12de5980d09b4fd4612fe57e614cae79 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 25 Apr 2024 23:18:20 +0200 Subject: [PATCH 3/7] Catch exceptions during the node update cycle --- controller_manager/src/controller_manager.cpp | 21 ++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index ab44809b98..314e3b9727 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2086,7 +2086,26 @@ controller_interface::return_type ControllerManager::update( { const auto controller_actual_period = (time - *loaded_controller.next_update_cycle_time) + controller_period; - auto controller_ret = loaded_controller.c->update(time, controller_actual_period); + auto controller_ret = controller_interface::return_type::OK; + // Catch exceptions thrown by the controller update function + try + { + auto controller_ret = loaded_controller.c->update(time, controller_actual_period); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_logger(), "Caught exception while updating controller '%s': %s", + loaded_controller.info.name.c_str(), e.what()); + controller_ret = controller_interface::return_type::ERROR; + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Caught unknown exception while updating controller '%s'", + loaded_controller.info.name.c_str()); + controller_ret = controller_interface::return_type::ERROR; + } if ( *loaded_controller.next_update_cycle_time == From fd7e845eed4b55e4bf390f60aff3947a485b7b8f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 25 Apr 2024 23:28:49 +0200 Subject: [PATCH 4/7] catch exceptions while configuring the controllers --- controller_manager/src/controller_manager.cpp | 41 +++++++++++++++---- 1 file changed, 34 insertions(+), 7 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 314e3b9727..2f3c479631 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -646,22 +646,49 @@ controller_interface::return_type ControllerManager::configure_controller( get_logger(), "Controller '%s' is cleaned-up before configuring", controller_name.c_str()); // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for // cleaning-up controllers? - new_state = controller->get_node()->cleanup(); - if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + try + { + new_state = controller->get_node()->cleanup(); + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + RCLCPP_ERROR( + get_logger(), "Controller '%s' can not be cleaned-up before configuring", + controller_name.c_str()); + return controller_interface::return_type::ERROR; + } + } + catch (...) { RCLCPP_ERROR( - get_logger(), "Controller '%s' can not be cleaned-up before configuring", + get_logger(), "Caught exception while cleaning-up controller '%s' before configuring", controller_name.c_str()); return controller_interface::return_type::ERROR; } } - new_state = controller->configure(); - if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + try + { + new_state = controller->configure(); + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + RCLCPP_ERROR( + get_logger(), "After configuring, controller '%s' is in state '%s' , expected inactive.", + controller_name.c_str(), new_state.label().c_str()); + return controller_interface::return_type::ERROR; + } + } + catch (const std::exception & e) { RCLCPP_ERROR( - get_logger(), "After configuring, controller '%s' is in state '%s' , expected inactive.", - controller_name.c_str(), new_state.label().c_str()); + get_logger(), "Caught exception while configuring controller '%s': %s", + controller_name.c_str(), e.what()); + return controller_interface::return_type::ERROR; + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Caught unknown exception while configuring controller '%s'", + controller_name.c_str()); return controller_interface::return_type::ERROR; } From 95f568992ebcdab555b3381ece67139a7e1aa9d9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 25 Apr 2024 23:29:10 +0200 Subject: [PATCH 5/7] catch exceptions while deactivating the controller --- controller_manager/src/controller_manager.cpp | 21 +++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 2f3c479631..2be46ef04d 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1399,8 +1399,25 @@ void ControllerManager::deactivate_controllers( auto controller = found_it->c; if (is_controller_active(*controller)) { - const auto new_state = controller->get_node()->deactivate(); - controller->release_interfaces(); + try + { + const auto new_state = controller->get_node()->deactivate(); + controller->release_interfaces(); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_logger(), "Caught exception while deactivating the controller '%s': %s", + controller_name.c_str(), e.what()); + continue; + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Caught unknown exception while deactivating the controller '%s'", + controller_name.c_str()); + continue; + } // if it is a chainable controller, make the reference interfaces unavailable on deactivation if (controller->is_chainable()) { From 5b539dd34275897ce20fd7b5a904840fd4f64c82 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 25 Apr 2024 23:29:21 +0200 Subject: [PATCH 6/7] catch exceptions while activating the controller --- controller_manager/src/controller_manager.cpp | 31 ++++++++++++++----- 1 file changed, 24 insertions(+), 7 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 2be46ef04d..3830adf165 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1583,15 +1583,32 @@ void ControllerManager::activate_controllers( } controller->assign_interfaces(std::move(command_loans), std::move(state_loans)); - const auto new_state = controller->get_node()->activate(); - if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + try + { + const auto new_state = controller->get_node()->activate(); + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + RCLCPP_ERROR( + get_logger(), + "After activation, controller '%s' is in state '%s' (%d), expected '%s' (%d).", + controller->get_node()->get_name(), new_state.label().c_str(), new_state.id(), + hardware_interface::lifecycle_state_names::ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } + } + catch (const std::exception & e) { RCLCPP_ERROR( - get_logger(), - "After activation, controller '%s' is in state '%s' (%d), expected '%s' (%d).", - controller->get_node()->get_name(), new_state.label().c_str(), new_state.id(), - hardware_interface::lifecycle_state_names::ACTIVE, - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + get_logger(), "Caught exception while activating the controller '%s': %s", + controller_name.c_str(), e.what()); + continue; + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Caught unknown exception while activating the controller '%s'", + controller_name.c_str()); + continue; } // if it is a chainable controller, make the reference interfaces available on activation From 5546fd67bc08f9abcc1d136183079329de2bbda3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 25 Apr 2024 23:34:09 +0200 Subject: [PATCH 7/7] Minor compilation fixes --- controller_manager/src/controller_manager.cpp | 26 ++++++++++--------- 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 3830adf165..43fdf5d015 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1403,6 +1403,19 @@ void ControllerManager::deactivate_controllers( { const auto new_state = controller->get_node()->deactivate(); controller->release_interfaces(); + + // if it is a chainable controller, make the reference interfaces unavailable on + // deactivation + if (controller->is_chainable()) + { + resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); + } + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + RCLCPP_ERROR( + get_logger(), "After deactivating, controller '%s' is in state '%s', expected Inactive", + controller_name.c_str(), new_state.label().c_str()); + } } catch (const std::exception & e) { @@ -1418,17 +1431,6 @@ void ControllerManager::deactivate_controllers( controller_name.c_str()); continue; } - // if it is a chainable controller, make the reference interfaces unavailable on deactivation - if (controller->is_chainable()) - { - resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); - } - if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - RCLCPP_ERROR( - get_logger(), "After deactivating, controller '%s' is in state '%s', expected Inactive", - controller_name.c_str(), new_state.label().c_str()); - } } } } @@ -2151,7 +2153,7 @@ controller_interface::return_type ControllerManager::update( // Catch exceptions thrown by the controller update function try { - auto controller_ret = loaded_controller.c->update(time, controller_actual_period); + controller_ret = loaded_controller.c->update(time, controller_actual_period); } catch (const std::exception & e) {