Support starting the driver, before the robot is booted (UniversalRobots#98)

* Support starting the driver, before the robot is booted

Refactored client library to support starting the client, before the robot is booted.

Changed producer to wait until robot is reachable on configured IP, instead of throwing an exception.

* Move the endless connection attempts to the tcp socket

This will enable the dashboard client and secondary client to wait for a robot to be booted

* Fix formatting.

Co-authored-by: Felix Exner <[email protected]>

Author: urmahp

.gitignore

@@ -6,4 +6,5 @@ install_manifest.txt
 *~
 .idea
 .directory
-.vscode
+.vscode
+build/

include/ur_client_library/comm/stream.h

@@ -104,6 +104,16 @@ class URStream : public TCPSocket
    */
   bool write(const uint8_t* buf, const size_t buf_len, size_t& written);
 
+  /*!
+   * \brief Get the host IP
+   *
+   * \returns The host IP
+   */
+  std::string getHost()
+  {
+    return host_;
+  }
+
 protected:
   virtual bool open(int socket_fd, struct sockaddr* address, size_t address_len)
   {

include/ur_client_library/rtde/control_package_pause.h

@@ -82,6 +82,18 @@ class ControlPackagePauseRequest : public RTDEPackage
   {
   }
   virtual ~ControlPackagePauseRequest() = default;
+
+  /*!
+   * \brief Generates a serialized package.
+   *
+   * \param buffer Buffer to fill with the serialization
+   *
+   * \returns The total size of the serialized package
+   */
+  static size_t generateSerializedRequest(uint8_t* buffer);
+
+private:
+  static const uint16_t PAYLOAD_SIZE = 0;
 };
 
 }  // namespace rtde_interface

include/ur_client_library/rtde/rtde_client.h

