renamed package

include/ur_rtde_driver/bin_parser.h

@@ -0,0 +1,203 @@
+/*
+ * 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 <assert.h>
+#include <endian.h>
+#include <inttypes.h>
+#include <array>
+#include <bitset>
+#include <cstddef>
+#include <cstring>
+#include <string>
+#include "ur_rtde_driver/log.h"
+#include "ur_rtde_driver/types.h"
+
+class BinParser
+{
+private:
+  uint8_t *buf_pos_, *buf_end_;
+  BinParser& parent_;
+
+public:
+  BinParser(uint8_t* buffer, size_t buf_len) : buf_pos_(buffer), buf_end_(buffer + buf_len), parent_(*this)
+  {
+    assert(buf_pos_ <= buf_end_);
+  }
+
+  BinParser(BinParser& parent, size_t sub_len)
+    : buf_pos_(parent.buf_pos_), buf_end_(parent.buf_pos_ + sub_len), parent_(parent)
+  {
+    assert(buf_pos_ <= buf_end_);
+  }
+
+  ~BinParser()
+  {
+    parent_.buf_pos_ = buf_pos_;
+  }
+
+  // Decode from network encoding (big endian) to host encoding
+  template <typename T>
+  T decode(T val)
+  {
+    return val;
+  }
+  uint16_t decode(uint16_t val)
+  {
+    return be16toh(val);
+  }
+  uint32_t decode(uint32_t val)
+  {
+    return be32toh(val);
+  }
+  uint64_t decode(uint64_t val)
+  {
+    return be64toh(val);
+  }
+  int16_t decode(int16_t val)
+  {
+    return be16toh(val);
+  }
+  int32_t decode(int32_t val)
+  {
+    return be32toh(val);
+  }
+  int64_t decode(int64_t val)
+  {
+    return be64toh(val);
+  }
+
+  template <typename T>
+  T peek()
+  {
+    assert(buf_pos_ + sizeof(T) <= buf_end_);
+    T val;
+    std::memcpy(&val, buf_pos_, sizeof(T));
+    return decode(val);
+  }
+
+  template <typename T>
+  void parse(T& val)
+  {
+    val = peek<T>();
+    buf_pos_ += sizeof(T);
+  }
+
+  void parse(double& val)
+  {
+    uint64_t inner;
+    parse<uint64_t>(inner);
+    std::memcpy(&val, &inner, sizeof(double));
+  }
+  void parse(float& val)
+  {
+    uint32_t inner;
+    parse<uint32_t>(inner);
+    std::memcpy(&val, &inner, sizeof(float));
+  }
+
+  // UR uses 1 byte for boolean values but sizeof(bool) is implementation
+  // defined so we must ensure they're parsed as uint8_t on all compilers
+  void parse(bool& val)
+  {
+    uint8_t inner;
+    parse<uint8_t>(inner);
+    val = inner != 0;
+  }
+
+  // Explicit parsing order of fields to avoid issues with struct layout
+  void parse(double3_t& val)
+  {
+    parse(val.x);
+    parse(val.y);
+    parse(val.z);
+  }
+
+  // Explicit parsing order of fields to avoid issues with struct layout
+  void parse(cartesian_coord_t& val)
+  {
+    parse(val.position);
+    parse(val.rotation);
+  }
+
+  void parse_remainder(std::string& val)
+  {
+    parse(val, size_t(buf_end_ - buf_pos_));
+  }
+
+  void parse(std::string& val, size_t len)
+  {
+    val.assign(reinterpret_cast<char*>(buf_pos_), len);
+    buf_pos_ += len;
+  }
+
+  // Special string parse function that assumes uint8_t len followed by chars
+  void parse(std::string& val)
+  {
+    uint8_t len;
+    parse(len);
+    parse(val, size_t(len));
+  }
+
+  template <typename T, size_t N>
+  void parse(std::array<T, N>& array)
+  {
+    for (size_t i = 0; i < N; i++)
+    {
+      parse(array[i]);
+    }
+  }
+
+  template <typename T, size_t N>
+  void parse(std::bitset<N>& set)
+  {
+    T val;
+    parse(val);
+    set = std::bitset<N>(val);
+  }
+
+  void consume()
+  {
+    buf_pos_ = buf_end_;
+  }
+  void consume(size_t bytes)
+  {
+    buf_pos_ += bytes;
+  }
+
+  bool checkSize(size_t bytes)
+  {
+    return bytes <= size_t(buf_end_ - buf_pos_);
+  }
+  template <typename T>
+  bool checkSize(void)
+  {
+    return checkSize(T::SIZE);
+  }
+
+  bool empty()
+  {
+    return buf_pos_ == buf_end_;
+  }
+
+  void debug()
+  {
+    LOG_DEBUG("BinParser: %p - %p (%zu bytes)", buf_pos_, buf_end_, buf_end_ - buf_pos_);
+  }
+};

include/ur_rtde_driver/event_counter.h

@@ -0,0 +1,102 @@
+/*
+ * 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 <chrono>
+#include <cstdlib>
+#include "ur_rtde_driver/log.h"
+#include "ur_rtde_driver/ur/consumer.h"
+
+class EventCounter : public URRTPacketConsumer
+{
+private:
+  typedef std::chrono::high_resolution_clock Clock;
+  Clock::time_point events_[250];
+  size_t idx_ = 0;
+
+  Clock::time_point last_;
+
+public:
+  void trigger()
+  {
+    // auto now = Clock::now();
+    // LOG_INFO("Time diff: %d ms", std::chrono::duration_cast<std::chrono::microseconds>(now - last_));
+    // last_ = now;
+    // return;
+
+    events_[idx_] = Clock::now();
+    idx_ += 1;
+
+    if (idx_ > 250)
+    {
+      std::chrono::time_point<std::chrono::high_resolution_clock> t_min =
+          std::chrono::time_point<std::chrono::high_resolution_clock>::max();
+      std::chrono::time_point<std::chrono::high_resolution_clock> t_max =
+          std::chrono::time_point<std::chrono::high_resolution_clock>::min();
+
+      for (auto const& e : events_)
+      {
+        if (e < t_min)
+          t_min = e;
+        if (e > t_max)
+          t_max = e;
+      }
+
+      auto diff = t_max - t_min;
+      auto secs = std::chrono::duration_cast<std::chrono::seconds>(diff).count();
+      auto ms = std::chrono::duration_cast<std::chrono::microseconds>(diff).count();
+      std::chrono::duration<double> test(t_max - t_min);
+      LOG_INFO("Recieved 250 messages at %f Hz", (250.0 / test.count()));
+      idx_ = 0;
+    }
+  }
+
+public:
+  bool consume(RTState_V1_6__7& state)
+  {
+    trigger();
+    return true;
+  }
+  bool consume(RTState_V1_8& state)
+  {
+    trigger();
+    return true;
+  }
+  bool consume(RTState_V3_0__1& state)
+  {
+    trigger();
+    return true;
+  }
+  bool consume(RTState_V3_2__3& state)
+  {
+    trigger();
+    return true;
+  }
+
+  void setupConsumer()
+  {
+    last_ = Clock::now();
+  }
+  void teardownConsumer()
+  {
+  }
+  void stopConsumer()
+  {
+  }
+};

include/ur_rtde_driver/log.h

@@ -0,0 +1,39 @@
+/*
+ * 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 <inttypes.h>
+
+#ifdef ROS_BUILD
+#include <ros/ros.h>
+
+#define LOG_DEBUG ROS_DEBUG
+#define LOG_WARN ROS_WARN
+#define LOG_INFO ROS_INFO
+#define LOG_ERROR ROS_ERROR
+#define LOG_FATAL ROS_FATAL
+
+#else
+
+#define LOG_DEBUG(format, ...) printf("[DEBUG]: " format "\n", ##__VA_ARGS__)
+#define LOG_WARN(format, ...) printf("[WARNING]: " format "\n", ##__VA_ARGS__)
+#define LOG_INFO(format, ...) printf("[INFO]: " format "\n", ##__VA_ARGS__)
+#define LOG_ERROR(format, ...) printf("[ERROR]: " format "\n", ##__VA_ARGS__)
+#define LOG_FATAL(format, ...) printf("[FATAL]: " format "\n", ##__VA_ARGS__)
+
+#endif

include/ur_rtde_driver/pipeline.h

@@ -0,0 +1,232 @@
+/*
+ * 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"
+
+using namespace moodycamel;
+using namespace std;
+
+template <typename T>
+class IConsumer
+{
+public:
+  virtual void setupConsumer()
+  {
+  }
+  virtual void teardownConsumer()
+  {
+  }
+  virtual void stopConsumer()
+  {
+  }
+  virtual void onTimeout()
+  {
+  }
+
+  virtual bool consume(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(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<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<unique_ptr<T>> queue_;
+  atomic<bool> running_;
+  thread pThread_, cThread_;
+
+  void run_producer()
+  {
+    producer_.setupProducer();
+    std::vector<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();
+    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_ = thread(&Pipeline::run_producer, this);
+    cThread_ = 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_);
+  }
+};

include/ur_rtde_driver/queue/LICENSE.md

@@ -0,0 +1,28 @@
+This license applies to all the code in this repository except that written by third
+parties, namely the files in benchmarks/ext, which have their own licenses, and Jeff
+Preshing's semaphore implementation (used in the blocking queue) which has a zlib
+license (embedded in atomicops.h).
+
+Simplified BSD License:
+
+Copyright (c) 2013-2015, Cameron Desrochers  
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+- Redistributions of source code must retain the above copyright notice, this list of
+conditions and the following disclaimer.
+- Redistributions in binary form must reproduce the above copyright notice, this list of
+conditions and the following disclaimer in the documentation and/or other materials
+provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
+EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
+THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

include/ur_rtde_driver/queue/atomicops.h

@@ -0,0 +1,738 @@
+// ©2013-2016 Cameron Desrochers.
+// Distributed under the simplified BSD license (see the license file that
+// should have come with this header).
+// Uses Jeff Preshing's semaphore implementation (under the terms of its
+// separate zlib license, embedded below).
+
+#pragma once
+
+// Provides portable (VC++2010+, Intel ICC 13, GCC 4.7+, and anything C++11 compliant) implementation
+// of low-level memory barriers, plus a few semi-portable utility macros (for inlining and alignment).
+// Also has a basic atomic type (limited to hardware-supported atomics with no memory ordering guarantees).
+// Uses the AE_* prefix for macros (historical reasons), and the "moodycamel" namespace for symbols.
+
+#include <cassert>
+#include <cerrno>
+#include <cstdint>
+#include <ctime>
+#include <type_traits>
+
+// Platform detection
+#if defined(__INTEL_COMPILER)
+#define AE_ICC
+#elif defined(_MSC_VER)
+#define AE_VCPP
+#elif defined(__GNUC__)
+#define AE_GCC
+#endif
+
+#if defined(_M_IA64) || defined(__ia64__)
+#define AE_ARCH_IA64
+#elif defined(_WIN64) || defined(__amd64__) || defined(_M_X64) || defined(__x86_64__)
+#define AE_ARCH_X64
+#elif defined(_M_IX86) || defined(__i386__)
+#define AE_ARCH_X86
+#elif defined(_M_PPC) || defined(__powerpc__)
+#define AE_ARCH_PPC
+#else
+#define AE_ARCH_UNKNOWN
+#endif
+
+// AE_UNUSED
+#define AE_UNUSED(x) ((void)x)
+
+// AE_FORCEINLINE
+#if defined(AE_VCPP) || defined(AE_ICC)
+#define AE_FORCEINLINE __forceinline
+#elif defined(AE_GCC)
+//#define AE_FORCEINLINE __attribute__((always_inline))
+#define AE_FORCEINLINE inline
+#else
+#define AE_FORCEINLINE inline
+#endif
+
+// AE_ALIGN
+#if defined(AE_VCPP) || defined(AE_ICC)
+#define AE_ALIGN(x) __declspec(align(x))
+#elif defined(AE_GCC)
+#define AE_ALIGN(x) __attribute__((aligned(x)))
+#else
+// Assume GCC compliant syntax...
+#define AE_ALIGN(x) __attribute__((aligned(x)))
+#endif
+
+// Portable atomic fences implemented below:
+
+namespace moodycamel
+{
+enum memory_order
+{
+  memory_order_relaxed,
+  memory_order_acquire,
+  memory_order_release,
+  memory_order_acq_rel,
+  memory_order_seq_cst,
+
+  // memory_order_sync: Forces a full sync:
+  // #LoadLoad, #LoadStore, #StoreStore, and most significantly, #StoreLoad
+  memory_order_sync = memory_order_seq_cst
+};
+
+}  // end namespace moodycamel
+
+#if (defined(AE_VCPP) && (_MSC_VER < 1700 || defined(__cplusplus_cli))) || defined(AE_ICC)
+// VS2010 and ICC13 don't support std::atomic_*_fence, implement our own fences
+
+#include <intrin.h>
+
+#if defined(AE_ARCH_X64) || defined(AE_ARCH_X86)
+#define AeFullSync _mm_mfence
+#define AeLiteSync _mm_mfence
+#elif defined(AE_ARCH_IA64)
+#define AeFullSync __mf
+#define AeLiteSync __mf
+#elif defined(AE_ARCH_PPC)
+#include <ppcintrinsics.h>
+#define AeFullSync __sync
+#define AeLiteSync __lwsync
+#endif
+
+#ifdef AE_VCPP
+#pragma warning(push)
+#pragma warning(disable : 4365)  // Disable erroneous 'conversion from long to unsigned int, signed/unsigned mismatch'
+                                 // error when using `assert`
+#ifdef __cplusplus_cli
+#pragma managed(push, off)
+#endif
+#endif
+
+namespace moodycamel
+{
+AE_FORCEINLINE void compiler_fence(memory_order order)
+{
+  switch (order)
+  {
+    case memory_order_relaxed:
+      break;
+    case memory_order_acquire:
+      _ReadBarrier();
+      break;
+    case memory_order_release:
+      _WriteBarrier();
+      break;
+    case memory_order_acq_rel:
+      _ReadWriteBarrier();
+      break;
+    case memory_order_seq_cst:
+      _ReadWriteBarrier();
+      break;
+    default:
+      assert(false);
+  }
+}
+
+// x86/x64 have a strong memory model -- all loads and stores have
+// acquire and release semantics automatically (so only need compiler
+// barriers for those).
+#if defined(AE_ARCH_X86) || defined(AE_ARCH_X64)
+AE_FORCEINLINE void fence(memory_order order)
+{
+  switch (order)
+  {
+    case memory_order_relaxed:
+      break;
+    case memory_order_acquire:
+      _ReadBarrier();
+      break;
+    case memory_order_release:
+      _WriteBarrier();
+      break;
+    case memory_order_acq_rel:
+      _ReadWriteBarrier();
+      break;
+    case memory_order_seq_cst:
+      _ReadWriteBarrier();
+      AeFullSync();
+      _ReadWriteBarrier();
+      break;
+    default:
+      assert(false);
+  }
+}
+#else
+AE_FORCEINLINE void fence(memory_order order)
+{
+  // Non-specialized arch, use heavier memory barriers everywhere just in case :-(
+  switch (order)
+  {
+    case memory_order_relaxed:
+      break;
+    case memory_order_acquire:
+      _ReadBarrier();
+      AeLiteSync();
+      _ReadBarrier();
+      break;
+    case memory_order_release:
+      _WriteBarrier();
+      AeLiteSync();
+      _WriteBarrier();
+      break;
+    case memory_order_acq_rel:
+      _ReadWriteBarrier();
+      AeLiteSync();
+      _ReadWriteBarrier();
+      break;
+    case memory_order_seq_cst:
+      _ReadWriteBarrier();
+      AeFullSync();
+      _ReadWriteBarrier();
+      break;
+    default:
+      assert(false);
+  }
+}
+#endif
+}  // end namespace moodycamel
+#else
+// Use standard library of atomics
+#include <atomic>
+
+namespace moodycamel
+{
+AE_FORCEINLINE void compiler_fence(memory_order order)
+{
+  switch (order)
+  {
+    case memory_order_relaxed:
+      break;
+    case memory_order_acquire:
+      std::atomic_signal_fence(std::memory_order_acquire);
+      break;
+    case memory_order_release:
+      std::atomic_signal_fence(std::memory_order_release);
+      break;
+    case memory_order_acq_rel:
+      std::atomic_signal_fence(std::memory_order_acq_rel);
+      break;
+    case memory_order_seq_cst:
+      std::atomic_signal_fence(std::memory_order_seq_cst);
+      break;
+    default:
+      assert(false);
+  }
+}
+
+AE_FORCEINLINE void fence(memory_order order)
+{
+  switch (order)
+  {
+    case memory_order_relaxed:
+      break;
+    case memory_order_acquire:
+      std::atomic_thread_fence(std::memory_order_acquire);
+      break;
+    case memory_order_release:
+      std::atomic_thread_fence(std::memory_order_release);
+      break;
+    case memory_order_acq_rel:
+      std::atomic_thread_fence(std::memory_order_acq_rel);
+      break;
+    case memory_order_seq_cst:
+      std::atomic_thread_fence(std::memory_order_seq_cst);
+      break;
+    default:
+      assert(false);
+  }
+}
+
+}  // end namespace moodycamel
+
+#endif
+
+#if !defined(AE_VCPP) || (_MSC_VER >= 1700 && !defined(__cplusplus_cli))
+#define AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC
+#endif
+
+#ifdef AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC
+#include <atomic>
+#endif
+#include <utility>
+
+// WARNING: *NOT* A REPLACEMENT FOR std::atomic. READ CAREFULLY:
+// Provides basic support for atomic variables -- no memory ordering guarantees are provided.
+// The guarantee of atomicity is only made for types that already have atomic load and store guarantees
+// at the hardware level -- on most platforms this generally means aligned pointers and integers (only).
+namespace moodycamel
+{
+template <typename T>
+class weak_atomic
+{
+public:
+  weak_atomic()
+  {
+  }
+#ifdef AE_VCPP
+#pragma warning(disable : 4100)  // Get rid of (erroneous) 'unreferenced formal parameter' warning
+#endif
+  template <typename U>
+  weak_atomic(U&& x) : value(std::forward<U>(x))
+  {
+  }
+#ifdef __cplusplus_cli
+  // Work around bug with universal reference/nullptr combination that only appears when /clr is on
+  weak_atomic(nullptr_t) : value(nullptr)
+  {
+  }
+#endif
+  weak_atomic(weak_atomic const& other) : value(other.value)
+  {
+  }
+  weak_atomic(weak_atomic&& other) : value(std::move(other.value))
+  {
+  }
+#ifdef AE_VCPP
+#pragma warning(default : 4100)
+#endif
+
+  AE_FORCEINLINE operator T() const
+  {
+    return load();
+  }
+
+#ifndef AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC
+  template <typename U>
+  AE_FORCEINLINE weak_atomic const& operator=(U&& x)
+  {
+    value = std::forward<U>(x);
+    return *this;
+  }
+  AE_FORCEINLINE weak_atomic const& operator=(weak_atomic const& other)
+  {
+    value = other.value;
+    return *this;
+  }
+
+  AE_FORCEINLINE T load() const
+  {
+    return value;
+  }
+
+  AE_FORCEINLINE T fetch_add_acquire(T increment)
+  {
+#if defined(AE_ARCH_X64) || defined(AE_ARCH_X86)
+    if (sizeof(T) == 4)
+      return _InterlockedExchangeAdd((long volatile*)&value, (long)increment);
+#if defined(_M_AMD64)
+    else if (sizeof(T) == 8)
+      return _InterlockedExchangeAdd64((long long volatile*)&value, (long long)increment);
+#endif
+#else
+#error Unsupported platform
+#endif
+    assert(false && "T must be either a 32 or 64 bit type");
+    return value;
+  }
+
+  AE_FORCEINLINE T fetch_add_release(T increment)
+  {
+#if defined(AE_ARCH_X64) || defined(AE_ARCH_X86)
+    if (sizeof(T) == 4)
+      return _InterlockedExchangeAdd((long volatile*)&value, (long)increment);
+#if defined(_M_AMD64)
+    else if (sizeof(T) == 8)
+      return _InterlockedExchangeAdd64((long long volatile*)&value, (long long)increment);
+#endif
+#else
+#error Unsupported platform
+#endif
+    assert(false && "T must be either a 32 or 64 bit type");
+    return value;
+  }
+#else
+  template <typename U>
+  AE_FORCEINLINE weak_atomic const& operator=(U&& x)
+  {
+    value.store(std::forward<U>(x), std::memory_order_relaxed);
+    return *this;
+  }
+
+  AE_FORCEINLINE weak_atomic const& operator=(weak_atomic const& other)
+  {
+    value.store(other.value.load(std::memory_order_relaxed), std::memory_order_relaxed);
+    return *this;
+  }
+
+  AE_FORCEINLINE T load() const
+  {
+    return value.load(std::memory_order_relaxed);
+  }
+
+  AE_FORCEINLINE T fetch_add_acquire(T increment)
+  {
+    return value.fetch_add(increment, std::memory_order_acquire);
+  }
+
+  AE_FORCEINLINE T fetch_add_release(T increment)
+  {
+    return value.fetch_add(increment, std::memory_order_release);
+  }
+#endif
+
+private:
+#ifndef AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC
+  // No std::atomic support, but still need to circumvent compiler optimizations.
+  // `volatile` will make memory access slow, but is guaranteed to be reliable.
+  volatile T value;
+#else
+  std::atomic<T> value;
+#endif
+};
+
+}  // end namespace moodycamel
+
+// Portable single-producer, single-consumer semaphore below:
+
+#if defined(_WIN32)
+// Avoid including windows.h in a header; we only need a handful of
+// items, so we'll redeclare them here (this is relatively safe since
+// the API generally has to remain stable between Windows versions).
+// I know this is an ugly hack but it still beats polluting the global
+// namespace with thousands of generic names or adding a .cpp for nothing.
+extern "C" {
+struct _SECURITY_ATTRIBUTES;
+__declspec(dllimport) void* __stdcall CreateSemaphoreW(_SECURITY_ATTRIBUTES* lpSemaphoreAttributes, long lInitialCount,
+                                                       long lMaximumCount, const wchar_t* lpName);
+__declspec(dllimport) int __stdcall CloseHandle(void* hObject);
+__declspec(dllimport) unsigned long __stdcall WaitForSingleObject(void* hHandle, unsigned long dwMilliseconds);
+__declspec(dllimport) int __stdcall ReleaseSemaphore(void* hSemaphore, long lReleaseCount, long* lpPreviousCount);
+}
+#elif defined(__MACH__)
+#include <mach/mach.h>
+#elif defined(__unix__)
+#include <semaphore.h>
+#endif
+
+namespace moodycamel
+{
+// Code in the spsc_sema namespace below is an adaptation of Jeff Preshing's
+// portable + lightweight semaphore implementations, originally from
+// https://github.com/preshing/cpp11-on-multicore/blob/master/common/sema.h
+// LICENSE:
+// Copyright (c) 2015 Jeff Preshing
+//
+// This software is provided 'as-is', without any express or implied
+// warranty. In no event will the authors be held liable for any damages
+// arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose,
+// including commercial applications, and to alter it and redistribute it
+// freely, subject to the following restrictions:
+//
+// 1. The origin of this software must not be misrepresented; you must not
+//    claim that you wrote the original software. If you use this software
+//    in a product, an acknowledgement in the product documentation would be
+//    appreciated but is not required.
+// 2. Altered source versions must be plainly marked as such, and must not be
+//    misrepresented as being the original software.
+// 3. This notice may not be removed or altered from any source distribution.
+namespace spsc_sema
+{
+#if defined(_WIN32)
+class Semaphore
+{
+private:
+  void* m_hSema;
+
+  Semaphore(const Semaphore& other);
+  Semaphore& operator=(const Semaphore& other);
+
+public:
+  Semaphore(int initialCount = 0)
+  {
+    assert(initialCount >= 0);
+    const long maxLong = 0x7fffffff;
+    m_hSema = CreateSemaphoreW(nullptr, initialCount, maxLong, nullptr);
+  }
+
+  ~Semaphore()
+  {
+    CloseHandle(m_hSema);
+  }
+
+  void wait()
+  {
+    const unsigned long infinite = 0xffffffff;
+    WaitForSingleObject(m_hSema, infinite);
+  }
+
+  bool try_wait()
+  {
+    const unsigned long RC_WAIT_TIMEOUT = 0x00000102;
+    return WaitForSingleObject(m_hSema, 0) != RC_WAIT_TIMEOUT;
+  }
+
+  bool timed_wait(std::uint64_t usecs)
+  {
+    const unsigned long RC_WAIT_TIMEOUT = 0x00000102;
+    return WaitForSingleObject(m_hSema, (unsigned long)(usecs / 1000)) != RC_WAIT_TIMEOUT;
+  }
+
+  void signal(int count = 1)
+  {
+    ReleaseSemaphore(m_hSema, count, nullptr);
+  }
+};
+#elif defined(__MACH__)
+//---------------------------------------------------------
+// Semaphore (Apple iOS and OSX)
+// Can't use POSIX semaphores due to http://lists.apple.com/archives/darwin-kernel/2009/Apr/msg00010.html
+//---------------------------------------------------------
+class Semaphore
+{
+private:
+  semaphore_t m_sema;
+
+  Semaphore(const Semaphore& other);
+  Semaphore& operator=(const Semaphore& other);
+
+public:
+  Semaphore(int initialCount = 0)
+  {
+    assert(initialCount >= 0);
+    semaphore_create(mach_task_self(), &m_sema, SYNC_POLICY_FIFO, initialCount);
+  }
+
+  ~Semaphore()
+  {
+    semaphore_destroy(mach_task_self(), m_sema);
+  }
+
+  void wait()
+  {
+    semaphore_wait(m_sema);
+  }
+
+  bool try_wait()
+  {
+    return timed_wait(0);
+  }
+
+  bool timed_wait(std::int64_t timeout_usecs)
+  {
+    mach_timespec_t ts;
+    ts.tv_sec = timeout_usecs / 1000000;
+    ts.tv_nsec = (timeout_usecs % 1000000) * 1000;
+
+    // added in OSX 10.10:
+    // https://developer.apple.com/library/prerelease/mac/documentation/General/Reference/APIDiffsMacOSX10_10SeedDiff/modules/Darwin.html
+    kern_return_t rc = semaphore_timedwait(m_sema, ts);
+
+    return rc != KERN_OPERATION_TIMED_OUT;
+  }
+
+  void signal()
+  {
+    semaphore_signal(m_sema);
+  }
+
+  void signal(int count)
+  {
+    while (count-- > 0)
+    {
+      semaphore_signal(m_sema);
+    }
+  }
+};
+#elif defined(__unix__)
+//---------------------------------------------------------
+// Semaphore (POSIX, Linux)
+//---------------------------------------------------------
+class Semaphore
+{
+private:
+  sem_t m_sema;
+
+  Semaphore(const Semaphore& other);
+  Semaphore& operator=(const Semaphore& other);
+
+public:
+  Semaphore(int initialCount = 0)
+  {
+    assert(initialCount >= 0);
+    sem_init(&m_sema, 0, initialCount);
+  }
+
+  ~Semaphore()
+  {
+    sem_destroy(&m_sema);
+  }
+
+  void wait()
+  {
+    // http://stackoverflow.com/questions/2013181/gdb-causes-sem-wait-to-fail-with-eintr-error
+    int rc;
+    do
+    {
+      rc = sem_wait(&m_sema);
+    } while (rc == -1 && errno == EINTR);
+  }
+
+  bool try_wait()
+  {
+    int rc;
+    do
+    {
+      rc = sem_trywait(&m_sema);
+    } while (rc == -1 && errno == EINTR);
+    return !(rc == -1 && errno == EAGAIN);
+  }
+
+  bool timed_wait(std::uint64_t usecs)
+  {
+    struct timespec ts;
+    const int usecs_in_1_sec = 1000000;
+    const int nsecs_in_1_sec = 1000000000;
+    clock_gettime(CLOCK_REALTIME, &ts);
+    ts.tv_sec += usecs / usecs_in_1_sec;
+    ts.tv_nsec += (usecs % usecs_in_1_sec) * 1000;
+    // sem_timedwait bombs if you have more than 1e9 in tv_nsec
+    // so we have to clean things up before passing it in
+    if (ts.tv_nsec > nsecs_in_1_sec)
+    {
+      ts.tv_nsec -= nsecs_in_1_sec;
+      ++ts.tv_sec;
+    }
+
+    int rc;
+    do
+    {
+      rc = sem_timedwait(&m_sema, &ts);
+    } while (rc == -1 && errno == EINTR);
+    return !(rc == -1 && errno == ETIMEDOUT);
+  }
+
+  void signal()
+  {
+    sem_post(&m_sema);
+  }
+
+  void signal(int count)
+  {
+    while (count-- > 0)
+    {
+      sem_post(&m_sema);
+    }
+  }
+};
+#else
+#error Unsupported platform! (No semaphore wrapper available)
+#endif
+
+//---------------------------------------------------------
+// LightweightSemaphore
+//---------------------------------------------------------
+class LightweightSemaphore
+{
+public:
+  typedef std::make_signed<std::size_t>::type ssize_t;
+
+private:
+  weak_atomic<ssize_t> m_count;
+  Semaphore m_sema;
+
+  bool waitWithPartialSpinning(std::int64_t timeout_usecs = -1)
+  {
+    ssize_t oldCount;
+    // Is there a better way to set the initial spin count?
+    // If we lower it to 1000, testBenaphore becomes 15x slower on my Core i7-5930K Windows PC,
+    // as threads start hitting the kernel semaphore.
+    int spin = 10000;
+    while (--spin >= 0)
+    {
+      if (m_count.load() > 0)
+      {
+        m_count.fetch_add_acquire(-1);
+        return true;
+      }
+      compiler_fence(memory_order_acquire);  // Prevent the compiler from collapsing the loop.
+    }
+    oldCount = m_count.fetch_add_acquire(-1);
+    if (oldCount > 0)
+      return true;
+    if (timeout_usecs < 0)
+    {
+      m_sema.wait();
+      return true;
+    }
+    if (m_sema.timed_wait(timeout_usecs))
+      return true;
+    // At this point, we've timed out waiting for the semaphore, but the
+    // count is still decremented indicating we may still be waiting on
+    // it. So we have to re-adjust the count, but only if the semaphore
+    // wasn't signaled enough times for us too since then. If it was, we
+    // need to release the semaphore too.
+    while (true)
+    {
+      oldCount = m_count.fetch_add_release(1);
+      if (oldCount < 0)
+        return false;  // successfully restored things to the way they were
+      // Oh, the producer thread just signaled the semaphore after all. Try again:
+      oldCount = m_count.fetch_add_acquire(-1);
+      if (oldCount > 0 && m_sema.try_wait())
+        return true;
+    }
+  }
+
+public:
+  LightweightSemaphore(ssize_t initialCount = 0) : m_count(initialCount)
+  {
+    assert(initialCount >= 0);
+  }
+
+  bool tryWait()
+  {
+    if (m_count.load() > 0)
+    {
+      m_count.fetch_add_acquire(-1);
+      return true;
+    }
+    return false;
+  }
+
+  void wait()
+  {
+    if (!tryWait())
+      waitWithPartialSpinning();
+  }
+
+  bool wait(std::int64_t timeout_usecs)
+  {
+    return tryWait() || waitWithPartialSpinning(timeout_usecs);
+  }
+
+  void signal(ssize_t count = 1)
+  {
+    assert(count >= 0);
+    ssize_t oldCount = m_count.fetch_add_release(count);
+    assert(oldCount >= -1);
+    if (oldCount < 0)
+    {
+      m_sema.signal(1);
+    }
+  }
+
+  ssize_t availableApprox() const
+  {
+    ssize_t count = m_count.load();
+    return count > 0 ? count : 0;
+  }
+};
+}  // end namespace spsc_sema
+}  // end namespace moodycamel
+
+#if defined(AE_VCPP) && (_MSC_VER < 1700 || defined(__cplusplus_cli))
+#pragma warning(pop)
+#ifdef __cplusplus_cli
+#pragma managed(pop)
+#endif
+#endif

include/ur_rtde_driver/queue/readerwriterqueue.h

@@ -0,0 +1,864 @@
+// ©2013-2016 Cameron Desrochers.
+// Distributed under the simplified BSD license (see the license file that
+// should have come with this header).
+
+#pragma once
+
+#include <cassert>
+#include <cstdint>
+#include <cstdlib>  // For malloc/free/abort & size_t
+#include <new>
+#include <stdexcept>
+#include <type_traits>
+#include <utility>
+#include "atomicops.h"
+#if __cplusplus > 199711L || _MSC_VER >= 1700  // C++11 or VS2012
+#include <chrono>
+#endif
+
+// A lock-free queue for a single-consumer, single-producer architecture.
+// The queue is also wait-free in the common path (except if more memory
+// needs to be allocated, in which case malloc is called).
+// Allocates memory sparingly (O(lg(n) times, amortized), and only once if
+// the original maximum size estimate is never exceeded.
+// Tested on x86/x64 processors, but semantics should be correct for all
+// architectures (given the right implementations in atomicops.h), provided
+// that aligned integer and pointer accesses are naturally atomic.
+// Note that there should only be one consumer thread and producer thread;
+// Switching roles of the threads, or using multiple consecutive threads for
+// one role, is not safe unless properly synchronized.
+// Using the queue exclusively from one thread is fine, though a bit silly.
+
+#ifndef MOODYCAMEL_CACHE_LINE_SIZE
+#define MOODYCAMEL_CACHE_LINE_SIZE 64
+#endif
+
+#ifndef MOODYCAMEL_EXCEPTIONS_ENABLED
+#if (defined(_MSC_VER) && defined(_CPPUNWIND)) || (defined(__GNUC__) && defined(__EXCEPTIONS)) ||                      \
+    (!defined(_MSC_VER) && !defined(__GNUC__))
+#define MOODYCAMEL_EXCEPTIONS_ENABLED
+#endif
+#endif
+
+#ifdef AE_VCPP
+#pragma warning(push)
+#pragma warning(disable : 4324)  // structure was padded due to __declspec(align())
+#pragma warning(disable : 4820)  // padding was added
+#pragma warning(disable : 4127)  // conditional expression is constant
+#endif
+
+namespace moodycamel
+{
+template <typename T, size_t MAX_BLOCK_SIZE = 512>
+class ReaderWriterQueue
+{
+  // Design: Based on a queue-of-queues. The low-level queues are just
+  // circular buffers with front and tail indices indicating where the
+  // next element to dequeue is and where the next element can be enqueued,
+  // respectively. Each low-level queue is called a "block". Each block
+  // wastes exactly one element's worth of space to keep the design simple
+  // (if front == tail then the queue is empty, and can't be full).
+  // The high-level queue is a circular linked list of blocks; again there
+  // is a front and tail, but this time they are pointers to the blocks.
+  // The front block is where the next element to be dequeued is, provided
+  // the block is not empty. The back block is where elements are to be
+  // enqueued, provided the block is not full.
+  // The producer thread owns all the tail indices/pointers. The consumer
+  // thread owns all the front indices/pointers. Both threads read each
+  // other's variables, but only the owning thread updates them. E.g. After
+  // the consumer reads the producer's tail, the tail may change before the
+  // consumer is done dequeuing an object, but the consumer knows the tail
+  // will never go backwards, only forwards.
+  // If there is no room to enqueue an object, an additional block (of
+  // equal size to the last block) is added. Blocks are never removed.
+
+public:
+  // Constructs a queue that can hold maxSize elements without further
+  // allocations. If more than MAX_BLOCK_SIZE elements are requested,
+  // then several blocks of MAX_BLOCK_SIZE each are reserved (including
+  // at least one extra buffer block).
+  explicit ReaderWriterQueue(size_t maxSize = 15)
+#ifndef NDEBUG
+    : enqueuing(false), dequeuing(false)
+#endif
+  {
+    assert(maxSize > 0);
+    assert(MAX_BLOCK_SIZE == ceilToPow2(MAX_BLOCK_SIZE) && "MAX_BLOCK_SIZE must be a power of 2");
+    assert(MAX_BLOCK_SIZE >= 2 && "MAX_BLOCK_SIZE must be at least 2");
+
+    Block* firstBlock = nullptr;
+
+    largestBlockSize = ceilToPow2(maxSize + 1);  // We need a spare slot to fit maxSize elements in the block
+    if (largestBlockSize > MAX_BLOCK_SIZE * 2)
+    {
+      // We need a spare block in case the producer is writing to a different block the consumer is reading from, and
+      // wants to enqueue the maximum number of elements. We also need a spare element in each block to avoid the
+      // ambiguity
+      // between front == tail meaning "empty" and "full".
+      // So the effective number of slots that are guaranteed to be usable at any time is the block size - 1 times the
+      // number of blocks - 1. Solving for maxSize and applying a ceiling to the division gives us (after simplifying):
+      size_t initialBlockCount = (maxSize + MAX_BLOCK_SIZE * 2 - 3) / (MAX_BLOCK_SIZE - 1);
+      largestBlockSize = MAX_BLOCK_SIZE;
+      Block* lastBlock = nullptr;
+      for (size_t i = 0; i != initialBlockCount; ++i)
+      {
+        auto block = make_block(largestBlockSize);
+        if (block == nullptr)
+        {
+#ifdef MOODYCAMEL_EXCEPTIONS_ENABLED
+          throw std::bad_alloc();
+#else
+          abort();
+#endif
+        }
+        if (firstBlock == nullptr)
+        {
+          firstBlock = block;
+        }
+        else
+        {
+          lastBlock->next = block;
+        }
+        lastBlock = block;
+        block->next = firstBlock;
+      }
+    }
+    else
+    {
+      firstBlock = make_block(largestBlockSize);
+      if (firstBlock == nullptr)
+      {
+#ifdef MOODYCAMEL_EXCEPTIONS_ENABLED
+        throw std::bad_alloc();
+#else
+        abort();
+#endif
+      }
+      firstBlock->next = firstBlock;
+    }
+    frontBlock = firstBlock;
+    tailBlock = firstBlock;
+
+    // Make sure the reader/writer threads will have the initialized memory setup above:
+    fence(memory_order_sync);
+  }
+
+  // Note: The queue should not be accessed concurrently while it's
+  // being deleted. It's up to the user to synchronize this.
+  ~ReaderWriterQueue()
+  {
+    // Make sure we get the latest version of all variables from other CPUs:
+    fence(memory_order_sync);
+
+    // Destroy any remaining objects in queue and free memory
+    Block* frontBlock_ = frontBlock;
+    Block* block = frontBlock_;
+    do
+    {
+      Block* nextBlock = block->next;
+      size_t blockFront = block->front;
+      size_t blockTail = block->tail;
+
+      for (size_t i = blockFront; i != blockTail; i = (i + 1) & block->sizeMask)
+      {
+        auto element = reinterpret_cast<T*>(block->data + i * sizeof(T));
+        element->~T();
+        (void)element;
+      }
+
+      auto rawBlock = block->rawThis;
+      block->~Block();
+      std::free(rawBlock);
+      block = nextBlock;
+    } while (block != frontBlock_);
+  }
+
+  // Enqueues a copy of element if there is room in the queue.
+  // Returns true if the element was enqueued, false otherwise.
+  // Does not allocate memory.
+  AE_FORCEINLINE bool try_enqueue(T const& element)
+  {
+    return inner_enqueue<CannotAlloc>(element);
+  }
+
+  // Enqueues a moved copy of element if there is room in the queue.
+  // Returns true if the element was enqueued, false otherwise.
+  // Does not allocate memory.
+  AE_FORCEINLINE bool try_enqueue(T&& element)
+  {
+    return inner_enqueue<CannotAlloc>(std::forward<T>(element));
+  }
+
+  // Enqueues a copy of element on the queue.
+  // Allocates an additional block of memory if needed.
+  // Only fails (returns false) if memory allocation fails.
+  AE_FORCEINLINE bool enqueue(T const& element)
+  {
+    return inner_enqueue<CanAlloc>(element);
+  }
+
+  // Enqueues a moved copy of element on the queue.
+  // Allocates an additional block of memory if needed.
+  // Only fails (returns false) if memory allocation fails.
+  AE_FORCEINLINE bool enqueue(T&& element)
+  {
+    return inner_enqueue<CanAlloc>(std::forward<T>(element));
+  }
+
+  // Attempts to dequeue an element; if the queue is empty,
+  // returns false instead. If the queue has at least one element,
+  // moves front to result using operator=, then returns true.
+  template <typename U>
+  bool try_dequeue(U& result)
+  {
+#ifndef NDEBUG
+    ReentrantGuard guard(this->dequeuing);
+#endif
+
+    // High-level pseudocode:
+    // Remember where the tail block is
+    // If the front block has an element in it, dequeue it
+    // Else
+    //     If front block was the tail block when we entered the function, return false
+    //     Else advance to next block and dequeue the item there
+
+    // Note that we have to use the value of the tail block from before we check if the front
+    // block is full or not, in case the front block is empty and then, before we check if the
+    // tail block is at the front block or not, the producer fills up the front block *and
+    // moves on*, which would make us skip a filled block. Seems unlikely, but was consistently
+    // reproducible in practice.
+    // In order to avoid overhead in the common case, though, we do a double-checked pattern
+    // where we have the fast path if the front block is not empty, then read the tail block,
+    // then re-read the front block and check if it's not empty again, then check if the tail
+    // block has advanced.
+
+    Block* frontBlock_ = frontBlock.load();
+    size_t blockTail = frontBlock_->localTail;
+    size_t blockFront = frontBlock_->front.load();
+
+    if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load()))
+    {
+      fence(memory_order_acquire);
+
+    non_empty_front_block:
+      // Front block not empty, dequeue from here
+      auto element = reinterpret_cast<T*>(frontBlock_->data + blockFront * sizeof(T));
+      result = std::move(*element);
+      element->~T();
+
+      blockFront = (blockFront + 1) & frontBlock_->sizeMask;
+
+      fence(memory_order_release);
+      frontBlock_->front = blockFront;
+    }
+    else if (frontBlock_ != tailBlock.load())
+    {
+      fence(memory_order_acquire);
+
+      frontBlock_ = frontBlock.load();
+      blockTail = frontBlock_->localTail = frontBlock_->tail.load();
+      blockFront = frontBlock_->front.load();
+      fence(memory_order_acquire);
+
+      if (blockFront != blockTail)
+      {
+        // Oh look, the front block isn't empty after all
+        goto non_empty_front_block;
+      }
+
+      // Front block is empty but there's another block ahead, advance to it
+      Block* nextBlock = frontBlock_->next;
+      // Don't need an acquire fence here since next can only ever be set on the tailBlock,
+      // and we're not the tailBlock, and we did an acquire earlier after reading tailBlock which
+      // ensures next is up-to-date on this CPU in case we recently were at tailBlock.
+
+      size_t nextBlockFront = nextBlock->front.load();
+      size_t nextBlockTail = nextBlock->localTail = nextBlock->tail.load();
+      fence(memory_order_acquire);
+
+      // Since the tailBlock is only ever advanced after being written to,
+      // we know there's for sure an element to dequeue on it
+      assert(nextBlockFront != nextBlockTail);
+      AE_UNUSED(nextBlockTail);
+
+      // We're done with this block, let the producer use it if it needs
+      fence(memory_order_release);  // Expose possibly pending changes to frontBlock->front from last dequeue
+      frontBlock = frontBlock_ = nextBlock;
+
+      compiler_fence(memory_order_release);  // Not strictly needed
+
+      auto element = reinterpret_cast<T*>(frontBlock_->data + nextBlockFront * sizeof(T));
+
+      result = std::move(*element);
+      element->~T();
+
+      nextBlockFront = (nextBlockFront + 1) & frontBlock_->sizeMask;
+
+      fence(memory_order_release);
+      frontBlock_->front = nextBlockFront;
+    }
+    else
+    {
+      // No elements in current block and no other block to advance to
+      return false;
+    }
+
+    return true;
+  }
+
+  // Returns a pointer to the front element in the queue (the one that
+  // would be removed next by a call to `try_dequeue` or `pop`). If the
+  // queue appears empty at the time the method is called, nullptr is
+  // returned instead.
+  // Must be called only from the consumer thread.
+  T* peek()
+  {
+#ifndef NDEBUG
+    ReentrantGuard guard(this->dequeuing);
+#endif
+    // See try_dequeue() for reasoning
+
+    Block* frontBlock_ = frontBlock.load();
+    size_t blockTail = frontBlock_->localTail;
+    size_t blockFront = frontBlock_->front.load();
+
+    if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load()))
+    {
+      fence(memory_order_acquire);
+    non_empty_front_block:
+      return reinterpret_cast<T*>(frontBlock_->data + blockFront * sizeof(T));
+    }
+    else if (frontBlock_ != tailBlock.load())
+    {
+      fence(memory_order_acquire);
+      frontBlock_ = frontBlock.load();
+      blockTail = frontBlock_->localTail = frontBlock_->tail.load();
+      blockFront = frontBlock_->front.load();
+      fence(memory_order_acquire);
+
+      if (blockFront != blockTail)
+      {
+        goto non_empty_front_block;
+      }
+
+      Block* nextBlock = frontBlock_->next;
+
+      size_t nextBlockFront = nextBlock->front.load();
+      fence(memory_order_acquire);
+
+      assert(nextBlockFront != nextBlock->tail.load());
+      return reinterpret_cast<T*>(nextBlock->data + nextBlockFront * sizeof(T));
+    }
+
+    return nullptr;
+  }
+
+  // Removes the front element from the queue, if any, without returning it.
+  // Returns true on success, or false if the queue appeared empty at the time
+  // `pop` was called.
+  bool pop()
+  {
+#ifndef NDEBUG
+    ReentrantGuard guard(this->dequeuing);
+#endif
+    // See try_dequeue() for reasoning
+
+    Block* frontBlock_ = frontBlock.load();
+    size_t blockTail = frontBlock_->localTail;
+    size_t blockFront = frontBlock_->front.load();
+
+    if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load()))
+    {
+      fence(memory_order_acquire);
+
+    non_empty_front_block:
+      auto element = reinterpret_cast<T*>(frontBlock_->data + blockFront * sizeof(T));
+      element->~T();
+
+      blockFront = (blockFront + 1) & frontBlock_->sizeMask;
+
+      fence(memory_order_release);
+      frontBlock_->front = blockFront;
+    }
+    else if (frontBlock_ != tailBlock.load())
+    {
+      fence(memory_order_acquire);
+      frontBlock_ = frontBlock.load();
+      blockTail = frontBlock_->localTail = frontBlock_->tail.load();
+      blockFront = frontBlock_->front.load();
+      fence(memory_order_acquire);
+
+      if (blockFront != blockTail)
+      {
+        goto non_empty_front_block;
+      }
+
+      // Front block is empty but there's another block ahead, advance to it
+      Block* nextBlock = frontBlock_->next;
+
+      size_t nextBlockFront = nextBlock->front.load();
+      size_t nextBlockTail = nextBlock->localTail = nextBlock->tail.load();
+      fence(memory_order_acquire);
+
+      assert(nextBlockFront != nextBlockTail);
+      AE_UNUSED(nextBlockTail);
+
+      fence(memory_order_release);
+      frontBlock = frontBlock_ = nextBlock;
+
+      compiler_fence(memory_order_release);
+
+      auto element = reinterpret_cast<T*>(frontBlock_->data + nextBlockFront * sizeof(T));
+      element->~T();
+
+      nextBlockFront = (nextBlockFront + 1) & frontBlock_->sizeMask;
+
+      fence(memory_order_release);
+      frontBlock_->front = nextBlockFront;
+    }
+    else
+    {
+      // No elements in current block and no other block to advance to
+      return false;
+    }
+
+    return true;
+  }
+
+  // Returns the approximate number of items currently in the queue.
+  // Safe to call from both the producer and consumer threads.
+  inline size_t size_approx() const
+  {
+    size_t result = 0;
+    Block* frontBlock_ = frontBlock.load();
+    Block* block = frontBlock_;
+    do
+    {
+      fence(memory_order_acquire);
+      size_t blockFront = block->front.load();
+      size_t blockTail = block->tail.load();
+      result += (blockTail - blockFront) & block->sizeMask;
+      block = block->next.load();
+    } while (block != frontBlock_);
+    return result;
+  }
+
+private:
+  enum AllocationMode
+  {
+    CanAlloc,
+    CannotAlloc
+  };
+
+  template <AllocationMode canAlloc, typename U>
+  bool inner_enqueue(U&& element)
+  {
+#ifndef NDEBUG
+    ReentrantGuard guard(this->enqueuing);
+#endif
+
+    // High-level pseudocode (assuming we're allowed to alloc a new block):
+    // If room in tail block, add to tail
+    // Else check next block
+    //     If next block is not the head block, enqueue on next block
+    //     Else create a new block and enqueue there
+    //     Advance tail to the block we just enqueued to
+
+    Block* tailBlock_ = tailBlock.load();
+    size_t blockFront = tailBlock_->localFront;
+    size_t blockTail = tailBlock_->tail.load();
+
+    size_t nextBlockTail = (blockTail + 1) & tailBlock_->sizeMask;
+    if (nextBlockTail != blockFront || nextBlockTail != (tailBlock_->localFront = tailBlock_->front.load()))
+    {
+      fence(memory_order_acquire);
+      // This block has room for at least one more element
+      char* location = tailBlock_->data + blockTail * sizeof(T);
+      new (location) T(std::forward<U>(element));
+
+      fence(memory_order_release);
+      tailBlock_->tail = nextBlockTail;
+    }
+    else
+    {
+      fence(memory_order_acquire);
+      if (tailBlock_->next.load() != frontBlock)
+      {
+        // Note that the reason we can't advance to the frontBlock and start adding new entries there
+        // is because if we did, then dequeue would stay in that block, eventually reading the new values,
+        // instead of advancing to the next full block (whose values were enqueued first and so should be
+        // consumed first).
+
+        fence(memory_order_acquire);  // Ensure we get latest writes if we got the latest frontBlock
+
+        // tailBlock is full, but there's a free block ahead, use it
+        Block* tailBlockNext = tailBlock_->next.load();
+        size_t nextBlockFront = tailBlockNext->localFront = tailBlockNext->front.load();
+        nextBlockTail = tailBlockNext->tail.load();
+        fence(memory_order_acquire);
+
+        // This block must be empty since it's not the head block and we
+        // go through the blocks in a circle
+        assert(nextBlockFront == nextBlockTail);
+        tailBlockNext->localFront = nextBlockFront;
+
+        char* location = tailBlockNext->data + nextBlockTail * sizeof(T);
+        new (location) T(std::forward<U>(element));
+
+        tailBlockNext->tail = (nextBlockTail + 1) & tailBlockNext->sizeMask;
+
+        fence(memory_order_release);
+        tailBlock = tailBlockNext;
+      }
+      else if (canAlloc == CanAlloc)
+      {
+        // tailBlock is full and there's no free block ahead; create a new block
+        auto newBlockSize = largestBlockSize >= MAX_BLOCK_SIZE ? largestBlockSize : largestBlockSize * 2;
+        auto newBlock = make_block(newBlockSize);
+        if (newBlock == nullptr)
+        {
+          // Could not allocate a block!
+          return false;
+        }
+        largestBlockSize = newBlockSize;
+
+        new (newBlock->data) T(std::forward<U>(element));
+
+        assert(newBlock->front == 0);
+        newBlock->tail = newBlock->localTail = 1;
+
+        newBlock->next = tailBlock_->next.load();
+        tailBlock_->next = newBlock;
+
+        // Might be possible for the dequeue thread to see the new tailBlock->next
+        // *without* seeing the new tailBlock value, but this is OK since it can't
+        // advance to the next block until tailBlock is set anyway (because the only
+        // case where it could try to read the next is if it's already at the tailBlock,
+        // and it won't advance past tailBlock in any circumstance).
+
+        fence(memory_order_release);
+        tailBlock = newBlock;
+      }
+      else if (canAlloc == CannotAlloc)
+      {
+        // Would have had to allocate a new block to enqueue, but not allowed
+        return false;
+      }
+      else
+      {
+        assert(false && "Should be unreachable code");
+        return false;
+      }
+    }
+
+    return true;
+  }
+
+  // Disable copying
+  ReaderWriterQueue(ReaderWriterQueue const&)
+  {
+  }
+
+  // Disable assignment
+  ReaderWriterQueue& operator=(ReaderWriterQueue const&)
+  {
+  }
+
+  AE_FORCEINLINE static size_t ceilToPow2(size_t x)
+  {
+    // From http://graphics.stanford.edu/~seander/bithacks.html#RoundUpPowerOf2
+    --x;
+    x |= x >> 1;
+    x |= x >> 2;
+    x |= x >> 4;
+    for (size_t i = 1; i < sizeof(size_t); i <<= 1)
+    {
+      x |= x >> (i << 3);
+    }
+    ++x;
+    return x;
+  }
+
+  template <typename U>
+  static AE_FORCEINLINE char* align_for(char* ptr)
+  {
+    const std::size_t alignment = std::alignment_of<U>::value;
+    return ptr + (alignment - (reinterpret_cast<std::uintptr_t>(ptr) % alignment)) % alignment;
+  }
+
+private:
+#ifndef NDEBUG
+  struct ReentrantGuard
+  {
+    ReentrantGuard(bool& _inSection) : inSection(_inSection)
+    {
+      assert(!inSection && "ReaderWriterQueue does not support enqueuing or dequeuing elements from other elements' "
+                           "ctors and dtors");
+      inSection = true;
+    }
+
+    ~ReentrantGuard()
+    {
+      inSection = false;
+    }
+
+  private:
+    ReentrantGuard& operator=(ReentrantGuard const&);
+
+  private:
+    bool& inSection;
+  };
+#endif
+
+  struct Block
+  {
+    // Avoid false-sharing by putting highly contended variables on their own cache lines
+    weak_atomic<size_t> front;  // (Atomic) Elements are read from here
+    size_t localTail;           // An uncontended shadow copy of tail, owned by the consumer
+
+    char cachelineFiller0[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic<size_t>) - sizeof(size_t)];
+    weak_atomic<size_t> tail;  // (Atomic) Elements are enqueued here
+    size_t localFront;
+
+    char cachelineFiller1[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic<size_t>) - sizeof(size_t)];  // next isn't
+                                                                                                       // very
+                                                                                                       // contended, but
+                                                                                                       // we don't want
+                                                                                                       // it on the same
+                                                                                                       // cache line as
+                                                                                                       // tail (which
+                                                                                                       // is)
+    weak_atomic<Block*> next;                                                                          // (Atomic)
+
+    char* data;  // Contents (on heap) are aligned to T's alignment
+
+    const size_t sizeMask;
+
+    // size must be a power of two (and greater than 0)
+    Block(size_t const& _size, char* _rawThis, char* _data)
+      : front(0)
+      , localTail(0)
+      , tail(0)
+      , localFront(0)
+      , next(nullptr)
+      , data(_data)
+      , sizeMask(_size - 1)
+      , rawThis(_rawThis)
+    {
+    }
+
+  private:
+    // C4512 - Assignment operator could not be generated
+    Block& operator=(Block const&);
+
+  public:
+    char* rawThis;
+  };
+
+  static Block* make_block(size_t capacity)
+  {
+    // Allocate enough memory for the block itself, as well as all the elements it will contain
+    auto size = sizeof(Block) + std::alignment_of<Block>::value - 1;
+    size += sizeof(T) * capacity + std::alignment_of<T>::value - 1;
+    auto newBlockRaw = static_cast<char*>(std::malloc(size));
+    if (newBlockRaw == nullptr)
+    {
+      return nullptr;
+    }
+
+    auto newBlockAligned = align_for<Block>(newBlockRaw);
+    auto newBlockData = align_for<T>(newBlockAligned + sizeof(Block));
+    return new (newBlockAligned) Block(capacity, newBlockRaw, newBlockData);
+  }
+
+private:
+  weak_atomic<Block*> frontBlock;  // (Atomic) Elements are enqueued to this block
+
+  char cachelineFiller[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic<Block*>)];
+  weak_atomic<Block*> tailBlock;  // (Atomic) Elements are dequeued from this block
+
+  size_t largestBlockSize;
+
+#ifndef NDEBUG
+  bool enqueuing;
+  bool dequeuing;
+#endif
+};
+
+// Like ReaderWriterQueue, but also providees blocking operations
+template <typename T, size_t MAX_BLOCK_SIZE = 512>
+class BlockingReaderWriterQueue
+{
+private:
+  typedef ::moodycamel::ReaderWriterQueue<T, MAX_BLOCK_SIZE> ReaderWriterQueue;
+
+public:
+  explicit BlockingReaderWriterQueue(size_t maxSize = 15) : inner(maxSize)
+  {
+  }
+
+  // Enqueues a copy of element if there is room in the queue.
+  // Returns true if the element was enqueued, false otherwise.
+  // Does not allocate memory.
+  AE_FORCEINLINE bool try_enqueue(T const& element)
+  {
+    if (inner.try_enqueue(element))
+    {
+      sema.signal();
+      return true;
+    }
+    return false;
+  }
+
+  // Enqueues a moved copy of element if there is room in the queue.
+  // Returns true if the element was enqueued, false otherwise.
+  // Does not allocate memory.
+  AE_FORCEINLINE bool try_enqueue(T&& element)
+  {
+    if (inner.try_enqueue(std::forward<T>(element)))
+    {
+      sema.signal();
+      return true;
+    }
+    return false;
+  }
+
+  // Enqueues a copy of element on the queue.
+  // Allocates an additional block of memory if needed.
+  // Only fails (returns false) if memory allocation fails.
+  AE_FORCEINLINE bool enqueue(T const& element)
+  {
+    if (inner.enqueue(element))
+    {
+      sema.signal();
+      return true;
+    }
+    return false;
+  }
+
+  // Enqueues a moved copy of element on the queue.
+  // Allocates an additional block of memory if needed.
+  // Only fails (returns false) if memory allocation fails.
+  AE_FORCEINLINE bool enqueue(T&& element)
+  {
+    if (inner.enqueue(std::forward<T>(element)))
+    {
+      sema.signal();
+      return true;
+    }
+    return false;
+  }
+
+  // Attempts to dequeue an element; if the queue is empty,
+  // returns false instead. If the queue has at least one element,
+  // moves front to result using operator=, then returns true.
+  template <typename U>
+  bool try_dequeue(U& result)
+  {
+    if (sema.tryWait())
+    {
+      bool success = inner.try_dequeue(result);
+      assert(success);
+      AE_UNUSED(success);
+      return true;
+    }
+    return false;
+  }
+
+  // Attempts to dequeue an element; if the queue is empty,
+  // waits until an element is available, then dequeues it.
+  template <typename U>
+  void wait_dequeue(U& result)
+  {
+    sema.wait();
+    bool success = inner.try_dequeue(result);
+    AE_UNUSED(result);
+    assert(success);
+    AE_UNUSED(success);
+  }
+
+  // Attempts to dequeue an element; if the queue is empty,
+  // waits until an element is available up to the specified timeout,
+  // then dequeues it and returns true, or returns false if the timeout
+  // expires before an element can be dequeued.
+  // Using a negative timeout indicates an indefinite timeout,
+  // and is thus functionally equivalent to calling wait_dequeue.
+  template <typename U>
+  bool wait_dequeue_timed(U& result, std::int64_t timeout_usecs)
+  {
+    if (!sema.wait(timeout_usecs))
+    {
+      return false;
+    }
+    bool success = inner.try_dequeue(result);
+    AE_UNUSED(result);
+    assert(success);
+    AE_UNUSED(success);
+    return true;
+  }
+
+#if __cplusplus > 199711L || _MSC_VER >= 1700
+  // Attempts to dequeue an element; if the queue is empty,
+  // waits until an element is available up to the specified timeout,
+  // then dequeues it and returns true, or returns false if the timeout
+  // expires before an element can be dequeued.
+  // Using a negative timeout indicates an indefinite timeout,
+  // and is thus functionally equivalent to calling wait_dequeue.
+  template <typename U, typename Rep, typename Period>
+  inline bool wait_dequeue_timed(U& result, std::chrono::duration<Rep, Period> const& timeout)
+  {
+    return wait_dequeue_timed(result, std::chrono::duration_cast<std::chrono::microseconds>(timeout).count());
+  }
+#endif
+
+  // Returns a pointer to the front element in the queue (the one that
+  // would be removed next by a call to `try_dequeue` or `pop`). If the
+  // queue appears empty at the time the method is called, nullptr is
+  // returned instead.
+  // Must be called only from the consumer thread.
+  AE_FORCEINLINE T* peek()
+  {
+    return inner.peek();
+  }
+
+  // Removes the front element from the queue, if any, without returning it.
+  // Returns true on success, or false if the queue appeared empty at the time
+  // `pop` was called.
+  AE_FORCEINLINE bool pop()
+  {
+    if (sema.tryWait())
+    {
+      bool result = inner.pop();
+      assert(result);
+      AE_UNUSED(result);
+      return true;
+    }
+    return false;
+  }
+
+  // Returns the approximate number of items currently in the queue.
+  // Safe to call from both the producer and consumer threads.
+  AE_FORCEINLINE size_t size_approx() const
+  {
+    return sema.availableApprox();
+  }
+
+private:
+  // Disable copying & assignment
+  BlockingReaderWriterQueue(ReaderWriterQueue const&)
+  {
+  }
+  BlockingReaderWriterQueue& operator=(ReaderWriterQueue const&)
+  {
+  }
+
+private:
+  ReaderWriterQueue inner;
+  spsc_sema::LightweightSemaphore sema;
+};
+
+}  // end namespace moodycamel
+
+#ifdef AE_VCPP
+#pragma warning(pop)
+#endif

include/ur_rtde_driver/ros/action_server.h

@@ -0,0 +1,93 @@
+/*
+ * 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.
+ */
+
+#pragma once
+
+#include <actionlib/server/action_server.h>
+#include <actionlib/server/server_goal_handle.h>
+#include <control_msgs/FollowJointTrajectoryAction.h>
+#include <ros/ros.h>
+#include <atomic>
+#include <chrono>
+#include <condition_variable>
+#include <mutex>
+#include <set>
+#include <thread>
+#include "ur_rtde_driver/log.h"
+#include "ur_rtde_driver/ros/service_stopper.h"
+#include "ur_rtde_driver/ros/trajectory_follower.h"
+#include "ur_rtde_driver/ur/consumer.h"
+#include "ur_rtde_driver/ur/master_board.h"
+#include "ur_rtde_driver/ur/state.h"
+
+class ActionServer : public Service, public URRTPacketConsumer
+{
+private:
+  typedef control_msgs::FollowJointTrajectoryAction Action;
+  typedef control_msgs::FollowJointTrajectoryResult Result;
+  typedef actionlib::ServerGoalHandle<Action> GoalHandle;
+  typedef actionlib::ActionServer<Action> Server;
+
+  ros::NodeHandle nh_;
+  Server as_;
+
+  std::vector<std::string> joint_names_;
+  std::set<std::string> joint_set_;
+  double max_velocity_;
+
+  GoalHandle curr_gh_;
+  std::atomic<bool> interrupt_traj_;
+  std::atomic<bool> has_goal_, running_;
+  std::mutex tj_mutex_;
+  std::condition_variable tj_cv_;
+  std::thread tj_thread_;
+
+  ActionTrajectoryFollowerInterface& follower_;
+
+  RobotState state_;
+  std::array<double, 6> q_actual_, qd_actual_;
+
+  void onGoal(GoalHandle gh);
+  void onCancel(GoalHandle gh);
+
+  bool validate(GoalHandle& gh, Result& res);
+  bool validateState(GoalHandle& gh, Result& res);
+  bool validateJoints(GoalHandle& gh, Result& res);
+  bool validateTrajectory(GoalHandle& gh, Result& res);
+
+  bool try_execute(GoalHandle& gh, Result& res);
+  void interruptGoal(GoalHandle& gh);
+
+  std::vector<size_t> reorderMap(std::vector<std::string> goal_joints);
+
+  void trajectoryThread();
+  bool updateState(RTShared& data);
+
+public:
+  ActionServer(ActionTrajectoryFollowerInterface& follower, std::vector<std::string>& joint_names, double max_velocity);
+
+  void start();
+  virtual void onRobotStateChange(RobotState state);
+
+  virtual bool consume(RTState_V1_6__7& state);
+  virtual bool consume(RTState_V1_8& state);
+  virtual bool consume(RTState_V3_0__1& state);
+  virtual bool consume(RTState_V3_2__3& state);
+};

