put communication-related classes to namespace

include/ur_rtde_driver/comm/parser.h

@@ -0,0 +1,36 @@
+/*
+ * 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.
+ */
+
+#pragma once
+#include <vector>
+#include "ur_rtde_driver/bin_parser.h"
+#include "ur_rtde_driver/comm/pipeline.h"
+
+namespace ur_rtde_driver
+{
+namespace comm
+{
+template <typename T>
+class URParser
+{
+public:
+  virtual bool parse(BinParser& bp, std::vector<std::unique_ptr<T>>& results) = 0;
+};  // namespace commtemplate<typenameT>classURParser
+
+}  // namespace comm
+}  // namespace ur_rtde_driver

include/ur_rtde_driver/comm/pipeline.h

@@ -0,0 +1,238 @@
+/*
+ * 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.
+ */
+
+#pragma once
+
+#include <atomic>
+#include <chrono>
+#include <thread>
+#include <vector>
+#include "ur_rtde_driver/log.h"
+#include "ur_rtde_driver/queue/readerwriterqueue.h"
+
+namespace ur_rtde_driver
+{
+namespace comm
+{
+// TODO: Remove these!!!
+using namespace moodycamel;
+
+template <typename T>
+class IConsumer
+{
+public:
+  virtual void setupConsumer()
+  {
+  }
+  virtual void teardownConsumer()
+  {
+  }
+  virtual void stopConsumer()
+  {
+  }
+  virtual void onTimeout()
+  {
+  }
+
+  virtual bool consume(std::shared_ptr<T> product) = 0;
+};
+
+template <typename T>
+class MultiConsumer : public IConsumer<T>
+{
+private:
+  std::vector<IConsumer<T>*> consumers_;
+
+public:
+  MultiConsumer(std::vector<IConsumer<T>*> consumers) : consumers_(consumers)
+  {
+  }
+
+  virtual void setupConsumer()
+  {
+    for (auto& con : consumers_)
+    {
+      con->setupConsumer();
+    }
+  }
+  virtual void teardownConsumer()
+  {
+    for (auto& con : consumers_)
+    {
+      con->teardownConsumer();
+    }
+  }
+  virtual void stopConsumer()
+  {
+    for (auto& con : consumers_)
+    {
+      con->stopConsumer();
+    }
+  }
+  virtual void onTimeout()
+  {
+    for (auto& con : consumers_)
+    {
+      con->onTimeout();
+    }
+  }
+
+  bool consume(std::shared_ptr<T> product)
+  {
+    bool res = true;
+    for (auto& con : consumers_)
+    {
+      if (!con->consume(product))
+        res = false;
+    }
+    return res;
+  }
+};
+
+template <typename T>
+class IProducer
+{
+public:
+  virtual void setupProducer()
+  {
+  }
+  virtual void teardownProducer()
+  {
+  }
+  virtual void stopProducer()
+  {
+  }
+
+  virtual bool tryGet(std::vector<std::unique_ptr<T>>& products) = 0;
+};
+
+class INotifier
+{
+public:
+  virtual void started(std::string name)
+  {
+  }
+  virtual void stopped(std::string name)
+  {
+  }
+};
+
+template <typename T>
+class Pipeline
+{
+private:
+  typedef std::chrono::high_resolution_clock Clock;
+  typedef Clock::time_point Time;
+  IProducer<T>& producer_;
+  IConsumer<T>& consumer_;
+  std::string name_;
+  INotifier& notifier_;
+  BlockingReaderWriterQueue<std::unique_ptr<T>> queue_;
+  std::atomic<bool> running_;
+  std::thread pThread_, cThread_;
+
+  void run_producer()
+  {
+    producer_.setupProducer();
+    std::vector<std::unique_ptr<T>> products;
+    while (running_)
+    {
+      if (!producer_.tryGet(products))
+      {
+        break;
+      }
+
+      for (auto& p : products)
+      {
+        if (!queue_.try_enqueue(std::move(p)))
+        {
+          LOG_ERROR("Pipeline producer overflowed! <%s>", name_.c_str());
+        }
+      }
+
+      products.clear();
+    }
+    producer_.teardownProducer();
+    LOG_DEBUG("Pipeline producer ended! <%s>", name_.c_str());
+    consumer_.stopConsumer();
+    running_ = false;
+    notifier_.stopped(name_);
+  }
+
+  void run_consumer()
+  {
+    consumer_.setupConsumer();
+    std::unique_ptr<T> product;
+    while (running_)
+    {
+      // timeout was chosen because we should receive messages
+      // at roughly 125hz (every 8ms) and have to update
+      // the controllers (i.e. the consumer) with *at least* 125Hz
+      // So we update the consumer more frequently via onTimeout
+      if (!queue_.wait_dequeue_timed(product, std::chrono::milliseconds(8)))
+      {
+        consumer_.onTimeout();
+        continue;
+      }
+
+      if (!consumer_.consume(std::move(product)))
+        break;
+    }
+    consumer_.teardownConsumer();
+    LOG_DEBUG("Pipeline consumer ended! <%s>", name_.c_str());
+    producer_.stopProducer();
+    running_ = false;
+    notifier_.stopped(name_);
+  }
+
+public:
+  Pipeline(IProducer<T>& producer, IConsumer<T>& consumer, std::string name, INotifier& notifier)
+    : producer_(producer), consumer_(consumer), name_(name), notifier_(notifier), queue_{ 32 }, running_{ false }
+  {
+  }
+
+  void run()
+  {
+    if (running_)
+      return;
+
+    running_ = true;
+    pThread_ = std::thread(&Pipeline::run_producer, this);
+    cThread_ = std::thread(&Pipeline::run_consumer, this);
+    notifier_.started(name_);
+  }
+
+  void stop()
+  {
+    if (!running_)
+      return;
+
+    LOG_DEBUG("Stopping pipeline! <%s>", name_.c_str());
+
+    consumer_.stopConsumer();
+    producer_.stopProducer();
+
+    running_ = false;
+
+    pThread_.join();
+    cThread_.join();
+    notifier_.stopped(name_);
+  }
+};
+}  // namespace comm
+}  // namespace ur_rtde_driver

