Major refactor

include/ur_modern_driver/ur/commander.h

@@ -11,18 +11,46 @@ private:
 
 protected:
   bool write(std::string& s);
+  void formatArray(std::ostringstream &out, std::array<double, 6> &values);
 
 public:
   URCommander(URStream& stream) : stream_(stream)
   {
   }
 
-  virtual bool uploadProg(std::string &s);
+  virtual bool speedj(std::array<double, 6> &speeds, double acceleration) = 0;
+  virtual bool setDigitalOut(uint8_t pin, bool value) = 0;
+  virtual bool setAnalogOut(uint8_t pin, double value) = 0;
+
+  //shared
+  bool uploadProg(std::string &s);
+  bool stopj(double a = 10.0);
+  bool setToolVoltage(uint8_t voltage);
+  bool setFlag(uint8_t pin, bool value);
+  bool setPayload(double value);
+};
+
+class URCommander_V1_X : public URCommander
+{
+public:
+  URCommander_V1_X(URStream& stream) : URCommander(stream)
+  {
+  }
+
+  virtual bool speedj(std::array<double, 6> &speeds, double acceleration);
+  virtual bool setDigitalOut(uint8_t pin, bool value);
+  virtual bool setAnalogOut(uint8_t pin, double value);
+};
+
+
+class URCommander_V3_X : public URCommander
+{
+public:
+  URCommander_V3_X(URStream& stream) : URCommander(stream)
+  {
+  }
+
   virtual bool speedj(std::array<double, 6> &speeds, double acceleration);
-  virtual bool stopj(double a = 10.0);
   virtual bool setDigitalOut(uint8_t pin, bool value);
   virtual bool setAnalogOut(uint8_t pin, double value);
-  virtual bool setToolVoltage(uint8_t voltage);
-  virtual bool setFlag(uint8_t pin, bool value);
-  virtual bool setPayload(double value);
 };

include/ur_modern_driver/ur/factory.h

@@ -71,6 +71,19 @@ public:
     prod.teardownProducer();
   }
 
+  bool isVersion3()
+  {
+    return major_version_ == 3;
+  }
+
+  std::unique_ptr<URCommander> getCommander(URStream &stream)
+  {
+    if(major_version_ == 1)
+      return std::unique_ptr<URCommander>(new URCommander_V1_X(stream));
+    else
+      return std::unique_ptr<URCommander>(new URCommander_V3_X(stream));    
+  }
+
   std::unique_ptr<URParser<StatePacket>> getStateParser()
   {
     if (major_version_ == 1)

include/ur_modern_driver/ur/producer.h

@@ -1,4 +1,5 @@
 #pragma once
+#include <chrono>
 #include "ur_modern_driver/pipeline.h"
 #include "ur_modern_driver/ur/parser.h"
 #include "ur_modern_driver/ur/stream.h"
@@ -9,9 +10,10 @@ class URProducer : public IProducer<T>
 private:
   URStream& stream_;
   URParser<T>& parser_;
+  std::chrono::seconds timeout_;
 
 public:
-  URProducer(URStream& stream, URParser<T>& parser) : stream_(stream), parser_(parser)
+  URProducer(URStream& stream, URParser<T>& parser) : stream_(stream), parser_(parser), timeout_(1)
   {
   }
 
@@ -32,24 +34,29 @@ public:
   {
     // 4KB should be enough to hold any packet received from UR
     uint8_t buf[4096];
-
-    // blocking call
-    ssize_t len = stream_.receive(buf, sizeof(buf));
-
-    // LOG_DEBUG("Read %d bytes from stream", len);
-
-    if (len == 0)
+    size_t read = 0;
+    //expoential backoff reconnects
+    while(true)
     {
-      LOG_WARN("Read nothing from stream");
-      return false;
-    }
-    else if (len < 0)
-    {
-      LOG_WARN("Stream closed");
-      return false;
+      if(stream_.read(buf, sizeof(buf), read))
+      {
+        //reset sleep amount
+        timeout_ = std::chrono::seconds(1);
+        break;
+      }
+
+      if(stream_.closed())
+        return false;
+
+      LOG_WARN("Failed to read from stream, reconnecting in %ld seconds...", timeout_.count());
+      std::this_thread::sleep_for(timeout_);
+      auto next = timeout_ * 2;
+      if(next <= std::chrono::seconds(120))
+        timeout_ = next;
     }
 
-    BinParser bp(buf, static_cast<size_t>(len));
+
+    BinParser bp(buf, read);
     return parser_.parse(bp, products);
   }
 };

