From c88022eefdae900f5ec6ca25738492c86a2fd531 Mon Sep 17 00:00:00 2001
From: Felix Mauch <mauch@fzi.de>
Date: Tue, 11 Jun 2019 10:25:50 +0200
Subject: [PATCH] Introduce UrException as exception base class

---
 .../include/ur_rtde_driver/exceptions.h       | 44 +++++++++++++++++++
 .../src/ros/hardware_interface_node.cpp       | 13 +++++-
 ur_rtde_driver/src/ur/ur_driver.cpp           |  3 +-
 3 files changed, 57 insertions(+), 3 deletions(-)
 create mode 100644 ur_rtde_driver/include/ur_rtde_driver/exceptions.h

diff --git a/ur_rtde_driver/include/ur_rtde_driver/exceptions.h b/ur_rtde_driver/include/ur_rtde_driver/exceptions.h
new file mode 100644
index 0000000..c73e5ff
--- /dev/null
+++ b/ur_rtde_driver/include/ur_rtde_driver/exceptions.h
@@ -0,0 +1,44 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Mauch mauch@fzi.de
+ * \date    2019-06-11
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_RTDE_DRIVER_EXCEPTIONS_H_INCLUDED
+#define UR_RTDE_DRIVER_EXCEPTIONS_H_INCLUDED
+
+#include <stdexcept>
+
+namespace ur_driver
+{
+/*!
+ * \brief Our base class for exceptions. Specialized exceptions should inherit from those.
+ */
+class UrException : virtual public std::runtime_error
+{
+public:
+  explicit UrException() : std::runtime_error("")
+  {
+  }
+  explicit UrException(const std::string& what_arg) : std::runtime_error(what_arg)
+  {
+  }
+  explicit UrException(const char* what_arg) : std::runtime_error(what_arg)
+  {
+  }
+
+  virtual ~UrException() = default;
+
+private:
+  /* data */
+};
+}  // namespace ur_driver
+#endif  // ifndef UR_RTDE_DRIVER_EXCEPTIONS_H_INCLUDED
diff --git a/ur_rtde_driver/src/ros/hardware_interface_node.cpp b/ur_rtde_driver/src/ros/hardware_interface_node.cpp
index c9a6821..af6d0a1 100644
--- a/ur_rtde_driver/src/ros/hardware_interface_node.cpp
+++ b/ur_rtde_driver/src/ros/hardware_interface_node.cpp
@@ -29,6 +29,7 @@
 
 #include <csignal>
 #include <ur_rtde_driver/ros/hardware_interface.h>
+#include <ur_rtde_driver/exceptions.h>
 
 std::unique_ptr<ur_driver::HardwareInterface> g_hw_interface;
 
@@ -64,9 +65,17 @@ int main(int argc, char** argv)
 
   g_hw_interface.reset(new ur_driver::HardwareInterface);
 
-  if (!g_hw_interface->init(nh, nh_priv))
+  try
   {
-    ROS_ERROR_STREAM("Could not correctly initialize robot. Exiting");
+    if (!g_hw_interface->init(nh, nh_priv))
+    {
+      ROS_ERROR_STREAM("Could not correctly initialize robot. Exiting");
+      exit(1);
+    }
+  }
+  catch (ur_driver::UrException& e)
+  {
+    ROS_FATAL_STREAM("Could not correctly initialize robot: " << e.what());
     exit(1);
   }
   ROS_INFO_STREAM("initialized hw interface");
diff --git a/ur_rtde_driver/src/ur/ur_driver.cpp b/ur_rtde_driver/src/ur/ur_driver.cpp
index bf75a61..1f8779d 100644
--- a/ur_rtde_driver/src/ur/ur_driver.cpp
+++ b/ur_rtde_driver/src/ur/ur_driver.cpp
@@ -32,6 +32,7 @@
 
 #include "ur_rtde_driver/ur/ur_driver.h"
 #include "ur_rtde_driver/primary/package_header.h"
+#include "ur_rtde_driver/exceptions.h"
 #include <memory>
 
 namespace ur_driver
@@ -51,7 +52,7 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc
 
   if (!rtde_client_->init())
   {
-    throw std::runtime_error("initialization went wrong");  // TODO: be less harsh
+    throw UrException("Initialization of RTDE client went wrong.");
   }
 
   rtde_frequency_ = rtde_client_->getMaxFrequency();