moved driver to subfolder

ur_rtde_driver/src/comm/server.cpp

@@ -0,0 +1,157 @@
+/*
+ * Copyright 2017, 2018 Jarek Potiuk (low bandwidth trajectory follower)
+ *
+ * Copyright 2017, 2018 Simon Rasmussen (refactor)
+ *
+ * Copyright 2015, 2016 Thomas Timm Andersen (original version)
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "ur_rtde_driver/comm/server.h"
+#include <arpa/inet.h>
+#include <netinet/tcp.h>
+#include <unistd.h>
+#include <cstring>
+#include "ur_rtde_driver/log.h"
+
+namespace ur_driver
+{
+namespace comm
+{
+URServer::URServer(int port) : port_(port)
+{
+}
+
+URServer::~URServer()
+{
+  TCPSocket::close();
+}
+
+std::string URServer::getIP()
+{
+  sockaddr_in name;
+  socklen_t len = sizeof(name);
+  int res = ::getsockname(getSocketFD(), (sockaddr*)&name, &len);
+
+  if (res < 0)
+  {
+    LOG_ERROR("Could not get local IP");
+    return std::string();
+  }
+
+  char buf[128];
+  inet_ntop(AF_INET, &name.sin_addr, buf, sizeof(buf));
+  return std::string(buf);
+}
+
+bool URServer::open(int socket_fd, struct sockaddr* address, size_t address_len)
+{
+  int flag = 1;
+  setsockopt(socket_fd, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int));
+  return ::bind(socket_fd, address, address_len) == 0;
+}
+
+bool URServer::bind()
+{
+  std::string empty;
+  bool res = TCPSocket::setup(empty, port_);
+
+  if (!res)
+    return false;
+
+  if (::listen(getSocketFD(), 1) < 0)
+    return false;
+
+  return true;
+}
+
+bool URServer::accept()
+{
+  if (TCPSocket::getState() != comm::SocketState::Connected || client_.getSocketFD() > 0)
+    return false;
+
+  struct sockaddr addr;
+  socklen_t addr_len;
+  int client_fd = -1;
+
+  int retry = 0;
+  while ((client_fd = ::accept(getSocketFD(), &addr, &addr_len)) == -1)
+  {
+    LOG_ERROR("Accepting socket connection failed. (errno: %d)", errno);
+    if (retry++ >= 5)
+      return false;
+  }
+
+  TCPSocket::setOptions(client_fd);
+
+  return client_.setSocketFD(client_fd);
+}
+
+void URServer::disconnectClient()
+{
+  if (client_.getState() != comm::SocketState::Connected)
+    return;
+
+  client_.close();
+}
+
+bool URServer::write(const uint8_t* buf, size_t buf_len, size_t& written)
+{
+  return client_.write(buf, buf_len, written);
+}
+
+bool URServer::readLine(char* buffer, size_t buf_len)
+{
+  char* current_pointer = buffer;
+  char ch;
+  size_t total_read;
+
+  if (buf_len <= 0 || buffer == NULL)
+  {
+    return false;
+  }
+
+  total_read = 0;
+  for (;;)
+  {
+    if (client_.read(&ch))
+    {
+      if (total_read < buf_len - 1)  // just in case ...
+      {
+        total_read++;
+        *current_pointer++ = ch;
+      }
+      if (ch == '\n')
+      {
+        break;
+      }
+    }
+    else
+    {
+      if (total_read == 0)
+      {
+        return false;
+      }
+      else
+      {
+        break;
+      }
+    }
+  }
+
+  *current_pointer = '\0';
+  return true;
+}
+}  // namespace comm
+}  // namespace ur_driver

ur_rtde_driver/src/comm/tcp_socket.cpp

@@ -0,0 +1,192 @@
+/*
+ * Copyright 2017, 2018 Jarek Potiuk (low bandwidth trajectory follower)
+ *
+ * Copyright 2017, 2018 Simon Rasmussen (refactor)
+ *
+ * Copyright 2015, 2016 Thomas Timm Andersen (original version)
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <arpa/inet.h>
+#include <endian.h>
+#include <netinet/tcp.h>
+#include <unistd.h>
+#include <cstring>
+
+#include "ur_rtde_driver/log.h"
+#include "ur_rtde_driver/comm/tcp_socket.h"
+
+namespace ur_driver
+{
+namespace comm
+{
+TCPSocket::TCPSocket() : socket_fd_(-1), state_(SocketState::Invalid)
+{
+}
+TCPSocket::~TCPSocket()
+{
+  close();
+}
+
+void TCPSocket::setOptions(int socket_fd)
+{
+  int flag = 1;
+  setsockopt(socket_fd, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(int));
+  setsockopt(socket_fd, IPPROTO_TCP, TCP_QUICKACK, &flag, sizeof(int));
+}
+
+bool TCPSocket::setup(std::string& host, int port)
+{
+  if (state_ == SocketState::Connected)
+    return false;
+
+  LOG_INFO("Setting up connection: %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
+
+  const char* host_name = host.empty() ? nullptr : host.c_str();
+  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_name, service.c_str(), &hints, &result) != 0)
+  {
+    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)
+  {
+    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))
+    {
+      connected = true;
+      break;
+    }
+  }
+
+  freeaddrinfo(result);
+
+  if (!connected)
+  {
+    state_ = SocketState::Invalid;
+    LOG_ERROR("Connection setup failed for %s:%d", host.c_str(), port);
+  }
+  else
+  {
+    setOptions(socket_fd_);
+    state_ = SocketState::Connected;
+    LOG_INFO("Connection established for %s:%d", host.c_str(), port);
+  }
+  return connected;
+}
+
+bool TCPSocket::setSocketFD(int socket_fd)
+{
+  if (state_ == SocketState::Connected)
+    return false;
+  socket_fd_ = socket_fd;
+  state_ = SocketState::Connected;
+  return true;
+}
+
+void TCPSocket::close()
+{
+  if (state_ != SocketState::Connected)
+    return;
+  state_ = SocketState::Closed;
+  ::close(socket_fd_);
+  socket_fd_ = -1;
+}
+
+std::string TCPSocket::getIP()
+{
+  sockaddr_in name;
+  socklen_t len = sizeof(name);
+  int res = ::getsockname(socket_fd_, (sockaddr*)&name, &len);
+
+  if (res < 0)
+  {
+    LOG_ERROR("Could not get local IP");
+    return std::string();
+  }
+
+  char buf[128];
+  inet_ntop(AF_INET, &name.sin_addr, buf, sizeof(buf));
+  return std::string(buf);
+}
+
+bool TCPSocket::read(char* character)
+{
+  size_t read_chars;
+  // It's inefficient, but in our case we read very small messages
+  // and the overhead connected with reading character by character is
+  // negligible - adding buffering would complicate the code needlessly.
+  return read((uint8_t*)character, 1, read_chars);
+}
+
+bool TCPSocket::read(uint8_t* buf, const size_t buf_len, size_t& read)
+{
+  read = 0;
+
+  if (state_ != SocketState::Connected)
+    return false;
+
+  ssize_t res = ::recv(socket_fd_, buf, buf_len, 0);
+
+  if (res == 0)
+  {
+    state_ = SocketState::Disconnected;
+    return false;
+  }
+  else if (res < 0)
+    return false;
+
+  read = static_cast<size_t>(res);
+  return true;
+}
+
+bool TCPSocket::write(const uint8_t* buf, const size_t buf_len, size_t& written)
+{
+  written = 0;
+
+  if (state_ != SocketState::Connected)
+    return false;
+
+  size_t remaining = buf_len;
+
+  // handle partial sends
+  while (written < buf_len)
+  {
+    ssize_t sent = ::send(socket_fd_, buf + written, remaining, 0);
+
+    if (sent <= 0)
+      return false;
+
+    written += sent;
+    remaining -= sent;
+  }
+
+  return true;
+}
+}  // namespace comm
+}  // namespace ur_driver