diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index e3729b90f8..43fdf5d015 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()); } } @@ -631,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; } @@ -1286,14 +1328,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; } @@ -1336,18 +1399,37 @@ 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(); - // if it is a chainable controller, make the reference interfaces unavailable on deactivation - if (controller->is_chainable()) + try + { + 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) { - resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); + RCLCPP_ERROR( + get_logger(), "Caught exception while deactivating the controller '%s': %s", + controller_name.c_str(), e.what()); + continue; } - if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + catch (...) { RCLCPP_ERROR( - get_logger(), "After deactivating, controller '%s' is in state '%s', expected Inactive", - controller_name.c_str(), new_state.label().c_str()); + get_logger(), "Caught unknown exception while deactivating the controller '%s'", + controller_name.c_str()); + continue; } } } @@ -1503,15 +1585,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 @@ -2050,7 +2149,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 + { + 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 ==