include/ur_rtde_driver/comm/stream.h

@@ -0,0 +1,70 @@
+/*
+ * 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.
+ */
+
+#pragma once
+#include <netdb.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <atomic>
+#include <mutex>
+#include <string>
+#include "ur_rtde_driver/log.h"
+#include "ur_rtde_driver/comm/tcp_socket.h"
+
+namespace ur_rtde_driver
+{
+namespace comm
+{
+class URStream : public TCPSocket
+{
+private:
+  std::string host_;
+  int port_;
+  std::mutex write_mutex_, read_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(std::string& host, int port) : host_(host), port_(port)
+  {
+  }
+
+  bool connect()
+  {
+    return TCPSocket::setup(host_, port_);
+  }
+  void disconnect()
+  {
+    LOG_INFO("Disconnecting from %s:%d", host_.c_str(), port_);
+    TCPSocket::close();
+  }
+
+  bool closed()
+  {
+    return getState() == SocketState::Closed;
+  }
+
+  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);
+};
+}  // namespace comm
+}  // namespace ur_rtde_driver

include/ur_rtde_driver/comm/tcp_socket.h

@@ -0,0 +1,78 @@
+/*
+ * 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.
+ */
+
+#pragma once
+#include <netdb.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <atomic>
+#include <mutex>
+#include <string>
+
+namespace ur_rtde_driver
+{
+namespace comm
+{
+enum class SocketState
+{
+  Invalid,
+  Connected,
+  Disconnected,
+  Closed
+};
+
+class TCPSocket
+{
+private:
+  std::atomic<int> socket_fd_;
+  std::atomic<SocketState> state_;
+
+protected:
+  virtual bool open(int socket_fd, struct sockaddr* address, size_t address_len)
+  {
+    return false;
+  }
+  virtual void setOptions(int socket_fd);
+
+  bool setup(std::string& host, int port);
+
+public:
+  TCPSocket();
+  virtual ~TCPSocket();
+
+  SocketState getState()
+  {
+    return state_;
+  }
+
+  int getSocketFD()
+  {
+    return socket_fd_;
+  }
+  bool setSocketFD(int socket_fd);
+
+  std::string getIP();
+
+  bool read(char* character);
+  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);
+
+  void close();
+};
+}  // namespace comm
+}  // namespace ur_rtde_driver