diff --git a/include/ur_client_library/comm/tcp_server.h b/include/ur_client_library/comm/tcp_server.h
index 7a74bd0fd..285eb662f 100644
--- a/include/ur_client_library/comm/tcp_server.h
+++ b/include/ur_client_library/comm/tcp_server.h
@@ -154,6 +154,21 @@ class TCPServer
     max_clients_allowed_ = max_clients_allowed;
   }
 
+  /*!
+   * \brief Get the port this server is bound to
+   *
+   * If port number 0 is passed during initialization, the server will bind to a random free port.
+   * In this case, this function can be used to get the actual port number the server is bound to.
+   * This should only be called after the server has been initialized, otherwise the returned port
+   * number might not be correct.
+   *
+   * \returns The port number this server is bound to
+   */
+  int getPort() const
+  {
+    return port_;
+  }
+
 private:
   void init();
   void bind(const size_t max_num_tries, const std::chrono::milliseconds reconnection_time);
@@ -197,4 +212,4 @@ class TCPServer
 }  // namespace comm
 }  // namespace urcl
 
-#endif  // ifndef UR_CLIENT_LIBRARY_TCP_SERVER_H_INCLUDED
\ No newline at end of file
+#endif  // ifndef UR_CLIENT_LIBRARY_TCP_SERVER_H_INCLUDED
diff --git a/include/ur_client_library/control/reverse_interface.h b/include/ur_client_library/control/reverse_interface.h
index 822020791..37e1dee9a 100644
--- a/include/ur_client_library/control/reverse_interface.h
+++ b/include/ur_client_library/control/reverse_interface.h
@@ -194,6 +194,19 @@ class ReverseInterface
     return client_fd_ != INVALID_SOCKET;
   }
 
+  /*!
+   * \brief Get the port number the server is bound to.
+   *
+   * If port number 0 is passed during initialization, the server will bind to a random free port.
+   * In this case, this function can be used to get the actual port number the server is bound to.
+   *
+   * \returns The port number the server is bound to.
+   */
+  int getPort() const
+  {
+    return server_.getPort();
+  }
+
 protected:
   virtual void connectionCallback(const socket_t filedescriptor);
 
diff --git a/src/comm/tcp_server.cpp b/src/comm/tcp_server.cpp
index 8b5797746..22505b6da 100644
--- a/src/comm/tcp_server.cpp
+++ b/src/comm/tcp_server.cpp
@@ -170,6 +170,17 @@ void TCPServer::startListen()
     ss << "Failed to start listen on port " << port_;
     throw std::system_error(std::error_code(errno, std::generic_category()), ss.str());
   }
+  struct sockaddr_in sin;
+  socklen_t len = sizeof(sin);
+  if (getsockname(listen_fd_, (struct sockaddr*)&sin, &len) == -1)
+  {
+    URCL_LOG_ERROR("getsockname() failed to get port number for listening socket: %s", strerror(errno));
+  }
+
+  else
+  {
+    port_ = ntohs(sin.sin_port);
+  }
   URCL_LOG_DEBUG("Listening on port %d", port_);
 }
 
diff --git a/tests/test_reverse_interface.cpp b/tests/test_reverse_interface.cpp
index a365b6dd8..bf15fc787 100644
--- a/tests/test_reverse_interface.cpp
+++ b/tests/test_reverse_interface.cpp
@@ -65,7 +65,7 @@ class TestableReverseInterface : public control::ReverseInterface
   std::atomic<bool> connected = false;
 };
 
