Major refactor

src/ur/commander.cpp

@@ -3,10 +3,21 @@
 
 bool URCommander::write(std::string& s)
 {
-    size_t len = s.size();
-    const uint8_t* data = reinterpret_cast<const uint8_t*>(s.c_str());
-    ssize_t res = stream_.send(data, len);
-    return res > 0 && static_cast<size_t>(res) == len;
+  size_t len = s.size();
+  const uint8_t* data = reinterpret_cast<const uint8_t*>(s.c_str());
+  size_t written;
+  return stream_.write(data, len, written);
+}
+
+void URCommander::formatArray(std::ostringstream &out, std::array<double, 6> &values)
+{
+  std::string mod("[");
+  for(auto const& val : values)
+  {
+      out << mod << val;
+      mod = ",";
+  }
+  out << "]";
 }
 
 bool URCommander::uploadProg(std::string &s)
@@ -14,59 +25,110 @@ bool URCommander::uploadProg(std::string &s)
   return write(s);
 }
 
-bool URCommander::speedj(std::array<double, 6> &speeds, double acceleration)
-{
-    std::ostringstream out;
-    out << std::fixed << std::setprecision(4);
-    out << "speedj([";
-    std::string mod;
-    for(auto const& val : speeds)
-    {
-        out << mod << val;
-        mod = ",";
-    }
-    out << "]," << acceleration << ")\n";
-    std::string s(out.str());
-    return write(s);
-}
-bool URCommander::stopj(double a)
-{
-
-}
-
-bool URCommander::setAnalogOut(uint8_t pin, double value)
-{
-    std::ostringstream out;
-    out << "set_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(4) << value << ")\n";
-    std::string s(out.str());
-    return write(s);
-}
-
-bool URCommander::setDigitalOut(uint8_t pin, bool value)
-{
-    std::ostringstream out;
-    out << "set_digital_out(" << (int)pin << "," << (value ? "True" : "False") << ")\n";
-    std::string s(out.str());
-    return write(s);
-}
-
 bool URCommander::setToolVoltage(uint8_t voltage)
 {
-    
-}
+  if(voltage != 0 || voltage != 12 || voltage != 24)
+    return false;
 
+  std::ostringstream out;
+  out << "set_tool_voltage(" << (int)voltage << ")\n";
+  std::string s(out.str());
+  return write(s);
+}
 
 bool URCommander::setFlag(uint8_t pin, bool value)
 {
-    std::ostringstream out;
-    out << "set_flag(" << (int)pin << "," << (value ? "True" : "False") << ")\n";
-    std::string s(out.str());
-    return write(s);
+  std::ostringstream out;
+  out << "set_flag(" << (int)pin << "," << (value ? "True" : "False") << ")\n";
+  std::string s(out.str());
+  return write(s);
 }
 bool URCommander::setPayload(double value)
 {
-    std::ostringstream out;
-    out << "set_payload(" << std::fixed << std::setprecision(4) << value << ")\n";
-    std::string s(out.str());
-    return write(s);
-}
+  std::ostringstream out;
+  out << "set_payload(" << std::fixed << std::setprecision(5) << value << ")\n";
+  std::string s(out.str());
+  return write(s);
+}
+
+bool URCommander::stopj(double a)
+{
+  std::ostringstream out;
+  out << "stopj(" << std::fixed << std::setprecision(5) << a << ")\n";
+  std::string s(out.str());
+  return write(s);
+}
+
+
+bool URCommander_V1_X::speedj(std::array<double, 6> &speeds, double acceleration)
+{
+  std::ostringstream out;
+  out << std::fixed << std::setprecision(5);
+  out << "speedj(";
+  formatArray(out, speeds);
+  out << "," << acceleration << "," << 0.02 << ")\n";
+  std::string s(out.str());
+  return write(s);
+}
+bool URCommander_V1_X::setAnalogOut(uint8_t pin, double value)
+{
+  std::ostringstream out;
+  out << "set_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(4) << value << ")\n";
+  std::string s(out.str());
+  return write(s);
+}
+
+bool URCommander_V1_X::setDigitalOut(uint8_t pin, bool value)
+{
+  std::ostringstream out;
+  out << "set_digital_out(" << (int)pin << "," << (value ? "True" : "False") << ")\n";
+  std::string s(out.str());
+  return write(s);
+}
+
+
+bool URCommander_V3_X::speedj(std::array<double, 6> &speeds, double acceleration)
+{
+  std::ostringstream out;
+  out << std::fixed << std::setprecision(5);
+  out << "speedj(";
+  formatArray(out, speeds);
+  out << "," << acceleration << ")\n";
+  std::string s(out.str());
+  return write(s);
+}
+bool URCommander_V3_X::setAnalogOut(uint8_t pin, double value)
+{
+  std::ostringstream out;
+  out << "set_standard_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(5) << value << ")\n";
+  std::string s(out.str());
+  return write(s);
+}
+
+bool URCommander_V3_X::setDigitalOut(uint8_t pin, bool value)
+{
+  std::ostringstream out;
+  std::string func;
+
+  if(pin < 8)
+  {
+    func = "set_standard_digital_out";
+  }
+  else if(pin < 16)
+  {
+    func = "set_configurable_digital_out";
+    pin -= 8;
+  }
+  else if(pin < 18)
+  {
+    func = "set_tool_digital_out";
+    pin -= 16;
+  }
+  else
+    return false;
+
+
+  out << func << "(" << (int)pin << "," << (value ? "True" : "False") << ")\n";
+  std::string s(out.str());
+  return write(s);
+}