@@ -51,6 +51,7 @@ namespace rtde_interface
 {
 static const uint16_t MAX_RTDE_PROTOCOL_VERSION = 2;
 static const unsigned MAX_REQUEST_RETRIES = 5;
+static const unsigned MAX_INITIALIZE_ATTEMPTS = 10;
 
 enum class UrRtdeRobotStatusBits
 {
@@ -75,6 +76,15 @@ enum class UrRtdeSafetyStatusBits
   IS_STOPPED_DUE_TO_SAFETY = 10
 };
 
+enum class ClientState
+{
+  UNINITIALIZED = 0,
+  INITIALIZING = 1,
+  INITIALIZED = 2,
+  RUNNING = 3,
+  PAUSED = 4
+};
+
 /*!
  * \brief The RTDEClient class manages communication over the RTDE interface. It contains the RTDE
  * handshake and read and write functionality to and from the robot.
@@ -109,6 +119,12 @@ class RTDEClient
    * \returns Success of the requested start
    */
   bool start();
+  /*!
+   * \brief Pauses RTDE data package communication
+   *
+   * \returns Wheter the RTDE data package communication was paussed succesfully
+   */
+  bool pause();
   /*!
    * \brief Reads the pipeline to fetch the next data package.
    *
@@ -177,15 +193,34 @@ class RTDEClient
   double max_frequency_;
   double target_frequency_;
 
+  ClientState client_state_;
+
   constexpr static const double CB3_MAX_FREQUENCY = 125.0;
   constexpr static const double URE_MAX_FREQUENCY = 500.0;
 
   std::vector<std::string> readRecipe(const std::string& recipe_file);
 
+  void setupCommunication();
   bool negotiateProtocolVersion(const uint16_t protocol_version);
   void queryURControlVersion();
   void setupOutputs(const uint16_t protocol_version);
   void setupInputs();
+  void disconnect();
+
+  /*!
+   * \brief Checks wheter the robot is booted, this is done by looking at the timestamp from the robot controller, this
+   * will show the time in seconds since the controller was started. If the timestamp is below 40, we will read from
+   * the stream for approximately 1 second to ensure that the RTDE interface is up and running. This will ensure that we
+   * don't finalize setting up communication, before the controller is up and running. Else we could end up connecting
+   * to the RTDE interface, before a restart occurs during robot boot which would then destroy the connection
+   * established.
+   *
+   * \returns true if the robot is booted, false otherwise which will essentially trigger a reconnection.
+   */
+  bool isRobotBooted();
+  bool sendStart();
+  bool sendPause();
+
   /*!
    * \brief Splits a variable_types string as reported from the robot into single variable type
    * strings

src/comm/tcp_socket.cpp

@@ -25,6 +25,8 @@
 #include <netinet/tcp.h>
 #include <unistd.h>
 #include <cstring>
+#include <sstream>
+#include <thread>
 
 #include "ur_client_library/log.h"
 #include "ur_client_library/comm/tcp_socket.h"
@@ -72,38 +74,41 @@ bool TCPSocket::setup(std::string& host, int port)
   hints.ai_socktype = SOCK_STREAM;
   hints.ai_flags = AI_PASSIVE;
 
-  if (getaddrinfo(host_name, service.c_str(), &hints, &result) != 0)
-  {
-    URCL_LOG_ERROR("Failed to get address for %s:%d", host.c_str(), port);
-    return false;
-  }
-
   bool connected = false;
-  // loop through the list of addresses untill we find one that's connectable
-  for (struct addrinfo* p = result; p != nullptr; p = p->ai_next)
+  while (!connected)
   {
-    socket_fd_ = ::socket(p->ai_family, p->ai_socktype, p->ai_protocol);
-
-    if (socket_fd_ != -1 && open(socket_fd_, p->ai_addr, p->ai_addrlen))
+    if (getaddrinfo(host_name, service.c_str(), &hints, &result) != 0)
     {
-      connected = true;
-      break;
+      URCL_LOG_ERROR("Failed to get address for %s:%d", host.c_str(), port);
+      return false;
     }
-  }
+    // loop through the list of addresses untill we find one that's connectable
+    for (struct addrinfo* p = result; p != nullptr; p = p->ai_next)
+    {
+      socket_fd_ = ::socket(p->ai_family, p->ai_socktype, p->ai_protocol);
 
-  freeaddrinfo(result);
+      if (socket_fd_ != -1 && open(socket_fd_, p->ai_addr, p->ai_addrlen))
+      {
+        connected = true;
+        break;
+      }
+    }
 
-  if (!connected)
-  {
-    state_ = SocketState::Invalid;
-    URCL_LOG_ERROR("Connection setup failed for %s:%d", host.c_str(), port);
-  }
-  else
-  {
-    setOptions(socket_fd_);
-    state_ = SocketState::Connected;
-    URCL_LOG_DEBUG("Connection established for %s:%d", host.c_str(), port);
+    freeaddrinfo(result);
+
+    if (!connected)
+    {
+      state_ = SocketState::Invalid;
+      std::stringstream ss;
+      ss << "Failed to connect to robot on IP " << host_name
+         << ". Please check that the robot is booted and reachable on " << host_name << ". Retrying in 10 seconds";
+      URCL_LOG_ERROR("%s", ss.str().c_str());
+      std::this_thread::sleep_for(std::chrono::seconds(10));
+    }
   }
+  setOptions(socket_fd_);
+  state_ = SocketState::Connected;
+  URCL_LOG_DEBUG("Connection established for %s:%d", host.c_str(), port);
   return connected;
 }
 

src/rtde/control_package_pause.cpp

@@ -45,5 +45,11 @@ std::string ControlPackagePause::toString() const
 
   return ss.str();
 }
+
+size_t ControlPackagePauseRequest::generateSerializedRequest(uint8_t* buffer)
+{
+  return PackageHeader::serializeHeader(buffer, PackageType::RTDE_CONTROL_PACKAGE_PAUSE, PAYLOAD_SIZE);
+}
+
 }  // namespace rtde_interface
 }  // namespace urcl