-class ReverseIntefaceTest : public ::testing::Test
+class ReverseInterfaceTest : public ::testing::Test
 {
 protected:
   class Client : public comm::TCPSocket
@@ -181,10 +181,11 @@ class ReverseIntefaceTest : public ::testing::Test
   void SetUp()
   {
     control::ReverseInterfaceConfig config;
-    config.port = 50001;
-    config.handle_program_state = std::bind(&ReverseIntefaceTest::handleProgramState, this, std::placeholders::_1);
+    config.port = 0;
+    config.handle_program_state = std::bind(&ReverseInterfaceTest::handleProgramState, this, std::placeholders::_1);
     reverse_interface_.reset(new TestableReverseInterface(config));
-    client_.reset(new Client(50001));
+    test_port_ = reverse_interface_->getPort();
+    client_.reset(new Client(test_port_));
     std::unique_lock<std::mutex> lk(g_connection_mutex);
     g_connection_condition.wait_for(lk, std::chrono::seconds(1),
                                     [&]() { return reverse_interface_->connected.load(); });
@@ -202,34 +203,47 @@ class ReverseIntefaceTest : public ::testing::Test
   void handleProgramState(bool program_state)
   {
     std::lock_guard<std::mutex> lk(program_running_mutex_);
-    program_running_.notify_one();
+    new_program_state_received_ = true;
     program_state_ = program_state;
+    program_running_.notify_one();
   }
 
   bool waitForProgramState(int milliseconds = 100, bool program_state = true)
   {
+    // Wait for new state until timeout has elapsed
     std::unique_lock<std::mutex> lk(program_running_mutex_);
-    if (program_running_.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout ||
-        program_state_ == program_state)
+    std::chrono::steady_clock::time_point start_time = std::chrono::steady_clock::now();
+    while (
+        program_state_ != program_state &&
+        std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - start_time).count() <
+            milliseconds)
     {
-      if (program_state_ == program_state)
+      if (program_running_.wait_for(lk, std::chrono::milliseconds(milliseconds / 10),
+                                    [this] { return new_program_state_received_.load(); }))
       {
-        return true;
+        new_program_state_received_ = false;
+        // Check whether the new state matches the expected state
+        if (program_state_ == program_state)
+        {
+          return true;
+        }
       }
     }
-    return false;
+    return program_state_ == program_state;
   }
 
   std::unique_ptr<TestableReverseInterface> reverse_interface_;
   std::unique_ptr<Client> client_;
+  int test_port_;
 
 private:
   std::atomic<bool> program_state_ = ATOMIC_VAR_INIT(false);
+  std::atomic<bool> new_program_state_received_ = ATOMIC_VAR_INIT(false);
   std::condition_variable program_running_;
   std::mutex program_running_mutex_;
 };
 
-TEST_F(ReverseIntefaceTest, handle_program_state)
+TEST_F(ReverseInterfaceTest, handle_program_state)
 {
   // Test that handle program state is called when the client connects to the server
   EXPECT_TRUE(waitForProgramState(1000, true));
@@ -239,7 +253,7 @@ TEST_F(ReverseIntefaceTest, handle_program_state)
   EXPECT_TRUE(waitForProgramState(1000, false));
 }
 
-TEST_F(ReverseIntefaceTest, write_positions)
+TEST_F(ReverseInterfaceTest, write_positions)
 {
   // Wait for the client to connect to the server
   EXPECT_TRUE(waitForProgramState(1000, true));
@@ -256,7 +270,7 @@ TEST_F(ReverseIntefaceTest, write_positions)
   EXPECT_EQ(written_positions[5], ((double)received_positions[5]) / reverse_interface_->MULT_JOINTSTATE);
 }
 
-TEST_F(ReverseIntefaceTest, write_trajectory_control_message)
+TEST_F(ReverseInterfaceTest, write_trajectory_control_message)
 {
   // Wait for the client to connect to the server
   EXPECT_TRUE(waitForProgramState(1000, true));
@@ -280,7 +294,7 @@ TEST_F(ReverseIntefaceTest, write_trajectory_control_message)
   EXPECT_EQ(toUnderlying(written_control_message), received_control_message);
 }
 
-TEST_F(ReverseIntefaceTest, write_trajectory_point_number)
+TEST_F(ReverseInterfaceTest, write_trajectory_point_number)
 {
   // Wait for the client to connect to the server
   EXPECT_TRUE(waitForProgramState(1000, true));
@@ -293,7 +307,7 @@ TEST_F(ReverseIntefaceTest, write_trajectory_point_number)
   EXPECT_EQ(written_point_number, received_point_number);
 }
 
-TEST_F(ReverseIntefaceTest, control_mode_is_forward)
+TEST_F(ReverseInterfaceTest, control_mode_is_forward)
 {
   // Wait for the client to connect to the server
   EXPECT_TRUE(waitForProgramState(1000, true));
@@ -306,7 +320,7 @@ TEST_F(ReverseIntefaceTest, control_mode_is_forward)
   EXPECT_EQ(toUnderlying(expected_control_mode), received_control_mode);
 }
 
