From 2ee0001d83460168ab0ed43b1d56b6d8f3f2fbc9 Mon Sep 17 00:00:00 2001
From: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
Date: Wed, 11 Sep 2024 13:12:07 +0200
Subject: [PATCH] Log exception type when catching the exception (#1749)

---
 controller_manager/src/controller_manager.cpp | 40 +++++++-----
 hardware_interface/src/resource_manager.cpp   | 62 ++++++++++---------
 2 files changed, 57 insertions(+), 45 deletions(-)

diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
index e769ea06ce..b4f18dd90e 100644
--- a/controller_manager/src/controller_manager.cpp
+++ b/controller_manager/src/controller_manager.cpp
@@ -470,8 +470,9 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c
   catch (const std::exception & e)
   {
     RCLCPP_ERROR(
-      get_logger(), "Caught exception while loading the controller '%s' of plugin type '%s':\n%s",
-      controller_name.c_str(), controller_type.c_str(), e.what());
+      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());
     return nullptr;
   }
   catch (...)
@@ -615,8 +616,9 @@ controller_interface::return_type ControllerManager::unload_controller(
     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());
+        get_logger(),
+        "Caught exception of type : %s while cleaning up the controller '%s' before unloading: %s",
+        typeid(e).name(), controller_name.c_str(), e.what());
     }
     catch (...)
     {
@@ -720,8 +722,8 @@ controller_interface::return_type ControllerManager::configure_controller(
   catch (const std::exception & e)
   {
     RCLCPP_ERROR(
-      get_logger(), "Caught exception while configuring controller '%s': %s",
-      controller_name.c_str(), e.what());
+      get_logger(), "Caught exception of type : %s while configuring controller '%s': %s",
+      typeid(e).name(), controller_name.c_str(), e.what());
     return controller_interface::return_type::ERROR;
   }
   catch (...)
@@ -1432,8 +1434,8 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co
   {
     to.clear();
     RCLCPP_ERROR(
-      get_logger(), "Caught exception while initializing controller '%s': %s",
-      controller.info.name.c_str(), e.what());
+      get_logger(), "Caught exception of type : %s while initializing controller '%s': %s",
+      typeid(e).name(), controller.info.name.c_str(), e.what());
     return nullptr;
   }
   catch (...)
@@ -1508,8 +1510,8 @@ void ControllerManager::deactivate_controllers(
       catch (const std::exception & e)
       {
         RCLCPP_ERROR(
-          get_logger(), "Caught exception while deactivating the  controller '%s': %s",
-          controller_name.c_str(), e.what());
+          get_logger(), "Caught exception of type : %s while deactivating the  controller '%s': %s",
+          typeid(e).name(), controller_name.c_str(), e.what());
         continue;
       }
       catch (...)
