Adopted roscpp code style and naming convention

.clang-format

@@ -1,95 +1,48 @@
 ---
-Language:        Cpp
+BasedOnStyle:  Google
-# BasedOnStyle:  WebKit
+AccessModifierOffset: -2
-AccessModifierOffset: -4
+ConstructorInitializerIndentWidth: 2
-AlignAfterOpenBracket: DontAlign
-AlignConsecutiveAssignments: false
-AlignConsecutiveDeclarations: false
 AlignEscapedNewlinesLeft: false
-AlignOperands:   false
+AlignTrailingComments: true
-AlignTrailingComments: false
+AllowAllParametersOfDeclarationOnNextLine: false
-AllowAllParametersOfDeclarationOnNextLine: true
-AllowShortBlocksOnASingleLine: false
-AllowShortCaseLabelsOnASingleLine: false
-AllowShortFunctionsOnASingleLine: All
 AllowShortIfStatementsOnASingleLine: false
 AllowShortLoopsOnASingleLine: false
-AlwaysBreakAfterDefinitionReturnType: None
+AllowShortFunctionsOnASingleLine: None
-AlwaysBreakAfterReturnType: None
+AllowShortLoopsOnASingleLine: false
+AlwaysBreakTemplateDeclarations: true
 AlwaysBreakBeforeMultilineStrings: false
-AlwaysBreakTemplateDeclarations: false
+BreakBeforeBinaryOperators: false
-BinPackArguments: true
+BreakBeforeTernaryOperators: false
-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
 BreakConstructorInitializersBeforeComma: true
-BreakAfterJavaFieldAnnotations: false
+BinPackParameters: true
-BreakStringLiterals: true
+ColumnLimit:    120
-ColumnLimit:     0
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
-CommentPragmas:  '^ IWYU pragma:'
+DerivePointerBinding: true
-ConstructorInitializerAllOnOneLineOrOnePerLine: false
-ConstructorInitializerIndentWidth: 4
-ContinuationIndentWidth: 4
-Cpp11BracedListStyle: false
-DerivePointerAlignment: false
-DisableFormat:   false
 ExperimentalAutoDetectBinPacking: false
-ForEachMacros:   [ foreach, Q_FOREACH, BOOST_FOREACH ]
+IndentCaseLabels: true
-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:   ''
 MaxEmptyLinesToKeep: 1
-NamespaceIndentation: Inner
+NamespaceIndentation: None
-ObjCBlockIndentWidth: 4
-ObjCSpaceAfterProperty: true
 ObjCSpaceBeforeProtocolList: true
 PenaltyBreakBeforeFirstCallParameter: 19
-PenaltyBreakComment: 300
+PenaltyBreakComment: 60
-PenaltyBreakFirstLessLess: 120
+PenaltyBreakString: 1
-PenaltyBreakString: 1000
+PenaltyBreakFirstLessLess: 1000
-PenaltyExcessCharacter: 1000000
+PenaltyExcessCharacter: 1000
-PenaltyReturnTypeOnItsOwnLine: 60
+PenaltyReturnTypeOnItsOwnLine: 90
-PointerAlignment: Left
+PointerBindsToType: false
-ReflowComments:  true
+SpacesBeforeTrailingComments: 2
-SortIncludes:    true
+Cpp11BracedListStyle: false
-SpaceAfterCStyleCast: false
+Standard:        Auto
-SpaceBeforeAssignmentOperators: true
+IndentWidth:     2
-SpaceBeforeParens: ControlStatements
+TabWidth:        2
-SpaceInEmptyParentheses: false
-SpacesBeforeTrailingComments: 1
-SpacesInAngles:  false
-SpacesInContainerLiterals: true
-SpacesInCStyleCastParentheses: false
-SpacesInParentheses: false
-SpacesInSquareBrackets: false
-Standard:        Cpp03
-TabWidth:        8
 UseTab:          Never
+BreakBeforeBraces: Allman
+IndentFunctionDeclarationAfterType: false
+SpacesInParentheses: false
+SpacesInAngles:  false
+SpaceInEmptyParentheses: false
+SpacesInCStyleCastParentheses: false
+SpaceAfterControlStatementKeyword: true
+SpaceBeforeAssignmentOperators: true
+ContinuationIndentWidth: 4
 ...
-

include/ur_modern_driver/bin_parser.h

@@ -1,39 +1,35 @@
 #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;
+  uint8_t *buf_pos_, *buf_end_;
-    BinParser& _parent;
+  BinParser& parent_;
 
 public:
-    BinParser(uint8_t* buffer, size_t buf_len)
+  BinParser(uint8_t* buffer, size_t buf_len) : buf_pos_(buffer), buf_end_(buffer + buf_len), parent_(*this)
-        : _buf_pos(buffer)
-        , _buf_end(buffer + buf_len)
-        , _parent(*this)
   {
-        assert(_buf_pos <= _buf_end);
+    assert(buf_pos_ <= buf_end_);
   }
 
   BinParser(BinParser& parent, size_t sub_len)
-        : _buf_pos(parent._buf_pos)
+    : buf_pos_(parent.buf_pos_), buf_end_(parent.buf_pos_ + sub_len), parent_(parent)
-        , _buf_end(parent._buf_pos + sub_len)
-        , _parent(parent)
   {
-        assert(_buf_pos <= _buf_end);
+    assert(buf_pos_ <= buf_end_);
   }
 
   ~BinParser()
   {
-        _parent._buf_pos = _buf_pos;
+    parent_.buf_pos_ = buf_pos_;
   }
 
   // Decode from network encoding (big endian) to host encoding
@@ -70,9 +66,9 @@ public:
   template <typename T>
   T peek()
   {
-        assert(_buf_pos <= _buf_end);
+    assert(buf_pos_ <= buf_end_);
     T val;
-        std::memcpy(&val, _buf_pos, sizeof(T));
+    std::memcpy(&val, buf_pos_, sizeof(T));
     return decode(val);
   }
 
@@ -80,7 +76,7 @@ public:
   void parse(T& val)
   {
     val = peek<T>();
-        _buf_pos += sizeof(T);
+    buf_pos_ += sizeof(T);
   }
 
   void parse(double& val)
@@ -122,13 +118,13 @@ public:
 
   void parse_remainder(std::string& val)
   {
-        parse(val, size_t(_buf_end - _buf_pos));
+    parse(val, size_t(buf_end_ - buf_pos_));
   }
 
   void parse(std::string& val, size_t len)
   {
-        val.assign(reinterpret_cast<char*>(_buf_pos), len);
+    val.assign(reinterpret_cast<char*>(buf_pos_), len);
-        _buf_pos += len;
+    buf_pos_ += len;
   }
 
   // Special string parse function that assumes uint8_t len followed by chars
@@ -142,37 +138,38 @@ public:
   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]);
     }
   }
 
   void consume()
   {
-        _buf_pos = _buf_end;
+    buf_pos_ = buf_end_;
   }
   void consume(size_t bytes)
   {
-        _buf_pos += bytes;
+    buf_pos_ += bytes;
   }
 
-    bool check_size(size_t bytes)
+  bool checkSize(size_t bytes)
   {
-        return bytes <= size_t(_buf_end - _buf_pos);
+    return bytes <= size_t(buf_end_ - buf_pos_);
   }
   template <typename T>
-    bool check_size(void)
+  bool checkSize(void)
   {
-        return check_size(T::SIZE);
+    return checkSize(T::SIZE);
   }
 
   bool empty()
   {
-        return _buf_pos == _buf_end;
+    return buf_pos_ == buf_end_;
   }
 
   void debug()
   {
-        LOG_DEBUG("BinParser: %p - %p (%zu bytes)", _buf_pos, _buf_end, _buf_end - _buf_pos);
+    LOG_DEBUG("BinParser: %p - %p (%zu bytes)", buf_pos_, buf_end_, buf_end_ - buf_pos_);
   }
 };

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;
 };

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;
 };

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 setupConsumer()
-    virtual void teardown_consumer() {}
+  {
-    virtual void stop_consumer() {}
+  }
+  virtual void teardownConsumer()
+  {
+  }
+  virtual void stopConsumer()
+  {
+  }
 
   virtual bool consume(unique_ptr<T> product) = 0;
 };
 
 template <typename T>