src/ur/server.cpp

@@ -5,47 +5,48 @@
 #include "ur_modern_driver/ur/server.h"
 
 URServer::URServer(int port)
+  : port_(port)
 {
-  std::string service = std::to_string(port);
-  struct addrinfo hints, *result;
-  std::memset(&hints, 0, sizeof(hints));
-
-  hints.ai_family = AF_UNSPEC;
-  hints.ai_socktype = SOCK_STREAM;
-  hints.ai_flags = AI_PASSIVE;
-
-  if (getaddrinfo(nullptr, service.c_str(), &hints, &result) != 0)
-  {
-    LOG_ERROR("Failed to setup recieving server");
-    return;
-  }
-
-    // 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);
-
-    if (socket_fd_ == -1)  // socket error?
-      continue;
-
-    if (bind(socket_fd_, p->ai_addr, p->ai_addrlen) != 0)
-      continue;
-
-    // disable Nagle's algorithm to ensure we sent packets as fast as possible
-    int flag = 1;
-    setsockopt(socket_fd_, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(flag));
-    LOG_INFO("Server awaiting connection");
-    return;
-  }
-  
-  LOG_ERROR("Failed to setup recieving server");
-  std::exit(EXIT_FAILURE);
 }
 
-URStream URServer::accept()
+std::string URServer::getIP()
 {
+  char buf[128];
+  int res = ::gethostname(buf, sizeof(buf));
+  return std::string(buf);
+}
+
+bool URServer::bind()
+{
+  std::string empty;
+  bool res = TCPSocket::setup(empty, port_);
+  state_ = TCPSocket::getState();
+  
+  if(!res)
+    return false;
+
+  if(::listen(getSocketFD(), 1) < 0)
+    return false;
+
+  return true;
+}
+
+bool URServer::accept()
+{
+  if(state_ != SocketState::Connected || client_.getSocketFD() > 0)
+    return false;
+
   struct sockaddr addr; 
   socklen_t addr_len;
-  int client_fd = ::accept(socket_fd_, &addr, &addr_len);
-  return URStream(client_fd);
+  int client_fd = ::accept(getSocketFD(), &addr, &addr_len);
+
+  if(client_fd <= 0)
+    return false;
+
+  return client_.setSocketFD(client_fd);
+}
+
+bool URServer::write(const uint8_t* buf, size_t buf_len, size_t &written)
+{
+  return client_.write(buf, buf_len, written);
 }

src/ur/stream.cpp

@@ -6,139 +6,38 @@
 #include "ur_modern_driver/log.h"
 #include "ur_modern_driver/ur/stream.h"
 