@@ -1626,7 +1628,10 @@ void ControllerManager::activate_controllers(
       catch (const std::exception & e)
       {
         RCLCPP_ERROR(
-          get_logger(), "Can't activate controller '%s': %s", controller_name.c_str(), e.what());
+          get_logger(),
+          "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());
         assignment_successful = false;
         break;
       }
@@ -1661,7 +1666,10 @@ void ControllerManager::activate_controllers(
       catch (const std::exception & e)
       {
         RCLCPP_ERROR(
-          get_logger(), "Can't activate controller '%s': %s", controller_name.c_str(), e.what());
+          get_logger(),
+          "Caught exception of type : %s while claiming the state interfaces. Can't activate "
+          "controller '%s': %s",
+          typeid(e).name(), controller_name.c_str(), e.what());
         assignment_successful = false;
         break;
       }
@@ -1689,8 +1697,8 @@ void ControllerManager::activate_controllers(
     catch (const std::exception & e)
     {
       RCLCPP_ERROR(
-        get_logger(), "Caught exception while activating the controller '%s': %s",
-        controller_name.c_str(), e.what());
+        get_logger(), "Caught exception of type : %s while activating the controller '%s': %s",
+        typeid(e).name(), controller_name.c_str(), e.what());
       continue;
     }
     catch (...)
@@ -2295,8 +2303,8 @@ controller_interface::return_type ControllerManager::update(
         catch (const std::exception & e)
         {
           RCLCPP_ERROR(
-            get_logger(), "Caught exception while updating controller '%s': %s",
-            loaded_controller.info.name.c_str(), e.what());
+            get_logger(), "Caught exception of type : %s while updating controller '%s': %s",
+            typeid(e).name(), loaded_controller.info.name.c_str(), e.what());
           controller_ret = controller_interface::return_type::ERROR;
         }
         catch (...)
diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp
index 72e966fcab..f368f2a00a 100644
--- a/hardware_interface/src/resource_manager.cpp
+++ b/hardware_interface/src/resource_manager.cpp
@@ -160,13 +160,15 @@ class ResourceStorage
     }
     catch (const pluginlib::PluginlibException & ex)
     {
-      RCLCPP_ERROR(get_logger(), "Exception while loading hardware: %s", ex.what());
+      RCLCPP_ERROR(
+        get_logger(), "Caught exception of type : %s while loading hardware: %s", typeid(ex).name(),
+        ex.what());
     }
     catch (const std::exception & ex)
     {
       RCLCPP_ERROR(
-        get_logger(), "Exception occurred while loading hardware '%s': %s",
-        hardware_info.name.c_str(), ex.what());
+        get_logger(), "Exception of type : %s occurred while loading hardware '%s': %s",
+        typeid(ex).name(), hardware_info.name.c_str(), ex.what());
     }
     catch (...)
     {
@@ -203,8 +205,8 @@ class ResourceStorage
     catch (const std::exception & ex)
     {
       RCLCPP_ERROR(
-        get_logger(), "Exception occurred while initializing hardware '%s': %s",
-        hardware_info.name.c_str(), ex.what());
+        get_logger(), "Exception of type : %s occurred while initializing hardware '%s': %s",
+        typeid(ex).name(), hardware_info.name.c_str(), ex.what());
     }
     catch (...)
     {
@@ -229,8 +231,8 @@ class ResourceStorage
     catch (const std::exception & ex)
     {
       RCLCPP_ERROR(
-        get_logger(), "Exception occurred while configuring hardware '%s': %s",
-        hardware.get_name().c_str(), ex.what());
+        get_logger(), "Exception of type : %s occurred while configuring hardware '%s': %s",
+        typeid(ex).name(), hardware.get_name().c_str(), ex.what());
     }
     catch (...)
     {
@@ -378,8 +380,8 @@ class ResourceStorage
     catch (const std::exception & ex)
     {
       RCLCPP_ERROR(
-        get_logger(), "Exception occurred while cleaning up hardware '%s': %s",
-        hardware.get_name().c_str(), ex.what());
+        get_logger(), "Exception of type : %s occurred while cleaning up hardware '%s': %s",
+        typeid(ex).name(), hardware.get_name().c_str(), ex.what());
     }
     catch (...)
     {
@@ -412,8 +414,8 @@ class ResourceStorage
     catch (const std::exception & ex)
     {
       RCLCPP_ERROR(
-        get_logger(), "Exception occurred while shutting down hardware '%s': %s",
-        hardware.get_name().c_str(), ex.what());
+        get_logger(), "Exception of type : %s occurred while shutting down hardware '%s': %s",
+        typeid(ex).name(), hardware.get_name().c_str(), ex.what());
     }
     catch (...)
     {
@@ -451,8 +453,8 @@ class ResourceStorage
     catch (const std::exception & ex)
     {
       RCLCPP_ERROR(
-        get_logger(), "Exception occurred while activating hardware '%s': %s",
-        hardware.get_name().c_str(), ex.what());
+        get_logger(), "Exception of type : %s occurred while activating hardware '%s': %s",
+        typeid(ex).name(), hardware.get_name().c_str(), ex.what());
     }
     catch (...)
     {
@@ -486,8 +488,8 @@ class ResourceStorage
     catch (const std::exception & ex)
     {
       RCLCPP_ERROR(
-        get_logger(), "Exception occurred while deactivating hardware '%s': %s",
-        hardware.get_name().c_str(), ex.what());
+        get_logger(), "Exception of type : %s occurred while deactivating hardware '%s': %s",
+        typeid(ex).name(), hardware.get_name().c_str(), ex.what());
     }
     catch (...)
     {
@@ -625,8 +627,9 @@ class ResourceStorage
     {
       RCLCPP_ERROR(
         get_logger(),
-        "Exception occurred while importing state interfaces for the hardware '%s' : %s",
-        hardware.get_name().c_str(), e.what());
+        "Exception of type : %s occurred while importing state interfaces for the hardware '%s' : "
+        "%s",
+        typeid(e).name(), hardware.get_name().c_str(), e.what());
     }
     catch (...)
     {
@@ -650,8 +653,9 @@ class ResourceStorage
     {
       RCLCPP_ERROR(
         get_logger(),
-        "Exception occurred while importing command interfaces for the hardware '%s' : %s",
-        hardware.get_name().c_str(), ex.what());
+        "Exception of type : %s occurred while importing command interfaces for the hardware '%s' "
+        ": %s",
+        typeid(ex).name(), hardware.get_name().c_str(), ex.what());
     }
     catch (...)
     {
@@ -1568,9 +1572,9 @@ bool ResourceManager::prepare_command_mode_switch(
         {
           RCLCPP_ERROR(
             logger,
-            "Exception occurred while preparing command mode switch for component '%s' for the "
-            "interfaces: \n %s : %s",
-            component.get_name().c_str(),
+            "Exception of type : %s occurred while preparing command mode switch for component "
+            "'%s' for the interfaces: \n %s : %s",
+            typeid(e).name(), component.get_name().c_str(),
             interfaces_to_string(start_interfaces, stop_interfaces).c_str(), e.what());
           ret = false;
         }
@@ -1632,9 +1636,9 @@ bool ResourceManager::perform_command_mode_switch(
         {
           RCLCPP_ERROR(
             logger,
-            "Exception occurred while performing command mode switch for component '%s' for the "
-            "interfaces: \n %s : %s",
-            component.get_name().c_str(),
+            "Exception of type : %s occurred while performing command mode switch for component "
+            "'%s' for the interfaces: \n %s : %s",
+            typeid(e).name(), component.get_name().c_str(),
             interfaces_to_string(start_interfaces, stop_interfaces).c_str(), e.what());
           ret = false;
         }
@@ -1791,8 +1795,8 @@ HardwareReadWriteStatus ResourceManager::read(
       catch (const std::exception & e)
       {
         RCLCPP_ERROR(
-          get_logger(), "Exception thrown durind read of the component '%s': %s",
-          component.get_name().c_str(), e.what());
+          get_logger(), "Exception of type : %s thrown during read of the component '%s': %s",
+          typeid(e).name(), component.get_name().c_str(), e.what());
         ret_val = return_type::ERROR;
       }
       catch (...)
@@ -1851,8 +1855,8 @@ HardwareReadWriteStatus ResourceManager::write(
       catch (const std::exception & e)
       {
         RCLCPP_ERROR(
-          get_logger(), "Exception thrown during write of the component '%s': %s",
-          component.get_name().c_str(), e.what());
+          get_logger(), "Exception of type : %s thrown during write of the component '%s': %s",
+          typeid(e).name(), component.get_name().c_str(), e.what());
         ret_val = return_type::ERROR;
       }
       catch (...)