From 316b1b30abb976a36ff9ac6e84fdcd9fb50bf4d8 Mon Sep 17 00:00:00 2001
From: Felix Exner <feex@universal-robots.com>
Date: Thu, 6 Feb 2025 13:06:06 +0100
Subject: [PATCH] RTDEClient: pause and stop in destructor only if running
 (#257)

Otherwise we produce an error when destructing an initialized, but
non-started client.
---
 src/rtde/rtde_client.cpp   | 21 ++++++++++++++-------
 tests/test_rtde_client.cpp |  2 +-
 2 files changed, 15 insertions(+), 8 deletions(-)

diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp
index 2e05fad4f..602088724 100644
--- a/src/rtde/rtde_client.cpp
+++ b/src/rtde/rtde_client.cpp
@@ -256,7 +256,6 @@ void RTDEClient::queryURControlVersion()
 
 void RTDEClient::resetOutputRecipe(const std::vector<std::string> new_recipe)
 {
-  prod_->teardownProducer();
   disconnect();
 
   output_recipe_.assign(new_recipe.begin(), new_recipe.end());
@@ -332,10 +331,11 @@ void RTDEClient::setupOutputs(const uint16_t protocol_version)
       if (!unavailable_variables.empty())
       {
         std::stringstream error_message;
-        error_message << "The following variables are not recognized by the robot: ";
-        std::for_each(unavailable_variables.begin(), unavailable_variables.end(),
-                      [&error_message](const std::string& variable_name) { error_message << variable_name << " "; });
-        error_message << ". Either your output recipe contains errors "
+        error_message << "The following variables are not recognized by the robot:";
+        std::for_each(
+            unavailable_variables.begin(), unavailable_variables.end(),
+            [&error_message](const std::string& variable_name) { error_message << "\n  - '" << variable_name << "'"; });
+        error_message << "\nEither your output recipe contains errors "
                          "or the urcontrol version does not support "
                          "them.";
 
@@ -346,6 +346,7 @@ void RTDEClient::setupOutputs(const uint16_t protocol_version)
 
           // Some variables are not available so retry setting up the communication with a stripped-down output recipe
           resetOutputRecipe(available_variables);
+          return;
         }
         else
         {
@@ -445,10 +446,16 @@ void RTDEClient::setupInputs()
 void RTDEClient::disconnect()
 {
   // If communication is started it should be paused before disconnecting
-  if (client_state_ > ClientState::UNINITIALIZED)
+  if (client_state_ == ClientState::RUNNING)
+  {
+    pause();
+  }
+  if (client_state_ >= ClientState::INITIALIZING)
   {
-    sendPause();
     pipeline_->stop();
+  }
+  if (client_state_ > ClientState::UNINITIALIZED)
+  {
     stream_.disconnect();
   }
   client_state_ = ClientState::UNINITIALIZED;
diff --git a/tests/test_rtde_client.cpp b/tests/test_rtde_client.cpp
index ead20bbe3..2831d6b47 100644
--- a/tests/test_rtde_client.cpp
+++ b/tests/test_rtde_client.cpp
@@ -380,7 +380,7 @@ TEST_F(RTDEClientTest, check_all_rtde_output_variables_exist)
   client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, exhaustive_output_recipe_file_,
                                                input_recipe_file_, 0.0, true));
 
-  EXPECT_NO_THROW(client_->init());
+  EXPECT_TRUE(client_->init());
   client_->start();
 
   // Test that we can receive and parse the timestamp from the received package to prove the setup was successful