Fix RobotWrapper shutdown regarting consumer thread

Author: urfeex

include/ur_client_library/example_robot_wrapper.h

@@ -17,6 +17,7 @@ class ExampleRobotWrapper
   ExampleRobotWrapper(const std::string& robot_ip, const std::string& output_recipe_file,
                       const std::string& input_recipe_file, const bool headless_mode = true,
                       const std::string& autostart_program = "", const std::string& script_file = SCRIPT_FILE);
+  ~ExampleRobotWrapper();
 
   void initializeRobotWithDashboard();
 

src/example_robot_wrapper.cpp

@@ -28,6 +28,14 @@ ExampleRobotWrapper::ExampleRobotWrapper(const std::string& robot_ip, const std:
     startRobotProgram(autostart_program);
   }
 }
+
+ExampleRobotWrapper::~ExampleRobotWrapper()
+{
+  if (rtde_communication_started_)
+  {
+    stopConsumingRTDEData();
+  }
+}
 void ExampleRobotWrapper::initializeRobotWithDashboard()
 {
   // // Stop program, if there is one running
@@ -97,8 +105,14 @@ void ExampleRobotWrapper::startConsumingRTDEData()
 
 void ExampleRobotWrapper::stopConsumingRTDEData()
 {
-  consume_rtde_packages_ = false;
-  rtde_consumer_thread_.join();
+  if (consume_rtde_packages_)
+  {
+    consume_rtde_packages_ = false;
+    if (rtde_consumer_thread_.joinable())
+    {
+      rtde_consumer_thread_.join();
+    }
+  }
 }
 
 bool ExampleRobotWrapper::readDataPackage(std::unique_ptr<rtde_interface::DataPackage>& data_pkg)