Added a running member to actually join the RTDEWriter thread

Author: Felix Exner

ur_robot_driver/include/ur_robot_driver/rtde/rtde_writer.h

@@ -55,7 +55,16 @@ class RTDEWriter
    * \param recipe The recipe to use for communication
    */
   RTDEWriter(comm::URStream<RTDEPackage>* stream, const std::vector<std::string>& recipe);
-  ~RTDEWriter() = default;
+
+  ~RTDEWriter()
+  {
+    running_ = false;
+    std::this_thread::sleep_for(std::chrono::seconds(5));
+    if (writer_thread_.joinable())
+    {
+      writer_thread_.join();
+    }
+  }
   /*!
    * \brief Starts the writer thread, which periodically clears the queue to write packages to the
    * robot.
@@ -120,6 +129,7 @@ class RTDEWriter
   uint8_t recipe_id_;
   moodycamel::BlockingReaderWriterQueue<std::unique_ptr<DataPackage>> queue_;
   std::thread writer_thread_;
+  bool running_;
 };
 
 }  // namespace rtde_interface

ur_robot_driver/src/rtde/rtde_writer.cpp

@@ -32,13 +32,14 @@ namespace ur_driver
 namespace rtde_interface
 {
 RTDEWriter::RTDEWriter(comm::URStream<RTDEPackage>* stream, const std::vector<std::string>& recipe)
-  : stream_(stream), recipe_(recipe), queue_{ 32 }
+  : stream_(stream), recipe_(recipe), queue_{ 32 }, running_(false)
 {
 }
 
 void RTDEWriter::init(uint8_t recipe_id)
 {
   recipe_id_ = recipe_id;
+  running_ = true;
   writer_thread_ = std::thread(&RTDEWriter::run, this);
 }
 
@@ -48,13 +49,16 @@ void RTDEWriter::run()
   size_t size;
   size_t written;
   std::unique_ptr<DataPackage> package;
-  while (true)
+  while (running_)
   {
-    queue_.waitDequeue(package);
-    package->setRecipeID(recipe_id_);
-    size = package->serializePackage(buffer);
-    stream_->write(buffer, size, written);
+    if (queue_.waitDequeTimed(package, 1000000))
+    {
+      package->setRecipeID(recipe_id_);
+      size = package->serializePackage(buffer);
+      stream_->write(buffer, size, written);
+    }
   }
+  LOG_DEBUG("Write thread ended.");
 }
 
 bool RTDEWriter::sendSpeedSlider(double speed_slider_fraction)