From 5f405b399640b7d323c4ce1855d27e680b662dde Mon Sep 17 00:00:00 2001
From: Felix Mauch <mauch@fzi.de>
Date: Wed, 12 Jun 2019 09:06:34 +0200
Subject: [PATCH] explicitly throw special exception when tool_comm is not
 available

This way, we can catch it and give a better feedback to the user.
---
 .../include/ur_rtde_driver/exceptions.h       | 12 +++++++++++
 ur_rtde_driver/src/ros/hardware_interface.cpp | 20 ++++++++++++++++---
 .../src/ros/hardware_interface_node.cpp       | 16 ++++-----------
 ur_rtde_driver/src/ur/ur_driver.cpp           |  5 +++--
 4 files changed, 36 insertions(+), 17 deletions(-)

diff --git a/ur_rtde_driver/include/ur_rtde_driver/exceptions.h b/ur_rtde_driver/include/ur_rtde_driver/exceptions.h
index 96bc450..88b017d 100644
--- a/ur_rtde_driver/include/ur_rtde_driver/exceptions.h
+++ b/ur_rtde_driver/include/ur_rtde_driver/exceptions.h
@@ -69,5 +69,17 @@ private:
   uint32_t version_actual_;
   std::string text_;
 };
+
+class ToolCommNotAvailable : public VersionMismatch
+{
+public:
+  explicit ToolCommNotAvailable() : ToolCommNotAvailable("", 0, 0)
+  {
+  }
+  explicit ToolCommNotAvailable(const std::string& text, const uint32_t version_req, const uint32_t version_actual)
+    : std::runtime_error(text), VersionMismatch(text, version_req, version_actual)
+  {
+  }
+};
 }  // namespace ur_driver
 #endif  // ifndef UR_RTDE_DRIVER_EXCEPTIONS_H_INCLUDED
diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp
index b44d7de..6eb0390 100644
--- a/ur_rtde_driver/src/ros/hardware_interface.cpp
+++ b/ur_rtde_driver/src/ros/hardware_interface.cpp
@@ -27,6 +27,7 @@
 
 #include "ur_rtde_driver/ros/hardware_interface.h"
 #include "ur_rtde_driver/ur/tool_communication.h"
+#include <ur_rtde_driver/exceptions.h>
 
 #include <Eigen/Geometry>
 
@@ -125,9 +126,22 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
   }
 
   ROS_INFO_STREAM("Initializing urdriver");
-  ur_driver_.reset(new UrDriver(robot_ip, script_filename, recipe_filename,
-                                std::bind(&HardwareInterface::handleRobotProgramState, this, std::placeholders::_1),
-                                std::move(tool_comm_setup)));
+  try
+  {
+    ur_driver_.reset(new UrDriver(robot_ip, script_filename, recipe_filename,
+                                  std::bind(&HardwareInterface::handleRobotProgramState, this, std::placeholders::_1),
+                                  std::move(tool_comm_setup)));
+  }
+  catch (ur_driver::ToolCommNotAvailable& e)
+  {
+    ROS_FATAL_STREAM(e.what() << " See parameter '" << robot_hw_nh.resolveName("use_tool_communication") << "'.");
+    return false;
+  }
+  catch (ur_driver::UrException& e)
+  {
+    ROS_FATAL_STREAM(e.what());
+    return false;
+  }
 
   if (!root_nh.getParam("hardware_interface/joints", joint_names_))
   {
diff --git a/ur_rtde_driver/src/ros/hardware_interface_node.cpp b/ur_rtde_driver/src/ros/hardware_interface_node.cpp
index 027a918..de33da5 100644
--- a/ur_rtde_driver/src/ros/hardware_interface_node.cpp
+++ b/ur_rtde_driver/src/ros/hardware_interface_node.cpp
@@ -29,7 +29,6 @@
 
 #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;
 
@@ -65,17 +64,9 @@ int main(int argc, char** argv)
 
   g_hw_interface.reset(new ur_driver::HardwareInterface);
 
-  try
+  if (!g_hw_interface->init(nh, nh_priv))
   {
-    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());
+    ROS_ERROR_STREAM("Could not correctly initialize robot. Exiting");
     exit(1);
   }
   ROS_INFO_STREAM("initialized hw interface");
@@ -152,7 +143,8 @@ int main(int argc, char** argv)
     period.fromSec(std::chrono::duration_cast<std::chrono::duration<double>>(stopwatch_now - stopwatch_last).count());
     stopwatch_last = stopwatch_now;
 
-    cm.update(timestamp, period, !g_hw_interface->isRobotProgramRunning());
+    // cm.update(timestamp, period, !g_hw_interface->isRobotProgramRunning());
+    cm.update(timestamp, period);
 
     g_hw_interface->write(timestamp, period);
     // if (!control_rate.sleep())
diff --git a/ur_rtde_driver/src/ur/ur_driver.cpp b/ur_rtde_driver/src/ur/ur_driver.cpp
index 6f79155..d7cd03c 100644
--- a/ur_rtde_driver/src/ur/ur_driver.cpp
+++ b/ur_rtde_driver/src/ur/ur_driver.cpp
@@ -89,8 +89,9 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc
   {
     if (urcontrol_version.major < 5)
     {
-      throw VersionMismatch("This robot version does not support using the tool communication interface.", 5,
-                            urcontrol_version.major);
+      throw ToolCommNotAvailable("Tool communication setup requested, but this robot version does not support using "
+                                 "the tool communication interface. Please check your configuration.",
+                                 5, urcontrol_version.major);
     }
     begin_replace << "set_tool_voltage("
                   << static_cast<std::underlying_type<ToolVoltage>::type>(tool_comm_setup->getToolVoltage()) << ")\n";