Add handle_exceptions parameter to controller manager (#2807)

Author: saikishor

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,13 +1291,15 @@ 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 (...)
   {
     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);
@@ -2134,6 +2141,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 (...)
@@ -2142,6 +2150,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;
   }
 
@@ -2211,13 +2220,15 @@ 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 (...)
       {
         RCLCPP_ERROR(
           get_logger(), "Caught unknown exception while deactivating the controller '%s'",
           controller_name.c_str());
+        params_->handle_exceptions ? void() : throw;
         continue;
       }
     }
@@ -2334,6 +2345,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;
@@ -2375,6 +2387,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;
       }
     }
@@ -2398,12 +2411,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)
     {
@@ -3128,13 +3143,15 @@ 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 (...)
         {
           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;
         }
 

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,

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<rclcpp::Parameter> cm_parameters = {})
   : robot_description_(robot_description)
   {
     executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+    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<CtrlMgr>(
       std::make_unique<hardware_interface::ResourceManager>(
         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())
     {

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*/)
 {

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