include/ur_rtde_driver/ros/action_trajectory_follower_interface.h

@@ -0,0 +1,52 @@
+/*
+ * 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.
+ */
+
+#pragma once
+
+#include <inttypes.h>
+#include <array>
+#include <atomic>
+#include <cstddef>
+#include <vector>
+
+struct TrajectoryPoint
+{
+  std::array<double, 6> positions;
+  std::array<double, 6> velocities;
+  std::chrono::microseconds time_from_start;
+
+  TrajectoryPoint()
+  {
+  }
+
+  TrajectoryPoint(std::array<double, 6> &pos, std::array<double, 6> &vel, std::chrono::microseconds tfs)
+    : positions(pos), velocities(vel), time_from_start(tfs)
+  {
+  }
+};
+
+class ActionTrajectoryFollowerInterface
+{
+public:
+  virtual bool start() = 0;
+  virtual bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt) = 0;
+  virtual void stop() = 0;
+  virtual ~ActionTrajectoryFollowerInterface(){};
+};

include/ur_rtde_driver/ros/controller.h

@@ -0,0 +1,110 @@
+/*
+ * 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 <controller_manager/controller_manager.h>
+#include <hardware_interface/force_torque_sensor_interface.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/joint_state_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <ros/ros.h>
+#include <atomic>
+#include "ur_rtde_driver/log.h"
+#include "ur_rtde_driver/ros/hardware_interface.h"
+#include "ur_rtde_driver/ros/service_stopper.h"
+#include "ur_rtde_driver/ur/commander.h"
+#include "ur_rtde_driver/ur/consumer.h"
+#include "ur_rtde_driver/ur/rt_state.h"
+
+class ROSController : private hardware_interface::RobotHW, public URRTPacketConsumer, public Service
+{
+private:
+  ros::NodeHandle nh_;
+  ros::Time lastUpdate_;
+  controller_manager::ControllerManager controller_;
+  bool robot_state_received_;
+
+  // state interfaces
+  JointInterface joint_interface_;
+  WrenchInterface wrench_interface_;
+  // controller interfaces
+  PositionInterface position_interface_;
+  VelocityInterface velocity_interface_;
+
+  // currently activated controller
+  HardwareInterface* active_interface_;
+  // map of switchable controllers
+  std::map<std::string, HardwareInterface*> available_interfaces_;
+
+  std::atomic<bool> service_enabled_;
+  std::atomic<uint32_t> service_cooldown_;
+
+  // helper functions to map interfaces
+  template <typename T>
+  void registerInterface(T* interface)
+  {
+    RobotHW::registerInterface<typename T::parent_type>(interface);
+  }
+  template <typename T>
+  void registerControllereInterface(T* interface)
+  {
+    registerInterface(interface);
+    available_interfaces_[T::INTERFACE_NAME] = interface;
+  }
+
+  void read(RTShared& state);
+  bool update();
+  bool write();
+  void reset();
+
+public:
+  ROSController(URCommander& commander, TrajectoryFollower& follower, std::vector<std::string>& joint_names,
+                double max_vel_change, std::string tcp_link);
+  virtual ~ROSController()
+  {
+  }
+  // from RobotHW
+  void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
+                const std::list<hardware_interface::ControllerInfo>& stop_list);
+  // from URRTPacketConsumer
+  virtual void setupConsumer();
+  virtual bool consume(RTState_V1_6__7& state)
+  {
+    read(state);
+    return update();
+  }
+  virtual bool consume(RTState_V1_8& state)
+  {
+    read(state);
+    return update();
+  }
+  virtual bool consume(RTState_V3_0__1& state)
+  {
+    read(state);
+    return update();
+  }
+  virtual bool consume(RTState_V3_2__3& state)
+  {
+    read(state);
+    return update();
+  }
+
+  virtual void onTimeout();
+
+  virtual void onRobotStateChange(RobotState state);
+};

include/ur_rtde_driver/ros/hardware_interface.h

@@ -0,0 +1,101 @@
+/*
+ * 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 <controller_manager/controller_manager.h>
+#include <hardware_interface/force_torque_sensor_interface.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/joint_state_interface.h>
+#include <algorithm>
+#include "ur_rtde_driver/ros/trajectory_follower.h"
+#include "ur_rtde_driver/ur/commander.h"
+#include "ur_rtde_driver/ur/rt_state.h"
+
+class HardwareInterface
+{
+public:
+  virtual bool write() = 0;
+  virtual void start()
+  {
+  }
+  virtual void stop()
+  {
+  }
+  virtual void reset()
+  {
+  }
+};
+
+using hardware_interface::JointHandle;
+
+class JointInterface : public hardware_interface::JointStateInterface
+{
+private:
+  std::array<double, 6> velocities_, positions_, efforts_;
+
+public:
+  JointInterface(std::vector<std::string> &joint_names);
+  void update(RTShared &packet);
+
+  typedef hardware_interface::JointStateInterface parent_type;
+  static const std::string INTERFACE_NAME;
+};
+
+class WrenchInterface : public hardware_interface::ForceTorqueSensorInterface
+{
+  std::array<double, 6> tcp_;
+
+public:
+  WrenchInterface(std::string tcp_link);
+  void update(RTShared &packet);
+  typedef hardware_interface::ForceTorqueSensorInterface parent_type;
+  static const std::string INTERFACE_NAME;
+};
+
+class VelocityInterface : public HardwareInterface, public hardware_interface::VelocityJointInterface
+{
+private:
+  URCommander &commander_;
+  std::array<double, 6> velocity_cmd_, prev_velocity_cmd_;
+  double max_vel_change_;
+
+public:
+  VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface,
+                    std::vector<std::string> &joint_names, double max_vel_change);
+  virtual bool write();
+  virtual void reset();
+  typedef hardware_interface::VelocityJointInterface parent_type;
+  static const std::string INTERFACE_NAME;
+};
+
+class PositionInterface : public HardwareInterface, public hardware_interface::PositionJointInterface
+{
+private:
+  TrajectoryFollower &follower_;
+  std::array<double, 6> position_cmd_;
+
+public:
+  PositionInterface(TrajectoryFollower &follower, hardware_interface::JointStateInterface &js_interface,
+                    std::vector<std::string> &joint_names);
+  virtual bool write();
+  virtual void start();
+  virtual void stop();
+
+  typedef hardware_interface::PositionJointInterface parent_type;
+  static const std::string INTERFACE_NAME;
+};

include/ur_rtde_driver/ros/io_service.h

@@ -0,0 +1,79 @@
+/*
+ * 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 <ros/ros.h>
+#include <ur_msgs/SetIO.h>
+#include <ur_msgs/SetIORequest.h>
+#include <ur_msgs/SetIOResponse.h>
+#include <ur_msgs/SetPayload.h>
+#include <ur_msgs/SetPayloadRequest.h>
+#include <ur_msgs/SetPayloadResponse.h>
+#include "ur_rtde_driver/log.h"
+#include "ur_rtde_driver/ur/commander.h"
+
+class IOService
+{
+private:
+  ros::NodeHandle nh_;
+  URCommander& commander_;
+  ros::ServiceServer io_service_;
+  ros::ServiceServer payload_service_;
+
+  bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp)
+  {
+    LOG_INFO("setIO called with [%d, %d]", req.fun, req.pin);
+    bool res = false;
+    bool flag = req.state > 0.0 ? true : false;
+    switch (req.fun)
+    {
+      case ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT:
+        res = commander_.setDigitalOut(req.pin, flag);
+        break;
+      case ur_msgs::SetIO::Request::FUN_SET_ANALOG_OUT:
+        res = commander_.setAnalogOut(req.pin, req.state);
+        break;
+      case ur_msgs::SetIO::Request::FUN_SET_TOOL_VOLTAGE:
+        res = commander_.setToolVoltage(static_cast<uint8_t>(req.state));
+        break;
+      case ur_msgs::SetIO::Request::FUN_SET_FLAG:
+        res = commander_.setFlag(req.pin, flag);
+        break;
+      default:
+        LOG_WARN("Invalid setIO function called (%d)", req.fun);
+    }
+
+    return (resp.success = res);
+  }
+
+  bool setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::SetPayloadResponse& resp)
+  {
+    LOG_INFO("setPayload called");
+    // TODO check min and max payload?
+    return (resp.success = commander_.setPayload(req.payload));
+  }
+
+public:
+  IOService(URCommander& commander)
+    : commander_(commander)
+    , io_service_(nh_.advertiseService("ur_driver/set_io", &IOService::setIO, this))
+    , payload_service_(nh_.advertiseService("ur_driver/set_payload", &IOService::setPayload, this))
+  {
+  }
+};

include/ur_rtde_driver/ros/lowbandwidth_trajectory_follower.h

@@ -0,0 +1,56 @@
+/*
+ * Copyright 2017, 2018 Jarek Potiuk (low bandwidth trajectory follower)
+ *
+ * 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 <inttypes.h>
+#include <array>
+#include <atomic>
+#include <cstddef>
+#include <cstring>
+#include <string>
+#include <thread>
+#include <vector>
+#include "ur_rtde_driver/log.h"
+#include "ur_rtde_driver/ros/action_trajectory_follower_interface.h"
+#include "ur_rtde_driver/ur/commander.h"
+#include "ur_rtde_driver/ur/server.h"
+
+class LowBandwidthTrajectoryFollower : public ActionTrajectoryFollowerInterface
+{
+private:
+  std::atomic<bool> running_;
+  std::array<double, 6> last_positions_;
+  URCommander &commander_;
+  URServer server_;
+
+  double time_interval_, servoj_time_, servoj_time_waiting_, max_waiting_time_, servoj_gain_, servoj_lookahead_time_,
+      max_joint_difference_;
+
+  std::string program_;
+
+  bool execute(const std::array<double, 6> &positions, const std::array<double, 6> &velocities, double sample_number,
+               double time_in_seconds);
+
+public:
+  LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, bool version_3);
+
+  bool start();
+  bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt);
+  void stop();
+
+  virtual ~LowBandwidthTrajectoryFollower(){};
+};

include/ur_rtde_driver/ros/mb_publisher.h

@@ -0,0 +1,79 @@
+/*
+ * 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 <industrial_msgs/RobotStatus.h>
+#include <ros/ros.h>
+#include <ur_msgs/Analog.h>
+#include <ur_msgs/Digital.h>
+#include <ur_msgs/IOStates.h>
+
+#include "ur_rtde_driver/ur/consumer.h"
+
+using namespace ros;
+
+class MBPublisher : public URStatePacketConsumer
+{
+private:
+  NodeHandle nh_;
+  Publisher io_pub_;
+  Publisher status_pub_;
+
+  template <size_t N>
+  inline void appendDigital(std::vector<ur_msgs::Digital>& vec, std::bitset<N> bits)
+  {
+    for (size_t i = 0; i < N; i++)
+    {
+      ur_msgs::Digital digi;
+      digi.pin = static_cast<uint8_t>(i);
+      digi.state = bits.test(i);
+      vec.push_back(digi);
+    }
+  }
+
+  void publish(ur_msgs::IOStates& io_msg, SharedMasterBoardData& data);
+  void publishRobotStatus(industrial_msgs::RobotStatus& status, const SharedRobotModeData& data) const;
+  void publishRobotStatus(const RobotModeData_V1_X& data) const;
+  void publishRobotStatus(const RobotModeData_V3_0__1& data) const;
+
+public:
+  MBPublisher()
+    : io_pub_(nh_.advertise<ur_msgs::IOStates>("ur_driver/io_states", 1))
+    , status_pub_(nh_.advertise<industrial_msgs::RobotStatus>("ur_driver/robot_status", 1))
+  {
+  }
+
+  virtual bool consume(MasterBoardData_V1_X& data);
+  virtual bool consume(MasterBoardData_V3_0__1& data);
+  virtual bool consume(MasterBoardData_V3_2& data);
+
+  virtual bool consume(RobotModeData_V1_X& data);
+  virtual bool consume(RobotModeData_V3_0__1& data);
+  virtual bool consume(RobotModeData_V3_2& data);
+
+  virtual void setupConsumer()
+  {
+  }
+  virtual void teardownConsumer()
+  {
+  }
+  virtual void stopConsumer()
+  {
+  }
+};

include/ur_rtde_driver/ros/rt_publisher.h

@@ -0,0 +1,92 @@
+/*
+ * 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 <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <geometry_msgs/WrenchStamped.h>
+#include <ros/ros.h>
+#include <sensor_msgs/JointState.h>
+#include <sensor_msgs/Temperature.h>
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
+#include <cstdlib>
+#include <vector>
+
+#include "ur_rtde_driver/ur/consumer.h"
+
+using namespace ros;
+using namespace tf;
+
+const std::string JOINTS[] = { "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
+                               "wrist_1_joint",      "wrist_2_joint",       "wrist_3_joint" };
+
+class RTPublisher : public URRTPacketConsumer
+{
+private:
+  NodeHandle nh_;
+  Publisher joint_pub_;
+  Publisher wrench_pub_;
+  Publisher tool_vel_pub_;
+  Publisher joint_temperature_pub_;
+  TransformBroadcaster transform_broadcaster_;
+  std::vector<std::string> joint_names_;
+  std::string base_frame_;
+  std::string tool_frame_;
+  bool temp_only_;
+
+  bool publishJoints(RTShared& packet, Time& t);
+  bool publishWrench(RTShared& packet, Time& t);
+  bool publishTool(RTShared& packet, Time& t);
+  bool publishTransform(RTShared& packet, Time& t);
+  bool publishTemperature(RTShared& packet, Time& t);
+
+  bool publish(RTShared& packet);
+
+public:
+  RTPublisher(std::string& joint_prefix, std::string& base_frame, std::string& tool_frame, bool temp_only = false)
+    : joint_pub_(nh_.advertise<sensor_msgs::JointState>("joint_states", 1))
+    , wrench_pub_(nh_.advertise<geometry_msgs::WrenchStamped>("wrench", 1))
+    , tool_vel_pub_(nh_.advertise<geometry_msgs::TwistStamped>("tool_velocity", 1))
+    , joint_temperature_pub_(nh_.advertise<sensor_msgs::Temperature>("joint_temperature", 1))
+    , base_frame_(base_frame)
+    , tool_frame_(tool_frame)
+    , temp_only_(temp_only)
+  {
+    for (auto const& j : JOINTS)
+    {
+      joint_names_.push_back(joint_prefix + j);
+    }
+  }
+
+  virtual bool consume(RTState_V1_6__7& state);
+  virtual bool consume(RTState_V1_8& state);
+  virtual bool consume(RTState_V3_0__1& state);
+  virtual bool consume(RTState_V3_2__3& state);
+
+  virtual void setupConsumer()
+  {
+  }
+  virtual void teardownConsumer()
+  {
+  }
+  virtual void stopConsumer()
+  {
+  }
+};

include/ur_rtde_driver/ros/service_stopper.h

@@ -0,0 +1,88 @@
+/*
+ * 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 <ros/ros.h>
+#include <std_srvs/Empty.h>
+#include "ur_rtde_driver/log.h"
+#include "ur_rtde_driver/ur/consumer.h"
+
+enum class RobotState
+{
+  Running,
+  Error,
+  EmergencyStopped,
+  ProtectiveStopped
+};
+
+enum class ActivationMode
+{
+  Never,
+  Always,
+  OnStartup
+};
+
+class Service
+{
+public:
+  virtual void onRobotStateChange(RobotState state) = 0;
+};
+
+class ServiceStopper : public URStatePacketConsumer
+{
+private:
+  ros::NodeHandle nh_;
+  ros::ServiceServer enable_service_;
+  std::vector<Service*> services_;
+  RobotState last_state_;
+  ActivationMode activation_mode_;
+
+  void notify_all(RobotState state);
+  bool handle(SharedRobotModeData& data, bool error);
+  bool enableCallback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp);
+
+public:
+  ServiceStopper(std::vector<Service*> services);
+
+  virtual bool consume(RobotModeData_V1_X& data)
+  {
+    return handle(data, data.robot_mode != robot_mode_V1_X::ROBOT_RUNNING_MODE);
+  }
+  virtual bool consume(RobotModeData_V3_0__1& data)
+  {
+    return handle(data, data.robot_mode != robot_mode_V3_X::RUNNING);
+  }
+  virtual bool consume(RobotModeData_V3_2& data)
+  {
+    return handle(data, data.robot_mode != robot_mode_V3_X::RUNNING);
+  }
+
+  // unused
+  virtual bool consume(MasterBoardData_V1_X& data)
+  {
+    return true;
+  }
+  virtual bool consume(MasterBoardData_V3_0__1& data)
+  {
+    return true;
+  }
+  virtual bool consume(MasterBoardData_V3_2& data)
+  {
+    return true;
+  }
+};

include/ur_rtde_driver/ros/trajectory_follower.h

@@ -0,0 +1,67 @@
+/*
+ * 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.
+ */
+
+#pragma once
+
+#include <inttypes.h>
+#include <array>
+#include <atomic>
+#include <cstddef>
+#include <cstring>
+#include <string>
+#include <thread>
+#include <vector>
+#include "ur_rtde_driver/log.h"
+#include "ur_rtde_driver/ros/action_trajectory_follower_interface.h"
+#include "ur_rtde_driver/ur/commander.h"
+#include "ur_rtde_driver/ur/server.h"
+
+class TrajectoryFollower : public ActionTrajectoryFollowerInterface
+{
+private:
+  std::atomic<bool> running_;
+  std::array<double, 6> last_positions_;
+  URCommander &commander_;
+  URServer server_;
+
+  double servoj_time_, servoj_lookahead_time_, servoj_gain_;
+  std::string program_;
+
+  template <typename T>
+  size_t append(uint8_t *buffer, T &val)
+  {
+    size_t s = sizeof(T);
+    std::memcpy(buffer, &val, s);
+    return s;
+  }
+
+  bool execute(std::array<double, 6> &positions, bool keep_alive);
+  double interpolate(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel);
+
+public:
+  TrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, bool version_3);
+
+  bool start();
+  bool execute(std::array<double, 6> &positions);
+  bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt);
+  void stop();
+
+  virtual ~TrajectoryFollower(){};
+};

