Make sure we wait for the trajectory interface to be stopped

Author: urfeex

include/ur_client_library/control/reverse_interface.h

@@ -146,6 +146,16 @@ class ReverseInterface
     disconnection_callback_ = disconnection_fun;
   }
 
+  /*!
+   * \brief Checks if the reverse interface is connected to the robot.
+   *
+   * \returns True, if the interface is connected, false otherwise.
+   */
+  bool isConnected() const
+  {
+    return client_fd_ != INVALID_SOCKET;
+  }
+
 protected:
   virtual void connectionCallback(const socket_t filedescriptor);
 

include/ur_client_library/ur/ur_driver.h

@@ -880,6 +880,16 @@ class UrDriver
     secondary_stream_->close();
   }
 
+  bool isReverseInterfaceConnected() const
+  {
+    return reverse_interface_->isConnected();
+  }
+
+  bool isTrajectoryInterfaceConnected() const
+  {
+    return trajectory_interface_->isConnected();
+  }
+
 private:
   void init(const UrDriverConfiguration& config);
 

src/control/trajectory_point_interface.cpp

@@ -30,6 +30,7 @@
 #include <ur_client_library/exceptions.h>
 #include <math.h>
 #include <stdexcept>
+#include "ur_client_library/comm/socket_t.h"
 
 namespace urcl
 {
@@ -217,11 +218,11 @@ void TrajectoryPointInterface::connectionCallback(const socket_t filedescriptor)
 void TrajectoryPointInterface::disconnectionCallback(const socket_t filedescriptor)
 {
   URCL_LOG_DEBUG("Connection to trajectory interface dropped.");
-  client_fd_ = -1;
   if (disconnection_callback_ != nullptr)
   {
     disconnection_callback_(filedescriptor);
   }
+  client_fd_ = INVALID_SOCKET;
 }
 
 void TrajectoryPointInterface::messageCallback(const socket_t filedescriptor, char* buffer, int nbytesrecv)

tests/test_instruction_executor.cpp

@@ -72,10 +72,15 @@ class InstructionExecutorTest : public ::testing::Test
       ASSERT_TRUE(g_my_robot->waitForProgramRunning());
     }
   }
-  void TearDown()
+  void TearDown() override
   {
     g_my_robot->ur_driver_->stopControl();
     g_my_robot->waitForProgramNotRunning(1000);
+    while (g_my_robot->ur_driver_->isTrajectoryInterfaceConnected())
+    {
+      std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+    URCL_LOG_INFO("Stopped robot control.");
   }
 
   static void TearDownTestSuite()