-TEST_F(ReverseIntefaceTest, remaining_message_points_are_zeros)
+TEST_F(ReverseInterfaceTest, remaining_message_points_are_zeros)
 {
   // Wait for the client to connect to the server
   EXPECT_TRUE(waitForProgramState(1000, true));
@@ -323,7 +337,7 @@ TEST_F(ReverseIntefaceTest, remaining_message_points_are_zeros)
   EXPECT_EQ(0, received_pos[5]);
 }
 
-TEST_F(ReverseIntefaceTest, read_timeout)
+TEST_F(ReverseInterfaceTest, read_timeout)
 {
   // Wait for the client to connect to the server
   EXPECT_TRUE(waitForProgramState(1000, true));
@@ -352,7 +366,7 @@ TEST_F(ReverseIntefaceTest, read_timeout)
   EXPECT_EQ(expected_read_timeout, received_read_timeout);
 }
 
-TEST_F(ReverseIntefaceTest, default_read_timeout)
+TEST_F(ReverseInterfaceTest, default_read_timeout)
 {
   // Wait for the client to connect to the server
   EXPECT_TRUE(waitForProgramState(1000, true));
@@ -379,7 +393,7 @@ TEST_F(ReverseIntefaceTest, default_read_timeout)
   EXPECT_EQ(expected_read_timeout, received_read_timeout);
 }
 
-TEST_F(ReverseIntefaceTest, write_control_mode)
+TEST_F(ReverseInterfaceTest, write_control_mode)
 {
   // Wait for the client to connect to the server
   EXPECT_TRUE(waitForProgramState(1000, true));
@@ -438,7 +452,7 @@ TEST_F(ReverseIntefaceTest, write_control_mode)
   EXPECT_EQ(toUnderlying(expected_control_mode), received_control_mode);
 }
 
-TEST_F(ReverseIntefaceTest, write_freedrive_control_message)
+TEST_F(ReverseInterfaceTest, write_freedrive_control_message)
 {
   // Wait for the client to connect to the server
   EXPECT_TRUE(waitForProgramState(1000, true));
@@ -462,7 +476,7 @@ TEST_F(ReverseIntefaceTest, write_freedrive_control_message)
   EXPECT_EQ(toUnderlying(written_freedrive_message), received_freedrive_message);
 }
 
-TEST_F(ReverseIntefaceTest, deprecated_set_keep_alive_count)
+TEST_F(ReverseInterfaceTest, deprecated_set_keep_alive_count)
 {
   // Wait for the client to connect to the server
   EXPECT_TRUE(waitForProgramState(1000, true));
@@ -489,7 +503,7 @@ TEST_F(ReverseIntefaceTest, deprecated_set_keep_alive_count)
   EXPECT_EQ(expected_read_timeout, received_read_timeout);
 }
 
-TEST_F(ReverseIntefaceTest, disconnected_callbacks_are_called)
+TEST_F(ReverseInterfaceTest, disconnected_callbacks_are_called)
 {
   // Wait for the client to connect to the server
   EXPECT_TRUE(waitForProgramState(1000, true));
@@ -520,7 +534,7 @@ TEST_F(ReverseIntefaceTest, disconnected_callbacks_are_called)
   // Unregister 1. 2 should still be called
   disconnect_called_1 = false;
   disconnect_called_2 = false;
-  client_.reset(new Client(50001));
+  client_.reset(new Client(test_port_));
   EXPECT_TRUE(waitForProgramState(1000, true));
   reverse_interface_->unregisterDisconnectionCallback(disconnection_callback_id_1);
   client_->close();
@@ -532,7 +546,7 @@ TEST_F(ReverseIntefaceTest, disconnected_callbacks_are_called)
   // Unregister both. None should be called
   disconnect_called_1 = false;
   disconnect_called_2 = false;
-  client_.reset(new Client(50001));
+  client_.reset(new Client(test_port_));
   EXPECT_TRUE(waitForProgramState(1000, true));
   reverse_interface_->unregisterDisconnectionCallback(disconnection_callback_id_2);
   client_->close();