include/ur_modern_driver/ur/server.h

@@ -6,14 +6,25 @@
 #include <mutex>
 #include <atomic>
 #include <string>
-#include "ur_modern_driver/ur/stream.h"
+#include "ur_modern_driver/tcp_socket.h"
 
-class URServer
+class URServer : private TCPSocket
 {
 private:
-  int socket_fd_ = -1;
+  int port_;
+  SocketState state_;
+  TCPSocket client_;
+
+protected:
+  virtual bool open(int socket_fd, struct sockaddr *address, size_t address_len)
+  {
+    return ::bind(socket_fd, address, address_len) == 0;
+  }
 
 public:
   URServer(int port);
-  URStream accept();
+  std::string getIP();
+  bool bind();
+  bool accept();
+  bool write(const uint8_t* buf, size_t buf_len, size_t &written);
 };

include/ur_modern_driver/ur/stream.h

@@ -5,56 +5,39 @@
 #include <mutex>
 #include <atomic>
 #include <string>
+#include "ur_modern_driver/log.h"
+#include "ur_modern_driver/tcp_socket.h"
 
-/// Encapsulates a TCP socket
-class URStream
+class URStream : private TCPSocket
 {
 private:
-  int socket_fd_ = -1;
   std::string host_;
   int port_;
+  std::mutex write_mutex_, read_mutex_;
 
-  std::atomic<bool> initialized_;
-  std::atomic<bool> stopping_;
-  std::mutex send_mutex_, receive_mutex_;
+protected:
+  virtual bool open(int socket_fd, struct sockaddr *address, size_t address_len)
+  {
+    return ::connect(socket_fd, address, address_len) == 0;
+  }
 
 public:
-  URStream() 
+  URStream(std::string& host, int port) : host_(host), port_(port)
   {
   }
 
-  URStream(std::string& host, int port) : host_(host), port_(port), initialized_(false), stopping_(false)
+  bool connect()
   {
+    return TCPSocket::setup(host_, port_);
+  }
+  void disconnect()
+  {
+    LOG_INFO("Disconnecting");
+    TCPSocket::close();
   }
 
-  URStream(int socket_fd) : socket_fd_(socket_fd), initialized_(true), stopping_(false)
-  {
-    
-  }
+  bool closed() { return getState() == SocketState::Closed; }
 
-  URStream(URStream&& other) noexcept : socket_fd_(other.socket_fd_), host_(other.host_), initialized_(other.initialized_.load()), stopping_(other.stopping_.load())
-  {
-    
-  }
-
-  ~URStream()
-  {
-    disconnect();
-  }
-
-  URStream& operator=(URStream&& other)
-  {
-    socket_fd_ = std::move(other.socket_fd_);
-    host_ = std::move(other.host_); 
-    initialized_ = std::move(other.initialized_.load());
-    stopping_ = std::move(other.stopping_.load());
-    return *this;
-  }
-
-  bool connect();
-  void disconnect();
-  void reconnect();
-
-  ssize_t send(const uint8_t* buf, size_t buf_len);
-  ssize_t receive(uint8_t* buf, size_t buf_len);
+  bool read(uint8_t* buf, size_t buf_len, size_t &read);
+  bool write(const uint8_t* buf, size_t buf_len, size_t &written);
 };