-bool URStream::connect()
+bool URStream::write(const uint8_t* buf, size_t buf_len, size_t &written)
 {
-  if (initialized_)
-    return false;
-
-  LOG_INFO("Connecting to UR @ %s:%d", host_.c_str(), port_);
-
-  // gethostbyname() is deprecated so use getadderinfo() as described in:
-  // http://www.beej.us/guide/bgnet/output/html/multipage/syscalls.html#getaddrinfo
-
-  std::string service = std::to_string(port_);
-  struct addrinfo hints, *result;
-  std::memset(&hints, 0, sizeof(hints));
-
-  hints.ai_family = AF_UNSPEC;
-  hints.ai_socktype = SOCK_STREAM;
-  hints.ai_flags = AI_PASSIVE;
-
-  if (getaddrinfo(host_.c_str(), service.c_str(), &hints, &result) != 0)
-  {
-    LOG_ERROR("Failed to get host name");
-    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);
-
-    if (socket_fd_ == -1)  // socket error?
-      continue;
-
-    if (::connect(socket_fd_, p->ai_addr, p->ai_addrlen) != 0)
-    {
-      if (stopping_)
-        break;
-      else
-        continue;  // try next addrinfo if connect fails
-    }
-
-    // disable Nagle's algorithm to ensure we sent packets as fast as possible
-    int flag = 1;
-    setsockopt(socket_fd_, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(flag));
-    initialized_ = true;
-    LOG_INFO("Connection successfully established");
-    break;
-  }
-
-  freeaddrinfo(result);
-  if (!initialized_)
-    LOG_ERROR("Connection failed");
-
-  return initialized_;
+  std::lock_guard<std::mutex> lock(write_mutex_);
+  return TCPSocket::write(buf, buf_len, written);
 }
 
-void URStream::disconnect()
+bool URStream::read(uint8_t* buf, size_t buf_len, size_t &total)
 {
-  if (!initialized_ || stopping_)
-    return;
+  std::lock_guard<std::mutex> lock(read_mutex_);   
 
-  LOG_INFO("Disconnecting from %s:%d", host_.c_str(), port_);
-
-  stopping_ = true;
-  close(socket_fd_);
-  initialized_ = false;
-}
-
-void URStream::reconnect()
-{
-  disconnect();
-  stopping_ = false;
-  connect();
-}
-
-ssize_t URStream::send(const uint8_t* buf, size_t buf_len)
-{
-  if (!initialized_)
-    return -1;
-  if (stopping_)
-    return 0;
-
-  std::lock_guard<std::mutex> lock(send_mutex_);
-
-  size_t total = 0;
-  size_t remaining = buf_len;
-
-  // TODO: handle reconnect?
-  // handle partial sends
-  while (total < buf_len)
-  {
-    ssize_t sent = ::send(socket_fd_, buf + total, remaining, 0);
-    if (sent <= 0)
-      return stopping_ ? 0 : sent;
-    total += sent;
-    remaining -= sent;
-  }
-
-  return total;
-}
-
-ssize_t URStream::receive(uint8_t* buf, size_t buf_len)
-{
-  if (!initialized_)
-    return -1;
-  if (stopping_)
-    return 0;
-
-  std::lock_guard<std::mutex> lock(receive_mutex_);    
-
-  size_t remainder = sizeof(int32_t);
-  uint8_t* buf_pos = buf;
   bool initial = true;
+  uint8_t* buf_pos = buf;
+  size_t remainder = sizeof(int32_t);
+  size_t read = 0;
 
-  do
+  while(remainder > 0 && TCPSocket::read(buf_pos, remainder, read))
   {
-    ssize_t read = recv(socket_fd_, buf_pos, remainder, 0);
-    if (read <= 0)  // failed reading from socket
-      return stopping_ ? 0 : read;
-
     if (initial)
     {
       remainder = be32toh(*(reinterpret_cast<int32_t*>(buf)));
       if (remainder >= (buf_len - sizeof(int32_t)))
       {
         LOG_ERROR("Packet size %zd is larger than buffer %zu, discarding.", remainder, buf_len);
-        return -1;
+        return false;
       }
       initial = false;
     }
 
+    total += read;
     buf_pos += read;
     remainder -= read;
-  } while (remainder > 0);
-
-  return buf_pos - buf;
+  }
+  
+  return remainder == 0;
 }