diff --git a/.clang-format b/.clang-format
index 95f7eb5..e03f80b 100644
--- a/.clang-format
+++ b/.clang-format
@@ -1,95 +1,48 @@
 ---
-Language:        Cpp
-# BasedOnStyle:  WebKit
-AccessModifierOffset: -4
-AlignAfterOpenBracket: DontAlign
-AlignConsecutiveAssignments: false
-AlignConsecutiveDeclarations: false
+BasedOnStyle:  Google
+AccessModifierOffset: -2
+ConstructorInitializerIndentWidth: 2
 AlignEscapedNewlinesLeft: false
-AlignOperands:   false
-AlignTrailingComments: false
-AllowAllParametersOfDeclarationOnNextLine: true
-AllowShortBlocksOnASingleLine: false
-AllowShortCaseLabelsOnASingleLine: false
-AllowShortFunctionsOnASingleLine: All
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: false
 AllowShortIfStatementsOnASingleLine: false
 AllowShortLoopsOnASingleLine: false
-AlwaysBreakAfterDefinitionReturnType: None
-AlwaysBreakAfterReturnType: None
+AllowShortFunctionsOnASingleLine: None
+AllowShortLoopsOnASingleLine: false
+AlwaysBreakTemplateDeclarations: true
 AlwaysBreakBeforeMultilineStrings: false
-AlwaysBreakTemplateDeclarations: false
-BinPackArguments: true
-BinPackParameters: true
-BraceWrapping:   
-  AfterClass:      false
-  AfterControlStatement: false
-  AfterEnum:       false
-  AfterFunction:   true
-  AfterNamespace:  false
-  AfterObjCDeclaration: false
-  AfterStruct:     false
-  AfterUnion:      false
-  BeforeCatch:     false
-  BeforeElse:      false
-  IndentBraces:    false
-BreakBeforeBinaryOperators: All
-BreakBeforeBraces: WebKit
-BreakBeforeTernaryOperators: true
+BreakBeforeBinaryOperators: false
+BreakBeforeTernaryOperators: false
 BreakConstructorInitializersBeforeComma: true
-BreakAfterJavaFieldAnnotations: false
-BreakStringLiterals: true
-ColumnLimit:     0
-CommentPragmas:  '^ IWYU pragma:'
-ConstructorInitializerAllOnOneLineOrOnePerLine: false
-ConstructorInitializerIndentWidth: 4
-ContinuationIndentWidth: 4
-Cpp11BracedListStyle: false
-DerivePointerAlignment: false
-DisableFormat:   false
+BinPackParameters: true
+ColumnLimit:    120
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+DerivePointerBinding: true
 ExperimentalAutoDetectBinPacking: false
-ForEachMacros:   [ foreach, Q_FOREACH, BOOST_FOREACH ]
-IncludeCategories: 
-  - Regex:           '^"(llvm|llvm-c|clang|clang-c)/'
-    Priority:        2
-  - Regex:           '^(<|"(gtest|isl|json)/)'
-    Priority:        3
-  - Regex:           '.*'
-    Priority:        1
-IncludeIsMainRegex: '$'
-IndentCaseLabels: false
-IndentWidth:     4
-IndentWrappedFunctionNames: false
-JavaScriptQuotes: Leave
-JavaScriptWrapImports: true
-KeepEmptyLinesAtTheStartOfBlocks: true
-MacroBlockBegin: ''
-MacroBlockEnd:   ''
+IndentCaseLabels: true
 MaxEmptyLinesToKeep: 1
-NamespaceIndentation: Inner
-ObjCBlockIndentWidth: 4
-ObjCSpaceAfterProperty: true
+NamespaceIndentation: None
 ObjCSpaceBeforeProtocolList: true
 PenaltyBreakBeforeFirstCallParameter: 19
-PenaltyBreakComment: 300
-PenaltyBreakFirstLessLess: 120
-PenaltyBreakString: 1000
-PenaltyExcessCharacter: 1000000
-PenaltyReturnTypeOnItsOwnLine: 60
-PointerAlignment: Left
-ReflowComments:  true
-SortIncludes:    true
-SpaceAfterCStyleCast: false
-SpaceBeforeAssignmentOperators: true
-SpaceBeforeParens: ControlStatements
-SpaceInEmptyParentheses: false
-SpacesBeforeTrailingComments: 1
-SpacesInAngles:  false
-SpacesInContainerLiterals: true
-SpacesInCStyleCastParentheses: false
-SpacesInParentheses: false
-SpacesInSquareBrackets: false
-Standard:        Cpp03
-TabWidth:        8
+PenaltyBreakComment: 60
+PenaltyBreakString: 1
+PenaltyBreakFirstLessLess: 1000
+PenaltyExcessCharacter: 1000
+PenaltyReturnTypeOnItsOwnLine: 90
+PointerBindsToType: false
+SpacesBeforeTrailingComments: 2
+Cpp11BracedListStyle: false
+Standard:        Auto
+IndentWidth:     2
+TabWidth:        2
 UseTab:          Never
+BreakBeforeBraces: Allman
+IndentFunctionDeclarationAfterType: false
+SpacesInParentheses: false
+SpacesInAngles:  false
+SpaceInEmptyParentheses: false
+SpacesInCStyleCastParentheses: false
+SpaceAfterControlStatementKeyword: true
+SpaceBeforeAssignmentOperators: true
+ContinuationIndentWidth: 4
 ...
-
diff --git a/include/ur_modern_driver/bin_parser.h b/include/ur_modern_driver/bin_parser.h
index 2bc102e..a4f9372 100644
--- a/include/ur_modern_driver/bin_parser.h
+++ b/include/ur_modern_driver/bin_parser.h
@@ -1,178 +1,175 @@
 #pragma once
 
-#include "ur_modern_driver/log.h"
-#include "ur_modern_driver/types.h"
 #include <assert.h>
-#include <cstddef>
-#include <cstring>
 #include <endian.h>
 #include <inttypes.h>
+#include <cstddef>
+#include <cstring>
 #include <string>
+#include "ur_modern_driver/log.h"
+#include "ur_modern_driver/types.h"
 
-class BinParser {
+class BinParser
+{
 private:
-    uint8_t *_buf_pos, *_buf_end;
-    BinParser& _parent;
+  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(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(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;
-    }
+  ~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);
-    }
+  // 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 <= _buf_end);
-        T val;
-        std::memcpy(&val, _buf_pos, sizeof(T));
-        return decode(val);
-    }
+  template <typename T>
+  T peek()
+  {
+    assert(buf_pos_ <= 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);
-    }
+  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));
-    }
+  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;
-    }
+  // 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(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);
-    }
+  // 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_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;
-    }
+  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));
-    }
+  // 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(T (&array)[N])
+  template <typename T, size_t N>
+  void parse(T (&array)[N])
+  {
+    for (size_t i = 0; i < N; i++)
     {
-        for (size_t i = 0; i < N; i++) {
-            parse(array[i]);
-        }
+      parse(array[i]);
     }
+  }
 
-    void consume()
-    {
-        _buf_pos = _buf_end;
-    }
-    void consume(size_t bytes)
-    {
-        _buf_pos += bytes;
-    }
+  void consume()
+  {
+    buf_pos_ = buf_end_;
+  }
+  void consume(size_t bytes)
+  {
+    buf_pos_ += bytes;
+  }
 
-    bool check_size(size_t bytes)
-    {
-        return bytes <= size_t(_buf_end - _buf_pos);
-    }
-    template <typename T>
-    bool check_size(void)
-    {
-        return check_size(T::SIZE);
-    }
+  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;
-    }
+  bool empty()
+  {
+    return buf_pos_ == buf_end_;
+  }
 
-    void debug()
-    {
-        LOG_DEBUG("BinParser: %p - %p (%zu bytes)", _buf_pos, _buf_end, _buf_end - _buf_pos);
-    }
+  void debug()
+  {
+    LOG_DEBUG("BinParser: %p - %p (%zu bytes)", buf_pos_, buf_end_, buf_end_ - buf_pos_);
+  }
 };
diff --git a/include/ur_modern_driver/packet.h b/include/ur_modern_driver/packet.h
index 541e3e5..521d75b 100644
--- a/include/ur_modern_driver/packet.h
+++ b/include/ur_modern_driver/packet.h
@@ -1,7 +1,8 @@
 #pragma once
 #include "ur_modern_driver/bin_parser.h"
 
-class Packet {
+class Packet
+{
 public:
-    virtual bool parse_with(BinParser& bp) = 0;
+  virtual bool parseWith(BinParser& bp) = 0;
 };
\ No newline at end of file
diff --git a/include/ur_modern_driver/parser.h b/include/ur_modern_driver/parser.h
index 9997515..e824c57 100644
--- a/include/ur_modern_driver/parser.h
+++ b/include/ur_modern_driver/parser.h
@@ -2,7 +2,8 @@
 #include "ur_modern_driver/bin_parser.h"
 #include "ur_modern_driver/packet.h"
 
-class Parser {
+class Parser
+{
 public:
-    virtual std::unique_ptr<Packet> parse(BinParser& bp) = 0;
+  virtual std::unique_ptr<Packet> parse(BinParser& bp) = 0;
 };
diff --git a/include/ur_modern_driver/pipeline.h b/include/ur_modern_driver/pipeline.h
index 199345a..49b1c9e 100644
--- a/include/ur_modern_driver/pipeline.h
+++ b/include/ur_modern_driver/pipeline.h
@@ -1,116 +1,134 @@
 #pragma once
 
-#include "ur_modern_driver/log.h"
-#include "ur_modern_driver/queue/readerwriterqueue.h"
 #include <atomic>
 #include <thread>
 #include <vector>
+#include "ur_modern_driver/log.h"
+#include "ur_modern_driver/queue/readerwriterqueue.h"
 
 using namespace moodycamel;
 using namespace std;
 
 template <typename T>
-class IConsumer {
+class IConsumer
+{
 public:
-    virtual void setup_consumer() {}
-    virtual void teardown_consumer() {}
-    virtual void stop_consumer() {}
+  virtual void setupConsumer()
+  {
+  }
+  virtual void teardownConsumer()
+  {
+  }
+  virtual void stopConsumer()
+  {
+  }
 
-    virtual bool consume(unique_ptr<T> product) = 0;
+  virtual bool consume(unique_ptr<T> product) = 0;
 };
 
 template <typename T>
-class IProducer {
+class IProducer
+{
 public:
-    virtual void setup_producer() {}
-    virtual void teardown_producer() {}
-    virtual void stop_producer() {}
+  virtual void setupProducer()
+  {
+  }
+  virtual void teardownProducer()
+  {
+  }
+  virtual void stopProducer()
+  {
+  }
 
-    virtual bool try_get(std::vector<unique_ptr<T> >& products) = 0;
+  virtual bool tryGet(std::vector<unique_ptr<T>>& products) = 0;
 };
 
 template <typename T>
-class Pipeline {
+class Pipeline
+{
 private:
-    IProducer<T>& _producer;
-    IConsumer<T>& _consumer;
-    BlockingReaderWriterQueue<unique_ptr<T> > _queue;
-    atomic<bool> _running;
-    thread _pThread, _cThread;
+  IProducer<T>& producer_;
+  IConsumer<T>& consumer_;
+  BlockingReaderWriterQueue<unique_ptr<T>> queue_;
+  atomic<bool> running_;
+  thread pThread_, cThread_;
 
-    void run_producer()
+  void run_producer()
+  {
+    producer_.setupProducer();
+    std::vector<unique_ptr<T>> products;
+    while (running_)
     {
-        _producer.setup_producer();
-        std::vector<unique_ptr<T> > products;
-        while (_running) {
-            if (!_producer.try_get(products)) {
-                break;
-            }
+      if (!producer_.tryGet(products))
+      {
+        break;
+      }
 
-            for (auto& p : products) {
-                if (!_queue.try_enqueue(std::move(p))) {
-                    LOG_ERROR("Pipeline producer owerflowed!");
-                }
-            }
-
-            products.clear();
+      for (auto& p : products)
+      {
+        if (!queue_.try_enqueue(std::move(p)))
+        {
+          LOG_ERROR("Pipeline producer owerflowed!");
         }
-        _producer.teardown_producer();
-        LOG_DEBUG("Pipline producer ended");
-        _consumer.stop_consumer();
-    }
+      }
 
-    void run_consumer()
+      products.clear();
+    }
+    producer_.teardownProducer();
+    LOG_DEBUG("Pipline producer ended");
+    consumer_.stopConsumer();
+  }
+
+  void run_consumer()
+  {
+    consumer_.setupConsumer();
+    unique_ptr<T> product;
+    while (running_)
     {
-        _consumer.setup_consumer();
-        unique_ptr<T> product;
-        while (_running) {
-            // 16000us timeout was chosen because we should
-            // roughly recieve messages at 125hz which is every
-            // 8ms == 8000us and double it for some error margin
-            if (!_queue.wait_dequeue_timed(product, 16000)) {
-                continue;
-            }
-            if (!_consumer.consume(std::move(product)))
-                break;
-        }
-        _consumer.teardown_consumer();
-        LOG_DEBUG("Pipline consumer ended");
-        _producer.stop_producer();
+      // 16000us timeout was chosen because we should
+      // roughly recieve messages at 125hz which is every
+      // 8ms == 8000us and double it for some error margin
+      if (!queue_.wait_dequeue_timed(product, 16000))
+      {
+        continue;
+      }
+      if (!consumer_.consume(std::move(product)))
+        break;
     }
+    consumer_.teardownConsumer();
+    LOG_DEBUG("Pipline consumer ended");
+    producer_.stopProducer();
+  }
 
 public:
-    Pipeline(IProducer<T>& producer, IConsumer<T>& consumer)
-        : _producer(producer)
-        , _consumer(consumer)
-        , _queue{ 32 }
-        , _running{ false }
-    {
-    }
+  Pipeline(IProducer<T>& producer, IConsumer<T>& consumer)
+    : producer_(producer), consumer_(consumer), queue_{ 32 }, running_{ false }
+  {
+  }
 
-    void run()
-    {
-        if (_running)
-            return;
+  void run()
+  {
+    if (running_)
+      return;
 
-        _running = true;
-        _pThread = thread(&Pipeline::run_producer, this);
-        _cThread = thread(&Pipeline::run_consumer, this);
-    }
+    running_ = true;
+    pThread_ = thread(&Pipeline::run_producer, this);
+    cThread_ = thread(&Pipeline::run_consumer, this);
+  }
 
-    void stop()
-    {
-        if (!_running)
-            return;
+  void stop()
+  {
+    if (!running_)
+      return;
 
-        LOG_DEBUG("Stopping pipeline");
+    LOG_DEBUG("Stopping pipeline");
 
-        _consumer.stop_consumer();
-        _producer.stop_producer();
+    consumer_.stopConsumer();
+    producer_.stopProducer();
 
-        _running = false;
+    running_ = false;
 
-        _pThread.join();
-        _cThread.join();
-    }
+    pThread_.join();
+    cThread_.join();
+  }
 };
\ No newline at end of file
diff --git a/include/ur_modern_driver/queue/atomicops.h b/include/ur_modern_driver/queue/atomicops.h
index af0089e..8cbf463 100644
--- a/include/ur_modern_driver/queue/atomicops.h
+++ b/include/ur_modern_driver/queue/atomicops.h
@@ -63,21 +63,22 @@
 
 // Portable atomic fences implemented below:
 
-namespace moodycamel {
+namespace moodycamel
+{
+enum memory_order
+{
+  memory_order_relaxed,
+  memory_order_acquire,
+  memory_order_release,
+  memory_order_acq_rel,
+  memory_order_seq_cst,
 
-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
+  // memory_order_sync: Forces a full sync:
+  // #LoadLoad, #LoadStore, #StoreStore, and most significantly, #StoreLoad
+  memory_order_sync = memory_order_seq_cst
 };
 
-} // end namespace moodycamel
+}  // 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
@@ -98,34 +99,36 @@ enum memory_order {
 
 #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`
+#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 {
-
+namespace moodycamel
+{
 AE_FORCEINLINE void compiler_fence(memory_order order)
 {
-    switch (order) {
+  switch (order)
+  {
     case memory_order_relaxed:
-        break;
+      break;
     case memory_order_acquire:
-        _ReadBarrier();
-        break;
+      _ReadBarrier();
+      break;
     case memory_order_release:
-        _WriteBarrier();
-        break;
+      _WriteBarrier();
+      break;
     case memory_order_acq_rel:
-        _ReadWriteBarrier();
-        break;
+      _ReadWriteBarrier();
+      break;
     case memory_order_seq_cst:
-        _ReadWriteBarrier();
-        break;
+      _ReadWriteBarrier();
+      break;
     default:
-        assert(false);
-    }
+      assert(false);
+  }
 }
 
 // x86/x64 have a strong memory model -- all loads and stores have
@@ -134,111 +137,115 @@ AE_FORCEINLINE void compiler_fence(memory_order order)
 #if defined(AE_ARCH_X86) || defined(AE_ARCH_X64)
 AE_FORCEINLINE void fence(memory_order order)
 {
-    switch (order) {
+  switch (order)
+  {
     case memory_order_relaxed:
-        break;
+      break;
     case memory_order_acquire:
-        _ReadBarrier();
-        break;
+      _ReadBarrier();
+      break;
     case memory_order_release:
-        _WriteBarrier();
-        break;
+      _WriteBarrier();
+      break;
     case memory_order_acq_rel:
-        _ReadWriteBarrier();
-        break;
+      _ReadWriteBarrier();
+      break;
     case memory_order_seq_cst:
-        _ReadWriteBarrier();
-        AeFullSync();
-        _ReadWriteBarrier();
-        break;
+      _ReadWriteBarrier();
+      AeFullSync();
+      _ReadWriteBarrier();
+      break;
     default:
-        assert(false);
-    }
+      assert(false);
+  }
 }
 #else
 AE_FORCEINLINE void fence(memory_order order)
 {
-    // Non-specialized arch, use heavier memory barriers everywhere just in case :-(
-    switch (order) {
+  // Non-specialized arch, use heavier memory barriers everywhere just in case :-(
+  switch (order)
+  {
     case memory_order_relaxed:
-        break;
+      break;
     case memory_order_acquire:
-        _ReadBarrier();
-        AeLiteSync();
-        _ReadBarrier();
-        break;
+      _ReadBarrier();
+      AeLiteSync();
+      _ReadBarrier();
+      break;
     case memory_order_release:
-        _WriteBarrier();
-        AeLiteSync();
-        _WriteBarrier();
-        break;
+      _WriteBarrier();
+      AeLiteSync();
+      _WriteBarrier();
+      break;
     case memory_order_acq_rel:
-        _ReadWriteBarrier();
-        AeLiteSync();
-        _ReadWriteBarrier();
-        break;
+      _ReadWriteBarrier();
+      AeLiteSync();
+      _ReadWriteBarrier();
+      break;
     case memory_order_seq_cst:
-        _ReadWriteBarrier();
-        AeFullSync();
-        _ReadWriteBarrier();
-        break;
+      _ReadWriteBarrier();
+      AeFullSync();
+      _ReadWriteBarrier();
+      break;
     default:
-        assert(false);
-    }
+      assert(false);
+  }
 }
 #endif
-} // end namespace moodycamel
+}  // end namespace moodycamel
 #else
 // Use standard library of atomics
 #include <atomic>
 
-namespace moodycamel {
-
+namespace moodycamel
+{
 AE_FORCEINLINE void compiler_fence(memory_order order)
 {
-    switch (order) {
+  switch (order)
+  {
     case memory_order_relaxed:
-        break;
+      break;
     case memory_order_acquire:
-        std::atomic_signal_fence(std::memory_order_acquire);
-        break;
+      std::atomic_signal_fence(std::memory_order_acquire);
+      break;
     case memory_order_release:
-        std::atomic_signal_fence(std::memory_order_release);
-        break;
+      std::atomic_signal_fence(std::memory_order_release);
+      break;
     case memory_order_acq_rel:
-        std::atomic_signal_fence(std::memory_order_acq_rel);
-        break;
+      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;
+      std::atomic_signal_fence(std::memory_order_seq_cst);
+      break;
     default:
-        assert(false);
-    }
+      assert(false);
+  }
 }
 
 AE_FORCEINLINE void fence(memory_order order)
 {
-    switch (order) {
+  switch (order)
+  {
     case memory_order_relaxed:
-        break;
+      break;
     case memory_order_acquire:
-        std::atomic_thread_fence(std::memory_order_acquire);
-        break;
+      std::atomic_thread_fence(std::memory_order_acquire);
+      break;
     case memory_order_release:
-        std::atomic_thread_fence(std::memory_order_release);
-        break;
+      std::atomic_thread_fence(std::memory_order_release);
+      break;
     case memory_order_acq_rel:
-        std::atomic_thread_fence(std::memory_order_acq_rel);
-        break;
+      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;
+      std::atomic_thread_fence(std::memory_order_seq_cst);
+      break;
     default:
-        assert(false);
-    }
+      assert(false);
+  }
 }
 
-} // end namespace moodycamel
+}  // end namespace moodycamel
 
 #endif
 
@@ -255,127 +262,133 @@ AE_FORCEINLINE void fence(memory_order order)
 // 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 {
+namespace moodycamel
+{
 template <typename T>
-class weak_atomic {
+class weak_atomic
+{
 public:
-    weak_atomic() {}
+  weak_atomic()
+  {
+  }
 #ifdef AE_VCPP
-#pragma warning(disable : 4100) // Get rid of (erroneous) 'unreferenced formal parameter' warning
+#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))
-    {
-    }
+  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)
-    {
-    }
+  // 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))
-    {
-    }
+  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();
-    }
+  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;
-    }
+  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 load() const
+  {
+    return value;
+  }
 
-    AE_FORCEINLINE T fetch_add_acquire(T increment)
-    {
+  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 (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);
+    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;
-    }
+    assert(false && "T must be either a 32 or 64 bit type");
+    return value;
+  }
 
-    AE_FORCEINLINE T fetch_add_release(T increment)
-    {
+  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 (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);
+    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;
-    }
+    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;
-    }
+  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 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 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_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);
-    }
+  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;
+  // 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;
+  std::atomic<T> value;
 #endif
 };
 
-} // end namespace moodycamel
+}  // end namespace moodycamel
 
 // Portable single-producer, single-consumer semaphore below:
 
@@ -387,7 +400,8 @@ private:
 // 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) 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);
@@ -398,7 +412,8 @@ __declspec(dllimport) int __stdcall ReleaseSemaphore(void* hSemaphore, long lRel
 #include <semaphore.h>
 #endif
 
-namespace moodycamel {
+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
@@ -420,283 +435,300 @@ namespace moodycamel {
 // 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 {
+namespace spsc_sema
+{
 #if defined(_WIN32)
-    class Semaphore {
-    private:
-        void* m_hSema;
+class Semaphore
+{
+private:
+  void* m_hSema;
 
-        Semaphore(const Semaphore& other);
-        Semaphore& operator=(const Semaphore& other);
+  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);
-        }
+public:
+  Semaphore(int initialCount = 0)
+  {
+    assert(initialCount >= 0);
+    const long maxLong = 0x7fffffff;
+    m_hSema = CreateSemaphoreW(nullptr, initialCount, maxLong, nullptr);
+  }
 
-        ~Semaphore()
-        {
-            CloseHandle(m_hSema);
-        }
+  ~Semaphore()
+  {
+    CloseHandle(m_hSema);
+  }
 
-        void wait()
-        {
-            const unsigned long infinite = 0xffffffff;
-            WaitForSingleObject(m_hSema, infinite);
-        }
+  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 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;
-        }
+  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);
-        }
-    };
+  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 (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);
+  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);
-        }
+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);
-        }
+  ~Semaphore()
+  {
+    semaphore_destroy(mach_task_self(), m_sema);
+  }
 
-        void wait()
-        {
-            semaphore_wait(m_sema);
-        }
+  void wait()
+  {
+    semaphore_wait(m_sema);
+  }
 
-        bool try_wait()
-        {
-            return timed_wait(0);
-        }
+  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;
+  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);
+    // 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;
-        }
+    return rc != KERN_OPERATION_TIMED_OUT;
+  }
 
-        void signal()
-        {
-            semaphore_signal(m_sema);
-        }
+  void signal()
+  {
+    semaphore_signal(m_sema);
+  }
 
-        void signal(int count)
-        {
-            while (count-- > 0) {
-                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 (POSIX, Linux)
+//---------------------------------------------------------
+class Semaphore
+{
+private:
+  sem_t m_sema;
 
-        Semaphore(const Semaphore& other);
-        Semaphore& operator=(const Semaphore& other);
+  Semaphore(const Semaphore& other);
+  Semaphore& operator=(const Semaphore& other);
 
-    public:
-        Semaphore(int initialCount = 0)
-        {
-            assert(initialCount >= 0);
-            sem_init(&m_sema, 0, initialCount);
-        }
+public:
+  Semaphore(int initialCount = 0)
+  {
+    assert(initialCount >= 0);
+    sem_init(&m_sema, 0, initialCount);
+  }
 
-        ~Semaphore()
-        {
-            sem_destroy(&m_sema);
-        }
+  ~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);
-        }
+  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 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;
-            }
+  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);
-        }
+    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()
+  {
+    sem_post(&m_sema);
+  }
 
-        void signal(int count)
-        {
-            while (count-- > 0) {
-                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;
+//---------------------------------------------------------
+// LightweightSemaphore
+//---------------------------------------------------------
+class LightweightSemaphore
+{
+public:
+  typedef std::make_signed<std::size_t>::type ssize_t;
 
-    private:
-        weak_atomic<ssize_t> m_count;
-        Semaphore m_sema;
+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;
-            }
-        }
+  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);
-        }
+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;
-        }
+  bool tryWait()
+  {
+    if (m_count.load() > 0)
+    {
+      m_count.fetch_add_acquire(-1);
+      return true;
+    }
+    return false;
+  }
 
-        void wait()
-        {
-            if (!tryWait())
-                waitWithPartialSpinning();
-        }
+  void wait()
+  {
+    if (!tryWait())
+      waitWithPartialSpinning();
+  }
 
-        bool wait(std::int64_t timeout_usecs)
-        {
-            return tryWait() || waitWithPartialSpinning(timeout_usecs);
-        }
+  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);
-            }
-        }
+  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
+  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)
diff --git a/include/ur_modern_driver/queue/readerwriterqueue.h b/include/ur_modern_driver/queue/readerwriterqueue.h
index 1503dcb..fdfcdf8 100644
--- a/include/ur_modern_driver/queue/readerwriterqueue.h
+++ b/include/ur_modern_driver/queue/readerwriterqueue.h
@@ -4,15 +4,15 @@
 
 #pragma once
 
-#include "atomicops.h"
 #include <cassert>
 #include <cstdint>
-#include <cstdlib> // For malloc/free/abort & size_t
+#include <cstdlib>  // For malloc/free/abort & size_t
 #include <new>
 #include <stdexcept>
 #include <type_traits>
 #include <utility>
-#if __cplusplus > 199711L || _MSC_VER >= 1700 // C++11 or VS2012
+#include "atomicops.h"
+#if __cplusplus > 199711L || _MSC_VER >= 1700  // C++11 or VS2012
 #include <chrono>
 #endif
 
@@ -34,757 +34,830 @@
 #endif
 
 #ifndef MOODYCAMEL_EXCEPTIONS_ENABLED
-#if (defined(_MSC_VER) && defined(_CPPUNWIND)) || (defined(__GNUC__) && defined(__EXCEPTIONS)) || (!defined(_MSC_VER) && !defined(__GNUC__))
+#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
+#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 {
-
+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.
+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)
+  // 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)
+    : 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)
     {
-        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)
+      // 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)
         {
-            assert(!inSection && "ReaderWriterQueue does not support enqueuing or dequeuing elements from other elements' ctors and dtors");
-            inSection = true;
+#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;
 
-        ~ReentrantGuard() { inSection = false; }
+    // Make sure the reader/writer threads will have the initialized memory setup above:
+    fence(memory_order_sync);
+  }
 
-    private:
-        ReentrantGuard& operator=(ReentrantGuard const&);
+  // 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);
 
-    private:
-        bool& inSection;
-    };
+    // 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
 
-    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
+    // 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
 
-        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;
+    // 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.
 
-        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)
+    Block* frontBlock_ = frontBlock.load();
+    size_t blockTail = frontBlock_->localTail;
+    size_t blockFront = frontBlock_->front.load();
 
-        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)
+    if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load()))
     {
-        // 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;
-        }
+      fence(memory_order_acquire);
 
-        auto newBlockAligned = align_for<Block>(newBlockRaw);
-        auto newBlockData = align_for<T>(newBlockAligned + sizeof(Block));
-        return new (newBlockAligned) Block(capacity, newBlockRaw, newBlockData);
+    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:
-    weak_atomic<Block*> frontBlock; // (Atomic) Elements are enqueued to this block
+  enum AllocationMode
+  {
+    CanAlloc,
+    CannotAlloc
+  };
 
-    char cachelineFiller[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic<Block*>)];
-    weak_atomic<Block*> tailBlock; // (Atomic) Elements are dequeued from this block
+  template <AllocationMode canAlloc, typename U>
+  bool inner_enqueue(U&& element)
+  {
+#ifndef NDEBUG
+    ReentrantGuard guard(this->enqueuing);
+#endif
 
-    size_t largestBlockSize;
+    // 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;
+  bool enqueuing;
+  bool dequeuing;
 #endif
 };
 
 // Like ReaderWriterQueue, but also providees blocking operations
 template <typename T, size_t MAX_BLOCK_SIZE = 512>
-class BlockingReaderWriterQueue {
+class BlockingReaderWriterQueue
+{
 private:
-    typedef ::moodycamel::ReaderWriterQueue<T, MAX_BLOCK_SIZE> ReaderWriterQueue;
+  typedef ::moodycamel::ReaderWriterQueue<T, MAX_BLOCK_SIZE> ReaderWriterQueue;
 
 public:
-    explicit BlockingReaderWriterQueue(size_t maxSize = 15)
-        : inner(maxSize)
-    {
-    }
+  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)
+  // 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))
     {
-        if (inner.try_enqueue(element)) {
-            sema.signal();
-            return true;
-        }
-        return false;
+      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)
+  // 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)))
     {
-        if (inner.try_enqueue(std::forward<T>(element))) {
-            sema.signal();
-            return true;
-        }
-        return false;
+      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)
+  // 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))
     {
-        if (inner.enqueue(element)) {
-            sema.signal();
-            return true;
-        }
-        return false;
+      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)
+  // 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)))
     {
-        if (inner.enqueue(std::forward<T>(element))) {
-            sema.signal();
-            return true;
-        }
-        return false;
+      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)
+  // 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())
     {
-        if (sema.tryWait()) {
-            bool success = inner.try_dequeue(result);
-            assert(success);
-            AE_UNUSED(success);
-            return true;
-        }
-        return false;
+      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, 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)
+  // 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))
     {
-        if (!sema.wait(timeout_usecs)) {
-            return false;
-        }
-        bool success = inner.try_dequeue(result);
-        AE_UNUSED(result);
-        assert(success);
-        AE_UNUSED(success);
-        return true;
+      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());
-    }
+  // 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();
-    }
+  // 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()
+  // 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())
     {
-        if (sema.tryWait()) {
-            bool result = inner.pop();
-            assert(result);
-            AE_UNUSED(result);
-            return true;
-        }
-        return false;
+      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();
-    }
+  // 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&) {}
+  // Disable copying & assignment
+  BlockingReaderWriterQueue(ReaderWriterQueue const&)
+  {
+  }
+  BlockingReaderWriterQueue& operator=(ReaderWriterQueue const&)
+  {
+  }
 
 private:
-    ReaderWriterQueue inner;
-    spsc_sema::LightweightSemaphore sema;
+  ReaderWriterQueue inner;
+  spsc_sema::LightweightSemaphore sema;
 };
 
-} // end namespace moodycamel
+}  // end namespace moodycamel
 
 #ifdef AE_VCPP
 #pragma warning(pop)
diff --git a/include/ur_modern_driver/robot_state.h b/include/ur_modern_driver/robot_state.h
index a35da44..621926a 100644
--- a/include/ur_modern_driver/robot_state.h
+++ b/include/ur_modern_driver/robot_state.h
@@ -19,202 +19,215 @@
 #ifndef ROBOT_STATE_H_
 #define ROBOT_STATE_H_
 
-#include <condition_variable>
 #include <inttypes.h>
-#include <mutex>
 #include <netinet/in.h>
 #include <stdlib.h>
 #include <string.h>
+#include <condition_variable>
+#include <mutex>
 #include <vector>
 
-namespace message_types {
-enum message_type {
-    ROBOT_STATE = 16,
-    ROBOT_MESSAGE = 20,
-    PROGRAM_STATE_MESSAGE = 25
+namespace message_types
+{
+enum message_type
+{
+  ROBOT_STATE = 16,
+  ROBOT_MESSAGE = 20,
+  PROGRAM_STATE_MESSAGE = 25
 };
 }
 typedef message_types::message_type messageType;
 
-namespace package_types {
-enum package_type {
-    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
+namespace package_types
+{
+enum package_type
+{
+  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
 };
 }
 typedef package_types::package_type packageType;
 
-namespace robot_message_types {
-enum robot_message_type {
-    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
+namespace robot_message_types
+{
+enum robot_message_type
+{
+  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
 };
 }
 typedef robot_message_types::robot_message_type robotMessageType;
 
-namespace robot_state_type_v18 {
-enum robot_state_type {
-    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
+namespace robot_state_type_v18
+{
+enum robot_state_type
+{
+  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
 };
 }
 typedef robot_state_type_v18::robot_state_type robotStateTypeV18;
-namespace robot_state_type_v30 {
-enum robot_state_type {
-    ROBOT_MODE_DISCONNECTED = 0,
-    ROBOT_MODE_CONFIRM_SAFETY = 1,
-    ROBOT_MODE_BOOTING = 2,
-    ROBOT_MODE_POWER_OFF = 3,
-    ROBOT_MODE_POWER_ON = 4,
-    ROBOT_MODE_IDLE = 5,
-    ROBOT_MODE_BACKDRIVE = 6,
-    ROBOT_MODE_RUNNING = 7,
-    ROBOT_MODE_UPDATING_FIRMWARE = 8
+namespace robot_state_type_v30
+{
+enum robot_state_type
+{
+  ROBOT_MODE_DISCONNECTED = 0,
+  ROBOT_MODE_CONFIRM_SAFETY = 1,
+  ROBOT_MODE_BOOTING = 2,
+  ROBOT_MODE_POWER_OFF = 3,
+  ROBOT_MODE_POWER_ON = 4,
+  ROBOT_MODE_IDLE = 5,
+  ROBOT_MODE_BACKDRIVE = 6,
+  ROBOT_MODE_RUNNING = 7,
+  ROBOT_MODE_UPDATING_FIRMWARE = 8
 };
 }
 
 typedef robot_state_type_v30::robot_state_type robotStateTypeV30;
 
-struct version_message {
-    uint64_t timestamp;
-    int8_t source;
-    int8_t robot_message_type;
-    int8_t project_name_size;
-    char project_name[15];
-    uint8_t major_version;
-    uint8_t minor_version;
-    int svn_revision;
-    char build_date[25];
+struct version_message
+{
+  uint64_t timestamp;
+  int8_t source;
+  int8_t robot_message_type;
+  int8_t project_name_size;
+  char project_name[15];
+  uint8_t major_version;
+  uint8_t minor_version;
+  int svn_revision;
+  char build_date[25];
 };
 
-struct masterboard_data {
-    int digitalInputBits;
-    int digitalOutputBits;
-    char analogInputRange0;
-    char analogInputRange1;
-    double analogInput0;
-    double analogInput1;
-    char analogOutputDomain0;
-    char analogOutputDomain1;
-    double analogOutput0;
-    double analogOutput1;
-    float masterBoardTemperature;
-    float robotVoltage48V;
-    float robotCurrent;
-    float masterIOCurrent;
-    unsigned char safetyMode;
-    unsigned char masterOnOffState;
-    char euromap67InterfaceInstalled;
-    int euromapInputBits;
-    int euromapOutputBits;
-    float euromapVoltage;
-    float euromapCurrent;
+struct masterboard_data
+{
+  int digitalInputBits;
+  int digitalOutputBits;
+  char analogInputRange0;
+  char analogInputRange1;
+  double analogInput0;
+  double analogInput1;
+  char analogOutputDomain0;
+  char analogOutputDomain1;
+  double analogOutput0;
+  double analogOutput1;
+  float masterBoardTemperature;
+  float robotVoltage48V;
+  float robotCurrent;
+  float masterIOCurrent;
+  unsigned char safetyMode;
+  unsigned char masterOnOffState;
+  char euromap67InterfaceInstalled;
+  int euromapInputBits;
+  int euromapOutputBits;
+  float euromapVoltage;
+  float euromapCurrent;
 };
 
-struct robot_mode_data {
-    uint64_t timestamp;
-    bool isRobotConnected;
-    bool isRealRobotEnabled;
-    bool isPowerOnRobot;
-    bool isEmergencyStopped;
-    bool isProtectiveStopped;
-    bool isProgramRunning;
-    bool isProgramPaused;
-    unsigned char robotMode;
-    unsigned char controlMode;
-    double targetSpeedFraction;
-    double speedScaling;
+struct robot_mode_data
+{
+  uint64_t timestamp;
+  bool isRobotConnected;
+  bool isRealRobotEnabled;
+  bool isPowerOnRobot;
+  bool isEmergencyStopped;
+  bool isProtectiveStopped;
+  bool isProgramRunning;
+  bool isProgramPaused;
+  unsigned char robotMode;
+  unsigned char controlMode;
+  double targetSpeedFraction;
+  double speedScaling;
 };
 
-class RobotState {
+class RobotState
+{
 private:
-    version_message version_msg_;
-    masterboard_data mb_data_;
-    robot_mode_data robot_mode_;
+  version_message version_msg_;
+  masterboard_data mb_data_;
+  robot_mode_data robot_mode_;
 
-    std::recursive_mutex val_lock_; // Locks the variables while unpack parses data;
+  std::recursive_mutex val_lock_;  // Locks the variables while unpack parses data;
 
-    std::condition_variable* pMsg_cond_; //Signals that new vars are available
-    bool new_data_available_; //to avoid spurious wakes
-    unsigned char robot_mode_running_;
+  std::condition_variable* pMsg_cond_;  // Signals that new vars are available
+  bool new_data_available_;             // to avoid spurious wakes
+  unsigned char robot_mode_running_;
 
-    double ntohd(uint64_t nf);
+  double ntohd(uint64_t nf);
 
 public:
-    RobotState(std::condition_variable& msg_cond);
-    ~RobotState();
-    double getVersion();
-    double getTime();
-    std::vector<double> getQTarget();
-    int getDigitalInputBits();
-    int getDigitalOutputBits();
-    char getAnalogInputRange0();
-    char getAnalogInputRange1();
-    double getAnalogInput0();
-    double getAnalogInput1();
-    char getAnalogOutputDomain0();
-    char getAnalogOutputDomain1();
-    double getAnalogOutput0();
-    double getAnalogOutput1();
-    std::vector<double> getVActual();
-    float getMasterBoardTemperature();
-    float getRobotVoltage48V();
-    float getRobotCurrent();
-    float getMasterIOCurrent();
-    unsigned char getSafetyMode();
-    unsigned char getInReducedMode();
-    char getEuromap67InterfaceInstalled();
-    int getEuromapInputBits();
-    int getEuromapOutputBits();
-    float getEuromapVoltage();
-    float getEuromapCurrent();
+  RobotState(std::condition_variable& msg_cond);
+  ~RobotState();
+  double getVersion();
+  double getTime();
+  std::vector<double> getQTarget();
+  int getDigitalInputBits();
+  int getDigitalOutputBits();
+  char getAnalogInputRange0();
+  char getAnalogInputRange1();
+  double getAnalogInput0();
+  double getAnalogInput1();
+  char getAnalogOutputDomain0();
+  char getAnalogOutputDomain1();
+  double getAnalogOutput0();
+  double getAnalogOutput1();
+  std::vector<double> getVActual();
+  float getMasterBoardTemperature();
+  float getRobotVoltage48V();
+  float getRobotCurrent();
+  float getMasterIOCurrent();
+  unsigned char getSafetyMode();
+  unsigned char getInReducedMode();
+  char getEuromap67InterfaceInstalled();
+  int getEuromapInputBits();
+  int getEuromapOutputBits();
+  float getEuromapVoltage();
+  float getEuromapCurrent();
 
-    bool isRobotConnected();
-    bool isRealRobotEnabled();
-    bool isPowerOnRobot();
-    bool isEmergencyStopped();
-    bool isProtectiveStopped();
-    bool isProgramRunning();
-    bool isProgramPaused();
-    unsigned char getRobotMode();
-    bool isReady();
+  bool isRobotConnected();
+  bool isRealRobotEnabled();
+  bool isPowerOnRobot();
+  bool isEmergencyStopped();
+  bool isProtectiveStopped();
+  bool isProgramRunning();
+  bool isProgramPaused();
+  unsigned char getRobotMode();
+  bool isReady();
 
-    void setDisconnected();
+  void setDisconnected();
 
-    bool getNewDataAvailable();
-    void finishedReading();
+  bool getNewDataAvailable();
+  void finishedReading();
 
-    void unpack(uint8_t* buf, unsigned int buf_length);
-    void unpackRobotMessage(uint8_t* buf, unsigned int offset, uint32_t len);
-    void unpackRobotMessageVersion(uint8_t* buf, unsigned int offset,
-        uint32_t len);
-    void unpackRobotState(uint8_t* buf, unsigned int offset, uint32_t len);
-    void unpackRobotStateMasterboard(uint8_t* buf, unsigned int offset);
-    void unpackRobotMode(uint8_t* buf, unsigned int offset);
+  void unpack(uint8_t* buf, unsigned int buf_length);
+  void unpackRobotMessage(uint8_t* buf, unsigned int offset, uint32_t len);
+  void unpackRobotMessageVersion(uint8_t* buf, unsigned int offset, uint32_t len);
+  void unpackRobotState(uint8_t* buf, unsigned int offset, uint32_t len);
+  void unpackRobotStateMasterboard(uint8_t* buf, unsigned int offset);
+  void unpackRobotMode(uint8_t* buf, unsigned int offset);
 };
 
 #endif /* ROBOT_STATE_H_ */
diff --git a/include/ur_modern_driver/robot_state_RT.h b/include/ur_modern_driver/robot_state_RT.h
index 3d3b2e8..bca6fac 100644
--- a/include/ur_modern_driver/robot_state_RT.h
+++ b/include/ur_modern_driver/robot_state_RT.h
@@ -19,98 +19,101 @@
 #ifndef ROBOT_STATE_RT_H_
 #define ROBOT_STATE_RT_H_
 
-#include <condition_variable>
 #include <inttypes.h>
-#include <mutex>
 #include <netinet/in.h>
 #include <stdlib.h>
 #include <string.h>
+#include <condition_variable>
+#include <mutex>
 #include <vector>
 
-class RobotStateRT {
+class RobotStateRT
+{
 private:
-    double version_; //protocol version
+  double version_;  // protocol version
 
-    double time_; //Time elapsed since the controller was started
-    std::vector<double> q_target_; //Target joint positions
-    std::vector<double> qd_target_; //Target joint velocities
-    std::vector<double> qdd_target_; //Target joint accelerations
-    std::vector<double> i_target_; //Target joint currents
-    std::vector<double> m_target_; //Target joint moments (torques)
-    std::vector<double> q_actual_; //Actual joint positions
-    std::vector<double> qd_actual_; //Actual joint velocities
-    std::vector<double> i_actual_; //Actual joint currents
-    std::vector<double> i_control_; //Joint control currents
-    std::vector<double> tool_vector_actual_; //Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation
-    std::vector<double> tcp_speed_actual_; //Actual speed of the tool given in Cartesian coordinates
-    std::vector<double> tcp_force_; //Generalised forces in the TC
-    std::vector<double> tool_vector_target_; //Target Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation
-    std::vector<double> tcp_speed_target_; //Target speed of the tool given in Cartesian coordinates
-    std::vector<bool> digital_input_bits_; //Current state of the digital inputs. NOTE: these are bits encoded as int64_t, e.g. a value of 5 corresponds to bit 0 and bit 2 set high
-    std::vector<double> motor_temperatures_; //Temperature of each joint in degrees celsius
-    double controller_timer_; //Controller realtime thread execution time
-    double robot_mode_; //Robot mode
-    std::vector<double> joint_modes_; //Joint control modes
-    double safety_mode_; //Safety mode
-    std::vector<double> tool_accelerometer_values_; //Tool x,y and z accelerometer values (software version 1.7)
-    double speed_scaling_; //Speed scaling of the trajectory limiter
-    double linear_momentum_norm_; //Norm of Cartesian linear momentum
-    double v_main_; //Masterboard: Main voltage
-    double v_robot_; //Matorborad: Robot voltage (48V)
-    double i_robot_; //Masterboard: Robot current
-    std::vector<double> v_actual_; //Actual joint voltages
+  double time_;                             // Time elapsed since the controller was started
+  std::vector<double> q_target_;            // Target joint positions
+  std::vector<double> qd_target_;           // Target joint velocities
+  std::vector<double> qdd_target_;          // Target joint accelerations
+  std::vector<double> i_target_;            // Target joint currents
+  std::vector<double> m_target_;            // Target joint moments (torques)
+  std::vector<double> q_actual_;            // Actual joint positions
+  std::vector<double> qd_actual_;           // Actual joint velocities
+  std::vector<double> i_actual_;            // Actual joint currents
+  std::vector<double> i_control_;           // Joint control currents
+  std::vector<double> tool_vector_actual_;  // Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry
+                                            // and rz is a rotation vector representation of the tool orientation
+  std::vector<double> tcp_speed_actual_;    // Actual speed of the tool given in Cartesian coordinates
+  std::vector<double> tcp_force_;           // Generalised forces in the TC
+  std::vector<double> tool_vector_target_;  // Target Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry
+                                            // and rz is a rotation vector representation of the tool orientation
+  std::vector<double> tcp_speed_target_;    // Target speed of the tool given in Cartesian coordinates
+  std::vector<bool> digital_input_bits_;    // Current state of the digital inputs. NOTE: these are bits encoded as
+                                            // int64_t, e.g. a value of 5 corresponds to bit 0 and bit 2 set high
+  std::vector<double> motor_temperatures_;  // Temperature of each joint in degrees celsius
+  double controller_timer_;                 // Controller realtime thread execution time
+  double robot_mode_;                       // Robot mode
+  std::vector<double> joint_modes_;         // Joint control modes
+  double safety_mode_;                      // Safety mode
+  std::vector<double> tool_accelerometer_values_;  // Tool x,y and z accelerometer values (software version 1.7)
+  double speed_scaling_;                           // Speed scaling of the trajectory limiter
+  double linear_momentum_norm_;                    // Norm of Cartesian linear momentum
+  double v_main_;                                  // Masterboard: Main voltage
+  double v_robot_;                                 // Matorborad: Robot voltage (48V)
+  double i_robot_;                                 // Masterboard: Robot current
+  std::vector<double> v_actual_;                   // Actual joint voltages
 
-    std::mutex val_lock_; // Locks the variables while unpack parses data;
+  std::mutex val_lock_;  // Locks the variables while unpack parses data;
 
-    std::condition_variable* pMsg_cond_; //Signals that new vars are available
-    bool data_published_; //to avoid spurious wakes
-    bool controller_updated_; //to avoid spurious wakes
+  std::condition_variable* pMsg_cond_;  // Signals that new vars are available
+  bool data_published_;                 // to avoid spurious wakes
+  bool controller_updated_;             // to avoid spurious wakes
 
-    std::vector<double> unpackVector(uint8_t* buf, int start_index,
-        int nr_of_vals);
-    std::vector<bool> unpackDigitalInputBits(int64_t data);
-    double ntohd(uint64_t nf);
+  std::vector<double> unpackVector(uint8_t* buf, int start_index, int nr_of_vals);
+  std::vector<bool> unpackDigitalInputBits(int64_t data);
+  double ntohd(uint64_t nf);
 
 public:
-    RobotStateRT(std::condition_variable& msg_cond);
-    ~RobotStateRT();
-    double getVersion();
-    double getTime();
-    std::vector<double> getQTarget();
-    std::vector<double> getQdTarget();
-    std::vector<double> getQddTarget();
-    std::vector<double> getITarget();
-    std::vector<double> getMTarget();
-    std::vector<double> getQActual();
-    std::vector<double> getQdActual();
-    std::vector<double> getIActual();
-    std::vector<double> getIControl();
-    std::vector<double> getToolVectorActual();
-    std::vector<double> getTcpSpeedActual();
-    std::vector<double> getTcpForce();
-    std::vector<double> getToolVectorTarget();
-    std::vector<double> getTcpSpeedTarget();
-    std::vector<bool> getDigitalInputBits();
-    std::vector<double> getMotorTemperatures();
-    double getControllerTimer();
-    double getRobotMode();
-    std::vector<double> getJointModes();
-    double getSafety_mode();
-    std::vector<double> getToolAccelerometerValues();
-    double getSpeedScaling();
-    double getLinearMomentumNorm();
-    double getVMain();
-    double getVRobot();
-    double getIRobot();
+  RobotStateRT(std::condition_variable& msg_cond);
+  ~RobotStateRT();
+  double getVersion();
+  double getTime();
+  std::vector<double> getQTarget();
+  std::vector<double> getQdTarget();
+  std::vector<double> getQddTarget();
+  std::vector<double> getITarget();
+  std::vector<double> getMTarget();
+  std::vector<double> getQActual();
+  std::vector<double> getQdActual();
+  std::vector<double> getIActual();
+  std::vector<double> getIControl();
+  std::vector<double> getToolVectorActual();
+  std::vector<double> getTcpSpeedActual();
+  std::vector<double> getTcpForce();
+  std::vector<double> getToolVectorTarget();
+  std::vector<double> getTcpSpeedTarget();
+  std::vector<bool> getDigitalInputBits();
+  std::vector<double> getMotorTemperatures();
+  double getControllerTimer();
+  double getRobotMode();
+  std::vector<double> getJointModes();
+  double getSafety_mode();
+  std::vector<double> getToolAccelerometerValues();
+  double getSpeedScaling();
+  double getLinearMomentumNorm();
+  double getVMain();
+  double getVRobot();
+  double getIRobot();
 
-    void setVersion(double ver);
+  void setVersion(double ver);
 
-    void setDataPublished();
-    bool getDataPublished();
-    bool getControllerUpdated();
-    void setControllerUpdated();
-    std::vector<double> getVActual();
-    void unpack(uint8_t* buf);
+  void setDataPublished();
+  bool getDataPublished();
+  bool getControllerUpdated();
+  void setControllerUpdated();
+  std::vector<double> getVActual();
+  void unpack(uint8_t* buf);
 };
 
 #endif /* ROBOT_STATE_RT_H_ */
diff --git a/include/ur_modern_driver/ros/rt_publisher.h b/include/ur_modern_driver/ros/rt_publisher.h
index 0870526..a3fbb78 100644
--- a/include/ur_modern_driver/ros/rt_publisher.h
+++ b/include/ur_modern_driver/ros/rt_publisher.h
@@ -1,6 +1,5 @@
 #pragma once
 
-#include <cstdlib>
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/TwistStamped.h>
 #include <geometry_msgs/WrenchStamped.h>
@@ -9,6 +8,7 @@
 #include <sensor_msgs/Temperature.h>
 #include <tf/tf.h>
 #include <tf/transform_broadcaster.h>
+#include <cstdlib>
 #include <vector>
 
 #include "ur_modern_driver/ur/consumer.h"
@@ -16,55 +16,57 @@
 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"
-};
+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 {
+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;
+  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 publish_joints(RTShared& packet, Time& t);
-    bool publish_wrench(RTShared& packet, Time& t);
-    bool publish_tool(RTShared& packet, Time& t);
-    bool publish_transform(RTShared& packet, Time& t);
-    bool publish_temperature(RTShared& packet, Time& t);
+  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);
+  bool publish(RTShared& packet);
 
 public:
-    RTPublisher(std::string& joint_prefix, std::string& base_frame, std::string& tool_frame)
-        : _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)
+  RTPublisher(std::string& joint_prefix, std::string& base_frame, std::string& tool_frame)
+    : 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)
+  {
+    for (auto const& j : JOINTS)
     {
-        for (auto const& j : JOINTS) {
-            _joint_names.push_back(joint_prefix + j);
-        }
+      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 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 setup_consumer() {}
-    virtual void teardown_consumer() {}
-    virtual void stop_consumer() {}
+  virtual void setupConsumer()
+  {
+  }
+  virtual void teardownConsumer()
+  {
+  }
+  virtual void stopConsumer()
+  {
+  }
 };
\ No newline at end of file
diff --git a/include/ur_modern_driver/types.h b/include/ur_modern_driver/types.h
index 86d3a24..34cb905 100644
--- a/include/ur_modern_driver/types.h
+++ b/include/ur_modern_driver/types.h
@@ -2,21 +2,23 @@
 
 #include <inttypes.h>
 
-struct double3_t {
-    double x, y, z;
+struct double3_t
+{
+  double x, y, z;
 };
 
-struct cartesian_coord_t {
-    double3_t position;
-    double3_t rotation;
+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;
+  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;
+  return lhs.position == rhs.position && lhs.rotation == rhs.rotation;
 }
\ No newline at end of file
diff --git a/include/ur_modern_driver/ur/consumer.h b/include/ur_modern_driver/ur/consumer.h
index 655e8bb..991401a 100644
--- a/include/ur_modern_driver/ur/consumer.h
+++ b/include/ur_modern_driver/ur/consumer.h
@@ -7,41 +7,44 @@
 #include "ur_modern_driver/ur/rt_state.h"
 #include "ur_modern_driver/ur/state.h"
 
-class URRTPacketConsumer : public IConsumer<RTPacket> {
+class URRTPacketConsumer : public IConsumer<RTPacket>
+{
 public:
-    virtual bool consume(unique_ptr<RTPacket> packet)
-    {
-        return packet->consume_with(*this);
-    }
+  virtual bool consume(unique_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;
+  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> {
+class URStatePacketConsumer : public IConsumer<StatePacket>
+{
 public:
-    virtual bool consume(unique_ptr<StatePacket> packet)
-    {
-        return packet->consume_with(*this);
-    }
+  virtual bool consume(unique_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(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;
+  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> {
+class URMessagePacketConsumer : public IConsumer<MessagePacket>
+{
 public:
-    virtual bool consume(unique_ptr<MessagePacket> packet)
-    {
-        return packet->consume_with(*this);
-    }
+  virtual bool consume(unique_ptr<MessagePacket> packet)
+  {
+    return packet->consumeWith(*this);
+  }
 
-    virtual bool consume(VersionMessage& message) = 0;
+  virtual bool consume(VersionMessage& message) = 0;
 };
\ No newline at end of file
diff --git a/include/ur_modern_driver/ur/factory.h b/include/ur_modern_driver/ur/factory.h
index d3266ea..83d89ab 100644
--- a/include/ur_modern_driver/ur/factory.h
+++ b/include/ur_modern_driver/ur/factory.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include <cstdlib>
 #include "ur_modern_driver/ur/consumer.h"
 #include "ur_modern_driver/ur/messages_parser.h"
 #include "ur_modern_driver/ur/parser.h"
@@ -7,83 +8,97 @@
 #include "ur_modern_driver/ur/rt_parser.h"
 #include "ur_modern_driver/ur/state_parser.h"
 #include "ur_modern_driver/ur/stream.h"
-#include <cstdlib>
 
-class URFactory : private URMessagePacketConsumer {
+class URFactory : private URMessagePacketConsumer
+{
 private:
-    URStream _stream;
-    URMessageParser _parser;
+  URStream stream_;
+  URMessageParser parser_;
 
-    uint8_t _major_version;
-    uint8_t _minor_version;
+  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());
+  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;
+    major_version_ = vm.major_version;
+    minor_version_ = vm.minor_version;
 
-        return true;
-    }
+    return true;
+  }
 
-    void setup_consumer() {}
-    void teardown_consumer() {}
-    void stop_consumer() {}
+  void setupConsumer()
+  {
+  }
+  void teardownConsumer()
+  {
+  }
+  void stopConsumer()
+  {
+  }
 
 public:
-    URFactory(std::string& host)
-        : _stream(host, 30001)
+  URFactory(std::string& host) : stream_(host, 30001)
+  {
+    URProducer<MessagePacket> prod(stream_, parser_);
+    std::vector<unique_ptr<MessagePacket>> results;
+
+    prod.setupProducer();
+
+    if (!prod.tryGet(results) || results.size() == 0)
     {
-        URProducer<MessagePacket> p(_stream, _parser);
-        std::vector<unique_ptr<MessagePacket> > results;
-
-        p.setup_producer();
-
-        if (!p.try_get(results) || results.size() == 0) {
-            LOG_FATAL("No version message received, init failed!");
-            std::exit(EXIT_FAILURE);
-        }
-
-        for (auto const& p : results) {
-            p->consume_with(*this);
-        }
-
-        if (_major_version == 0 && _minor_version == 0) {
-            LOG_FATAL("No version message received, init failed!");
-            std::exit(EXIT_FAILURE);
-        }
-
-        p.teardown_producer();
+      LOG_FATAL("No version message received, init failed!");
+      std::exit(EXIT_FAILURE);
     }
 
-    std::unique_ptr<URParser<StatePacket> > get_state_parser()
+    for (auto const& p : results)
     {
-        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
-                return std::unique_ptr<URParser<StatePacket> >(new URStateParser_V3_2);
-        }
+      p->consumeWith(*this);
     }
 
-    std::unique_ptr<URParser<RTPacket> > get_rt_parser()
+    if (major_version_ == 0 && minor_version_ == 0)
     {
-        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);
-        }
+      LOG_FATAL("No version message received, init failed!");
+      std::exit(EXIT_FAILURE);
     }
+
+    prod.teardownProducer();
+  }
+
+  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
+        return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V3_2);
+    }
+  }
+
+  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);
+    }
+  }
 };
\ No newline at end of file
diff --git a/include/ur_modern_driver/ur/master_board.h b/include/ur_modern_driver/ur/master_board.h
index 5f23397..a0d9e7a 100644
--- a/include/ur_modern_driver/ur/master_board.h
+++ b/include/ur_modern_driver/ur/master_board.h
@@ -1,97 +1,91 @@
 #pragma once
 
+#include <inttypes.h>
+#include <cstddef>
 #include "ur_modern_driver/bin_parser.h"
 #include "ur_modern_driver/types.h"
 #include "ur_modern_driver/ur/state.h"
-#include <cstddef>
-#include <inttypes.h>
 
-class SharedMasterBoardData {
+class SharedMasterBoardData
+{
 public:
-    virtual bool parse_with(BinParser& bp);
+  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;
+  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;
+  bool euromap67_interface_installed;
 
-    //euromap fields are dynamic so don't include in SIZE
-    int32_t euromap_input_bits;
-    int32_t euromap_output_bits;
+  // 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 SIZE = sizeof(double) * 4 + sizeof(int8_t) * 4 + sizeof(float) * 4 + sizeof(uint8_t);
 
-    static const size_t EURO_SIZE = sizeof(int32_t) * 2;
+  static const size_t EURO_SIZE = sizeof(int32_t) * 2;
 };
 
-class MasterBoardData_V1_X : public SharedMasterBoardData, public StatePacket {
+class MasterBoardData_V1_X : public SharedMasterBoardData, public StatePacket
+{
 public:
-    virtual bool parse_with(BinParser& bp);
-    virtual bool consume_with(URStatePacketConsumer& consumer);
+  virtual bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URStatePacketConsumer& consumer);
 
-    int16_t digital_input_bits;
-    int16_t digital_output_bits;
+  int16_t digital_input_bits;
+  int16_t digital_output_bits;
 
-    uint8_t master_safety_state;
-    bool master_on_off_state;
+  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;
+  // 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 SIZE = SharedMasterBoardData::SIZE + sizeof(int16_t) * 2 + sizeof(uint8_t) * 2;
 
-    static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE
-        + sizeof(int16_t) * 2;
+  static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE + sizeof(int16_t) * 2;
 };
 
-class MasterBoardData_V3_0__1 : public SharedMasterBoardData, public StatePacket {
+class MasterBoardData_V3_0__1 : public SharedMasterBoardData, public StatePacket
+{
 public:
-    virtual bool parse_with(BinParser& bp);
-    virtual bool consume_with(URStatePacketConsumer& consumer);
+  virtual bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URStatePacketConsumer& consumer);
 
-    int32_t digital_input_bits;
-    int32_t digital_output_bits;
+  int32_t digital_input_bits;
+  int32_t digital_output_bits;
 
-    uint8_t safety_mode;
-    bool in_reduced_mode;
+  uint8_t safety_mode;
+  bool in_reduced_mode;
 
-    //euromap fields are dynamic so don't include in SIZE
-    float euromap_voltage;
-    float euromap_current;
+  // 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 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;
+  static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE + sizeof(float) * 2;
 };
 
-class MasterBoardData_V3_2 : public MasterBoardData_V3_0__1 {
+class MasterBoardData_V3_2 : public MasterBoardData_V3_0__1
+{
 public:
-    virtual bool parse_with(BinParser& bp);
-    virtual bool consume_with(URStatePacketConsumer& consumer);
+  virtual bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URStatePacketConsumer& consumer);
 
-    uint8_t operational_mode_selector_input;
-    uint8_t three_position_enabling_device_input;
+  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;
+  static const size_t SIZE = MasterBoardData_V3_0__1::SIZE + sizeof(uint8_t) * 2;
 };
diff --git a/include/ur_modern_driver/ur/messages.h b/include/ur_modern_driver/ur/messages.h
index dafad61..b693c1e 100644
--- a/include/ur_modern_driver/ur/messages.h
+++ b/include/ur_modern_driver/ur/messages.h
@@ -1,51 +1,51 @@
 #pragma once
 
+#include <inttypes.h>
+#include <cstddef>
 #include "ur_modern_driver/bin_parser.h"
 #include "ur_modern_driver/pipeline.h"
-#include <cstddef>
-#include <inttypes.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
+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 {
+class MessagePacket
+{
 public:
-    MessagePacket(uint64_t timestamp, uint8_t source)
-        : timestamp(timestamp)
-        , source(source)
-    {
-    }
-    virtual bool parse_with(BinParser& bp) = 0;
-    virtual bool consume_with(URMessagePacketConsumer& consumer) = 0;
+  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;
+  uint64_t timestamp;
+  uint8_t source;
 };
 
-class VersionMessage : public MessagePacket {
+class VersionMessage : public MessagePacket
+{
 public:
-    VersionMessage(uint64_t timestamp, uint8_t source)
-        : MessagePacket(timestamp, source)
-    {
-    }
+  VersionMessage(uint64_t timestamp, uint8_t source) : MessagePacket(timestamp, source)
+  {
+  }
 
-    virtual bool parse_with(BinParser& bp);
-    virtual bool consume_with(URMessagePacketConsumer& consumer);
+  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;
+  std::string project_name;
+  uint8_t major_version;
+  uint8_t minor_version;
+  int32_t svn_version;
+  std::string build_date;
 };
\ No newline at end of file
diff --git a/include/ur_modern_driver/ur/messages_parser.h b/include/ur_modern_driver/ur/messages_parser.h
index b0a781f..80d5806 100644
--- a/include/ur_modern_driver/ur/messages_parser.h
+++ b/include/ur_modern_driver/ur/messages_parser.h
@@ -1,49 +1,53 @@
 #pragma once
+#include <vector>
 #include "ur_modern_driver/bin_parser.h"
 #include "ur_modern_driver/log.h"
 #include "ur_modern_driver/pipeline.h"
 #include "ur_modern_driver/ur/messages.h"
 #include "ur_modern_driver/ur/parser.h"
-#include <vector>
 
-class URMessageParser : public URParser<MessagePacket> {
+class URMessageParser : public URParser<MessagePacket>
+{
 public:
-    bool parse(BinParser& bp, std::vector<unique_ptr<MessagePacket> >& results)
+  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)
     {
-        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->parse_with(bp);
-            result.reset(vm);
-            break;
-        }
-
-        default:
-            return false;
-        }
-
-        results.push_back(std::move(result));
-        return parsed;
+      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;
+  }
 };
\ No newline at end of file
diff --git a/include/ur_modern_driver/ur/parser.h b/include/ur_modern_driver/ur/parser.h
index ea53b40..4eb9026 100644
--- a/include/ur_modern_driver/ur/parser.h
+++ b/include/ur_modern_driver/ur/parser.h
@@ -1,10 +1,11 @@
 #pragma once
+#include <vector>
 #include "ur_modern_driver/bin_parser.h"
 #include "ur_modern_driver/pipeline.h"
-#include <vector>
 
 template <typename T>
-class URParser {
+class URParser
+{
 public:
-    virtual bool parse(BinParser& bp, std::vector<std::unique_ptr<T> >& results) = 0;
+  virtual bool parse(BinParser& bp, std::vector<std::unique_ptr<T>>& results) = 0;
 };
diff --git a/include/ur_modern_driver/ur/producer.h b/include/ur_modern_driver/ur/producer.h
index 7db11c9..5907f97 100644
--- a/include/ur_modern_driver/ur/producer.h
+++ b/include/ur_modern_driver/ur/producer.h
@@ -4,41 +4,52 @@
 #include "ur_modern_driver/ur/stream.h"
 
 template <typename T>
-class URProducer : public IProducer<T> {
+class URProducer : public IProducer<T>
+{
 private:
-    URStream& _stream;
-    URParser<T>& _parser;
+  URStream& stream_;
+  URParser<T>& parser_;
 
 public:
-    URProducer(URStream& stream, URParser<T>& parser)
-        : _stream(stream)
-        , _parser(parser)
+  URProducer(URStream& stream, URParser<T>& parser) : stream_(stream), parser_(parser)
+  {
+  }
+
+  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];
+
+    // blocking call
+    ssize_t len = stream_.receive(buf, sizeof(buf));
+
+    // LOG_DEBUG("Read %d bytes from stream", len);
+
+    if (len == 0)
     {
+      LOG_WARN("Read nothing from stream");
+      return false;
+    }
+    else if (len < 0)
+    {
+      LOG_WARN("Stream closed");
+      return false;
     }
 
-    void setup_producer() { _stream.connect(); }
-    void teardown_producer() { _stream.disconnect(); }
-    void stop_producer() { _stream.disconnect(); }
-
-    bool try_get(std::vector<unique_ptr<T> >& products)
-    {
-        //4KB should be enough to hold any packet received from UR
-        uint8_t buf[4096];
-
-        //blocking call
-        ssize_t len = _stream.receive(buf, sizeof(buf));
-
-        //LOG_DEBUG("Read %d bytes from stream", len);
-
-        if (len == 0) {
-            LOG_WARN("Read nothing from stream");
-            return false;
-        } else if (len < 0) {
-            LOG_WARN("Stream closed");
-            return false;
-        }
-
-        BinParser bp(buf, static_cast<size_t>(len));
-        return _parser.parse(bp, products);
-    }
+    BinParser bp(buf, static_cast<size_t>(len));
+    return parser_.parse(bp, products);
+  }
 };
\ No newline at end of file
diff --git a/include/ur_modern_driver/ur/robot_mode.h b/include/ur_modern_driver/ur/robot_mode.h
index b5307b9..90c9eb8 100644
--- a/include/ur_modern_driver/ur/robot_mode.h
+++ b/include/ur_modern_driver/ur/robot_mode.h
@@ -1,108 +1,107 @@
 #pragma once
 
+#include <inttypes.h>
+#include <cstddef>
 #include "ur_modern_driver/bin_parser.h"
 #include "ur_modern_driver/types.h"
 #include "ur_modern_driver/ur/state.h"
-#include <cstddef>
-#include <inttypes.h>
 
-class SharedRobotModeData {
+class SharedRobotModeData
+{
 public:
-    virtual bool parse_with(BinParser& bp);
+  virtual bool parseWith(BinParser& bp);
 
-    uint64_t timestamp;
-    bool physical_robot_connected;
-    bool real_robot_enabled;
-    bool robot_power_on;
-    bool emergency_stopped;
-    bool program_running;
-    bool program_paused;
+  uint64_t timestamp;
+  bool physical_robot_connected;
+  bool real_robot_enabled;
+  bool robot_power_on;
+  bool emergency_stopped;
+  bool program_running;
+  bool program_paused;
 
-    static const size_t SIZE = sizeof(uint64_t) + sizeof(uint8_t) * 6;
+  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
+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 {
+class RobotModeData_V1_X : public SharedRobotModeData, public StatePacket
+{
 public:
-    virtual bool parse_with(BinParser& bp);
-    virtual bool consume_with(URStatePacketConsumer& consumer);
+  virtual bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URStatePacketConsumer& consumer);
 
-    bool security_stopped;
-    robot_mode_V1_X robot_mode;
-    double speed_fraction;
+  bool security_stopped;
+  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 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");
+  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_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
+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 {
+class RobotModeData_V3_0__1 : public SharedRobotModeData, public StatePacket
+{
 public:
-    virtual bool parse_with(BinParser& bp);
-    virtual bool consume_with(URStatePacketConsumer& consumer);
+  virtual bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URStatePacketConsumer& consumer);
 
-    bool protective_stopped;
+  bool protective_stopped;
 
-    robot_mode_V3_X robot_mode;
-    robot_control_mode_V3_X control_mode;
+  robot_mode_V3_X robot_mode;
+  robot_control_mode_V3_X control_mode;
 
-    double target_speed_fraction;
-    double speed_scaling;
+  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 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");
+  static_assert(RobotModeData_V3_0__1::SIZE == 33, "RobotModeData_V3_0__1 has missmatched size");
 };
 
-class RobotModeData_V3_2 : public RobotModeData_V3_0__1 {
+class RobotModeData_V3_2 : public RobotModeData_V3_0__1
+{
 public:
-    virtual bool parse_with(BinParser& bp);
-    virtual bool consume_with(URStatePacketConsumer& consumer);
+  virtual bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URStatePacketConsumer& consumer);
 
-    double target_speed_fraction_limit;
+  double target_speed_fraction_limit;
 
-    static const size_t SIZE = RobotModeData_V3_0__1::SIZE
-        + sizeof(double);
+  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");
+  static_assert(RobotModeData_V3_2::SIZE == 41, "RobotModeData_V3_2 has missmatched size");
 };
\ No newline at end of file
diff --git a/include/ur_modern_driver/ur/rt_parser.h b/include/ur_modern_driver/ur/rt_parser.h
index e0cde0d..952cd56 100644
--- a/include/ur_modern_driver/ur/rt_parser.h
+++ b/include/ur_modern_driver/ur/rt_parser.h
@@ -1,33 +1,35 @@
 #pragma once
+#include <vector>
 #include "ur_modern_driver/bin_parser.h"
 #include "ur_modern_driver/log.h"
 #include "ur_modern_driver/pipeline.h"
 #include "ur_modern_driver/ur/parser.h"
 #include "ur_modern_driver/ur/rt_state.h"
-#include <vector>
 
 template <typename T>
-class URRTStateParser : public URParser<RTPacket> {
+class URRTStateParser : public URParser<RTPacket>
+{
 public:
-    bool parse(BinParser& bp, std::vector<std::unique_ptr<RTPacket> >& results)
+  bool parse(BinParser& bp, std::vector<std::unique_ptr<RTPacket>>& results)
+  {
+    int32_t packet_size = bp.peek<int32_t>();
+
+    if (!bp.checkSize(packet_size))
     {
-        int32_t packet_size = bp.peek<int32_t>();
-
-        if (!bp.check_size(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->parse_with(bp))
-            return false;
-
-        results.push_back(std::move(packet));
-
-        return true;
+      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;
diff --git a/include/ur_modern_driver/ur/rt_state.h b/include/ur_modern_driver/ur/rt_state.h
index ea2dd5e..bb4e9e9 100644
--- a/include/ur_modern_driver/ur/rt_state.h
+++ b/include/ur_modern_driver/ur/rt_state.h
@@ -1,122 +1,119 @@
 #pragma once
 
+#include <inttypes.h>
+#include <cstddef>
 #include "ur_modern_driver/bin_parser.h"
 #include "ur_modern_driver/pipeline.h"
 #include "ur_modern_driver/types.h"
-#include <cstddef>
-#include <inttypes.h>
 
 class URRTPacketConsumer;
 
-class RTPacket {
+class RTPacket
+{
 public:
-    virtual bool parse_with(BinParser& bp) = 0;
-    virtual bool consume_with(URRTPacketConsumer& consumer) = 0;
+  virtual bool parseWith(BinParser& bp) = 0;
+  virtual bool consumeWith(URRTPacketConsumer& consumer) = 0;
 };
 
-class RTShared {
+class RTShared
+{
 protected:
-    void parse_shared1(BinParser& bp);
-    void parse_shared2(BinParser& bp);
+  void parse_shared1(BinParser& bp);
+  void parse_shared2(BinParser& bp);
 
 public:
-    double time;
-    double q_target[6];
-    double qd_target[6];
-    double qdd_target[6];
-    double i_target[6];
-    double m_target[6];
-    double q_actual[6];
-    double qd_actual[6];
-    double i_actual[6];
+  double time;
+  double q_target[6];
+  double qd_target[6];
+  double qdd_target[6];
+  double i_target[6];
+  double m_target[6];
+  double q_actual[6];
+  double qd_actual[6];
+  double i_actual[6];
 
-    //gap here depending on version
+  // gap here depending on version
 
-    double tcp_force[6];
+  double tcp_force[6];
 
-    //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;
+  // 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
+  // gap here depending on version
 
-    uint64_t digital_inputs;
-    double motor_temperatures[6];
-    double controller_time;
-    double robot_mode;
+  uint64_t digital_inputs;
+  double motor_temperatures[6];
+  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);
+  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 {
+class RTState_V1_6__7 : public RTShared, public RTPacket
+{
 public:
-    bool parse_with(BinParser& bp);
-    virtual bool consume_with(URRTPacketConsumer& consumer);
+  bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URRTPacketConsumer& consumer);
 
-    double3_t tool_accelerometer_values;
+  double3_t tool_accelerometer_values;
 
-    static const size_t SIZE = RTShared::SIZE
-        + sizeof(double3_t);
+  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!");
+  static_assert(RTState_V1_6__7::SIZE == 632, "RTState_V1_6__7 has mismatched size!");
 };
 
-class RTState_V1_8 : public RTState_V1_6__7 {
+class RTState_V1_8 : public RTState_V1_6__7
+{
 public:
-    bool parse_with(BinParser& bp);
-    virtual bool consume_with(URRTPacketConsumer& consumer);
+  bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URRTPacketConsumer& consumer);
 
-    double joint_modes[6];
+  double joint_modes[6];
 
-    static const size_t SIZE = RTState_V1_6__7::SIZE
-        + sizeof(double[6]);
+  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!");
+  static_assert(RTState_V1_8::SIZE == 680, "RTState_V1_8 has mismatched size!");
 };
 
-class RTState_V3_0__1 : public RTShared, public RTPacket {
+class RTState_V3_0__1 : public RTShared, public RTPacket
+{
 public:
-    bool parse_with(BinParser& bp);
-    virtual bool consume_with(URRTPacketConsumer& consumer);
+  bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URRTPacketConsumer& consumer);
 
-    double i_control[6];
-    cartesian_coord_t tool_vector_target;
-    cartesian_coord_t tcp_speed_target;
+  double i_control[6];
+  cartesian_coord_t tool_vector_target;
+  cartesian_coord_t tcp_speed_target;
 
-    double joint_modes[6];
-    double safety_mode;
-    double3_t tool_accelerometer_values;
-    double speed_scaling;
-    double linear_momentum_norm;
-    double v_main;
-    double v_robot;
-    double i_robot;
-    double v_actual[6];
+  double joint_modes[6];
+  double safety_mode;
+  double3_t tool_accelerometer_values;
+  double speed_scaling;
+  double linear_momentum_norm;
+  double v_main;
+  double v_robot;
+  double i_robot;
+  double v_actual[6];
 
-    static const size_t SIZE = RTShared::SIZE
-        + sizeof(double[6]) * 3
-        + sizeof(double3_t)
-        + sizeof(cartesian_coord_t) * 2
-        + sizeof(double) * 6;
+  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!");
+  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 {
+class RTState_V3_2__3 : public RTState_V3_0__1
+{
 public:
-    bool parse_with(BinParser& bp);
-    virtual bool consume_with(URRTPacketConsumer& consumer);
+  bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URRTPacketConsumer& consumer);
 
-    uint64_t digital_outputs;
-    double program_state;
+  uint64_t digital_outputs;
+  double program_state;
 
-    static const size_t SIZE = RTState_V3_0__1::SIZE
-        + sizeof(uint64_t)
-        + sizeof(double);
+  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!");
+  static_assert(RTState_V3_2__3::SIZE == 936, "RTState_V3_2__3 has mismatched size!");
 };
\ No newline at end of file
diff --git a/include/ur_modern_driver/ur/state.h b/include/ur_modern_driver/ur/state.h
index 0ac8e69..b76d34e 100644
--- a/include/ur_modern_driver/ur/state.h
+++ b/include/ur_modern_driver/ur/state.h
@@ -1,34 +1,37 @@
 #pragma once
 
+#include <inttypes.h>
+#include <cstddef>
 #include "ur_modern_driver/bin_parser.h"
 #include "ur_modern_driver/log.h"
 #include "ur_modern_driver/pipeline.h"
-#include <cstddef>
-#include <inttypes.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 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
+enum class message_type : uint8_t
+{
+  ROBOT_STATE = 16,
+  ROBOT_MESSAGE = 20,
+  PROGRAM_STATE_MESSAGE = 25
 };
 
 class URStatePacketConsumer;
 
-class StatePacket {
+class StatePacket
+{
 public:
-    virtual bool parse_with(BinParser& bp) = 0;
-    virtual bool consume_with(URStatePacketConsumer& consumer) = 0;
+  virtual bool parseWith(BinParser& bp) = 0;
+  virtual bool consumeWith(URStatePacketConsumer& consumer) = 0;
 };
diff --git a/include/ur_modern_driver/ur/state_parser.h b/include/ur_modern_driver/ur/state_parser.h
index a68a611..031efb2 100644
--- a/include/ur_modern_driver/ur/state_parser.h
+++ b/include/ur_modern_driver/ur/state_parser.h
@@ -1,4 +1,5 @@
 #pragma once
+#include <vector>
 #include "ur_modern_driver/bin_parser.h"
 #include "ur_modern_driver/log.h"
 #include "ur_modern_driver/pipeline.h"
@@ -6,76 +7,84 @@
 #include "ur_modern_driver/ur/parser.h"
 #include "ur_modern_driver/ur/robot_mode.h"
 #include "ur_modern_driver/ur/state.h"
-#include <vector>
 
 template <typename RMD, typename MBD>
-class URStateParser : public URParser<StatePacket> {
+class URStateParser : public URParser<StatePacket>
+{
 private:
-    StatePacket* from_type(package_type type)
+  StatePacket* from_type(package_type type)
+  {
+    switch (type)
     {
-        switch (type) {
-        case package_type::ROBOT_MODE_DATA:
-            return new RMD;
-        case package_type::MASTERBOARD_DATA:
-            return new MBD;
-        default:
-            return nullptr;
-        }
+      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)
+  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)
     {
-        int32_t packet_size;
-        message_type type;
-        bp.parse(packet_size);
-        bp.parse(type);
-
-        if (type != message_type::ROBOT_STATE) {
-            LOG_WARN("Invalid message type recieved: %u", static_cast<uint8_t>(type));
-            return false;
-        }
-
-        while (!bp.empty()) {
-            if (!bp.check_size(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.check_size(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();
-                LOG_INFO("Skipping sub-packet of type %d", type);
-                continue;
-            }
-
-            if (!packet->parse_with(sbp)) {
-                LOG_ERROR("Sub-package parsing of type %d failed!", type);
-                return false;
-            }
-
-            results.push_back(std::move(packet));
-
-            if (!sbp.empty()) {
-                LOG_ERROR("Sub-package was not parsed completely!");
-                return false;
-            }
-        }
-
-        return true;
+      LOG_WARN("Invalid message type recieved: %u", static_cast<uint8_t>(type));
+      return false;
     }
+
+    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();
+        LOG_INFO("Skipping sub-packet of type %d", type);
+        continue;
+      }
+
+      if (!packet->parseWith(sbp))
+      {
+        LOG_ERROR("Sub-package parsing of type %d failed!", type);
+        return false;
+      }
+
+      results.push_back(std::move(packet));
+
+      if (!sbp.empty())
+      {
+        LOG_ERROR("Sub-package was not parsed completely!");
+        return false;
+      }
+    }
+
+    return true;
+  }
 };
 
 typedef URStateParser<RobotModeData_V1_X, MasterBoardData_V1_X> URStateParser_V1_X;
diff --git a/include/ur_modern_driver/ur/stream.h b/include/ur_modern_driver/ur/stream.h
index 7d7363b..6990c4a 100644
--- a/include/ur_modern_driver/ur/stream.h
+++ b/include/ur_modern_driver/ur/stream.h
@@ -1,37 +1,34 @@
 #pragma once
-#include <atomic>
 #include <netdb.h>
-#include <string>
 #include <sys/socket.h>
 #include <sys/types.h>
+#include <atomic>
+#include <string>
 
 /// Encapsulates a TCP socket
-class URStream {
+class URStream
+{
 private:
-    int _socket_fd = -1;
-    std::string _host;
-    int _port;
+  int socket_fd_ = -1;
+  std::string host_;
+  int port_;
 
-    std::atomic<bool> _initialized;
-    std::atomic<bool> _stopping;
+  std::atomic<bool> initialized_;
+  std::atomic<bool> stopping_;
 
 public:
-    URStream(std::string& host, int port)
-        : _host(host)
-        , _port(port)
-        , _initialized(false)
-        , _stopping(false)
-    {
-    }
+  URStream(std::string& host, int port) : host_(host), port_(port), initialized_(false), stopping_(false)
+  {
+  }
 
-    ~URStream()
-    {
-        disconnect();
-    }
+  ~URStream()
+  {
+    disconnect();
+  }
 
-    bool connect();
-    void disconnect();
+  bool connect();
+  void disconnect();
 
-    ssize_t send(uint8_t* buf, size_t buf_len);
-    ssize_t receive(uint8_t* buf, size_t buf_len);
+  ssize_t send(uint8_t* buf, size_t buf_len);
+  ssize_t receive(uint8_t* buf, size_t buf_len);
 };
\ No newline at end of file
diff --git a/include/ur_modern_driver/ur_communication.h b/include/ur_modern_driver/ur_communication.h
index cad98e1..29460c5 100644
--- a/include/ur_modern_driver/ur_communication.h
+++ b/include/ur_modern_driver/ur_communication.h
@@ -19,13 +19,7 @@
 #ifndef UR_COMMUNICATION_H_
 #define UR_COMMUNICATION_H_
 
-#include "do_output.h"
-#include "robot_state.h"
-#include <chrono>
-#include <condition_variable>
 #include <fcntl.h>
-#include <iostream>
-#include <mutex>
 #include <netdb.h>
 #include <netinet/in.h>
 #include <netinet/tcp.h>
@@ -36,27 +30,34 @@
 #include <sys/time.h>
 #include <sys/types.h>
 #include <sys/types.h>
-#include <thread>
 #include <unistd.h>
+#include <chrono>
+#include <condition_variable>
+#include <iostream>
+#include <mutex>
+#include <thread>
 #include <vector>
+#include "do_output.h"
+#include "robot_state.h"
 
-class UrCommunication {
+class UrCommunication
+{
 private:
-    int pri_sockfd_, sec_sockfd_;
-    struct sockaddr_in pri_serv_addr_, sec_serv_addr_;
-    struct hostent* server_;
-    bool keepalive_;
-    std::thread comThread_;
-    int flag_;
-    void run();
+  int pri_sockfd_, sec_sockfd_;
+  struct sockaddr_in pri_serv_addr_, sec_serv_addr_;
+  struct hostent* server_;
+  bool keepalive_;
+  std::thread comThread_;
+  int flag_;
+  void run();
 
 public:
-    bool connected_;
-    RobotState* robot_state_;
+  bool connected_;
+  RobotState* robot_state_;
 
-    UrCommunication(std::condition_variable& msg_cond, std::string host);
-    bool start();
-    void halt();
+  UrCommunication(std::condition_variable& msg_cond, std::string host);
+  bool start();
+  void halt();
 };
 
 #endif /* UR_COMMUNICATION_H_ */
diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h
index 1d414c1..dbfb3bd 100644
--- a/include/ur_modern_driver/ur_driver.h
+++ b/include/ur_modern_driver/ur_driver.h
@@ -19,81 +19,79 @@
 #ifndef UR_DRIVER_H_
 #define UR_DRIVER_H_
 
+#include <math.h>
+#include <netinet/in.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <condition_variable>
+#include <mutex>
+#include <string>
+#include <vector>
 #include "do_output.h"
 #include "ur_communication.h"
 #include "ur_realtime_communication.h"
-#include <condition_variable>
-#include <math.h>
-#include <mutex>
-#include <netinet/in.h>
-#include <string>
-#include <sys/socket.h>
-#include <sys/types.h>
-#include <vector>
 
 #include <chrono>
 
-class UrDriver {
+class UrDriver
+{
 private:
-    double maximum_time_step_;
-    double minimum_payload_;
-    double maximum_payload_;
-    std::vector<std::string> joint_names_;
-    std::string ip_addr_;
-    const int MULT_JOINTSTATE_ = 1000000;
-    const int MULT_TIME_ = 1000000;
-    const unsigned int REVERSE_PORT_;
-    int incoming_sockfd_;
-    int new_sockfd_;
-    bool reverse_connected_;
-    double servoj_time_;
-    bool executing_traj_;
-    double firmware_version_;
-    double servoj_lookahead_time_;
-    double servoj_gain_;
+  double maximum_time_step_;
+  double minimum_payload_;
+  double maximum_payload_;
+  std::vector<std::string> joint_names_;
+  std::string ip_addr_;
+  const int MULT_JOINTSTATE_ = 1000000;
+  const int MULT_TIME_ = 1000000;
+  const unsigned int REVERSE_PORT_;
+  int incoming_sockfd_;
+  int new_sockfd_;
+  bool reverse_connected_;
+  double servoj_time_;
+  bool executing_traj_;
+  double firmware_version_;
+  double servoj_lookahead_time_;
+  double servoj_gain_;
 
 public:
-    UrRealtimeCommunication* rt_interface_;
-    UrCommunication* sec_interface_;
+  UrRealtimeCommunication* rt_interface_;
+  UrCommunication* sec_interface_;
 
-    UrDriver(std::condition_variable& rt_msg_cond,
-        std::condition_variable& msg_cond, std::string host,
-        unsigned int reverse_port = 50007, double servoj_time = 0.016, unsigned int safety_count_max = 12, double max_time_step = 0.08, double min_payload = 0.,
-        double max_payload = 1., double servoj_lookahead_time = 0.03, double servoj_gain = 300.);
-    bool start();
-    void halt();
+  UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host,
+           unsigned int reverse_port = 50007, double servoj_time = 0.016, unsigned int safety_count_max = 12,
+           double max_time_step = 0.08, double min_payload = 0., double max_payload = 1.,
+           double servoj_lookahead_time = 0.03, double servoj_gain = 300.);
+  bool start();
+  void halt();
 
-    void setSpeed(double q0, double q1, double q2, double q3, double q4,
-        double q5, double acc = 100.);
+  void setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc = 100.);
 
-    bool doTraj(std::vector<double> inp_timestamps,
-        std::vector<std::vector<double> > inp_positions,
-        std::vector<std::vector<double> > inp_velocities);
-    void servoj(std::vector<double> positions, int keepalive = 1);
+  bool doTraj(std::vector<double> inp_timestamps, std::vector<std::vector<double>> inp_positions,
+              std::vector<std::vector<double>> inp_velocities);
+  void servoj(std::vector<double> positions, int keepalive = 1);
 
-    void stopTraj();
+  void stopTraj();
 
-    bool uploadProg();
-    bool openServo();
-    void closeServo(std::vector<double> positions);
+  bool uploadProg();
+  bool openServo();
+  void closeServo(std::vector<double> positions);
 
-    std::vector<double> interp_cubic(double t, double T,
-        std::vector<double> p0_pos, std::vector<double> p1_pos,
-        std::vector<double> p0_vel, std::vector<double> p1_vel);
+  std::vector<double> interp_cubic(double t, double T, std::vector<double> p0_pos, std::vector<double> p1_pos,
+                                   std::vector<double> p0_vel, std::vector<double> p1_vel);
 
-    std::vector<std::string> getJointNames();
-    void setJointNames(std::vector<std::string> jn);
-    void setToolVoltage(unsigned int v);
-    void setFlag(unsigned int n, bool b);
-    void setDigitalOut(unsigned int n, bool b);
-    void setAnalogOut(unsigned int n, double f);
-    bool setPayload(double m);
+  std::vector<std::string> getJointNames();
+  void setJointNames(std::vector<std::string> jn);
+  void setToolVoltage(unsigned int v);
+  void setFlag(unsigned int n, bool b);
+  void setDigitalOut(unsigned int n, bool b);
+  void setAnalogOut(unsigned int n, double f);
+  bool setPayload(double m);
 
-    void setMinPayload(double m);
-    void setMaxPayload(double m);
-    void setServojTime(double t);
-    void setServojLookahead(double t);
-    void setServojGain(double g);
+  void setMinPayload(double m);
+  void setMaxPayload(double m);
+  void setServojTime(double t);
+  void setServojLookahead(double t);
+  void setServojGain(double g);
 };
 
 #endif /* UR_DRIVER_H_ */
diff --git a/include/ur_modern_driver/ur_hardware_interface.h b/include/ur_modern_driver/ur_hardware_interface.h
index 0724703..b2a7db1 100644
--- a/include/ur_modern_driver/ur_hardware_interface.h
+++ b/include/ur_modern_driver/ur_hardware_interface.h
@@ -58,9 +58,6 @@
 #ifndef UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H
 #define UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H
 
-#include "do_output.h"
-#include "ur_driver.h"
-#include <boost/scoped_ptr.hpp>
 #include <controller_manager/controller_manager.h>
 #include <hardware_interface/force_torque_sensor_interface.h>
 #include <hardware_interface/joint_command_interface.h>
@@ -68,69 +65,72 @@
 #include <hardware_interface/robot_hw.h>
 #include <math.h>
 #include <ros/ros.h>
+#include <boost/scoped_ptr.hpp>
+#include "do_output.h"
+#include "ur_driver.h"
 
-namespace ros_control_ur {
-
+namespace ros_control_ur
+{
 // For simulation only - determines how fast a trajectory is followed
 static const double POSITION_STEP_FACTOR = 1;
 static const double VELOCITY_STEP_FACTOR = 1;
 
 /// \brief Hardware interface for a robot
-class UrHardwareInterface : public hardware_interface::RobotHW {
+class UrHardwareInterface : public hardware_interface::RobotHW
+{
 public:
-    /**
-	 * \brief Constructor
-	 * \param nh - Node handle for topics.
-	 */
-    UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot);
+  /**
+ * \brief Constructor
+ * \param nh - Node handle for topics.
+ */
+  UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot);
 
-    /// \brief Initialize the hardware interface
-    virtual void init();
+  /// \brief Initialize the hardware interface
+  virtual void init();
 
-    /// \brief Read the state from the robot hardware.
-    virtual void read();
+  /// \brief Read the state from the robot hardware.
+  virtual void read();
 
-    /// \brief write the command to the robot hardware.
-    virtual void write();
+  /// \brief write the command to the robot hardware.
+  virtual void write();
 
-    void setMaxVelChange(double inp);
+  void setMaxVelChange(double inp);
 
-    bool canSwitch(
-        const std::list<hardware_interface::ControllerInfo>& start_list,
-        const std::list<hardware_interface::ControllerInfo>& stop_list) const;
-    void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
-        const std::list<hardware_interface::ControllerInfo>& stop_list);
+  bool canSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
+                 const std::list<hardware_interface::ControllerInfo>& stop_list) const;
+  void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
+                const std::list<hardware_interface::ControllerInfo>& stop_list);
 
 protected:
-    // Startup and shutdown of the internal node inside a roscpp program
-    ros::NodeHandle nh_;
+  // Startup and shutdown of the internal node inside a roscpp program
+  ros::NodeHandle nh_;
 
-    // Interfaces
-    hardware_interface::JointStateInterface joint_state_interface_;
-    hardware_interface::ForceTorqueSensorInterface force_torque_interface_;
-    hardware_interface::PositionJointInterface position_joint_interface_;
-    hardware_interface::VelocityJointInterface velocity_joint_interface_;
-    bool velocity_interface_running_;
-    bool position_interface_running_;
-    // Shared memory
-    std::vector<std::string> joint_names_;
-    std::vector<double> joint_position_;
-    std::vector<double> joint_velocity_;
-    std::vector<double> joint_effort_;
-    std::vector<double> joint_position_command_;
-    std::vector<double> joint_velocity_command_;
-    std::vector<double> prev_joint_velocity_command_;
-    std::size_t num_joints_;
-    double robot_force_[3] = { 0., 0., 0. };
-    double robot_torque_[3] = { 0., 0., 0. };
+  // Interfaces
+  hardware_interface::JointStateInterface joint_state_interface_;
+  hardware_interface::ForceTorqueSensorInterface force_torque_interface_;
+  hardware_interface::PositionJointInterface position_joint_interface_;
+  hardware_interface::VelocityJointInterface velocity_joint_interface_;
+  bool velocity_interface_running_;
+  bool position_interface_running_;
+  // Shared memory
+  std::vector<std::string> joint_names_;
+  std::vector<double> joint_position_;
+  std::vector<double> joint_velocity_;
+  std::vector<double> joint_effort_;
+  std::vector<double> joint_position_command_;
+  std::vector<double> joint_velocity_command_;
+  std::vector<double> prev_joint_velocity_command_;
+  std::size_t num_joints_;
+  double robot_force_[3] = { 0., 0., 0. };
+  double robot_torque_[3] = { 0., 0., 0. };
 
-    double max_vel_change_;
+  double max_vel_change_;
 
-    // Robot API
-    UrDriver* robot_;
+  // Robot API
+  UrDriver* robot_;
 };
 // class
 
-} // namespace
+}  // namespace
 
 #endif
diff --git a/include/ur_modern_driver/ur_realtime_communication.h b/include/ur_modern_driver/ur_realtime_communication.h
index 61e5474..1de9fdb 100644
--- a/include/ur_modern_driver/ur_realtime_communication.h
+++ b/include/ur_modern_driver/ur_realtime_communication.h
@@ -19,14 +19,9 @@
 #ifndef UR_REALTIME_COMMUNICATION_H_
 #define UR_REALTIME_COMMUNICATION_H_
 
-#include "do_output.h"
-#include "robot_state_RT.h"
 #include <arpa/inet.h>
-#include <condition_variable>
 #include <errno.h>
 #include <fcntl.h>
-#include <iostream>
-#include <mutex>
 #include <netdb.h>
 #include <netinet/in.h>
 #include <netinet/tcp.h>
@@ -37,38 +32,42 @@
 #include <sys/time.h>
 #include <sys/types.h>
 #include <sys/types.h>
-#include <thread>
 #include <unistd.h>
+#include <condition_variable>
+#include <iostream>
+#include <mutex>
+#include <thread>
 #include <vector>
+#include "do_output.h"
+#include "robot_state_RT.h"
 
-class UrRealtimeCommunication {
+class UrRealtimeCommunication
+{
 private:
-    unsigned int safety_count_max_;
-    int sockfd_;
-    struct sockaddr_in serv_addr_;
-    struct hostent* server_;
-    std::string local_ip_;
-    bool keepalive_;
-    std::thread comThread_;
-    int flag_;
-    std::recursive_mutex command_string_lock_;
-    std::string command_;
-    unsigned int safety_count_;
-    void run();
+  unsigned int safety_count_max_;
+  int sockfd_;
+  struct sockaddr_in serv_addr_;
+  struct hostent* server_;
+  std::string local_ip_;
+  bool keepalive_;
+  std::thread comThread_;
+  int flag_;
+  std::recursive_mutex command_string_lock_;
+  std::string command_;
+  unsigned int safety_count_;
+  void run();
 
 public:
-    bool connected_;
-    RobotStateRT* robot_state_;
+  bool connected_;
+  RobotStateRT* robot_state_;
 
-    UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host,
-        unsigned int safety_count_max = 12);
-    bool start();
-    void halt();
-    void setSpeed(double q0, double q1, double q2, double q3, double q4,
-        double q5, double acc = 100.);
-    void addCommandToQueue(std::string inp);
-    void setSafetyCountMax(uint inp);
-    std::string getLocalIp();
+  UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host, unsigned int safety_count_max = 12);
+  bool start();
+  void halt();
+  void setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc = 100.);
+  void addCommandToQueue(std::string inp);
+  void setSafetyCountMax(uint inp);
+  std::string getLocalIp();
 };
 
 #endif /* UR_REALTIME_COMMUNICATION_H_ */
diff --git a/src/do_output.cpp b/src/do_output.cpp
index f4b0b40..765e9b4 100644
--- a/src/do_output.cpp
+++ b/src/do_output.cpp
@@ -21,42 +21,42 @@
 void print_debug(std::string inp)
 {
 #ifdef ROS_BUILD
-    ROS_DEBUG("%s", inp.c_str());
+  ROS_DEBUG("%s", inp.c_str());
 #else
-    printf("DEBUG: %s\n", inp.c_str());
+  printf("DEBUG: %s\n", inp.c_str());
 #endif
 }
 void print_info(std::string inp)
 {
 #ifdef ROS_BUILD
-    ROS_INFO("%s", inp.c_str());
+  ROS_INFO("%s", inp.c_str());
 #else
-    printf("INFO: %s\n", inp.c_str());
+  printf("INFO: %s\n", inp.c_str());
 #endif
 }
 void print_warning(std::string inp)
 {
 #ifdef ROS_BUILD
-    ROS_WARN("%s", inp.c_str());
+  ROS_WARN("%s", inp.c_str());
 #else
-    printf("WARNING: %s\n", inp.c_str());
+  printf("WARNING: %s\n", inp.c_str());
 #endif
 }
 void print_error(std::string inp)
 {
 #ifdef ROS_BUILD
-    ROS_ERROR("%s", inp.c_str());
+  ROS_ERROR("%s", inp.c_str());
 #else
-    printf("ERROR: %s\n", inp.c_str());
+  printf("ERROR: %s\n", inp.c_str());
 #endif
 }
 void print_fatal(std::string inp)
 {
 #ifdef ROS_BUILD
-    ROS_FATAL("%s", inp.c_str());
-    ros::shutdown();
+  ROS_FATAL("%s", inp.c_str());
+  ros::shutdown();
 #else
-    printf("FATAL: %s\n", inp.c_str());
-    exit(1);
+  printf("FATAL: %s\n", inp.c_str());
+  exit(1);
 #endif
 }
diff --git a/src/robot_state.cpp b/src/robot_state.cpp
index 743c2c2..786517c 100644
--- a/src/robot_state.cpp
+++ b/src/robot_state.cpp
@@ -20,389 +20,380 @@
 
 RobotState::RobotState(std::condition_variable& msg_cond)
 {
-    version_msg_.major_version = 0;
-    version_msg_.minor_version = 0;
-    new_data_available_ = false;
-    pMsg_cond_ = &msg_cond;
-    RobotState::setDisconnected();
-    robot_mode_running_ = robotStateTypeV30::ROBOT_MODE_RUNNING;
+  version_msg_.major_version = 0;
+  version_msg_.minor_version = 0;
+  new_data_available_ = false;
+  pMsg_cond_ = &msg_cond;
+  RobotState::setDisconnected();
+  robot_mode_running_ = robotStateTypeV30::ROBOT_MODE_RUNNING;
 }
 double RobotState::ntohd(uint64_t nf)
 {
-    double x;
-    nf = be64toh(nf);
-    memcpy(&x, &nf, sizeof(x));
-    return x;
+  double x;
+  nf = be64toh(nf);
+  memcpy(&x, &nf, sizeof(x));
+  return x;
 }
 void RobotState::unpack(uint8_t* buf, unsigned int buf_length)
 {
-    /* Returns missing bytes to unpack a message, or 0 if all data was parsed */
-    unsigned int offset = 0;
-    while (buf_length > offset) {
-        int len;
-        unsigned char message_type;
-        memcpy(&len, &buf[offset], sizeof(len));
-        len = ntohl(len);
-        if (len + offset > buf_length) {
-            return;
-        }
-        memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type));
-        switch (message_type) {
-        case messageType::ROBOT_MESSAGE:
-            RobotState::unpackRobotMessage(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType
-            break;
-        case messageType::ROBOT_STATE:
-            RobotState::unpackRobotState(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType
-            break;
-        case messageType::PROGRAM_STATE_MESSAGE:
-        //Don't do anything atm...
-        default:
-            break;
-        }
-        offset += len;
+  /* Returns missing bytes to unpack a message, or 0 if all data was parsed */
+  unsigned int offset = 0;
+  while (buf_length > offset)
+  {
+    int len;
+    unsigned char message_type;
+    memcpy(&len, &buf[offset], sizeof(len));
+    len = ntohl(len);
+    if (len + offset > buf_length)
+    {
+      return;
     }
-    return;
+    memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type));
+    switch (message_type)
+    {
+      case messageType::ROBOT_MESSAGE:
+        RobotState::unpackRobotMessage(buf, offset,
+                                       len);  //'len' is inclusive the 5 bytes from messageSize and messageType
+        break;
+      case messageType::ROBOT_STATE:
+        RobotState::unpackRobotState(buf, offset,
+                                     len);  //'len' is inclusive the 5 bytes from messageSize and messageType
+        break;
+      case messageType::PROGRAM_STATE_MESSAGE:
+      // Don't do anything atm...
+      default:
+        break;
+    }
+    offset += len;
+  }
+  return;
 }
 
-void RobotState::unpackRobotMessage(uint8_t* buf, unsigned int offset,
-    uint32_t len)
+void RobotState::unpackRobotMessage(uint8_t* buf, unsigned int offset, uint32_t len)
 {
-    offset += 5;
-    uint64_t timestamp;
-    int8_t source, robot_message_type;
-    memcpy(&timestamp, &buf[offset], sizeof(timestamp));
-    offset += sizeof(timestamp);
-    memcpy(&source, &buf[offset], sizeof(source));
-    offset += sizeof(source);
-    memcpy(&robot_message_type, &buf[offset], sizeof(robot_message_type));
-    offset += sizeof(robot_message_type);
-    switch (robot_message_type) {
+  offset += 5;
+  uint64_t timestamp;
+  int8_t source, robot_message_type;
+  memcpy(&timestamp, &buf[offset], sizeof(timestamp));
+  offset += sizeof(timestamp);
+  memcpy(&source, &buf[offset], sizeof(source));
+  offset += sizeof(source);
+  memcpy(&robot_message_type, &buf[offset], sizeof(robot_message_type));
+  offset += sizeof(robot_message_type);
+  switch (robot_message_type)
+  {
     case robotMessageType::ROBOT_MESSAGE_VERSION:
+      val_lock_.lock();
+      version_msg_.timestamp = timestamp;
+      version_msg_.source = source;
+      version_msg_.robot_message_type = robot_message_type;
+      RobotState::unpackRobotMessageVersion(buf, offset, len);
+      val_lock_.unlock();
+      break;
+    default:
+      break;
+  }
+}
+
+void RobotState::unpackRobotState(uint8_t* buf, unsigned int offset, uint32_t len)
+{
+  offset += 5;
+  while (offset < len)
+  {
+    int32_t length;
+    uint8_t package_type;
+    memcpy(&length, &buf[offset], sizeof(length));
+    length = ntohl(length);
+    memcpy(&package_type, &buf[offset + sizeof(length)], sizeof(package_type));
+    switch (package_type)
+    {
+      case packageType::ROBOT_MODE_DATA:
         val_lock_.lock();
-        version_msg_.timestamp = timestamp;
-        version_msg_.source = source;
-        version_msg_.robot_message_type = robot_message_type;
-        RobotState::unpackRobotMessageVersion(buf, offset, len);
+        RobotState::unpackRobotMode(buf, offset + 5);
         val_lock_.unlock();
         break;
-    default:
+
+      case packageType::MASTERBOARD_DATA:
+        val_lock_.lock();
+        RobotState::unpackRobotStateMasterboard(buf, offset + 5);
+        val_lock_.unlock();
+        break;
+      default:
         break;
     }
+    offset += length;
+  }
+  new_data_available_ = true;
+  pMsg_cond_->notify_all();
 }
 
-void RobotState::unpackRobotState(uint8_t* buf, unsigned int offset,
-    uint32_t len)
+void RobotState::unpackRobotMessageVersion(uint8_t* buf, unsigned int offset, uint32_t len)
 {
-    offset += 5;
-    while (offset < len) {
-        int32_t length;
-        uint8_t package_type;
-        memcpy(&length, &buf[offset], sizeof(length));
-        length = ntohl(length);
-        memcpy(&package_type, &buf[offset + sizeof(length)],
-            sizeof(package_type));
-        switch (package_type) {
-        case packageType::ROBOT_MODE_DATA:
-            val_lock_.lock();
-            RobotState::unpackRobotMode(buf, offset + 5);
-            val_lock_.unlock();
-            break;
-
-        case packageType::MASTERBOARD_DATA:
-            val_lock_.lock();
-            RobotState::unpackRobotStateMasterboard(buf, offset + 5);
-            val_lock_.unlock();
-            break;
-        default:
-            break;
-        }
-        offset += length;
-    }
-    new_data_available_ = true;
-    pMsg_cond_->notify_all();
-}
-
-void RobotState::unpackRobotMessageVersion(uint8_t* buf, unsigned int offset,
-    uint32_t len)
-{
-    memcpy(&version_msg_.project_name_size, &buf[offset],
-        sizeof(version_msg_.project_name_size));
-    offset += sizeof(version_msg_.project_name_size);
-    memcpy(&version_msg_.project_name, &buf[offset],
-        sizeof(char) * version_msg_.project_name_size);
-    offset += version_msg_.project_name_size;
-    version_msg_.project_name[version_msg_.project_name_size] = '\0';
-    memcpy(&version_msg_.major_version, &buf[offset],
-        sizeof(version_msg_.major_version));
-    offset += sizeof(version_msg_.major_version);
-    memcpy(&version_msg_.minor_version, &buf[offset],
-        sizeof(version_msg_.minor_version));
-    offset += sizeof(version_msg_.minor_version);
-    memcpy(&version_msg_.svn_revision, &buf[offset],
-        sizeof(version_msg_.svn_revision));
-    offset += sizeof(version_msg_.svn_revision);
-    version_msg_.svn_revision = ntohl(version_msg_.svn_revision);
-    memcpy(&version_msg_.build_date, &buf[offset], sizeof(char) * len - offset);
-    version_msg_.build_date[len - offset] = '\0';
-    if (version_msg_.major_version < 2) {
-        robot_mode_running_ = robotStateTypeV18::ROBOT_RUNNING_MODE;
-    }
+  memcpy(&version_msg_.project_name_size, &buf[offset], sizeof(version_msg_.project_name_size));
+  offset += sizeof(version_msg_.project_name_size);
+  memcpy(&version_msg_.project_name, &buf[offset], sizeof(char) * version_msg_.project_name_size);
+  offset += version_msg_.project_name_size;
+  version_msg_.project_name[version_msg_.project_name_size] = '\0';
+  memcpy(&version_msg_.major_version, &buf[offset], sizeof(version_msg_.major_version));
+  offset += sizeof(version_msg_.major_version);
+  memcpy(&version_msg_.minor_version, &buf[offset], sizeof(version_msg_.minor_version));
+  offset += sizeof(version_msg_.minor_version);
+  memcpy(&version_msg_.svn_revision, &buf[offset], sizeof(version_msg_.svn_revision));
+  offset += sizeof(version_msg_.svn_revision);
+  version_msg_.svn_revision = ntohl(version_msg_.svn_revision);
+  memcpy(&version_msg_.build_date, &buf[offset], sizeof(char) * len - offset);
+  version_msg_.build_date[len - offset] = '\0';
+  if (version_msg_.major_version < 2)
+  {
+    robot_mode_running_ = robotStateTypeV18::ROBOT_RUNNING_MODE;
+  }
 }
 
 void RobotState::unpackRobotMode(uint8_t* buf, unsigned int offset)
 {
-    memcpy(&robot_mode_.timestamp, &buf[offset], sizeof(robot_mode_.timestamp));
-    offset += sizeof(robot_mode_.timestamp);
-    uint8_t tmp;
-    memcpy(&tmp, &buf[offset], sizeof(tmp));
-    if (tmp > 0)
-        robot_mode_.isRobotConnected = true;
-    else
-        robot_mode_.isRobotConnected = false;
-    offset += sizeof(tmp);
-    memcpy(&tmp, &buf[offset], sizeof(tmp));
-    if (tmp > 0)
-        robot_mode_.isRealRobotEnabled = true;
-    else
-        robot_mode_.isRealRobotEnabled = false;
-    offset += sizeof(tmp);
-    memcpy(&tmp, &buf[offset], sizeof(tmp));
-    //printf("PowerOnRobot: %d\n", tmp);
-    if (tmp > 0)
-        robot_mode_.isPowerOnRobot = true;
-    else
-        robot_mode_.isPowerOnRobot = false;
-    offset += sizeof(tmp);
-    memcpy(&tmp, &buf[offset], sizeof(tmp));
-    if (tmp > 0)
-        robot_mode_.isEmergencyStopped = true;
-    else
-        robot_mode_.isEmergencyStopped = false;
-    offset += sizeof(tmp);
-    memcpy(&tmp, &buf[offset], sizeof(tmp));
-    if (tmp > 0)
-        robot_mode_.isProtectiveStopped = true;
-    else
-        robot_mode_.isProtectiveStopped = false;
-    offset += sizeof(tmp);
-    memcpy(&tmp, &buf[offset], sizeof(tmp));
-    if (tmp > 0)
-        robot_mode_.isProgramRunning = true;
-    else
-        robot_mode_.isProgramRunning = false;
-    offset += sizeof(tmp);
-    memcpy(&tmp, &buf[offset], sizeof(tmp));
-    if (tmp > 0)
-        robot_mode_.isProgramPaused = true;
-    else
-        robot_mode_.isProgramPaused = false;
-    offset += sizeof(tmp);
-    memcpy(&robot_mode_.robotMode, &buf[offset], sizeof(robot_mode_.robotMode));
-    offset += sizeof(robot_mode_.robotMode);
-    uint64_t temp;
-    if (RobotState::getVersion() > 2.) {
-        memcpy(&robot_mode_.controlMode, &buf[offset],
-            sizeof(robot_mode_.controlMode));
-        offset += sizeof(robot_mode_.controlMode);
-        memcpy(&temp, &buf[offset], sizeof(temp));
-        offset += sizeof(temp);
-        robot_mode_.targetSpeedFraction = RobotState::ntohd(temp);
-    }
+  memcpy(&robot_mode_.timestamp, &buf[offset], sizeof(robot_mode_.timestamp));
+  offset += sizeof(robot_mode_.timestamp);
+  uint8_t tmp;
+  memcpy(&tmp, &buf[offset], sizeof(tmp));
+  if (tmp > 0)
+    robot_mode_.isRobotConnected = true;
+  else
+    robot_mode_.isRobotConnected = false;
+  offset += sizeof(tmp);
+  memcpy(&tmp, &buf[offset], sizeof(tmp));
+  if (tmp > 0)
+    robot_mode_.isRealRobotEnabled = true;
+  else
+    robot_mode_.isRealRobotEnabled = false;
+  offset += sizeof(tmp);
+  memcpy(&tmp, &buf[offset], sizeof(tmp));
+  // printf("PowerOnRobot: %d\n", tmp);
+  if (tmp > 0)
+    robot_mode_.isPowerOnRobot = true;
+  else
+    robot_mode_.isPowerOnRobot = false;
+  offset += sizeof(tmp);
+  memcpy(&tmp, &buf[offset], sizeof(tmp));
+  if (tmp > 0)
+    robot_mode_.isEmergencyStopped = true;
+  else
+    robot_mode_.isEmergencyStopped = false;
+  offset += sizeof(tmp);
+  memcpy(&tmp, &buf[offset], sizeof(tmp));
+  if (tmp > 0)
+    robot_mode_.isProtectiveStopped = true;
+  else
+    robot_mode_.isProtectiveStopped = false;
+  offset += sizeof(tmp);
+  memcpy(&tmp, &buf[offset], sizeof(tmp));
+  if (tmp > 0)
+    robot_mode_.isProgramRunning = true;
+  else
+    robot_mode_.isProgramRunning = false;
+  offset += sizeof(tmp);
+  memcpy(&tmp, &buf[offset], sizeof(tmp));
+  if (tmp > 0)
+    robot_mode_.isProgramPaused = true;
+  else
+    robot_mode_.isProgramPaused = false;
+  offset += sizeof(tmp);
+  memcpy(&robot_mode_.robotMode, &buf[offset], sizeof(robot_mode_.robotMode));
+  offset += sizeof(robot_mode_.robotMode);
+  uint64_t temp;
+  if (RobotState::getVersion() > 2.)
+  {
+    memcpy(&robot_mode_.controlMode, &buf[offset], sizeof(robot_mode_.controlMode));
+    offset += sizeof(robot_mode_.controlMode);
     memcpy(&temp, &buf[offset], sizeof(temp));
     offset += sizeof(temp);
-    robot_mode_.speedScaling = RobotState::ntohd(temp);
+    robot_mode_.targetSpeedFraction = RobotState::ntohd(temp);
+  }
+  memcpy(&temp, &buf[offset], sizeof(temp));
+  offset += sizeof(temp);
+  robot_mode_.speedScaling = RobotState::ntohd(temp);
 }
 
-void RobotState::unpackRobotStateMasterboard(uint8_t* buf,
-    unsigned int offset)
+void RobotState::unpackRobotStateMasterboard(uint8_t* buf, unsigned int offset)
 {
-    if (RobotState::getVersion() < 3.0) {
-        int16_t digital_input_bits, digital_output_bits;
-        memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits));
-        offset += sizeof(digital_input_bits);
-        memcpy(&digital_output_bits, &buf[offset], sizeof(digital_output_bits));
-        offset += sizeof(digital_output_bits);
-        mb_data_.digitalInputBits = ntohs(digital_input_bits);
-        mb_data_.digitalOutputBits = ntohs(digital_output_bits);
-    } else {
-        memcpy(&mb_data_.digitalInputBits, &buf[offset],
-            sizeof(mb_data_.digitalInputBits));
-        offset += sizeof(mb_data_.digitalInputBits);
-        mb_data_.digitalInputBits = ntohl(mb_data_.digitalInputBits);
-        memcpy(&mb_data_.digitalOutputBits, &buf[offset],
-            sizeof(mb_data_.digitalOutputBits));
-        offset += sizeof(mb_data_.digitalOutputBits);
-        mb_data_.digitalOutputBits = ntohl(mb_data_.digitalOutputBits);
+  if (RobotState::getVersion() < 3.0)
+  {
+    int16_t digital_input_bits, digital_output_bits;
+    memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits));
+    offset += sizeof(digital_input_bits);
+    memcpy(&digital_output_bits, &buf[offset], sizeof(digital_output_bits));
+    offset += sizeof(digital_output_bits);
+    mb_data_.digitalInputBits = ntohs(digital_input_bits);
+    mb_data_.digitalOutputBits = ntohs(digital_output_bits);
+  }
+  else
+  {
+    memcpy(&mb_data_.digitalInputBits, &buf[offset], sizeof(mb_data_.digitalInputBits));
+    offset += sizeof(mb_data_.digitalInputBits);
+    mb_data_.digitalInputBits = ntohl(mb_data_.digitalInputBits);
+    memcpy(&mb_data_.digitalOutputBits, &buf[offset], sizeof(mb_data_.digitalOutputBits));
+    offset += sizeof(mb_data_.digitalOutputBits);
+    mb_data_.digitalOutputBits = ntohl(mb_data_.digitalOutputBits);
+  }
+
+  memcpy(&mb_data_.analogInputRange0, &buf[offset], sizeof(mb_data_.analogInputRange0));
+  offset += sizeof(mb_data_.analogInputRange0);
+  memcpy(&mb_data_.analogInputRange1, &buf[offset], sizeof(mb_data_.analogInputRange1));
+  offset += sizeof(mb_data_.analogInputRange1);
+  uint64_t temp;
+  memcpy(&temp, &buf[offset], sizeof(temp));
+  offset += sizeof(temp);
+  mb_data_.analogInput0 = RobotState::ntohd(temp);
+  memcpy(&temp, &buf[offset], sizeof(temp));
+  offset += sizeof(temp);
+  mb_data_.analogInput1 = RobotState::ntohd(temp);
+  memcpy(&mb_data_.analogOutputDomain0, &buf[offset], sizeof(mb_data_.analogOutputDomain0));
+  offset += sizeof(mb_data_.analogOutputDomain0);
+  memcpy(&mb_data_.analogOutputDomain1, &buf[offset], sizeof(mb_data_.analogOutputDomain1));
+  offset += sizeof(mb_data_.analogOutputDomain1);
+  memcpy(&temp, &buf[offset], sizeof(temp));
+  offset += sizeof(temp);
+  mb_data_.analogOutput0 = RobotState::ntohd(temp);
+  memcpy(&temp, &buf[offset], sizeof(temp));
+  offset += sizeof(temp);
+  mb_data_.analogOutput1 = RobotState::ntohd(temp);
+
+  memcpy(&mb_data_.masterBoardTemperature, &buf[offset], sizeof(mb_data_.masterBoardTemperature));
+  offset += sizeof(mb_data_.masterBoardTemperature);
+  mb_data_.masterBoardTemperature = ntohl(mb_data_.masterBoardTemperature);
+  memcpy(&mb_data_.robotVoltage48V, &buf[offset], sizeof(mb_data_.robotVoltage48V));
+  offset += sizeof(mb_data_.robotVoltage48V);
+  mb_data_.robotVoltage48V = ntohl(mb_data_.robotVoltage48V);
+  memcpy(&mb_data_.robotCurrent, &buf[offset], sizeof(mb_data_.robotCurrent));
+  offset += sizeof(mb_data_.robotCurrent);
+  mb_data_.robotCurrent = ntohl(mb_data_.robotCurrent);
+  memcpy(&mb_data_.masterIOCurrent, &buf[offset], sizeof(mb_data_.masterIOCurrent));
+  offset += sizeof(mb_data_.masterIOCurrent);
+  mb_data_.masterIOCurrent = ntohl(mb_data_.masterIOCurrent);
+
+  memcpy(&mb_data_.safetyMode, &buf[offset], sizeof(mb_data_.safetyMode));
+  offset += sizeof(mb_data_.safetyMode);
+  memcpy(&mb_data_.masterOnOffState, &buf[offset], sizeof(mb_data_.masterOnOffState));
+  offset += sizeof(mb_data_.masterOnOffState);
+
+  memcpy(&mb_data_.euromap67InterfaceInstalled, &buf[offset], sizeof(mb_data_.euromap67InterfaceInstalled));
+  offset += sizeof(mb_data_.euromap67InterfaceInstalled);
+  if (mb_data_.euromap67InterfaceInstalled != 0)
+  {
+    memcpy(&mb_data_.euromapInputBits, &buf[offset], sizeof(mb_data_.euromapInputBits));
+    offset += sizeof(mb_data_.euromapInputBits);
+    mb_data_.euromapInputBits = ntohl(mb_data_.euromapInputBits);
+    memcpy(&mb_data_.euromapOutputBits, &buf[offset], sizeof(mb_data_.euromapOutputBits));
+    offset += sizeof(mb_data_.euromapOutputBits);
+    mb_data_.euromapOutputBits = ntohl(mb_data_.euromapOutputBits);
+    if (RobotState::getVersion() < 3.0)
+    {
+      int16_t euromap_voltage, euromap_current;
+      memcpy(&euromap_voltage, &buf[offset], sizeof(euromap_voltage));
+      offset += sizeof(euromap_voltage);
+      memcpy(&euromap_current, &buf[offset], sizeof(euromap_current));
+      offset += sizeof(euromap_current);
+      mb_data_.euromapVoltage = ntohs(euromap_voltage);
+      mb_data_.euromapCurrent = ntohs(euromap_current);
     }
-
-    memcpy(&mb_data_.analogInputRange0, &buf[offset],
-        sizeof(mb_data_.analogInputRange0));
-    offset += sizeof(mb_data_.analogInputRange0);
-    memcpy(&mb_data_.analogInputRange1, &buf[offset],
-        sizeof(mb_data_.analogInputRange1));
-    offset += sizeof(mb_data_.analogInputRange1);
-    uint64_t temp;
-    memcpy(&temp, &buf[offset], sizeof(temp));
-    offset += sizeof(temp);
-    mb_data_.analogInput0 = RobotState::ntohd(temp);
-    memcpy(&temp, &buf[offset], sizeof(temp));
-    offset += sizeof(temp);
-    mb_data_.analogInput1 = RobotState::ntohd(temp);
-    memcpy(&mb_data_.analogOutputDomain0, &buf[offset],
-        sizeof(mb_data_.analogOutputDomain0));
-    offset += sizeof(mb_data_.analogOutputDomain0);
-    memcpy(&mb_data_.analogOutputDomain1, &buf[offset],
-        sizeof(mb_data_.analogOutputDomain1));
-    offset += sizeof(mb_data_.analogOutputDomain1);
-    memcpy(&temp, &buf[offset], sizeof(temp));
-    offset += sizeof(temp);
-    mb_data_.analogOutput0 = RobotState::ntohd(temp);
-    memcpy(&temp, &buf[offset], sizeof(temp));
-    offset += sizeof(temp);
-    mb_data_.analogOutput1 = RobotState::ntohd(temp);
-
-    memcpy(&mb_data_.masterBoardTemperature, &buf[offset],
-        sizeof(mb_data_.masterBoardTemperature));
-    offset += sizeof(mb_data_.masterBoardTemperature);
-    mb_data_.masterBoardTemperature = ntohl(mb_data_.masterBoardTemperature);
-    memcpy(&mb_data_.robotVoltage48V, &buf[offset],
-        sizeof(mb_data_.robotVoltage48V));
-    offset += sizeof(mb_data_.robotVoltage48V);
-    mb_data_.robotVoltage48V = ntohl(mb_data_.robotVoltage48V);
-    memcpy(&mb_data_.robotCurrent, &buf[offset], sizeof(mb_data_.robotCurrent));
-    offset += sizeof(mb_data_.robotCurrent);
-    mb_data_.robotCurrent = ntohl(mb_data_.robotCurrent);
-    memcpy(&mb_data_.masterIOCurrent, &buf[offset],
-        sizeof(mb_data_.masterIOCurrent));
-    offset += sizeof(mb_data_.masterIOCurrent);
-    mb_data_.masterIOCurrent = ntohl(mb_data_.masterIOCurrent);
-
-    memcpy(&mb_data_.safetyMode, &buf[offset], sizeof(mb_data_.safetyMode));
-    offset += sizeof(mb_data_.safetyMode);
-    memcpy(&mb_data_.masterOnOffState, &buf[offset],
-        sizeof(mb_data_.masterOnOffState));
-    offset += sizeof(mb_data_.masterOnOffState);
-
-    memcpy(&mb_data_.euromap67InterfaceInstalled, &buf[offset],
-        sizeof(mb_data_.euromap67InterfaceInstalled));
-    offset += sizeof(mb_data_.euromap67InterfaceInstalled);
-    if (mb_data_.euromap67InterfaceInstalled != 0) {
-        memcpy(&mb_data_.euromapInputBits, &buf[offset],
-            sizeof(mb_data_.euromapInputBits));
-        offset += sizeof(mb_data_.euromapInputBits);
-        mb_data_.euromapInputBits = ntohl(mb_data_.euromapInputBits);
-        memcpy(&mb_data_.euromapOutputBits, &buf[offset],
-            sizeof(mb_data_.euromapOutputBits));
-        offset += sizeof(mb_data_.euromapOutputBits);
-        mb_data_.euromapOutputBits = ntohl(mb_data_.euromapOutputBits);
-        if (RobotState::getVersion() < 3.0) {
-            int16_t euromap_voltage, euromap_current;
-            memcpy(&euromap_voltage, &buf[offset], sizeof(euromap_voltage));
-            offset += sizeof(euromap_voltage);
-            memcpy(&euromap_current, &buf[offset], sizeof(euromap_current));
-            offset += sizeof(euromap_current);
-            mb_data_.euromapVoltage = ntohs(euromap_voltage);
-            mb_data_.euromapCurrent = ntohs(euromap_current);
-        } else {
-            memcpy(&mb_data_.euromapVoltage, &buf[offset],
-                sizeof(mb_data_.euromapVoltage));
-            offset += sizeof(mb_data_.euromapVoltage);
-            mb_data_.euromapVoltage = ntohl(mb_data_.euromapVoltage);
-            memcpy(&mb_data_.euromapCurrent, &buf[offset],
-                sizeof(mb_data_.euromapCurrent));
-            offset += sizeof(mb_data_.euromapCurrent);
-            mb_data_.euromapCurrent = ntohl(mb_data_.euromapCurrent);
-        }
+    else
+    {
+      memcpy(&mb_data_.euromapVoltage, &buf[offset], sizeof(mb_data_.euromapVoltage));
+      offset += sizeof(mb_data_.euromapVoltage);
+      mb_data_.euromapVoltage = ntohl(mb_data_.euromapVoltage);
+      memcpy(&mb_data_.euromapCurrent, &buf[offset], sizeof(mb_data_.euromapCurrent));
+      offset += sizeof(mb_data_.euromapCurrent);
+      mb_data_.euromapCurrent = ntohl(mb_data_.euromapCurrent);
     }
+  }
 }
 
 double RobotState::getVersion()
 {
-    double ver;
-    val_lock_.lock();
-    ver = version_msg_.major_version + 0.1 * version_msg_.minor_version
-        + .0000001 * version_msg_.svn_revision;
-    val_lock_.unlock();
-    return ver;
+  double ver;
+  val_lock_.lock();
+  ver = version_msg_.major_version + 0.1 * version_msg_.minor_version + .0000001 * version_msg_.svn_revision;
+  val_lock_.unlock();
+  return ver;
 }
 
 void RobotState::finishedReading()
 {
-    new_data_available_ = false;
+  new_data_available_ = false;
 }
 
 bool RobotState::getNewDataAvailable()
 {
-    return new_data_available_;
+  return new_data_available_;
 }
 
 int RobotState::getDigitalInputBits()
 {
-    return mb_data_.digitalInputBits;
+  return mb_data_.digitalInputBits;
 }
 int RobotState::getDigitalOutputBits()
 {
-    return mb_data_.digitalOutputBits;
+  return mb_data_.digitalOutputBits;
 }
 double RobotState::getAnalogInput0()
 {
-    return mb_data_.analogInput0;
+  return mb_data_.analogInput0;
 }
 double RobotState::getAnalogInput1()
 {
-    return mb_data_.analogInput1;
+  return mb_data_.analogInput1;
 }
 double RobotState::getAnalogOutput0()
 {
-    return mb_data_.analogOutput0;
+  return mb_data_.analogOutput0;
 }
 double RobotState::getAnalogOutput1()
 {
-    return mb_data_.analogOutput1;
+  return mb_data_.analogOutput1;
 }
 bool RobotState::isRobotConnected()
 {
-    return robot_mode_.isRobotConnected;
+  return robot_mode_.isRobotConnected;
 }
 bool RobotState::isRealRobotEnabled()
 {
-    return robot_mode_.isRealRobotEnabled;
+  return robot_mode_.isRealRobotEnabled;
 }
 bool RobotState::isPowerOnRobot()
 {
-    return robot_mode_.isPowerOnRobot;
+  return robot_mode_.isPowerOnRobot;
 }
 bool RobotState::isEmergencyStopped()
 {
-    return robot_mode_.isEmergencyStopped;
+  return robot_mode_.isEmergencyStopped;
 }
 bool RobotState::isProtectiveStopped()
 {
-    return robot_mode_.isProtectiveStopped;
+  return robot_mode_.isProtectiveStopped;
 }
 bool RobotState::isProgramRunning()
 {
-    return robot_mode_.isProgramRunning;
+  return robot_mode_.isProgramRunning;
 }
 bool RobotState::isProgramPaused()
 {
-    return robot_mode_.isProgramPaused;
+  return robot_mode_.isProgramPaused;
 }
 unsigned char RobotState::getRobotMode()
 {
-    return robot_mode_.robotMode;
+  return robot_mode_.robotMode;
 }
 bool RobotState::isReady()
 {
-    if (robot_mode_.robotMode == robot_mode_running_) {
-        return true;
-    }
-    return false;
+  if (robot_mode_.robotMode == robot_mode_running_)
+  {
+    return true;
+  }
+  return false;
 }
 
 void RobotState::setDisconnected()
 {
-    robot_mode_.isRobotConnected = false;
-    robot_mode_.isRealRobotEnabled = false;
-    robot_mode_.isPowerOnRobot = false;
+  robot_mode_.isRobotConnected = false;
+  robot_mode_.isRealRobotEnabled = false;
+  robot_mode_.isPowerOnRobot = false;
 }
diff --git a/src/robot_state_RT.cpp b/src/robot_state_RT.cpp
index 46467fe..71bb7ba 100644
--- a/src/robot_state_RT.cpp
+++ b/src/robot_state_RT.cpp
@@ -20,456 +20,473 @@
 
 RobotStateRT::RobotStateRT(std::condition_variable& msg_cond)
 {
-    version_ = 0.0;
-    time_ = 0.0;
-    q_target_.assign(6, 0.0);
-    qd_target_.assign(6, 0.0);
-    qdd_target_.assign(6, 0.0);
-    i_target_.assign(6, 0.0);
-    m_target_.assign(6, 0.0);
-    q_actual_.assign(6, 0.0);
-    qd_actual_.assign(6, 0.0);
-    i_actual_.assign(6, 0.0);
-    i_control_.assign(6, 0.0);
-    tool_vector_actual_.assign(6, 0.0);
-    tcp_speed_actual_.assign(6, 0.0);
-    tcp_force_.assign(6, 0.0);
-    tool_vector_target_.assign(6, 0.0);
-    tcp_speed_target_.assign(6, 0.0);
-    digital_input_bits_.assign(64, false);
-    motor_temperatures_.assign(6, 0.0);
-    controller_timer_ = 0.0;
-    robot_mode_ = 0.0;
-    joint_modes_.assign(6, 0.0);
-    safety_mode_ = 0.0;
-    tool_accelerometer_values_.assign(3, 0.0);
-    speed_scaling_ = 0.0;
-    linear_momentum_norm_ = 0.0;
-    v_main_ = 0.0;
-    v_robot_ = 0.0;
-    i_robot_ = 0.0;
-    v_actual_.assign(6, 0.0);
-    data_published_ = false;
-    controller_updated_ = false;
-    pMsg_cond_ = &msg_cond;
+  version_ = 0.0;
+  time_ = 0.0;
+  q_target_.assign(6, 0.0);
+  qd_target_.assign(6, 0.0);
+  qdd_target_.assign(6, 0.0);
+  i_target_.assign(6, 0.0);
+  m_target_.assign(6, 0.0);
+  q_actual_.assign(6, 0.0);
+  qd_actual_.assign(6, 0.0);
+  i_actual_.assign(6, 0.0);
+  i_control_.assign(6, 0.0);
+  tool_vector_actual_.assign(6, 0.0);
+  tcp_speed_actual_.assign(6, 0.0);
+  tcp_force_.assign(6, 0.0);
+  tool_vector_target_.assign(6, 0.0);
+  tcp_speed_target_.assign(6, 0.0);
+  digital_input_bits_.assign(64, false);
+  motor_temperatures_.assign(6, 0.0);
+  controller_timer_ = 0.0;
+  robot_mode_ = 0.0;
+  joint_modes_.assign(6, 0.0);
+  safety_mode_ = 0.0;
+  tool_accelerometer_values_.assign(3, 0.0);
+  speed_scaling_ = 0.0;
+  linear_momentum_norm_ = 0.0;
+  v_main_ = 0.0;
+  v_robot_ = 0.0;
+  i_robot_ = 0.0;
+  v_actual_.assign(6, 0.0);
+  data_published_ = false;
+  controller_updated_ = false;
+  pMsg_cond_ = &msg_cond;
 }
 
 RobotStateRT::~RobotStateRT()
 {
-    /* Make sure nobody is waiting after this thread is destroyed */
-    data_published_ = true;
-    controller_updated_ = true;
-    pMsg_cond_->notify_all();
+  /* Make sure nobody is waiting after this thread is destroyed */
+  data_published_ = true;
+  controller_updated_ = true;
+  pMsg_cond_->notify_all();
 }
 
 void RobotStateRT::setDataPublished()
 {
-    data_published_ = false;
+  data_published_ = false;
 }
 bool RobotStateRT::getDataPublished()
 {
-    return data_published_;
+  return data_published_;
 }
 
 void RobotStateRT::setControllerUpdated()
 {
-    controller_updated_ = false;
+  controller_updated_ = false;
 }
 bool RobotStateRT::getControllerUpdated()
 {
-    return controller_updated_;
+  return controller_updated_;
 }
 
 double RobotStateRT::ntohd(uint64_t nf)
 {
-    double x;
-    nf = be64toh(nf);
-    memcpy(&x, &nf, sizeof(x));
-    return x;
+  double x;
+  nf = be64toh(nf);
+  memcpy(&x, &nf, sizeof(x));
+  return x;
 }
 
-std::vector<double> RobotStateRT::unpackVector(uint8_t* buf, int start_index,
-    int nr_of_vals)
+std::vector<double> RobotStateRT::unpackVector(uint8_t* buf, int start_index, int nr_of_vals)
 {
-    uint64_t q;
-    std::vector<double> ret;
-    for (int i = 0; i < nr_of_vals; i++) {
-        memcpy(&q, &buf[start_index + i * sizeof(q)], sizeof(q));
-        ret.push_back(ntohd(q));
-    }
-    return ret;
+  uint64_t q;
+  std::vector<double> ret;
+  for (int i = 0; i < nr_of_vals; i++)
+  {
+    memcpy(&q, &buf[start_index + i * sizeof(q)], sizeof(q));
+    ret.push_back(ntohd(q));
+  }
+  return ret;
 }
 
 std::vector<bool> RobotStateRT::unpackDigitalInputBits(int64_t data)
 {
-    std::vector<bool> ret;
-    for (int i = 0; i < 64; i++) {
-        ret.push_back((data & (1 << i)) >> i);
-    }
-    return ret;
+  std::vector<bool> ret;
+  for (int i = 0; i < 64; i++)
+  {
+    ret.push_back((data & (1 << i)) >> i);
+  }
+  return ret;
 }
 
 void RobotStateRT::setVersion(double ver)
 {
-    val_lock_.lock();
-    version_ = ver;
-    val_lock_.unlock();
+  val_lock_.lock();
+  version_ = ver;
+  val_lock_.unlock();
 }
 
 double RobotStateRT::getVersion()
 {
-    double ret;
-    val_lock_.lock();
-    ret = version_;
-    val_lock_.unlock();
-    return ret;
+  double ret;
+  val_lock_.lock();
+  ret = version_;
+  val_lock_.unlock();
+  return ret;
 }
 double RobotStateRT::getTime()
 {
-    double ret;
-    val_lock_.lock();
-    ret = time_;
-    val_lock_.unlock();
-    return ret;
+  double ret;
+  val_lock_.lock();
+  ret = time_;
+  val_lock_.unlock();
+  return ret;
 }
 std::vector<double> RobotStateRT::getQTarget()
 {
-    std::vector<double> ret;
-    val_lock_.lock();
-    ret = q_target_;
-    val_lock_.unlock();
-    return ret;
+  std::vector<double> ret;
+  val_lock_.lock();
+  ret = q_target_;
+  val_lock_.unlock();
+  return ret;
 }
 std::vector<double> RobotStateRT::getQdTarget()
 {
-    std::vector<double> ret;
-    val_lock_.lock();
-    ret = qd_target_;
-    val_lock_.unlock();
-    return ret;
+  std::vector<double> ret;
+  val_lock_.lock();
+  ret = qd_target_;
+  val_lock_.unlock();
+  return ret;
 }
 std::vector<double> RobotStateRT::getQddTarget()
 {
-    std::vector<double> ret;
-    val_lock_.lock();
-    ret = qdd_target_;
-    val_lock_.unlock();
-    return ret;
+  std::vector<double> ret;
+  val_lock_.lock();
+  ret = qdd_target_;
+  val_lock_.unlock();
+  return ret;
 }
 std::vector<double> RobotStateRT::getITarget()
 {
-    std::vector<double> ret;
-    val_lock_.lock();
-    ret = i_target_;
-    val_lock_.unlock();
-    return ret;
+  std::vector<double> ret;
+  val_lock_.lock();
+  ret = i_target_;
+  val_lock_.unlock();
+  return ret;
 }
 std::vector<double> RobotStateRT::getMTarget()
 {
-    std::vector<double> ret;
-    val_lock_.lock();
-    ret = m_target_;
-    val_lock_.unlock();
-    return ret;
+  std::vector<double> ret;
+  val_lock_.lock();
+  ret = m_target_;
+  val_lock_.unlock();
+  return ret;
 }
 std::vector<double> RobotStateRT::getQActual()
 {
-    std::vector<double> ret;
-    val_lock_.lock();
-    ret = q_actual_;
-    val_lock_.unlock();
-    return ret;
+  std::vector<double> ret;
+  val_lock_.lock();
+  ret = q_actual_;
+  val_lock_.unlock();
+  return ret;
 }
 std::vector<double> RobotStateRT::getQdActual()
 {
-    std::vector<double> ret;
-    val_lock_.lock();
-    ret = qd_actual_;
-    val_lock_.unlock();
-    return ret;
+  std::vector<double> ret;
+  val_lock_.lock();
+  ret = qd_actual_;
+  val_lock_.unlock();
+  return ret;
 }
 std::vector<double> RobotStateRT::getIActual()
 {
-    std::vector<double> ret;
-    val_lock_.lock();
-    ret = i_actual_;
-    val_lock_.unlock();
-    return ret;
+  std::vector<double> ret;
+  val_lock_.lock();
+  ret = i_actual_;
+  val_lock_.unlock();
+  return ret;
 }
 std::vector<double> RobotStateRT::getIControl()
 {
-    std::vector<double> ret;
-    val_lock_.lock();
-    ret = i_control_;
-    val_lock_.unlock();
-    return ret;
+  std::vector<double> ret;
+  val_lock_.lock();
+  ret = i_control_;
+  val_lock_.unlock();
+  return ret;
 }
 std::vector<double> RobotStateRT::getToolVectorActual()
 {
-    std::vector<double> ret;
-    val_lock_.lock();
-    ret = tool_vector_actual_;
-    val_lock_.unlock();
-    return ret;
+  std::vector<double> ret;
+  val_lock_.lock();
+  ret = tool_vector_actual_;
+  val_lock_.unlock();
+  return ret;
 }
 std::vector<double> RobotStateRT::getTcpSpeedActual()
 {
-    std::vector<double> ret;
-    val_lock_.lock();
-    ret = tcp_speed_actual_;
-    val_lock_.unlock();
-    return ret;
+  std::vector<double> ret;
+  val_lock_.lock();
+  ret = tcp_speed_actual_;
+  val_lock_.unlock();
+  return ret;
 }
 std::vector<double> RobotStateRT::getTcpForce()
 {
-    std::vector<double> ret;
-    val_lock_.lock();
-    ret = tcp_force_;
-    val_lock_.unlock();
-    return ret;
+  std::vector<double> ret;
+  val_lock_.lock();
+  ret = tcp_force_;
+  val_lock_.unlock();
+  return ret;
 }
 std::vector<double> RobotStateRT::getToolVectorTarget()
 {
-    std::vector<double> ret;
-    val_lock_.lock();
-    ret = tool_vector_target_;
-    val_lock_.unlock();
-    return ret;
+  std::vector<double> ret;
+  val_lock_.lock();
+  ret = tool_vector_target_;
+  val_lock_.unlock();
+  return ret;
 }
 std::vector<double> RobotStateRT::getTcpSpeedTarget()
 {
-    std::vector<double> ret;
-    val_lock_.lock();
-    ret = tcp_speed_target_;
-    val_lock_.unlock();
-    return ret;
+  std::vector<double> ret;
+  val_lock_.lock();
+  ret = tcp_speed_target_;
+  val_lock_.unlock();
+  return ret;
 }
 std::vector<bool> RobotStateRT::getDigitalInputBits()
 {
-    std::vector<bool> ret;
-    val_lock_.lock();
-    ret = digital_input_bits_;
-    val_lock_.unlock();
-    return ret;
+  std::vector<bool> ret;
+  val_lock_.lock();
+  ret = digital_input_bits_;
+  val_lock_.unlock();
+  return ret;
 }
 std::vector<double> RobotStateRT::getMotorTemperatures()
 {
-    std::vector<double> ret;
-    val_lock_.lock();
-    ret = motor_temperatures_;
-    val_lock_.unlock();
-    return ret;
+  std::vector<double> ret;
+  val_lock_.lock();
+  ret = motor_temperatures_;
+  val_lock_.unlock();
+  return ret;
 }
 double RobotStateRT::getControllerTimer()
 {
-    double ret;
-    val_lock_.lock();
-    ret = controller_timer_;
-    val_lock_.unlock();
-    return ret;
+  double ret;
+  val_lock_.lock();
+  ret = controller_timer_;
+  val_lock_.unlock();
+  return ret;
 }
 double RobotStateRT::getRobotMode()
 {
-    double ret;
-    val_lock_.lock();
-    ret = robot_mode_;
-    val_lock_.unlock();
-    return ret;
+  double ret;
+  val_lock_.lock();
+  ret = robot_mode_;
+  val_lock_.unlock();
+  return ret;
 }
 std::vector<double> RobotStateRT::getJointModes()
 {
-    std::vector<double> ret;
-    val_lock_.lock();
-    ret = joint_modes_;
-    val_lock_.unlock();
-    return ret;
+  std::vector<double> ret;
+  val_lock_.lock();
+  ret = joint_modes_;
+  val_lock_.unlock();
+  return ret;
 }
 double RobotStateRT::getSafety_mode()
 {
-    double ret;
-    val_lock_.lock();
-    ret = safety_mode_;
-    val_lock_.unlock();
-    return ret;
+  double ret;
+  val_lock_.lock();
+  ret = safety_mode_;
+  val_lock_.unlock();
+  return ret;
 }
 std::vector<double> RobotStateRT::getToolAccelerometerValues()
 {
-    std::vector<double> ret;
-    val_lock_.lock();
-    ret = tool_accelerometer_values_;
-    val_lock_.unlock();
-    return ret;
+  std::vector<double> ret;
+  val_lock_.lock();
+  ret = tool_accelerometer_values_;
+  val_lock_.unlock();
+  return ret;
 }
 double RobotStateRT::getSpeedScaling()
 {
-    double ret;
-    val_lock_.lock();
-    ret = speed_scaling_;
-    val_lock_.unlock();
-    return ret;
+  double ret;
+  val_lock_.lock();
+  ret = speed_scaling_;
+  val_lock_.unlock();
+  return ret;
 }
 double RobotStateRT::getLinearMomentumNorm()
 {
-    double ret;
-    val_lock_.lock();
-    ret = linear_momentum_norm_;
-    val_lock_.unlock();
-    return ret;
+  double ret;
+  val_lock_.lock();
+  ret = linear_momentum_norm_;
+  val_lock_.unlock();
+  return ret;
 }
 double RobotStateRT::getVMain()
 {
-    double ret;
-    val_lock_.lock();
-    ret = v_main_;
-    val_lock_.unlock();
-    return ret;
+  double ret;
+  val_lock_.lock();
+  ret = v_main_;
+  val_lock_.unlock();
+  return ret;
 }
 double RobotStateRT::getVRobot()
 {
-    double ret;
-    val_lock_.lock();
-    ret = v_robot_;
-    val_lock_.unlock();
-    return ret;
+  double ret;
+  val_lock_.lock();
+  ret = v_robot_;
+  val_lock_.unlock();
+  return ret;
 }
 double RobotStateRT::getIRobot()
 {
-    double ret;
-    val_lock_.lock();
-    ret = i_robot_;
-    val_lock_.unlock();
-    return ret;
+  double ret;
+  val_lock_.lock();
+  ret = i_robot_;
+  val_lock_.unlock();
+  return ret;
 }
 std::vector<double> RobotStateRT::getVActual()
 {
-    std::vector<double> ret;
-    val_lock_.lock();
-    ret = v_actual_;
-    val_lock_.unlock();
-    return ret;
+  std::vector<double> ret;
+  val_lock_.lock();
+  ret = v_actual_;
+  val_lock_.unlock();
+  return ret;
 }
 void RobotStateRT::unpack(uint8_t* buf)
 {
-    int64_t digital_input_bits;
-    uint64_t unpack_to;
-    uint16_t offset = 0;
-    val_lock_.lock();
-    int len;
-    memcpy(&len, &buf[offset], sizeof(len));
+  int64_t digital_input_bits;
+  uint64_t unpack_to;
+  uint16_t offset = 0;
+  val_lock_.lock();
+  int len;
+  memcpy(&len, &buf[offset], sizeof(len));
 
-    offset += sizeof(len);
-    len = ntohl(len);
+  offset += sizeof(len);
+  len = ntohl(len);
 
-    //Check the correct message length is received
-    bool len_good = true;
-    if (version_ >= 1.6 && version_ < 1.7) { //v1.6
-        if (len != 756)
-            len_good = false;
-    } else if (version_ >= 1.7 && version_ < 1.8) { //v1.7
-        if (len != 764)
-            len_good = false;
-    } else if (version_ >= 1.8 && version_ < 1.9) { //v1.8
-        if (len != 812)
-            len_good = false;
-    } else if (version_ >= 3.0 && version_ < 3.2) { //v3.0 & v3.1
-        if (len != 1044)
-            len_good = false;
-    } else if (version_ >= 3.2 && version_ < 3.3) { //v3.2
-        if (len != 1060)
-            len_good = false;
-    }
+  // Check the correct message length is received
+  bool len_good = true;
+  if (version_ >= 1.6 && version_ < 1.7)
+  {  // v1.6
+    if (len != 756)
+      len_good = false;
+  }
+  else if (version_ >= 1.7 && version_ < 1.8)
+  {  // v1.7
+    if (len != 764)
+      len_good = false;
+  }
+  else if (version_ >= 1.8 && version_ < 1.9)
+  {  // v1.8
+    if (len != 812)
+      len_good = false;
+  }
+  else if (version_ >= 3.0 && version_ < 3.2)
+  {  // v3.0 & v3.1
+    if (len != 1044)
+      len_good = false;
+  }
+  else if (version_ >= 3.2 && version_ < 3.3)
+  {  // v3.2
+    if (len != 1060)
+      len_good = false;
+  }
 
-    if (!len_good) {
-        printf("Wrong length of message on RT interface: %i\n", len);
-        val_lock_.unlock();
-        return;
-    }
-
-    memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
-    time_ = RobotStateRT::ntohd(unpack_to);
-    offset += sizeof(double);
-    q_target_ = unpackVector(buf, offset, 6);
-    offset += sizeof(double) * 6;
-    qd_target_ = unpackVector(buf, offset, 6);
-    offset += sizeof(double) * 6;
-    qdd_target_ = unpackVector(buf, offset, 6);
-    offset += sizeof(double) * 6;
-    i_target_ = unpackVector(buf, offset, 6);
-    offset += sizeof(double) * 6;
-    m_target_ = unpackVector(buf, offset, 6);
-    offset += sizeof(double) * 6;
-    q_actual_ = unpackVector(buf, offset, 6);
-    offset += sizeof(double) * 6;
-    qd_actual_ = unpackVector(buf, offset, 6);
-    offset += sizeof(double) * 6;
-    i_actual_ = unpackVector(buf, offset, 6);
-    offset += sizeof(double) * 6;
-    if (version_ <= 1.9) {
-        if (version_ > 1.6)
-            tool_accelerometer_values_ = unpackVector(buf, offset, 3);
-        offset += sizeof(double) * (3 + 15);
-        tcp_force_ = unpackVector(buf, offset, 6);
-        offset += sizeof(double) * 6;
-        tool_vector_actual_ = unpackVector(buf, offset, 6);
-        offset += sizeof(double) * 6;
-        tcp_speed_actual_ = unpackVector(buf, offset, 6);
-    } else {
-        i_control_ = unpackVector(buf, offset, 6);
-        offset += sizeof(double) * 6;
-        tool_vector_actual_ = unpackVector(buf, offset, 6);
-        offset += sizeof(double) * 6;
-        tcp_speed_actual_ = unpackVector(buf, offset, 6);
-        offset += sizeof(double) * 6;
-        tcp_force_ = unpackVector(buf, offset, 6);
-        offset += sizeof(double) * 6;
-        tool_vector_target_ = unpackVector(buf, offset, 6);
-        offset += sizeof(double) * 6;
-        tcp_speed_target_ = unpackVector(buf, offset, 6);
-    }
-    offset += sizeof(double) * 6;
-
-    memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits));
-    digital_input_bits_ = unpackDigitalInputBits(be64toh(digital_input_bits));
-    offset += sizeof(double);
-    motor_temperatures_ = unpackVector(buf, offset, 6);
-    offset += sizeof(double) * 6;
-    memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
-    controller_timer_ = ntohd(unpack_to);
-    if (version_ > 1.6) {
-        offset += sizeof(double) * 2;
-        memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
-        robot_mode_ = ntohd(unpack_to);
-        if (version_ > 1.7) {
-            offset += sizeof(double);
-            joint_modes_ = unpackVector(buf, offset, 6);
-        }
-    }
-    if (version_ > 1.8) {
-        offset += sizeof(double) * 6;
-        memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
-        safety_mode_ = ntohd(unpack_to);
-        offset += sizeof(double);
-        tool_accelerometer_values_ = unpackVector(buf, offset, 3);
-        offset += sizeof(double) * 3;
-        memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
-        speed_scaling_ = ntohd(unpack_to);
-        offset += sizeof(double);
-        memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
-        linear_momentum_norm_ = ntohd(unpack_to);
-        offset += sizeof(double);
-        memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
-        v_main_ = ntohd(unpack_to);
-        offset += sizeof(double);
-        memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
-        v_robot_ = ntohd(unpack_to);
-        offset += sizeof(double);
-        memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
-        i_robot_ = ntohd(unpack_to);
-        offset += sizeof(double);
-        v_actual_ = unpackVector(buf, offset, 6);
-    }
+  if (!len_good)
+  {
+    printf("Wrong length of message on RT interface: %i\n", len);
     val_lock_.unlock();
-    controller_updated_ = true;
-    data_published_ = true;
-    pMsg_cond_->notify_all();
+    return;
+  }
+
+  memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+  time_ = RobotStateRT::ntohd(unpack_to);
+  offset += sizeof(double);
+  q_target_ = unpackVector(buf, offset, 6);
+  offset += sizeof(double) * 6;
+  qd_target_ = unpackVector(buf, offset, 6);
+  offset += sizeof(double) * 6;
+  qdd_target_ = unpackVector(buf, offset, 6);
+  offset += sizeof(double) * 6;
+  i_target_ = unpackVector(buf, offset, 6);
+  offset += sizeof(double) * 6;
+  m_target_ = unpackVector(buf, offset, 6);
+  offset += sizeof(double) * 6;
+  q_actual_ = unpackVector(buf, offset, 6);
+  offset += sizeof(double) * 6;
+  qd_actual_ = unpackVector(buf, offset, 6);
+  offset += sizeof(double) * 6;
+  i_actual_ = unpackVector(buf, offset, 6);
+  offset += sizeof(double) * 6;
+  if (version_ <= 1.9)
+  {
+    if (version_ > 1.6)
+      tool_accelerometer_values_ = unpackVector(buf, offset, 3);
+    offset += sizeof(double) * (3 + 15);
+    tcp_force_ = unpackVector(buf, offset, 6);
+    offset += sizeof(double) * 6;
+    tool_vector_actual_ = unpackVector(buf, offset, 6);
+    offset += sizeof(double) * 6;
+    tcp_speed_actual_ = unpackVector(buf, offset, 6);
+  }
+  else
+  {
+    i_control_ = unpackVector(buf, offset, 6);
+    offset += sizeof(double) * 6;
+    tool_vector_actual_ = unpackVector(buf, offset, 6);
+    offset += sizeof(double) * 6;
+    tcp_speed_actual_ = unpackVector(buf, offset, 6);
+    offset += sizeof(double) * 6;
+    tcp_force_ = unpackVector(buf, offset, 6);
+    offset += sizeof(double) * 6;
+    tool_vector_target_ = unpackVector(buf, offset, 6);
+    offset += sizeof(double) * 6;
+    tcp_speed_target_ = unpackVector(buf, offset, 6);
+  }
+  offset += sizeof(double) * 6;
+
+  memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits));
+  digital_input_bits_ = unpackDigitalInputBits(be64toh(digital_input_bits));
+  offset += sizeof(double);
+  motor_temperatures_ = unpackVector(buf, offset, 6);
+  offset += sizeof(double) * 6;
+  memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+  controller_timer_ = ntohd(unpack_to);
+  if (version_ > 1.6)
+  {
+    offset += sizeof(double) * 2;
+    memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+    robot_mode_ = ntohd(unpack_to);
+    if (version_ > 1.7)
+    {
+      offset += sizeof(double);
+      joint_modes_ = unpackVector(buf, offset, 6);
+    }
+  }
+  if (version_ > 1.8)
+  {
+    offset += sizeof(double) * 6;
+    memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+    safety_mode_ = ntohd(unpack_to);
+    offset += sizeof(double);
+    tool_accelerometer_values_ = unpackVector(buf, offset, 3);
+    offset += sizeof(double) * 3;
+    memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+    speed_scaling_ = ntohd(unpack_to);
+    offset += sizeof(double);
+    memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+    linear_momentum_norm_ = ntohd(unpack_to);
+    offset += sizeof(double);
+    memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+    v_main_ = ntohd(unpack_to);
+    offset += sizeof(double);
+    memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+    v_robot_ = ntohd(unpack_to);
+    offset += sizeof(double);
+    memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+    i_robot_ = ntohd(unpack_to);
+    offset += sizeof(double);
+    v_actual_ = unpackVector(buf, offset, 6);
+  }
+  val_lock_.unlock();
+  controller_updated_ = true;
+  data_published_ = true;
+  pMsg_cond_->notify_all();
 }
diff --git a/src/ros/rt_publisher.cpp b/src/ros/rt_publisher.cpp
index d58b023..26a283b 100644
--- a/src/ros/rt_publisher.cpp
+++ b/src/ros/rt_publisher.cpp
@@ -1,119 +1,122 @@
 #include "ur_modern_driver/ros/rt_publisher.h"
 
-bool RTPublisher::publish_joints(RTShared& packet, Time& t)
+bool RTPublisher::publishJoints(RTShared& packet, Time& t)
 {
-    sensor_msgs::JointState joint_msg;
-    joint_msg.header.stamp = t;
-    joint_msg.name = _joint_names;
+  sensor_msgs::JointState joint_msg;
+  joint_msg.header.stamp = t;
+  joint_msg.name = joint_names_;
 
-    for (auto const& q : packet.q_actual) {
-        joint_msg.position.push_back(q);
-    }
-    for (auto const& qd : packet.qd_actual) {
-        joint_msg.velocity.push_back(qd);
-    }
-    for (auto const& i : packet.i_actual) {
-        joint_msg.effort.push_back(i);
-    }
+  for (auto const& q : packet.q_actual)
+  {
+    joint_msg.position.push_back(q);
+  }
+  for (auto const& qd : packet.qd_actual)
+  {
+    joint_msg.velocity.push_back(qd);
+  }
+  for (auto const& i : packet.i_actual)
+  {
+    joint_msg.effort.push_back(i);
+  }
 
-    _joint_pub.publish(joint_msg);
+  joint_pub_.publish(joint_msg);
 
-    return true;
+  return true;
 }
 
-bool RTPublisher::publish_wrench(RTShared& packet, Time& t)
+bool RTPublisher::publishWrench(RTShared& packet, Time& t)
 {
-    geometry_msgs::WrenchStamped wrench_msg;
-    wrench_msg.header.stamp = t;
-    wrench_msg.wrench.force.x = packet.tcp_force[0];
-    wrench_msg.wrench.force.y = packet.tcp_force[1];
-    wrench_msg.wrench.force.z = packet.tcp_force[2];
-    wrench_msg.wrench.torque.x = packet.tcp_force[3];
-    wrench_msg.wrench.torque.y = packet.tcp_force[4];
-    wrench_msg.wrench.torque.z = packet.tcp_force[5];
+  geometry_msgs::WrenchStamped wrench_msg;
+  wrench_msg.header.stamp = t;
+  wrench_msg.wrench.force.x = packet.tcp_force[0];
+  wrench_msg.wrench.force.y = packet.tcp_force[1];
+  wrench_msg.wrench.force.z = packet.tcp_force[2];
+  wrench_msg.wrench.torque.x = packet.tcp_force[3];
+  wrench_msg.wrench.torque.y = packet.tcp_force[4];
+  wrench_msg.wrench.torque.z = packet.tcp_force[5];
 
-    _wrench_pub.publish(wrench_msg);
-    return true;
+  wrench_pub_.publish(wrench_msg);
+  return true;
 }
 
-bool RTPublisher::publish_tool(RTShared& packet, Time& t)
+bool RTPublisher::publishTool(RTShared& packet, Time& t)
 {
-    geometry_msgs::TwistStamped tool_twist;
-    tool_twist.header.stamp = t;
-    tool_twist.header.frame_id = _base_frame;
-    tool_twist.twist.linear.x = packet.tcp_speed_actual.position.x;
-    tool_twist.twist.linear.y = packet.tcp_speed_actual.position.y;
-    tool_twist.twist.linear.z = packet.tcp_speed_actual.position.z;
-    tool_twist.twist.angular.x = packet.tcp_speed_actual.rotation.x;
-    tool_twist.twist.angular.y = packet.tcp_speed_actual.rotation.y;
-    tool_twist.twist.angular.z = packet.tcp_speed_actual.rotation.z;
+  geometry_msgs::TwistStamped tool_twist;
+  tool_twist.header.stamp = t;
+  tool_twist.header.frame_id = base_frame_;
+  tool_twist.twist.linear.x = packet.tcp_speed_actual.position.x;
+  tool_twist.twist.linear.y = packet.tcp_speed_actual.position.y;
+  tool_twist.twist.linear.z = packet.tcp_speed_actual.position.z;
+  tool_twist.twist.angular.x = packet.tcp_speed_actual.rotation.x;
+  tool_twist.twist.angular.y = packet.tcp_speed_actual.rotation.y;
+  tool_twist.twist.angular.z = packet.tcp_speed_actual.rotation.z;
 
-    _tool_vel_pub.publish(tool_twist);
-    return true;
+  tool_vel_pub_.publish(tool_twist);
+  return true;
 }
 
-bool RTPublisher::publish_transform(RTShared& packet, Time& t)
+bool RTPublisher::publishTransform(RTShared& packet, Time& t)
 {
+  auto tv = packet.tool_vector_actual;
 
-    auto tv = packet.tool_vector_actual;
+  Transform transform;
+  transform.setOrigin(Vector3(tv.position.x, tv.position.y, tv.position.z));
 
-    Transform transform;
-    transform.setOrigin(Vector3(tv.position.x, tv.position.y, tv.position.z));
+  // Create quaternion
+  Quaternion quat;
 
-    //Create quaternion
-    Quaternion quat;
+  double angle = std::sqrt(std::pow(tv.rotation.x, 2) + std::pow(tv.rotation.y, 2) + std::pow(tv.rotation.z, 2));
+  if (angle < 1e-16)
+  {
+    quat.setValue(0, 0, 0, 1);
+  }
+  else
+  {
+    quat.setRotation(Vector3(tv.rotation.x / angle, tv.rotation.y / angle, tv.rotation.z / angle), angle);
+  }
 
-    double angle = std::sqrt(std::pow(tv.rotation.x, 2) + std::pow(tv.rotation.y, 2) + std::pow(tv.rotation.z, 2));
-    if (angle < 1e-16) {
-        quat.setValue(0, 0, 0, 1);
-    } else {
-        quat.setRotation(Vector3(tv.rotation.x / angle, tv.rotation.y / angle, tv.rotation.z / angle), angle);
-    }
+  transform.setRotation(quat);
 
-    transform.setRotation(quat);
+  transform_broadcaster_.sendTransform(StampedTransform(transform, t, base_frame_, tool_frame_));
 
-    _transform_broadcaster.sendTransform(StampedTransform(transform, t, _base_frame, _tool_frame));
-
-    return true;
+  return true;
 }
 
-bool RTPublisher::publish_temperature(RTShared& packet, Time& t)
+bool RTPublisher::publishTemperature(RTShared& packet, Time& t)
 {
-    size_t len = _joint_names.size();
-    for (size_t i = 0; i < len; i++) {
-        sensor_msgs::Temperature msg;
-        msg.header.stamp = t;
-        msg.header.frame_id = _joint_names[i];
-        msg.temperature = packet.motor_temperatures[i];
+  size_t len = joint_names_.size();
+  for (size_t i = 0; i < len; i++)
+  {
+    sensor_msgs::Temperature msg;
+    msg.header.stamp = t;
+    msg.header.frame_id = joint_names_[i];
+    msg.temperature = packet.motor_temperatures[i];
 
-        _joint_temperature_pub.publish(msg);
-    }
-    return true;
+    joint_temperature_pub_.publish(msg);
+  }
+  return true;
 }
 
 bool RTPublisher::publish(RTShared& packet)
 {
-    Time time = Time::now();
-    return publish_joints(packet, time)
-        && publish_wrench(packet, time)
-        && publish_tool(packet, time)
-        && publish_transform(packet, time)
-        && publish_temperature(packet, time);
+  Time time = Time::now();
+  return publishJoints(packet, time) && publishWrench(packet, time) && publishTool(packet, time) &&
+         publishTransform(packet, time) && publishTemperature(packet, time);
 }
 
 bool RTPublisher::consume(RTState_V1_6__7& state)
 {
-    return publish(state);
+  return publish(state);
 }
 bool RTPublisher::consume(RTState_V1_8& state)
 {
-    return publish(state);
+  return publish(state);
 }
 bool RTPublisher::consume(RTState_V3_0__1& state)
 {
-    return publish(state);
+  return publish(state);
 }
 bool RTPublisher::consume(RTState_V3_2__3& state)
 {
-    return publish(state);
+  return publish(state);
 }
\ No newline at end of file
diff --git a/src/ros_main.cpp b/src/ros_main.cpp
index adf7a99..09b142a 100644
--- a/src/ros_main.cpp
+++ b/src/ros_main.cpp
@@ -1,6 +1,6 @@
+#include <ros/ros.h>
 #include <chrono>
 #include <cstdlib>
-#include <ros/ros.h>
 #include <string>
 #include <thread>
 
@@ -21,55 +21,59 @@ static const std::string PREFIX_ARG("~prefix");
 static const std::string BASE_FRAME_ARG("~base_frame");
 static const std::string TOOL_FRAME_ARG("~tool_frame");
 
-struct ProgArgs {
+struct ProgArgs
+{
 public:
-    std::string host;
-    std::string prefix;
-    std::string base_frame;
-    std::string tool_frame;
-    int32_t reverse_port;
-    bool use_sim_time;
+  std::string host;
+  std::string prefix;
+  std::string base_frame;
+  std::string tool_frame;
+  int32_t reverse_port;
+  bool use_sim_time;
 };
 
 bool parse_args(ProgArgs& args)
 {
-    if (!ros::param::get(IP_ADDR_ARG, args.host)) {
-        LOG_ERROR("robot_ip_address parameter must be set!");
-        return false;
-    }
-    ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001));
-    ros::param::param(SIM_TIME_ARG, args.use_sim_time, false);
-    ros::param::param(PREFIX_ARG, args.prefix, std::string());
-    ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link");
-    ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller");
-    return true;
+  if (!ros::param::get(IP_ADDR_ARG, args.host))
+  {
+    LOG_ERROR("robot_ip_address parameter must be set!");
+    return false;
+  }
+  ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001));
+  ros::param::param(SIM_TIME_ARG, args.use_sim_time, false);
+  ros::param::param(PREFIX_ARG, args.prefix, std::string());
+  ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link");
+  ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller");
+  return true;
 }
 
 int main(int argc, char** argv)
 {
-    ros::init(argc, argv, "ur_driver");
+  ros::init(argc, argv, "ur_driver");
 
-    ProgArgs args;
-    if (!parse_args(args)) {
-        return EXIT_FAILURE;
-    }
+  ProgArgs args;
+  if (!parse_args(args))
+  {
+    return EXIT_FAILURE;
+  }
 
-    URFactory factory(args.host);
-    auto parser = factory.get_rt_parser();
+  URFactory factory(args.host);
+  auto parser = factory.getRTParser();
 
-    URStream s2(args.host, 30003);
-    URProducer<RTPacket> p2(s2, *parser);
-    RTPublisher pub(args.prefix, args.base_frame, args.tool_frame);
+  URStream s2(args.host, 30003);
+  URProducer<RTPacket> p2(s2, *parser);
+  RTPublisher pub(args.prefix, args.base_frame, args.tool_frame);
 
-    Pipeline<RTPacket> pl(p2, pub);
+  Pipeline<RTPacket> pl(p2, pub);
 
-    pl.run();
+  pl.run();
 
-    while (ros::ok()) {
-        std::this_thread::sleep_for(std::chrono::milliseconds(33));
-    }
+  while (ros::ok())
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(33));
+  }
 
-    pl.stop();
+  pl.stop();
 
-    return EXIT_SUCCESS;
+  return EXIT_SUCCESS;
 }
\ No newline at end of file
diff --git a/src/ur/master_board.cpp b/src/ur/master_board.cpp
index 046d0ee..ecada69 100644
--- a/src/ur/master_board.cpp
+++ b/src/ur/master_board.cpp
@@ -1,97 +1,99 @@
 #include "ur_modern_driver/ur/master_board.h"
 #include "ur_modern_driver/ur/consumer.h"
 
-bool SharedMasterBoardData::parse_with(BinParser& bp)
+bool SharedMasterBoardData::parseWith(BinParser& bp)
 {
-    bp.parse(analog_input_range0);
-    bp.parse(analog_input_range1);
-    bp.parse(analog_input0);
-    bp.parse(analog_input1);
-    bp.parse(analog_output_domain0);
-    bp.parse(analog_output_domain1);
-    bp.parse(analog_output0);
-    bp.parse(analog_output1);
-    bp.parse(master_board_temperature);
-    bp.parse(robot_voltage_48V);
-    bp.parse(robot_current);
-    bp.parse(master_IO_current);
-    return true;
+  bp.parse(analog_input_range0);
+  bp.parse(analog_input_range1);
+  bp.parse(analog_input0);
+  bp.parse(analog_input1);
+  bp.parse(analog_output_domain0);
+  bp.parse(analog_output_domain1);
+  bp.parse(analog_output0);
+  bp.parse(analog_output1);
+  bp.parse(master_board_temperature);
+  bp.parse(robot_voltage_48V);
+  bp.parse(robot_current);
+  bp.parse(master_IO_current);
+  return true;
 }
 
-bool MasterBoardData_V1_X::parse_with(BinParser& bp)
+bool MasterBoardData_V1_X::parseWith(BinParser& bp)
 {
-    if (!bp.check_size<MasterBoardData_V1_X>())
-        return false;
+  if (!bp.checkSize<MasterBoardData_V1_X>())
+    return false;
 
-    bp.parse(digital_input_bits);
-    bp.parse(digital_output_bits);
+  bp.parse(digital_input_bits);
+  bp.parse(digital_output_bits);
 
-    SharedMasterBoardData::parse_with(bp);
+  SharedMasterBoardData::parseWith(bp);
 
-    bp.parse(master_safety_state);
-    bp.parse(master_on_off_state);
-    bp.parse(euromap67_interface_installed);
+  bp.parse(master_safety_state);
+  bp.parse(master_on_off_state);
+  bp.parse(euromap67_interface_installed);
 
-    if (euromap67_interface_installed) {
-        if (!bp.check_size(MasterBoardData_V1_X::EURO_SIZE))
-            return false;
+  if (euromap67_interface_installed)
+  {
+    if (!bp.checkSize(MasterBoardData_V1_X::EURO_SIZE))
+      return false;
 
-        bp.parse(euromap_voltage);
-        bp.parse(euromap_current);
-    }
+    bp.parse(euromap_voltage);
+    bp.parse(euromap_current);
+  }
 
-    return true;
+  return true;
 }
 
-bool MasterBoardData_V3_0__1::parse_with(BinParser& bp)
+bool MasterBoardData_V3_0__1::parseWith(BinParser& bp)
 {
-    if (!bp.check_size<MasterBoardData_V3_0__1>())
-        return false;
+  if (!bp.checkSize<MasterBoardData_V3_0__1>())
+    return false;
 
-    bp.parse(digital_input_bits);
-    bp.parse(digital_output_bits);
+  bp.parse(digital_input_bits);
+  bp.parse(digital_output_bits);
 
-    SharedMasterBoardData::parse_with(bp);
+  SharedMasterBoardData::parseWith(bp);
 
-    bp.parse(safety_mode);
-    bp.parse(in_reduced_mode);
-    bp.parse(euromap67_interface_installed);
+  bp.parse(safety_mode);
+  bp.parse(in_reduced_mode);
+  bp.parse(euromap67_interface_installed);
 
-    if (euromap67_interface_installed) {
-        if (!bp.check_size(MasterBoardData_V3_0__1::EURO_SIZE))
-            return false;
+  if (euromap67_interface_installed)
+  {
+    if (!bp.checkSize(MasterBoardData_V3_0__1::EURO_SIZE))
+      return false;
 
-        bp.parse(euromap_voltage);
-        bp.parse(euromap_current);
-    }
+    bp.parse(euromap_voltage);
+    bp.parse(euromap_current);
+  }
 
-    bp.consume(sizeof(uint32_t));
+  bp.consume(sizeof(uint32_t));
 
-    return true;
+  return true;
 }
 
-bool MasterBoardData_V3_2::parse_with(BinParser& bp)
+bool MasterBoardData_V3_2::parseWith(BinParser& bp)
 {
-    if (!bp.check_size<MasterBoardData_V3_2>())
-        return false;
+  if (!bp.checkSize<MasterBoardData_V3_2>())
+    return false;
 
-    MasterBoardData_V3_0__1::parse_with(bp);
+  MasterBoardData_V3_0__1::parseWith(bp);
 
-    bp.parse(operational_mode_selector_input);
-    bp.parse(three_position_enabling_device_input);
+  bp.parse(operational_mode_selector_input);
+  bp.parse(three_position_enabling_device_input);
 
-    return true;
+  return true;
 }
 
-bool MasterBoardData_V1_X::consume_with(URStatePacketConsumer& consumer)
+bool MasterBoardData_V1_X::consumeWith(URStatePacketConsumer& consumer)
 {
-    return consumer.consume(*this);
+  return consumer.consume(*this);
 }
-bool MasterBoardData_V3_0__1::consume_with(URStatePacketConsumer& consumer)
+bool MasterBoardData_V3_0__1::consumeWith(URStatePacketConsumer& consumer)
 {
-    return consumer.consume(*this);
+  return consumer.consume(*this);
 }
-bool MasterBoardData_V3_2::consume_with(URStatePacketConsumer& consumer)
+bool MasterBoardData_V3_2::consumeWith(URStatePacketConsumer& consumer)
 {
-    return consumer.consume(*this);
+  return consumer.consume(*this);
 }
\ No newline at end of file
diff --git a/src/ur/messages.cpp b/src/ur/messages.cpp
index b532792..5cfc440 100644
--- a/src/ur/messages.cpp
+++ b/src/ur/messages.cpp
@@ -1,20 +1,19 @@
 #include "ur_modern_driver/ur/messages.h"
 #include "ur_modern_driver/ur/consumer.h"
 
-bool VersionMessage::parse_with(BinParser& bp)
+bool VersionMessage::parseWith(BinParser& bp)
 {
+  bp.parse(project_name);
+  bp.parse(major_version);
+  bp.parse(minor_version);
+  bp.parse(svn_version);
+  bp.consume(sizeof(uint32_t));  // undocumented field??
+  bp.parse_remainder(build_date);
 
-    bp.parse(project_name);
-    bp.parse(major_version);
-    bp.parse(minor_version);
-    bp.parse(svn_version);
-    bp.consume(sizeof(uint32_t)); //undocumented field??
-    bp.parse_remainder(build_date);
-
-    return true; //not really possible to check dynamic size packets
+  return true;  // not really possible to check dynamic size packets
 }
 
-bool VersionMessage::consume_with(URMessagePacketConsumer& consumer)
+bool VersionMessage::consumeWith(URMessagePacketConsumer& consumer)
 {
-    return consumer.consume(*this);
+  return consumer.consume(*this);
 }
\ No newline at end of file
diff --git a/src/ur/robot_mode.cpp b/src/ur/robot_mode.cpp
index be0770f..5bee4cf 100644
--- a/src/ur/robot_mode.cpp
+++ b/src/ur/robot_mode.cpp
@@ -1,71 +1,71 @@
 #include "ur_modern_driver/ur/robot_mode.h"
 #include "ur_modern_driver/ur/consumer.h"
 
-bool SharedRobotModeData::parse_with(BinParser& bp)
+bool SharedRobotModeData::parseWith(BinParser& bp)
 {
-    bp.parse(timestamp);
-    bp.parse(physical_robot_connected);
-    bp.parse(real_robot_enabled);
-    bp.parse(robot_power_on);
-    bp.parse(emergency_stopped);
-    return true;
+  bp.parse(timestamp);
+  bp.parse(physical_robot_connected);
+  bp.parse(real_robot_enabled);
+  bp.parse(robot_power_on);
+  bp.parse(emergency_stopped);
+  return true;
 }
 
-bool RobotModeData_V1_X::parse_with(BinParser& bp)
+bool RobotModeData_V1_X::parseWith(BinParser& bp)
 {
-    if (!bp.check_size<RobotModeData_V1_X>())
-        return false;
+  if (!bp.checkSize<RobotModeData_V1_X>())
+    return false;
 
-    SharedRobotModeData::parse_with(bp);
+  SharedRobotModeData::parseWith(bp);
 
-    bp.parse(security_stopped);
-    bp.parse(program_running);
-    bp.parse(program_paused);
-    bp.parse(robot_mode);
-    bp.parse(speed_fraction);
+  bp.parse(security_stopped);
+  bp.parse(program_running);
+  bp.parse(program_paused);
+  bp.parse(robot_mode);
+  bp.parse(speed_fraction);
 
-    return true;
+  return true;
 }
 
-bool RobotModeData_V3_0__1::parse_with(BinParser& bp)
+bool RobotModeData_V3_0__1::parseWith(BinParser& bp)
 {
-    if (!bp.check_size<RobotModeData_V3_0__1>())
-        return false;
+  if (!bp.checkSize<RobotModeData_V3_0__1>())
+    return false;
 
-    SharedRobotModeData::parse_with(bp);
+  SharedRobotModeData::parseWith(bp);
 
-    bp.parse(protective_stopped);
-    bp.parse(program_running);
-    bp.parse(program_paused);
-    bp.parse(robot_mode);
-    bp.parse(control_mode);
-    bp.parse(target_speed_fraction);
-    bp.parse(speed_scaling);
+  bp.parse(protective_stopped);
+  bp.parse(program_running);
+  bp.parse(program_paused);
+  bp.parse(robot_mode);
+  bp.parse(control_mode);
+  bp.parse(target_speed_fraction);
+  bp.parse(speed_scaling);
 
-    return true;
+  return true;
 }
 
-bool RobotModeData_V3_2::parse_with(BinParser& bp)
+bool RobotModeData_V3_2::parseWith(BinParser& bp)
 {
-    if (!bp.check_size<RobotModeData_V3_2>())
-        return false;
+  if (!bp.checkSize<RobotModeData_V3_2>())
+    return false;
 
-    RobotModeData_V3_0__1::parse_with(bp);
+  RobotModeData_V3_0__1::parseWith(bp);
 
-    bp.parse(target_speed_fraction_limit);
+  bp.parse(target_speed_fraction_limit);
 
-    return true;
+  return true;
 }
 
-bool RobotModeData_V1_X::consume_with(URStatePacketConsumer& consumer)
+bool RobotModeData_V1_X::consumeWith(URStatePacketConsumer& consumer)
 {
-    return consumer.consume(*this);
+  return consumer.consume(*this);
 }
-bool RobotModeData_V3_0__1::consume_with(URStatePacketConsumer& consumer)
+bool RobotModeData_V3_0__1::consumeWith(URStatePacketConsumer& consumer)
 {
-    return consumer.consume(*this);
+  return consumer.consume(*this);
 }
-bool RobotModeData_V3_2::consume_with(URStatePacketConsumer& consumer)
+bool RobotModeData_V3_2::consumeWith(URStatePacketConsumer& consumer)
 {
-    return consumer.consume(*this);
+  return consumer.consume(*this);
 }
\ No newline at end of file
diff --git a/src/ur/rt_state.cpp b/src/ur/rt_state.cpp
index 2264835..18d6822 100644
--- a/src/ur/rt_state.cpp
+++ b/src/ur/rt_state.cpp
@@ -3,115 +3,115 @@
 
 void RTShared::parse_shared1(BinParser& bp)
 {
-    bp.parse(time);
-    bp.parse(q_target);
-    bp.parse(qd_target);
-    bp.parse(qdd_target);
-    bp.parse(i_target);
-    bp.parse(m_target);
-    bp.parse(q_actual);
-    bp.parse(qd_actual);
-    bp.parse(i_actual);
+  bp.parse(time);
+  bp.parse(q_target);
+  bp.parse(qd_target);
+  bp.parse(qdd_target);
+  bp.parse(i_target);
+  bp.parse(m_target);
+  bp.parse(q_actual);
+  bp.parse(qd_actual);
+  bp.parse(i_actual);
 }
 
 void RTShared::parse_shared2(BinParser& bp)
 {
-    bp.parse(digital_inputs);
-    bp.parse(motor_temperatures);
-    bp.parse(controller_time);
-    bp.consume(sizeof(double)); //Unused "Test value" field
-    bp.parse(robot_mode);
+  bp.parse(digital_inputs);
+  bp.parse(motor_temperatures);
+  bp.parse(controller_time);
+  bp.consume(sizeof(double));  // Unused "Test value" field
+  bp.parse(robot_mode);
 }
 
-bool RTState_V1_6__7::parse_with(BinParser& bp)
+bool RTState_V1_6__7::parseWith(BinParser& bp)
 {
-    if (!bp.check_size<RTState_V1_6__7>())
-        return false;
+  if (!bp.checkSize<RTState_V1_6__7>())
+    return false;
 
-    parse_shared1(bp);
+  parse_shared1(bp);
 
-    bp.parse(tool_accelerometer_values);
-    bp.consume(sizeof(double)*15);
-    bp.parse(tcp_force);
-    bp.parse(tool_vector_actual);
-    bp.parse(tcp_speed_actual);
+  bp.parse(tool_accelerometer_values);
+  bp.consume(sizeof(double) * 15);
+  bp.parse(tcp_force);
+  bp.parse(tool_vector_actual);
+  bp.parse(tcp_speed_actual);
 
-    parse_shared2(bp);
+  parse_shared2(bp);
 
-    return true;
+  return true;
 }
 
-bool RTState_V1_8::parse_with(BinParser& bp)
+bool RTState_V1_8::parseWith(BinParser& bp)
 {
-    if (!bp.check_size<RTState_V1_8>())
-        return false;
+  if (!bp.checkSize<RTState_V1_8>())
+    return false;
 
-    RTState_V1_6__7::parse_with(bp);
+  RTState_V1_6__7::parseWith(bp);
 
-    bp.parse(joint_modes);
+  bp.parse(joint_modes);
 
-    return true;
+  return true;
 }
 
-bool RTState_V3_0__1::parse_with(BinParser& bp)
+bool RTState_V3_0__1::parseWith(BinParser& bp)
 {
-    if (!bp.check_size<RTState_V3_0__1>())
-        return false;
+  if (!bp.checkSize<RTState_V3_0__1>())
+    return false;
 
-    parse_shared1(bp);
+  parse_shared1(bp);
 
-    bp.parse(i_control);
-    bp.parse(tool_vector_actual);
-    bp.parse(tcp_speed_actual);
-    bp.parse(tcp_force);
-    bp.parse(tool_vector_target);
-    bp.parse(tcp_speed_target);
+  bp.parse(i_control);
+  bp.parse(tool_vector_actual);
+  bp.parse(tcp_speed_actual);
+  bp.parse(tcp_force);
+  bp.parse(tool_vector_target);
+  bp.parse(tcp_speed_target);
 
-    parse_shared2(bp);
+  parse_shared2(bp);
 
-    bp.parse(joint_modes);
-    bp.parse(safety_mode);
-    bp.consume(sizeof(double[6])); //skip undocumented
-    bp.parse(tool_accelerometer_values);
-    bp.consume(sizeof(double[6])); //skip undocumented
-    bp.parse(speed_scaling);
-    bp.parse(linear_momentum_norm);
-    bp.consume(sizeof(double)); //skip undocumented
-    bp.consume(sizeof(double)); //skip undocumented
-    bp.parse(v_main);
-    bp.parse(v_robot);
-    bp.parse(i_robot);
-    bp.parse(v_actual);
+  bp.parse(joint_modes);
+  bp.parse(safety_mode);
+  bp.consume(sizeof(double[6]));  // skip undocumented
+  bp.parse(tool_accelerometer_values);
+  bp.consume(sizeof(double[6]));  // skip undocumented
+  bp.parse(speed_scaling);
+  bp.parse(linear_momentum_norm);
+  bp.consume(sizeof(double));  // skip undocumented
+  bp.consume(sizeof(double));  // skip undocumented
+  bp.parse(v_main);
+  bp.parse(v_robot);
+  bp.parse(i_robot);
+  bp.parse(v_actual);
 
-    return true;
+  return true;
 }
 
-bool RTState_V3_2__3::parse_with(BinParser& bp)
+bool RTState_V3_2__3::parseWith(BinParser& bp)
 {
-    if (!bp.check_size<RTState_V3_2__3>())
-        return false;
+  if (!bp.checkSize<RTState_V3_2__3>())
+    return false;
 
-    RTState_V3_0__1::parse_with(bp);
+  RTState_V3_0__1::parseWith(bp);
 
-    bp.parse(digital_outputs);
-    bp.parse(program_state);
+  bp.parse(digital_outputs);
+  bp.parse(program_state);
 
-    return true;
+  return true;
 }
 
-bool RTState_V1_6__7::consume_with(URRTPacketConsumer& consumer)
+bool RTState_V1_6__7::consumeWith(URRTPacketConsumer& consumer)
 {
-    return consumer.consume(*this);
+  return consumer.consume(*this);
 }
-bool RTState_V1_8::consume_with(URRTPacketConsumer& consumer)
+bool RTState_V1_8::consumeWith(URRTPacketConsumer& consumer)
 {
-    return consumer.consume(*this);
+  return consumer.consume(*this);
 }
-bool RTState_V3_0__1::consume_with(URRTPacketConsumer& consumer)
+bool RTState_V3_0__1::consumeWith(URRTPacketConsumer& consumer)
 {
-    return consumer.consume(*this);
+  return consumer.consume(*this);
 }
-bool RTState_V3_2__3::consume_with(URRTPacketConsumer& consumer)
+bool RTState_V3_2__3::consumeWith(URRTPacketConsumer& consumer)
 {
-    return consumer.consume(*this);
+  return consumer.consume(*this);
 }
\ No newline at end of file
diff --git a/src/ur/state.cpp b/src/ur/state.cpp
index 0b4f4f2..66af615 100644
--- a/src/ur/state.cpp
+++ b/src/ur/state.cpp
@@ -1,25 +1,25 @@
 #include "ur_modern_driver/ur/state.h"
 #include "ur_modern_driver/log.h"
 
-//StatePacket::~StatePacket() { }
+// StatePacket::~StatePacket() { }
 
 /*
-bool RobotState::parse_with(BinParser &bp) {
+bool RobotState::parseWith(BinParser &bp) {
     //continue as long as there are bytes to read
     while(!bp.empty()) {
-        
-        if(!bp.check_size(sizeof(uint32_t))){
+
+        if(!bp.checkSize(sizeof(uint32_t))){
             LOG_ERROR("Failed to read sub-package length, there's likely a parsing error");
-            return false;    
+            return false;
         }
 
         uint32_t sub_size = bp.peek<uint32_t>();
-        if(!bp.check_size(static_cast<size_t>(sub_size))) {
+        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 
+
+        //deconstruction of a sub parser will increment the position of the parent parser
         BinParser sub_parser(bp, sub_size);
         sub_parser.consume(sizeof(sub_size));
 
@@ -43,19 +43,19 @@ bool parse_base(BinParser &bp, T &pkg) {
         case package_type::ROBOT_MODE_DATA:
             LOG_DEBUG("Parsing robot_mode");
             bp.consume(sizeof(package_type));
-            pkg.robot_mode.parse_with(bp);
+            pkg.robot_mode.parseWith(bp);
             break;
         case package_type::MASTERBOARD_DATA:
             LOG_DEBUG("Parsing master_board");
             bp.consume(sizeof(package_type));
-            pkg.master_board.parse_with(bp);
+            pkg.master_board.parseWith(bp);
             break;
 
         case package_type::TOOL_DATA:
         case package_type::CARTESIAN_INFO:
         case package_type::JOINT_DATA:
-            LOG_DEBUG("Skipping tool, cartesian or joint data");        
-            //for unhandled packages we consume the rest of the 
+            LOG_DEBUG("Skipping tool, cartesian or joint data");
+            //for unhandled packages we consume the rest of the
             //package buffer
             bp.consume();
             break;
@@ -79,8 +79,8 @@ bool parse_advanced(BinParser &bp, T &pkg) {
         case package_type::ADDITIONAL_INFO:
         case package_type::CALIBRATION_DATA:
         case package_type::FORCE_MODE_DATA:
-            LOG_DEBUG("Skipping kinematics, config, additional, calibration or force mode data");        
-            //for unhandled packages we consume the rest of the 
+            LOG_DEBUG("Skipping kinematics, config, additional, calibration or force mode data");
+            //for unhandled packages we consume the rest of the
             //package buffer
             bp.consume();
             break;
diff --git a/src/ur/stream.cpp b/src/ur/stream.cpp
index 955d1f3..819651e 100644
--- a/src/ur/stream.cpp
+++ b/src/ur/stream.cpp
@@ -1,126 +1,133 @@
-#include <cstring>
 #include <endian.h>
 #include <netinet/tcp.h>
 #include <unistd.h>
+#include <cstring>
 
 #include "ur_modern_driver/log.h"
 #include "ur_modern_driver/ur/stream.h"
 
 bool URStream::connect()
 {
-    if (_initialized)
-        return false;
+  if (initialized_)
+    return false;
 
-    LOG_INFO("Connecting to UR @ %s:%d", _host.c_str(), _port);
+  LOG_INFO("Connecting to UR @ %s:%d", host_.c_str(), port_);
 
-    //gethostbyname() is deprecated so use getadderinfo() as described in:
-    //http://www.beej.us/guide/bgnet/output/html/multipage/syscalls.html#getaddrinfo
+  // gethostbyname() is deprecated so use getadderinfo() as described in:
+  // http://www.beej.us/guide/bgnet/output/html/multipage/syscalls.html#getaddrinfo
 
-    std::string service = std::to_string(_port);
-    struct addrinfo hints, *result;
-    std::memset(&hints, 0, sizeof(hints));
+  std::string service = std::to_string(port_);
+  struct addrinfo hints, *result;
+  std::memset(&hints, 0, sizeof(hints));
 
-    hints.ai_family = AF_UNSPEC;
-    hints.ai_socktype = SOCK_STREAM;
-    hints.ai_flags = AI_PASSIVE;
+  hints.ai_family = AF_UNSPEC;
+  hints.ai_socktype = SOCK_STREAM;
+  hints.ai_flags = AI_PASSIVE;
 
-    if (getaddrinfo(_host.c_str(), service.c_str(), &hints, &result) != 0) {
-        LOG_ERROR("Failed to get host name");
-        return false;
-    }
+  if (getaddrinfo(host_.c_str(), service.c_str(), &hints, &result) != 0)
+  {
+    LOG_ERROR("Failed to get host name");
+    return false;
+  }
 
-    //loop through the list of addresses untill we find one that's connectable
-    for (struct addrinfo* p = result; p != nullptr; p = p->ai_next) {
-        _socket_fd = socket(p->ai_family, p->ai_socktype, p->ai_protocol);
+  // loop through the list of addresses untill we find one that's connectable
+  for (struct addrinfo* p = result; p != nullptr; p = p->ai_next)
+  {
+    socket_fd_ = socket(p->ai_family, p->ai_socktype, p->ai_protocol);
 
-        if (_socket_fd == -1) //socket error?
-            continue;
+    if (socket_fd_ == -1)  // socket error?
+      continue;
 
-        if (::connect(_socket_fd, p->ai_addr, p->ai_addrlen) != 0) {
-            if (_stopping)
-                break;
-            else
-                continue; //try next addrinfo if connect fails
-        }
-
-        //disable Nagle's algorithm to ensure we sent packets as fast as possible
-        int flag = 1;
-        setsockopt(_socket_fd, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(flag));
-        _initialized = true;
-        LOG_INFO("Connection successfully established");
+    if (::connect(socket_fd_, p->ai_addr, p->ai_addrlen) != 0)
+    {
+      if (stopping_)
         break;
+      else
+        continue;  // try next addrinfo if connect fails
     }
 
-    freeaddrinfo(result);
-    if (!_initialized)
-        LOG_ERROR("Connection failed");
+    // disable Nagle's algorithm to ensure we sent packets as fast as possible
+    int flag = 1;
+    setsockopt(socket_fd_, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(flag));
+    initialized_ = true;
+    LOG_INFO("Connection successfully established");
+    break;
+  }
 
-    return _initialized;
+  freeaddrinfo(result);
+  if (!initialized_)
+    LOG_ERROR("Connection failed");
+
+  return initialized_;
 }
 
 void URStream::disconnect()
 {
-    if (!_initialized || _stopping)
-        return;
+  if (!initialized_ || stopping_)
+    return;
 
-    LOG_INFO("Disconnecting from %s:%d", _host.c_str(), _port);
+  LOG_INFO("Disconnecting from %s:%d", host_.c_str(), port_);
 
-    _stopping = true;
-    close(_socket_fd);
-    _initialized = false;
+  stopping_ = true;
+  close(socket_fd_);
+  initialized_ = false;
 }
 
 ssize_t URStream::send(uint8_t* buf, size_t buf_len)
 {
-    if (!_initialized)
-        return -1;
-    if (_stopping)
-        return 0;
+  if (!initialized_)
+    return -1;
+  if (stopping_)
+    return 0;
 
-    size_t total = 0;
-    size_t remaining = buf_len;
+  size_t total = 0;
+  size_t remaining = buf_len;
 
-    //TODO: handle reconnect?
-    //handle partial sends
-    while (total < buf_len) {
-        ssize_t sent = ::send(_socket_fd, buf + total, remaining, 0);
-        if (sent <= 0)
-            return _stopping ? 0 : sent;
-        total += sent;
-        remaining -= sent;
-    }
+  // TODO: handle reconnect?
+  // handle partial sends
+  while (total < buf_len)
+  {
+    ssize_t sent = ::send(socket_fd_, buf + total, remaining, 0);
+    if (sent <= 0)
+      return stopping_ ? 0 : sent;
+    total += sent;
+    remaining -= sent;
+  }
 
-    return total;
+  return total;
 }
 
 ssize_t URStream::receive(uint8_t* buf, size_t buf_len)
 {
-    if (!_initialized)
+  if (!initialized_)
+    return -1;
+  if (stopping_)
+    return 0;
+
+  size_t remainder = sizeof(int32_t);
+  uint8_t* buf_pos = buf;
+  bool initial = true;
+
+  do
+  {
+    ssize_t read = recv(socket_fd_, buf_pos, remainder, 0);
+    if (read <= 0)  // failed reading from socket
+      return stopping_ ? 0 : read;
+
+    if (initial)
+    {
+      remainder = be32toh(*(reinterpret_cast<int32_t*>(buf)));
+      if (remainder >= (buf_len - sizeof(int32_t)))
+      {
+        LOG_ERROR("Packet size %zd is larger than buffer %zu, discarding.", remainder, buf_len);
         return -1;
-    if (_stopping)
-        return 0;
+      }
+      initial = false;
+    }
 
-    size_t remainder = sizeof(int32_t);
-    uint8_t* buf_pos = buf;
-    bool initial = true;
+    buf_pos += read;
+    remainder -= read;
+  } while (remainder > 0);
 
-    do {
-        ssize_t read = recv(_socket_fd, buf_pos, remainder, 0);
-        if (read <= 0) //failed reading from socket
-            return _stopping ? 0 : read;
-
-        if (initial) {
-            remainder = be32toh(*(reinterpret_cast<int32_t*>(buf)));
-            if (remainder >= (buf_len - sizeof(int32_t))) {
-                LOG_ERROR("Packet size %zd is larger than buffer %zu, discarding.", remainder, buf_len);
-                return -1;
-            }
-            initial = false;
-        }
-
-        buf_pos += read;
-        remainder -= read;
-    } while (remainder > 0);
-
-    return buf_pos - buf;
+  return buf_pos - buf;
 }
\ No newline at end of file
diff --git a/src/ur_communication.cpp b/src/ur_communication.cpp
index b883004..d7da04f 100644
--- a/src/ur_communication.cpp
+++ b/src/ur_communication.cpp
@@ -18,168 +18,167 @@
 
 #include "ur_modern_driver/ur_communication.h"
 
-UrCommunication::UrCommunication(std::condition_variable& msg_cond,
-    std::string host)
+UrCommunication::UrCommunication(std::condition_variable& msg_cond, std::string host)
 {
-    robot_state_ = new RobotState(msg_cond);
-    bzero((char*)&pri_serv_addr_, sizeof(pri_serv_addr_));
-    bzero((char*)&sec_serv_addr_, sizeof(sec_serv_addr_));
-    pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
-    if (pri_sockfd_ < 0) {
-        print_fatal("ERROR opening socket pri_sockfd");
-    }
-    sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
-    if (sec_sockfd_ < 0) {
-        print_fatal("ERROR opening socket sec_sockfd");
-    }
-    server_ = gethostbyname(host.c_str());
-    if (server_ == NULL) {
-        print_fatal("ERROR, unknown host");
-    }
-    pri_serv_addr_.sin_family = AF_INET;
-    sec_serv_addr_.sin_family = AF_INET;
-    bcopy((char*)server_->h_addr, (char*)&pri_serv_addr_.sin_addr.s_addr, server_->h_length);
-    bcopy((char*)server_->h_addr, (char*)&sec_serv_addr_.sin_addr.s_addr, server_->h_length);
-    pri_serv_addr_.sin_port = htons(30001);
-    sec_serv_addr_.sin_port = htons(30002);
-    flag_ = 1;
-    setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_,
-        sizeof(int));
-    setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
-        sizeof(int));
-    setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_,
-        sizeof(int));
-    setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_,
-        sizeof(int));
-    setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
-        sizeof(int));
-    setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_,
-        sizeof(int));
-    fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK);
-    connected_ = false;
-    keepalive_ = false;
+  robot_state_ = new RobotState(msg_cond);
+  bzero((char*)&pri_serv_addr_, sizeof(pri_serv_addr_));
+  bzero((char*)&sec_serv_addr_, sizeof(sec_serv_addr_));
+  pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
+  if (pri_sockfd_ < 0)
+  {
+    print_fatal("ERROR opening socket pri_sockfd");
+  }
+  sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
+  if (sec_sockfd_ < 0)
+  {
+    print_fatal("ERROR opening socket sec_sockfd");
+  }
+  server_ = gethostbyname(host.c_str());
+  if (server_ == NULL)
+  {
+    print_fatal("ERROR, unknown host");
+  }
+  pri_serv_addr_.sin_family = AF_INET;
+  sec_serv_addr_.sin_family = AF_INET;
+  bcopy((char*)server_->h_addr, (char*)&pri_serv_addr_.sin_addr.s_addr, server_->h_length);
+  bcopy((char*)server_->h_addr, (char*)&sec_serv_addr_.sin_addr.s_addr, server_->h_length);
+  pri_serv_addr_.sin_port = htons(30001);
+  sec_serv_addr_.sin_port = htons(30002);
+  flag_ = 1;
+  setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
+  setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
+  setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
+  setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
+  setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
+  setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
+  fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK);
+  connected_ = false;
+  keepalive_ = false;
 }
 
 bool UrCommunication::start()
 {
-    keepalive_ = true;
-    uint8_t buf[512];
-    unsigned int bytes_read;
-    std::string cmd;
-    bzero(buf, 512);
-    print_debug("Acquire firmware version: Connecting...");
-    if (connect(pri_sockfd_, (struct sockaddr*)&pri_serv_addr_,
-            sizeof(pri_serv_addr_))
-        < 0) {
-        print_fatal("Error connecting to get firmware version");
-        return false;
-    }
-    print_debug("Acquire firmware version: Got connection");
-    bytes_read = read(pri_sockfd_, buf, 512);
-    setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
-        sizeof(int));
-    robot_state_->unpack(buf, bytes_read);
-    //wait for some traffic so the UR socket doesn't die in version 3.1.
-    std::this_thread::sleep_for(std::chrono::milliseconds(500));
-    char tmp[64];
-    sprintf(tmp, "Firmware version detected: %.7f", robot_state_->getVersion());
-    print_debug(tmp);
-    close(pri_sockfd_);
+  keepalive_ = true;
+  uint8_t buf[512];
+  unsigned int bytes_read;
+  std::string cmd;
+  bzero(buf, 512);
+  print_debug("Acquire firmware version: Connecting...");
+  if (connect(pri_sockfd_, (struct sockaddr*)&pri_serv_addr_, sizeof(pri_serv_addr_)) < 0)
+  {
+    print_fatal("Error connecting to get firmware version");
+    return false;
+  }
+  print_debug("Acquire firmware version: Got connection");
+  bytes_read = read(pri_sockfd_, buf, 512);
+  setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
+  robot_state_->unpack(buf, bytes_read);
+  // wait for some traffic so the UR socket doesn't die in version 3.1.
+  std::this_thread::sleep_for(std::chrono::milliseconds(500));
+  char tmp[64];
+  sprintf(tmp, "Firmware version detected: %.7f", robot_state_->getVersion());
+  print_debug(tmp);
+  close(pri_sockfd_);
 
-    print_debug(
-        "Switching to secondary interface for masterboard data: Connecting...");
+  print_debug("Switching to secondary interface for masterboard data: Connecting...");
 
-    fd_set writefds;
-    struct timeval timeout;
+  fd_set writefds;
+  struct timeval timeout;
 
-    connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_,
-        sizeof(sec_serv_addr_));
-    FD_ZERO(&writefds);
-    FD_SET(sec_sockfd_, &writefds);
-    timeout.tv_sec = 10;
-    timeout.tv_usec = 0;
-    select(sec_sockfd_ + 1, NULL, &writefds, NULL, &timeout);
-    unsigned int flag_len;
-    getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
-    if (flag_ < 0) {
-        print_fatal("Error connecting to secondary interface");
-        return false;
-    }
-    print_debug("Secondary interface: Got connection");
-    comThread_ = std::thread(&UrCommunication::run, this);
-    return true;
+  connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_, sizeof(sec_serv_addr_));
+  FD_ZERO(&writefds);
+  FD_SET(sec_sockfd_, &writefds);
+  timeout.tv_sec = 10;
+  timeout.tv_usec = 0;
+  select(sec_sockfd_ + 1, NULL, &writefds, NULL, &timeout);
+  unsigned int flag_len;
+  getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
+  if (flag_ < 0)
+  {
+    print_fatal("Error connecting to secondary interface");
+    return false;
+  }
+  print_debug("Secondary interface: Got connection");
+  comThread_ = std::thread(&UrCommunication::run, this);
+  return true;
 }
 
 void UrCommunication::halt()
 {
-    keepalive_ = false;
-    comThread_.join();
+  keepalive_ = false;
+  comThread_.join();
 }
 
 void UrCommunication::run()
 {
-    uint8_t buf[2048];
-    int bytes_read;
-    bzero(buf, 2048);
-    struct timeval timeout;
-    fd_set readfds;
-    FD_ZERO(&readfds);
-    FD_SET(sec_sockfd_, &readfds);
-    connected_ = true;
-    while (keepalive_) {
-        while (connected_ && keepalive_) {
-            timeout.tv_sec = 0; //do this each loop as selects modifies timeout
-            timeout.tv_usec = 500000; // timeout of 0.5 sec
-            select(sec_sockfd_ + 1, &readfds, NULL, NULL, &timeout);
-            bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes
-            if (bytes_read > 0) {
-                setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK,
-                    (char*)&flag_, sizeof(int));
-                robot_state_->unpack(buf, bytes_read);
-            } else {
-                connected_ = false;
-                robot_state_->setDisconnected();
-                close(sec_sockfd_);
-            }
-        }
-        if (keepalive_) {
-            //reconnect
-            print_warning("Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...");
-            sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
-            if (sec_sockfd_ < 0) {
-                print_fatal("ERROR opening secondary socket");
-            }
-            flag_ = 1;
-            setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_,
-                sizeof(int));
-            setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
-                sizeof(int));
-            setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_,
-                sizeof(int));
-            fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK);
-            while (keepalive_ && !connected_) {
-                std::this_thread::sleep_for(std::chrono::seconds(10));
-                fd_set writefds;
-
-                connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_,
-                    sizeof(sec_serv_addr_));
-                FD_ZERO(&writefds);
-                FD_SET(sec_sockfd_, &writefds);
-                select(sec_sockfd_ + 1, NULL, &writefds, NULL, NULL);
-                unsigned int flag_len;
-                getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_,
-                    &flag_len);
-                if (flag_ < 0) {
-                    print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds...");
-                } else {
-                    connected_ = true;
-                    print_info("Secondary port: Reconnected");
-                }
-            }
-        }
+  uint8_t buf[2048];
+  int bytes_read;
+  bzero(buf, 2048);
+  struct timeval timeout;
+  fd_set readfds;
+  FD_ZERO(&readfds);
+  FD_SET(sec_sockfd_, &readfds);
+  connected_ = true;
+  while (keepalive_)
+  {
+    while (connected_ && keepalive_)
+    {
+      timeout.tv_sec = 0;        // do this each loop as selects modifies timeout
+      timeout.tv_usec = 500000;  // timeout of 0.5 sec
+      select(sec_sockfd_ + 1, &readfds, NULL, NULL, &timeout);
+      bytes_read = read(sec_sockfd_, buf, 2048);  // usually only up to 1295 bytes
+      if (bytes_read > 0)
+      {
+        setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
+        robot_state_->unpack(buf, bytes_read);
+      }
+      else
+      {
+        connected_ = false;
+        robot_state_->setDisconnected();
+        close(sec_sockfd_);
+      }
     }
+    if (keepalive_)
+    {
+      // reconnect
+      print_warning("Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...");
+      sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
+      if (sec_sockfd_ < 0)
+      {
+        print_fatal("ERROR opening secondary socket");
+      }
+      flag_ = 1;
+      setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
+      setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
+      setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
+      fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK);
+      while (keepalive_ && !connected_)
+      {
+        std::this_thread::sleep_for(std::chrono::seconds(10));
+        fd_set writefds;
 
-    //wait for some traffic so the UR socket doesn't die in version 3.1.
-    std::this_thread::sleep_for(std::chrono::milliseconds(500));
-    close(sec_sockfd_);
+        connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_, sizeof(sec_serv_addr_));
+        FD_ZERO(&writefds);
+        FD_SET(sec_sockfd_, &writefds);
+        select(sec_sockfd_ + 1, NULL, &writefds, NULL, NULL);
+        unsigned int flag_len;
+        getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
+        if (flag_ < 0)
+        {
+          print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 "
+                      "seconds...");
+        }
+        else
+        {
+          connected_ = true;
+          print_info("Secondary port: Reconnected");
+        }
+      }
+    }
+  }
+
+  // wait for some traffic so the UR socket doesn't die in version 3.1.
+  std::this_thread::sleep_for(std::chrono::milliseconds(500));
+  close(sec_sockfd_);
 }
diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp
index 08a535a..c5cf3bd 100644
--- a/src/ur_driver.cpp
+++ b/src/ur_driver.cpp
@@ -18,397 +18,403 @@
 
 #include "ur_modern_driver/ur_driver.h"
 
-UrDriver::UrDriver(std::condition_variable& rt_msg_cond,
-    std::condition_variable& msg_cond, std::string host,
-    unsigned int reverse_port, double servoj_time,
-    unsigned int safety_count_max, double max_time_step, double min_payload,
-    double max_payload, double servoj_lookahead_time, double servoj_gain)
-    : REVERSE_PORT_(reverse_port)
-    , maximum_time_step_(max_time_step)
-    , minimum_payload_(
-          min_payload)
-    , maximum_payload_(max_payload)
-    , servoj_time_(
-          servoj_time)
-    , servoj_lookahead_time_(servoj_lookahead_time)
-    , servoj_gain_(servoj_gain)
+UrDriver::UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host,
+                   unsigned int reverse_port, double servoj_time, unsigned int safety_count_max, double max_time_step,
+                   double min_payload, double max_payload, double servoj_lookahead_time, double servoj_gain)
+  : REVERSE_PORT_(reverse_port)
+  , maximum_time_step_(max_time_step)
+  , minimum_payload_(min_payload)
+  , maximum_payload_(max_payload)
+  , servoj_time_(servoj_time)
+  , servoj_lookahead_time_(servoj_lookahead_time)
+  , servoj_gain_(servoj_gain)
 {
-    char buffer[256];
-    struct sockaddr_in serv_addr;
-    int n, flag;
+  char buffer[256];
+  struct sockaddr_in serv_addr;
+  int n, flag;
 
-    firmware_version_ = 0;
-    reverse_connected_ = false;
-    executing_traj_ = false;
-    rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host,
-        safety_count_max);
-    new_sockfd_ = -1;
-    sec_interface_ = new UrCommunication(msg_cond, host);
+  firmware_version_ = 0;
+  reverse_connected_ = false;
+  executing_traj_ = false;
+  rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host, safety_count_max);
+  new_sockfd_ = -1;
+  sec_interface_ = new UrCommunication(msg_cond, host);
 
-    incoming_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
-    if (incoming_sockfd_ < 0) {
-        print_fatal("ERROR opening socket for reverse communication");
-    }
-    bzero((char*)&serv_addr, sizeof(serv_addr));
+  incoming_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
+  if (incoming_sockfd_ < 0)
+  {
+    print_fatal("ERROR opening socket for reverse communication");
+  }
+  bzero((char*)&serv_addr, sizeof(serv_addr));
 
-    serv_addr.sin_family = AF_INET;
-    serv_addr.sin_addr.s_addr = INADDR_ANY;
-    serv_addr.sin_port = htons(REVERSE_PORT_);
-    flag = 1;
-    setsockopt(incoming_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag,
-        sizeof(int));
-    setsockopt(incoming_sockfd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int));
-    if (bind(incoming_sockfd_, (struct sockaddr*)&serv_addr,
-            sizeof(serv_addr))
-        < 0) {
-        print_fatal("ERROR on binding socket for reverse communication");
-    }
-    listen(incoming_sockfd_, 5);
+  serv_addr.sin_family = AF_INET;
+  serv_addr.sin_addr.s_addr = INADDR_ANY;
+  serv_addr.sin_port = htons(REVERSE_PORT_);
+  flag = 1;
+  setsockopt(incoming_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag, sizeof(int));
+  setsockopt(incoming_sockfd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int));
+  if (bind(incoming_sockfd_, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0)
+  {
+    print_fatal("ERROR on binding socket for reverse communication");
+  }
+  listen(incoming_sockfd_, 5);
 }
 
-std::vector<double> UrDriver::interp_cubic(double t, double T,
-    std::vector<double> p0_pos, std::vector<double> p1_pos,
-    std::vector<double> p0_vel, std::vector<double> p1_vel)
+std::vector<double> UrDriver::interp_cubic(double t, double T, std::vector<double> p0_pos, std::vector<double> p1_pos,
+                                           std::vector<double> p0_vel, std::vector<double> p1_vel)
 {
-    /*Returns positions of the joints at time 't' */
-    std::vector<double> positions;
-    for (unsigned int i = 0; i < p0_pos.size(); i++) {
-        double a = p0_pos[i];
-        double b = p0_vel[i];
-        double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i]
-                       - T * p1_vel[i])
-            / pow(T, 2);
-        double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i]
-                       + T * p1_vel[i])
-            / pow(T, 3);
-        positions.push_back(a + b * t + c * pow(t, 2) + d * pow(t, 3));
-    }
-    return positions;
+  /*Returns positions of the joints at time 't' */
+  std::vector<double> positions;
+  for (unsigned int i = 0; i < p0_pos.size(); i++)
+  {
+    double a = p0_pos[i];
+    double b = p0_vel[i];
+    double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i] - T * p1_vel[i]) / pow(T, 2);
+    double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i] + T * p1_vel[i]) / pow(T, 3);
+    positions.push_back(a + b * t + c * pow(t, 2) + d * pow(t, 3));
+  }
+  return positions;
 }
 
-bool UrDriver::doTraj(std::vector<double> inp_timestamps,
-    std::vector<std::vector<double> > inp_positions,
-    std::vector<std::vector<double> > inp_velocities)
+bool UrDriver::doTraj(std::vector<double> inp_timestamps, std::vector<std::vector<double>> inp_positions,
+                      std::vector<std::vector<double>> inp_velocities)
 {
-    std::chrono::high_resolution_clock::time_point t0, t;
-    std::vector<double> positions;
-    unsigned int j;
+  std::chrono::high_resolution_clock::time_point t0, t;
+  std::vector<double> positions;
+  unsigned int j;
 
-    if (!UrDriver::uploadProg()) {
-        return false;
+  if (!UrDriver::uploadProg())
+  {
+    return false;
+  }
+  executing_traj_ = true;
+  t0 = std::chrono::high_resolution_clock::now();
+  t = t0;
+  j = 0;
+  while ((inp_timestamps[inp_timestamps.size() - 1] >=
+          std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count()) and
+         executing_traj_)
+  {
+    while (inp_timestamps[j] <= std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count() &&
+           j < inp_timestamps.size() - 1)
+    {
+      j += 1;
     }
-    executing_traj_ = true;
-    t0 = std::chrono::high_resolution_clock::now();
-    t = t0;
-    j = 0;
-    while ((inp_timestamps[inp_timestamps.size() - 1]
-               >= std::chrono::duration_cast<std::chrono::duration<double> >(t - t0).count())
-        and executing_traj_) {
-        while (inp_timestamps[j]
-                <= std::chrono::duration_cast<std::chrono::duration<double> >(
-                       t - t0)
-                       .count()
-            && j < inp_timestamps.size() - 1) {
-            j += 1;
-        }
-        positions = UrDriver::interp_cubic(
-            std::chrono::duration_cast<std::chrono::duration<double> >(
-                t - t0)
-                    .count()
-                - inp_timestamps[j - 1],
-            inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1],
-            inp_positions[j], inp_velocities[j - 1], inp_velocities[j]);
-        UrDriver::servoj(positions);
+    positions = UrDriver::interp_cubic(std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count() -
+                                           inp_timestamps[j - 1],
+                                       inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1],
+                                       inp_positions[j], inp_velocities[j - 1], inp_velocities[j]);
+    UrDriver::servoj(positions);
 
-        // oversample with 4 * sample_time
-        std::this_thread::sleep_for(
-            std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.)));
-        t = std::chrono::high_resolution_clock::now();
-    }
-    executing_traj_ = false;
-    //Signal robot to stop driverProg()
-    UrDriver::closeServo(positions);
-    return true;
+    // oversample with 4 * sample_time
+    std::this_thread::sleep_for(std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.)));
+    t = std::chrono::high_resolution_clock::now();
+  }
+  executing_traj_ = false;
+  // Signal robot to stop driverProg()
+  UrDriver::closeServo(positions);
+  return true;
 }
 
 void UrDriver::servoj(std::vector<double> positions, int keepalive)
 {
-    if (!reverse_connected_) {
-        print_error(
-            "UrDriver::servoj called without a reverse connection present. Keepalive: "
-            + std::to_string(keepalive));
-        return;
-    }
-    unsigned int bytes_written;
-    int tmp;
-    unsigned char buf[28];
-    for (int i = 0; i < 6; i++) {
-        tmp = htonl((int)(positions[i] * MULT_JOINTSTATE_));
-        buf[i * 4] = tmp & 0xff;
-        buf[i * 4 + 1] = (tmp >> 8) & 0xff;
-        buf[i * 4 + 2] = (tmp >> 16) & 0xff;
-        buf[i * 4 + 3] = (tmp >> 24) & 0xff;
-    }
-    tmp = htonl((int)keepalive);
-    buf[6 * 4] = tmp & 0xff;
-    buf[6 * 4 + 1] = (tmp >> 8) & 0xff;
-    buf[6 * 4 + 2] = (tmp >> 16) & 0xff;
-    buf[6 * 4 + 3] = (tmp >> 24) & 0xff;
-    bytes_written = write(new_sockfd_, buf, 28);
+  if (!reverse_connected_)
+  {
+    print_error("UrDriver::servoj called without a reverse connection present. Keepalive: " +
+                std::to_string(keepalive));
+    return;
+  }
+  unsigned int bytes_written;
+  int tmp;
+  unsigned char buf[28];
+  for (int i = 0; i < 6; i++)
+  {
+    tmp = htonl((int)(positions[i] * MULT_JOINTSTATE_));
+    buf[i * 4] = tmp & 0xff;
+    buf[i * 4 + 1] = (tmp >> 8) & 0xff;
+    buf[i * 4 + 2] = (tmp >> 16) & 0xff;
+    buf[i * 4 + 3] = (tmp >> 24) & 0xff;
+  }
+  tmp = htonl((int)keepalive);
+  buf[6 * 4] = tmp & 0xff;
+  buf[6 * 4 + 1] = (tmp >> 8) & 0xff;
+  buf[6 * 4 + 2] = (tmp >> 16) & 0xff;
+  buf[6 * 4 + 3] = (tmp >> 24) & 0xff;
+  bytes_written = write(new_sockfd_, buf, 28);
 }
 
 void UrDriver::stopTraj()
 {
-    executing_traj_ = false;
-    rt_interface_->addCommandToQueue("stopj(10)\n");
+  executing_traj_ = false;
+  rt_interface_->addCommandToQueue("stopj(10)\n");
 }
 
 bool UrDriver::uploadProg()
 {
-    std::string cmd_str;
-    char buf[128];
-    cmd_str = "def driverProg():\n";
+  std::string cmd_str;
+  char buf[128];
+  cmd_str = "def driverProg():\n";
 
-    sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_);
-    cmd_str += buf;
+  sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_);
+  cmd_str += buf;
 
-    cmd_str += "\tSERVO_IDLE = 0\n";
-    cmd_str += "\tSERVO_RUNNING = 1\n";
-    cmd_str += "\tcmd_servo_state = SERVO_IDLE\n";
-    cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n";
-    cmd_str += "\tdef set_servo_setpoint(q):\n";
-    cmd_str += "\t\tenter_critical\n";
-    cmd_str += "\t\tcmd_servo_state = SERVO_RUNNING\n";
-    cmd_str += "\t\tcmd_servo_q = q\n";
-    cmd_str += "\t\texit_critical\n";
-    cmd_str += "\tend\n";
-    cmd_str += "\tthread servoThread():\n";
-    cmd_str += "\t\tstate = SERVO_IDLE\n";
-    cmd_str += "\t\twhile True:\n";
-    cmd_str += "\t\t\tenter_critical\n";
-    cmd_str += "\t\t\tq = cmd_servo_q\n";
-    cmd_str += "\t\t\tdo_brake = False\n";
-    cmd_str += "\t\t\tif (state == SERVO_RUNNING) and ";
-    cmd_str += "(cmd_servo_state == SERVO_IDLE):\n";
-    cmd_str += "\t\t\t\tdo_brake = True\n";
-    cmd_str += "\t\t\tend\n";
-    cmd_str += "\t\t\tstate = cmd_servo_state\n";
-    cmd_str += "\t\t\tcmd_servo_state = SERVO_IDLE\n";
-    cmd_str += "\t\t\texit_critical\n";
-    cmd_str += "\t\t\tif do_brake:\n";
-    cmd_str += "\t\t\t\tstopj(1.0)\n";
-    cmd_str += "\t\t\t\tsync()\n";
-    cmd_str += "\t\t\telif state == SERVO_RUNNING:\n";
+  cmd_str += "\tSERVO_IDLE = 0\n";
+  cmd_str += "\tSERVO_RUNNING = 1\n";
+  cmd_str += "\tcmd_servo_state = SERVO_IDLE\n";
+  cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n";
+  cmd_str += "\tdef set_servo_setpoint(q):\n";
+  cmd_str += "\t\tenter_critical\n";
+  cmd_str += "\t\tcmd_servo_state = SERVO_RUNNING\n";
+  cmd_str += "\t\tcmd_servo_q = q\n";
+  cmd_str += "\t\texit_critical\n";
+  cmd_str += "\tend\n";
+  cmd_str += "\tthread servoThread():\n";
+  cmd_str += "\t\tstate = SERVO_IDLE\n";
+  cmd_str += "\t\twhile True:\n";
+  cmd_str += "\t\t\tenter_critical\n";
+  cmd_str += "\t\t\tq = cmd_servo_q\n";
+  cmd_str += "\t\t\tdo_brake = False\n";
+  cmd_str += "\t\t\tif (state == SERVO_RUNNING) and ";
+  cmd_str += "(cmd_servo_state == SERVO_IDLE):\n";
+  cmd_str += "\t\t\t\tdo_brake = True\n";
+  cmd_str += "\t\t\tend\n";
+  cmd_str += "\t\t\tstate = cmd_servo_state\n";
+  cmd_str += "\t\t\tcmd_servo_state = SERVO_IDLE\n";
+  cmd_str += "\t\t\texit_critical\n";
+  cmd_str += "\t\t\tif do_brake:\n";
+  cmd_str += "\t\t\t\tstopj(1.0)\n";
+  cmd_str += "\t\t\t\tsync()\n";
+  cmd_str += "\t\t\telif state == SERVO_RUNNING:\n";
 
-    if (sec_interface_->robot_state_->getVersion() >= 3.1)
-        sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=%.4f, gain=%.0f)\n",
-            servoj_time_, servoj_lookahead_time_, servoj_gain_);
-    else
-        sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_);
-    cmd_str += buf;
+  if (sec_interface_->robot_state_->getVersion() >= 3.1)
+    sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=%.4f, gain=%.0f)\n", servoj_time_, servoj_lookahead_time_,
+            servoj_gain_);
+  else
+    sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_);
+  cmd_str += buf;
 
-    cmd_str += "\t\t\telse:\n";
-    cmd_str += "\t\t\t\tsync()\n";
-    cmd_str += "\t\t\tend\n";
-    cmd_str += "\t\tend\n";
-    cmd_str += "\tend\n";
+  cmd_str += "\t\t\telse:\n";
+  cmd_str += "\t\t\t\tsync()\n";
+  cmd_str += "\t\t\tend\n";
+  cmd_str += "\t\tend\n";
+  cmd_str += "\tend\n";
 
-    sprintf(buf, "\tsocket_open(\"%s\", %i)\n", ip_addr_.c_str(),
-        REVERSE_PORT_);
-    cmd_str += buf;
+  sprintf(buf, "\tsocket_open(\"%s\", %i)\n", ip_addr_.c_str(), REVERSE_PORT_);
+  cmd_str += buf;
 
-    cmd_str += "\tthread_servo = run servoThread()\n";
-    cmd_str += "\tkeepalive = 1\n";
-    cmd_str += "\twhile keepalive > 0:\n";
-    cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1)\n";
-    cmd_str += "\t\tif params_mult[0] > 0:\n";
-    cmd_str += "\t\t\tq = [params_mult[1] / MULT_jointstate, ";
-    cmd_str += "params_mult[2] / MULT_jointstate, ";
-    cmd_str += "params_mult[3] / MULT_jointstate, ";
-    cmd_str += "params_mult[4] / MULT_jointstate, ";
-    cmd_str += "params_mult[5] / MULT_jointstate, ";
-    cmd_str += "params_mult[6] / MULT_jointstate]\n";
-    cmd_str += "\t\t\tkeepalive = params_mult[7]\n";
-    cmd_str += "\t\t\tset_servo_setpoint(q)\n";
-    cmd_str += "\t\tend\n";
-    cmd_str += "\tend\n";
-    cmd_str += "\tsleep(.1)\n";
-    cmd_str += "\tsocket_close()\n";
-    cmd_str += "\tkill thread_servo\n";
-    cmd_str += "end\n";
+  cmd_str += "\tthread_servo = run servoThread()\n";
+  cmd_str += "\tkeepalive = 1\n";
+  cmd_str += "\twhile keepalive > 0:\n";
+  cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1)\n";
+  cmd_str += "\t\tif params_mult[0] > 0:\n";
+  cmd_str += "\t\t\tq = [params_mult[1] / MULT_jointstate, ";
+  cmd_str += "params_mult[2] / MULT_jointstate, ";
+  cmd_str += "params_mult[3] / MULT_jointstate, ";
+  cmd_str += "params_mult[4] / MULT_jointstate, ";
+  cmd_str += "params_mult[5] / MULT_jointstate, ";
+  cmd_str += "params_mult[6] / MULT_jointstate]\n";
+  cmd_str += "\t\t\tkeepalive = params_mult[7]\n";
+  cmd_str += "\t\t\tset_servo_setpoint(q)\n";
+  cmd_str += "\t\tend\n";
+  cmd_str += "\tend\n";
+  cmd_str += "\tsleep(.1)\n";
+  cmd_str += "\tsocket_close()\n";
+  cmd_str += "\tkill thread_servo\n";
+  cmd_str += "end\n";
 
-    rt_interface_->addCommandToQueue(cmd_str);
-    return UrDriver::openServo();
+  rt_interface_->addCommandToQueue(cmd_str);
+  return UrDriver::openServo();
 }
 
 bool UrDriver::openServo()
 {
-    struct sockaddr_in cli_addr;
-    socklen_t clilen;
-    clilen = sizeof(cli_addr);
-    new_sockfd_ = accept(incoming_sockfd_, (struct sockaddr*)&cli_addr,
-        &clilen);
-    if (new_sockfd_ < 0) {
-        print_fatal("ERROR on accepting reverse communication");
-        return false;
-    }
-    reverse_connected_ = true;
-    return true;
+  struct sockaddr_in cli_addr;
+  socklen_t clilen;
+  clilen = sizeof(cli_addr);
+  new_sockfd_ = accept(incoming_sockfd_, (struct sockaddr*)&cli_addr, &clilen);
+  if (new_sockfd_ < 0)
+  {
+    print_fatal("ERROR on accepting reverse communication");
+    return false;
+  }
+  reverse_connected_ = true;
+  return true;
 }
 void UrDriver::closeServo(std::vector<double> positions)
 {
-    if (positions.size() != 6)
-        UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0);
-    else
-        UrDriver::servoj(positions, 0);
+  if (positions.size() != 6)
+    UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0);
+  else
+    UrDriver::servoj(positions, 0);
 
-    reverse_connected_ = false;
-    close(new_sockfd_);
+  reverse_connected_ = false;
+  close(new_sockfd_);
 }
 
 bool UrDriver::start()
 {
-    if (!sec_interface_->start())
-        return false;
-    firmware_version_ = sec_interface_->robot_state_->getVersion();
-    rt_interface_->robot_state_->setVersion(firmware_version_);
-    if (!rt_interface_->start())
-        return false;
-    ip_addr_ = rt_interface_->getLocalIp();
-    print_debug(
-        "Listening on " + ip_addr_ + ":" + std::to_string(REVERSE_PORT_)
-        + "\n");
-    return true;
+  if (!sec_interface_->start())
+    return false;
+  firmware_version_ = sec_interface_->robot_state_->getVersion();
+  rt_interface_->robot_state_->setVersion(firmware_version_);
+  if (!rt_interface_->start())
+    return false;
+  ip_addr_ = rt_interface_->getLocalIp();
+  print_debug("Listening on " + ip_addr_ + ":" + std::to_string(REVERSE_PORT_) + "\n");
+  return true;
 }
 
 void UrDriver::halt()
 {
-    if (executing_traj_) {
-        UrDriver::stopTraj();
-    }
-    sec_interface_->halt();
-    rt_interface_->halt();
-    close(incoming_sockfd_);
+  if (executing_traj_)
+  {
+    UrDriver::stopTraj();
+  }
+  sec_interface_->halt();
+  rt_interface_->halt();
+  close(incoming_sockfd_);
 }
 
-void UrDriver::setSpeed(double q0, double q1, double q2, double q3, double q4,
-    double q5, double acc)
+void UrDriver::setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc)
 {
-    rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc);
+  rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc);
 }
 
 std::vector<std::string> UrDriver::getJointNames()
 {
-    return joint_names_;
+  return joint_names_;
 }
 
 void UrDriver::setJointNames(std::vector<std::string> jn)
 {
-    joint_names_ = jn;
+  joint_names_ = jn;
 }
 
 void UrDriver::setToolVoltage(unsigned int v)
 {
-    char buf[256];
-    sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v);
-    rt_interface_->addCommandToQueue(buf);
-    print_debug(buf);
+  char buf[256];
+  sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v);
+  rt_interface_->addCommandToQueue(buf);
+  print_debug(buf);
 }
 void UrDriver::setFlag(unsigned int n, bool b)
 {
-    char buf[256];
-    sprintf(buf, "sec setOut():\n\tset_flag(%d, %s)\nend\n", n,
-        b ? "True" : "False");
-    rt_interface_->addCommandToQueue(buf);
-    print_debug(buf);
+  char buf[256];
+  sprintf(buf, "sec setOut():\n\tset_flag(%d, %s)\nend\n", n, b ? "True" : "False");
+  rt_interface_->addCommandToQueue(buf);
+  print_debug(buf);
 }
 void UrDriver::setDigitalOut(unsigned int n, bool b)
 {
-    char buf[256];
-    if (firmware_version_ < 2) {
-        sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n,
-            b ? "True" : "False");
-    } else if (n > 15) {
-        sprintf(buf,
-            "sec setOut():\n\tset_tool_digital_out(%d, %s)\nend\n",
-            n - 16, b ? "True" : "False");
-    } else if (n > 7) {
-        sprintf(buf, "sec setOut():\n\tset_configurable_digital_out(%d, %s)\nend\n",
-            n - 8, b ? "True" : "False");
-
-    } else {
-        sprintf(buf, "sec setOut():\n\tset_standard_digital_out(%d, %s)\nend\n",
-            n, b ? "True" : "False");
-    }
-    rt_interface_->addCommandToQueue(buf);
-    print_debug(buf);
+  char buf[256];
+  if (firmware_version_ < 2)
+  {
+    sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n, b ? "True" : "False");
+  }
+  else if (n > 15)
+  {
+    sprintf(buf, "sec setOut():\n\tset_tool_digital_out(%d, %s)\nend\n", n - 16, b ? "True" : "False");
+  }
+  else if (n > 7)
+  {
+    sprintf(buf, "sec setOut():\n\tset_configurable_digital_out(%d, %s)\nend\n", n - 8, b ? "True" : "False");
+  }
+  else
+  {
+    sprintf(buf, "sec setOut():\n\tset_standard_digital_out(%d, %s)\nend\n", n, b ? "True" : "False");
+  }
+  rt_interface_->addCommandToQueue(buf);
+  print_debug(buf);
 }
 void UrDriver::setAnalogOut(unsigned int n, double f)
 {
-    char buf[256];
-    if (firmware_version_ < 2) {
-        sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f);
-    } else {
-        sprintf(buf, "sec setOut():\n\tset_standard_analog_out(%d, %1.4f)\nend\n", n, f);
-    }
+  char buf[256];
+  if (firmware_version_ < 2)
+  {
+    sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f);
+  }
+  else
+  {
+    sprintf(buf, "sec setOut():\n\tset_standard_analog_out(%d, %1.4f)\nend\n", n, f);
+  }
 
-    rt_interface_->addCommandToQueue(buf);
-    print_debug(buf);
+  rt_interface_->addCommandToQueue(buf);
+  print_debug(buf);
 }
 
 bool UrDriver::setPayload(double m)
 {
-    if ((m < maximum_payload_) && (m > minimum_payload_)) {
-        char buf[256];
-        sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m);
-        rt_interface_->addCommandToQueue(buf);
-        print_debug(buf);
-        return true;
-    } else
-        return false;
+  if ((m < maximum_payload_) && (m > minimum_payload_))
+  {
+    char buf[256];
+    sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m);
+    rt_interface_->addCommandToQueue(buf);
+    print_debug(buf);
+    return true;
+  }
+  else
+    return false;
 }
 
 void UrDriver::setMinPayload(double m)
 {
-    if (m > 0) {
-        minimum_payload_ = m;
-    } else {
-        minimum_payload_ = 0;
-    }
+  if (m > 0)
+  {
+    minimum_payload_ = m;
+  }
+  else
+  {
+    minimum_payload_ = 0;
+  }
 }
 void UrDriver::setMaxPayload(double m)
 {
-    maximum_payload_ = m;
+  maximum_payload_ = m;
 }
 void UrDriver::setServojTime(double t)
 {
-    if (t > 0.008) {
-        servoj_time_ = t;
-    } else {
-        servoj_time_ = 0.008;
-    }
+  if (t > 0.008)
+  {
+    servoj_time_ = t;
+  }
+  else
+  {
+    servoj_time_ = 0.008;
+  }
 }
 void UrDriver::setServojLookahead(double t)
 {
-    if (t > 0.03) {
-        if (t < 0.2) {
-            servoj_lookahead_time_ = t;
-        } else {
-            servoj_lookahead_time_ = 0.2;
-        }
-    } else {
-        servoj_lookahead_time_ = 0.03;
+  if (t > 0.03)
+  {
+    if (t < 0.2)
+    {
+      servoj_lookahead_time_ = t;
     }
+    else
+    {
+      servoj_lookahead_time_ = 0.2;
+    }
+  }
+  else
+  {
+    servoj_lookahead_time_ = 0.03;
+  }
 }
 void UrDriver::setServojGain(double g)
 {
-    if (g > 100) {
-        if (g < 2000) {
-            servoj_gain_ = g;
-        } else {
-            servoj_gain_ = 2000;
-        }
-    } else {
-        servoj_gain_ = 100;
+  if (g > 100)
+  {
+    if (g < 2000)
+    {
+      servoj_gain_ = g;
     }
+    else
+    {
+      servoj_gain_ = 2000;
+    }
+  }
+  else
+  {
+    servoj_gain_ = 100;
+  }
 }
diff --git a/src/ur_hardware_interface.cpp b/src/ur_hardware_interface.cpp
index 152452c..06b59f9 100644
--- a/src/ur_hardware_interface.cpp
+++ b/src/ur_hardware_interface.cpp
@@ -57,228 +57,227 @@
 
 #include <ur_modern_driver/ur_hardware_interface.h>
 
-namespace ros_control_ur {
-
-UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot)
-    : nh_(nh)
-    , robot_(robot)
+namespace ros_control_ur
 {
-    // Initialize shared memory and interfaces here
-    init(); // this implementation loads from rosparam
+UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) : nh_(nh), robot_(robot)
+{
+  // Initialize shared memory and interfaces here
+  init();  // this implementation loads from rosparam
 
-    max_vel_change_ = 0.12; // equivalent of an acceleration of 15 rad/sec^2
+  max_vel_change_ = 0.12;  // equivalent of an acceleration of 15 rad/sec^2
 
-    ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface.");
+  ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface.");
 }
 
 void UrHardwareInterface::init()
 {
-    ROS_INFO_STREAM_NAMED("ur_hardware_interface",
-        "Reading rosparams from namespace: " << nh_.getNamespace());
+  ROS_INFO_STREAM_NAMED("ur_hardware_interface", "Reading rosparams from namespace: " << nh_.getNamespace());
 
-    // Get joint names
-    nh_.getParam("hardware_interface/joints", joint_names_);
-    if (joint_names_.size() == 0) {
-        ROS_FATAL_STREAM_NAMED("ur_hardware_interface",
-            "No joints found on parameter server for controller, did you load the proper yaml file?"
-                << " Namespace: " << nh_.getNamespace());
-        exit(-1);
-    }
-    num_joints_ = joint_names_.size();
+  // Get joint names
+  nh_.getParam("hardware_interface/joints", joint_names_);
+  if (joint_names_.size() == 0)
+  {
+    ROS_FATAL_STREAM_NAMED("ur_hardware_interface",
+                           "No joints found on parameter server for controller, did you load the proper yaml file?"
+                               << " Namespace: " << nh_.getNamespace());
+    exit(-1);
+  }
+  num_joints_ = joint_names_.size();
 
-    // Resize vectors
-    joint_position_.resize(num_joints_);
-    joint_velocity_.resize(num_joints_);
-    joint_effort_.resize(num_joints_);
-    joint_position_command_.resize(num_joints_);
-    joint_velocity_command_.resize(num_joints_);
-    prev_joint_velocity_command_.resize(num_joints_);
+  // Resize vectors
+  joint_position_.resize(num_joints_);
+  joint_velocity_.resize(num_joints_);
+  joint_effort_.resize(num_joints_);
+  joint_position_command_.resize(num_joints_);
+  joint_velocity_command_.resize(num_joints_);
+  prev_joint_velocity_command_.resize(num_joints_);
 
-    // Initialize controller
-    for (std::size_t i = 0; i < num_joints_; ++i) {
-        ROS_DEBUG_STREAM_NAMED("ur_hardware_interface",
-            "Loading joint name: " << joint_names_[i]);
+  // Initialize controller
+  for (std::size_t i = 0; i < num_joints_; ++i)
+  {
+    ROS_DEBUG_STREAM_NAMED("ur_hardware_interface", "Loading joint name: " << joint_names_[i]);
 
-        // Create joint state interface
-        joint_state_interface_.registerHandle(
-            hardware_interface::JointStateHandle(joint_names_[i],
-                &joint_position_[i], &joint_velocity_[i],
-                &joint_effort_[i]));
+    // Create joint state interface
+    joint_state_interface_.registerHandle(hardware_interface::JointStateHandle(joint_names_[i], &joint_position_[i],
+                                                                               &joint_velocity_[i], &joint_effort_[i]));
 
-        // Create position joint interface
-        position_joint_interface_.registerHandle(
-            hardware_interface::JointHandle(
-                joint_state_interface_.getHandle(joint_names_[i]),
-                &joint_position_command_[i]));
+    // Create position joint interface
+    position_joint_interface_.registerHandle(hardware_interface::JointHandle(
+        joint_state_interface_.getHandle(joint_names_[i]), &joint_position_command_[i]));
 
-        // Create velocity joint interface
-        velocity_joint_interface_.registerHandle(
-            hardware_interface::JointHandle(
-                joint_state_interface_.getHandle(joint_names_[i]),
-                &joint_velocity_command_[i]));
-        prev_joint_velocity_command_[i] = 0.;
-    }
+    // Create velocity joint interface
+    velocity_joint_interface_.registerHandle(hardware_interface::JointHandle(
+        joint_state_interface_.getHandle(joint_names_[i]), &joint_velocity_command_[i]));
+    prev_joint_velocity_command_[i] = 0.;
+  }
 
-    // Create force torque interface
-    force_torque_interface_.registerHandle(
-        hardware_interface::ForceTorqueSensorHandle("wrench", "",
-            robot_force_, robot_torque_));
+  // Create force torque interface
+  force_torque_interface_.registerHandle(
+      hardware_interface::ForceTorqueSensorHandle("wrench", "", robot_force_, robot_torque_));
 
-    registerInterface(&joint_state_interface_); // From RobotHW base class.
-    registerInterface(&position_joint_interface_); // From RobotHW base class.
-    registerInterface(&velocity_joint_interface_); // From RobotHW base class.
-    registerInterface(&force_torque_interface_); // From RobotHW base class.
-    velocity_interface_running_ = false;
-    position_interface_running_ = false;
+  registerInterface(&joint_state_interface_);     // From RobotHW base class.
+  registerInterface(&position_joint_interface_);  // From RobotHW base class.
+  registerInterface(&velocity_joint_interface_);  // From RobotHW base class.
+  registerInterface(&force_torque_interface_);    // From RobotHW base class.
+  velocity_interface_running_ = false;
+  position_interface_running_ = false;
 }
 
 void UrHardwareInterface::read()
 {
-    std::vector<double> pos, vel, current, tcp;
-    pos = robot_->rt_interface_->robot_state_->getQActual();
-    vel = robot_->rt_interface_->robot_state_->getQdActual();
-    current = robot_->rt_interface_->robot_state_->getIActual();
-    tcp = robot_->rt_interface_->robot_state_->getTcpForce();
-    for (std::size_t i = 0; i < num_joints_; ++i) {
-        joint_position_[i] = pos[i];
-        joint_velocity_[i] = vel[i];
-        joint_effort_[i] = current[i];
-    }
-    for (std::size_t i = 0; i < 3; ++i) {
-        robot_force_[i] = tcp[i];
-        robot_torque_[i] = tcp[i + 3];
-    }
+  std::vector<double> pos, vel, current, tcp;
+  pos = robot_->rt_interface_->robot_state_->getQActual();
+  vel = robot_->rt_interface_->robot_state_->getQdActual();
+  current = robot_->rt_interface_->robot_state_->getIActual();
+  tcp = robot_->rt_interface_->robot_state_->getTcpForce();
+  for (std::size_t i = 0; i < num_joints_; ++i)
+  {
+    joint_position_[i] = pos[i];
+    joint_velocity_[i] = vel[i];
+    joint_effort_[i] = current[i];
+  }
+  for (std::size_t i = 0; i < 3; ++i)
+  {
+    robot_force_[i] = tcp[i];
+    robot_torque_[i] = tcp[i + 3];
+  }
 }
 
 void UrHardwareInterface::setMaxVelChange(double inp)
 {
-    max_vel_change_ = inp;
+  max_vel_change_ = inp;
 }
 
 void UrHardwareInterface::write()
 {
-    if (velocity_interface_running_) {
-        std::vector<double> cmd;
-        //do some rate limiting
-        cmd.resize(joint_velocity_command_.size());
-        for (unsigned int i = 0; i < joint_velocity_command_.size(); i++) {
-            cmd[i] = joint_velocity_command_[i];
-            if (cmd[i] > prev_joint_velocity_command_[i] + max_vel_change_) {
-                cmd[i] = prev_joint_velocity_command_[i] + max_vel_change_;
-            } else if (cmd[i]
-                < prev_joint_velocity_command_[i] - max_vel_change_) {
-                cmd[i] = prev_joint_velocity_command_[i] - max_vel_change_;
-            }
-            prev_joint_velocity_command_[i] = cmd[i];
-        }
-        robot_->setSpeed(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], max_vel_change_ * 125);
-    } else if (position_interface_running_) {
-        robot_->servoj(joint_position_command_);
+  if (velocity_interface_running_)
+  {
+    std::vector<double> cmd;
+    // do some rate limiting
+    cmd.resize(joint_velocity_command_.size());
+    for (unsigned int i = 0; i < joint_velocity_command_.size(); i++)
+    {
+      cmd[i] = joint_velocity_command_[i];
+      if (cmd[i] > prev_joint_velocity_command_[i] + max_vel_change_)
+      {
+        cmd[i] = prev_joint_velocity_command_[i] + max_vel_change_;
+      }
+      else if (cmd[i] < prev_joint_velocity_command_[i] - max_vel_change_)
+      {
+        cmd[i] = prev_joint_velocity_command_[i] - max_vel_change_;
+      }
+      prev_joint_velocity_command_[i] = cmd[i];
     }
+    robot_->setSpeed(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], max_vel_change_ * 125);
+  }
+  else if (position_interface_running_)
+  {
+    robot_->servoj(joint_position_command_);
+  }
 }
 
-bool UrHardwareInterface::canSwitch(
-    const std::list<hardware_interface::ControllerInfo>& start_list,
-    const std::list<hardware_interface::ControllerInfo>& stop_list) const
+bool UrHardwareInterface::canSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
+                                    const std::list<hardware_interface::ControllerInfo>& stop_list) const
 {
-    for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin(); controller_it != start_list.end();
-         ++controller_it) {
-        if (controller_it->hardware_interface
-            == "hardware_interface::VelocityJointInterface") {
-            if (velocity_interface_running_) {
-                ROS_ERROR(
-                    "%s: An interface of that type (%s) is already running",
-                    controller_it->name.c_str(),
-                    controller_it->hardware_interface.c_str());
-                return false;
-            }
-            if (position_interface_running_) {
-                bool error = true;
-                for (std::list<hardware_interface::ControllerInfo>::const_iterator stop_controller_it = stop_list.begin();
-                     stop_controller_it != stop_list.end();
-                     ++stop_controller_it) {
-                    if (stop_controller_it->hardware_interface
-                        == "hardware_interface::PositionJointInterface") {
-                        error = false;
-                        break;
-                    }
-                }
-                if (error) {
-                    ROS_ERROR(
-                        "%s (type %s) can not be run simultaneously with a PositionJointInterface",
-                        controller_it->name.c_str(),
-                        controller_it->hardware_interface.c_str());
-                    return false;
-                }
-            }
-        } else if (controller_it->hardware_interface
-            == "hardware_interface::PositionJointInterface") {
-            if (position_interface_running_) {
-                ROS_ERROR(
-                    "%s: An interface of that type (%s) is already running",
-                    controller_it->name.c_str(),
-                    controller_it->hardware_interface.c_str());
-                return false;
-            }
-            if (velocity_interface_running_) {
-                bool error = true;
-                for (std::list<hardware_interface::ControllerInfo>::const_iterator stop_controller_it = stop_list.begin();
-                     stop_controller_it != stop_list.end();
-                     ++stop_controller_it) {
-                    if (stop_controller_it->hardware_interface
-                        == "hardware_interface::VelocityJointInterface") {
-                        error = false;
-                        break;
-                    }
-                }
-                if (error) {
-                    ROS_ERROR(
-                        "%s (type %s) can not be run simultaneously with a VelocityJointInterface",
-                        controller_it->name.c_str(),
-                        controller_it->hardware_interface.c_str());
-                    return false;
-                }
-            }
+  for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin();
+       controller_it != start_list.end(); ++controller_it)
+  {
+    if (controller_it->hardware_interface == "hardware_interface::VelocityJointInterface")
+    {
+      if (velocity_interface_running_)
+      {
+        ROS_ERROR("%s: An interface of that type (%s) is already running", controller_it->name.c_str(),
+                  controller_it->hardware_interface.c_str());
+        return false;
+      }
+      if (position_interface_running_)
+      {
+        bool error = true;
+        for (std::list<hardware_interface::ControllerInfo>::const_iterator stop_controller_it = stop_list.begin();
+             stop_controller_it != stop_list.end(); ++stop_controller_it)
+        {
+          if (stop_controller_it->hardware_interface == "hardware_interface::PositionJointInterface")
+          {
+            error = false;
+            break;
+          }
         }
+        if (error)
+        {
+          ROS_ERROR("%s (type %s) can not be run simultaneously with a PositionJointInterface",
+                    controller_it->name.c_str(), controller_it->hardware_interface.c_str());
+          return false;
+        }
+      }
     }
+    else if (controller_it->hardware_interface == "hardware_interface::PositionJointInterface")
+    {
+      if (position_interface_running_)
+      {
+        ROS_ERROR("%s: An interface of that type (%s) is already running", controller_it->name.c_str(),
+                  controller_it->hardware_interface.c_str());
+        return false;
+      }
+      if (velocity_interface_running_)
+      {
+        bool error = true;
+        for (std::list<hardware_interface::ControllerInfo>::const_iterator stop_controller_it = stop_list.begin();
+             stop_controller_it != stop_list.end(); ++stop_controller_it)
+        {
+          if (stop_controller_it->hardware_interface == "hardware_interface::VelocityJointInterface")
+          {
+            error = false;
+            break;
+          }
+        }
+        if (error)
+        {
+          ROS_ERROR("%s (type %s) can not be run simultaneously with a VelocityJointInterface",
+                    controller_it->name.c_str(), controller_it->hardware_interface.c_str());
+          return false;
+        }
+      }
+    }
+  }
 
-    // we can always stop a controller
-    return true;
+  // we can always stop a controller
+  return true;
 }
 
-void UrHardwareInterface::doSwitch(
-    const std::list<hardware_interface::ControllerInfo>& start_list,
-    const std::list<hardware_interface::ControllerInfo>& stop_list)
+void UrHardwareInterface::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
+                                   const std::list<hardware_interface::ControllerInfo>& stop_list)
 {
-    for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = stop_list.begin(); controller_it != stop_list.end();
-         ++controller_it) {
-        if (controller_it->hardware_interface
-            == "hardware_interface::VelocityJointInterface") {
-            velocity_interface_running_ = false;
-            ROS_DEBUG("Stopping velocity interface");
-        }
-        if (controller_it->hardware_interface
-            == "hardware_interface::PositionJointInterface") {
-            position_interface_running_ = false;
-            std::vector<double> tmp;
-            robot_->closeServo(tmp);
-            ROS_DEBUG("Stopping position interface");
-        }
+  for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = stop_list.begin();
+       controller_it != stop_list.end(); ++controller_it)
+  {
+    if (controller_it->hardware_interface == "hardware_interface::VelocityJointInterface")
+    {
+      velocity_interface_running_ = false;
+      ROS_DEBUG("Stopping velocity interface");
     }
-    for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin(); controller_it != start_list.end();
-         ++controller_it) {
-        if (controller_it->hardware_interface
-            == "hardware_interface::VelocityJointInterface") {
-            velocity_interface_running_ = true;
-            ROS_DEBUG("Starting velocity interface");
-        }
-        if (controller_it->hardware_interface
-            == "hardware_interface::PositionJointInterface") {
-            position_interface_running_ = true;
-            robot_->uploadProg();
-            ROS_DEBUG("Starting position interface");
-        }
+    if (controller_it->hardware_interface == "hardware_interface::PositionJointInterface")
+    {
+      position_interface_running_ = false;
+      std::vector<double> tmp;
+      robot_->closeServo(tmp);
+      ROS_DEBUG("Stopping position interface");
     }
+  }
+  for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin();
+       controller_it != start_list.end(); ++controller_it)
+  {
+    if (controller_it->hardware_interface == "hardware_interface::VelocityJointInterface")
+    {
+      velocity_interface_running_ = true;
+      ROS_DEBUG("Starting velocity interface");
+    }
+    if (controller_it->hardware_interface == "hardware_interface::PositionJointInterface")
+    {
+      position_interface_running_ = true;
+      robot_->uploadProg();
+      ROS_DEBUG("Starting position interface");
+    }
+  }
 }
 
-} // namespace
+}  // namespace
diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp
index b45224b..e947aa9 100644
--- a/src/ur_realtime_communication.cpp
+++ b/src/ur_realtime_communication.cpp
@@ -18,183 +18,194 @@
 
 #include "ur_modern_driver/ur_realtime_communication.h"
 
-UrRealtimeCommunication::UrRealtimeCommunication(
-    std::condition_variable& msg_cond, std::string host,
-    unsigned int safety_count_max)
+UrRealtimeCommunication::UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host,
+                                                 unsigned int safety_count_max)
 {
-    robot_state_ = new RobotStateRT(msg_cond);
-    bzero((char*)&serv_addr_, sizeof(serv_addr_));
-    sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
-    if (sockfd_ < 0) {
-        print_fatal("ERROR opening socket");
-    }
-    server_ = gethostbyname(host.c_str());
-    if (server_ == NULL) {
-        print_fatal("ERROR, no such host");
-    }
-    serv_addr_.sin_family = AF_INET;
-    bcopy((char*)server_->h_addr, (char*)&serv_addr_.sin_addr.s_addr, server_->h_length);
-    serv_addr_.sin_port = htons(30003);
-    flag_ = 1;
-    setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
-    setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
-    setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
-    fcntl(sockfd_, F_SETFL, O_NONBLOCK);
-    connected_ = false;
-    keepalive_ = false;
-    safety_count_ = safety_count_max + 1;
-    safety_count_max_ = safety_count_max;
+  robot_state_ = new RobotStateRT(msg_cond);
+  bzero((char*)&serv_addr_, sizeof(serv_addr_));
+  sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
+  if (sockfd_ < 0)
+  {
+    print_fatal("ERROR opening socket");
+  }
+  server_ = gethostbyname(host.c_str());
+  if (server_ == NULL)
+  {
+    print_fatal("ERROR, no such host");
+  }
+  serv_addr_.sin_family = AF_INET;
+  bcopy((char*)server_->h_addr, (char*)&serv_addr_.sin_addr.s_addr, server_->h_length);
+  serv_addr_.sin_port = htons(30003);
+  flag_ = 1;
+  setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
+  setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
+  setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
+  fcntl(sockfd_, F_SETFL, O_NONBLOCK);
+  connected_ = false;
+  keepalive_ = false;
+  safety_count_ = safety_count_max + 1;
+  safety_count_max_ = safety_count_max;
 }
 
 bool UrRealtimeCommunication::start()
 {
-    fd_set writefds;
-    struct timeval timeout;
+  fd_set writefds;
+  struct timeval timeout;
 
-    keepalive_ = true;
-    print_debug("Realtime port: Connecting...");
+  keepalive_ = true;
+  print_debug("Realtime port: Connecting...");
 
-    connect(sockfd_, (struct sockaddr*)&serv_addr_, sizeof(serv_addr_));
-    FD_ZERO(&writefds);
-    FD_SET(sockfd_, &writefds);
-    timeout.tv_sec = 10;
-    timeout.tv_usec = 0;
-    select(sockfd_ + 1, NULL, &writefds, NULL, &timeout);
-    unsigned int flag_len;
-    getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
-    if (flag_ < 0) {
-        print_fatal("Error connecting to RT port 30003");
-        return false;
-    }
-    sockaddr_in name;
-    socklen_t namelen = sizeof(name);
-    int err = getsockname(sockfd_, (sockaddr*)&name, &namelen);
-    if (err < 0) {
-        print_fatal("Could not get local IP");
-        close(sockfd_);
-        return false;
-    }
-    char str[18];
-    inet_ntop(AF_INET, &name.sin_addr, str, 18);
-    local_ip_ = str;
-    comThread_ = std::thread(&UrRealtimeCommunication::run, this);
-    return true;
+  connect(sockfd_, (struct sockaddr*)&serv_addr_, sizeof(serv_addr_));
+  FD_ZERO(&writefds);
+  FD_SET(sockfd_, &writefds);
+  timeout.tv_sec = 10;
+  timeout.tv_usec = 0;
+  select(sockfd_ + 1, NULL, &writefds, NULL, &timeout);
+  unsigned int flag_len;
+  getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
+  if (flag_ < 0)
+  {
+    print_fatal("Error connecting to RT port 30003");
+    return false;
+  }
+  sockaddr_in name;
+  socklen_t namelen = sizeof(name);
+  int err = getsockname(sockfd_, (sockaddr*)&name, &namelen);
+  if (err < 0)
+  {
+    print_fatal("Could not get local IP");
+    close(sockfd_);
+    return false;
+  }
+  char str[18];
+  inet_ntop(AF_INET, &name.sin_addr, str, 18);
+  local_ip_ = str;
+  comThread_ = std::thread(&UrRealtimeCommunication::run, this);
+  return true;
 }
 
 void UrRealtimeCommunication::halt()
 {
-    keepalive_ = false;
-    comThread_.join();
+  keepalive_ = false;
+  comThread_.join();
 }
 
 void UrRealtimeCommunication::addCommandToQueue(std::string inp)
 {
-    int bytes_written;
-    if (inp.back() != '\n') {
-        inp.append("\n");
-    }
-    if (connected_)
-        bytes_written = write(sockfd_, inp.c_str(), inp.length());
-    else
-        print_error("Could not send command \"" + inp + "\". The robot is not connected! Command is discarded");
+  int bytes_written;
+  if (inp.back() != '\n')
+  {
+    inp.append("\n");
+  }
+  if (connected_)
+    bytes_written = write(sockfd_, inp.c_str(), inp.length());
+  else
+    print_error("Could not send command \"" + inp + "\". The robot is not connected! Command is discarded");
 }
 
-void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2,
-    double q3, double q4, double q5, double acc)
+void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc)
 {
-    char cmd[1024];
-    if (robot_state_->getVersion() >= 3.1) {
-        sprintf(cmd,
-            "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n",
-            q0, q1, q2, q3, q4, q5, acc);
-    } else {
-        sprintf(cmd,
-            "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n",
-            q0, q1, q2, q3, q4, q5, acc);
-    }
-    addCommandToQueue((std::string)(cmd));
-    if (q0 != 0. or q1 != 0. or q2 != 0. or q3 != 0. or q4 != 0. or q5 != 0.) {
-        //If a joint speed is set, make sure we stop it again after some time if the user doesn't
-        safety_count_ = 0;
-    }
+  char cmd[1024];
+  if (robot_state_->getVersion() >= 3.1)
+  {
+    sprintf(cmd, "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n", q0, q1, q2, q3, q4, q5, acc);
+  }
+  else
+  {
+    sprintf(cmd, "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n", q0, q1, q2, q3, q4, q5, acc);
+  }
+  addCommandToQueue((std::string)(cmd));
+  if (q0 != 0. or q1 != 0. or q2 != 0. or q3 != 0. or q4 != 0. or q5 != 0.)
+  {
+    // If a joint speed is set, make sure we stop it again after some time if the user doesn't
+    safety_count_ = 0;
+  }
 }
 
 void UrRealtimeCommunication::run()
 {
-    uint8_t buf[2048];
-    int bytes_read;
-    bzero(buf, 2048);
-    struct timeval timeout;
-    fd_set readfds;
-    FD_ZERO(&readfds);
-    FD_SET(sockfd_, &readfds);
-    print_debug("Realtime port: Got connection");
-    connected_ = true;
-    while (keepalive_) {
-        while (connected_ && keepalive_) {
-            timeout.tv_sec = 0; //do this each loop as selects modifies timeout
-            timeout.tv_usec = 500000; // timeout of 0.5 sec
-            select(sockfd_ + 1, &readfds, NULL, NULL, &timeout);
-            bytes_read = read(sockfd_, buf, 2048);
-            if (bytes_read > 0) {
-                setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
-                    sizeof(int));
-                robot_state_->unpack(buf);
-                if (safety_count_ == safety_count_max_) {
-                    setSpeed(0., 0., 0., 0., 0., 0.);
-                }
-                safety_count_ += 1;
-            } else {
-                connected_ = false;
-                close(sockfd_);
-            }
-        }
-        if (keepalive_) {
-            //reconnect
-            print_warning("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...");
-            sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
-            if (sockfd_ < 0) {
-                print_fatal("ERROR opening socket");
-            }
-            flag_ = 1;
-            setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_,
-                sizeof(int));
-            setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
-                sizeof(int));
-
-            setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_,
-                sizeof(int));
-            fcntl(sockfd_, F_SETFL, O_NONBLOCK);
-            while (keepalive_ && !connected_) {
-                std::this_thread::sleep_for(std::chrono::seconds(10));
-                fd_set writefds;
-
-                connect(sockfd_, (struct sockaddr*)&serv_addr_,
-                    sizeof(serv_addr_));
-                FD_ZERO(&writefds);
-                FD_SET(sockfd_, &writefds);
-                select(sockfd_ + 1, NULL, &writefds, NULL, NULL);
-                unsigned int flag_len;
-                getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
-                if (flag_ < 0) {
-                    print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds...");
-                } else {
-                    connected_ = true;
-                    print_info("Realtime port: Reconnected");
-                }
-            }
+  uint8_t buf[2048];
+  int bytes_read;
+  bzero(buf, 2048);
+  struct timeval timeout;
+  fd_set readfds;
+  FD_ZERO(&readfds);
+  FD_SET(sockfd_, &readfds);
+  print_debug("Realtime port: Got connection");
+  connected_ = true;
+  while (keepalive_)
+  {
+    while (connected_ && keepalive_)
+    {
+      timeout.tv_sec = 0;        // do this each loop as selects modifies timeout
+      timeout.tv_usec = 500000;  // timeout of 0.5 sec
+      select(sockfd_ + 1, &readfds, NULL, NULL, &timeout);
+      bytes_read = read(sockfd_, buf, 2048);
+      if (bytes_read > 0)
+      {
+        setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
+        robot_state_->unpack(buf);
+        if (safety_count_ == safety_count_max_)
+        {
+          setSpeed(0., 0., 0., 0., 0., 0.);
         }
+        safety_count_ += 1;
+      }
+      else
+      {
+        connected_ = false;
+        close(sockfd_);
+      }
     }
-    setSpeed(0., 0., 0., 0., 0., 0.);
-    close(sockfd_);
+    if (keepalive_)
+    {
+      // reconnect
+      print_warning("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...");
+      sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
+      if (sockfd_ < 0)
+      {
+        print_fatal("ERROR opening socket");
+      }
+      flag_ = 1;
+      setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
+      setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
+
+      setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
+      fcntl(sockfd_, F_SETFL, O_NONBLOCK);
+      while (keepalive_ && !connected_)
+      {
+        std::this_thread::sleep_for(std::chrono::seconds(10));
+        fd_set writefds;
+
+        connect(sockfd_, (struct sockaddr*)&serv_addr_, sizeof(serv_addr_));
+        FD_ZERO(&writefds);
+        FD_SET(sockfd_, &writefds);
+        select(sockfd_ + 1, NULL, &writefds, NULL, NULL);
+        unsigned int flag_len;
+        getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
+        if (flag_ < 0)
+        {
+          print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 "
+                      "seconds...");
+        }
+        else
+        {
+          connected_ = true;
+          print_info("Realtime port: Reconnected");
+        }
+      }
+    }
+  }
+  setSpeed(0., 0., 0., 0., 0., 0.);
+  close(sockfd_);
 }
 
 void UrRealtimeCommunication::setSafetyCountMax(uint inp)
 {
-    safety_count_max_ = inp;
+  safety_count_max_ = inp;
 }
 
 std::string UrRealtimeCommunication::getLocalIp()
 {
-    return local_ip_;
+  return local_ip_;
 }
diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp
index 92fe341..6f3f854 100644
--- a/src/ur_ros_wrapper.cpp
+++ b/src/ur_ros_wrapper.cpp
@@ -16,19 +16,22 @@
  * limitations under the License.
  */
 
-#include "ur_modern_driver/do_output.h"
-#include "ur_modern_driver/ur_driver.h"
-#include "ur_modern_driver/ur_hardware_interface.h"
+#include <string.h>
+#include <time.h>
 #include <algorithm>
 #include <chrono>
 #include <cmath>
 #include <condition_variable>
 #include <mutex>
-#include <string.h>
 #include <thread>
-#include <time.h>
 #include <vector>
+#include "ur_modern_driver/do_output.h"
+#include "ur_modern_driver/ur_driver.h"
+#include "ur_modern_driver/ur_hardware_interface.h"
 
+#include <controller_manager/controller_manager.h>
+#include <realtime_tools/realtime_publisher.h>
+#include <ros/console.h>
 #include "actionlib/server/action_server.h"
 #include "actionlib/server/server_goal_handle.h"
 #include "control_msgs/FollowJointTrajectoryAction.h"
@@ -47,785 +50,822 @@
 #include "ur_msgs/SetPayload.h"
 #include "ur_msgs/SetPayloadRequest.h"
 #include "ur_msgs/SetPayloadResponse.h"
-#include <controller_manager/controller_manager.h>
-#include <realtime_tools/realtime_publisher.h>
-#include <ros/console.h>
 
 /// TF
 #include <tf/tf.h>
 #include <tf/transform_broadcaster.h>
 
-class RosWrapper {
+class RosWrapper
+{
 protected:
-    UrDriver robot_;
-    std::condition_variable rt_msg_cond_;
-    std::condition_variable msg_cond_;
-    ros::NodeHandle nh_;
-    actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction> as_;
-    actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> goal_handle_;
-    bool has_goal_;
-    control_msgs::FollowJointTrajectoryFeedback feedback_;
-    control_msgs::FollowJointTrajectoryResult result_;
-    ros::Subscriber speed_sub_;
-    ros::Subscriber urscript_sub_;
-    ros::ServiceServer io_srv_;
-    ros::ServiceServer payload_srv_;
-    std::thread* rt_publish_thread_;
-    std::thread* mb_publish_thread_;
-    double io_flag_delay_;
-    double max_velocity_;
-    std::vector<double> joint_offsets_;
-    std::string base_frame_;
-    std::string tool_frame_;
-    bool use_ros_control_;
-    std::thread* ros_control_thread_;
-    boost::shared_ptr<ros_control_ur::UrHardwareInterface> hardware_interface_;
-    boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;
+  UrDriver robot_;
+  std::condition_variable rt_msg_cond_;
+  std::condition_variable msg_cond_;
+  ros::NodeHandle nh_;
+  actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction> as_;
+  actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> goal_handle_;
+  bool has_goal_;
+  control_msgs::FollowJointTrajectoryFeedback feedback_;
+  control_msgs::FollowJointTrajectoryResult result_;
+  ros::Subscriber speed_sub_;
+  ros::Subscriber urscript_sub_;
+  ros::ServiceServer io_srv_;
+  ros::ServiceServer payload_srv_;
+  std::thread* rt_publish_thread_;
+  std::thread* mb_publish_thread_;
+  double io_flag_delay_;
+  double max_velocity_;
+  std::vector<double> joint_offsets_;
+  std::string base_frame_;
+  std::string tool_frame_;
+  bool use_ros_control_;
+  std::thread* ros_control_thread_;
+  boost::shared_ptr<ros_control_ur::UrHardwareInterface> hardware_interface_;
+  boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;
 
 public:
-    RosWrapper(std::string host, int reverse_port)
-        : as_(nh_, "follow_joint_trajectory",
-              boost::bind(&RosWrapper::goalCB, this, _1),
-              boost::bind(&RosWrapper::cancelCB, this, _1), false)
-        , robot_(
-              rt_msg_cond_, msg_cond_, host, reverse_port, 0.03, 300)
-        , io_flag_delay_(0.05)
-        , joint_offsets_(
-              6, 0.0)
+  RosWrapper(std::string host, int reverse_port)
+    : as_(nh_, "follow_joint_trajectory", boost::bind(&RosWrapper::goalCB, this, _1),
+          boost::bind(&RosWrapper::cancelCB, this, _1), false)
+    , robot_(rt_msg_cond_, msg_cond_, host, reverse_port, 0.03, 300)
+    , io_flag_delay_(0.05)
+    , joint_offsets_(6, 0.0)
+  {
+    std::string joint_prefix = "";
+    std::vector<std::string> joint_names;
+    char buf[256];
+
+    if (ros::param::get("~prefix", joint_prefix))
     {
+      if (joint_prefix.length() > 0)
+      {
+        sprintf(buf, "Setting prefix to %s", joint_prefix.c_str());
+        print_info(buf);
+      }
+    }
+    joint_names.push_back(joint_prefix + "shoulder_pan_joint");
+    joint_names.push_back(joint_prefix + "shoulder_lift_joint");
+    joint_names.push_back(joint_prefix + "elbow_joint");
+    joint_names.push_back(joint_prefix + "wrist_1_joint");
+    joint_names.push_back(joint_prefix + "wrist_2_joint");
+    joint_names.push_back(joint_prefix + "wrist_3_joint");
+    robot_.setJointNames(joint_names);
 
-        std::string joint_prefix = "";
-        std::vector<std::string> joint_names;
-        char buf[256];
+    use_ros_control_ = false;
+    ros::param::get("~use_ros_control", use_ros_control_);
 
-        if (ros::param::get("~prefix", joint_prefix)) {
-            if (joint_prefix.length() > 0) {
-                sprintf(buf, "Setting prefix to %s", joint_prefix.c_str());
-                print_info(buf);
-            }
-        }
-        joint_names.push_back(joint_prefix + "shoulder_pan_joint");
-        joint_names.push_back(joint_prefix + "shoulder_lift_joint");
-        joint_names.push_back(joint_prefix + "elbow_joint");
-        joint_names.push_back(joint_prefix + "wrist_1_joint");
-        joint_names.push_back(joint_prefix + "wrist_2_joint");
-        joint_names.push_back(joint_prefix + "wrist_3_joint");
-        robot_.setJointNames(joint_names);
-
-        use_ros_control_ = false;
-        ros::param::get("~use_ros_control", use_ros_control_);
-
-        if (use_ros_control_) {
-            hardware_interface_.reset(
-                new ros_control_ur::UrHardwareInterface(nh_, &robot_));
-            controller_manager_.reset(
-                new controller_manager::ControllerManager(
-                    hardware_interface_.get(), nh_));
-            double max_vel_change = 0.12; // equivalent of an acceleration of 15 rad/sec^2
-            if (ros::param::get("~max_acceleration", max_vel_change)) {
-                max_vel_change = max_vel_change / 125;
-            }
-            sprintf(buf, "Max acceleration set to: %f [rad/sec²]",
-                max_vel_change * 125);
-            print_debug(buf);
-            hardware_interface_->setMaxVelChange(max_vel_change);
-        }
-        //Using a very high value in order to not limit execution of trajectories being sent from MoveIt!
-        max_velocity_ = 10.;
-        if (ros::param::get("~max_velocity", max_velocity_)) {
-            sprintf(buf, "Max velocity accepted by ur_driver: %f [rad/s]",
-                max_velocity_);
-            print_debug(buf);
-        }
-
-        //Bounds for SetPayload service
-        //Using a very conservative value as it should be set through the parameter server
-        double min_payload = 0.;
-        double max_payload = 1.;
-        if (ros::param::get("~min_payload", min_payload)) {
-            sprintf(buf, "Min payload set to: %f [kg]", min_payload);
-            print_debug(buf);
-        }
-        if (ros::param::get("~max_payload", max_payload)) {
-            sprintf(buf, "Max payload set to: %f [kg]", max_payload);
-            print_debug(buf);
-        }
-        robot_.setMinPayload(min_payload);
-        robot_.setMaxPayload(max_payload);
-        sprintf(buf, "Bounds for set_payload service calls: [%f, %f]",
-            min_payload, max_payload);
-        print_debug(buf);
-
-        double servoj_time = 0.008;
-        if (ros::param::get("~servoj_time", servoj_time)) {
-            sprintf(buf, "Servoj_time set to: %f [sec]", servoj_time);
-            print_debug(buf);
-        }
-        robot_.setServojTime(servoj_time);
-
-        double servoj_lookahead_time = 0.03;
-        if (ros::param::get("~servoj_lookahead_time", servoj_lookahead_time)) {
-            sprintf(buf, "Servoj_lookahead_time set to: %f [sec]", servoj_lookahead_time);
-            print_debug(buf);
-        }
-        robot_.setServojLookahead(servoj_lookahead_time);
-
-        double servoj_gain = 300.;
-        if (ros::param::get("~servoj_gain", servoj_gain)) {
-            sprintf(buf, "Servoj_gain set to: %f [sec]", servoj_gain);
-            print_debug(buf);
-        }
-        robot_.setServojGain(servoj_gain);
-
-        //Base and tool frames
-        base_frame_ = joint_prefix + "base_link";
-        tool_frame_ = joint_prefix + "tool0_controller";
-        if (ros::param::get("~base_frame", base_frame_)) {
-            sprintf(buf, "Base frame set to: %s", base_frame_.c_str());
-            print_debug(buf);
-        }
-        if (ros::param::get("~tool_frame", tool_frame_)) {
-            sprintf(buf, "Tool frame set to: %s", tool_frame_.c_str());
-            print_debug(buf);
-        }
-
-        if (robot_.start()) {
-            if (use_ros_control_) {
-                ros_control_thread_ = new std::thread(
-                    boost::bind(&RosWrapper::rosControlLoop, this));
-                print_debug(
-                    "The control thread for this driver has been started");
-            } else {
-                //start actionserver
-                has_goal_ = false;
-                as_.start();
-
-                //subscribe to the data topic of interest
-                rt_publish_thread_ = new std::thread(
-                    boost::bind(&RosWrapper::publishRTMsg, this));
-                print_debug(
-                    "The action server for this driver has been started");
-            }
-            mb_publish_thread_ = new std::thread(
-                boost::bind(&RosWrapper::publishMbMsg, this));
-            speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1,
-                &RosWrapper::speedInterface, this);
-            urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1,
-                &RosWrapper::urscriptInterface, this);
-
-            io_srv_ = nh_.advertiseService("ur_driver/set_io",
-                &RosWrapper::setIO, this);
-            payload_srv_ = nh_.advertiseService("ur_driver/set_payload",
-                &RosWrapper::setPayload, this);
-        }
+    if (use_ros_control_)
+    {
+      hardware_interface_.reset(new ros_control_ur::UrHardwareInterface(nh_, &robot_));
+      controller_manager_.reset(new controller_manager::ControllerManager(hardware_interface_.get(), nh_));
+      double max_vel_change = 0.12;  // equivalent of an acceleration of 15 rad/sec^2
+      if (ros::param::get("~max_acceleration", max_vel_change))
+      {
+        max_vel_change = max_vel_change / 125;
+      }
+      sprintf(buf, "Max acceleration set to: %f [rad/sec²]", max_vel_change * 125);
+      print_debug(buf);
+      hardware_interface_->setMaxVelChange(max_vel_change);
+    }
+    // Using a very high value in order to not limit execution of trajectories being sent from MoveIt!
+    max_velocity_ = 10.;
+    if (ros::param::get("~max_velocity", max_velocity_))
+    {
+      sprintf(buf, "Max velocity accepted by ur_driver: %f [rad/s]", max_velocity_);
+      print_debug(buf);
     }
 
-    void halt()
+    // Bounds for SetPayload service
+    // Using a very conservative value as it should be set through the parameter server
+    double min_payload = 0.;
+    double max_payload = 1.;
+    if (ros::param::get("~min_payload", min_payload))
     {
-        robot_.halt();
-        rt_publish_thread_->join();
+      sprintf(buf, "Min payload set to: %f [kg]", min_payload);
+      print_debug(buf);
     }
+    if (ros::param::get("~max_payload", max_payload))
+    {
+      sprintf(buf, "Max payload set to: %f [kg]", max_payload);
+      print_debug(buf);
+    }
+    robot_.setMinPayload(min_payload);
+    robot_.setMaxPayload(max_payload);
+    sprintf(buf, "Bounds for set_payload service calls: [%f, %f]", min_payload, max_payload);
+    print_debug(buf);
+
+    double servoj_time = 0.008;
+    if (ros::param::get("~servoj_time", servoj_time))
+    {
+      sprintf(buf, "Servoj_time set to: %f [sec]", servoj_time);
+      print_debug(buf);
+    }
+    robot_.setServojTime(servoj_time);
+
+    double servoj_lookahead_time = 0.03;
+    if (ros::param::get("~servoj_lookahead_time", servoj_lookahead_time))
+    {
+      sprintf(buf, "Servoj_lookahead_time set to: %f [sec]", servoj_lookahead_time);
+      print_debug(buf);
+    }
+    robot_.setServojLookahead(servoj_lookahead_time);
+
+    double servoj_gain = 300.;
+    if (ros::param::get("~servoj_gain", servoj_gain))
+    {
+      sprintf(buf, "Servoj_gain set to: %f [sec]", servoj_gain);
+      print_debug(buf);
+    }
+    robot_.setServojGain(servoj_gain);
+
+    // Base and tool frames
+    base_frame_ = joint_prefix + "base_link";
+    tool_frame_ = joint_prefix + "tool0_controller";
+    if (ros::param::get("~base_frame", base_frame_))
+    {
+      sprintf(buf, "Base frame set to: %s", base_frame_.c_str());
+      print_debug(buf);
+    }
+    if (ros::param::get("~tool_frame", tool_frame_))
+    {
+      sprintf(buf, "Tool frame set to: %s", tool_frame_.c_str());
+      print_debug(buf);
+    }
+
+    if (robot_.start())
+    {
+      if (use_ros_control_)
+      {
+        ros_control_thread_ = new std::thread(boost::bind(&RosWrapper::rosControlLoop, this));
+        print_debug("The control thread for this driver has been started");
+      }
+      else
+      {
+        // start actionserver
+        has_goal_ = false;
+        as_.start();
+
+        // subscribe to the data topic of interest
+        rt_publish_thread_ = new std::thread(boost::bind(&RosWrapper::publishRTMsg, this));
+        print_debug("The action server for this driver has been started");
+      }
+      mb_publish_thread_ = new std::thread(boost::bind(&RosWrapper::publishMbMsg, this));
+      speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1, &RosWrapper::speedInterface, this);
+      urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, &RosWrapper::urscriptInterface, this);
+
+      io_srv_ = nh_.advertiseService("ur_driver/set_io", &RosWrapper::setIO, this);
+      payload_srv_ = nh_.advertiseService("ur_driver/set_payload", &RosWrapper::setPayload, this);
+    }
+  }
+
+  void halt()
+  {
+    robot_.halt();
+    rt_publish_thread_->join();
+  }
 
 private:
-    void trajThread(std::vector<double> timestamps,
-        std::vector<std::vector<double> > positions,
-        std::vector<std::vector<double> > velocities)
+  void trajThread(std::vector<double> timestamps, std::vector<std::vector<double>> positions,
+                  std::vector<std::vector<double>> velocities)
+  {
+    robot_.doTraj(timestamps, positions, velocities);
+    if (has_goal_)
     {
-
-        robot_.doTraj(timestamps, positions, velocities);
-        if (has_goal_) {
-            result_.error_code = result_.SUCCESSFUL;
-            goal_handle_.setSucceeded(result_);
-            has_goal_ = false;
-        }
+      result_.error_code = result_.SUCCESSFUL;
+      goal_handle_.setSucceeded(result_);
+      has_goal_ = false;
     }
-    void goalCB(
-        actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> gh)
+  }
+  void goalCB(actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> gh)
+  {
+    std::string buf;
+    print_info("on_goal");
+    if (!robot_.sec_interface_->robot_state_->isReady())
     {
-        std::string buf;
-        print_info("on_goal");
-        if (!robot_.sec_interface_->robot_state_->isReady()) {
-            result_.error_code = -100; //nothing is defined for this...?
+      result_.error_code = -100;  // nothing is defined for this...?
 
-            if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) {
-                result_.error_string = "Cannot accept new trajectories: Robot arm is not powered on";
-                gh.setRejected(result_, result_.error_string);
-                print_error(result_.error_string);
-                return;
-            }
-            if (!robot_.sec_interface_->robot_state_->isRealRobotEnabled()) {
-                result_.error_string = "Cannot accept new trajectories: Robot is not enabled";
-                gh.setRejected(result_, result_.error_string);
-                print_error(result_.error_string);
-                return;
-            }
-            result_.error_string = "Cannot accept new trajectories. (Debug: Robot mode is "
-                + std::to_string(
-                      robot_.sec_interface_->robot_state_->getRobotMode())
-                + ")";
-            gh.setRejected(result_, result_.error_string);
-            print_error(result_.error_string);
-            return;
-        }
-        if (robot_.sec_interface_->robot_state_->isEmergencyStopped()) {
-            result_.error_code = -100; //nothing is defined for this...?
-            result_.error_string = "Cannot accept new trajectories: Robot is emergency stopped";
-            gh.setRejected(result_, result_.error_string);
-            print_error(result_.error_string);
-            return;
-        }
-        if (robot_.sec_interface_->robot_state_->isProtectiveStopped()) {
-            result_.error_code = -100; //nothing is defined for this...?
-            result_.error_string = "Cannot accept new trajectories: Robot is protective stopped";
-            gh.setRejected(result_, result_.error_string);
-            print_error(result_.error_string);
-            return;
-        }
-
-        actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *gh.getGoal(); //make a copy that we can modify
-        if (has_goal_) {
-            print_warning(
-                "Received new goal while still executing previous trajectory. Canceling previous trajectory");
-            has_goal_ = false;
-            robot_.stopTraj();
-            result_.error_code = -100; //nothing is defined for this...?
-            result_.error_string = "Received another trajectory";
-            goal_handle_.setAborted(result_, result_.error_string);
-            std::this_thread::sleep_for(std::chrono::milliseconds(250));
-        }
-        goal_handle_ = gh;
-        if (!validateJointNames()) {
-            std::string outp_joint_names = "";
-            for (unsigned int i = 0; i < goal.trajectory.joint_names.size();
-                 i++) {
-                outp_joint_names += goal.trajectory.joint_names[i] + " ";
-            }
-            result_.error_code = result_.INVALID_JOINTS;
-            result_.error_string = "Received a goal with incorrect joint names: "
-                + outp_joint_names;
-            gh.setRejected(result_, result_.error_string);
-            print_error(result_.error_string);
-            return;
-        }
-        if (!has_positions()) {
-            result_.error_code = result_.INVALID_GOAL;
-            result_.error_string = "Received a goal without positions";
-            gh.setRejected(result_, result_.error_string);
-            print_error(result_.error_string);
-            return;
-        }
-
-        if (!has_velocities()) {
-            result_.error_code = result_.INVALID_GOAL;
-            result_.error_string = "Received a goal without velocities";
-            gh.setRejected(result_, result_.error_string);
-            print_error(result_.error_string);
-            return;
-        }
-
-        if (!traj_is_finite()) {
-            result_.error_string = "Received a goal with infinities or NaNs";
-            result_.error_code = result_.INVALID_GOAL;
-            gh.setRejected(result_, result_.error_string);
-            print_error(result_.error_string);
-            return;
-        }
-
-        if (!has_limited_velocities()) {
-            result_.error_code = result_.INVALID_GOAL;
-            result_.error_string = "Received a goal with velocities that are higher than "
-                + std::to_string(max_velocity_);
-            gh.setRejected(result_, result_.error_string);
-            print_error(result_.error_string);
-            return;
-        }
-
-        reorder_traj_joints(goal.trajectory);
-
-        if (!start_positions_match(goal.trajectory, 0.01)) {
-            result_.error_code = result_.INVALID_GOAL;
-            result_.error_string = "Goal start doesn't match current pose";
-            gh.setRejected(result_, result_.error_string);
-            print_error(result_.error_string);
-            return;
-        }
-
-        std::vector<double> timestamps;
-        std::vector<std::vector<double> > positions, velocities;
-        if (goal.trajectory.points[0].time_from_start.toSec() != 0.) {
-            print_warning(
-                "Trajectory's first point should be the current position, with time_from_start set to 0.0 - Inserting point in malformed trajectory");
-            timestamps.push_back(0.0);
-            positions.push_back(
-                robot_.rt_interface_->robot_state_->getQActual());
-            velocities.push_back(
-                robot_.rt_interface_->robot_state_->getQdActual());
-        }
-        for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
-            timestamps.push_back(
-                goal.trajectory.points[i].time_from_start.toSec());
-            positions.push_back(goal.trajectory.points[i].positions);
-            velocities.push_back(goal.trajectory.points[i].velocities);
-        }
-
-        goal_handle_.setAccepted();
-        has_goal_ = true;
-        std::thread(&RosWrapper::trajThread, this, timestamps, positions,
-            velocities)
-            .detach();
+      if (!robot_.sec_interface_->robot_state_->isPowerOnRobot())
+      {
+        result_.error_string = "Cannot accept new trajectories: Robot arm is not powered on";
+        gh.setRejected(result_, result_.error_string);
+        print_error(result_.error_string);
+        return;
+      }
+      if (!robot_.sec_interface_->robot_state_->isRealRobotEnabled())
+      {
+        result_.error_string = "Cannot accept new trajectories: Robot is not enabled";
+        gh.setRejected(result_, result_.error_string);
+        print_error(result_.error_string);
+        return;
+      }
+      result_.error_string = "Cannot accept new trajectories. (Debug: Robot mode is " +
+                             std::to_string(robot_.sec_interface_->robot_state_->getRobotMode()) + ")";
+      gh.setRejected(result_, result_.error_string);
+      print_error(result_.error_string);
+      return;
+    }
+    if (robot_.sec_interface_->robot_state_->isEmergencyStopped())
+    {
+      result_.error_code = -100;  // nothing is defined for this...?
+      result_.error_string = "Cannot accept new trajectories: Robot is emergency stopped";
+      gh.setRejected(result_, result_.error_string);
+      print_error(result_.error_string);
+      return;
+    }
+    if (robot_.sec_interface_->robot_state_->isProtectiveStopped())
+    {
+      result_.error_code = -100;  // nothing is defined for this...?
+      result_.error_string = "Cannot accept new trajectories: Robot is protective stopped";
+      gh.setRejected(result_, result_.error_string);
+      print_error(result_.error_string);
+      return;
     }
 
-    void cancelCB(
-        actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> gh)
+    actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
+        *gh.getGoal();  // make a copy that we can modify
+    if (has_goal_)
     {
-        // set the action state to preempted
-        print_info("on_cancel");
-        if (has_goal_) {
-            if (gh == goal_handle_) {
-                robot_.stopTraj();
-                has_goal_ = false;
-            }
-        }
-        result_.error_code = -100; //nothing is defined for this...?
-        result_.error_string = "Goal cancelled by client";
-        gh.setCanceled(result_);
+      print_warning("Received new goal while still executing previous trajectory. Canceling previous trajectory");
+      has_goal_ = false;
+      robot_.stopTraj();
+      result_.error_code = -100;  // nothing is defined for this...?
+      result_.error_string = "Received another trajectory";
+      goal_handle_.setAborted(result_, result_.error_string);
+      std::this_thread::sleep_for(std::chrono::milliseconds(250));
+    }
+    goal_handle_ = gh;
+    if (!validateJointNames())
+    {
+      std::string outp_joint_names = "";
+      for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++)
+      {
+        outp_joint_names += goal.trajectory.joint_names[i] + " ";
+      }
+      result_.error_code = result_.INVALID_JOINTS;
+      result_.error_string = "Received a goal with incorrect joint names: " + outp_joint_names;
+      gh.setRejected(result_, result_.error_string);
+      print_error(result_.error_string);
+      return;
+    }
+    if (!has_positions())
+    {
+      result_.error_code = result_.INVALID_GOAL;
+      result_.error_string = "Received a goal without positions";
+      gh.setRejected(result_, result_.error_string);
+      print_error(result_.error_string);
+      return;
     }
 
-    bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp)
+    if (!has_velocities())
     {
-        resp.success = true;
-        //if (req.fun == ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT) {
-        if (req.fun == 1) {
-            robot_.setDigitalOut(req.pin, req.state > 0.0 ? true : false);
-        } else if (req.fun == 2) {
-            //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_FLAG) {
-            robot_.setFlag(req.pin, req.state > 0.0 ? true : false);
-            //According to urdriver.py, set_flag will fail if called to rapidly in succession
-            ros::Duration(io_flag_delay_).sleep();
-        } else if (req.fun == 3) {
-            //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_ANALOG_OUT) {
-            robot_.setAnalogOut(req.pin, req.state);
-        } else if (req.fun == 4) {
-            //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_TOOL_VOLTAGE) {
-            robot_.setToolVoltage((int)req.state);
-        } else {
-            resp.success = false;
-        }
-        return resp.success;
+      result_.error_code = result_.INVALID_GOAL;
+      result_.error_string = "Received a goal without velocities";
+      gh.setRejected(result_, result_.error_string);
+      print_error(result_.error_string);
+      return;
     }
 
-    bool setPayload(ur_msgs::SetPayloadRequest& req,
-        ur_msgs::SetPayloadResponse& resp)
+    if (!traj_is_finite())
     {
-        if (robot_.setPayload(req.payload))
-            resp.success = true;
+      result_.error_string = "Received a goal with infinities or NaNs";
+      result_.error_code = result_.INVALID_GOAL;
+      gh.setRejected(result_, result_.error_string);
+      print_error(result_.error_string);
+      return;
+    }
+
+    if (!has_limited_velocities())
+    {
+      result_.error_code = result_.INVALID_GOAL;
+      result_.error_string = "Received a goal with velocities that are higher than " + std::to_string(max_velocity_);
+      gh.setRejected(result_, result_.error_string);
+      print_error(result_.error_string);
+      return;
+    }
+
+    reorder_traj_joints(goal.trajectory);
+
+    if (!start_positions_match(goal.trajectory, 0.01))
+    {
+      result_.error_code = result_.INVALID_GOAL;
+      result_.error_string = "Goal start doesn't match current pose";
+      gh.setRejected(result_, result_.error_string);
+      print_error(result_.error_string);
+      return;
+    }
+
+    std::vector<double> timestamps;
+    std::vector<std::vector<double>> positions, velocities;
+    if (goal.trajectory.points[0].time_from_start.toSec() != 0.)
+    {
+      print_warning("Trajectory's first point should be the current position, with time_from_start set to 0.0 - "
+                    "Inserting point in malformed trajectory");
+      timestamps.push_back(0.0);
+      positions.push_back(robot_.rt_interface_->robot_state_->getQActual());
+      velocities.push_back(robot_.rt_interface_->robot_state_->getQdActual());
+    }
+    for (unsigned int i = 0; i < goal.trajectory.points.size(); i++)
+    {
+      timestamps.push_back(goal.trajectory.points[i].time_from_start.toSec());
+      positions.push_back(goal.trajectory.points[i].positions);
+      velocities.push_back(goal.trajectory.points[i].velocities);
+    }
+
+    goal_handle_.setAccepted();
+    has_goal_ = true;
+    std::thread(&RosWrapper::trajThread, this, timestamps, positions, velocities).detach();
+  }
+
+  void cancelCB(actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> gh)
+  {
+    // set the action state to preempted
+    print_info("on_cancel");
+    if (has_goal_)
+    {
+      if (gh == goal_handle_)
+      {
+        robot_.stopTraj();
+        has_goal_ = false;
+      }
+    }
+    result_.error_code = -100;  // nothing is defined for this...?
+    result_.error_string = "Goal cancelled by client";
+    gh.setCanceled(result_);
+  }
+
+  bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp)
+  {
+    resp.success = true;
+    // if (req.fun == ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT) {
+    if (req.fun == 1)
+    {
+      robot_.setDigitalOut(req.pin, req.state > 0.0 ? true : false);
+    }
+    else if (req.fun == 2)
+    {
+      //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_FLAG) {
+      robot_.setFlag(req.pin, req.state > 0.0 ? true : false);
+      // According to urdriver.py, set_flag will fail if called to rapidly in succession
+      ros::Duration(io_flag_delay_).sleep();
+    }
+    else if (req.fun == 3)
+    {
+      //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_ANALOG_OUT) {
+      robot_.setAnalogOut(req.pin, req.state);
+    }
+    else if (req.fun == 4)
+    {
+      //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_TOOL_VOLTAGE) {
+      robot_.setToolVoltage((int)req.state);
+    }
+    else
+    {
+      resp.success = false;
+    }
+    return resp.success;
+  }
+
+  bool setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::SetPayloadResponse& resp)
+  {
+    if (robot_.setPayload(req.payload))
+      resp.success = true;
+    else
+      resp.success = true;
+    return resp.success;
+  }
+
+  bool validateJointNames()
+  {
+    std::vector<std::string> actual_joint_names = robot_.getJointNames();
+    actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_.getGoal();
+    if (goal.trajectory.joint_names.size() != actual_joint_names.size())
+      return false;
+
+    for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++)
+    {
+      unsigned int j;
+      for (j = 0; j < actual_joint_names.size(); j++)
+      {
+        if (goal.trajectory.joint_names[i] == actual_joint_names[j])
+          break;
+      }
+      if (goal.trajectory.joint_names[i] == actual_joint_names[j])
+      {
+        actual_joint_names.erase(actual_joint_names.begin() + j);
+      }
+      else
+      {
+        return false;
+      }
+    }
+
+    return true;
+  }
+
+  void reorder_traj_joints(trajectory_msgs::JointTrajectory& traj)
+  {
+    /* Reorders trajectory - destructive */
+    std::vector<std::string> actual_joint_names = robot_.getJointNames();
+    std::vector<unsigned int> mapping;
+    mapping.resize(actual_joint_names.size(), actual_joint_names.size());
+    for (unsigned int i = 0; i < traj.joint_names.size(); i++)
+    {
+      for (unsigned int j = 0; j < actual_joint_names.size(); j++)
+      {
+        if (traj.joint_names[i] == actual_joint_names[j])
+          mapping[j] = i;
+      }
+    }
+    traj.joint_names = actual_joint_names;
+    std::vector<trajectory_msgs::JointTrajectoryPoint> new_traj;
+    for (unsigned int i = 0; i < traj.points.size(); i++)
+    {
+      trajectory_msgs::JointTrajectoryPoint new_point;
+      for (unsigned int j = 0; j < traj.points[i].positions.size(); j++)
+      {
+        new_point.positions.push_back(traj.points[i].positions[mapping[j]]);
+        new_point.velocities.push_back(traj.points[i].velocities[mapping[j]]);
+        if (traj.points[i].accelerations.size() != 0)
+          new_point.accelerations.push_back(traj.points[i].accelerations[mapping[j]]);
+      }
+      new_point.time_from_start = traj.points[i].time_from_start;
+      new_traj.push_back(new_point);
+    }
+    traj.points = new_traj;
+  }
+
+  bool has_velocities()
+  {
+    actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_.getGoal();
+    for (unsigned int i = 0; i < goal.trajectory.points.size(); i++)
+    {
+      if (goal.trajectory.points[i].positions.size() != goal.trajectory.points[i].velocities.size())
+        return false;
+    }
+    return true;
+  }
+
+  bool has_positions()
+  {
+    actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_.getGoal();
+    if (goal.trajectory.points.size() == 0)
+      return false;
+    for (unsigned int i = 0; i < goal.trajectory.points.size(); i++)
+    {
+      if (goal.trajectory.points[i].positions.size() != goal.trajectory.joint_names.size())
+        return false;
+    }
+    return true;
+  }
+
+  bool start_positions_match(const trajectory_msgs::JointTrajectory& traj, double eps)
+  {
+    for (unsigned int i = 0; i < traj.points[0].positions.size(); i++)
+    {
+      std::vector<double> qActual = robot_.rt_interface_->robot_state_->getQActual();
+      if (fabs(traj.points[0].positions[i] - qActual[i]) > eps)
+      {
+        return false;
+      }
+    }
+    return true;
+  }
+
+  bool has_limited_velocities()
+  {
+    actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_.getGoal();
+    for (unsigned int i = 0; i < goal.trajectory.points.size(); i++)
+    {
+      for (unsigned int j = 0; j < goal.trajectory.points[i].velocities.size(); j++)
+      {
+        if (fabs(goal.trajectory.points[i].velocities[j]) > max_velocity_)
+          return false;
+      }
+    }
+    return true;
+  }
+
+  bool traj_is_finite()
+  {
+    actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_.getGoal();
+    for (unsigned int i = 0; i < goal.trajectory.points.size(); i++)
+    {
+      for (unsigned int j = 0; j < goal.trajectory.points[i].velocities.size(); j++)
+      {
+        if (!std::isfinite(goal.trajectory.points[i].positions[j]))
+          return false;
+        if (!std::isfinite(goal.trajectory.points[i].velocities[j]))
+          return false;
+      }
+    }
+    return true;
+  }
+
+  void speedInterface(const trajectory_msgs::JointTrajectory::Ptr& msg)
+  {
+    if (msg->points[0].velocities.size() == 6)
+    {
+      double acc = 100;
+      if (msg->points[0].accelerations.size() > 0)
+        acc = *std::max_element(msg->points[0].accelerations.begin(), msg->points[0].accelerations.end());
+      robot_.setSpeed(msg->points[0].velocities[0], msg->points[0].velocities[1], msg->points[0].velocities[2],
+                      msg->points[0].velocities[3], msg->points[0].velocities[4], msg->points[0].velocities[5], acc);
+    }
+  }
+  void urscriptInterface(const std_msgs::String::ConstPtr& msg)
+  {
+    robot_.rt_interface_->addCommandToQueue(msg->data);
+  }
+
+  void rosControlLoop()
+  {
+    ros::Duration elapsed_time;
+    struct timespec last_time, current_time;
+    static const double BILLION = 1000000000.0;
+
+    realtime_tools::RealtimePublisher<tf::tfMessage> tf_pub(nh_, "/tf", 1);
+    geometry_msgs::TransformStamped tool_transform;
+    tool_transform.header.frame_id = base_frame_;
+    tool_transform.child_frame_id = tool_frame_;
+    tf_pub.msg_.transforms.push_back(tool_transform);
+
+    realtime_tools::RealtimePublisher<geometry_msgs::TwistStamped> tool_vel_pub(nh_, "tool_velocity", 1);
+    tool_vel_pub.msg_.header.frame_id = base_frame_;
+
+    clock_gettime(CLOCK_MONOTONIC, &last_time);
+    while (ros::ok())
+    {
+      std::mutex msg_lock;  // The values are locked for reading in the class, so just use a dummy mutex
+      std::unique_lock<std::mutex> locker(msg_lock);
+      while (!robot_.rt_interface_->robot_state_->getControllerUpdated())
+      {
+        rt_msg_cond_.wait(locker);
+      }
+      // Input
+      hardware_interface_->read();
+      robot_.rt_interface_->robot_state_->setControllerUpdated();
+
+      // Control
+      clock_gettime(CLOCK_MONOTONIC, &current_time);
+      elapsed_time =
+          ros::Duration(current_time.tv_sec - last_time.tv_sec + (current_time.tv_nsec - last_time.tv_nsec) / BILLION);
+      ros::Time ros_time = ros::Time::now();
+      controller_manager_->update(ros_time, elapsed_time);
+      last_time = current_time;
+
+      // Output
+      hardware_interface_->write();
+
+      // Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation
+      // vector representation of the tool orientation
+      std::vector<double> tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual();
+
+      // Compute rotation angle
+      double rx = tool_vector_actual[3];
+      double ry = tool_vector_actual[4];
+      double rz = tool_vector_actual[5];
+      double angle = std::sqrt(std::pow(rx, 2) + std::pow(ry, 2) + std::pow(rz, 2));
+
+      // Broadcast transform
+      if (tf_pub.trylock())
+      {
+        tf_pub.msg_.transforms[0].header.stamp = ros_time;
+        if (angle < 1e-16)
+        {
+          tf_pub.msg_.transforms[0].transform.rotation.x = 0;
+          tf_pub.msg_.transforms[0].transform.rotation.y = 0;
+          tf_pub.msg_.transforms[0].transform.rotation.z = 0;
+          tf_pub.msg_.transforms[0].transform.rotation.w = 1;
+        }
         else
-            resp.success = true;
-        return resp.success;
-    }
-
-    bool validateJointNames()
-    {
-        std::vector<std::string> actual_joint_names = robot_.getJointNames();
-        actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_.getGoal();
-        if (goal.trajectory.joint_names.size() != actual_joint_names.size())
-            return false;
-
-        for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) {
-            unsigned int j;
-            for (j = 0; j < actual_joint_names.size(); j++) {
-                if (goal.trajectory.joint_names[i] == actual_joint_names[j])
-                    break;
-            }
-            if (goal.trajectory.joint_names[i] == actual_joint_names[j]) {
-                actual_joint_names.erase(actual_joint_names.begin() + j);
-            } else {
-                return false;
-            }
+        {
+          tf_pub.msg_.transforms[0].transform.rotation.x = (rx / angle) * std::sin(angle * 0.5);
+          tf_pub.msg_.transforms[0].transform.rotation.y = (ry / angle) * std::sin(angle * 0.5);
+          tf_pub.msg_.transforms[0].transform.rotation.z = (rz / angle) * std::sin(angle * 0.5);
+          tf_pub.msg_.transforms[0].transform.rotation.w = std::cos(angle * 0.5);
         }
+        tf_pub.msg_.transforms[0].transform.translation.x = tool_vector_actual[0];
+        tf_pub.msg_.transforms[0].transform.translation.y = tool_vector_actual[1];
+        tf_pub.msg_.transforms[0].transform.translation.z = tool_vector_actual[2];
 
-        return true;
+        tf_pub.unlockAndPublish();
+      }
+
+      // Publish tool velocity
+      std::vector<double> tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual();
+
+      if (tool_vel_pub.trylock())
+      {
+        tool_vel_pub.msg_.header.stamp = ros_time;
+        tool_vel_pub.msg_.twist.linear.x = tcp_speed[0];
+        tool_vel_pub.msg_.twist.linear.y = tcp_speed[1];
+        tool_vel_pub.msg_.twist.linear.z = tcp_speed[2];
+        tool_vel_pub.msg_.twist.angular.x = tcp_speed[3];
+        tool_vel_pub.msg_.twist.angular.y = tcp_speed[4];
+        tool_vel_pub.msg_.twist.angular.z = tcp_speed[5];
+
+        tool_vel_pub.unlockAndPublish();
+      }
     }
+  }
 
-    void reorder_traj_joints(trajectory_msgs::JointTrajectory& traj)
+  void publishRTMsg()
+  {
+    ros::Publisher joint_pub = nh_.advertise<sensor_msgs::JointState>("joint_states", 1);
+    ros::Publisher wrench_pub = nh_.advertise<geometry_msgs::WrenchStamped>("wrench", 1);
+    ros::Publisher tool_vel_pub = nh_.advertise<geometry_msgs::TwistStamped>("tool_velocity", 1);
+    static tf::TransformBroadcaster br;
+    while (ros::ok())
     {
-        /* Reorders trajectory - destructive */
-        std::vector<std::string> actual_joint_names = robot_.getJointNames();
-        std::vector<unsigned int> mapping;
-        mapping.resize(actual_joint_names.size(), actual_joint_names.size());
-        for (unsigned int i = 0; i < traj.joint_names.size(); i++) {
-            for (unsigned int j = 0; j < actual_joint_names.size(); j++) {
-                if (traj.joint_names[i] == actual_joint_names[j])
-                    mapping[j] = i;
-            }
-        }
-        traj.joint_names = actual_joint_names;
-        std::vector<trajectory_msgs::JointTrajectoryPoint> new_traj;
-        for (unsigned int i = 0; i < traj.points.size(); i++) {
-            trajectory_msgs::JointTrajectoryPoint new_point;
-            for (unsigned int j = 0; j < traj.points[i].positions.size(); j++) {
-                new_point.positions.push_back(
-                    traj.points[i].positions[mapping[j]]);
-                new_point.velocities.push_back(
-                    traj.points[i].velocities[mapping[j]]);
-                if (traj.points[i].accelerations.size() != 0)
-                    new_point.accelerations.push_back(
-                        traj.points[i].accelerations[mapping[j]]);
-            }
-            new_point.time_from_start = traj.points[i].time_from_start;
-            new_traj.push_back(new_point);
-        }
-        traj.points = new_traj;
-    }
+      sensor_msgs::JointState joint_msg;
+      joint_msg.name = robot_.getJointNames();
+      geometry_msgs::WrenchStamped wrench_msg;
+      geometry_msgs::PoseStamped tool_pose_msg;
+      std::mutex msg_lock;  // The values are locked for reading in the class, so just use a dummy mutex
+      std::unique_lock<std::mutex> locker(msg_lock);
+      while (!robot_.rt_interface_->robot_state_->getDataPublished())
+      {
+        rt_msg_cond_.wait(locker);
+      }
+      joint_msg.header.stamp = ros::Time::now();
+      joint_msg.position = robot_.rt_interface_->robot_state_->getQActual();
+      for (unsigned int i = 0; i < joint_msg.position.size(); i++)
+      {
+        joint_msg.position[i] += joint_offsets_[i];
+      }
+      joint_msg.velocity = robot_.rt_interface_->robot_state_->getQdActual();
+      joint_msg.effort = robot_.rt_interface_->robot_state_->getIActual();
+      joint_pub.publish(joint_msg);
+      std::vector<double> tcp_force = robot_.rt_interface_->robot_state_->getTcpForce();
+      wrench_msg.header.stamp = joint_msg.header.stamp;
+      wrench_msg.wrench.force.x = tcp_force[0];
+      wrench_msg.wrench.force.y = tcp_force[1];
+      wrench_msg.wrench.force.z = tcp_force[2];
+      wrench_msg.wrench.torque.x = tcp_force[3];
+      wrench_msg.wrench.torque.y = tcp_force[4];
+      wrench_msg.wrench.torque.z = tcp_force[5];
+      wrench_pub.publish(wrench_msg);
 
-    bool has_velocities()
+      // Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation
+      // vector representation of the tool orientation
+      std::vector<double> tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual();
+
+      // Create quaternion
+      tf::Quaternion quat;
+      double rx = tool_vector_actual[3];
+      double ry = tool_vector_actual[4];
+      double rz = tool_vector_actual[5];
+      double angle = std::sqrt(std::pow(rx, 2) + std::pow(ry, 2) + std::pow(rz, 2));
+      if (angle < 1e-16)
+      {
+        quat.setValue(0, 0, 0, 1);
+      }
+      else
+      {
+        quat.setRotation(tf::Vector3(rx / angle, ry / angle, rz / angle), angle);
+      }
+
+      // Create and broadcast transform
+      tf::Transform transform;
+      transform.setOrigin(tf::Vector3(tool_vector_actual[0], tool_vector_actual[1], tool_vector_actual[2]));
+      transform.setRotation(quat);
+      br.sendTransform(tf::StampedTransform(transform, joint_msg.header.stamp, base_frame_, tool_frame_));
+
+      // Publish tool velocity
+      std::vector<double> tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual();
+      geometry_msgs::TwistStamped tool_twist;
+      tool_twist.header.frame_id = base_frame_;
+      tool_twist.header.stamp = joint_msg.header.stamp;
+      tool_twist.twist.linear.x = tcp_speed[0];
+      tool_twist.twist.linear.y = tcp_speed[1];
+      tool_twist.twist.linear.z = tcp_speed[2];
+      tool_twist.twist.angular.x = tcp_speed[3];
+      tool_twist.twist.angular.y = tcp_speed[4];
+      tool_twist.twist.angular.z = tcp_speed[5];
+      tool_vel_pub.publish(tool_twist);
+
+      robot_.rt_interface_->robot_state_->setDataPublished();
+    }
+  }
+
+  void publishMbMsg()
+  {
+    bool warned = false;
+    ros::Publisher io_pub = nh_.advertise<ur_msgs::IOStates>("ur_driver/io_states", 1);
+
+    while (ros::ok())
     {
-        actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_.getGoal();
-        for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
-            if (goal.trajectory.points[i].positions.size()
-                != goal.trajectory.points[i].velocities.size())
-                return false;
+      ur_msgs::IOStates io_msg;
+      std::mutex msg_lock;  // The values are locked for reading in the class, so just use a dummy mutex
+      std::unique_lock<std::mutex> locker(msg_lock);
+      while (!robot_.sec_interface_->robot_state_->getNewDataAvailable())
+      {
+        msg_cond_.wait(locker);
+      }
+      int i_max = 10;
+      if (robot_.sec_interface_->robot_state_->getVersion() > 3.0)
+        i_max = 18;  // From version 3.0, there are up to 18 inputs and outputs
+      for (unsigned int i = 0; i < i_max; i++)
+      {
+        ur_msgs::Digital digi;
+        digi.pin = i;
+        digi.state = ((robot_.sec_interface_->robot_state_->getDigitalInputBits() & (1 << i)) >> i);
+        io_msg.digital_in_states.push_back(digi);
+        digi.state = ((robot_.sec_interface_->robot_state_->getDigitalOutputBits() & (1 << i)) >> i);
+        io_msg.digital_out_states.push_back(digi);
+      }
+      ur_msgs::Analog ana;
+      ana.pin = 0;
+      ana.state = robot_.sec_interface_->robot_state_->getAnalogInput0();
+      io_msg.analog_in_states.push_back(ana);
+      ana.pin = 1;
+      ana.state = robot_.sec_interface_->robot_state_->getAnalogInput1();
+      io_msg.analog_in_states.push_back(ana);
+
+      ana.pin = 0;
+      ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput0();
+      io_msg.analog_out_states.push_back(ana);
+      ana.pin = 1;
+      ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput1();
+      io_msg.analog_out_states.push_back(ana);
+      io_pub.publish(io_msg);
+
+      if (robot_.sec_interface_->robot_state_->isEmergencyStopped() or
+          robot_.sec_interface_->robot_state_->isProtectiveStopped())
+      {
+        if (robot_.sec_interface_->robot_state_->isEmergencyStopped() and !warned)
+        {
+          print_error("Emergency stop pressed!");
         }
-        return true;
-    }
-
-    bool has_positions()
-    {
-        actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_.getGoal();
-        if (goal.trajectory.points.size() == 0)
-            return false;
-        for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
-            if (goal.trajectory.points[i].positions.size()
-                != goal.trajectory.joint_names.size())
-                return false;
+        else if (robot_.sec_interface_->robot_state_->isProtectiveStopped() and !warned)
+        {
+          print_error("Robot is protective stopped!");
         }
-        return true;
-    }
-
-    bool start_positions_match(const trajectory_msgs::JointTrajectory& traj, double eps)
-    {
-        for (unsigned int i = 0; i < traj.points[0].positions.size(); i++) {
-            std::vector<double> qActual = robot_.rt_interface_->robot_state_->getQActual();
-            if (fabs(traj.points[0].positions[i] - qActual[i]) > eps) {
-                return false;
-            }
-        }
-        return true;
-    }
-
-    bool has_limited_velocities()
-    {
-        actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_.getGoal();
-        for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
-            for (unsigned int j = 0;
-                 j < goal.trajectory.points[i].velocities.size(); j++) {
-                if (fabs(goal.trajectory.points[i].velocities[j])
-                    > max_velocity_)
-                    return false;
-            }
-        }
-        return true;
-    }
-
-    bool traj_is_finite()
-    {
-        actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_.getGoal();
-        for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
-            for (unsigned int j = 0;
-                 j < goal.trajectory.points[i].velocities.size(); j++) {
-                if (!std::isfinite(goal.trajectory.points[i].positions[j]))
-                    return false;
-                if (!std::isfinite(goal.trajectory.points[i].velocities[j]))
-                    return false;
-            }
-        }
-        return true;
-    }
-
-    void speedInterface(const trajectory_msgs::JointTrajectory::Ptr& msg)
-    {
-        if (msg->points[0].velocities.size() == 6) {
-            double acc = 100;
-            if (msg->points[0].accelerations.size() > 0)
-                acc = *std::max_element(msg->points[0].accelerations.begin(),
-                    msg->points[0].accelerations.end());
-            robot_.setSpeed(msg->points[0].velocities[0],
-                msg->points[0].velocities[1], msg->points[0].velocities[2],
-                msg->points[0].velocities[3], msg->points[0].velocities[4],
-                msg->points[0].velocities[5], acc);
-        }
-    }
-    void urscriptInterface(const std_msgs::String::ConstPtr& msg)
-    {
-
-        robot_.rt_interface_->addCommandToQueue(msg->data);
-    }
-
-    void rosControlLoop()
-    {
-        ros::Duration elapsed_time;
-        struct timespec last_time, current_time;
-        static const double BILLION = 1000000000.0;
-
-        realtime_tools::RealtimePublisher<tf::tfMessage> tf_pub(nh_, "/tf", 1);
-        geometry_msgs::TransformStamped tool_transform;
-        tool_transform.header.frame_id = base_frame_;
-        tool_transform.child_frame_id = tool_frame_;
-        tf_pub.msg_.transforms.push_back(tool_transform);
-
-        realtime_tools::RealtimePublisher<geometry_msgs::TwistStamped> tool_vel_pub(nh_, "tool_velocity", 1);
-        tool_vel_pub.msg_.header.frame_id = base_frame_;
-
-        clock_gettime(CLOCK_MONOTONIC, &last_time);
-        while (ros::ok()) {
-            std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex
-            std::unique_lock<std::mutex> locker(msg_lock);
-            while (!robot_.rt_interface_->robot_state_->getControllerUpdated()) {
-                rt_msg_cond_.wait(locker);
-            }
-            // Input
-            hardware_interface_->read();
-            robot_.rt_interface_->robot_state_->setControllerUpdated();
-
-            // Control
-            clock_gettime(CLOCK_MONOTONIC, &current_time);
-            elapsed_time = ros::Duration(current_time.tv_sec - last_time.tv_sec + (current_time.tv_nsec - last_time.tv_nsec) / BILLION);
-            ros::Time ros_time = ros::Time::now();
-            controller_manager_->update(ros_time, elapsed_time);
-            last_time = current_time;
-
-            // Output
-            hardware_interface_->write();
-
-            // Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation
-            std::vector<double> tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual();
-
-            // Compute rotation angle
-            double rx = tool_vector_actual[3];
-            double ry = tool_vector_actual[4];
-            double rz = tool_vector_actual[5];
-            double angle = std::sqrt(std::pow(rx, 2) + std::pow(ry, 2) + std::pow(rz, 2));
-
-            // Broadcast transform
-            if (tf_pub.trylock()) {
-                tf_pub.msg_.transforms[0].header.stamp = ros_time;
-                if (angle < 1e-16) {
-                    tf_pub.msg_.transforms[0].transform.rotation.x = 0;
-                    tf_pub.msg_.transforms[0].transform.rotation.y = 0;
-                    tf_pub.msg_.transforms[0].transform.rotation.z = 0;
-                    tf_pub.msg_.transforms[0].transform.rotation.w = 1;
-                } else {
-                    tf_pub.msg_.transforms[0].transform.rotation.x = (rx / angle) * std::sin(angle * 0.5);
-                    tf_pub.msg_.transforms[0].transform.rotation.y = (ry / angle) * std::sin(angle * 0.5);
-                    tf_pub.msg_.transforms[0].transform.rotation.z = (rz / angle) * std::sin(angle * 0.5);
-                    tf_pub.msg_.transforms[0].transform.rotation.w = std::cos(angle * 0.5);
-                }
-                tf_pub.msg_.transforms[0].transform.translation.x = tool_vector_actual[0];
-                tf_pub.msg_.transforms[0].transform.translation.y = tool_vector_actual[1];
-                tf_pub.msg_.transforms[0].transform.translation.z = tool_vector_actual[2];
-
-                tf_pub.unlockAndPublish();
-            }
-
-            //Publish tool velocity
-            std::vector<double> tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual();
-
-            if (tool_vel_pub.trylock()) {
-                tool_vel_pub.msg_.header.stamp = ros_time;
-                tool_vel_pub.msg_.twist.linear.x = tcp_speed[0];
-                tool_vel_pub.msg_.twist.linear.y = tcp_speed[1];
-                tool_vel_pub.msg_.twist.linear.z = tcp_speed[2];
-                tool_vel_pub.msg_.twist.angular.x = tcp_speed[3];
-                tool_vel_pub.msg_.twist.angular.y = tcp_speed[4];
-                tool_vel_pub.msg_.twist.angular.z = tcp_speed[5];
-
-                tool_vel_pub.unlockAndPublish();
-            }
-        }
-    }
-
-    void publishRTMsg()
-    {
-        ros::Publisher joint_pub = nh_.advertise<sensor_msgs::JointState>(
-            "joint_states", 1);
-        ros::Publisher wrench_pub = nh_.advertise<geometry_msgs::WrenchStamped>(
-            "wrench", 1);
-        ros::Publisher tool_vel_pub = nh_.advertise<geometry_msgs::TwistStamped>("tool_velocity", 1);
-        static tf::TransformBroadcaster br;
-        while (ros::ok()) {
-            sensor_msgs::JointState joint_msg;
-            joint_msg.name = robot_.getJointNames();
-            geometry_msgs::WrenchStamped wrench_msg;
-            geometry_msgs::PoseStamped tool_pose_msg;
-            std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex
-            std::unique_lock<std::mutex> locker(msg_lock);
-            while (!robot_.rt_interface_->robot_state_->getDataPublished()) {
-                rt_msg_cond_.wait(locker);
-            }
-            joint_msg.header.stamp = ros::Time::now();
-            joint_msg.position = robot_.rt_interface_->robot_state_->getQActual();
-            for (unsigned int i = 0; i < joint_msg.position.size(); i++) {
-                joint_msg.position[i] += joint_offsets_[i];
-            }
-            joint_msg.velocity = robot_.rt_interface_->robot_state_->getQdActual();
-            joint_msg.effort = robot_.rt_interface_->robot_state_->getIActual();
-            joint_pub.publish(joint_msg);
-            std::vector<double> tcp_force = robot_.rt_interface_->robot_state_->getTcpForce();
-            wrench_msg.header.stamp = joint_msg.header.stamp;
-            wrench_msg.wrench.force.x = tcp_force[0];
-            wrench_msg.wrench.force.y = tcp_force[1];
-            wrench_msg.wrench.force.z = tcp_force[2];
-            wrench_msg.wrench.torque.x = tcp_force[3];
-            wrench_msg.wrench.torque.y = tcp_force[4];
-            wrench_msg.wrench.torque.z = tcp_force[5];
-            wrench_pub.publish(wrench_msg);
-
-            // Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation
-            std::vector<double> tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual();
-
-            //Create quaternion
-            tf::Quaternion quat;
-            double rx = tool_vector_actual[3];
-            double ry = tool_vector_actual[4];
-            double rz = tool_vector_actual[5];
-            double angle = std::sqrt(std::pow(rx, 2) + std::pow(ry, 2) + std::pow(rz, 2));
-            if (angle < 1e-16) {
-                quat.setValue(0, 0, 0, 1);
-            } else {
-                quat.setRotation(tf::Vector3(rx / angle, ry / angle, rz / angle), angle);
-            }
-
-            //Create and broadcast transform
-            tf::Transform transform;
-            transform.setOrigin(tf::Vector3(tool_vector_actual[0], tool_vector_actual[1], tool_vector_actual[2]));
-            transform.setRotation(quat);
-            br.sendTransform(tf::StampedTransform(transform, joint_msg.header.stamp, base_frame_, tool_frame_));
-
-            //Publish tool velocity
-            std::vector<double> tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual();
-            geometry_msgs::TwistStamped tool_twist;
-            tool_twist.header.frame_id = base_frame_;
-            tool_twist.header.stamp = joint_msg.header.stamp;
-            tool_twist.twist.linear.x = tcp_speed[0];
-            tool_twist.twist.linear.y = tcp_speed[1];
-            tool_twist.twist.linear.z = tcp_speed[2];
-            tool_twist.twist.angular.x = tcp_speed[3];
-            tool_twist.twist.angular.y = tcp_speed[4];
-            tool_twist.twist.angular.z = tcp_speed[5];
-            tool_vel_pub.publish(tool_twist);
-
-            robot_.rt_interface_->robot_state_->setDataPublished();
-        }
-    }
-
-    void publishMbMsg()
-    {
-        bool warned = false;
-        ros::Publisher io_pub = nh_.advertise<ur_msgs::IOStates>(
-            "ur_driver/io_states", 1);
-
-        while (ros::ok()) {
-            ur_msgs::IOStates io_msg;
-            std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex
-            std::unique_lock<std::mutex> locker(msg_lock);
-            while (!robot_.sec_interface_->robot_state_->getNewDataAvailable()) {
-                msg_cond_.wait(locker);
-            }
-            int i_max = 10;
-            if (robot_.sec_interface_->robot_state_->getVersion() > 3.0)
-                i_max = 18; // From version 3.0, there are up to 18 inputs and outputs
-            for (unsigned int i = 0; i < i_max; i++) {
-                ur_msgs::Digital digi;
-                digi.pin = i;
-                digi.state = ((robot_.sec_interface_->robot_state_->getDigitalInputBits()
-                                  & (1 << i))
-                    >> i);
-                io_msg.digital_in_states.push_back(digi);
-                digi.state = ((robot_.sec_interface_->robot_state_->getDigitalOutputBits()
-                                  & (1 << i))
-                    >> i);
-                io_msg.digital_out_states.push_back(digi);
-            }
-            ur_msgs::Analog ana;
-            ana.pin = 0;
-            ana.state = robot_.sec_interface_->robot_state_->getAnalogInput0();
-            io_msg.analog_in_states.push_back(ana);
-            ana.pin = 1;
-            ana.state = robot_.sec_interface_->robot_state_->getAnalogInput1();
-            io_msg.analog_in_states.push_back(ana);
-
-            ana.pin = 0;
-            ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput0();
-            io_msg.analog_out_states.push_back(ana);
-            ana.pin = 1;
-            ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput1();
-            io_msg.analog_out_states.push_back(ana);
-            io_pub.publish(io_msg);
-
-            if (robot_.sec_interface_->robot_state_->isEmergencyStopped()
-                or robot_.sec_interface_->robot_state_->isProtectiveStopped()) {
-                if (robot_.sec_interface_->robot_state_->isEmergencyStopped()
-                    and !warned) {
-                    print_error("Emergency stop pressed!");
-                } else if (robot_.sec_interface_->robot_state_->isProtectiveStopped()
-                    and !warned) {
-                    print_error("Robot is protective stopped!");
-                }
-                if (has_goal_) {
-                    print_error("Aborting trajectory");
-                    robot_.stopTraj();
-                    result_.error_code = result_.SUCCESSFUL;
-                    result_.error_string = "Robot was halted";
-                    goal_handle_.setAborted(result_, result_.error_string);
-                    has_goal_ = false;
-                }
-                warned = true;
-            } else
-                warned = false;
-
-            robot_.sec_interface_->robot_state_->finishedReading();
+        if (has_goal_)
+        {
+          print_error("Aborting trajectory");
+          robot_.stopTraj();
+          result_.error_code = result_.SUCCESSFUL;
+          result_.error_string = "Robot was halted";
+          goal_handle_.setAborted(result_, result_.error_string);
+          has_goal_ = false;
         }
+        warned = true;
+      }
+      else
+        warned = false;
+
+      robot_.sec_interface_->robot_state_->finishedReading();
     }
+  }
 };
 
 int main(int argc, char** argv)
 {
-    bool use_sim_time = false;
-    std::string host;
-    int reverse_port = 50001;
+  bool use_sim_time = false;
+  std::string host;
+  int reverse_port = 50001;
 
-    ros::init(argc, argv, "ur_driver");
-    ros::NodeHandle nh;
-    if (ros::param::get("use_sim_time", use_sim_time)) {
-        print_warning("use_sim_time is set!!");
+  ros::init(argc, argv, "ur_driver");
+  ros::NodeHandle nh;
+  if (ros::param::get("use_sim_time", use_sim_time))
+  {
+    print_warning("use_sim_time is set!!");
+  }
+  if (!(ros::param::get("~robot_ip_address", host)))
+  {
+    if (argc > 1)
+    {
+      print_warning("Please set the parameter robot_ip_address instead of giving it as a command line argument. This "
+                    "method is DEPRECATED");
+      host = argv[1];
     }
-    if (!(ros::param::get("~robot_ip_address", host))) {
-        if (argc > 1) {
-            print_warning(
-                "Please set the parameter robot_ip_address instead of giving it as a command line argument. This method is DEPRECATED");
-            host = argv[1];
-        } else {
-            print_fatal(
-                "Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip");
-            exit(1);
-        }
+    else
+    {
+      print_fatal("Could not get robot ip. Please supply it as command line parameter or on the parameter server as "
+                  "robot_ip");
+      exit(1);
     }
-    if ((ros::param::get("~reverse_port", reverse_port))) {
-        if ((reverse_port <= 0) or (reverse_port >= 65535)) {
-            print_warning("Reverse port value is not valid (Use number between 1 and 65534. Using default value of 50001");
-            reverse_port = 50001;
-        }
-    } else
-        reverse_port = 50001;
+  }
+  if ((ros::param::get("~reverse_port", reverse_port)))
+  {
+    if ((reverse_port <= 0) or (reverse_port >= 65535))
+    {
+      print_warning("Reverse port value is not valid (Use number between 1 and 65534. Using default value of 50001");
+      reverse_port = 50001;
+    }
+  }
+  else
+    reverse_port = 50001;
 
-    RosWrapper interface(host, reverse_port);
+  RosWrapper interface(host, reverse_port);
 
-    ros::AsyncSpinner spinner(3);
-    spinner.start();
+  ros::AsyncSpinner spinner(3);
+  spinner.start();
 
-    ros::waitForShutdown();
+  ros::waitForShutdown();
 
-    interface.halt();
+  interface.halt();
 
-    exit(0);
+  exit(0);
 }