include/ur_rtde_driver/ros/urscript_handler.h

@@ -0,0 +1,41 @@
+/*
+ * 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 <ros/ros.h>
+#include <string>
+
+#include "std_msgs/String.h"
+#include "ur_rtde_driver/log.h"
+#include "ur_rtde_driver/ros/service_stopper.h"
+#include "ur_rtde_driver/ur/commander.h"
+
+class URScriptHandler : public Service
+{
+private:
+  ros::NodeHandle nh_;
+  URCommander &commander_;
+  ros::Subscriber urscript_sub_;
+  RobotState state_;
+
+public:
+  URScriptHandler(URCommander &commander);
+  void urscriptInterface(const std_msgs::String::ConstPtr &msg);
+  void onRobotStateChange(RobotState state);
+};

include/ur_rtde_driver/tcp_socket.h

@@ -0,0 +1,72 @@
+/*
+ * 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>
+
+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();
+};

include/ur_rtde_driver/test/random_data.h

@@ -0,0 +1,80 @@
+/*
+ * 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 <inttypes.h>
+#include <algorithm>
+#include <bitset>
+#include <climits>
+#include <cstddef>
+#include <functional>
+#include <random>
+#include "ur_rtde_driver/bin_parser.h"
+
+class RandomDataTest
+{
+private:
+  using random_bytes_engine = std::independent_bits_engine<std::default_random_engine, CHAR_BIT, uint8_t>;
+  uint8_t* buf_;
+  BinParser bp_;
+  size_t n_;
+
+public:
+  RandomDataTest(size_t n) : buf_(new uint8_t[n]), bp_(buf_, n), n_(n)
+  {
+    random_bytes_engine rbe;
+    std::generate(buf_, buf_ + n, std::ref(rbe));
+  }
+
+  ~RandomDataTest()
+  {
+    delete buf_;
+  }
+
+  BinParser getParser(bool skip = false)
+  {
+    return BinParser(buf_, n_ - (skip ? sizeof(int32_t) : 0));
+  }
+
+  template <typename T>
+  T getNext()
+  {
+    T actual;
+    bp_.parse(actual);
+    return actual;
+  }
+
+  template <typename T, size_t N>
+  std::bitset<N> getNext()
+  {
+    T actual;
+    bp_.parse(actual);
+    return std::bitset<N>(actual);
+  }
+
+  template <typename T>
+  void set(T data, size_t pos)
+  {
+    std::memcpy(&data, buf_ + pos, sizeof(T));
+  }
+
+  void skip(size_t n)
+  {
+    bp_.consume(n);
+  }
+};

include/ur_rtde_driver/test/utils.h

@@ -0,0 +1,25 @@
+/*
+ * 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
+
+#define ASSERT_DOUBLE_ARRAY_EQ(fn, name)                                                                               \
+  for (auto const& v : name)                                                                                           \
+  {                                                                                                                    \
+    ASSERT_EQ(fn, v) << #name " failed parsing";                                                                       \
+  }

include/ur_rtde_driver/types.h

@@ -0,0 +1,42 @@
+/*
+ * 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 <inttypes.h>
+
+struct double3_t
+{
+  double x, y, z;
+};
+
+struct cartesian_coord_t
+{
+  double3_t position;
+  double3_t rotation;
+};
+
+inline bool operator==(const double3_t& lhs, const double3_t& rhs)
+{
+  return lhs.x == rhs.x && lhs.y == rhs.y && lhs.z == rhs.z;
+}
+
+inline bool operator==(const cartesian_coord_t& lhs, const cartesian_coord_t& rhs)
+{
+  return lhs.position == rhs.position && lhs.rotation == rhs.rotation;
+}

include/ur_rtde_driver/ur/commander.h

@@ -0,0 +1,93 @@
+/*
+ * 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 <array>
+#include <iomanip>
+#include <sstream>
+#include "ur_rtde_driver/ur/stream.h"
+
+class URCommander
+{
+private:
+  URStream &stream_;
+
+protected:
+  bool write(const std::string &s);
+  void formatArray(std::ostringstream &out, std::array<double, 6> &values);
+
+public:
+  URCommander(URStream &stream) : stream_(stream)
+  {
+  }
+
+  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(const 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) = 0;
+  virtual bool setDigitalOut(uint8_t pin, bool value);
+  virtual bool setAnalogOut(uint8_t pin, double value);
+};
+
+class URCommander_V3_1__2 : public URCommander_V3_X
+{
+public:
+  URCommander_V3_1__2(URStream &stream) : URCommander_V3_X(stream)
+  {
+  }
+
+  virtual bool speedj(std::array<double, 6> &speeds, double acceleration);
+};
+
+class URCommander_V3_3 : public URCommander_V3_X
+{
+public:
+  URCommander_V3_3(URStream &stream) : URCommander_V3_X(stream)
+  {
+  }
+
+  virtual bool speedj(std::array<double, 6> &speeds, double acceleration);
+};

include/ur_rtde_driver/ur/consumer.h

@@ -0,0 +1,68 @@
+/*
+ * 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 "ur_rtde_driver/pipeline.h"
+#include "ur_rtde_driver/ur/master_board.h"
+#include "ur_rtde_driver/ur/messages.h"
+#include "ur_rtde_driver/ur/robot_mode.h"
+#include "ur_rtde_driver/ur/rt_state.h"
+#include "ur_rtde_driver/ur/state.h"
+
+class URRTPacketConsumer : public IConsumer<RTPacket>
+{
+public:
+  virtual bool consume(shared_ptr<RTPacket> packet)
+  {
+    return packet->consumeWith(*this);
+  }
+
+  virtual bool consume(RTState_V1_6__7& state) = 0;
+  virtual bool consume(RTState_V1_8& state) = 0;
+  virtual bool consume(RTState_V3_0__1& state) = 0;
+  virtual bool consume(RTState_V3_2__3& state) = 0;
+};
+
+class URStatePacketConsumer : public IConsumer<StatePacket>
+{
+public:
+  virtual bool consume(shared_ptr<StatePacket> packet)
+  {
+    return packet->consumeWith(*this);
+  }
+
+  virtual bool consume(MasterBoardData_V1_X& data) = 0;
+  virtual bool consume(MasterBoardData_V3_0__1& data) = 0;
+  virtual bool consume(MasterBoardData_V3_2& data) = 0;
+
+  virtual bool consume(RobotModeData_V1_X& data) = 0;
+  virtual bool consume(RobotModeData_V3_0__1& data) = 0;
+  virtual bool consume(RobotModeData_V3_2& data) = 0;
+};
+
+class URMessagePacketConsumer : public IConsumer<MessagePacket>
+{
+public:
+  virtual bool consume(shared_ptr<MessagePacket> packet)
+  {
+    return packet->consumeWith(*this);
+  }
+
+  virtual bool consume(VersionMessage& message) = 0;
+};

include/ur_rtde_driver/ur/factory.h

@@ -0,0 +1,141 @@
+/*
+ * 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 <cstdlib>
+#include "ur_rtde_driver/ur/consumer.h"
+#include "ur_rtde_driver/ur/messages_parser.h"
+#include "ur_rtde_driver/ur/parser.h"
+#include "ur_rtde_driver/ur/producer.h"
+#include "ur_rtde_driver/ur/rt_parser.h"
+#include "ur_rtde_driver/ur/state_parser.h"
+#include "ur_rtde_driver/ur/stream.h"
+
+static const int UR_PRIMARY_PORT = 30001;
+
+class URFactory : private URMessagePacketConsumer
+{
+private:
+  URStream stream_;
+  URMessageParser parser_;
+
+  uint8_t major_version_;
+  uint8_t minor_version_;
+
+  bool consume(VersionMessage& vm)
+  {
+    LOG_INFO("Got VersionMessage:");
+    LOG_INFO("project name: %s", vm.project_name.c_str());
+    LOG_INFO("version: %u.%u.%d", vm.major_version, vm.minor_version, vm.svn_version);
+    LOG_INFO("build date: %s", vm.build_date.c_str());
+
+    major_version_ = vm.major_version;
+    minor_version_ = vm.minor_version;
+
+    return true;
+  }
+
+  void setupConsumer()
+  {
+  }
+  void teardownConsumer()
+  {
+  }
+  void stopConsumer()
+  {
+  }
+
+public:
+  URFactory(std::string& host) : stream_(host, UR_PRIMARY_PORT)
+  {
+    URProducer<MessagePacket> prod(stream_, parser_);
+    std::vector<unique_ptr<MessagePacket>> results;
+
+    prod.setupProducer();
+
+    if (!prod.tryGet(results) || results.size() == 0)
+    {
+      LOG_FATAL("No version message received, init failed!");
+      std::exit(EXIT_FAILURE);
+    }
+
+    for (auto const& p : results)
+    {
+      p->consumeWith(*this);
+    }
+
+    if (major_version_ == 0 && minor_version_ == 0)
+    {
+      LOG_FATAL("No version message received, init failed!");
+      std::exit(EXIT_FAILURE);
+    }
+
+    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 if (minor_version_ < 3)
+      return std::unique_ptr<URCommander>(new URCommander_V3_1__2(stream));
+    else
+      return std::unique_ptr<URCommander>(new URCommander_V3_3(stream));
+  }
+
+  std::unique_ptr<URParser<StatePacket>> getStateParser()
+  {
+    if (major_version_ == 1)
+    {
+      return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V1_X);
+    }
+    else
+    {
+      if (minor_version_ < 3)
+        return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V3_0__1);
+      else if (minor_version_ < 5)
+        return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V3_2);
+      else
+        return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V3_5);
+    }
+  }
+
+  std::unique_ptr<URParser<RTPacket>> getRTParser()
+  {
+    if (major_version_ == 1)
+    {
+      if (minor_version_ < 8)
+        return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V1_6__7);
+      else
+        return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V1_8);
+    }
+    else
+    {
+      if (minor_version_ < 3)
+        return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V3_0__1);
+      else
+        return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V3_2__3);
+    }
+  }
+};

include/ur_rtde_driver/ur/master_board.h

@@ -0,0 +1,110 @@
+/*
+ * 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 <inttypes.h>
+#include <bitset>
+#include <cstddef>
+#include "ur_rtde_driver/bin_parser.h"
+#include "ur_rtde_driver/types.h"
+#include "ur_rtde_driver/ur/state.h"
+
+class SharedMasterBoardData
+{
+public:
+  virtual bool parseWith(BinParser& bp);
+
+  int8_t analog_input_range0;
+  int8_t analog_input_range1;
+  double analog_input0;
+  double analog_input1;
+  int8_t analog_output_domain0;
+  int8_t analog_output_domain1;
+  double analog_output0;
+  double analog_output1;
+  float master_board_temperature;
+  float robot_voltage_48V;
+  float robot_current;
+  float master_IO_current;
+
+  bool euromap67_interface_installed;
+
+  // euromap fields are dynamic so don't include in SIZE
+  int32_t euromap_input_bits;
+  int32_t euromap_output_bits;
+
+  static const size_t SIZE = sizeof(double) * 4 + sizeof(int8_t) * 4 + sizeof(float) * 4 + sizeof(uint8_t);
+
+  static const size_t EURO_SIZE = sizeof(int32_t) * 2;
+};
+
+class MasterBoardData_V1_X : public SharedMasterBoardData, public StatePacket
+{
+public:
+  virtual bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URStatePacketConsumer& consumer);
+
+  std::bitset<10> digital_input_bits;
+  std::bitset<10> digital_output_bits;
+
+  uint8_t master_safety_state;
+  bool master_on_off_state;
+
+  // euromap fields are dynamic so don't include in SIZE
+  int16_t euromap_voltage;
+  int16_t euromap_current;
+
+  static const size_t SIZE = SharedMasterBoardData::SIZE + sizeof(int16_t) * 2 + sizeof(uint8_t) * 2;
+
+  static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE + sizeof(int16_t) * 2;
+};
+
+class MasterBoardData_V3_0__1 : public SharedMasterBoardData, public StatePacket
+{
+public:
+  virtual bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URStatePacketConsumer& consumer);
+
+  std::bitset<18> digital_input_bits;
+  std::bitset<18> digital_output_bits;
+
+  uint8_t safety_mode;
+  bool in_reduced_mode;
+
+  // euromap fields are dynamic so don't include in SIZE
+  float euromap_voltage;
+  float euromap_current;
+
+  static const size_t SIZE = SharedMasterBoardData::SIZE + sizeof(int32_t) * 2 + sizeof(uint8_t) * 2 +
+                             sizeof(uint32_t);  // UR internal field we skip
+
+  static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE + sizeof(float) * 2;
+};
+
+class MasterBoardData_V3_2 : public MasterBoardData_V3_0__1
+{
+public:
+  virtual bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URStatePacketConsumer& consumer);
+
+  uint8_t operational_mode_selector_input;
+  uint8_t three_position_enabling_device_input;
+
+  static const size_t SIZE = MasterBoardData_V3_0__1::SIZE + sizeof(uint8_t) * 2;
+};

include/ur_rtde_driver/ur/messages.h

@@ -0,0 +1,69 @@
+/*
+ * 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 <inttypes.h>
+#include <cstddef>
+#include "ur_rtde_driver/bin_parser.h"
+#include "ur_rtde_driver/pipeline.h"
+
+enum class robot_message_type : uint8_t
+{
+  ROBOT_MESSAGE_TEXT = 0,
+  ROBOT_MESSAGE_PROGRAM_LABEL = 1,
+  PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2,
+  ROBOT_MESSAGE_VERSION = 3,
+  ROBOT_MESSAGE_SAFETY_MODE = 5,
+  ROBOT_MESSAGE_ERROR_CODE = 6,
+  ROBOT_MESSAGE_KEY = 7,
+  ROBOT_MESSAGE_REQUEST_VALUE = 9,
+  ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10
+};
+
+class URMessagePacketConsumer;
+
+class MessagePacket
+{
+public:
+  MessagePacket(uint64_t timestamp, uint8_t source) : timestamp(timestamp), source(source)
+  {
+  }
+  virtual bool parseWith(BinParser& bp) = 0;
+  virtual bool consumeWith(URMessagePacketConsumer& consumer) = 0;
+
+  uint64_t timestamp;
+  uint8_t source;
+};
+
+class VersionMessage : public MessagePacket
+{
+public:
+  VersionMessage(uint64_t timestamp, uint8_t source) : MessagePacket(timestamp, source)
+  {
+  }
+
+  virtual bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URMessagePacketConsumer& consumer);
+
+  std::string project_name;
+  uint8_t major_version;
+  uint8_t minor_version;
+  int32_t svn_version;
+  std::string build_date;
+};

include/ur_rtde_driver/ur/messages_parser.h

@@ -0,0 +1,71 @@
+/*
+ * 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/log.h"
+#include "ur_rtde_driver/pipeline.h"
+#include "ur_rtde_driver/ur/messages.h"
+#include "ur_rtde_driver/ur/parser.h"
+
+class URMessageParser : public URParser<MessagePacket>
+{
+public:
+  bool parse(BinParser& bp, std::vector<unique_ptr<MessagePacket>>& results)
+  {
+    int32_t packet_size;
+    message_type type;
+    bp.parse(packet_size);
+    bp.parse(type);
+
+    if (type != message_type::ROBOT_MESSAGE)
+    {
+      LOG_WARN("Invalid message type recieved: %u", static_cast<uint8_t>(type));
+      return false;
+    }
+
+    uint64_t timestamp;
+    uint8_t source;
+    robot_message_type message_type;
+
+    bp.parse(timestamp);
+    bp.parse(source);
+    bp.parse(message_type);
+
+    std::unique_ptr<MessagePacket> result;
+    bool parsed = false;
+
+    switch (message_type)
+    {
+      case robot_message_type::ROBOT_MESSAGE_VERSION:
+      {
+        VersionMessage* vm = new VersionMessage(timestamp, source);
+        parsed = vm->parseWith(bp);
+        result.reset(vm);
+        break;
+      }
+
+      default:
+        return false;
+    }
+
+    results.push_back(std::move(result));
+    return parsed;
+  }
+};

include/ur_rtde_driver/ur/parser.h

@@ -0,0 +1,29 @@
+/*
+ * 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/pipeline.h"
+
+template <typename T>
+class URParser
+{
+public:
+  virtual bool parse(BinParser& bp, std::vector<std::unique_ptr<T>>& results) = 0;
+};

include/ur_rtde_driver/ur/producer.h

@@ -0,0 +1,83 @@
+/*
+ * 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 <chrono>
+#include "ur_rtde_driver/pipeline.h"
+#include "ur_rtde_driver/ur/parser.h"
+#include "ur_rtde_driver/ur/stream.h"
+
+template <typename T>
+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), timeout_(1)
+  {
+  }
+
+  void setupProducer()
+  {
+    stream_.connect();
+  }
+  void teardownProducer()
+  {
+    stream_.disconnect();
+  }
+  void stopProducer()
+  {
+    stream_.disconnect();
+  }
+
+  bool tryGet(std::vector<unique_ptr<T>>& products)
+  {
+    // 4KB should be enough to hold any packet received from UR
+    uint8_t buf[4096];
+    size_t read = 0;
+    // expoential backoff reconnects
+    while (true)
+    {
+      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_);
+
+      if (stream_.connect())
+        continue;
+
+      auto next = timeout_ * 2;
+      if (next <= std::chrono::seconds(120))
+        timeout_ = next;
+    }
+
+    BinParser bp(buf, read);
+    return parser_.parse(bp, products);
+  }
+};

include/ur_rtde_driver/ur/robot_mode.h

@@ -0,0 +1,136 @@
+/*
+ * 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 <inttypes.h>
+#include <cstddef>
+#include "ur_rtde_driver/bin_parser.h"
+#include "ur_rtde_driver/types.h"
+#include "ur_rtde_driver/ur/state.h"
+
+class SharedRobotModeData
+{
+public:
+  virtual bool parseWith(BinParser& bp);
+
+  uint64_t timestamp;
+  bool physical_robot_connected;
+  bool real_robot_enabled;
+  bool robot_power_on;
+  bool emergency_stopped;
+  bool protective_stopped;  // AKA security_stopped
+  bool program_running;
+  bool program_paused;
+
+  static const size_t SIZE = sizeof(uint64_t) + sizeof(uint8_t) * 6;
+};
+
+enum class robot_mode_V1_X : uint8_t
+{
+  ROBOT_RUNNING_MODE = 0,
+  ROBOT_FREEDRIVE_MODE = 1,
+  ROBOT_READY_MODE = 2,
+  ROBOT_INITIALIZING_MODE = 3,
+  ROBOT_SECURITY_STOPPED_MODE = 4,
+  ROBOT_EMERGENCY_STOPPED_MODE = 5,
+  ROBOT_FATAL_ERROR_MODE = 6,
+  ROBOT_NO_POWER_MODE = 7,
+  ROBOT_NOT_CONNECTED_MODE = 8,
+  ROBOT_SHUTDOWN_MODE = 9,
+  ROBOT_SAFEGUARD_STOP_MODE = 10
+};
+
+class RobotModeData_V1_X : public SharedRobotModeData, public StatePacket
+{
+public:
+  virtual bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URStatePacketConsumer& consumer);
+
+  robot_mode_V1_X robot_mode;
+  double speed_fraction;
+
+  static const size_t SIZE = SharedRobotModeData::SIZE + sizeof(uint8_t) + sizeof(robot_mode_V1_X) + sizeof(double);
+
+  static_assert(RobotModeData_V1_X::SIZE == 24, "RobotModeData_V1_X has missmatched size");
+};
+
+enum class robot_mode_V3_X : uint8_t
+{
+  DISCONNECTED = 0,
+  CONFIRM_SAFETY = 1,
+  BOOTING = 2,
+  POWER_OFF = 3,
+  POWER_ON = 4,
+  IDLE = 5,
+  BACKDRIVE = 6,
+  RUNNING = 7,
+  UPDATING_FIRMWARE = 8
+};
+
+enum class robot_control_mode_V3_X : uint8_t
+{
+  POSITION = 0,
+  TEACH = 1,
+  FORCE = 2,
+  TORQUE = 3
+};
+
+class RobotModeData_V3_0__1 : public SharedRobotModeData, public StatePacket
+{
+public:
+  virtual bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URStatePacketConsumer& consumer);
+
+  robot_mode_V3_X robot_mode;
+  robot_control_mode_V3_X control_mode;
+
+  double target_speed_fraction;
+  double speed_scaling;
+
+  static const size_t SIZE = SharedRobotModeData::SIZE + sizeof(uint8_t) + sizeof(robot_mode_V3_X) +
+                             sizeof(robot_control_mode_V3_X) + sizeof(double) + sizeof(double);
+
+  static_assert(RobotModeData_V3_0__1::SIZE == 33, "RobotModeData_V3_0__1 has missmatched size");
+};
+
+class RobotModeData_V3_2 : public RobotModeData_V3_0__1
+{
+public:
+  virtual bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URStatePacketConsumer& consumer);
+
+  double target_speed_fraction_limit;
+
+  static const size_t SIZE = RobotModeData_V3_0__1::SIZE + sizeof(double);
+
+  static_assert(RobotModeData_V3_2::SIZE == 41, "RobotModeData_V3_2 has missmatched size");
+};
+
+class RobotModeData_V3_5 : public RobotModeData_V3_2
+{
+public:
+  virtual bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URStatePacketConsumer& consumer);
+
+  unsigned char unknown_internal_use;
+
+  static const size_t SIZE = RobotModeData_V3_2::SIZE + sizeof(unsigned char);
+
+  static_assert(RobotModeData_V3_5::SIZE == 42, "RobotModeData_V3_5 has missmatched size");
+};

include/ur_rtde_driver/ur/rt_parser.h

@@ -0,0 +1,56 @@
+/*
+ * 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/log.h"
+#include "ur_rtde_driver/pipeline.h"
+#include "ur_rtde_driver/ur/parser.h"
+#include "ur_rtde_driver/ur/rt_state.h"
+
+template <typename T>
+class URRTStateParser : public URParser<RTPacket>
+{
+public:
+  bool parse(BinParser& bp, std::vector<std::unique_ptr<RTPacket>>& results)
+  {
+    int32_t packet_size = bp.peek<int32_t>();
+
+    if (!bp.checkSize(packet_size))
+    {
+      LOG_ERROR("Buffer len shorter than expected packet length");
+      return false;
+    }
+
+    bp.parse(packet_size);  // consumes the peeked data
+
+    std::unique_ptr<RTPacket> packet(new T);
+    if (!packet->parseWith(bp))
+      return false;
+
+    results.push_back(std::move(packet));
+
+    return true;
+  }
+};
+
+typedef URRTStateParser<RTState_V1_6__7> URRTStateParser_V1_6__7;
+typedef URRTStateParser<RTState_V1_8> URRTStateParser_V1_8;
+typedef URRTStateParser<RTState_V3_0__1> URRTStateParser_V3_0__1;
+typedef URRTStateParser<RTState_V3_2__3> URRTStateParser_V3_2__3;

include/ur_rtde_driver/ur/rt_state.h

@@ -0,0 +1,137 @@
+/*
+ * 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 <inttypes.h>
+#include <cstddef>
+#include "ur_rtde_driver/bin_parser.h"
+#include "ur_rtde_driver/pipeline.h"
+#include "ur_rtde_driver/types.h"
+
+class URRTPacketConsumer;
+
+class RTPacket
+{
+public:
+  virtual bool parseWith(BinParser& bp) = 0;
+  virtual bool consumeWith(URRTPacketConsumer& consumer) = 0;
+};
+
+class RTShared
+{
+protected:
+  void parse_shared1(BinParser& bp);
+  void parse_shared2(BinParser& bp);
+
+public:
+  double time;
+  std::array<double, 6> q_target;
+  std::array<double, 6> qd_target;
+  std::array<double, 6> qdd_target;
+  std::array<double, 6> i_target;
+  std::array<double, 6> m_target;
+  std::array<double, 6> q_actual;
+  std::array<double, 6> qd_actual;
+  std::array<double, 6> i_actual;
+
+  // gap here depending on version
+
+  std::array<double, 6> tcp_force;
+
+  // does not contain "_actual" postfix in V1_X but
+  // they're the same fields so share anyway
+  cartesian_coord_t tool_vector_actual;
+  cartesian_coord_t tcp_speed_actual;
+
+  // gap here depending on version
+
+  uint64_t digital_inputs;
+  std::array<double, 6> motor_temperatures;
+  double controller_time;
+  double robot_mode;
+
+  static const size_t SIZE =
+      sizeof(double) * 3 + sizeof(double[6]) * 10 + sizeof(cartesian_coord_t) * 2 + sizeof(uint64_t);
+};
+
+class RTState_V1_6__7 : public RTShared, public RTPacket
+{
+public:
+  bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URRTPacketConsumer& consumer);
+
+  double3_t tool_accelerometer_values;
+
+  static const size_t SIZE = RTShared::SIZE + sizeof(double3_t);
+
+  static_assert(RTState_V1_6__7::SIZE == 632, "RTState_V1_6__7 has mismatched size!");
+};
+
+class RTState_V1_8 : public RTState_V1_6__7
+{
+public:
+  bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URRTPacketConsumer& consumer);
+
+  std::array<double, 6> joint_modes;
+
+  static const size_t SIZE = RTState_V1_6__7::SIZE + sizeof(double[6]);
+
+  static_assert(RTState_V1_8::SIZE == 680, "RTState_V1_8 has mismatched size!");
+};
+
+class RTState_V3_0__1 : public RTShared, public RTPacket
+{
+public:
+  bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URRTPacketConsumer& consumer);
+
+  std::array<double, 6> i_control;
+  cartesian_coord_t tool_vector_target;
+  cartesian_coord_t tcp_speed_target;
+
+  std::array<double, 6> joint_modes;
+  double safety_mode;
+  double3_t tool_accelerometer_values;
+  double speed_scaling;
+  double linear_momentum_norm;
+  double v_main;
+  double v_robot;
+  double i_robot;
+  std::array<double, 6> v_actual;
+
+  static const size_t SIZE =
+      RTShared::SIZE + sizeof(double[6]) * 3 + sizeof(double3_t) + sizeof(cartesian_coord_t) * 2 + sizeof(double) * 6;
+
+  static_assert(RTState_V3_0__1::SIZE == 920, "RTState_V3_0__1 has mismatched size!");
+};
+
+class RTState_V3_2__3 : public RTState_V3_0__1
+{
+public:
+  bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URRTPacketConsumer& consumer);
+
+  uint64_t digital_outputs;
+  double program_state;
+
+  static const size_t SIZE = RTState_V3_0__1::SIZE + sizeof(uint64_t) + sizeof(double);
+
+  static_assert(RTState_V3_2__3::SIZE == 936, "RTState_V3_2__3 has mismatched size!");
+};

include/ur_rtde_driver/ur/server.h

@@ -0,0 +1,49 @@
+/*
+ * 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 <cstdlib>
+#include <mutex>
+#include <string>
+#include "ur_rtde_driver/tcp_socket.h"
+
+#define MAX_SERVER_BUF_LEN 50
+
+class URServer : private TCPSocket
+{
+private:
+  int port_;
+  TCPSocket client_;
+
+protected:
+  virtual bool open(int socket_fd, struct sockaddr *address, size_t address_len);
+
+public:
+  URServer(int port);
+  ~URServer();
+  std::string getIP();
+  bool bind();
+  bool accept();
+  void disconnectClient();
+  bool readLine(char *buffer, size_t buf_len);
+  bool write(const uint8_t *buf, size_t buf_len, size_t &written);
+};

include/ur_rtde_driver/ur/state.h

@@ -0,0 +1,61 @@
+/*
+ * 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 <inttypes.h>
+#include <cstddef>
+#include "ur_rtde_driver/bin_parser.h"
+#include "ur_rtde_driver/log.h"
+#include "ur_rtde_driver/pipeline.h"
+
+enum class package_type : uint8_t
+{
+  ROBOT_MODE_DATA = 0,
+  JOINT_DATA = 1,
+  TOOL_DATA = 2,
+  MASTERBOARD_DATA = 3,
+  CARTESIAN_INFO = 4,
+  KINEMATICS_INFO = 5,
+  CONFIGURATION_DATA = 6,
+  FORCE_MODE_DATA = 7,
+  ADDITIONAL_INFO = 8,
+  CALIBRATION_DATA = 9
+};
+
+enum class message_type : uint8_t
+{
+  ROBOT_STATE = 16,
+  ROBOT_MESSAGE = 20,
+  PROGRAM_STATE_MESSAGE = 25
+};
+
+class URStatePacketConsumer;
+
+class StatePacket
+{
+public:
+  StatePacket()
+  {
+  }
+  virtual ~StatePacket()
+  {
+  }
+  virtual bool parseWith(BinParser& bp) = 0;
+  virtual bool consumeWith(URStatePacketConsumer& consumer) = 0;
+};

include/ur_rtde_driver/ur/state_parser.h

@@ -0,0 +1,117 @@
+/*
+ * 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/log.h"
+#include "ur_rtde_driver/pipeline.h"
+#include "ur_rtde_driver/ur/master_board.h"
+#include "ur_rtde_driver/ur/parser.h"
+#include "ur_rtde_driver/ur/robot_mode.h"
+#include "ur_rtde_driver/ur/state.h"
+
+template <typename RMD, typename MBD>
+class URStateParser : public URParser<StatePacket>
+{
+private:
+  StatePacket* from_type(package_type type)
+  {
+    switch (type)
+    {
+      case package_type::ROBOT_MODE_DATA:
+        return new RMD;
+      case package_type::MASTERBOARD_DATA:
+        return new MBD;
+      default:
+        return nullptr;
+    }
+  }
+
+public:
+  bool parse(BinParser& bp, std::vector<std::unique_ptr<StatePacket>>& results)
+  {
+    int32_t packet_size;
+    message_type type;
+    bp.parse(packet_size);
+    bp.parse(type);
+
+    if (type != message_type::ROBOT_STATE)
+    {
+      // quietly ignore the intial version message
+      if (type != message_type::ROBOT_MESSAGE)
+      {
+        LOG_WARN("Invalid state message type recieved: %u", static_cast<uint8_t>(type));
+      }
+
+      bp.consume();
+      return true;
+    }
+
+    while (!bp.empty())
+    {
+      if (!bp.checkSize(sizeof(uint32_t)))
+      {
+        LOG_ERROR("Failed to read sub-package length, there's likely a parsing error");
+        return false;
+      }
+      uint32_t sub_size = bp.peek<uint32_t>();
+      if (!bp.checkSize(static_cast<size_t>(sub_size)))
+      {
+        LOG_WARN("Invalid sub-package size of %" PRIu32 " received!", sub_size);
+        return false;
+      }
+
+      // deconstruction of a sub parser will increment the position of the parent parser
+      BinParser sbp(bp, sub_size);
+      sbp.consume(sizeof(sub_size));
+      package_type type;
+      sbp.parse(type);
+
+      std::unique_ptr<StatePacket> packet(from_type(type));
+
+      if (packet == nullptr)
+      {
+        sbp.consume();
+        continue;
+      }
+
+      if (!packet->parseWith(sbp))
+      {
+        LOG_ERROR("Sub-package parsing of type %d failed!", static_cast<int>(type));
+        return false;
+      }
+
+      results.push_back(std::move(packet));
+
+      if (!sbp.empty())
+      {
+        LOG_ERROR("Sub-package of type %d was not parsed completely!", static_cast<int>(type));
+        sbp.debug();
+        return false;
+      }
+    }
+
+    return true;
+  }
+};
+
+typedef URStateParser<RobotModeData_V1_X, MasterBoardData_V1_X> URStateParser_V1_X;
+typedef URStateParser<RobotModeData_V3_0__1, MasterBoardData_V3_0__1> URStateParser_V3_0__1;
+typedef URStateParser<RobotModeData_V3_2, MasterBoardData_V3_2> URStateParser_V3_2;
+typedef URStateParser<RobotModeData_V3_5, MasterBoardData_V3_2> URStateParser_V3_5;

include/ur_rtde_driver/ur/stream.h

@@ -0,0 +1,64 @@
+/*
+ * 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/tcp_socket.h"
+
+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);
+};