-class IProducer {
+class IProducer
+{
 public:
-    virtual void setup_producer() {}
+  virtual void setupProducer()
-    virtual void teardown_producer() {}
+  {
-    virtual void stop_producer() {}
+  }
+  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;
+  IProducer<T>& producer_;
-    IConsumer<T>& _consumer;
+  IConsumer<T>& consumer_;
-    BlockingReaderWriterQueue<unique_ptr<T> > _queue;
+  BlockingReaderWriterQueue<unique_ptr<T>> queue_;
-    atomic<bool> _running;
+  atomic<bool> running_;
-    thread _pThread, _cThread;
+  thread pThread_, cThread_;
 
   void run_producer()
   {
-        _producer.setup_producer();
+    producer_.setupProducer();
     std::vector<unique_ptr<T>> products;
-        while (_running) {
+    while (running_)
-            if (!_producer.try_get(products)) {
+    {
+      if (!producer_.tryGet(products))
+      {
         break;
       }
 
-            for (auto& p : products) {
+      for (auto& p : products)
-                if (!_queue.try_enqueue(std::move(p))) {
+      {
+        if (!queue_.try_enqueue(std::move(p)))
+        {
           LOG_ERROR("Pipeline producer owerflowed!");
         }
       }
 
       products.clear();
     }
-        _producer.teardown_producer();
+    producer_.teardownProducer();
     LOG_DEBUG("Pipline producer ended");
-        _consumer.stop_consumer();
+    consumer_.stopConsumer();
   }
 
   void run_consumer()
   {
-        _consumer.setup_consumer();
+    consumer_.setupConsumer();
     unique_ptr<T> product;
-        while (_running) {
+    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)) {
+      if (!queue_.wait_dequeue_timed(product, 16000))
+      {
         continue;
       }
-            if (!_consumer.consume(std::move(product)))
+      if (!consumer_.consume(std::move(product)))
         break;
     }
-        _consumer.teardown_consumer();
+    consumer_.teardownConsumer();
     LOG_DEBUG("Pipline consumer ended");
-        _producer.stop_producer();
+    producer_.stopProducer();
   }
 
 public:
   Pipeline(IProducer<T>& producer, IConsumer<T>& consumer)
-        : _producer(producer)
+    : producer_(producer), consumer_(consumer), queue_{ 32 }, running_{ false }
-        , _consumer(consumer)
-        , _queue{ 32 }
-        , _running{ false }
   {
   }
 
   void run()
   {
-        if (_running)
+    if (running_)
       return;
 
-        _running = true;
+    running_ = true;
-        _pThread = thread(&Pipeline::run_producer, this);
+    pThread_ = thread(&Pipeline::run_producer, this);
-        _cThread = thread(&Pipeline::run_consumer, this);
+    cThread_ = thread(&Pipeline::run_consumer, this);
   }
 
   void stop()
   {
-        if (!_running)
+    if (!running_)
       return;
 
     LOG_DEBUG("Stopping pipeline");
 
-        _consumer.stop_consumer();
+    consumer_.stopConsumer();
-        _producer.stop_producer();
+    producer_.stopProducer();
 
-        _running = false;
+    running_ = false;
 
-        _pThread.join();
+    pThread_.join();
-        _cThread.join();
+    cThread_.join();
   }
 };

include/ur_modern_driver/queue/atomicops.h

@@ -63,9 +63,10 @@
 
 // Portable atomic fences implemented below:
 
-namespace moodycamel {
+namespace moodycamel
-
+{
-enum memory_order {
+enum memory_order
+{
   memory_order_relaxed,
   memory_order_acquire,
   memory_order_release,
@@ -98,17 +99,19 @@ 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;
     case memory_order_acquire:
@@ -134,7 +137,8 @@ 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;
     case memory_order_acquire:
@@ -159,7 +163,8 @@ AE_FORCEINLINE void fence(memory_order order)
 AE_FORCEINLINE void fence(memory_order order)
 {
   // Non-specialized arch, use heavier memory barriers everywhere just in case :-(
-    switch (order) {
+  switch (order)
+  {
     case memory_order_relaxed:
       break;
     case memory_order_acquire:
@@ -192,11 +197,12 @@ AE_FORCEINLINE void fence(memory_order order)
 // 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;
     case memory_order_acquire:
@@ -218,7 +224,8 @@ AE_FORCEINLINE void compiler_fence(memory_order order)
 
 AE_FORCEINLINE void fence(memory_order order)
 {
-    switch (order) {
+  switch (order)
+  {
     case memory_order_relaxed:
       break;
     case memory_order_acquire:
@@ -255,32 +262,32 @@ 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
 #endif
   template <typename U>
-    weak_atomic(U&& x)
+  weak_atomic(U&& x) : value(std::forward<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)
+  weak_atomic(nullptr_t) : value(nullptr)
-        : value(nullptr)
   {
   }
 #endif
-    weak_atomic(weak_atomic const& other)
+  weak_atomic(weak_atomic const& other) : value(other.value)
-        : value(other.value)
   {
   }
-    weak_atomic(weak_atomic&& other)
+  weak_atomic(weak_atomic&& other) : value(std::move(other.value))
-        : value(std::move(other.value))
   {
   }
 #ifdef AE_VCPP
@@ -305,7 +312,10 @@ public:
     return *this;
   }
 
-    AE_FORCEINLINE T load() const { return value; }
+  AE_FORCEINLINE T load() const
+  {
+    return value;
+  }
 
   AE_FORCEINLINE T fetch_add_acquire(T increment)
   {
@@ -352,7 +362,10 @@ public:
     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)
   {
@@ -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,9 +435,11 @@ 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 {
+class Semaphore
+{
 private:
   void* m_hSema;
 
@@ -470,7 +487,8 @@ namespace spsc_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 {
+class Semaphore
+{
 private:
   semaphore_t m_sema;
 
@@ -505,7 +523,8 @@ namespace spsc_sema {
     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
+    // 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;
@@ -518,7 +537,8 @@ namespace spsc_sema {
 
   void signal(int count)
   {
-            while (count-- > 0) {
+    while (count-- > 0)
+    {
       semaphore_signal(m_sema);
     }
   }
@@ -527,7 +547,8 @@ namespace spsc_sema {
 //---------------------------------------------------------
 // Semaphore (POSIX, Linux)
 //---------------------------------------------------------
-    class Semaphore {
+class Semaphore
+{
 private:
   sem_t m_sema;
 
@@ -550,7 +571,8 @@ namespace spsc_sema {
   {
     // http://stackoverflow.com/questions/2013181/gdb-causes-sem-wait-to-fail-with-eintr-error
     int rc;
-            do {
+    do
+    {
       rc = sem_wait(&m_sema);
     } while (rc == -1 && errno == EINTR);
   }
@@ -558,7 +580,8 @@ namespace spsc_sema {
   bool try_wait()
   {
     int rc;
-            do {
+    do
+    {
       rc = sem_trywait(&m_sema);
     } while (rc == -1 && errno == EINTR);
     return !(rc == -1 && errno == EAGAIN);
@@ -574,13 +597,15 @@ namespace spsc_sema {
     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) {
+    if (ts.tv_nsec > nsecs_in_1_sec)
+    {
       ts.tv_nsec -= nsecs_in_1_sec;
       ++ts.tv_sec;
     }
 
     int rc;
-            do {
+    do
+    {
       rc = sem_timedwait(&m_sema, &ts);
     } while (rc == -1 && errno == EINTR);
     return !(rc == -1 && errno == ETIMEDOUT);
@@ -593,7 +618,8 @@ namespace spsc_sema {
 
   void signal(int count)
   {
-            while (count-- > 0) {
+    while (count-- > 0)
+    {
       sem_post(&m_sema);
     }
   }
@@ -605,7 +631,8 @@ namespace spsc_sema {
 //---------------------------------------------------------
 // LightweightSemaphore
 //---------------------------------------------------------
-    class LightweightSemaphore {
+class LightweightSemaphore
+{
 public:
   typedef std::make_signed<std::size_t>::type ssize_t;
 
@@ -620,8 +647,10 @@ namespace spsc_sema {
     // 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) {
+    while (--spin >= 0)
-                if (m_count.load() > 0) {
+    {
+      if (m_count.load() > 0)
+      {
         m_count.fetch_add_acquire(-1);
         return true;
       }
@@ -630,7 +659,8 @@ namespace spsc_sema {
     oldCount = m_count.fetch_add_acquire(-1);
     if (oldCount > 0)
       return true;
-            if (timeout_usecs < 0) {
+    if (timeout_usecs < 0)
+    {
       m_sema.wait();
       return true;
     }
@@ -641,7 +671,8 @@ namespace spsc_sema {
     // 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) {
+    while (true)
+    {
       oldCount = m_count.fetch_add_release(1);
       if (oldCount < 0)
         return false;  // successfully restored things to the way they were
@@ -653,15 +684,15 @@ namespace spsc_sema {
   }
 
 public:
-        LightweightSemaphore(ssize_t initialCount = 0)
+  LightweightSemaphore(ssize_t initialCount = 0) : m_count(initialCount)
-            : m_count(initialCount)
   {
     assert(initialCount >= 0);
   }
 
   bool tryWait()
   {
-            if (m_count.load() > 0) {
+    if (m_count.load() > 0)
+    {
       m_count.fetch_add_acquire(-1);
       return true;
     }
@@ -684,7 +715,8 @@ namespace spsc_sema {
     assert(count >= 0);
     ssize_t oldCount = m_count.fetch_add_release(count);
     assert(oldCount >= -1);
-            if (oldCount < 0) {
+    if (oldCount < 0)
+    {
       m_sema.signal(1);
     }
   }

include/ur_modern_driver/queue/readerwriterqueue.h

@@ -4,7 +4,6 @@
 
 #pragma once
 
-#include "atomicops.h"
 #include <cassert>
 #include <cstdint>
 #include <cstdlib>  // For malloc/free/abort & size_t
@@ -12,6 +11,7 @@
 #include <stdexcept>
 #include <type_traits>
 #include <utility>
+#include "atomicops.h"
 #if __cplusplus > 199711L || _MSC_VER >= 1700  // C++11 or VS2012
 #include <chrono>
 #endif
@@ -34,7 +34,8 @@
 #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
@@ -46,10 +47,11 @@
 #pragma warning(disable : 4127)  // conditional expression is constant
 #endif
 
-namespace moodycamel {
+namespace moodycamel
-
+{
 template <typename T, size_t MAX_BLOCK_SIZE = 512>
-class ReaderWriterQueue {
+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,
@@ -77,8 +79,7 @@ public:
   // at least one extra buffer block).
   explicit ReaderWriterQueue(size_t maxSize = 15)
 #ifndef NDEBUG
-        : enqueuing(false)
+    : enqueuing(false), dequeuing(false)
-        , dequeuing(false)
 #endif
   {
     assert(maxSize > 0);
@@ -88,35 +89,45 @@ public:
     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) {
+    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
+      // 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) {
+      for (size_t i = 0; i != initialBlockCount; ++i)
+      {
         auto block = make_block(largestBlockSize);
-                if (block == nullptr) {
+        if (block == nullptr)
+        {
 #ifdef MOODYCAMEL_EXCEPTIONS_ENABLED
           throw std::bad_alloc();
 #else
           abort();
 #endif
         }
-                if (firstBlock == nullptr) {
+        if (firstBlock == nullptr)
+        {
           firstBlock = block;
-                } else {
+        }
+        else
+        {
           lastBlock->next = block;
         }
         lastBlock = block;
         block->next = firstBlock;
       }
-        } else {
+    }
+    else
+    {
       firstBlock = make_block(largestBlockSize);
-            if (firstBlock == nullptr) {
+      if (firstBlock == nullptr)
+      {
 #ifdef MOODYCAMEL_EXCEPTIONS_ENABLED
         throw std::bad_alloc();
 #else
@@ -142,12 +153,14 @@ public:
     // Destroy any remaining objects in queue and free memory
     Block* frontBlock_ = frontBlock;
     Block* block = frontBlock_;
-        do {
+    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) {
+      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;
@@ -223,7 +236,8 @@ public:
     size_t blockTail = frontBlock_->localTail;
     size_t blockFront = frontBlock_->front.load();
 
-        if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) {
+    if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load()))
+    {
       fence(memory_order_acquire);
 
     non_empty_front_block:
@@ -236,7 +250,9 @@ public:
 
       fence(memory_order_release);
       frontBlock_->front = blockFront;
-        } else if (frontBlock_ != tailBlock.load()) {
+    }
+    else if (frontBlock_ != tailBlock.load())
+    {
       fence(memory_order_acquire);
 
       frontBlock_ = frontBlock.load();
@@ -244,7 +260,8 @@ public:
       blockFront = frontBlock_->front.load();
       fence(memory_order_acquire);
 
-            if (blockFront != blockTail) {
+      if (blockFront != blockTail)
+      {
         // Oh look, the front block isn't empty after all
         goto non_empty_front_block;
       }
@@ -279,7 +296,9 @@ public:
 
       fence(memory_order_release);
       frontBlock_->front = nextBlockFront;
-        } else {
+    }
+    else
+    {
       // No elements in current block and no other block to advance to
       return false;
     }
@@ -303,18 +322,22 @@ public:
     size_t blockTail = frontBlock_->localTail;
     size_t blockFront = frontBlock_->front.load();
 
-        if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.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()) {
+    }
+    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) {
+      if (blockFront != blockTail)
+      {
         goto non_empty_front_block;
       }
 
@@ -344,7 +367,8 @@ public:
     size_t blockTail = frontBlock_->localTail;
     size_t blockFront = frontBlock_->front.load();
 
-        if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) {
+    if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load()))
+    {
       fence(memory_order_acquire);
 
     non_empty_front_block:
@@ -355,14 +379,17 @@ public:
 
       fence(memory_order_release);
       frontBlock_->front = blockFront;
-        } else if (frontBlock_ != tailBlock.load()) {
+    }
+    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) {
+      if (blockFront != blockTail)
+      {
         goto non_empty_front_block;
       }
 
@@ -388,7 +415,9 @@ public:
 
       fence(memory_order_release);
       frontBlock_->front = nextBlockFront;
-        } else {
+    }
+    else
+    {
       // No elements in current block and no other block to advance to
       return false;
     }
@@ -403,7 +432,8 @@ public:
     size_t result = 0;
     Block* frontBlock_ = frontBlock.load();
     Block* block = frontBlock_;
-        do {
+    do
+    {
       fence(memory_order_acquire);
       size_t blockFront = block->front.load();
       size_t blockTail = block->tail.load();
@@ -414,8 +444,11 @@ public:
   }
 
 private:
-    enum AllocationMode { CanAlloc,
+  enum AllocationMode
-        CannotAlloc };
+  {
+    CanAlloc,
+    CannotAlloc
+  };
 
   template <AllocationMode canAlloc, typename U>
   bool inner_enqueue(U&& element)
@@ -436,7 +469,8 @@ private:
     size_t blockTail = tailBlock_->tail.load();
 
     size_t nextBlockTail = (blockTail + 1) & tailBlock_->sizeMask;
-        if (nextBlockTail != blockFront || nextBlockTail != (tailBlock_->localFront = tailBlock_->front.load())) {
+    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);
@@ -444,9 +478,12 @@ private:
 
       fence(memory_order_release);
       tailBlock_->tail = nextBlockTail;
-        } else {
+    }
+    else
+    {
       fence(memory_order_acquire);
-            if (tailBlock_->next.load() != frontBlock) {
+      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
@@ -472,11 +509,14 @@ private:
 
         fence(memory_order_release);
         tailBlock = tailBlockNext;
-            } else if (canAlloc == CanAlloc) {
+      }
+      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) {
+        if (newBlock == nullptr)
+        {
           // Could not allocate a block!
           return false;
         }
@@ -498,10 +538,14 @@ private:
 
         fence(memory_order_release);
         tailBlock = newBlock;
-            } else if (canAlloc == CannotAlloc) {
+      }
+      else if (canAlloc == CannotAlloc)
+      {
         // Would have had to allocate a new block to enqueue, but not allowed
         return false;
-            } else {
+      }
+      else
+      {
         assert(false && "Should be unreachable code");
         return false;
       }
@@ -511,10 +555,14 @@ private:
   }
 
   // Disable copying
-    ReaderWriterQueue(ReaderWriterQueue const&) {}
+  ReaderWriterQueue(ReaderWriterQueue const&)
+  {
+  }
 
   // Disable assignment
-    ReaderWriterQueue& operator=(ReaderWriterQueue const&) {}
+  ReaderWriterQueue& operator=(ReaderWriterQueue const&)
+  {
+  }
 
   AE_FORCEINLINE static size_t ceilToPow2(size_t x)
   {
@@ -523,7 +571,8 @@ private:
     x |= x >> 1;
     x |= x >> 2;
     x |= x >> 4;
-        for (size_t i = 1; i < sizeof(size_t); i <<= 1) {
+    for (size_t i = 1; i < sizeof(size_t); i <<= 1)
+    {
       x |= x >> (i << 3);
     }
     ++x;
@@ -539,15 +588,19 @@ private:
 
 private:
 #ifndef NDEBUG
-    struct ReentrantGuard {
+  struct ReentrantGuard
-        ReentrantGuard(bool& _inSection)
-            : inSection(_inSection)
   {
-            assert(!inSection && "ReaderWriterQueue does not support enqueuing or dequeuing elements from other elements' ctors and dtors");
+    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; }
+    ~ReentrantGuard()
+    {
+      inSection = false;
+    }
 
   private:
     ReentrantGuard& operator=(ReentrantGuard const&);
@@ -557,7 +610,8 @@ private:
   };
 #endif
 
-    struct Block {
+  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
@@ -566,7 +620,14 @@ private:
     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)
+    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
@@ -600,7 +661,8 @@ private:
     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) {
+    if (newBlockRaw == nullptr)
+    {
       return nullptr;
     }
 
@@ -625,13 +687,13 @@ private:
 
 // 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;
 
 public:
-    explicit BlockingReaderWriterQueue(size_t maxSize = 15)
+  explicit BlockingReaderWriterQueue(size_t maxSize = 15) : inner(maxSize)
-        : inner(maxSize)
   {
   }
 
@@ -640,7 +702,8 @@ public:
   // 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;
     }
@@ -652,7 +715,8 @@ public:
   // 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;
     }
@@ -664,7 +728,8 @@ public:
   // 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;
     }
@@ -676,7 +741,8 @@ public:
   // 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;
     }
@@ -689,7 +755,8 @@ public:
   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);
@@ -719,7 +786,8 @@ public:
   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);
@@ -758,7 +826,8 @@ public:
   // `pop` was called.
   AE_FORCEINLINE bool pop()
   {
-        if (sema.tryWait()) {
+    if (sema.tryWait())
+    {
       bool result = inner.pop();
       assert(result);
       AE_UNUSED(result);
@@ -776,8 +845,12 @@ public:
 
 private:
   // Disable copying & assignment
-    BlockingReaderWriterQueue(ReaderWriterQueue const&) {}
+  BlockingReaderWriterQueue(ReaderWriterQueue const&)
-    BlockingReaderWriterQueue& operator=(ReaderWriterQueue const&) {}
+  {
+  }
+  BlockingReaderWriterQueue& operator=(ReaderWriterQueue const&)
+  {
+  }
 
 private:
   ReaderWriterQueue inner;

include/ur_modern_driver/robot_state.h

@@ -19,16 +19,18 @@
 #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 {
+namespace message_types
-enum message_type {
+{
+enum message_type
+{
   ROBOT_STATE = 16,
   ROBOT_MESSAGE = 20,
   PROGRAM_STATE_MESSAGE = 25
@@ -36,8 +38,10 @@ enum message_type {
 }
 typedef message_types::message_type messageType;
 
-namespace package_types {
+namespace package_types
-enum package_type {
+{
+enum package_type
+{
   ROBOT_MODE_DATA = 0,
   JOINT_DATA = 1,
   TOOL_DATA = 2,
@@ -52,8 +56,10 @@ enum package_type {
 }
 typedef package_types::package_type packageType;
 
-namespace robot_message_types {
+namespace robot_message_types
-enum robot_message_type {
+{
+enum robot_message_type
+{
   ROBOT_MESSAGE_TEXT = 0,
   ROBOT_MESSAGE_PROGRAM_LABEL = 1,
   PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2,
@@ -67,8 +73,10 @@ enum robot_message_type {
 }
 typedef robot_message_types::robot_message_type robotMessageType;
 
-namespace robot_state_type_v18 {
+namespace robot_state_type_v18
-enum robot_state_type {
+{
+enum robot_state_type
+{
   ROBOT_RUNNING_MODE = 0,
   ROBOT_FREEDRIVE_MODE = 1,
   ROBOT_READY_MODE = 2,
@@ -83,8 +91,10 @@ enum robot_state_type {
 };
 }
 typedef robot_state_type_v18::robot_state_type robotStateTypeV18;
-namespace robot_state_type_v30 {
+namespace robot_state_type_v30
-enum robot_state_type {
+{
+enum robot_state_type
+{
   ROBOT_MODE_DISCONNECTED = 0,
   ROBOT_MODE_CONFIRM_SAFETY = 1,
   ROBOT_MODE_BOOTING = 2,
@@ -99,7 +109,8 @@ enum robot_state_type {
 
 typedef robot_state_type_v30::robot_state_type robotStateTypeV30;
 
-struct version_message {
+struct version_message
+{
   uint64_t timestamp;
   int8_t source;
   int8_t robot_message_type;
@@ -111,7 +122,8 @@ struct version_message {
   char build_date[25];
 };
 
-struct masterboard_data {
+struct masterboard_data
+{
   int digitalInputBits;
   int digitalOutputBits;
   char analogInputRange0;
@@ -135,7 +147,8 @@ struct masterboard_data {
   float euromapCurrent;
 };
 
-struct robot_mode_data {
+struct robot_mode_data
+{
   uint64_t timestamp;
   bool isRobotConnected;
   bool isRealRobotEnabled;
@@ -150,7 +163,8 @@ struct robot_mode_data {
   double speedScaling;
 };
 
-class RobotState {
+class RobotState
+{
 private:
   version_message version_msg_;
   masterboard_data mb_data_;
@@ -210,8 +224,7 @@ public:
 
   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,
+  void unpackRobotMessageVersion(uint8_t* buf, unsigned int offset, uint32_t len);
-        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);

include/ur_modern_driver/robot_state_RT.h

@@ -19,15 +19,16 @@
 #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
 
@@ -41,12 +42,15 @@ private:
   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> 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> 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<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
@@ -66,8 +70,7 @@ private:
   bool data_published_;                 // to avoid spurious wakes
   bool controller_updated_;             // to avoid spurious wakes
 
-    std::vector<double> unpackVector(uint8_t* buf, int start_index,
+  std::vector<double> unpackVector(uint8_t* buf, int start_index, int nr_of_vals);
-        int nr_of_vals);
   std::vector<bool> unpackDigitalInputBits(int64_t data);
   double ntohd(uint64_t nf);
 

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,46 +16,42 @@
 using namespace ros;
 using namespace tf;
 
-const std::string JOINTS[] = {
+const std::string JOINTS[] = { "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
-    "shoulder_pan_joint",
+                               "wrist_1_joint",      "wrist_2_joint",       "wrist_3_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;
+  NodeHandle nh_;
-    Publisher _joint_pub;
+  Publisher joint_pub_;
-    Publisher _wrench_pub;
+  Publisher wrench_pub_;
-    Publisher _tool_vel_pub;
+  Publisher tool_vel_pub_;
-    Publisher _joint_temperature_pub;
+  Publisher joint_temperature_pub_;
-    TransformBroadcaster _transform_broadcaster;
+  TransformBroadcaster transform_broadcaster_;
-    std::vector<std::string> _joint_names;
+  std::vector<std::string> joint_names_;
-    std::string _base_frame;
+  std::string base_frame_;
-    std::string _tool_frame;
+  std::string tool_frame_;
 
-    bool publish_joints(RTShared& packet, Time& t);
+  bool publishJoints(RTShared& packet, Time& t);
-    bool publish_wrench(RTShared& packet, Time& t);
+  bool publishWrench(RTShared& packet, Time& t);
-    bool publish_tool(RTShared& packet, Time& t);
+  bool publishTool(RTShared& packet, Time& t);
-    bool publish_transform(RTShared& packet, Time& t);
+  bool publishTransform(RTShared& packet, Time& t);
-    bool publish_temperature(RTShared& packet, Time& t);
+  bool publishTemperature(RTShared& packet, Time& t);
 
   bool publish(RTShared& packet);
 
 public:
   RTPublisher(std::string& joint_prefix, std::string& base_frame, std::string& tool_frame)
-        : _joint_pub(_nh.advertise<sensor_msgs::JointState>("joint_states", 1))
+    : joint_pub_(nh_.advertise<sensor_msgs::JointState>("joint_states", 1))
-        , _wrench_pub(_nh.advertise<geometry_msgs::WrenchStamped>("wrench", 1))
+    , wrench_pub_(nh_.advertise<geometry_msgs::WrenchStamped>("wrench", 1))
-        , _tool_vel_pub(_nh.advertise<geometry_msgs::TwistStamped>("tool_velocity", 1))
+    , tool_vel_pub_(nh_.advertise<geometry_msgs::TwistStamped>("tool_velocity", 1))
-        , _joint_temperature_pub(_nh.advertise<sensor_msgs::Temperature>("joint_temperature", 1))
+    , joint_temperature_pub_(nh_.advertise<sensor_msgs::Temperature>("joint_temperature", 1))
-        , _base_frame(base_frame)
+    , base_frame_(base_frame)
-        , _tool_frame(tool_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);
     }
   }
 
@@ -64,7 +60,13 @@ public:
   virtual bool consume(RTState_V3_0__1& state);
   virtual bool consume(RTState_V3_2__3& state);
 
-    virtual void setup_consumer() {}
+  virtual void setupConsumer()
-    virtual void teardown_consumer() {}
+  {
-    virtual void stop_consumer() {}
+  }
+  virtual void teardownConsumer()
+  {
+  }
+  virtual void stopConsumer()
+  {
+  }
 };

include/ur_modern_driver/types.h

@@ -2,11 +2,13 @@
 
 #include <inttypes.h>
 
-struct double3_t {
+struct double3_t
+{
   double x, y, z;
 };
 
-struct cartesian_coord_t {
+struct cartesian_coord_t
+{
   double3_t position;
   double3_t rotation;
 };

include/ur_modern_driver/ur/consumer.h

@@ -7,11 +7,12 @@
 #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);
+    return packet->consumeWith(*this);
   }
 
   virtual bool consume(RTState_V1_6__7& state) = 0;
@@ -20,11 +21,12 @@ public:
   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);
+    return packet->consumeWith(*this);
   }
 
   virtual bool consume(MasterBoardData_V1_X& data) = 0;
@@ -36,11 +38,12 @@ public:
   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);
+    return packet->consumeWith(*this);
   }
 
   virtual bool consume(VersionMessage& message) = 0;

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,15 +8,15 @@
 #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;
+  URStream stream_;
-    URMessageParser _parser;
+  URMessageParser parser_;
 
-    uint8_t _major_version;
+  uint8_t major_version_;
-    uint8_t _minor_version;
+  uint8_t minor_version_;
 
   bool consume(VersionMessage& vm)
   {
@@ -24,63 +25,77 @@ private:
     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;
+    major_version_ = vm.major_version;
-        _minor_version = vm.minor_version;
+    minor_version_ = vm.minor_version;
 
     return true;
   }
 
-    void setup_consumer() {}
+  void setupConsumer()
-    void teardown_consumer() {}
+  {
-    void stop_consumer() {}
+  }
+  void teardownConsumer()
+  {
+  }
+  void stopConsumer()
+  {
+  }
 
 public:
-    URFactory(std::string& host)
+  URFactory(std::string& host) : stream_(host, 30001)
-        : _stream(host, 30001)
   {
-        URProducer<MessagePacket> p(_stream, _parser);
+    URProducer<MessagePacket> prod(stream_, parser_);
     std::vector<unique_ptr<MessagePacket>> results;
 
-        p.setup_producer();
+    prod.setupProducer();
 
-        if (!p.try_get(results) || results.size() == 0) {
+    if (!prod.tryGet(results) || results.size() == 0)
-            LOG_FATAL("No version message received, init failed!");
+    {
-            std::exit(EXIT_FAILURE);
+      LOG_FATAL("No version message received, init failed!");
-        }
+      std::exit(EXIT_FAILURE);
-
+    }
-        for (auto const& p : results) {
+
-            p->consume_with(*this);
+    for (auto const& p : results)
-        }
+    {
-
+      p->consumeWith(*this);
-        if (_major_version == 0 && _minor_version == 0) {
+    }
-            LOG_FATAL("No version message received, init failed!");
+
-            std::exit(EXIT_FAILURE);
+    if (major_version_ == 0 && minor_version_ == 0)
-        }
+    {
-
+      LOG_FATAL("No version message received, init failed!");
-        p.teardown_producer();
+      std::exit(EXIT_FAILURE);
-    }
+    }
-
+
-    std::unique_ptr<URParser<StatePacket> > get_state_parser()
+    prod.teardownProducer();
+  }
+
+  std::unique_ptr<URParser<StatePacket>> getStateParser()
+  {
+    if (major_version_ == 1)
     {
-        if (_major_version == 1) {
       return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V1_X);
-        } else {
+    }
-            if (_minor_version < 3)
+    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> > get_rt_parser()
+  std::unique_ptr<URParser<RTPacket>> getRTParser()
   {
-        if (_major_version == 1) {
+    if (major_version_ == 1)
-            if (_minor_version < 8)
+    {
+      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)
+    else
+    {
+      if (minor_version_ < 3)
         return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V3_0__1);
       else
         return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V3_2__3);

include/ur_modern_driver/ur/master_board.h

@@ -1,14 +1,15 @@
 #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;
@@ -29,18 +30,16 @@ public:
   int32_t euromap_input_bits;
   int32_t euromap_output_bits;
 
-    static const size_t SIZE = sizeof(double) * 4
+  static const size_t SIZE = sizeof(double) * 4 + sizeof(int8_t) * 4 + sizeof(float) * 4 + sizeof(uint8_t);
-        + sizeof(int8_t) * 4
-        + sizeof(float) * 4
-        + sizeof(uint8_t);
 
   static const size_t EURO_SIZE = sizeof(int32_t) * 2;
 };
 
-class MasterBoardData_V1_X : public SharedMasterBoardData, public StatePacket {
+class MasterBoardData_V1_X : public SharedMasterBoardData, public StatePacket
+{
 public:
-    virtual bool parse_with(BinParser& bp);
+  virtual bool parseWith(BinParser& bp);
-    virtual bool consume_with(URStatePacketConsumer& consumer);
+  virtual bool consumeWith(URStatePacketConsumer& consumer);
 
   int16_t digital_input_bits;
   int16_t digital_output_bits;
@@ -52,18 +51,16 @@ public:
   int16_t euromap_voltage;
   int16_t euromap_current;
 
-    static const size_t SIZE = SharedMasterBoardData::SIZE
+  static const size_t SIZE = SharedMasterBoardData::SIZE + sizeof(int16_t) * 2 + sizeof(uint8_t) * 2;
-        + sizeof(int16_t) * 2
-        + sizeof(uint8_t) * 2;
 
-    static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE
+  static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE + sizeof(int16_t) * 2;
-        + 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 parseWith(BinParser& bp);
-    virtual bool consume_with(URStatePacketConsumer& consumer);
+  virtual bool consumeWith(URStatePacketConsumer& consumer);
 
   int32_t digital_input_bits;
   int32_t digital_output_bits;
@@ -75,23 +72,20 @@ public:
   float euromap_voltage;
   float euromap_current;
 
-    static const size_t SIZE = SharedMasterBoardData::SIZE
+  static const size_t SIZE = SharedMasterBoardData::SIZE + sizeof(int32_t) * 2 + sizeof(uint8_t) * 2 +
-        + sizeof(int32_t) * 2
+                             sizeof(uint32_t);  // UR internal field we skip
-        + sizeof(uint8_t) * 2
-        + sizeof(uint32_t); //UR internal field we skip
 
-    static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE
+  static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE + sizeof(float) * 2;
-        + 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 parseWith(BinParser& bp);
-    virtual bool consume_with(URStatePacketConsumer& consumer);
+  virtual bool consumeWith(URStatePacketConsumer& consumer);
 
   uint8_t operational_mode_selector_input;
   uint8_t three_position_enabling_device_input;
 
-    static const size_t SIZE = MasterBoardData_V3_0__1::SIZE
+  static const size_t SIZE = MasterBoardData_V3_0__1::SIZE + sizeof(uint8_t) * 2;
-        + sizeof(uint8_t) * 2;
 };

include/ur_modern_driver/ur/messages.h

@@ -1,11 +1,12 @@
 #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 {
+enum class robot_message_type : uint8_t
+{
   ROBOT_MESSAGE_TEXT = 0,
   ROBOT_MESSAGE_PROGRAM_LABEL = 1,
   PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2,
@@ -19,29 +20,28 @@ enum class robot_message_type : uint8_t {
 
 class URMessagePacketConsumer;
 
-class MessagePacket {
+class MessagePacket
+{
 public:
-    MessagePacket(uint64_t timestamp, uint8_t source)
+  MessagePacket(uint64_t timestamp, uint8_t source) : timestamp(timestamp), source(source)
-        : timestamp(timestamp)
-        , source(source)
   {
   }
-    virtual bool parse_with(BinParser& bp) = 0;
+  virtual bool parseWith(BinParser& bp) = 0;
-    virtual bool consume_with(URMessagePacketConsumer& consumer) = 0;
+  virtual bool consumeWith(URMessagePacketConsumer& consumer) = 0;
 
   uint64_t timestamp;
   uint8_t source;
 };
 
-class VersionMessage : public MessagePacket {
+class VersionMessage : public MessagePacket
+{
 public:
-    VersionMessage(uint64_t timestamp, uint8_t source)
+  VersionMessage(uint64_t timestamp, uint8_t source) : MessagePacket(timestamp, source)
-        : MessagePacket(timestamp, source)
   {
   }
 
-    virtual bool parse_with(BinParser& bp);
+  virtual bool parseWith(BinParser& bp);
-    virtual bool consume_with(URMessagePacketConsumer& consumer);
+  virtual bool consumeWith(URMessagePacketConsumer& consumer);
 
   std::string project_name;
   uint8_t major_version;

include/ur_modern_driver/ur/messages_parser.h

@@ -1,12 +1,13 @@
 #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)
   {
@@ -15,7 +16,8 @@ public:
     bp.parse(packet_size);
     bp.parse(type);
 
-        if (type != message_type::ROBOT_MESSAGE) {
+    if (type != message_type::ROBOT_MESSAGE)
+    {
       LOG_WARN("Invalid message type recieved: %u", static_cast<uint8_t>(type));
       return false;
     }
@@ -31,10 +33,12 @@ public:
     std::unique_ptr<MessagePacket> result;
     bool parsed = false;
 
-        switch (message_type) {
+    switch (message_type)
-        case robot_message_type::ROBOT_MESSAGE_VERSION: {
+    {
+      case robot_message_type::ROBOT_MESSAGE_VERSION:
+      {
         VersionMessage* vm = new VersionMessage(timestamp, source);
-            parsed = vm->parse_with(bp);
+        parsed = vm->parseWith(bp);
         result.reset(vm);
         break;
       }

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;
 };

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;
+  URStream& stream_;
-    URParser<T>& _parser;
+  URParser<T>& parser_;
 
 public:
-    URProducer(URStream& stream, URParser<T>& parser)
+  URProducer(URStream& stream, URParser<T>& parser) : stream_(stream), parser_(parser)
-        : _stream(stream)
-        , _parser(parser)
   {
   }
 
-    void setup_producer() { _stream.connect(); }
+  void setupProducer()
-    void teardown_producer() { _stream.disconnect(); }
+  {
-    void stop_producer() { _stream.disconnect(); }
+    stream_.connect();
+  }
+  void teardownProducer()
+  {
+    stream_.disconnect();
+  }
+  void stopProducer()
+  {
+    stream_.disconnect();
+  }
 
-    bool try_get(std::vector<unique_ptr<T> >& products)
+  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));
+    ssize_t len = stream_.receive(buf, sizeof(buf));
 
     // LOG_DEBUG("Read %d bytes from stream", len);
 
-        if (len == 0) {
+    if (len == 0)
+    {
       LOG_WARN("Read nothing from stream");
       return false;
-        } else if (len < 0) {
+    }
+    else if (len < 0)
+    {
       LOG_WARN("Stream closed");
       return false;
     }
 
     BinParser bp(buf, static_cast<size_t>(len));
-        return _parser.parse(bp, products);
+    return parser_.parse(bp, products);
   }
 };

include/ur_modern_driver/ur/robot_mode.h

@@ -1,14 +1,15 @@
 #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;
@@ -21,7 +22,8 @@ public:
   static const size_t SIZE = sizeof(uint64_t) + sizeof(uint8_t) * 6;
 };
 
-enum class robot_mode_V1_X : uint8_t {
+enum class robot_mode_V1_X : uint8_t
+{
   ROBOT_RUNNING_MODE = 0,
   ROBOT_FREEDRIVE_MODE = 1,
   ROBOT_READY_MODE = 2,
@@ -35,24 +37,23 @@ enum class robot_mode_V1_X : uint8_t {
   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 parseWith(BinParser& bp);
-    virtual bool consume_with(URStatePacketConsumer& consumer);
+  virtual bool consumeWith(URStatePacketConsumer& consumer);
 
   bool security_stopped;
   robot_mode_V1_X robot_mode;
   double speed_fraction;
 
-    static const size_t SIZE = SharedRobotModeData::SIZE
+  static const size_t SIZE = SharedRobotModeData::SIZE + sizeof(uint8_t) + sizeof(robot_mode_V1_X) + sizeof(double);
-        + sizeof(uint8_t)
-        + sizeof(robot_mode_V1_X)
-        + sizeof(double);
 
   static_assert(RobotModeData_V1_X::SIZE == 24, "RobotModeData_V1_X has missmatched size");
 };
 
-enum class robot_mode_V3_X : uint8_t {
+enum class robot_mode_V3_X : uint8_t
+{
   DISCONNECTED = 0,
   CONFIRM_SAFETY = 1,
   BOOTING = 2,
@@ -64,17 +65,19 @@ enum class robot_mode_V3_X : uint8_t {
   UPDATING_FIRMWARE = 8
 };
 
-enum class robot_control_mode_V3_X : uint8_t {
+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 parseWith(BinParser& bp);
-    virtual bool consume_with(URStatePacketConsumer& consumer);
+  virtual bool consumeWith(URStatePacketConsumer& consumer);
 
   bool protective_stopped;
 
@@ -84,25 +87,21 @@ public:
   double target_speed_fraction;
   double speed_scaling;
 
-    static const size_t SIZE = SharedRobotModeData::SIZE
+  static const size_t SIZE = SharedRobotModeData::SIZE + sizeof(uint8_t) + sizeof(robot_mode_V3_X) +
-        + sizeof(uint8_t)
+                             sizeof(robot_control_mode_V3_X) + sizeof(double) + sizeof(double);
-        + sizeof(robot_mode_V3_X)
-        + sizeof(robot_control_mode_V3_X)
-        + sizeof(double)
-        + sizeof(double);
 
   static_assert(RobotModeData_V3_0__1::SIZE == 33, "RobotModeData_V3_0__1 has missmatched size");
 };
 
-class RobotModeData_V3_2 : public RobotModeData_V3_0__1 {
+class RobotModeData_V3_2 : public RobotModeData_V3_0__1
+{
 public:
-    virtual bool parse_with(BinParser& bp);
+  virtual bool parseWith(BinParser& bp);
-    virtual bool consume_with(URStatePacketConsumer& consumer);
+  virtual bool consumeWith(URStatePacketConsumer& consumer);
 
   double target_speed_fraction_limit;
 
-    static const size_t SIZE = RobotModeData_V3_0__1::SIZE
+  static const size_t SIZE = RobotModeData_V3_0__1::SIZE + sizeof(double);
-        + sizeof(double);
 
   static_assert(RobotModeData_V3_2::SIZE == 41, "RobotModeData_V3_2 has missmatched size");
 };

include/ur_modern_driver/ur/rt_parser.h

@@ -1,19 +1,21 @@
 #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)
   {
     int32_t packet_size = bp.peek<int32_t>();
 
-        if (!bp.check_size(packet_size)) {
+    if (!bp.checkSize(packet_size))
+    {
       LOG_ERROR("Buffer len shorter than expected packet length");
       return false;
     }
@@ -21,7 +23,7 @@ public:
     bp.parse(packet_size);  // consumes the peeked data
 
     std::unique_ptr<RTPacket> packet(new T);
-        if (!packet->parse_with(bp))
+    if (!packet->parseWith(bp))
       return false;
 
     results.push_back(std::move(packet));

include/ur_modern_driver/ur/rt_state.h

@@ -1,20 +1,22 @@
 #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 parseWith(BinParser& bp) = 0;
-    virtual bool consume_with(URRTPacketConsumer& consumer) = 0;
+  virtual bool consumeWith(URRTPacketConsumer& consumer) = 0;
 };
 
-class RTShared {
+class RTShared
+{
 protected:
   void parse_shared1(BinParser& bp);
   void parse_shared2(BinParser& bp);
@@ -46,42 +48,41 @@ public:
   double controller_time;
   double robot_mode;
 
-    static const size_t SIZE = sizeof(double) * 3
+  static const size_t SIZE =
-        + sizeof(double[6]) * 10
+      sizeof(double) * 3 + sizeof(double[6]) * 10 + sizeof(cartesian_coord_t) * 2 + sizeof(uint64_t);
-        + 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);
+  bool parseWith(BinParser& bp);
-    virtual bool consume_with(URRTPacketConsumer& consumer);
+  virtual bool consumeWith(URRTPacketConsumer& consumer);
 
   double3_t tool_accelerometer_values;
 
-    static const size_t SIZE = RTShared::SIZE
+  static const size_t SIZE = RTShared::SIZE + sizeof(double3_t);
-        + sizeof(double3_t);
 
   static_assert(RTState_V1_6__7::SIZE == 632, "RTState_V1_6__7 has mismatched size!");
 };
 
-class RTState_V1_8 : public RTState_V1_6__7 {
+class RTState_V1_8 : public RTState_V1_6__7
+{
 public:
-    bool parse_with(BinParser& bp);
+  bool parseWith(BinParser& bp);
-    virtual bool consume_with(URRTPacketConsumer& consumer);
+  virtual bool consumeWith(URRTPacketConsumer& consumer);
 
   double joint_modes[6];
 
-    static const size_t SIZE = RTState_V1_6__7::SIZE
+  static const size_t SIZE = RTState_V1_6__7::SIZE + sizeof(double[6]);
-        + sizeof(double[6]);
 
   static_assert(RTState_V1_8::SIZE == 680, "RTState_V1_8 has mismatched size!");
 };
 
-class RTState_V3_0__1 : public RTShared, public RTPacket {
+class RTState_V3_0__1 : public RTShared, public RTPacket
+{
 public:
-    bool parse_with(BinParser& bp);
+  bool parseWith(BinParser& bp);
-    virtual bool consume_with(URRTPacketConsumer& consumer);
+  virtual bool consumeWith(URRTPacketConsumer& consumer);
 
   double i_control[6];
   cartesian_coord_t tool_vector_target;
@@ -97,26 +98,22 @@ public:
   double i_robot;
   double v_actual[6];
 
-    static const size_t SIZE = RTShared::SIZE
+  static const size_t SIZE =
-        + sizeof(double[6]) * 3
+      RTShared::SIZE + sizeof(double[6]) * 3 + sizeof(double3_t) + sizeof(cartesian_coord_t) * 2 + sizeof(double) * 6;
-        + sizeof(double3_t)
-        + sizeof(cartesian_coord_t) * 2
-        + sizeof(double) * 6;
 
   static_assert(RTState_V3_0__1::SIZE == 920, "RTState_V3_0__1 has mismatched size!");
 };
 
-class RTState_V3_2__3 : public RTState_V3_0__1 {
+class RTState_V3_2__3 : public RTState_V3_0__1
+{
 public:
-    bool parse_with(BinParser& bp);
+  bool parseWith(BinParser& bp);
-    virtual bool consume_with(URRTPacketConsumer& consumer);
+  virtual bool consumeWith(URRTPacketConsumer& consumer);
 
   uint64_t digital_outputs;
   double program_state;
 
-    static const size_t SIZE = RTState_V3_0__1::SIZE
+  static const size_t SIZE = RTState_V3_0__1::SIZE + sizeof(uint64_t) + sizeof(double);
-        + sizeof(uint64_t)
-        + sizeof(double);
 
   static_assert(RTState_V3_2__3::SIZE == 936, "RTState_V3_2__3 has mismatched size!");
 };

include/ur_modern_driver/ur/state.h

@@ -1,12 +1,13 @@
 #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 {
+enum class package_type : uint8_t
+{
   ROBOT_MODE_DATA = 0,
   JOINT_DATA = 1,
   TOOL_DATA = 2,
@@ -19,7 +20,8 @@ enum class package_type : uint8_t {
   CALIBRATION_DATA = 9
 };
 
-enum class message_type : uint8_t {
+enum class message_type : uint8_t
+{
   ROBOT_STATE = 16,
   ROBOT_MESSAGE = 20,
   PROGRAM_STATE_MESSAGE = 25
@@ -27,8 +29,9 @@ enum class message_type : uint8_t {
 
 class URStatePacketConsumer;
 
-class StatePacket {
+class StatePacket
+{
 public:
-    virtual bool parse_with(BinParser& bp) = 0;
+  virtual bool parseWith(BinParser& bp) = 0;
-    virtual bool consume_with(URStatePacketConsumer& consumer) = 0;
+  virtual bool consumeWith(URStatePacketConsumer& consumer) = 0;
 };

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,14 +7,15 @@
 #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)
   {
-        switch (type) {
+    switch (type)
+    {
       case package_type::ROBOT_MODE_DATA:
         return new RMD;
       case package_type::MASTERBOARD_DATA:
@@ -31,18 +33,22 @@ public:
     bp.parse(packet_size);
     bp.parse(type);
 
-        if (type != message_type::ROBOT_STATE) {
+    if (type != message_type::ROBOT_STATE)
+    {
       LOG_WARN("Invalid message type recieved: %u", static_cast<uint8_t>(type));
       return false;
     }
 
-        while (!bp.empty()) {
+    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;
       }
       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;
       }
@@ -55,20 +61,23 @@ public:
 
       std::unique_ptr<StatePacket> packet(from_type(type));
 
-            if (packet == nullptr) {
+      if (packet == nullptr)
+      {
         sbp.consume();
         LOG_INFO("Skipping sub-packet of type %d", type);
         continue;
       }
 
-            if (!packet->parse_with(sbp)) {
+      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()) {
+      if (!sbp.empty())
+      {
         LOG_ERROR("Sub-package was not parsed completely!");
         return false;
       }

include/ur_modern_driver/ur/stream.h

@@ -1,26 +1,23 @@
 #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;
+  int socket_fd_ = -1;
-    std::string _host;
+  std::string host_;
-    int _port;
+  int port_;
 
-    std::atomic<bool> _initialized;
+  std::atomic<bool> initialized_;
-    std::atomic<bool> _stopping;
+  std::atomic<bool> stopping_;
 
 public:
-    URStream(std::string& host, int port)
+  URStream(std::string& host, int port) : host_(host), port_(port), initialized_(false), stopping_(false)
-        : _host(host)
-        , _port(port)
-        , _initialized(false)
-        , _stopping(false)
   {
   }
 

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,11 +30,18 @@
 #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_;

include/ur_modern_driver/ur_driver.h

@@ -19,21 +19,22 @@
 #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_;
@@ -56,18 +57,16 @@ public:
   UrRealtimeCommunication* rt_interface_;
   UrCommunication* sec_interface_;
 
-    UrDriver(std::condition_variable& rt_msg_cond,
+  UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host,
-        std::condition_variable& msg_cond, std::string host,
+           unsigned int reverse_port = 50007, double servoj_time = 0.016, unsigned int safety_count_max = 12,
-        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_time_step = 0.08, double min_payload = 0., double max_payload = 1.,
-        double max_payload = 1., double servoj_lookahead_time = 0.03, double servoj_gain = 300.);
+           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,
+  void setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc = 100.);
-        double q5, double acc = 100.);
 
-    bool doTraj(std::vector<double> inp_timestamps,
+  bool doTraj(std::vector<double> inp_timestamps, std::vector<std::vector<double>> inp_positions,
-        std::vector<std::vector<double> > inp_positions,
               std::vector<std::vector<double>> inp_velocities);
   void servoj(std::vector<double> positions, int keepalive = 1);
 
@@ -77,8 +76,7 @@ public:
   bool openServo();
   void closeServo(std::vector<double> positions);
 
-    std::vector<double> interp_cubic(double t, double T,
+  std::vector<double> interp_cubic(double t, double T, std::vector<double> p0_pos, std::vector<double> p1_pos,
-        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();

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,15 +65,19 @@
 #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
@@ -95,8 +96,7 @@ public:
 
   void setMaxVelChange(double inp);
 
-    bool canSwitch(
+  bool canSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
-        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);

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,11 +32,17 @@
 #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_;
@@ -60,12 +61,10 @@ public:
   bool connected_;
   RobotStateRT* robot_state_;
 
-    UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host,
+  UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host, unsigned int safety_count_max = 12);
-        unsigned int safety_count_max = 12);
   bool start();
   void halt();
-    void setSpeed(double q0, double q1, double q2, double q3, double q4,
+  void setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc = 100.);
-        double q5, double acc = 100.);
   void addCommandToQueue(std::string inp);
   void setSafetyCountMax(uint inp);
   std::string getLocalIp();

src/robot_state.cpp

@@ -38,21 +38,26 @@ 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) {
+  while (buf_length > offset)
+  {
     int len;
     unsigned char message_type;
     memcpy(&len, &buf[offset], sizeof(len));
     len = ntohl(len);
-        if (len + offset > buf_length) {
+    if (len + offset > buf_length)
+    {
       return;
     }
     memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type));
-        switch (message_type) {
+    switch (message_type)
+    {
       case messageType::ROBOT_MESSAGE:
-            RobotState::unpackRobotMessage(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType
+        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
+        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...
@@ -64,8 +69,7 @@ void RobotState::unpack(uint8_t* buf, unsigned int buf_length)
   return;
 }
 
-void RobotState::unpackRobotMessage(uint8_t* buf, unsigned int offset,
+void RobotState::unpackRobotMessage(uint8_t* buf, unsigned int offset, uint32_t len)
-    uint32_t len)
 {
   offset += 5;
   uint64_t timestamp;
@@ -76,7 +80,8 @@ void RobotState::unpackRobotMessage(uint8_t* buf, unsigned int offset,
   offset += sizeof(source);
   memcpy(&robot_message_type, &buf[offset], sizeof(robot_message_type));
   offset += sizeof(robot_message_type);
-    switch (robot_message_type) {
+  switch (robot_message_type)
+  {
     case robotMessageType::ROBOT_MESSAGE_VERSION:
       val_lock_.lock();
       version_msg_.timestamp = timestamp;
@@ -90,18 +95,18 @@ void RobotState::unpackRobotMessage(uint8_t* buf, unsigned int offset,
   }
 }
 
-void RobotState::unpackRobotState(uint8_t* buf, unsigned int offset,
+void RobotState::unpackRobotState(uint8_t* buf, unsigned int offset, uint32_t len)
-    uint32_t len)
 {
   offset += 5;
-    while (offset < len) {
+  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)],
+    memcpy(&package_type, &buf[offset + sizeof(length)], sizeof(package_type));
-            sizeof(package_type));
+    switch (package_type)
-        switch (package_type) {
+    {
       case packageType::ROBOT_MODE_DATA:
         val_lock_.lock();
         RobotState::unpackRobotMode(buf, offset + 5);
@@ -122,29 +127,24 @@ void RobotState::unpackRobotState(uint8_t* buf, unsigned int offset,
   pMsg_cond_->notify_all();
 }
 
-void RobotState::unpackRobotMessageVersion(uint8_t* buf, unsigned int offset,
+void RobotState::unpackRobotMessageVersion(uint8_t* buf, unsigned int offset, uint32_t len)
-    uint32_t len)
 {
-    memcpy(&version_msg_.project_name_size, &buf[offset],
+  memcpy(&version_msg_.project_name_size, &buf[offset], sizeof(version_msg_.project_name_size));
-        sizeof(version_msg_.project_name_size));
   offset += sizeof(version_msg_.project_name_size);
-    memcpy(&version_msg_.project_name, &buf[offset],
+  memcpy(&version_msg_.project_name, &buf[offset], sizeof(char) * version_msg_.project_name_size);
-        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],
+  memcpy(&version_msg_.major_version, &buf[offset], sizeof(version_msg_.major_version));
-        sizeof(version_msg_.major_version));
   offset += sizeof(version_msg_.major_version);
-    memcpy(&version_msg_.minor_version, &buf[offset],
+  memcpy(&version_msg_.minor_version, &buf[offset], sizeof(version_msg_.minor_version));
-        sizeof(version_msg_.minor_version));
   offset += sizeof(version_msg_.minor_version);
-    memcpy(&version_msg_.svn_revision, &buf[offset],
+  memcpy(&version_msg_.svn_revision, &buf[offset], sizeof(version_msg_.svn_revision));
-        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) {
+  if (version_msg_.major_version < 2)
+  {
     robot_mode_running_ = robotStateTypeV18::ROBOT_RUNNING_MODE;
   }
 }
@@ -200,9 +200,9 @@ void RobotState::unpackRobotMode(uint8_t* buf, unsigned int offset)
   memcpy(&robot_mode_.robotMode, &buf[offset], sizeof(robot_mode_.robotMode));
   offset += sizeof(robot_mode_.robotMode);
   uint64_t temp;
-    if (RobotState::getVersion() > 2.) {
+  if (RobotState::getVersion() > 2.)
-        memcpy(&robot_mode_.controlMode, &buf[offset],
+  {
-            sizeof(robot_mode_.controlMode));
+    memcpy(&robot_mode_.controlMode, &buf[offset], sizeof(robot_mode_.controlMode));
     offset += sizeof(robot_mode_.controlMode);
     memcpy(&temp, &buf[offset], sizeof(temp));
     offset += sizeof(temp);
@@ -213,10 +213,10 @@ void RobotState::unpackRobotMode(uint8_t* buf, unsigned int offset)
   robot_mode_.speedScaling = RobotState::ntohd(temp);
 }
 
-void RobotState::unpackRobotStateMasterboard(uint8_t* buf,
+void RobotState::unpackRobotStateMasterboard(uint8_t* buf, unsigned int offset)
-    unsigned int offset)
+{
+  if (RobotState::getVersion() < 3.0)
   {
-    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);
@@ -224,22 +224,20 @@ void RobotState::unpackRobotStateMasterboard(uint8_t* buf,
     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],
+  else
-            sizeof(mb_data_.digitalInputBits));
+  {
+    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],
+    memcpy(&mb_data_.digitalOutputBits, &buf[offset], sizeof(mb_data_.digitalOutputBits));
-            sizeof(mb_data_.digitalOutputBits));
     offset += sizeof(mb_data_.digitalOutputBits);
     mb_data_.digitalOutputBits = ntohl(mb_data_.digitalOutputBits);
   }
 
-    memcpy(&mb_data_.analogInputRange0, &buf[offset],
+  memcpy(&mb_data_.analogInputRange0, &buf[offset], sizeof(mb_data_.analogInputRange0));
-        sizeof(mb_data_.analogInputRange0));
   offset += sizeof(mb_data_.analogInputRange0);
-    memcpy(&mb_data_.analogInputRange1, &buf[offset],
+  memcpy(&mb_data_.analogInputRange1, &buf[offset], sizeof(mb_data_.analogInputRange1));
-        sizeof(mb_data_.analogInputRange1));
   offset += sizeof(mb_data_.analogInputRange1);
   uint64_t temp;
   memcpy(&temp, &buf[offset], sizeof(temp));
@@ -248,11 +246,9 @@ void RobotState::unpackRobotStateMasterboard(uint8_t* buf,
   memcpy(&temp, &buf[offset], sizeof(temp));
   offset += sizeof(temp);
   mb_data_.analogInput1 = RobotState::ntohd(temp);
-    memcpy(&mb_data_.analogOutputDomain0, &buf[offset],
+  memcpy(&mb_data_.analogOutputDomain0, &buf[offset], sizeof(mb_data_.analogOutputDomain0));
-        sizeof(mb_data_.analogOutputDomain0));
   offset += sizeof(mb_data_.analogOutputDomain0);
-    memcpy(&mb_data_.analogOutputDomain1, &buf[offset],
+  memcpy(&mb_data_.analogOutputDomain1, &buf[offset], sizeof(mb_data_.analogOutputDomain1));
-        sizeof(mb_data_.analogOutputDomain1));
   offset += sizeof(mb_data_.analogOutputDomain1);
   memcpy(&temp, &buf[offset], sizeof(temp));
   offset += sizeof(temp);
@@ -261,41 +257,36 @@ void RobotState::unpackRobotStateMasterboard(uint8_t* buf,
   offset += sizeof(temp);
   mb_data_.analogOutput1 = RobotState::ntohd(temp);
 
-    memcpy(&mb_data_.masterBoardTemperature, &buf[offset],
+  memcpy(&mb_data_.masterBoardTemperature, &buf[offset], sizeof(mb_data_.masterBoardTemperature));
-        sizeof(mb_data_.masterBoardTemperature));
   offset += sizeof(mb_data_.masterBoardTemperature);
   mb_data_.masterBoardTemperature = ntohl(mb_data_.masterBoardTemperature);
-    memcpy(&mb_data_.robotVoltage48V, &buf[offset],
+  memcpy(&mb_data_.robotVoltage48V, &buf[offset], sizeof(mb_data_.robotVoltage48V));
-        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],
+  memcpy(&mb_data_.masterIOCurrent, &buf[offset], sizeof(mb_data_.masterIOCurrent));
-        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],
+  memcpy(&mb_data_.masterOnOffState, &buf[offset], sizeof(mb_data_.masterOnOffState));
-        sizeof(mb_data_.masterOnOffState));
   offset += sizeof(mb_data_.masterOnOffState);
 
-    memcpy(&mb_data_.euromap67InterfaceInstalled, &buf[offset],
+  memcpy(&mb_data_.euromap67InterfaceInstalled, &buf[offset], sizeof(mb_data_.euromap67InterfaceInstalled));
-        sizeof(mb_data_.euromap67InterfaceInstalled));
   offset += sizeof(mb_data_.euromap67InterfaceInstalled);
-    if (mb_data_.euromap67InterfaceInstalled != 0) {
+  if (mb_data_.euromap67InterfaceInstalled != 0)
-        memcpy(&mb_data_.euromapInputBits, &buf[offset],
+  {
-            sizeof(mb_data_.euromapInputBits));
+    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],
+    memcpy(&mb_data_.euromapOutputBits, &buf[offset], sizeof(mb_data_.euromapOutputBits));
-            sizeof(mb_data_.euromapOutputBits));
     offset += sizeof(mb_data_.euromapOutputBits);
     mb_data_.euromapOutputBits = ntohl(mb_data_.euromapOutputBits);
-        if (RobotState::getVersion() < 3.0) {
+    if (RobotState::getVersion() < 3.0)
+    {
       int16_t euromap_voltage, euromap_current;
       memcpy(&euromap_voltage, &buf[offset], sizeof(euromap_voltage));
       offset += sizeof(euromap_voltage);
@@ -303,13 +294,13 @@ void RobotState::unpackRobotStateMasterboard(uint8_t* buf,
       offset += sizeof(euromap_current);
       mb_data_.euromapVoltage = ntohs(euromap_voltage);
       mb_data_.euromapCurrent = ntohs(euromap_current);
-        } else {
+    }
-            memcpy(&mb_data_.euromapVoltage, &buf[offset],
+    else
-                sizeof(mb_data_.euromapVoltage));
+    {
+      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],
+      memcpy(&mb_data_.euromapCurrent, &buf[offset], sizeof(mb_data_.euromapCurrent));
-                sizeof(mb_data_.euromapCurrent));
       offset += sizeof(mb_data_.euromapCurrent);
       mb_data_.euromapCurrent = ntohl(mb_data_.euromapCurrent);
     }
@@ -320,8 +311,7 @@ double RobotState::getVersion()
 {
   double ver;
   val_lock_.lock();
-    ver = version_msg_.major_version + 0.1 * version_msg_.minor_version
+  ver = version_msg_.major_version + 0.1 * version_msg_.minor_version + .0000001 * version_msg_.svn_revision;
-        + .0000001 * version_msg_.svn_revision;
   val_lock_.unlock();
   return ver;
 }
@@ -394,7 +384,8 @@ unsigned char RobotState::getRobotMode()
 }
 bool RobotState::isReady()
 {
-    if (robot_mode_.robotMode == robot_mode_running_) {
+  if (robot_mode_.robotMode == robot_mode_running_)
+  {
     return true;
   }
   return false;

src/robot_state_RT.cpp

@@ -88,12 +88,12 @@ double RobotStateRT::ntohd(uint64_t nf)
   return x;
 }
 
-std::vector<double> RobotStateRT::unpackVector(uint8_t* buf, int start_index,
+std::vector<double> RobotStateRT::unpackVector(uint8_t* buf, int start_index, int nr_of_vals)
-    int nr_of_vals)
 {
   uint64_t q;
   std::vector<double> ret;
-    for (int i = 0; i < nr_of_vals; i++) {
+  for (int i = 0; i < nr_of_vals; i++)
+  {
     memcpy(&q, &buf[start_index + i * sizeof(q)], sizeof(q));
     ret.push_back(ntohd(q));
   }
@@ -103,7 +103,8 @@ std::vector<double> RobotStateRT::unpackVector(uint8_t* buf, int start_index,
 std::vector<bool> RobotStateRT::unpackDigitalInputBits(int64_t data)
 {
   std::vector<bool> ret;
-    for (int i = 0; i < 64; i++) {
+  for (int i = 0; i < 64; i++)
+  {
     ret.push_back((data & (1 << i)) >> i);
   }
   return ret;
@@ -362,24 +363,34 @@ void RobotStateRT::unpack(uint8_t* buf)
 
   // Check the correct message length is received
   bool len_good = true;
-    if (version_ >= 1.6 && version_ < 1.7) { //v1.6
+  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
+  }
+  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
+  }
+  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
+  }
+  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
+  }
+  else if (version_ >= 3.2 && version_ < 3.3)
+  {  // v3.2
     if (len != 1060)
       len_good = false;
   }
 
-    if (!len_good) {
+  if (!len_good)
+  {
     printf("Wrong length of message on RT interface: %i\n", len);
     val_lock_.unlock();
     return;
@@ -404,7 +415,8 @@ void RobotStateRT::unpack(uint8_t* buf)
   offset += sizeof(double) * 6;
   i_actual_ = unpackVector(buf, offset, 6);
   offset += sizeof(double) * 6;
-    if (version_ <= 1.9) {
+  if (version_ <= 1.9)
+  {
     if (version_ > 1.6)
       tool_accelerometer_values_ = unpackVector(buf, offset, 3);
     offset += sizeof(double) * (3 + 15);
@@ -413,7 +425,9 @@ void RobotStateRT::unpack(uint8_t* buf)
     tool_vector_actual_ = unpackVector(buf, offset, 6);
     offset += sizeof(double) * 6;
     tcp_speed_actual_ = unpackVector(buf, offset, 6);
-    } else {
+  }
+  else
+  {
     i_control_ = unpackVector(buf, offset, 6);
     offset += sizeof(double) * 6;
     tool_vector_actual_ = unpackVector(buf, offset, 6);
@@ -435,16 +449,19 @@ void RobotStateRT::unpack(uint8_t* buf)
   offset += sizeof(double) * 6;
   memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
   controller_timer_ = ntohd(unpack_to);
-    if (version_ > 1.6) {
+  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) {
+    if (version_ > 1.7)
+    {
       offset += sizeof(double);
       joint_modes_ = unpackVector(buf, offset, 6);
     }
   }
-    if (version_ > 1.8) {
+  if (version_ > 1.8)
+  {
     offset += sizeof(double) * 6;
     memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
     safety_mode_ = ntohd(unpack_to);

src/ros/rt_publisher.cpp

@@ -1,27 +1,30 @@
 #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;
+  joint_msg.name = joint_names_;
 
-    for (auto const& q : packet.q_actual) {
+  for (auto const& q : packet.q_actual)
+  {
     joint_msg.position.push_back(q);
   }
-    for (auto const& qd : packet.qd_actual) {
+  for (auto const& qd : packet.qd_actual)
+  {
     joint_msg.velocity.push_back(qd);
   }
-    for (auto const& i : packet.i_actual) {
+  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;
 }
 
-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;
@@ -32,15 +35,15 @@ bool RTPublisher::publish_wrench(RTShared& packet, Time& t)
   wrench_msg.wrench.torque.y = packet.tcp_force[4];
   wrench_msg.wrench.torque.z = packet.tcp_force[5];
 
-    _wrench_pub.publish(wrench_msg);
+  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.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;
@@ -48,13 +51,12 @@ bool RTPublisher::publish_tool(RTShared& packet, Time& t)
   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);
+  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;
 
   Transform transform;
@@ -64,29 +66,33 @@ bool RTPublisher::publish_transform(RTShared& packet, Time& t)
   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) {
+  if (angle < 1e-16)
+  {
     quat.setValue(0, 0, 0, 1);
-    } else {
+  }
+  else
+  {
     quat.setRotation(Vector3(tv.rotation.x / angle, tv.rotation.y / angle, tv.rotation.z / angle), angle);
   }
 
   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;
 }
 
-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++)
   {
-    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.header.frame_id = joint_names_[i];
     msg.temperature = packet.motor_temperatures[i];
 
-        _joint_temperature_pub.publish(msg);
+    joint_temperature_pub_.publish(msg);
   }
   return true;
 }
@@ -94,11 +100,8 @@ bool RTPublisher::publish_temperature(RTShared& packet, Time& t)
 bool RTPublisher::publish(RTShared& packet)
 {
   Time time = Time::now();
-    return publish_joints(packet, time)
+  return publishJoints(packet, time) && publishWrench(packet, time) && publishTool(packet, time) &&
-        && publish_wrench(packet, time)
+         publishTransform(packet, time) && publishTemperature(packet, time);
-        && publish_tool(packet, time)
-        && publish_transform(packet, time)
-        && publish_temperature(packet, time);
 }
 
 bool RTPublisher::consume(RTState_V1_6__7& state)

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,7 +21,8 @@ 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;
@@ -33,7 +34,8 @@ public:
 
 bool parse_args(ProgArgs& args)
 {
-    if (!ros::param::get(IP_ADDR_ARG, args.host)) {
+  if (!ros::param::get(IP_ADDR_ARG, args.host))
+  {
     LOG_ERROR("robot_ip_address parameter must be set!");
     return false;
   }
@@ -50,12 +52,13 @@ int main(int argc, char** argv)
   ros::init(argc, argv, "ur_driver");
 
   ProgArgs args;
-    if (!parse_args(args)) {
+  if (!parse_args(args))
+  {
     return EXIT_FAILURE;
   }
 
   URFactory factory(args.host);
-    auto parser = factory.get_rt_parser();
+  auto parser = factory.getRTParser();
 
   URStream s2(args.host, 30003);
   URProducer<RTPacket> p2(s2, *parser);
@@ -65,7 +68,8 @@ int main(int argc, char** argv)
 
   pl.run();
 
-    while (ros::ok()) {
+  while (ros::ok())
+  {
     std::this_thread::sleep_for(std::chrono::milliseconds(33));
   }
 

src/ur/master_board.cpp

@@ -1,7 +1,7 @@
 #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);
@@ -18,22 +18,23 @@ bool SharedMasterBoardData::parse_with(BinParser& bp)
   return true;
 }
 
-bool MasterBoardData_V1_X::parse_with(BinParser& bp)
+bool MasterBoardData_V1_X::parseWith(BinParser& bp)
 {
-    if (!bp.check_size<MasterBoardData_V1_X>())
+  if (!bp.checkSize<MasterBoardData_V1_X>())
     return false;
 
   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);
 
-    if (euromap67_interface_installed) {
+  if (euromap67_interface_installed)
-        if (!bp.check_size(MasterBoardData_V1_X::EURO_SIZE))
+  {
+    if (!bp.checkSize(MasterBoardData_V1_X::EURO_SIZE))
       return false;
 
     bp.parse(euromap_voltage);
@@ -43,22 +44,23 @@ bool MasterBoardData_V1_X::parse_with(BinParser& bp)
   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>())
+  if (!bp.checkSize<MasterBoardData_V3_0__1>())
     return false;
 
   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);
 
-    if (euromap67_interface_installed) {
+  if (euromap67_interface_installed)
-        if (!bp.check_size(MasterBoardData_V3_0__1::EURO_SIZE))
+  {
+    if (!bp.checkSize(MasterBoardData_V3_0__1::EURO_SIZE))
       return false;
 
     bp.parse(euromap_voltage);
@@ -70,12 +72,12 @@ bool MasterBoardData_V3_0__1::parse_with(BinParser& bp)
   return true;
 }
 
-bool MasterBoardData_V3_2::parse_with(BinParser& bp)
+bool MasterBoardData_V3_2::parseWith(BinParser& bp)
 {
-    if (!bp.check_size<MasterBoardData_V3_2>())
+  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);
@@ -83,15 +85,15 @@ bool MasterBoardData_V3_2::parse_with(BinParser& bp)
   return true;
 }
 
-bool MasterBoardData_V1_X::consume_with(URStatePacketConsumer& consumer)
+bool MasterBoardData_V1_X::consumeWith(URStatePacketConsumer& consumer)
 {
   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);
 }
-bool MasterBoardData_V3_2::consume_with(URStatePacketConsumer& consumer)
+bool MasterBoardData_V3_2::consumeWith(URStatePacketConsumer& consumer)
 {
   return consumer.consume(*this);
 }

src/ur/messages.cpp

@@ -1,9 +1,8 @@
 #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);
@@ -14,7 +13,7 @@ bool VersionMessage::parse_with(BinParser& bp)
   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);
 }

src/ur/robot_mode.cpp

@@ -1,7 +1,7 @@
 #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);
@@ -11,12 +11,12 @@ bool SharedRobotModeData::parse_with(BinParser& bp)
   return true;
 }
 
-bool RobotModeData_V1_X::parse_with(BinParser& bp)
+bool RobotModeData_V1_X::parseWith(BinParser& bp)
 {
-    if (!bp.check_size<RobotModeData_V1_X>())
+  if (!bp.checkSize<RobotModeData_V1_X>())
     return false;
 
-    SharedRobotModeData::parse_with(bp);
+  SharedRobotModeData::parseWith(bp);
 
   bp.parse(security_stopped);
   bp.parse(program_running);
@@ -27,12 +27,12 @@ bool RobotModeData_V1_X::parse_with(BinParser& bp)
   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>())
+  if (!bp.checkSize<RobotModeData_V3_0__1>())
     return false;
 
-    SharedRobotModeData::parse_with(bp);
+  SharedRobotModeData::parseWith(bp);
 
   bp.parse(protective_stopped);
   bp.parse(program_running);
@@ -45,27 +45,27 @@ bool RobotModeData_V3_0__1::parse_with(BinParser& bp)
   return true;
 }
 
-bool RobotModeData_V3_2::parse_with(BinParser& bp)
+bool RobotModeData_V3_2::parseWith(BinParser& bp)
 {
-    if (!bp.check_size<RobotModeData_V3_2>())
+  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);
 
   return true;
 }
 
-bool RobotModeData_V1_X::consume_with(URStatePacketConsumer& consumer)
+bool RobotModeData_V1_X::consumeWith(URStatePacketConsumer& consumer)
 {
   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);
 }
-bool RobotModeData_V3_2::consume_with(URStatePacketConsumer& consumer)
+bool RobotModeData_V3_2::consumeWith(URStatePacketConsumer& consumer)
 {
   return consumer.consume(*this);
 }

src/ur/rt_state.cpp

@@ -23,9 +23,9 @@ void RTShared::parse_shared2(BinParser& bp)
   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>())
+  if (!bp.checkSize<RTState_V1_6__7>())
     return false;
 
   parse_shared1(bp);
@@ -41,21 +41,21 @@ bool RTState_V1_6__7::parse_with(BinParser& bp)
   return true;
 }
 
-bool RTState_V1_8::parse_with(BinParser& bp)
+bool RTState_V1_8::parseWith(BinParser& bp)
 {
-    if (!bp.check_size<RTState_V1_8>())
+  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);
 
   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>())
+  if (!bp.checkSize<RTState_V3_0__1>())
     return false;
 
   parse_shared1(bp);
@@ -86,12 +86,12 @@ bool RTState_V3_0__1::parse_with(BinParser& bp)
   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>())
+  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);
@@ -99,19 +99,19 @@ bool RTState_V3_2__3::parse_with(BinParser& bp)
   return true;
 }
 
-bool RTState_V1_6__7::consume_with(URRTPacketConsumer& consumer)
+bool RTState_V1_6__7::consumeWith(URRTPacketConsumer& consumer)
 {
   return consumer.consume(*this);
 }
-bool RTState_V1_8::consume_with(URRTPacketConsumer& consumer)
+bool RTState_V1_8::consumeWith(URRTPacketConsumer& consumer)
 {
   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);
 }
-bool RTState_V3_2__3::consume_with(URRTPacketConsumer& consumer)
+bool RTState_V3_2__3::consumeWith(URRTPacketConsumer& consumer)
 {
   return consumer.consume(*this);
 }

src/ur/state.cpp

@@ -4,17 +4,17 @@
 // 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;
         }
 
         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;
         }
@@ -43,12 +43,12 @@ 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:

src/ur/stream.cpp

@@ -1,22 +1,22 @@
-#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)
+  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
 
-    std::string service = std::to_string(_port);
+  std::string service = std::to_string(port_);
   struct addrinfo hints, *result;
   std::memset(&hints, 0, sizeof(hints));
 
@@ -24,20 +24,23 @@ bool URStream::connect()
   hints.ai_socktype = SOCK_STREAM;
   hints.ai_flags = AI_PASSIVE;
 
-    if (getaddrinfo(_host.c_str(), service.c_str(), &hints, &result) != 0) {
+  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) {
+  for (struct addrinfo* p = result; p != nullptr; p = p->ai_next)
-        _socket_fd = socket(p->ai_family, p->ai_socktype, p->ai_protocol);
+  {
+    socket_fd_ = socket(p->ai_family, p->ai_socktype, p->ai_protocol);
 
-        if (_socket_fd == -1) //socket error?
+    if (socket_fd_ == -1)  // socket error?
       continue;
 
-        if (::connect(_socket_fd, p->ai_addr, p->ai_addrlen) != 0) {
+    if (::connect(socket_fd_, p->ai_addr, p->ai_addrlen) != 0)
-            if (_stopping)
+    {
+      if (stopping_)
         break;
       else
         continue;  // try next addrinfo if connect fails
@@ -45,36 +48,36 @@ bool URStream::connect()
 
     // 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));
+    setsockopt(socket_fd_, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(flag));
-        _initialized = true;
+    initialized_ = true;
     LOG_INFO("Connection successfully established");
     break;
   }
 
   freeaddrinfo(result);
-    if (!_initialized)
+  if (!initialized_)
     LOG_ERROR("Connection failed");
 
-    return _initialized;
+  return initialized_;
 }
 
 void URStream::disconnect()
 {
-    if (!_initialized || _stopping)
+  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;
+  stopping_ = true;
-    close(_socket_fd);
+  close(socket_fd_);
-    _initialized = false;
+  initialized_ = false;
 }
 
 ssize_t URStream::send(uint8_t* buf, size_t buf_len)
 {
-    if (!_initialized)
+  if (!initialized_)
     return -1;
-    if (_stopping)
+  if (stopping_)
     return 0;
 
   size_t total = 0;
@@ -82,10 +85,11 @@ ssize_t URStream::send(uint8_t* buf, size_t buf_len)
 
   // TODO: handle reconnect?
   // handle partial sends
-    while (total < buf_len) {
+  while (total < buf_len)
-        ssize_t sent = ::send(_socket_fd, buf + total, remaining, 0);
+  {
+    ssize_t sent = ::send(socket_fd_, buf + total, remaining, 0);
     if (sent <= 0)
-            return _stopping ? 0 : sent;
+      return stopping_ ? 0 : sent;
     total += sent;
     remaining -= sent;
   }
@@ -95,23 +99,26 @@ ssize_t URStream::send(uint8_t* buf, size_t buf_len)
 
 ssize_t URStream::receive(uint8_t* buf, size_t buf_len)
 {
-    if (!_initialized)
+  if (!initialized_)
     return -1;
-    if (_stopping)
+  if (stopping_)
     return 0;
 
   size_t remainder = sizeof(int32_t);
   uint8_t* buf_pos = buf;
   bool initial = true;
 
-    do {
+  do
-        ssize_t read = recv(_socket_fd, buf_pos, remainder, 0);
+  {
+    ssize_t read = recv(socket_fd_, buf_pos, remainder, 0);
     if (read <= 0)  // failed reading from socket
-            return _stopping ? 0 : read;
+      return stopping_ ? 0 : read;
 
-        if (initial) {
+    if (initial)
+    {
       remainder = be32toh(*(reinterpret_cast<int32_t*>(buf)));
-            if (remainder >= (buf_len - sizeof(int32_t))) {
+      if (remainder >= (buf_len - sizeof(int32_t)))
+      {
         LOG_ERROR("Packet size %zd is larger than buffer %zu, discarding.", remainder, buf_len);
         return -1;
       }

src/ur_communication.cpp

@@ -18,22 +18,24 @@
 
 #include "ur_modern_driver/ur_communication.h"
 
-UrCommunication::UrCommunication(std::condition_variable& msg_cond,
+UrCommunication::UrCommunication(std::condition_variable& msg_cond, std::string host)
-    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) {
+  if (pri_sockfd_ < 0)
+  {
     print_fatal("ERROR opening socket pri_sockfd");
   }
   sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
-    if (sec_sockfd_ < 0) {
+  if (sec_sockfd_ < 0)
+  {
     print_fatal("ERROR opening socket sec_sockfd");
   }
   server_ = gethostbyname(host.c_str());
-    if (server_ == NULL) {
+  if (server_ == NULL)
+  {
     print_fatal("ERROR, unknown host");
   }
   pri_serv_addr_.sin_family = AF_INET;
@@ -43,18 +45,12 @@ UrCommunication::UrCommunication(std::condition_variable& msg_cond,
   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_,
+  setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
-        sizeof(int));
+  setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
-    setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
+  setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
-        sizeof(int));
+  setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
-    setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_,
+  setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
-        sizeof(int));
+  setsockopt(sec_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;
@@ -68,16 +64,14 @@ bool UrCommunication::start()
   std::string cmd;
   bzero(buf, 512);
   print_debug("Acquire firmware version: Connecting...");
-    if (connect(pri_sockfd_, (struct sockaddr*)&pri_serv_addr_,
+  if (connect(pri_sockfd_, (struct sockaddr*)&pri_serv_addr_, sizeof(pri_serv_addr_)) < 0)
-            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_,
+  setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
-        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));
@@ -86,14 +80,12 @@ bool UrCommunication::start()
   print_debug(tmp);
   close(pri_sockfd_);
 
-    print_debug(
+  print_debug("Switching to secondary interface for masterboard data: Connecting...");
-        "Switching to secondary interface for masterboard data: Connecting...");
 
   fd_set writefds;
   struct timeval timeout;
 
-    connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_,
+  connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_, sizeof(sec_serv_addr_));
-        sizeof(sec_serv_addr_));
   FD_ZERO(&writefds);
   FD_SET(sec_sockfd_, &writefds);
   timeout.tv_sec = 10;
@@ -101,7 +93,8 @@ bool UrCommunication::start()
   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) {
+  if (flag_ < 0)
+  {
     print_fatal("Error connecting to secondary interface");
     return false;
   }
@@ -126,52 +119,58 @@ void UrCommunication::run()
   FD_ZERO(&readfds);
   FD_SET(sec_sockfd_, &readfds);
   connected_ = true;
-    while (keepalive_) {
+  while (keepalive_)
-        while (connected_ && 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) {
+      if (bytes_read > 0)
-                setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK,
+      {
-                    (char*)&flag_, sizeof(int));
+        setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
         robot_state_->unpack(buf, bytes_read);
-            } else {
+      }
+      else
+      {
         connected_ = false;
         robot_state_->setDisconnected();
         close(sec_sockfd_);
       }
     }
-        if (keepalive_) {
+    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) {
+      if (sec_sockfd_ < 0)
+      {
         print_fatal("ERROR opening secondary socket");
       }
       flag_ = 1;
-            setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_,
+      setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
-                sizeof(int));
+      setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
-            setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
+      setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
-                sizeof(int));
-            setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_,
-                sizeof(int));
       fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK);
-            while (keepalive_ && !connected_) {
+      while (keepalive_ && !connected_)
+      {
         std::this_thread::sleep_for(std::chrono::seconds(10));
         fd_set writefds;
 
-                connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_,
+        connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_, sizeof(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_,
+        getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
-                    &flag_len);
+        if (flag_ < 0)
-                if (flag_ < 0) {
+        {
-                    print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds...");
+          print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 "
-                } else {
+                      "seconds...");
+        }
+        else
+        {
           connected_ = true;
           print_info("Secondary port: Reconnected");
         }

src/ur_driver.cpp

@@ -18,18 +18,14 @@
 
 #include "ur_modern_driver/ur_driver.h"
 
-UrDriver::UrDriver(std::condition_variable& rt_msg_cond,
+UrDriver::UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host,
-    std::condition_variable& msg_cond, std::string host,
+                   unsigned int reverse_port, double servoj_time, unsigned int safety_count_max, double max_time_step,
-    unsigned int reverse_port, double servoj_time,
+                   double min_payload, double max_payload, double servoj_lookahead_time, double servoj_gain)
-    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_(
+  , minimum_payload_(min_payload)
-          min_payload)
   , maximum_payload_(max_payload)
-    , servoj_time_(
+  , servoj_time_(servoj_time)
-          servoj_time)
   , servoj_lookahead_time_(servoj_lookahead_time)
   , servoj_gain_(servoj_gain)
 {
@@ -40,13 +36,13 @@ UrDriver::UrDriver(std::condition_variable& rt_msg_cond,
   firmware_version_ = 0;
   reverse_connected_ = false;
   executing_traj_ = false;
-    rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host,
+  rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host, safety_count_max);
-        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) {
+  if (incoming_sockfd_ < 0)
+  {
     print_fatal("ERROR opening socket for reverse communication");
   }
   bzero((char*)&serv_addr, sizeof(serv_addr));
@@ -55,74 +51,63 @@ UrDriver::UrDriver(std::condition_variable& rt_msg_cond,
   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,
+  setsockopt(incoming_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag, sizeof(int));
-        sizeof(int));
   setsockopt(incoming_sockfd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int));
-    if (bind(incoming_sockfd_, (struct sockaddr*)&serv_addr,
+  if (bind(incoming_sockfd_, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0)
-            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> UrDriver::interp_cubic(double t, double T, std::vector<double> p0_pos, std::vector<double> p1_pos,
-    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++) {
+  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]
+    double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i] - T * p1_vel[i]) / pow(T, 2);
-                       - T * p1_vel[i])
+    double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i] + T * p1_vel[i]) / pow(T, 3);
-            / 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,
+bool UrDriver::doTraj(std::vector<double> inp_timestamps, std::vector<std::vector<double>> inp_positions,
-    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;
 
-    if (!UrDriver::uploadProg()) {
+  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]
+  while ((inp_timestamps[inp_timestamps.size() - 1] >=
-               >= std::chrono::duration_cast<std::chrono::duration<double> >(t - t0).count())
+          std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count()) and
-        and executing_traj_) {
+         executing_traj_)
-        while (inp_timestamps[j]
+  {
-                <= std::chrono::duration_cast<std::chrono::duration<double> >(
+    while (inp_timestamps[j] <= std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count() &&
-                       t - t0)
+           j < inp_timestamps.size() - 1)
-                       .count()
+    {
-            && j < inp_timestamps.size() - 1) {
       j += 1;
     }
-        positions = UrDriver::interp_cubic(
+    positions = UrDriver::interp_cubic(std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count() -
-            std::chrono::duration_cast<std::chrono::duration<double> >(
+                                           inp_timestamps[j - 1],
-                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::this_thread::sleep_for(std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.)));
-            std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.)));
     t = std::chrono::high_resolution_clock::now();
   }
   executing_traj_ = false;
@@ -133,16 +118,17 @@ bool UrDriver::doTraj(std::vector<double> inp_timestamps,
 
 void UrDriver::servoj(std::vector<double> positions, int keepalive)
 {
-    if (!reverse_connected_) {
+  if (!reverse_connected_)
-        print_error(
+  {
-            "UrDriver::servoj called without a reverse connection present. Keepalive: "
+    print_error("UrDriver::servoj called without a reverse connection present. Keepalive: " +
-            + std::to_string(keepalive));
+                std::to_string(keepalive));
     return;
   }
   unsigned int bytes_written;
   int tmp;
   unsigned char buf[28];
-    for (int i = 0; i < 6; i++) {
+  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;
@@ -201,8 +187,8 @@ bool UrDriver::uploadProg()
   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",
+    sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=%.4f, gain=%.0f)\n", servoj_time_, servoj_lookahead_time_,
-            servoj_time_, servoj_lookahead_time_, servoj_gain_);
+            servoj_gain_);
   else
     sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_);
   cmd_str += buf;
@@ -213,8 +199,7 @@ bool UrDriver::uploadProg()
   cmd_str += "\t\tend\n";
   cmd_str += "\tend\n";
 
-    sprintf(buf, "\tsocket_open(\"%s\", %i)\n", ip_addr_.c_str(),
+  sprintf(buf, "\tsocket_open(\"%s\", %i)\n", ip_addr_.c_str(), REVERSE_PORT_);
-        REVERSE_PORT_);
   cmd_str += buf;
 
   cmd_str += "\tthread_servo = run servoThread()\n";
@@ -246,9 +231,9 @@ bool UrDriver::openServo()
   struct sockaddr_in cli_addr;
   socklen_t clilen;
   clilen = sizeof(cli_addr);
-    new_sockfd_ = accept(incoming_sockfd_, (struct sockaddr*)&cli_addr,
+  new_sockfd_ = accept(incoming_sockfd_, (struct sockaddr*)&cli_addr, &clilen);
-        &clilen);
+  if (new_sockfd_ < 0)
-    if (new_sockfd_ < 0) {
+  {
     print_fatal("ERROR on accepting reverse communication");
     return false;
   }
@@ -275,15 +260,14 @@ bool UrDriver::start()
   if (!rt_interface_->start())
     return false;
   ip_addr_ = rt_interface_->getLocalIp();
-    print_debug(
+  print_debug("Listening on " + ip_addr_ + ":" + std::to_string(REVERSE_PORT_) + "\n");
-        "Listening on " + ip_addr_ + ":" + std::to_string(REVERSE_PORT_)
-        + "\n");
   return true;
 }
 
 void UrDriver::halt()
 {
-    if (executing_traj_) {
+  if (executing_traj_)
+  {
     UrDriver::stopTraj();
   }
   sec_interface_->halt();
@@ -291,8 +275,7 @@ void UrDriver::halt()
   close(incoming_sockfd_);
 }
 
-void UrDriver::setSpeed(double q0, double q1, double q2, double q3, double q4,
+void UrDriver::setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc)
-    double q5, double acc)
 {
   rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc);
 }
@@ -317,28 +300,28 @@ void UrDriver::setToolVoltage(unsigned int v)
 void UrDriver::setFlag(unsigned int n, bool b)
 {
   char buf[256];
-    sprintf(buf, "sec setOut():\n\tset_flag(%d, %s)\nend\n", n,
+  sprintf(buf, "sec setOut():\n\tset_flag(%d, %s)\nend\n", n, b ? "True" : "False");
-        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) {
+  if (firmware_version_ < 2)
-        sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n,
+  {
-            b ? "True" : "False");
+    sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n, b ? "True" : "False");
-    } else if (n > 15) {
+  }
-        sprintf(buf,
+  else if (n > 15)
-            "sec setOut():\n\tset_tool_digital_out(%d, %s)\nend\n",
+  {
-            n - 16, b ? "True" : "False");
+    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",
+  else if (n > 7)
-            n - 8, b ? "True" : "False");
+  {
-
+    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",
+  else
-            n, b ? "True" : "False");
+  {
+    sprintf(buf, "sec setOut():\n\tset_standard_digital_out(%d, %s)\nend\n", n, b ? "True" : "False");
   }
   rt_interface_->addCommandToQueue(buf);
   print_debug(buf);
@@ -346,9 +329,12 @@ void UrDriver::setDigitalOut(unsigned int n, bool b)
 void UrDriver::setAnalogOut(unsigned int n, double f)
 {
   char buf[256];
-    if (firmware_version_ < 2) {
+  if (firmware_version_ < 2)
+  {
     sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f);
-    } else {
+  }
+  else
+  {
     sprintf(buf, "sec setOut():\n\tset_standard_analog_out(%d, %1.4f)\nend\n", n, f);
   }
 
@@ -358,21 +344,26 @@ void UrDriver::setAnalogOut(unsigned int n, double f)
 
 bool UrDriver::setPayload(double m)
 {
-    if ((m < maximum_payload_) && (m > minimum_payload_)) {
+  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
+  }
+  else
     return false;
 }
 
 void UrDriver::setMinPayload(double m)
 {
-    if (m > 0) {
+  if (m > 0)
+  {
     minimum_payload_ = m;
-    } else {
+  }
+  else
+  {
     minimum_payload_ = 0;
   }
 }
@@ -382,33 +373,48 @@ void UrDriver::setMaxPayload(double m)
 }
 void UrDriver::setServojTime(double t)
 {
-    if (t > 0.008) {
+  if (t > 0.008)
+  {
     servoj_time_ = t;
-    } else {
+  }
+  else
+  {
     servoj_time_ = 0.008;
   }
 }
 void UrDriver::setServojLookahead(double t)
 {
-    if (t > 0.03) {
+  if (t > 0.03)
-        if (t < 0.2) {
+  {
+    if (t < 0.2)
+    {
       servoj_lookahead_time_ = t;
-        } else {
+    }
+    else
+    {
       servoj_lookahead_time_ = 0.2;
     }
-    } else {
+  }
+  else
+  {
     servoj_lookahead_time_ = 0.03;
   }
 }
 void UrDriver::setServojGain(double g)
 {
-    if (g > 100) {
+  if (g > 100)
-        if (g < 2000) {
+  {
+    if (g < 2000)
+    {
       servoj_gain_ = g;
-        } else {
+    }
+    else
+    {
       servoj_gain_ = 2000;
     }
-    } else {
+  }
+  else
+  {
     servoj_gain_ = 100;
   }
 }

src/ur_hardware_interface.cpp

@@ -57,11 +57,9 @@
 
 #include <ur_modern_driver/ur_hardware_interface.h>
 
-namespace ros_control_ur {
+namespace ros_control_ur
-
+{
-UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot)
+UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) : nh_(nh), robot_(robot)
-    : nh_(nh)
-    , robot_(robot)
 {
   // Initialize shared memory and interfaces here
   init();  // this implementation loads from rosparam
@@ -73,12 +71,12 @@ UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot)
 
 void UrHardwareInterface::init()
 {
-    ROS_INFO_STREAM_NAMED("ur_hardware_interface",
+  ROS_INFO_STREAM_NAMED("ur_hardware_interface", "Reading rosparams from namespace: " << nh_.getNamespace());
-        "Reading rosparams from namespace: " << nh_.getNamespace());
 
   // Get joint names
   nh_.getParam("hardware_interface/joints", joint_names_);
-    if (joint_names_.size() == 0) {
+  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());
@@ -95,34 +93,27 @@ void UrHardwareInterface::init()
   prev_joint_velocity_command_.resize(num_joints_);
 
   // Initialize controller
-    for (std::size_t i = 0; i < num_joints_; ++i) {
+  for (std::size_t i = 0; i < num_joints_; ++i)
-        ROS_DEBUG_STREAM_NAMED("ur_hardware_interface",
+  {
-            "Loading joint name: " << joint_names_[i]);
+    ROS_DEBUG_STREAM_NAMED("ur_hardware_interface", "Loading joint name: " << joint_names_[i]);
 
     // Create joint state interface
-        joint_state_interface_.registerHandle(
+    joint_state_interface_.registerHandle(hardware_interface::JointStateHandle(joint_names_[i], &joint_position_[i],
-            hardware_interface::JointStateHandle(joint_names_[i],
+                                                                               &joint_velocity_[i], &joint_effort_[i]));
-                &joint_position_[i], &joint_velocity_[i],
-                &joint_effort_[i]));
 
     // Create position joint interface
-        position_joint_interface_.registerHandle(
+    position_joint_interface_.registerHandle(hardware_interface::JointHandle(
-            hardware_interface::JointHandle(
+        joint_state_interface_.getHandle(joint_names_[i]), &joint_position_command_[i]));
-                joint_state_interface_.getHandle(joint_names_[i]),
-                &joint_position_command_[i]));
 
     // Create velocity joint interface
-        velocity_joint_interface_.registerHandle(
+    velocity_joint_interface_.registerHandle(hardware_interface::JointHandle(
-            hardware_interface::JointHandle(
+        joint_state_interface_.getHandle(joint_names_[i]), &joint_velocity_command_[i]));
-                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", "",
+      hardware_interface::ForceTorqueSensorHandle("wrench", "", robot_force_, robot_torque_));
-            robot_force_, robot_torque_));
 
   registerInterface(&joint_state_interface_);     // From RobotHW base class.
   registerInterface(&position_joint_interface_);  // From RobotHW base class.
@@ -139,12 +130,14 @@ void UrHardwareInterface::read()
   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) {
+  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) {
+  for (std::size_t i = 0; i < 3; ++i)
+  {
     robot_force_[i] = tcp[i];
     robot_torque_[i] = tcp[i + 3];
   }
@@ -157,85 +150,90 @@ void UrHardwareInterface::setMaxVelChange(double inp)
 
 void UrHardwareInterface::write()
 {
-    if (velocity_interface_running_) {
+  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++) {
+    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_) {
+      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_) {
+      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_) {
+  }
+  else if (position_interface_running_)
+  {
     robot_->servoj(joint_position_command_);
   }
 }
 
-bool UrHardwareInterface::canSwitch(
+bool UrHardwareInterface::canSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
-    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();
+  for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin();
-         ++controller_it) {
+       controller_it != start_list.end(); ++controller_it)
-        if (controller_it->hardware_interface
+  {
-            == "hardware_interface::VelocityJointInterface") {
+    if (controller_it->hardware_interface == "hardware_interface::VelocityJointInterface")
-            if (velocity_interface_running_) {
+    {
-                ROS_ERROR(
+      if (velocity_interface_running_)
-                    "%s: An interface of that type (%s) is already running",
+      {
-                    controller_it->name.c_str(),
+        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_) {
+      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 != stop_list.end(); ++stop_controller_it)
-                     ++stop_controller_it) {
+        {
-                    if (stop_controller_it->hardware_interface
+          if (stop_controller_it->hardware_interface == "hardware_interface::PositionJointInterface")
-                        == "hardware_interface::PositionJointInterface") {
+          {
             error = false;
             break;
           }
         }
-                if (error) {
+        if (error)
-                    ROS_ERROR(
+        {
-                        "%s (type %s) can not be run simultaneously with a PositionJointInterface",
+          ROS_ERROR("%s (type %s) can not be run simultaneously with a PositionJointInterface",
-                        controller_it->name.c_str(),
+                    controller_it->name.c_str(), controller_it->hardware_interface.c_str());
-                        controller_it->hardware_interface.c_str());
           return false;
         }
       }
-        } else if (controller_it->hardware_interface
+    }
-            == "hardware_interface::PositionJointInterface") {
+    else if (controller_it->hardware_interface == "hardware_interface::PositionJointInterface")
-            if (position_interface_running_) {
+    {
-                ROS_ERROR(
+      if (position_interface_running_)
-                    "%s: An interface of that type (%s) is already running",
+      {
-                    controller_it->name.c_str(),
+        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_) {
+      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 != stop_list.end(); ++stop_controller_it)
-                     ++stop_controller_it) {
+        {
-                    if (stop_controller_it->hardware_interface
+          if (stop_controller_it->hardware_interface == "hardware_interface::VelocityJointInterface")
-                        == "hardware_interface::VelocityJointInterface") {
+          {
             error = false;
             break;
           }
         }
-                if (error) {
+        if (error)
-                    ROS_ERROR(
+        {
-                        "%s (type %s) can not be run simultaneously with a VelocityJointInterface",
+          ROS_ERROR("%s (type %s) can not be run simultaneously with a VelocityJointInterface",
-                        controller_it->name.c_str(),
+                    controller_it->name.c_str(), controller_it->hardware_interface.c_str());
-                        controller_it->hardware_interface.c_str());
           return false;
         }
       }
@@ -246,34 +244,35 @@ bool UrHardwareInterface::canSwitch(
   return true;
 }
 
-void UrHardwareInterface::doSwitch(
+void UrHardwareInterface::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
-    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();
+  for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = stop_list.begin();
-         ++controller_it) {
+       controller_it != stop_list.end(); ++controller_it)
-        if (controller_it->hardware_interface
+  {
-            == "hardware_interface::VelocityJointInterface") {
+    if (controller_it->hardware_interface == "hardware_interface::VelocityJointInterface")
+    {
       velocity_interface_running_ = false;
       ROS_DEBUG("Stopping velocity interface");
     }
-        if (controller_it->hardware_interface
+    if (controller_it->hardware_interface == "hardware_interface::PositionJointInterface")
-            == "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();
+  for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin();
-         ++controller_it) {
+       controller_it != start_list.end(); ++controller_it)
-        if (controller_it->hardware_interface
+  {
-            == "hardware_interface::VelocityJointInterface") {
+    if (controller_it->hardware_interface == "hardware_interface::VelocityJointInterface")
+    {
       velocity_interface_running_ = true;
       ROS_DEBUG("Starting velocity interface");
     }
-        if (controller_it->hardware_interface
+    if (controller_it->hardware_interface == "hardware_interface::PositionJointInterface")
-            == "hardware_interface::PositionJointInterface") {
+    {
       position_interface_running_ = true;
       robot_->uploadProg();
       ROS_DEBUG("Starting position interface");

src/ur_realtime_communication.cpp

@@ -18,18 +18,19 @@
 
 #include "ur_modern_driver/ur_realtime_communication.h"
 
-UrRealtimeCommunication::UrRealtimeCommunication(
+UrRealtimeCommunication::UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host,
-    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) {
+  if (sockfd_ < 0)
+  {
     print_fatal("ERROR opening socket");
   }
   server_ = gethostbyname(host.c_str());
-    if (server_ == NULL) {
+  if (server_ == NULL)
+  {
     print_fatal("ERROR, no such host");
   }
   serv_addr_.sin_family = AF_INET;
@@ -62,14 +63,16 @@ bool UrRealtimeCommunication::start()
   select(sockfd_ + 1, NULL, &writefds, NULL, &timeout);
   unsigned int flag_len;
   getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
-    if (flag_ < 0) {
+  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) {
+  if (err < 0)
+  {
     print_fatal("Could not get local IP");
     close(sockfd_);
     return false;
@@ -90,7 +93,8 @@ void UrRealtimeCommunication::halt()
 void UrRealtimeCommunication::addCommandToQueue(std::string inp)
 {
   int bytes_written;
-    if (inp.back() != '\n') {
+  if (inp.back() != '\n')
+  {
     inp.append("\n");
   }
   if (connected_)
@@ -99,21 +103,20 @@ void UrRealtimeCommunication::addCommandToQueue(std::string inp)
     print_error("Could not send command \"" + inp + "\". The robot is not connected! Command is discarded");
 }
 
-void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2,
+void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc)
-    double q3, double q4, double q5, double acc)
 {
   char cmd[1024];
-    if (robot_state_->getVersion() >= 3.1) {
+  if (robot_state_->getVersion() >= 3.1)
-        sprintf(cmd,
+  {
-            "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n",
+    sprintf(cmd, "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n", q0, q1, q2, q3, q4, q5, acc);
-            q0, q1, q2, q3, q4, q5, acc);
+  }
-    } else {
+  else
-        sprintf(cmd,
+  {
-            "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n",
+    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);
-            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 (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;
   }
@@ -130,55 +133,63 @@ void UrRealtimeCommunication::run()
   FD_SET(sockfd_, &readfds);
   print_debug("Realtime port: Got connection");
   connected_ = true;
-    while (keepalive_) {
+  while (keepalive_)
-        while (connected_ && 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) {
+      if (bytes_read > 0)
-                setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
+      {
-                    sizeof(int));
+        setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
         robot_state_->unpack(buf);
-                if (safety_count_ == safety_count_max_) {
+        if (safety_count_ == safety_count_max_)
+        {
           setSpeed(0., 0., 0., 0., 0., 0.);
         }
         safety_count_ += 1;
-            } else {
+      }
+      else
+      {
         connected_ = false;
         close(sockfd_);
       }
     }
-        if (keepalive_) {
+    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) {
+      if (sockfd_ < 0)
+      {
         print_fatal("ERROR opening socket");
       }
       flag_ = 1;
-            setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_,
+      setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
-                sizeof(int));
+      setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
-            setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
-                sizeof(int));
 
-            setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_,
+      setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
-                sizeof(int));
       fcntl(sockfd_, F_SETFL, O_NONBLOCK);
-            while (keepalive_ && !connected_) {
+      while (keepalive_ && !connected_)
+      {
         std::this_thread::sleep_for(std::chrono::seconds(10));
         fd_set writefds;
 
-                connect(sockfd_, (struct sockaddr*)&serv_addr_,
+        connect(sockfd_, (struct sockaddr*)&serv_addr_, sizeof(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) {
+        if (flag_ < 0)
-                    print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds...");
+        {
-                } else {
+          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");
         }

src/ur_ros_wrapper.cpp

@@ -16,19 +16,22 @@
  * limitations under the License.
  */
 
-#include "ur_modern_driver/do_output.h"
+#include <string.h>
-#include "ur_modern_driver/ur_driver.h"
+#include <time.h>
-#include "ur_modern_driver/ur_hardware_interface.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,15 +50,13 @@
 #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_;
@@ -84,22 +85,20 @@ protected:
 
 public:
   RosWrapper(std::string host, int reverse_port)
-        : as_(nh_, "follow_joint_trajectory",
+    : as_(nh_, "follow_joint_trajectory", boost::bind(&RosWrapper::goalCB, this, _1),
-              boost::bind(&RosWrapper::goalCB, this, _1),
           boost::bind(&RosWrapper::cancelCB, this, _1), false)
-        , robot_(
+    , robot_(rt_msg_cond_, msg_cond_, host, reverse_port, 0.03, 300)
-              rt_msg_cond_, msg_cond_, host, reverse_port, 0.03, 300)
     , io_flag_delay_(0.05)
-        , joint_offsets_(
+    , joint_offsets_(6, 0.0)
-              6, 0.0)
   {
-
     std::string joint_prefix = "";
     std::vector<std::string> joint_names;
     char buf[256];
 
-        if (ros::param::get("~prefix", joint_prefix)) {
+    if (ros::param::get("~prefix", joint_prefix))
-            if (joint_prefix.length() > 0) {
+    {
+      if (joint_prefix.length() > 0)
+      {
         sprintf(buf, "Setting prefix to %s", joint_prefix.c_str());
         print_info(buf);
       }
@@ -115,26 +114,24 @@ public:
     use_ros_control_ = false;
     ros::param::get("~use_ros_control", use_ros_control_);
 
-        if (use_ros_control_) {
+    if (use_ros_control_)
-            hardware_interface_.reset(
+    {
-                new ros_control_ur::UrHardwareInterface(nh_, &robot_));
+      hardware_interface_.reset(new ros_control_ur::UrHardwareInterface(nh_, &robot_));
-            controller_manager_.reset(
+      controller_manager_.reset(new controller_manager::ControllerManager(hardware_interface_.get(), nh_));
-                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)) {
+      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²]",
+      sprintf(buf, "Max acceleration set to: %f [rad/sec²]", max_vel_change * 125);
-                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_)) {
+    if (ros::param::get("~max_velocity", max_velocity_))
-            sprintf(buf, "Max velocity accepted by ur_driver: %f [rad/s]",
+    {
-                max_velocity_);
+      sprintf(buf, "Max velocity accepted by ur_driver: %f [rad/s]", max_velocity_);
       print_debug(buf);
     }
 
@@ -142,36 +139,40 @@ public:
     // 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)) {
+    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)) {
+    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]",
+    sprintf(buf, "Bounds for set_payload service calls: [%f, %f]", min_payload, max_payload);
-            min_payload, max_payload);
     print_debug(buf);
 
     double servoj_time = 0.008;
-        if (ros::param::get("~servoj_time", servoj_time)) {
+    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)) {
+    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)) {
+    if (ros::param::get("~servoj_gain", servoj_gain))
+    {
       sprintf(buf, "Servoj_gain set to: %f [sec]", servoj_gain);
       print_debug(buf);
     }
@@ -180,43 +181,40 @@ public:
     // Base and tool frames
     base_frame_ = joint_prefix + "base_link";
     tool_frame_ = joint_prefix + "tool0_controller";
-        if (ros::param::get("~base_frame", base_frame_)) {
+    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_)) {
+    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 (robot_.start())
-            if (use_ros_control_) {
+    {
-                ros_control_thread_ = new std::thread(
+      if (use_ros_control_)
-                    boost::bind(&RosWrapper::rosControlLoop, this));
+      {
-                print_debug(
+        ros_control_thread_ = new std::thread(boost::bind(&RosWrapper::rosControlLoop, this));
-                    "The control thread for this driver has been started");
+        print_debug("The control thread for this driver has been started");
-            } else {
+      }
+      else
+      {
         // start actionserver
         has_goal_ = false;
         as_.start();
 
         // subscribe to the data topic of interest
-                rt_publish_thread_ = new std::thread(
+        rt_publish_thread_ = new std::thread(boost::bind(&RosWrapper::publishRTMsg, this));
-                    boost::bind(&RosWrapper::publishRTMsg, this));
+        print_debug("The action server for this driver has been started");
-                print_debug(
-                    "The action server for this driver has been started");
       }
-            mb_publish_thread_ = new std::thread(
+      mb_publish_thread_ = new std::thread(boost::bind(&RosWrapper::publishMbMsg, this));
-                boost::bind(&RosWrapper::publishMbMsg, this));
+      speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1, &RosWrapper::speedInterface, this);
-            speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1,
+      urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, &RosWrapper::urscriptInterface, this);
-                &RosWrapper::speedInterface, this);
-            urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1,
-                &RosWrapper::urscriptInterface, this);
 
-            io_srv_ = nh_.advertiseService("ur_driver/set_io",
+      io_srv_ = nh_.advertiseService("ur_driver/set_io", &RosWrapper::setIO, this);
-                &RosWrapper::setIO, this);
+      payload_srv_ = nh_.advertiseService("ur_driver/set_payload", &RosWrapper::setPayload, this);
-            payload_srv_ = nh_.advertiseService("ur_driver/set_payload",
-                &RosWrapper::setPayload, this);
     }
   }
 
@@ -227,54 +225,55 @@ public:
   }
 
 private:
-    void trajThread(std::vector<double> timestamps,
+  void trajThread(std::vector<double> timestamps, std::vector<std::vector<double>> positions,
-        std::vector<std::vector<double> > positions,
                   std::vector<std::vector<double>> velocities)
   {
-
     robot_.doTraj(timestamps, positions, velocities);
-        if (has_goal_) {
+    if (has_goal_)
+    {
       result_.error_code = result_.SUCCESSFUL;
       goal_handle_.setSucceeded(result_);
       has_goal_ = false;
     }
   }
-    void goalCB(
+  void goalCB(actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> gh)
-        actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> gh)
   {
     std::string buf;
     print_info("on_goal");
-        if (!robot_.sec_interface_->robot_state_->isReady()) {
+    if (!robot_.sec_interface_->robot_state_->isReady())
+    {
       result_.error_code = -100;  // nothing is defined for this...?
 
-            if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) {
+      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()) {
+      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 "
+      result_.error_string = "Cannot accept new trajectories. (Debug: Robot mode is " +
-                + std::to_string(
+                             std::to_string(robot_.sec_interface_->robot_state_->getRobotMode()) + ")";
-                      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()) {
+    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()) {
+    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);
@@ -282,10 +281,11 @@ private:
       return;
     }
 
-        actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *gh.getGoal(); //make a copy that we can modify
+    actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
-        if (has_goal_) {
+        *gh.getGoal();  // make a copy that we can modify
-            print_warning(
+    if (has_goal_)
-                "Received new goal while still executing previous trajectory. Canceling previous trajectory");
+    {
+      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...?
@@ -294,20 +294,21 @@ private:
       std::this_thread::sleep_for(std::chrono::milliseconds(250));
     }
     goal_handle_ = gh;
-        if (!validateJointNames()) {
+    if (!validateJointNames())
+    {
       std::string outp_joint_names = "";
-            for (unsigned int i = 0; i < goal.trajectory.joint_names.size();
+      for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++)
-                 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: "
+      result_.error_string = "Received a goal with incorrect joint names: " + outp_joint_names;
-                + outp_joint_names;
       gh.setRejected(result_, result_.error_string);
       print_error(result_.error_string);
       return;
     }
-        if (!has_positions()) {
+    if (!has_positions())
+    {
       result_.error_code = result_.INVALID_GOAL;
       result_.error_string = "Received a goal without positions";
       gh.setRejected(result_, result_.error_string);
@@ -315,7 +316,8 @@ private:
       return;
     }
 
-        if (!has_velocities()) {
+    if (!has_velocities())
+    {
       result_.error_code = result_.INVALID_GOAL;
       result_.error_string = "Received a goal without velocities";
       gh.setRejected(result_, result_.error_string);
@@ -323,7 +325,8 @@ private:
       return;
     }
 
-        if (!traj_is_finite()) {
+    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);
@@ -331,10 +334,10 @@ private:
       return;
     }
 
-        if (!has_limited_velocities()) {
+    if (!has_limited_velocities())
+    {
       result_.error_code = result_.INVALID_GOAL;
-            result_.error_string = "Received a goal with velocities that are higher than "
+      result_.error_string = "Received a goal with velocities that are higher than " + std::to_string(max_velocity_);
-                + std::to_string(max_velocity_);
       gh.setRejected(result_, result_.error_string);
       print_error(result_.error_string);
       return;
@@ -342,7 +345,8 @@ private:
 
     reorder_traj_joints(goal.trajectory);
 
-        if (!start_positions_match(goal.trajectory, 0.01)) {
+    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);
@@ -352,36 +356,34 @@ private:
 
     std::vector<double> timestamps;
     std::vector<std::vector<double>> positions, velocities;
-        if (goal.trajectory.points[0].time_from_start.toSec() != 0.) {
+    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");
+      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(
+      positions.push_back(robot_.rt_interface_->robot_state_->getQActual());
-                robot_.rt_interface_->robot_state_->getQActual());
+      velocities.push_back(robot_.rt_interface_->robot_state_->getQdActual());
-            velocities.push_back(
-                robot_.rt_interface_->robot_state_->getQdActual());
     }
-        for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
+    for (unsigned int i = 0; i < goal.trajectory.points.size(); i++)
-            timestamps.push_back(
+    {
-                goal.trajectory.points[i].time_from_start.toSec());
+      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,
+    std::thread(&RosWrapper::trajThread, this, timestamps, positions, velocities).detach();
-            velocities)
-            .detach();
   }
 
-    void cancelCB(
+  void cancelCB(actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> gh)
-        actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> gh)
   {
     // set the action state to preempted
     print_info("on_cancel");
-        if (has_goal_) {
+    if (has_goal_)
-            if (gh == goal_handle_) {
+    {
+      if (gh == goal_handle_)
+      {
         robot_.stopTraj();
         has_goal_ = false;
       }
@@ -395,27 +397,35 @@ private:
   {
     resp.success = true;
     // if (req.fun == ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT) {
-        if (req.fun == 1) {
+    if (req.fun == 1)
+    {
       robot_.setDigitalOut(req.pin, req.state > 0.0 ? true : false);
-        } else if (req.fun == 2) {
+    }
+    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 == 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 == 4)
+    {
       //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_TOOL_VOLTAGE) {
       robot_.setToolVoltage((int)req.state);
-        } else {
+    }
+    else
+    {
       resp.success = false;
     }
     return resp.success;
   }
 
-    bool setPayload(ur_msgs::SetPayloadRequest& req,
+  bool setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::SetPayloadResponse& resp)
-        ur_msgs::SetPayloadResponse& resp)
   {
     if (robot_.setPayload(req.payload))
       resp.success = true;
@@ -431,15 +441,20 @@ private:
     if (goal.trajectory.joint_names.size() != actual_joint_names.size())
       return false;
 
-        for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) {
+    for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++)
+    {
       unsigned int j;
-            for (j = 0; j < actual_joint_names.size(); 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]) {
+      if (goal.trajectory.joint_names[i] == actual_joint_names[j])
+      {
         actual_joint_names.erase(actual_joint_names.begin() + j);
-            } else {
+      }
+      else
+      {
         return false;
       }
     }
@@ -453,24 +468,25 @@ private:
     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 i = 0; i < traj.joint_names.size(); i++)
-            for (unsigned int j = 0; j < actual_joint_names.size(); j++) {
+    {
+      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++) {
+    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++) {
+      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.positions.push_back(traj.points[i].positions[mapping[j]]);
-                new_point.velocities.push_back(
+        new_point.velocities.push_back(traj.points[i].velocities[mapping[j]]);
-                    traj.points[i].velocities[mapping[j]]);
         if (traj.points[i].accelerations.size() != 0)
-                    new_point.accelerations.push_back(
+          new_point.accelerations.push_back(traj.points[i].accelerations[mapping[j]]);
-                        traj.points[i].accelerations[mapping[j]]);
       }
       new_point.time_from_start = traj.points[i].time_from_start;
       new_traj.push_back(new_point);
@@ -481,9 +497,9 @@ private:
   bool has_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 i = 0; i < goal.trajectory.points.size(); i++)
-            if (goal.trajectory.points[i].positions.size()
+    {
-                != goal.trajectory.points[i].velocities.size())
+      if (goal.trajectory.points[i].positions.size() != goal.trajectory.points[i].velocities.size())
         return false;
     }
     return true;
@@ -494,9 +510,9 @@ private:
     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++) {
+    for (unsigned int i = 0; i < goal.trajectory.points.size(); i++)
-            if (goal.trajectory.points[i].positions.size()
+    {
-                != goal.trajectory.joint_names.size())
+      if (goal.trajectory.points[i].positions.size() != goal.trajectory.joint_names.size())
         return false;
     }
     return true;
@@ -504,9 +520,11 @@ private:
 
   bool start_positions_match(const trajectory_msgs::JointTrajectory& traj, double eps)
   {
-        for (unsigned int i = 0; i < traj.points[0].positions.size(); i++) {
+    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) {
+      if (fabs(traj.points[0].positions[i] - qActual[i]) > eps)
+      {
         return false;
       }
     }
@@ -516,11 +534,11 @@ private:
   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 i = 0; i < goal.trajectory.points.size(); i++)
-            for (unsigned int j = 0;
+    {
-                 j < goal.trajectory.points[i].velocities.size(); j++) {
+      for (unsigned int j = 0; j < goal.trajectory.points[i].velocities.size(); j++)
-                if (fabs(goal.trajectory.points[i].velocities[j])
+      {
-                    > max_velocity_)
+        if (fabs(goal.trajectory.points[i].velocities[j]) > max_velocity_)
           return false;
       }
     }
@@ -530,9 +548,10 @@ private:
   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 i = 0; i < goal.trajectory.points.size(); i++)
-            for (unsigned int j = 0;
+    {
-                 j < goal.trajectory.points[i].velocities.size(); j++) {
+      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]))
@@ -544,20 +563,17 @@ private:
 
   void speedInterface(const trajectory_msgs::JointTrajectory::Ptr& msg)
   {
-        if (msg->points[0].velocities.size() == 6) {
+    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(),
+        acc = *std::max_element(msg->points[0].accelerations.begin(), msg->points[0].accelerations.end());
-                    msg->points[0].accelerations.end());
+      robot_.setSpeed(msg->points[0].velocities[0], msg->points[0].velocities[1], msg->points[0].velocities[2],
-            robot_.setSpeed(msg->points[0].velocities[0],
+                      msg->points[0].velocities[3], msg->points[0].velocities[4], msg->points[0].velocities[5], acc);
-                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);
   }
 
@@ -577,10 +593,12 @@ private:
     tool_vel_pub.msg_.header.frame_id = base_frame_;
 
     clock_gettime(CLOCK_MONOTONIC, &last_time);
-        while (ros::ok()) {
+    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()) {
+      while (!robot_.rt_interface_->robot_state_->getControllerUpdated())
+      {
         rt_msg_cond_.wait(locker);
       }
       // Input
@@ -589,7 +607,8 @@ private:
 
       // 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);
+      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;
@@ -597,7 +616,8 @@ private:
       // 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
+      // 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
@@ -607,14 +627,18 @@ private:
       double angle = std::sqrt(std::pow(rx, 2) + std::pow(ry, 2) + std::pow(rz, 2));
 
       // Broadcast transform
-            if (tf_pub.trylock()) {
+      if (tf_pub.trylock())
+      {
         tf_pub.msg_.transforms[0].header.stamp = ros_time;
-                if (angle < 1e-16) {
+        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 {
+        }
+        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);
@@ -630,7 +654,8 @@ private:
       // Publish tool velocity
       std::vector<double> tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual();
 
-            if (tool_vel_pub.trylock()) {
+      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];
@@ -646,25 +671,26 @@ private:
 
   void publishRTMsg()
   {
-        ros::Publisher joint_pub = nh_.advertise<sensor_msgs::JointState>(
+    ros::Publisher joint_pub = nh_.advertise<sensor_msgs::JointState>("joint_states", 1);
-            "joint_states", 1);
+    ros::Publisher wrench_pub = nh_.advertise<geometry_msgs::WrenchStamped>("wrench", 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()) {
+    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()) {
+      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++) {
+      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();
@@ -680,7 +706,8 @@ private:
       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
+      // 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
@@ -689,9 +716,12 @@ private:
       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) {
+      if (angle < 1e-16)
+      {
         quat.setValue(0, 0, 0, 1);
-            } else {
+      }
+      else
+      {
         quat.setRotation(tf::Vector3(rx / angle, ry / angle, rz / angle), angle);
       }
 
@@ -721,29 +751,27 @@ private:
   void publishMbMsg()
   {
     bool warned = false;
-        ros::Publisher io_pub = nh_.advertise<ur_msgs::IOStates>(
+    ros::Publisher io_pub = nh_.advertise<ur_msgs::IOStates>("ur_driver/io_states", 1);
-            "ur_driver/io_states", 1);
 
-        while (ros::ok()) {
+    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()) {
+      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++) {
+      for (unsigned int i = 0; i < i_max; i++)
+      {
         ur_msgs::Digital digi;
         digi.pin = i;
-                digi.state = ((robot_.sec_interface_->robot_state_->getDigitalInputBits()
+        digi.state = ((robot_.sec_interface_->robot_state_->getDigitalInputBits() & (1 << i)) >> i);
-                                  & (1 << i))
-                    >> i);
         io_msg.digital_in_states.push_back(digi);
-                digi.state = ((robot_.sec_interface_->robot_state_->getDigitalOutputBits()
+        digi.state = ((robot_.sec_interface_->robot_state_->getDigitalOutputBits() & (1 << i)) >> i);
-                                  & (1 << i))
-                    >> i);
         io_msg.digital_out_states.push_back(digi);
       }
       ur_msgs::Analog ana;
@@ -762,16 +790,19 @@ private:
       io_msg.analog_out_states.push_back(ana);
       io_pub.publish(io_msg);
 
-            if (robot_.sec_interface_->robot_state_->isEmergencyStopped()
+      if (robot_.sec_interface_->robot_state_->isEmergencyStopped() or
-                or robot_.sec_interface_->robot_state_->isProtectiveStopped()) {
+          robot_.sec_interface_->robot_state_->isProtectiveStopped())
-                if (robot_.sec_interface_->robot_state_->isEmergencyStopped()
+      {
-                    and !warned) {
+        if (robot_.sec_interface_->robot_state_->isEmergencyStopped() and !warned)
+        {
           print_error("Emergency stop pressed!");
-                } else if (robot_.sec_interface_->robot_state_->isProtectiveStopped()
+        }
-                    and !warned) {
+        else if (robot_.sec_interface_->robot_state_->isProtectiveStopped() and !warned)
+        {
           print_error("Robot is protective stopped!");
         }
-                if (has_goal_) {
+        if (has_goal_)
+        {
           print_error("Aborting trajectory");
           robot_.stopTraj();
           result_.error_code = result_.SUCCESSFUL;
@@ -780,7 +811,8 @@ private:
           has_goal_ = false;
         }
         warned = true;
-            } else
+      }
+      else
         warned = false;
 
       robot_.sec_interface_->robot_state_->finishedReading();
@@ -796,26 +828,34 @@ int main(int argc, char** argv)
 
   ros::init(argc, argv, "ur_driver");
   ros::NodeHandle nh;
-    if (ros::param::get("use_sim_time", use_sim_time)) {
+  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 (!(ros::param::get("~robot_ip_address", host)))
-        if (argc > 1) {
+  {
-            print_warning(
+    if (argc > 1)
-                "Please set the parameter robot_ip_address instead of giving it as a command line argument. This method is DEPRECATED");
+    {
+      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(
+    else
-                "Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip");
+    {
+      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 ((ros::param::get("~reverse_port", reverse_port)))
-        if ((reverse_port <= 0) or (reverse_port >= 65535)) {
+  {
+    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
+  }
+  else
     reverse_port = 50001;
 
   RosWrapper interface(host, reverse_port);