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,178 +1,175 @@
 #pragma once
 
-#include "ur_modern_driver/log.h"
-#include "ur_modern_driver/types.h"
 #include <assert.h>
-#include <cstddef>
-#include <cstring>
 #include <endian.h>
 #include <inttypes.h>
+#include <cstddef>
+#include <cstring>
 #include <string>
+#include "ur_modern_driver/log.h"
+#include "ur_modern_driver/types.h"
 
-class BinParser {
+class BinParser
+{
 private:
-    uint8_t *_buf_pos, *_buf_end;
+  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)
+    assert(buf_pos_ <= buf_end_);
-        , _parent(*this)
+  }
-    {
-        assert(_buf_pos <= _buf_end);
-    }
 
-    BinParser(BinParser& parent, size_t sub_len)
+  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()
+  ~BinParser()
-    {
+  {
-        _parent._buf_pos = _buf_pos;
+    parent_.buf_pos_ = buf_pos_;
-    }
+  }
 
-    //Decode from network encoding (big endian) to host encoding
+  // Decode from network encoding (big endian) to host encoding
-    template <typename T>
+  template <typename T>
-    T decode(T val)
+  T decode(T val)
-    {
+  {
-        return val;
+    return val;
-    }
+  }
-    uint16_t decode(uint16_t val)
+  uint16_t decode(uint16_t val)
-    {
+  {
-        return be16toh(val);
+    return be16toh(val);
-    }
+  }
-    uint32_t decode(uint32_t val)
+  uint32_t decode(uint32_t val)
-    {
+  {
-        return be32toh(val);
+    return be32toh(val);
-    }
+  }
-    uint64_t decode(uint64_t val)
+  uint64_t decode(uint64_t val)
-    {
+  {
-        return be64toh(val);
+    return be64toh(val);
-    }
+  }
-    int16_t decode(int16_t val)
+  int16_t decode(int16_t val)
-    {
+  {
-        return be16toh(val);
+    return be16toh(val);
-    }
+  }
-    int32_t decode(int32_t val)
+  int32_t decode(int32_t val)
-    {
+  {
-        return be32toh(val);
+    return be32toh(val);
-    }
+  }
-    int64_t decode(int64_t val)
+  int64_t decode(int64_t val)
-    {
+  {
-        return be64toh(val);
+    return be64toh(val);
-    }
+  }
 
-    template <typename T>
+  template <typename T>
-    T peek()
+  T peek()
-    {
+  {
-        assert(_buf_pos <= _buf_end);
+    assert(buf_pos_ <= buf_end_);
-        T val;
+    T val;
-        std::memcpy(&val, _buf_pos, sizeof(T));
+    std::memcpy(&val, buf_pos_, sizeof(T));
-        return decode(val);
+    return decode(val);
-    }
+  }
 
-    template <typename T>
+  template <typename T>
-    void parse(T& val)
+  void parse(T& val)
-    {
+  {
-        val = peek<T>();
+    val = peek<T>();
-        _buf_pos += sizeof(T);
+    buf_pos_ += sizeof(T);
-    }
+  }
 
-    void parse(double& val)
+  void parse(double& val)
-    {
+  {
-        uint64_t inner;
+    uint64_t inner;
-        parse<uint64_t>(inner);
+    parse<uint64_t>(inner);
-        std::memcpy(&val, &inner, sizeof(double));
+    std::memcpy(&val, &inner, sizeof(double));
-    }
+  }
-    void parse(float& val)
+  void parse(float& val)
-    {
+  {
-        uint32_t inner;
+    uint32_t inner;
-        parse<uint32_t>(inner);
+    parse<uint32_t>(inner);
-        std::memcpy(&val, &inner, sizeof(float));
+    std::memcpy(&val, &inner, sizeof(float));
-    }
+  }
 
-    // UR uses 1 byte for boolean values but sizeof(bool) is implementation
+  // UR uses 1 byte for boolean values but sizeof(bool) is implementation
-    // defined so we must ensure they're parsed as uint8_t on all compilers
+  // defined so we must ensure they're parsed as uint8_t on all compilers
-    void parse(bool& val)
+  void parse(bool& val)
-    {
+  {
-        uint8_t inner;
+    uint8_t inner;
-        parse<uint8_t>(inner);
+    parse<uint8_t>(inner);
-        val = inner != 0;
+    val = inner != 0;
-    }
+  }
 
-    // Explicit parsing order of fields to avoid issues with struct layout
+  // Explicit parsing order of fields to avoid issues with struct layout
-    void parse(double3_t& val)
+  void parse(double3_t& val)
-    {
+  {
-        parse(val.x);
+    parse(val.x);
-        parse(val.y);
+    parse(val.y);
-        parse(val.z);
+    parse(val.z);
-    }
+  }
 
-    // Explicit parsing order of fields to avoid issues with struct layout
+  // Explicit parsing order of fields to avoid issues with struct layout
-    void parse(cartesian_coord_t& val)
+  void parse(cartesian_coord_t& val)
-    {
+  {
-        parse(val.position);
+    parse(val.position);
-        parse(val.rotation);
+    parse(val.rotation);
-    }
+  }
 
-    void parse_remainder(std::string& val)
+  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)
+  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
+  // Special string parse function that assumes uint8_t len followed by chars
-    void parse(std::string& val)
+  void parse(std::string& val)
-    {
+  {
-        uint8_t len;
+    uint8_t len;
-        parse(len);
+    parse(len);
-        parse(val, size_t(len));
+    parse(val, size_t(len));
-    }
+  }
 
-    template <typename T, size_t N>
+  template <typename T, size_t N>
-    void parse(T (&array)[N])
+  void parse(T (&array)[N])
+  {
+    for (size_t i = 0; i < N; i++)
     {
-        for (size_t i = 0; i < N; i++) {
+      parse(array[i]);
-            parse(array[i]);
-        }
     }
+  }
 
-    void consume()
+  void consume()
-    {
+  {
-        _buf_pos = _buf_end;
+    buf_pos_ = buf_end_;
-    }
+  }
-    void consume(size_t bytes)
+  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>
+  template <typename T>
-    bool check_size(void)
+  bool checkSize(void)
-    {
+  {
-        return check_size(T::SIZE);
+    return checkSize(T::SIZE);
-    }
+  }
 
-    bool empty()
+  bool empty()
-    {
+  {
-        return _buf_pos == _buf_end;
+    return buf_pos_ == buf_end_;
-    }
+  }
 
-    void debug()
+  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;
+  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;
+  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()
+  void run_producer()
+  {
+    producer_.setupProducer();
+    std::vector<unique_ptr<T>> products;
+    while (running_)
     {
-        _producer.setup_producer();
+      if (!producer_.tryGet(products))
-        std::vector<unique_ptr<T> > products;
+      {
-        while (_running) {
+        break;
-            if (!_producer.try_get(products)) {
+      }
-                break;
-            }
 
-            for (auto& p : products) {
+      for (auto& p : products)
-                if (!_queue.try_enqueue(std::move(p))) {
+      {
-                    LOG_ERROR("Pipeline producer owerflowed!");
+        if (!queue_.try_enqueue(std::move(p)))
-                }
+        {
-            }
+          LOG_ERROR("Pipeline producer owerflowed!");
-
-            products.clear();
         }
-        _producer.teardown_producer();
+      }
-        LOG_DEBUG("Pipline producer ended");
-        _consumer.stop_consumer();
-    }
 
-    void run_consumer()
+      products.clear();
+    }
+    producer_.teardownProducer();
+    LOG_DEBUG("Pipline producer ended");
+    consumer_.stopConsumer();
+  }
+
+  void run_consumer()
+  {
+    consumer_.setupConsumer();
+    unique_ptr<T> product;
+    while (running_)
     {
-        _consumer.setup_consumer();
+      // 16000us timeout was chosen because we should
-        unique_ptr<T> product;
+      // roughly recieve messages at 125hz which is every
-        while (_running) {
+      // 8ms == 8000us and double it for some error margin
-            // 16000us timeout was chosen because we should
+      if (!queue_.wait_dequeue_timed(product, 16000))
-            // roughly recieve messages at 125hz which is every
+      {
-            // 8ms == 8000us and double it for some error margin
+        continue;
-            if (!_queue.wait_dequeue_timed(product, 16000)) {
+      }
-                continue;
+      if (!consumer_.consume(std::move(product)))
-            }
+        break;
-            if (!_consumer.consume(std::move(product)))
-                break;
-        }
-        _consumer.teardown_consumer();
-        LOG_DEBUG("Pipline consumer ended");
-        _producer.stop_producer();
     }
+    consumer_.teardownConsumer();
+    LOG_DEBUG("Pipline consumer ended");
+    producer_.stopProducer();
+  }
 
 public:
-    Pipeline(IProducer<T>& producer, IConsumer<T>& consumer)
+  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()
+  void run()
-    {
+  {
-        if (_running)
+    if (running_)
-            return;
+      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()
+  void stop()
-    {
+  {
-        if (!_running)
+    if (!running_)
-            return;
+      return;
 
-        LOG_DEBUG("Stopping pipeline");
+    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/robot_state.h

@@ -19,202 +19,215 @@
 #ifndef ROBOT_STATE_H_
 #define ROBOT_STATE_H_
 
-#include <condition_variable>
 #include <inttypes.h>
-#include <mutex>
 #include <netinet/in.h>
 #include <stdlib.h>
 #include <string.h>
+#include <condition_variable>
+#include <mutex>
 #include <vector>
 
-namespace message_types {
+namespace message_types
-enum message_type {
+{
-    ROBOT_STATE = 16,
+enum message_type
-    ROBOT_MESSAGE = 20,
+{
-    PROGRAM_STATE_MESSAGE = 25
+  ROBOT_STATE = 16,
+  ROBOT_MESSAGE = 20,
+  PROGRAM_STATE_MESSAGE = 25
 };
 }
 typedef message_types::message_type messageType;
 
-namespace package_types {
+namespace package_types
-enum package_type {
+{
-    ROBOT_MODE_DATA = 0,
+enum package_type
-    JOINT_DATA = 1,
+{
-    TOOL_DATA = 2,
+  ROBOT_MODE_DATA = 0,
-    MASTERBOARD_DATA = 3,
+  JOINT_DATA = 1,
-    CARTESIAN_INFO = 4,
+  TOOL_DATA = 2,
-    KINEMATICS_INFO = 5,
+  MASTERBOARD_DATA = 3,
-    CONFIGURATION_DATA = 6,
+  CARTESIAN_INFO = 4,
-    FORCE_MODE_DATA = 7,
+  KINEMATICS_INFO = 5,
-    ADDITIONAL_INFO = 8,
+  CONFIGURATION_DATA = 6,
-    CALIBRATION_DATA = 9
+  FORCE_MODE_DATA = 7,
+  ADDITIONAL_INFO = 8,
+  CALIBRATION_DATA = 9
 };
 }
 typedef package_types::package_type packageType;
 
-namespace robot_message_types {
+namespace robot_message_types
-enum robot_message_type {
+{
-    ROBOT_MESSAGE_TEXT = 0,
+enum robot_message_type
-    ROBOT_MESSAGE_PROGRAM_LABEL = 1,
+{
-    PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2,
+  ROBOT_MESSAGE_TEXT = 0,
-    ROBOT_MESSAGE_VERSION = 3,
+  ROBOT_MESSAGE_PROGRAM_LABEL = 1,
-    ROBOT_MESSAGE_SAFETY_MODE = 5,
+  PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2,
-    ROBOT_MESSAGE_ERROR_CODE = 6,
+  ROBOT_MESSAGE_VERSION = 3,
-    ROBOT_MESSAGE_KEY = 7,
+  ROBOT_MESSAGE_SAFETY_MODE = 5,
-    ROBOT_MESSAGE_REQUEST_VALUE = 9,
+  ROBOT_MESSAGE_ERROR_CODE = 6,
-    ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10
+  ROBOT_MESSAGE_KEY = 7,
+  ROBOT_MESSAGE_REQUEST_VALUE = 9,
+  ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10
 };
 }
 typedef robot_message_types::robot_message_type robotMessageType;
 
-namespace robot_state_type_v18 {
+namespace robot_state_type_v18
-enum robot_state_type {
+{
-    ROBOT_RUNNING_MODE = 0,
+enum robot_state_type
-    ROBOT_FREEDRIVE_MODE = 1,
+{
-    ROBOT_READY_MODE = 2,
+  ROBOT_RUNNING_MODE = 0,
-    ROBOT_INITIALIZING_MODE = 3,
+  ROBOT_FREEDRIVE_MODE = 1,
-    ROBOT_SECURITY_STOPPED_MODE = 4,
+  ROBOT_READY_MODE = 2,
-    ROBOT_EMERGENCY_STOPPED_MODE = 5,
+  ROBOT_INITIALIZING_MODE = 3,
-    ROBOT_FATAL_ERROR_MODE = 6,
+  ROBOT_SECURITY_STOPPED_MODE = 4,
-    ROBOT_NO_POWER_MODE = 7,
+  ROBOT_EMERGENCY_STOPPED_MODE = 5,
-    ROBOT_NOT_CONNECTED_MODE = 8,
+  ROBOT_FATAL_ERROR_MODE = 6,
-    ROBOT_SHUTDOWN_MODE = 9,
+  ROBOT_NO_POWER_MODE = 7,
-    ROBOT_SAFEGUARD_STOP_MODE = 10
+  ROBOT_NOT_CONNECTED_MODE = 8,
+  ROBOT_SHUTDOWN_MODE = 9,
+  ROBOT_SAFEGUARD_STOP_MODE = 10
 };
 }
 typedef robot_state_type_v18::robot_state_type robotStateTypeV18;
-namespace robot_state_type_v30 {
+namespace robot_state_type_v30
-enum robot_state_type {
+{
-    ROBOT_MODE_DISCONNECTED = 0,
+enum robot_state_type
-    ROBOT_MODE_CONFIRM_SAFETY = 1,
+{
-    ROBOT_MODE_BOOTING = 2,
+  ROBOT_MODE_DISCONNECTED = 0,
-    ROBOT_MODE_POWER_OFF = 3,
+  ROBOT_MODE_CONFIRM_SAFETY = 1,
-    ROBOT_MODE_POWER_ON = 4,
+  ROBOT_MODE_BOOTING = 2,
-    ROBOT_MODE_IDLE = 5,
+  ROBOT_MODE_POWER_OFF = 3,
-    ROBOT_MODE_BACKDRIVE = 6,
+  ROBOT_MODE_POWER_ON = 4,
-    ROBOT_MODE_RUNNING = 7,
+  ROBOT_MODE_IDLE = 5,
-    ROBOT_MODE_UPDATING_FIRMWARE = 8
+  ROBOT_MODE_BACKDRIVE = 6,
+  ROBOT_MODE_RUNNING = 7,
+  ROBOT_MODE_UPDATING_FIRMWARE = 8
 };
 }
 
 typedef robot_state_type_v30::robot_state_type robotStateTypeV30;
 
-struct version_message {
+struct version_message
-    uint64_t timestamp;
+{
-    int8_t source;
+  uint64_t timestamp;
-    int8_t robot_message_type;
+  int8_t source;
-    int8_t project_name_size;
+  int8_t robot_message_type;
-    char project_name[15];
+  int8_t project_name_size;
-    uint8_t major_version;
+  char project_name[15];
-    uint8_t minor_version;
+  uint8_t major_version;
-    int svn_revision;
+  uint8_t minor_version;
-    char build_date[25];
+  int svn_revision;
+  char build_date[25];
 };
 
-struct masterboard_data {
+struct masterboard_data
-    int digitalInputBits;
+{
-    int digitalOutputBits;
+  int digitalInputBits;
-    char analogInputRange0;
+  int digitalOutputBits;
-    char analogInputRange1;
+  char analogInputRange0;
-    double analogInput0;
+  char analogInputRange1;
-    double analogInput1;
+  double analogInput0;
-    char analogOutputDomain0;
+  double analogInput1;
-    char analogOutputDomain1;
+  char analogOutputDomain0;
-    double analogOutput0;
+  char analogOutputDomain1;
-    double analogOutput1;
+  double analogOutput0;
-    float masterBoardTemperature;
+  double analogOutput1;
-    float robotVoltage48V;
+  float masterBoardTemperature;
-    float robotCurrent;
+  float robotVoltage48V;
-    float masterIOCurrent;
+  float robotCurrent;
-    unsigned char safetyMode;
+  float masterIOCurrent;
-    unsigned char masterOnOffState;
+  unsigned char safetyMode;
-    char euromap67InterfaceInstalled;
+  unsigned char masterOnOffState;
-    int euromapInputBits;
+  char euromap67InterfaceInstalled;
-    int euromapOutputBits;
+  int euromapInputBits;
-    float euromapVoltage;
+  int euromapOutputBits;
-    float euromapCurrent;
+  float euromapVoltage;
+  float euromapCurrent;
 };
 
-struct robot_mode_data {
+struct robot_mode_data
-    uint64_t timestamp;
+{
-    bool isRobotConnected;
+  uint64_t timestamp;
-    bool isRealRobotEnabled;
+  bool isRobotConnected;
-    bool isPowerOnRobot;
+  bool isRealRobotEnabled;
-    bool isEmergencyStopped;
+  bool isPowerOnRobot;
-    bool isProtectiveStopped;
+  bool isEmergencyStopped;
-    bool isProgramRunning;
+  bool isProtectiveStopped;
-    bool isProgramPaused;
+  bool isProgramRunning;
-    unsigned char robotMode;
+  bool isProgramPaused;
-    unsigned char controlMode;
+  unsigned char robotMode;
-    double targetSpeedFraction;
+  unsigned char controlMode;
-    double speedScaling;
+  double targetSpeedFraction;
+  double speedScaling;
 };
 
-class RobotState {
+class RobotState
+{
 private:
-    version_message version_msg_;
+  version_message version_msg_;
-    masterboard_data mb_data_;
+  masterboard_data mb_data_;
-    robot_mode_data robot_mode_;
+  robot_mode_data robot_mode_;
 
-    std::recursive_mutex val_lock_; // Locks the variables while unpack parses data;
+  std::recursive_mutex val_lock_;  // Locks the variables while unpack parses data;
 
-    std::condition_variable* pMsg_cond_; //Signals that new vars are available
+  std::condition_variable* pMsg_cond_;  // Signals that new vars are available
-    bool new_data_available_; //to avoid spurious wakes
+  bool new_data_available_;             // to avoid spurious wakes
-    unsigned char robot_mode_running_;
+  unsigned char robot_mode_running_;
 
-    double ntohd(uint64_t nf);
+  double ntohd(uint64_t nf);
 
 public:
-    RobotState(std::condition_variable& msg_cond);
+  RobotState(std::condition_variable& msg_cond);
-    ~RobotState();
+  ~RobotState();
-    double getVersion();
+  double getVersion();
-    double getTime();
+  double getTime();
-    std::vector<double> getQTarget();
+  std::vector<double> getQTarget();
-    int getDigitalInputBits();
+  int getDigitalInputBits();
-    int getDigitalOutputBits();
+  int getDigitalOutputBits();
-    char getAnalogInputRange0();
+  char getAnalogInputRange0();
-    char getAnalogInputRange1();
+  char getAnalogInputRange1();
-    double getAnalogInput0();
+  double getAnalogInput0();
-    double getAnalogInput1();
+  double getAnalogInput1();
-    char getAnalogOutputDomain0();
+  char getAnalogOutputDomain0();
-    char getAnalogOutputDomain1();
+  char getAnalogOutputDomain1();
-    double getAnalogOutput0();
+  double getAnalogOutput0();
-    double getAnalogOutput1();
+  double getAnalogOutput1();
-    std::vector<double> getVActual();
+  std::vector<double> getVActual();
-    float getMasterBoardTemperature();
+  float getMasterBoardTemperature();
-    float getRobotVoltage48V();
+  float getRobotVoltage48V();
-    float getRobotCurrent();
+  float getRobotCurrent();
-    float getMasterIOCurrent();
+  float getMasterIOCurrent();
-    unsigned char getSafetyMode();
+  unsigned char getSafetyMode();
-    unsigned char getInReducedMode();
+  unsigned char getInReducedMode();
-    char getEuromap67InterfaceInstalled();
+  char getEuromap67InterfaceInstalled();
-    int getEuromapInputBits();
+  int getEuromapInputBits();
-    int getEuromapOutputBits();
+  int getEuromapOutputBits();
-    float getEuromapVoltage();
+  float getEuromapVoltage();
-    float getEuromapCurrent();
+  float getEuromapCurrent();
 
-    bool isRobotConnected();
+  bool isRobotConnected();
-    bool isRealRobotEnabled();
+  bool isRealRobotEnabled();
-    bool isPowerOnRobot();
+  bool isPowerOnRobot();
-    bool isEmergencyStopped();
+  bool isEmergencyStopped();
-    bool isProtectiveStopped();
+  bool isProtectiveStopped();
-    bool isProgramRunning();
+  bool isProgramRunning();
-    bool isProgramPaused();
+  bool isProgramPaused();
-    unsigned char getRobotMode();
+  unsigned char getRobotMode();
-    bool isReady();
+  bool isReady();
 
-    void setDisconnected();
+  void setDisconnected();
 
-    bool getNewDataAvailable();
+  bool getNewDataAvailable();
-    void finishedReading();
+  void finishedReading();
 
-    void unpack(uint8_t* buf, unsigned int buf_length);
+  void unpack(uint8_t* buf, unsigned int buf_length);
-    void unpackRobotMessage(uint8_t* buf, unsigned int offset, uint32_t len);
+  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 unpackRobotState(uint8_t* buf, unsigned int offset, uint32_t len);
+  void unpackRobotStateMasterboard(uint8_t* buf, unsigned int offset);
-    void unpackRobotStateMasterboard(uint8_t* buf, unsigned int offset);
+  void unpackRobotMode(uint8_t* buf, unsigned int offset);
-    void unpackRobotMode(uint8_t* buf, unsigned int offset);
 };
 
 #endif /* ROBOT_STATE_H_ */

include/ur_modern_driver/robot_state_RT.h

@@ -19,98 +19,101 @@
 #ifndef ROBOT_STATE_RT_H_
 #define ROBOT_STATE_RT_H_
 
-#include <condition_variable>
 #include <inttypes.h>
-#include <mutex>
 #include <netinet/in.h>
 #include <stdlib.h>
 #include <string.h>
+#include <condition_variable>
+#include <mutex>
 #include <vector>
 
-class RobotStateRT {
+class RobotStateRT
+{
 private:
-    double version_; //protocol version
+  double version_;  // protocol version
 
-    double time_; //Time elapsed since the controller was started
+  double time_;                             // Time elapsed since the controller was started
-    std::vector<double> q_target_; //Target joint positions
+  std::vector<double> q_target_;            // Target joint positions
-    std::vector<double> qd_target_; //Target joint velocities
+  std::vector<double> qd_target_;           // Target joint velocities
-    std::vector<double> qdd_target_; //Target joint accelerations
+  std::vector<double> qdd_target_;          // Target joint accelerations
-    std::vector<double> i_target_; //Target joint currents
+  std::vector<double> i_target_;            // Target joint currents
-    std::vector<double> m_target_; //Target joint moments (torques)
+  std::vector<double> m_target_;            // Target joint moments (torques)
-    std::vector<double> q_actual_; //Actual joint positions
+  std::vector<double> q_actual_;            // Actual joint positions
-    std::vector<double> qd_actual_; //Actual joint velocities
+  std::vector<double> qd_actual_;           // Actual joint velocities
-    std::vector<double> i_actual_; //Actual joint currents
+  std::vector<double> i_actual_;            // Actual joint currents
-    std::vector<double> i_control_; //Joint control 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
-    std::vector<double> tcp_speed_actual_; //Actual speed of the tool given in Cartesian coordinates
+                                            // and rz is a rotation vector representation of the tool orientation
-    std::vector<double> tcp_force_; //Generalised forces in the TC
+  std::vector<double> tcp_speed_actual_;    // Actual speed of the tool given in Cartesian coordinates
-    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_force_;           // Generalised forces in the TC
-    std::vector<double> tcp_speed_target_; //Target speed of the tool given in Cartesian coordinates
+  std::vector<double> tool_vector_target_;  // Target Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry
-    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
+                                            // and rz is a rotation vector representation of the tool orientation
-    std::vector<double> motor_temperatures_; //Temperature of each joint in degrees celsius
+  std::vector<double> tcp_speed_target_;    // Target speed of the tool given in Cartesian coordinates
-    double controller_timer_; //Controller realtime thread execution time
+  std::vector<bool> digital_input_bits_;    // Current state of the digital inputs. NOTE: these are bits encoded as
-    double robot_mode_; //Robot mode
+                                            // int64_t, e.g. a value of 5 corresponds to bit 0 and bit 2 set high
-    std::vector<double> joint_modes_; //Joint control modes
+  std::vector<double> motor_temperatures_;  // Temperature of each joint in degrees celsius
-    double safety_mode_; //Safety mode
+  double controller_timer_;                 // Controller realtime thread execution time
-    std::vector<double> tool_accelerometer_values_; //Tool x,y and z accelerometer values (software version 1.7)
+  double robot_mode_;                       // Robot mode
-    double speed_scaling_; //Speed scaling of the trajectory limiter
+  std::vector<double> joint_modes_;         // Joint control modes
-    double linear_momentum_norm_; //Norm of Cartesian linear momentum
+  double safety_mode_;                      // Safety mode
-    double v_main_; //Masterboard: Main voltage
+  std::vector<double> tool_accelerometer_values_;  // Tool x,y and z accelerometer values (software version 1.7)
-    double v_robot_; //Matorborad: Robot voltage (48V)
+  double speed_scaling_;                           // Speed scaling of the trajectory limiter
-    double i_robot_; //Masterboard: Robot current
+  double linear_momentum_norm_;                    // Norm of Cartesian linear momentum
-    std::vector<double> v_actual_; //Actual joint voltages
+  double v_main_;                                  // Masterboard: Main voltage
+  double v_robot_;                                 // Matorborad: Robot voltage (48V)
+  double i_robot_;                                 // Masterboard: Robot current
+  std::vector<double> v_actual_;                   // Actual joint voltages
 
-    std::mutex val_lock_; // Locks the variables while unpack parses data;
+  std::mutex val_lock_;  // Locks the variables while unpack parses data;
 
-    std::condition_variable* pMsg_cond_; //Signals that new vars are available
+  std::condition_variable* pMsg_cond_;  // Signals that new vars are available
-    bool data_published_; //to avoid spurious wakes
+  bool data_published_;                 // to avoid spurious wakes
-    bool controller_updated_; //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);
-    std::vector<bool> unpackDigitalInputBits(int64_t data);
+  double ntohd(uint64_t nf);
-    double ntohd(uint64_t nf);
 
 public:
-    RobotStateRT(std::condition_variable& msg_cond);
+  RobotStateRT(std::condition_variable& msg_cond);
-    ~RobotStateRT();
+  ~RobotStateRT();
-    double getVersion();
+  double getVersion();
-    double getTime();
+  double getTime();
-    std::vector<double> getQTarget();
+  std::vector<double> getQTarget();
-    std::vector<double> getQdTarget();
+  std::vector<double> getQdTarget();
-    std::vector<double> getQddTarget();
+  std::vector<double> getQddTarget();
-    std::vector<double> getITarget();
+  std::vector<double> getITarget();
-    std::vector<double> getMTarget();
+  std::vector<double> getMTarget();
-    std::vector<double> getQActual();
+  std::vector<double> getQActual();
-    std::vector<double> getQdActual();
+  std::vector<double> getQdActual();
-    std::vector<double> getIActual();
+  std::vector<double> getIActual();
-    std::vector<double> getIControl();
+  std::vector<double> getIControl();
-    std::vector<double> getToolVectorActual();
+  std::vector<double> getToolVectorActual();
-    std::vector<double> getTcpSpeedActual();
+  std::vector<double> getTcpSpeedActual();
-    std::vector<double> getTcpForce();
+  std::vector<double> getTcpForce();
-    std::vector<double> getToolVectorTarget();
+  std::vector<double> getToolVectorTarget();
-    std::vector<double> getTcpSpeedTarget();
+  std::vector<double> getTcpSpeedTarget();
-    std::vector<bool> getDigitalInputBits();
+  std::vector<bool> getDigitalInputBits();
-    std::vector<double> getMotorTemperatures();
+  std::vector<double> getMotorTemperatures();
-    double getControllerTimer();
+  double getControllerTimer();
-    double getRobotMode();
+  double getRobotMode();
-    std::vector<double> getJointModes();
+  std::vector<double> getJointModes();
-    double getSafety_mode();
+  double getSafety_mode();
-    std::vector<double> getToolAccelerometerValues();
+  std::vector<double> getToolAccelerometerValues();
-    double getSpeedScaling();
+  double getSpeedScaling();
-    double getLinearMomentumNorm();
+  double getLinearMomentumNorm();
-    double getVMain();
+  double getVMain();
-    double getVRobot();
+  double getVRobot();
-    double getIRobot();
+  double getIRobot();
 
-    void setVersion(double ver);
+  void setVersion(double ver);
 
-    void setDataPublished();
+  void setDataPublished();
-    bool getDataPublished();
+  bool getDataPublished();
-    bool getControllerUpdated();
+  bool getControllerUpdated();
-    void setControllerUpdated();
+  void setControllerUpdated();
-    std::vector<double> getVActual();
+  std::vector<double> getVActual();
-    void unpack(uint8_t* buf);
+  void unpack(uint8_t* buf);
 };
 
 #endif /* ROBOT_STATE_RT_H_ */

include/ur_modern_driver/ros/rt_publisher.h

@@ -1,6 +1,5 @@
 #pragma once
 
-#include <cstdlib>
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/TwistStamped.h>
 #include <geometry_msgs/WrenchStamped.h>
@@ -9,6 +8,7 @@
 #include <sensor_msgs/Temperature.h>
 #include <tf/tf.h>
 #include <tf/transform_broadcaster.h>
+#include <cstdlib>
 #include <vector>
 
 #include "ur_modern_driver/ur/consumer.h"
@@ -16,55 +16,57 @@
 using namespace ros;
 using namespace tf;
 
-const std::string JOINTS[] = {
+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);
+  bool publish(RTShared& packet);
 
 public:
-    RTPublisher(std::string& joint_prefix, std::string& base_frame, std::string& tool_frame)
+  RTPublisher(std::string& joint_prefix, std::string& base_frame, std::string& tool_frame)
-        : _joint_pub(_nh.advertise<sensor_msgs::JointState>("joint_states", 1))
+    : 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);
-        }
     }
+  }
 
-    virtual bool consume(RTState_V1_6__7& state);
+  virtual bool consume(RTState_V1_6__7& state);
-    virtual bool consume(RTState_V1_8& state);
+  virtual bool consume(RTState_V1_8& state);
-    virtual bool consume(RTState_V3_0__1& state);
+  virtual bool consume(RTState_V3_0__1& state);
-    virtual bool consume(RTState_V3_2__3& 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,21 +2,23 @@
 
 #include <inttypes.h>
 
-struct double3_t {
+struct double3_t
-    double x, y, z;
+{
+  double x, y, z;
 };
 
-struct cartesian_coord_t {
+struct cartesian_coord_t
-    double3_t position;
+{
-    double3_t rotation;
+  double3_t position;
+  double3_t rotation;
 };
 
 inline bool operator==(const double3_t& lhs, const double3_t& rhs)
 {
-    return lhs.x == rhs.x && lhs.y == rhs.y && lhs.z == rhs.z;
+  return lhs.x == rhs.x && lhs.y == rhs.y && lhs.z == rhs.z;
 }
 
 inline bool operator==(const cartesian_coord_t& lhs, const cartesian_coord_t& rhs)
 {
-    return lhs.position == rhs.position && lhs.rotation == rhs.rotation;
+  return lhs.position == rhs.position && lhs.rotation == rhs.rotation;
 }

include/ur_modern_driver/ur/consumer.h

@@ -7,41 +7,44 @@
 #include "ur_modern_driver/ur/rt_state.h"
 #include "ur_modern_driver/ur/state.h"
 
-class URRTPacketConsumer : public IConsumer<RTPacket> {
+class URRTPacketConsumer : public IConsumer<RTPacket>
+{
 public:
-    virtual bool consume(unique_ptr<RTPacket> packet)
+  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;
+  virtual bool consume(RTState_V1_6__7& state) = 0;
-    virtual bool consume(RTState_V1_8& state) = 0;
+  virtual bool consume(RTState_V1_8& state) = 0;
-    virtual bool consume(RTState_V3_0__1& state) = 0;
+  virtual bool consume(RTState_V3_0__1& state) = 0;
-    virtual bool consume(RTState_V3_2__3& state) = 0;
+  virtual bool consume(RTState_V3_2__3& state) = 0;
 };
 
-class URStatePacketConsumer : public IConsumer<StatePacket> {
+class URStatePacketConsumer : public IConsumer<StatePacket>
+{
 public:
-    virtual bool consume(unique_ptr<StatePacket> packet)
+  virtual bool consume(unique_ptr<StatePacket> packet)
-    {
+  {
-        return packet->consume_with(*this);
+    return packet->consumeWith(*this);
-    }
+  }
 
-    virtual bool consume(MasterBoardData_V1_X& data) = 0;
+  virtual bool consume(MasterBoardData_V1_X& data) = 0;
-    virtual bool consume(MasterBoardData_V3_0__1& data) = 0;
+  virtual bool consume(MasterBoardData_V3_0__1& data) = 0;
-    virtual bool consume(MasterBoardData_V3_2& data) = 0;
+  virtual bool consume(MasterBoardData_V3_2& data) = 0;
 
-    virtual bool consume(RobotModeData_V1_X& data) = 0;
+  virtual bool consume(RobotModeData_V1_X& data) = 0;
-    virtual bool consume(RobotModeData_V3_0__1& data) = 0;
+  virtual bool consume(RobotModeData_V3_0__1& data) = 0;
-    virtual bool consume(RobotModeData_V3_2& data) = 0;
+  virtual bool consume(RobotModeData_V3_2& data) = 0;
 };
 
-class URMessagePacketConsumer : public IConsumer<MessagePacket> {
+class URMessagePacketConsumer : public IConsumer<MessagePacket>
+{
 public:
-    virtual bool consume(unique_ptr<MessagePacket> packet)
+  virtual bool consume(unique_ptr<MessagePacket> packet)
-    {
+  {
-        return packet->consume_with(*this);
+    return packet->consumeWith(*this);
-    }
+  }
 
-    virtual bool consume(VersionMessage& message) = 0;
+  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,83 +8,97 @@
 #include "ur_modern_driver/ur/rt_parser.h"
 #include "ur_modern_driver/ur/state_parser.h"
 #include "ur_modern_driver/ur/stream.h"
-#include <cstdlib>
 
-class URFactory : private URMessagePacketConsumer {
+class URFactory : private URMessagePacketConsumer
+{
 private:
-    URStream _stream;
+  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)
+  bool consume(VersionMessage& vm)
-    {
+  {
-        LOG_INFO("Got VersionMessage:");
+    LOG_INFO("Got VersionMessage:");
-        LOG_INFO("project name: %s", vm.project_name.c_str());
+    LOG_INFO("project name: %s", vm.project_name.c_str());
-        LOG_INFO("version: %u.%u.%d", vm.major_version, vm.minor_version, vm.svn_version);
+    LOG_INFO("version: %u.%u.%d", vm.major_version, vm.minor_version, vm.svn_version);
-        LOG_INFO("build date: %s", vm.build_date.c_str());
+    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;
+    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> prod(stream_, parser_);
+    std::vector<unique_ptr<MessagePacket>> results;
+
+    prod.setupProducer();
+
+    if (!prod.tryGet(results) || results.size() == 0)
     {
-        URProducer<MessagePacket> p(_stream, _parser);
+      LOG_FATAL("No version message received, init failed!");
-        std::vector<unique_ptr<MessagePacket> > results;
+      std::exit(EXIT_FAILURE);
-
-        p.setup_producer();
-
-        if (!p.try_get(results) || results.size() == 0) {
-            LOG_FATAL("No version message received, init failed!");
-            std::exit(EXIT_FAILURE);
-        }
-
-        for (auto const& p : results) {
-            p->consume_with(*this);
-        }
-
-        if (_major_version == 0 && _minor_version == 0) {
-            LOG_FATAL("No version message received, init failed!");
-            std::exit(EXIT_FAILURE);
-        }
-
-        p.teardown_producer();
     }
 
-    std::unique_ptr<URParser<StatePacket> > get_state_parser()
+    for (auto const& p : results)
     {
-        if (_major_version == 1) {
+      p->consumeWith(*this);
-            return std::unique_ptr<URParser<StatePacket> >(new URStateParser_V1_X);
-        } else {
-            if (_minor_version < 3)
-                return std::unique_ptr<URParser<StatePacket> >(new URStateParser_V3_0__1);
-            else
-                return std::unique_ptr<URParser<StatePacket> >(new URStateParser_V3_2);
-        }
     }
 
-    std::unique_ptr<URParser<RTPacket> > get_rt_parser()
+    if (major_version_ == 0 && minor_version_ == 0)
     {
-        if (_major_version == 1) {
+      LOG_FATAL("No version message received, init failed!");
-            if (_minor_version < 8)
+      std::exit(EXIT_FAILURE);
-                return std::unique_ptr<URParser<RTPacket> >(new URRTStateParser_V1_6__7);
-            else
-                return std::unique_ptr<URParser<RTPacket> >(new URRTStateParser_V1_8);
-        } else {
-            if (_minor_version < 3)
-                return std::unique_ptr<URParser<RTPacket> >(new URRTStateParser_V3_0__1);
-            else
-                return std::unique_ptr<URParser<RTPacket> >(new URRTStateParser_V3_2__3);
-        }
     }
+
+    prod.teardownProducer();
+  }
+
+  std::unique_ptr<URParser<StatePacket>> getStateParser()
+  {
+    if (major_version_ == 1)
+    {
+      return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V1_X);
+    }
+    else
+    {
+      if (minor_version_ < 3)
+        return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V3_0__1);
+      else
+        return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V3_2);
+    }
+  }
+
+  std::unique_ptr<URParser<RTPacket>> getRTParser()
+  {
+    if (major_version_ == 1)
+    {
+      if (minor_version_ < 8)
+        return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V1_6__7);
+      else
+        return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V1_8);
+    }
+    else
+    {
+      if (minor_version_ < 3)
+        return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V3_0__1);
+      else
+        return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V3_2__3);
+    }
+  }
 };

include/ur_modern_driver/ur/master_board.h

@@ -1,97 +1,91 @@
 #pragma once
 
+#include <inttypes.h>
+#include <cstddef>
 #include "ur_modern_driver/bin_parser.h"
 #include "ur_modern_driver/types.h"
 #include "ur_modern_driver/ur/state.h"
-#include <cstddef>
-#include <inttypes.h>
 
-class SharedMasterBoardData {
+class SharedMasterBoardData
+{
 public:
-    virtual bool parse_with(BinParser& bp);
+  virtual bool parseWith(BinParser& bp);
 
-    int8_t analog_input_range0;
+  int8_t analog_input_range0;
-    int8_t analog_input_range1;
+  int8_t analog_input_range1;
-    double analog_input0;
+  double analog_input0;
-    double analog_input1;
+  double analog_input1;
-    int8_t analog_output_domain0;
+  int8_t analog_output_domain0;
-    int8_t analog_output_domain1;
+  int8_t analog_output_domain1;
-    double analog_output0;
+  double analog_output0;
-    double analog_output1;
+  double analog_output1;
-    float master_board_temperature;
+  float master_board_temperature;
-    float robot_voltage_48V;
+  float robot_voltage_48V;
-    float robot_current;
+  float robot_current;
-    float master_IO_current;
+  float master_IO_current;
 
-    bool euromap67_interface_installed;
+  bool euromap67_interface_installed;
 
-    //euromap fields are dynamic so don't include in SIZE
+  // euromap fields are dynamic so don't include in SIZE
-    int32_t euromap_input_bits;
+  int32_t euromap_input_bits;
-    int32_t euromap_output_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;
+  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_input_bits;
-    int16_t digital_output_bits;
+  int16_t digital_output_bits;
 
-    uint8_t master_safety_state;
+  uint8_t master_safety_state;
-    bool master_on_off_state;
+  bool master_on_off_state;
 
-    //euromap fields are dynamic so don't include in SIZE
+  // euromap fields are dynamic so don't include in SIZE
-    int16_t euromap_voltage;
+  int16_t euromap_voltage;
-    int16_t euromap_current;
+  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_input_bits;
-    int32_t digital_output_bits;
+  int32_t digital_output_bits;
 
-    uint8_t safety_mode;
+  uint8_t safety_mode;
-    bool in_reduced_mode;
+  bool in_reduced_mode;
 
-    //euromap fields are dynamic so don't include in SIZE
+  // euromap fields are dynamic so don't include in SIZE
-    float euromap_voltage;
+  float euromap_voltage;
-    float euromap_current;
+  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 operational_mode_selector_input;
-    uint8_t three_position_enabling_device_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,51 +1,51 @@
 #pragma once
 
+#include <inttypes.h>
+#include <cstddef>
 #include "ur_modern_driver/bin_parser.h"
 #include "ur_modern_driver/pipeline.h"
-#include <cstddef>
-#include <inttypes.h>
 
-enum class robot_message_type : uint8_t {
+enum class robot_message_type : uint8_t
-    ROBOT_MESSAGE_TEXT = 0,
+{
-    ROBOT_MESSAGE_PROGRAM_LABEL = 1,
+  ROBOT_MESSAGE_TEXT = 0,
-    PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2,
+  ROBOT_MESSAGE_PROGRAM_LABEL = 1,
-    ROBOT_MESSAGE_VERSION = 3,
+  PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2,
-    ROBOT_MESSAGE_SAFETY_MODE = 5,
+  ROBOT_MESSAGE_VERSION = 3,
-    ROBOT_MESSAGE_ERROR_CODE = 6,
+  ROBOT_MESSAGE_SAFETY_MODE = 5,
-    ROBOT_MESSAGE_KEY = 7,
+  ROBOT_MESSAGE_ERROR_CODE = 6,
-    ROBOT_MESSAGE_REQUEST_VALUE = 9,
+  ROBOT_MESSAGE_KEY = 7,
-    ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10
+  ROBOT_MESSAGE_REQUEST_VALUE = 9,
+  ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10
 };
 
 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 parseWith(BinParser& bp) = 0;
-    }
+  virtual bool consumeWith(URMessagePacketConsumer& consumer) = 0;
-    virtual bool parse_with(BinParser& bp) = 0;
-    virtual bool consume_with(URMessagePacketConsumer& consumer) = 0;
 
-    uint64_t timestamp;
+  uint64_t timestamp;
-    uint8_t source;
+  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;
+  std::string project_name;
-    uint8_t major_version;
+  uint8_t major_version;
-    uint8_t minor_version;
+  uint8_t minor_version;
-    int32_t svn_version;
+  int32_t svn_version;
-    std::string build_date;
+  std::string build_date;
 };

include/ur_modern_driver/ur/messages_parser.h

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

include/ur_modern_driver/ur/parser.h

@@ -1,10 +1,11 @@
 #pragma once
+#include <vector>
 #include "ur_modern_driver/bin_parser.h"
 #include "ur_modern_driver/pipeline.h"
-#include <vector>
 
 template <typename T>
-class URParser {
+class URParser
+{
 public:
-    virtual bool parse(BinParser& bp, std::vector<std::unique_ptr<T> >& results) = 0;
+  virtual bool parse(BinParser& bp, std::vector<std::unique_ptr<T>>& results) = 0;
 };

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 setupProducer()
+  {
+    stream_.connect();
+  }
+  void teardownProducer()
+  {
+    stream_.disconnect();
+  }
+  void stopProducer()
+  {
+    stream_.disconnect();
+  }
+
+  bool tryGet(std::vector<unique_ptr<T>>& products)
+  {
+    // 4KB should be enough to hold any packet received from UR
+    uint8_t buf[4096];
+
+    // blocking call
+    ssize_t len = stream_.receive(buf, sizeof(buf));
+
+    // LOG_DEBUG("Read %d bytes from stream", len);
+
+    if (len == 0)
     {
+      LOG_WARN("Read nothing from stream");
+      return false;
+    }
+    else if (len < 0)
+    {
+      LOG_WARN("Stream closed");
+      return false;
     }
 
-    void setup_producer() { _stream.connect(); }
+    BinParser bp(buf, static_cast<size_t>(len));
-    void teardown_producer() { _stream.disconnect(); }
+    return parser_.parse(bp, products);
-    void stop_producer() { _stream.disconnect(); }
+  }
-
-    bool try_get(std::vector<unique_ptr<T> >& products)
-    {
-        //4KB should be enough to hold any packet received from UR
-        uint8_t buf[4096];
-
-        //blocking call
-        ssize_t len = _stream.receive(buf, sizeof(buf));
-
-        //LOG_DEBUG("Read %d bytes from stream", len);
-
-        if (len == 0) {
-            LOG_WARN("Read nothing from stream");
-            return false;
-        } else if (len < 0) {
-            LOG_WARN("Stream closed");
-            return false;
-        }
-
-        BinParser bp(buf, static_cast<size_t>(len));
-        return _parser.parse(bp, products);
-    }
 };

include/ur_modern_driver/ur/robot_mode.h

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

include/ur_modern_driver/ur/rt_parser.h

@@ -1,33 +1,35 @@
 #pragma once
+#include <vector>
 #include "ur_modern_driver/bin_parser.h"
 #include "ur_modern_driver/log.h"
 #include "ur_modern_driver/pipeline.h"
 #include "ur_modern_driver/ur/parser.h"
 #include "ur_modern_driver/ur/rt_state.h"
-#include <vector>
 
 template <typename T>
-class URRTStateParser : public URParser<RTPacket> {
+class URRTStateParser : public URParser<RTPacket>
+{
 public:
-    bool parse(BinParser& bp, std::vector<std::unique_ptr<RTPacket> >& results)
+  bool parse(BinParser& bp, std::vector<std::unique_ptr<RTPacket>>& results)
+  {
+    int32_t packet_size = bp.peek<int32_t>();
+
+    if (!bp.checkSize(packet_size))
     {
-        int32_t packet_size = bp.peek<int32_t>();
+      LOG_ERROR("Buffer len shorter than expected packet length");
-
+      return false;
-        if (!bp.check_size(packet_size)) {
-            LOG_ERROR("Buffer len shorter than expected packet length");
-            return false;
-        }
-
-        bp.parse(packet_size); //consumes the peeked data
-
-        std::unique_ptr<RTPacket> packet(new T);
-        if (!packet->parse_with(bp))
-            return false;
-
-        results.push_back(std::move(packet));
-
-        return true;
     }
+
+    bp.parse(packet_size);  // consumes the peeked data
+
+    std::unique_ptr<RTPacket> packet(new T);
+    if (!packet->parseWith(bp))
+      return false;
+
+    results.push_back(std::move(packet));
+
+    return true;
+  }
 };
 
 typedef URRTStateParser<RTState_V1_6__7> URRTStateParser_V1_6__7;

include/ur_modern_driver/ur/rt_state.h

@@ -1,122 +1,119 @@
 #pragma once
 
+#include <inttypes.h>
+#include <cstddef>
 #include "ur_modern_driver/bin_parser.h"
 #include "ur_modern_driver/pipeline.h"
 #include "ur_modern_driver/types.h"
-#include <cstddef>
-#include <inttypes.h>
 
 class URRTPacketConsumer;
 
-class RTPacket {
+class RTPacket
+{
 public:
-    virtual bool parse_with(BinParser& bp) = 0;
+  virtual bool 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_shared1(BinParser& bp);
-    void parse_shared2(BinParser& bp);
+  void parse_shared2(BinParser& bp);
 
 public:
-    double time;
+  double time;
-    double q_target[6];
+  double q_target[6];
-    double qd_target[6];
+  double qd_target[6];
-    double qdd_target[6];
+  double qdd_target[6];
-    double i_target[6];
+  double i_target[6];
-    double m_target[6];
+  double m_target[6];
-    double q_actual[6];
+  double q_actual[6];
-    double qd_actual[6];
+  double qd_actual[6];
-    double i_actual[6];
+  double i_actual[6];
 
-    //gap here depending on version
+  // gap here depending on version
 
-    double tcp_force[6];
+  double tcp_force[6];
 
-    //does not contain "_actual" postfix in V1_X but
+  // does not contain "_actual" postfix in V1_X but
-    //they're the same fields so share anyway
+  // they're the same fields so share anyway
-    cartesian_coord_t tool_vector_actual;
+  cartesian_coord_t tool_vector_actual;
-    cartesian_coord_t tcp_speed_actual;
+  cartesian_coord_t tcp_speed_actual;
 
-    //gap here depending on version
+  // gap here depending on version
 
-    uint64_t digital_inputs;
+  uint64_t digital_inputs;
-    double motor_temperatures[6];
+  double motor_temperatures[6];
-    double controller_time;
+  double controller_time;
-    double robot_mode;
+  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;
+  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!");
+  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];
+  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!");
+  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];
+  double i_control[6];
-    cartesian_coord_t tool_vector_target;
+  cartesian_coord_t tool_vector_target;
-    cartesian_coord_t tcp_speed_target;
+  cartesian_coord_t tcp_speed_target;
 
-    double joint_modes[6];
+  double joint_modes[6];
-    double safety_mode;
+  double safety_mode;
-    double3_t tool_accelerometer_values;
+  double3_t tool_accelerometer_values;
-    double speed_scaling;
+  double speed_scaling;
-    double linear_momentum_norm;
+  double linear_momentum_norm;
-    double v_main;
+  double v_main;
-    double v_robot;
+  double v_robot;
-    double i_robot;
+  double i_robot;
-    double v_actual[6];
+  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!");
+  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;
+  uint64_t digital_outputs;
-    double program_state;
+  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!");
+  static_assert(RTState_V3_2__3::SIZE == 936, "RTState_V3_2__3 has mismatched size!");
 };

include/ur_modern_driver/ur/state.h

@@ -1,34 +1,37 @@
 #pragma once
 
+#include <inttypes.h>
+#include <cstddef>
 #include "ur_modern_driver/bin_parser.h"
 #include "ur_modern_driver/log.h"
 #include "ur_modern_driver/pipeline.h"
-#include <cstddef>
-#include <inttypes.h>
 
-enum class package_type : uint8_t {
+enum class package_type : uint8_t
-    ROBOT_MODE_DATA = 0,
+{
-    JOINT_DATA = 1,
+  ROBOT_MODE_DATA = 0,
-    TOOL_DATA = 2,
+  JOINT_DATA = 1,
-    MASTERBOARD_DATA = 3,
+  TOOL_DATA = 2,
-    CARTESIAN_INFO = 4,
+  MASTERBOARD_DATA = 3,
-    KINEMATICS_INFO = 5,
+  CARTESIAN_INFO = 4,
-    CONFIGURATION_DATA = 6,
+  KINEMATICS_INFO = 5,
-    FORCE_MODE_DATA = 7,
+  CONFIGURATION_DATA = 6,
-    ADDITIONAL_INFO = 8,
+  FORCE_MODE_DATA = 7,
-    CALIBRATION_DATA = 9
+  ADDITIONAL_INFO = 8,
+  CALIBRATION_DATA = 9
 };
 
-enum class message_type : uint8_t {
+enum class message_type : uint8_t
-    ROBOT_STATE = 16,
+{
-    ROBOT_MESSAGE = 20,
+  ROBOT_STATE = 16,
-    PROGRAM_STATE_MESSAGE = 25
+  ROBOT_MESSAGE = 20,
+  PROGRAM_STATE_MESSAGE = 25
 };
 
 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,76 +7,84 @@
 #include "ur_modern_driver/ur/parser.h"
 #include "ur_modern_driver/ur/robot_mode.h"
 #include "ur_modern_driver/ur/state.h"
-#include <vector>
 
 template <typename RMD, typename MBD>
-class URStateParser : public URParser<StatePacket> {
+class URStateParser : public URParser<StatePacket>
+{
 private:
-    StatePacket* from_type(package_type type)
+  StatePacket* from_type(package_type type)
+  {
+    switch (type)
     {
-        switch (type) {
+      case package_type::ROBOT_MODE_DATA:
-        case package_type::ROBOT_MODE_DATA:
+        return new RMD;
-            return new RMD;
+      case package_type::MASTERBOARD_DATA:
-        case package_type::MASTERBOARD_DATA:
+        return new MBD;
-            return new MBD;
+      default:
-        default:
+        return nullptr;
-            return nullptr;
-        }
     }
+  }
 
 public:
-    bool parse(BinParser& bp, std::vector<std::unique_ptr<StatePacket> >& results)
+  bool parse(BinParser& bp, std::vector<std::unique_ptr<StatePacket>>& results)
+  {
+    int32_t packet_size;
+    message_type type;
+    bp.parse(packet_size);
+    bp.parse(type);
+
+    if (type != message_type::ROBOT_STATE)
     {
-        int32_t packet_size;
+      LOG_WARN("Invalid message type recieved: %u", static_cast<uint8_t>(type));
-        message_type type;
+      return false;
-        bp.parse(packet_size);
-        bp.parse(type);
-
-        if (type != message_type::ROBOT_STATE) {
-            LOG_WARN("Invalid message type recieved: %u", static_cast<uint8_t>(type));
-            return false;
-        }
-
-        while (!bp.empty()) {
-            if (!bp.check_size(sizeof(uint32_t))) {
-                LOG_ERROR("Failed to read sub-package length, there's likely a parsing error");
-                return false;
-            }
-            uint32_t sub_size = bp.peek<uint32_t>();
-            if (!bp.check_size(static_cast<size_t>(sub_size))) {
-                LOG_WARN("Invalid sub-package size of %" PRIu32 " received!", sub_size);
-                return false;
-            }
-
-            //deconstruction of a sub parser will increment the position of the parent parser
-            BinParser sbp(bp, sub_size);
-            sbp.consume(sizeof(sub_size));
-            package_type type;
-            sbp.parse(type);
-
-            std::unique_ptr<StatePacket> packet(from_type(type));
-
-            if (packet == nullptr) {
-                sbp.consume();
-                LOG_INFO("Skipping sub-packet of type %d", type);
-                continue;
-            }
-
-            if (!packet->parse_with(sbp)) {
-                LOG_ERROR("Sub-package parsing of type %d failed!", type);
-                return false;
-            }
-
-            results.push_back(std::move(packet));
-
-            if (!sbp.empty()) {
-                LOG_ERROR("Sub-package was not parsed completely!");
-                return false;
-            }
-        }
-
-        return true;
     }
+
+    while (!bp.empty())
+    {
+      if (!bp.checkSize(sizeof(uint32_t)))
+      {
+        LOG_ERROR("Failed to read sub-package length, there's likely a parsing error");
+        return false;
+      }
+      uint32_t sub_size = bp.peek<uint32_t>();
+      if (!bp.checkSize(static_cast<size_t>(sub_size)))
+      {
+        LOG_WARN("Invalid sub-package size of %" PRIu32 " received!", sub_size);
+        return false;
+      }
+
+      // deconstruction of a sub parser will increment the position of the parent parser
+      BinParser sbp(bp, sub_size);
+      sbp.consume(sizeof(sub_size));
+      package_type type;
+      sbp.parse(type);
+
+      std::unique_ptr<StatePacket> packet(from_type(type));
+
+      if (packet == nullptr)
+      {
+        sbp.consume();
+        LOG_INFO("Skipping sub-packet of type %d", type);
+        continue;
+      }
+
+      if (!packet->parseWith(sbp))
+      {
+        LOG_ERROR("Sub-package parsing of type %d failed!", type);
+        return false;
+      }
+
+      results.push_back(std::move(packet));
+
+      if (!sbp.empty())
+      {
+        LOG_ERROR("Sub-package was not parsed completely!");
+        return false;
+      }
+    }
+
+    return true;
+  }
 };
 
 typedef URStateParser<RobotModeData_V1_X, MasterBoardData_V1_X> URStateParser_V1_X;

include/ur_modern_driver/ur/stream.h

@@ -1,37 +1,34 @@
 #pragma once
-#include <atomic>
 #include <netdb.h>
-#include <string>
 #include <sys/socket.h>
 #include <sys/types.h>
+#include <atomic>
+#include <string>
 
 /// Encapsulates a TCP socket
-class URStream {
+class URStream
+{
 private:
-    int _socket_fd = -1;
+  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)
-    {
-    }
 
-    ~URStream()
+  ~URStream()
-    {
+  {
-        disconnect();
+    disconnect();
-    }
+  }
 
-    bool connect();
+  bool connect();
-    void disconnect();
+  void disconnect();
 
-    ssize_t send(uint8_t* buf, size_t buf_len);
+  ssize_t send(uint8_t* buf, size_t buf_len);
-    ssize_t receive(uint8_t* buf, size_t buf_len);
+  ssize_t receive(uint8_t* buf, size_t buf_len);
 };

include/ur_modern_driver/ur_communication.h

@@ -19,13 +19,7 @@
 #ifndef UR_COMMUNICATION_H_
 #define UR_COMMUNICATION_H_
 
-#include "do_output.h"
-#include "robot_state.h"
-#include <chrono>
-#include <condition_variable>
 #include <fcntl.h>
-#include <iostream>
-#include <mutex>
 #include <netdb.h>
 #include <netinet/in.h>
 #include <netinet/tcp.h>
@@ -36,27 +30,34 @@
 #include <sys/time.h>
 #include <sys/types.h>
 #include <sys/types.h>
-#include <thread>
 #include <unistd.h>
+#include <chrono>
+#include <condition_variable>
+#include <iostream>
+#include <mutex>
+#include <thread>
 #include <vector>
+#include "do_output.h"
+#include "robot_state.h"
 
-class UrCommunication {
+class UrCommunication
+{
 private:
-    int pri_sockfd_, sec_sockfd_;
+  int pri_sockfd_, sec_sockfd_;
-    struct sockaddr_in pri_serv_addr_, sec_serv_addr_;
+  struct sockaddr_in pri_serv_addr_, sec_serv_addr_;
-    struct hostent* server_;
+  struct hostent* server_;
-    bool keepalive_;
+  bool keepalive_;
-    std::thread comThread_;
+  std::thread comThread_;
-    int flag_;
+  int flag_;
-    void run();
+  void run();
 
 public:
-    bool connected_;
+  bool connected_;
-    RobotState* robot_state_;
+  RobotState* robot_state_;
 
-    UrCommunication(std::condition_variable& msg_cond, std::string host);
+  UrCommunication(std::condition_variable& msg_cond, std::string host);
-    bool start();
+  bool start();
-    void halt();
+  void halt();
 };
 
 #endif /* UR_COMMUNICATION_H_ */

include/ur_modern_driver/ur_driver.h

@@ -19,81 +19,79 @@
 #ifndef UR_DRIVER_H_
 #define UR_DRIVER_H_
 
+#include <math.h>
+#include <netinet/in.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <condition_variable>
+#include <mutex>
+#include <string>
+#include <vector>
 #include "do_output.h"
 #include "ur_communication.h"
 #include "ur_realtime_communication.h"
-#include <condition_variable>
-#include <math.h>
-#include <mutex>
-#include <netinet/in.h>
-#include <string>
-#include <sys/socket.h>
-#include <sys/types.h>
-#include <vector>
 
 #include <chrono>
 
-class UrDriver {
+class UrDriver
+{
 private:
-    double maximum_time_step_;
+  double maximum_time_step_;
-    double minimum_payload_;
+  double minimum_payload_;
-    double maximum_payload_;
+  double maximum_payload_;
-    std::vector<std::string> joint_names_;
+  std::vector<std::string> joint_names_;
-    std::string ip_addr_;
+  std::string ip_addr_;
-    const int MULT_JOINTSTATE_ = 1000000;
+  const int MULT_JOINTSTATE_ = 1000000;
-    const int MULT_TIME_ = 1000000;
+  const int MULT_TIME_ = 1000000;
-    const unsigned int REVERSE_PORT_;
+  const unsigned int REVERSE_PORT_;
-    int incoming_sockfd_;
+  int incoming_sockfd_;
-    int new_sockfd_;
+  int new_sockfd_;
-    bool reverse_connected_;
+  bool reverse_connected_;
-    double servoj_time_;
+  double servoj_time_;
-    bool executing_traj_;
+  bool executing_traj_;
-    double firmware_version_;
+  double firmware_version_;
-    double servoj_lookahead_time_;
+  double servoj_lookahead_time_;
-    double servoj_gain_;
+  double servoj_gain_;
 
 public:
-    UrRealtimeCommunication* rt_interface_;
+  UrRealtimeCommunication* rt_interface_;
-    UrCommunication* sec_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();
+  bool start();
-    void halt();
+  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);
-        std::vector<std::vector<double> > inp_velocities);
+  void servoj(std::vector<double> positions, int keepalive = 1);
-    void servoj(std::vector<double> positions, int keepalive = 1);
 
-    void stopTraj();
+  void stopTraj();
 
-    bool uploadProg();
+  bool uploadProg();
-    bool openServo();
+  bool openServo();
-    void closeServo(std::vector<double> positions);
+  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<double> p0_vel, std::vector<double> p1_vel);
 
-    std::vector<std::string> getJointNames();
+  std::vector<std::string> getJointNames();
-    void setJointNames(std::vector<std::string> jn);
+  void setJointNames(std::vector<std::string> jn);
-    void setToolVoltage(unsigned int v);
+  void setToolVoltage(unsigned int v);
-    void setFlag(unsigned int n, bool b);
+  void setFlag(unsigned int n, bool b);
-    void setDigitalOut(unsigned int n, bool b);
+  void setDigitalOut(unsigned int n, bool b);
-    void setAnalogOut(unsigned int n, double f);
+  void setAnalogOut(unsigned int n, double f);
-    bool setPayload(double m);
+  bool setPayload(double m);
 
-    void setMinPayload(double m);
+  void setMinPayload(double m);
-    void setMaxPayload(double m);
+  void setMaxPayload(double m);
-    void setServojTime(double t);
+  void setServojTime(double t);
-    void setServojLookahead(double t);
+  void setServojLookahead(double t);
-    void setServojGain(double g);
+  void setServojGain(double g);
 };
 
 #endif /* UR_DRIVER_H_ */

include/ur_modern_driver/ur_hardware_interface.h

@@ -58,9 +58,6 @@
 #ifndef UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H
 #define UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H
 
-#include "do_output.h"
-#include "ur_driver.h"
-#include <boost/scoped_ptr.hpp>
 #include <controller_manager/controller_manager.h>
 #include <hardware_interface/force_torque_sensor_interface.h>
 #include <hardware_interface/joint_command_interface.h>
@@ -68,69 +65,72 @@
 #include <hardware_interface/robot_hw.h>
 #include <math.h>
 #include <ros/ros.h>
+#include <boost/scoped_ptr.hpp>
+#include "do_output.h"
+#include "ur_driver.h"
 
-namespace ros_control_ur {
+namespace ros_control_ur
-
+{
 // For simulation only - determines how fast a trajectory is followed
 static const double POSITION_STEP_FACTOR = 1;
 static const double VELOCITY_STEP_FACTOR = 1;
 
 /// \brief Hardware interface for a robot
-class UrHardwareInterface : public hardware_interface::RobotHW {
+class UrHardwareInterface : public hardware_interface::RobotHW
+{
 public:
-    /**
+  /**
-	 * \brief Constructor
+ * \brief Constructor
-	 * \param nh - Node handle for topics.
+ * \param nh - Node handle for topics.
-	 */
+ */
-    UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot);
+  UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot);
 
-    /// \brief Initialize the hardware interface
+  /// \brief Initialize the hardware interface
-    virtual void init();
+  virtual void init();
 
-    /// \brief Read the state from the robot hardware.
+  /// \brief Read the state from the robot hardware.
-    virtual void read();
+  virtual void read();
 
-    /// \brief write the command to the robot hardware.
+  /// \brief write the command to the robot hardware.
-    virtual void write();
+  virtual void write();
 
-    void setMaxVelChange(double inp);
+  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;
-        const std::list<hardware_interface::ControllerInfo>& stop_list) const;
+  void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
-    void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
+                const std::list<hardware_interface::ControllerInfo>& stop_list);
-        const std::list<hardware_interface::ControllerInfo>& stop_list);
 
 protected:
-    // Startup and shutdown of the internal node inside a roscpp program
+  // Startup and shutdown of the internal node inside a roscpp program
-    ros::NodeHandle nh_;
+  ros::NodeHandle nh_;
 
-    // Interfaces
+  // Interfaces
-    hardware_interface::JointStateInterface joint_state_interface_;
+  hardware_interface::JointStateInterface joint_state_interface_;
-    hardware_interface::ForceTorqueSensorInterface force_torque_interface_;
+  hardware_interface::ForceTorqueSensorInterface force_torque_interface_;
-    hardware_interface::PositionJointInterface position_joint_interface_;
+  hardware_interface::PositionJointInterface position_joint_interface_;
-    hardware_interface::VelocityJointInterface velocity_joint_interface_;
+  hardware_interface::VelocityJointInterface velocity_joint_interface_;
-    bool velocity_interface_running_;
+  bool velocity_interface_running_;
-    bool position_interface_running_;
+  bool position_interface_running_;
-    // Shared memory
+  // Shared memory
-    std::vector<std::string> joint_names_;
+  std::vector<std::string> joint_names_;
-    std::vector<double> joint_position_;
+  std::vector<double> joint_position_;
-    std::vector<double> joint_velocity_;
+  std::vector<double> joint_velocity_;
-    std::vector<double> joint_effort_;
+  std::vector<double> joint_effort_;
-    std::vector<double> joint_position_command_;
+  std::vector<double> joint_position_command_;
-    std::vector<double> joint_velocity_command_;
+  std::vector<double> joint_velocity_command_;
-    std::vector<double> prev_joint_velocity_command_;
+  std::vector<double> prev_joint_velocity_command_;
-    std::size_t num_joints_;
+  std::size_t num_joints_;
-    double robot_force_[3] = { 0., 0., 0. };
+  double robot_force_[3] = { 0., 0., 0. };
-    double robot_torque_[3] = { 0., 0., 0. };
+  double robot_torque_[3] = { 0., 0., 0. };
 
-    double max_vel_change_;
+  double max_vel_change_;
 
-    // Robot API
+  // Robot API
-    UrDriver* robot_;
+  UrDriver* robot_;
 };
 // class
 
-} // namespace
+}  // namespace
 
 #endif

include/ur_modern_driver/ur_realtime_communication.h

@@ -19,14 +19,9 @@
 #ifndef UR_REALTIME_COMMUNICATION_H_
 #define UR_REALTIME_COMMUNICATION_H_
 
-#include "do_output.h"
-#include "robot_state_RT.h"
 #include <arpa/inet.h>
-#include <condition_variable>
 #include <errno.h>
 #include <fcntl.h>
-#include <iostream>
-#include <mutex>
 #include <netdb.h>
 #include <netinet/in.h>
 #include <netinet/tcp.h>
@@ -37,38 +32,42 @@
 #include <sys/time.h>
 #include <sys/types.h>
 #include <sys/types.h>
-#include <thread>
 #include <unistd.h>
+#include <condition_variable>
+#include <iostream>
+#include <mutex>
+#include <thread>
 #include <vector>
+#include "do_output.h"
+#include "robot_state_RT.h"
 
-class UrRealtimeCommunication {
+class UrRealtimeCommunication
+{
 private:
-    unsigned int safety_count_max_;
+  unsigned int safety_count_max_;
-    int sockfd_;
+  int sockfd_;
-    struct sockaddr_in serv_addr_;
+  struct sockaddr_in serv_addr_;
-    struct hostent* server_;
+  struct hostent* server_;
-    std::string local_ip_;
+  std::string local_ip_;
-    bool keepalive_;
+  bool keepalive_;
-    std::thread comThread_;
+  std::thread comThread_;
-    int flag_;
+  int flag_;
-    std::recursive_mutex command_string_lock_;
+  std::recursive_mutex command_string_lock_;
-    std::string command_;
+  std::string command_;
-    unsigned int safety_count_;
+  unsigned int safety_count_;
-    void run();
+  void run();
 
 public:
-    bool connected_;
+  bool connected_;
-    RobotStateRT* robot_state_;
+  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();
-    bool start();
+  void halt();
-    void halt();
+  void setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc = 100.);
-    void setSpeed(double q0, double q1, double q2, double q3, double q4,
+  void addCommandToQueue(std::string inp);
-        double q5, double acc = 100.);
+  void setSafetyCountMax(uint inp);
-    void addCommandToQueue(std::string inp);
+  std::string getLocalIp();
-    void setSafetyCountMax(uint inp);
-    std::string getLocalIp();
 };
 
 #endif /* UR_REALTIME_COMMUNICATION_H_ */

src/do_output.cpp

@@ -21,42 +21,42 @@
 void print_debug(std::string inp)
 {
 #ifdef ROS_BUILD
-    ROS_DEBUG("%s", inp.c_str());
+  ROS_DEBUG("%s", inp.c_str());
 #else
-    printf("DEBUG: %s\n", inp.c_str());
+  printf("DEBUG: %s\n", inp.c_str());
 #endif
 }
 void print_info(std::string inp)
 {
 #ifdef ROS_BUILD
-    ROS_INFO("%s", inp.c_str());
+  ROS_INFO("%s", inp.c_str());
 #else
-    printf("INFO: %s\n", inp.c_str());
+  printf("INFO: %s\n", inp.c_str());
 #endif
 }
 void print_warning(std::string inp)
 {
 #ifdef ROS_BUILD
-    ROS_WARN("%s", inp.c_str());
+  ROS_WARN("%s", inp.c_str());
 #else
-    printf("WARNING: %s\n", inp.c_str());
+  printf("WARNING: %s\n", inp.c_str());
 #endif
 }
 void print_error(std::string inp)
 {
 #ifdef ROS_BUILD
-    ROS_ERROR("%s", inp.c_str());
+  ROS_ERROR("%s", inp.c_str());
 #else
-    printf("ERROR: %s\n", inp.c_str());
+  printf("ERROR: %s\n", inp.c_str());
 #endif
 }
 void print_fatal(std::string inp)
 {
 #ifdef ROS_BUILD
-    ROS_FATAL("%s", inp.c_str());
+  ROS_FATAL("%s", inp.c_str());
-    ros::shutdown();
+  ros::shutdown();
 #else
-    printf("FATAL: %s\n", inp.c_str());
+  printf("FATAL: %s\n", inp.c_str());
-    exit(1);
+  exit(1);
 #endif
 }

src/robot_state.cpp

@@ -20,389 +20,380 @@
 
 RobotState::RobotState(std::condition_variable& msg_cond)
 {
-    version_msg_.major_version = 0;
+  version_msg_.major_version = 0;
-    version_msg_.minor_version = 0;
+  version_msg_.minor_version = 0;
-    new_data_available_ = false;
+  new_data_available_ = false;
-    pMsg_cond_ = &msg_cond;
+  pMsg_cond_ = &msg_cond;
-    RobotState::setDisconnected();
+  RobotState::setDisconnected();
-    robot_mode_running_ = robotStateTypeV30::ROBOT_MODE_RUNNING;
+  robot_mode_running_ = robotStateTypeV30::ROBOT_MODE_RUNNING;
 }
 double RobotState::ntohd(uint64_t nf)
 {
-    double x;
+  double x;
-    nf = be64toh(nf);
+  nf = be64toh(nf);
-    memcpy(&x, &nf, sizeof(x));
+  memcpy(&x, &nf, sizeof(x));
-    return x;
+  return x;
 }
 void RobotState::unpack(uint8_t* buf, unsigned int buf_length)
 {
-    /* Returns missing bytes to unpack a message, or 0 if all data was parsed */
+  /* Returns missing bytes to unpack a message, or 0 if all data was parsed */
-    unsigned int offset = 0;
+  unsigned int offset = 0;
-    while (buf_length > offset) {
+  while (buf_length > offset)
-        int len;
+  {
-        unsigned char message_type;
+    int len;
-        memcpy(&len, &buf[offset], sizeof(len));
+    unsigned char message_type;
-        len = ntohl(len);
+    memcpy(&len, &buf[offset], sizeof(len));
-        if (len + offset > buf_length) {
+    len = ntohl(len);
-            return;
+    if (len + offset > buf_length)
-        }
+    {
-        memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type));
+      return;
-        switch (message_type) {
-        case messageType::ROBOT_MESSAGE:
-            RobotState::unpackRobotMessage(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType
-            break;
-        case messageType::ROBOT_STATE:
-            RobotState::unpackRobotState(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType
-            break;
-        case messageType::PROGRAM_STATE_MESSAGE:
-        //Don't do anything atm...
-        default:
-            break;
-        }
-        offset += len;
     }
-    return;
+    memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type));
+    switch (message_type)
+    {
+      case messageType::ROBOT_MESSAGE:
+        RobotState::unpackRobotMessage(buf, offset,
+                                       len);  //'len' is inclusive the 5 bytes from messageSize and messageType
+        break;
+      case messageType::ROBOT_STATE:
+        RobotState::unpackRobotState(buf, offset,
+                                     len);  //'len' is inclusive the 5 bytes from messageSize and messageType
+        break;
+      case messageType::PROGRAM_STATE_MESSAGE:
+      // Don't do anything atm...
+      default:
+        break;
+    }
+    offset += len;
+  }
+  return;
 }
 
-void RobotState::unpackRobotMessage(uint8_t* buf, unsigned int offset,
+void RobotState::unpackRobotMessage(uint8_t* buf, unsigned int offset, uint32_t len)
-    uint32_t len)
 {
-    offset += 5;
+  offset += 5;
-    uint64_t timestamp;
+  uint64_t timestamp;
-    int8_t source, robot_message_type;
+  int8_t source, robot_message_type;
-    memcpy(&timestamp, &buf[offset], sizeof(timestamp));
+  memcpy(&timestamp, &buf[offset], sizeof(timestamp));
-    offset += sizeof(timestamp);
+  offset += sizeof(timestamp);
-    memcpy(&source, &buf[offset], sizeof(source));
+  memcpy(&source, &buf[offset], sizeof(source));
-    offset += sizeof(source);
+  offset += sizeof(source);
-    memcpy(&robot_message_type, &buf[offset], sizeof(robot_message_type));
+  memcpy(&robot_message_type, &buf[offset], sizeof(robot_message_type));
-    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;
+      version_msg_.source = source;
+      version_msg_.robot_message_type = robot_message_type;
+      RobotState::unpackRobotMessageVersion(buf, offset, len);
+      val_lock_.unlock();
+      break;
+    default:
+      break;
+  }
+}
+
+void RobotState::unpackRobotState(uint8_t* buf, unsigned int offset, uint32_t len)
+{
+  offset += 5;
+  while (offset < len)
+  {
+    int32_t length;
+    uint8_t package_type;
+    memcpy(&length, &buf[offset], sizeof(length));
+    length = ntohl(length);
+    memcpy(&package_type, &buf[offset + sizeof(length)], sizeof(package_type));
+    switch (package_type)
+    {
+      case packageType::ROBOT_MODE_DATA:
         val_lock_.lock();
-        version_msg_.timestamp = timestamp;
+        RobotState::unpackRobotMode(buf, offset + 5);
-        version_msg_.source = source;
-        version_msg_.robot_message_type = robot_message_type;
-        RobotState::unpackRobotMessageVersion(buf, offset, len);
         val_lock_.unlock();
         break;
-    default:
+
+      case packageType::MASTERBOARD_DATA:
+        val_lock_.lock();
+        RobotState::unpackRobotStateMasterboard(buf, offset + 5);
+        val_lock_.unlock();
+        break;
+      default:
         break;
     }
+    offset += length;
+  }
+  new_data_available_ = true;
+  pMsg_cond_->notify_all();
 }
 
-void RobotState::unpackRobotState(uint8_t* buf, unsigned int offset,
+void RobotState::unpackRobotMessageVersion(uint8_t* buf, unsigned int offset, uint32_t len)
-    uint32_t len)
 {
-    offset += 5;
+  memcpy(&version_msg_.project_name_size, &buf[offset], sizeof(version_msg_.project_name_size));
-    while (offset < len) {
+  offset += sizeof(version_msg_.project_name_size);
-        int32_t length;
+  memcpy(&version_msg_.project_name, &buf[offset], sizeof(char) * version_msg_.project_name_size);
-        uint8_t package_type;
+  offset += version_msg_.project_name_size;
-        memcpy(&length, &buf[offset], sizeof(length));
+  version_msg_.project_name[version_msg_.project_name_size] = '\0';
-        length = ntohl(length);
+  memcpy(&version_msg_.major_version, &buf[offset], sizeof(version_msg_.major_version));
-        memcpy(&package_type, &buf[offset + sizeof(length)],
+  offset += sizeof(version_msg_.major_version);
-            sizeof(package_type));
+  memcpy(&version_msg_.minor_version, &buf[offset], sizeof(version_msg_.minor_version));
-        switch (package_type) {
+  offset += sizeof(version_msg_.minor_version);
-        case packageType::ROBOT_MODE_DATA:
+  memcpy(&version_msg_.svn_revision, &buf[offset], sizeof(version_msg_.svn_revision));
-            val_lock_.lock();
+  offset += sizeof(version_msg_.svn_revision);
-            RobotState::unpackRobotMode(buf, offset + 5);
+  version_msg_.svn_revision = ntohl(version_msg_.svn_revision);
-            val_lock_.unlock();
+  memcpy(&version_msg_.build_date, &buf[offset], sizeof(char) * len - offset);
-            break;
+  version_msg_.build_date[len - offset] = '\0';
-
+  if (version_msg_.major_version < 2)
-        case packageType::MASTERBOARD_DATA:
+  {
-            val_lock_.lock();
+    robot_mode_running_ = robotStateTypeV18::ROBOT_RUNNING_MODE;
-            RobotState::unpackRobotStateMasterboard(buf, offset + 5);
+  }
-            val_lock_.unlock();
-            break;
-        default:
-            break;
-        }
-        offset += length;
-    }
-    new_data_available_ = true;
-    pMsg_cond_->notify_all();
-}
-
-void RobotState::unpackRobotMessageVersion(uint8_t* buf, unsigned int offset,
-    uint32_t len)
-{
-    memcpy(&version_msg_.project_name_size, &buf[offset],
-        sizeof(version_msg_.project_name_size));
-    offset += sizeof(version_msg_.project_name_size);
-    memcpy(&version_msg_.project_name, &buf[offset],
-        sizeof(char) * version_msg_.project_name_size);
-    offset += version_msg_.project_name_size;
-    version_msg_.project_name[version_msg_.project_name_size] = '\0';
-    memcpy(&version_msg_.major_version, &buf[offset],
-        sizeof(version_msg_.major_version));
-    offset += sizeof(version_msg_.major_version);
-    memcpy(&version_msg_.minor_version, &buf[offset],
-        sizeof(version_msg_.minor_version));
-    offset += sizeof(version_msg_.minor_version);
-    memcpy(&version_msg_.svn_revision, &buf[offset],
-        sizeof(version_msg_.svn_revision));
-    offset += sizeof(version_msg_.svn_revision);
-    version_msg_.svn_revision = ntohl(version_msg_.svn_revision);
-    memcpy(&version_msg_.build_date, &buf[offset], sizeof(char) * len - offset);
-    version_msg_.build_date[len - offset] = '\0';
-    if (version_msg_.major_version < 2) {
-        robot_mode_running_ = robotStateTypeV18::ROBOT_RUNNING_MODE;
-    }
 }
 
 void RobotState::unpackRobotMode(uint8_t* buf, unsigned int offset)
 {
-    memcpy(&robot_mode_.timestamp, &buf[offset], sizeof(robot_mode_.timestamp));
+  memcpy(&robot_mode_.timestamp, &buf[offset], sizeof(robot_mode_.timestamp));
-    offset += sizeof(robot_mode_.timestamp);
+  offset += sizeof(robot_mode_.timestamp);
-    uint8_t tmp;
+  uint8_t tmp;
-    memcpy(&tmp, &buf[offset], sizeof(tmp));
+  memcpy(&tmp, &buf[offset], sizeof(tmp));
-    if (tmp > 0)
+  if (tmp > 0)
-        robot_mode_.isRobotConnected = true;
+    robot_mode_.isRobotConnected = true;
-    else
+  else
-        robot_mode_.isRobotConnected = false;
+    robot_mode_.isRobotConnected = false;
-    offset += sizeof(tmp);
+  offset += sizeof(tmp);
-    memcpy(&tmp, &buf[offset], sizeof(tmp));
+  memcpy(&tmp, &buf[offset], sizeof(tmp));
-    if (tmp > 0)
+  if (tmp > 0)
-        robot_mode_.isRealRobotEnabled = true;
+    robot_mode_.isRealRobotEnabled = true;
-    else
+  else
-        robot_mode_.isRealRobotEnabled = false;
+    robot_mode_.isRealRobotEnabled = false;
-    offset += sizeof(tmp);
+  offset += sizeof(tmp);
-    memcpy(&tmp, &buf[offset], sizeof(tmp));
+  memcpy(&tmp, &buf[offset], sizeof(tmp));
-    //printf("PowerOnRobot: %d\n", tmp);
+  // printf("PowerOnRobot: %d\n", tmp);
-    if (tmp > 0)
+  if (tmp > 0)
-        robot_mode_.isPowerOnRobot = true;
+    robot_mode_.isPowerOnRobot = true;
-    else
+  else
-        robot_mode_.isPowerOnRobot = false;
+    robot_mode_.isPowerOnRobot = false;
-    offset += sizeof(tmp);
+  offset += sizeof(tmp);
-    memcpy(&tmp, &buf[offset], sizeof(tmp));
+  memcpy(&tmp, &buf[offset], sizeof(tmp));
-    if (tmp > 0)
+  if (tmp > 0)
-        robot_mode_.isEmergencyStopped = true;
+    robot_mode_.isEmergencyStopped = true;
-    else
+  else
-        robot_mode_.isEmergencyStopped = false;
+    robot_mode_.isEmergencyStopped = false;
-    offset += sizeof(tmp);
+  offset += sizeof(tmp);
-    memcpy(&tmp, &buf[offset], sizeof(tmp));
+  memcpy(&tmp, &buf[offset], sizeof(tmp));
-    if (tmp > 0)
+  if (tmp > 0)
-        robot_mode_.isProtectiveStopped = true;
+    robot_mode_.isProtectiveStopped = true;
-    else
+  else
-        robot_mode_.isProtectiveStopped = false;
+    robot_mode_.isProtectiveStopped = false;
-    offset += sizeof(tmp);
+  offset += sizeof(tmp);
-    memcpy(&tmp, &buf[offset], sizeof(tmp));
+  memcpy(&tmp, &buf[offset], sizeof(tmp));
-    if (tmp > 0)
+  if (tmp > 0)
-        robot_mode_.isProgramRunning = true;
+    robot_mode_.isProgramRunning = true;
-    else
+  else
-        robot_mode_.isProgramRunning = false;
+    robot_mode_.isProgramRunning = false;
-    offset += sizeof(tmp);
+  offset += sizeof(tmp);
-    memcpy(&tmp, &buf[offset], sizeof(tmp));
+  memcpy(&tmp, &buf[offset], sizeof(tmp));
-    if (tmp > 0)
+  if (tmp > 0)
-        robot_mode_.isProgramPaused = true;
+    robot_mode_.isProgramPaused = true;
-    else
+  else
-        robot_mode_.isProgramPaused = false;
+    robot_mode_.isProgramPaused = false;
-    offset += sizeof(tmp);
+  offset += sizeof(tmp);
-    memcpy(&robot_mode_.robotMode, &buf[offset], sizeof(robot_mode_.robotMode));
+  memcpy(&robot_mode_.robotMode, &buf[offset], sizeof(robot_mode_.robotMode));
-    offset += sizeof(robot_mode_.robotMode);
+  offset += sizeof(robot_mode_.robotMode);
-    uint64_t temp;
+  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);
+    offset += sizeof(robot_mode_.controlMode);
-        memcpy(&temp, &buf[offset], sizeof(temp));
-        offset += sizeof(temp);
-        robot_mode_.targetSpeedFraction = RobotState::ntohd(temp);
-    }
     memcpy(&temp, &buf[offset], sizeof(temp));
     offset += sizeof(temp);
-    robot_mode_.speedScaling = RobotState::ntohd(temp);
+    robot_mode_.targetSpeedFraction = RobotState::ntohd(temp);
+  }
+  memcpy(&temp, &buf[offset], sizeof(temp));
+  offset += sizeof(temp);
+  robot_mode_.speedScaling = RobotState::ntohd(temp);
 }
 
-void RobotState::unpackRobotStateMasterboard(uint8_t* buf,
+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));
+    int16_t digital_input_bits, digital_output_bits;
-        offset += sizeof(digital_input_bits);
+    memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits));
-        memcpy(&digital_output_bits, &buf[offset], sizeof(digital_output_bits));
+    offset += sizeof(digital_input_bits);
-        offset += sizeof(digital_output_bits);
+    memcpy(&digital_output_bits, &buf[offset], sizeof(digital_output_bits));
-        mb_data_.digitalInputBits = ntohs(digital_input_bits);
+    offset += sizeof(digital_output_bits);
-        mb_data_.digitalOutputBits = ntohs(digital_output_bits);
+    mb_data_.digitalInputBits = ntohs(digital_input_bits);
-    } else {
+    mb_data_.digitalOutputBits = ntohs(digital_output_bits);
-        memcpy(&mb_data_.digitalInputBits, &buf[offset],
+  }
-            sizeof(mb_data_.digitalInputBits));
+  else
-        offset += sizeof(mb_data_.digitalInputBits);
+  {
-        mb_data_.digitalInputBits = ntohl(mb_data_.digitalInputBits);
+    memcpy(&mb_data_.digitalInputBits, &buf[offset], sizeof(mb_data_.digitalInputBits));
-        memcpy(&mb_data_.digitalOutputBits, &buf[offset],
+    offset += sizeof(mb_data_.digitalInputBits);
-            sizeof(mb_data_.digitalOutputBits));
+    mb_data_.digitalInputBits = ntohl(mb_data_.digitalInputBits);
-        offset += sizeof(mb_data_.digitalOutputBits);
+    memcpy(&mb_data_.digitalOutputBits, &buf[offset], sizeof(mb_data_.digitalOutputBits));
-        mb_data_.digitalOutputBits = ntohl(mb_data_.digitalOutputBits);
+    offset += sizeof(mb_data_.digitalOutputBits);
+    mb_data_.digitalOutputBits = ntohl(mb_data_.digitalOutputBits);
+  }
+
+  memcpy(&mb_data_.analogInputRange0, &buf[offset], sizeof(mb_data_.analogInputRange0));
+  offset += sizeof(mb_data_.analogInputRange0);
+  memcpy(&mb_data_.analogInputRange1, &buf[offset], sizeof(mb_data_.analogInputRange1));
+  offset += sizeof(mb_data_.analogInputRange1);
+  uint64_t temp;
+  memcpy(&temp, &buf[offset], sizeof(temp));
+  offset += sizeof(temp);
+  mb_data_.analogInput0 = RobotState::ntohd(temp);
+  memcpy(&temp, &buf[offset], sizeof(temp));
+  offset += sizeof(temp);
+  mb_data_.analogInput1 = RobotState::ntohd(temp);
+  memcpy(&mb_data_.analogOutputDomain0, &buf[offset], sizeof(mb_data_.analogOutputDomain0));
+  offset += sizeof(mb_data_.analogOutputDomain0);
+  memcpy(&mb_data_.analogOutputDomain1, &buf[offset], sizeof(mb_data_.analogOutputDomain1));
+  offset += sizeof(mb_data_.analogOutputDomain1);
+  memcpy(&temp, &buf[offset], sizeof(temp));
+  offset += sizeof(temp);
+  mb_data_.analogOutput0 = RobotState::ntohd(temp);
+  memcpy(&temp, &buf[offset], sizeof(temp));
+  offset += sizeof(temp);
+  mb_data_.analogOutput1 = RobotState::ntohd(temp);
+
+  memcpy(&mb_data_.masterBoardTemperature, &buf[offset], sizeof(mb_data_.masterBoardTemperature));
+  offset += sizeof(mb_data_.masterBoardTemperature);
+  mb_data_.masterBoardTemperature = ntohl(mb_data_.masterBoardTemperature);
+  memcpy(&mb_data_.robotVoltage48V, &buf[offset], sizeof(mb_data_.robotVoltage48V));
+  offset += sizeof(mb_data_.robotVoltage48V);
+  mb_data_.robotVoltage48V = ntohl(mb_data_.robotVoltage48V);
+  memcpy(&mb_data_.robotCurrent, &buf[offset], sizeof(mb_data_.robotCurrent));
+  offset += sizeof(mb_data_.robotCurrent);
+  mb_data_.robotCurrent = ntohl(mb_data_.robotCurrent);
+  memcpy(&mb_data_.masterIOCurrent, &buf[offset], sizeof(mb_data_.masterIOCurrent));
+  offset += sizeof(mb_data_.masterIOCurrent);
+  mb_data_.masterIOCurrent = ntohl(mb_data_.masterIOCurrent);
+
+  memcpy(&mb_data_.safetyMode, &buf[offset], sizeof(mb_data_.safetyMode));
+  offset += sizeof(mb_data_.safetyMode);
+  memcpy(&mb_data_.masterOnOffState, &buf[offset], sizeof(mb_data_.masterOnOffState));
+  offset += sizeof(mb_data_.masterOnOffState);
+
+  memcpy(&mb_data_.euromap67InterfaceInstalled, &buf[offset], sizeof(mb_data_.euromap67InterfaceInstalled));
+  offset += sizeof(mb_data_.euromap67InterfaceInstalled);
+  if (mb_data_.euromap67InterfaceInstalled != 0)
+  {
+    memcpy(&mb_data_.euromapInputBits, &buf[offset], sizeof(mb_data_.euromapInputBits));
+    offset += sizeof(mb_data_.euromapInputBits);
+    mb_data_.euromapInputBits = ntohl(mb_data_.euromapInputBits);
+    memcpy(&mb_data_.euromapOutputBits, &buf[offset], sizeof(mb_data_.euromapOutputBits));
+    offset += sizeof(mb_data_.euromapOutputBits);
+    mb_data_.euromapOutputBits = ntohl(mb_data_.euromapOutputBits);
+    if (RobotState::getVersion() < 3.0)
+    {
+      int16_t euromap_voltage, euromap_current;
+      memcpy(&euromap_voltage, &buf[offset], sizeof(euromap_voltage));
+      offset += sizeof(euromap_voltage);
+      memcpy(&euromap_current, &buf[offset], sizeof(euromap_current));
+      offset += sizeof(euromap_current);
+      mb_data_.euromapVoltage = ntohs(euromap_voltage);
+      mb_data_.euromapCurrent = ntohs(euromap_current);
     }
-
+    else
-    memcpy(&mb_data_.analogInputRange0, &buf[offset],
+    {
-        sizeof(mb_data_.analogInputRange0));
+      memcpy(&mb_data_.euromapVoltage, &buf[offset], sizeof(mb_data_.euromapVoltage));
-    offset += sizeof(mb_data_.analogInputRange0);
+      offset += sizeof(mb_data_.euromapVoltage);
-    memcpy(&mb_data_.analogInputRange1, &buf[offset],
+      mb_data_.euromapVoltage = ntohl(mb_data_.euromapVoltage);
-        sizeof(mb_data_.analogInputRange1));
+      memcpy(&mb_data_.euromapCurrent, &buf[offset], sizeof(mb_data_.euromapCurrent));
-    offset += sizeof(mb_data_.analogInputRange1);
+      offset += sizeof(mb_data_.euromapCurrent);
-    uint64_t temp;
+      mb_data_.euromapCurrent = ntohl(mb_data_.euromapCurrent);
-    memcpy(&temp, &buf[offset], sizeof(temp));
-    offset += sizeof(temp);
-    mb_data_.analogInput0 = RobotState::ntohd(temp);
-    memcpy(&temp, &buf[offset], sizeof(temp));
-    offset += sizeof(temp);
-    mb_data_.analogInput1 = RobotState::ntohd(temp);
-    memcpy(&mb_data_.analogOutputDomain0, &buf[offset],
-        sizeof(mb_data_.analogOutputDomain0));
-    offset += sizeof(mb_data_.analogOutputDomain0);
-    memcpy(&mb_data_.analogOutputDomain1, &buf[offset],
-        sizeof(mb_data_.analogOutputDomain1));
-    offset += sizeof(mb_data_.analogOutputDomain1);
-    memcpy(&temp, &buf[offset], sizeof(temp));
-    offset += sizeof(temp);
-    mb_data_.analogOutput0 = RobotState::ntohd(temp);
-    memcpy(&temp, &buf[offset], sizeof(temp));
-    offset += sizeof(temp);
-    mb_data_.analogOutput1 = RobotState::ntohd(temp);
-
-    memcpy(&mb_data_.masterBoardTemperature, &buf[offset],
-        sizeof(mb_data_.masterBoardTemperature));
-    offset += sizeof(mb_data_.masterBoardTemperature);
-    mb_data_.masterBoardTemperature = ntohl(mb_data_.masterBoardTemperature);
-    memcpy(&mb_data_.robotVoltage48V, &buf[offset],
-        sizeof(mb_data_.robotVoltage48V));
-    offset += sizeof(mb_data_.robotVoltage48V);
-    mb_data_.robotVoltage48V = ntohl(mb_data_.robotVoltage48V);
-    memcpy(&mb_data_.robotCurrent, &buf[offset], sizeof(mb_data_.robotCurrent));
-    offset += sizeof(mb_data_.robotCurrent);
-    mb_data_.robotCurrent = ntohl(mb_data_.robotCurrent);
-    memcpy(&mb_data_.masterIOCurrent, &buf[offset],
-        sizeof(mb_data_.masterIOCurrent));
-    offset += sizeof(mb_data_.masterIOCurrent);
-    mb_data_.masterIOCurrent = ntohl(mb_data_.masterIOCurrent);
-
-    memcpy(&mb_data_.safetyMode, &buf[offset], sizeof(mb_data_.safetyMode));
-    offset += sizeof(mb_data_.safetyMode);
-    memcpy(&mb_data_.masterOnOffState, &buf[offset],
-        sizeof(mb_data_.masterOnOffState));
-    offset += sizeof(mb_data_.masterOnOffState);
-
-    memcpy(&mb_data_.euromap67InterfaceInstalled, &buf[offset],
-        sizeof(mb_data_.euromap67InterfaceInstalled));
-    offset += sizeof(mb_data_.euromap67InterfaceInstalled);
-    if (mb_data_.euromap67InterfaceInstalled != 0) {
-        memcpy(&mb_data_.euromapInputBits, &buf[offset],
-            sizeof(mb_data_.euromapInputBits));
-        offset += sizeof(mb_data_.euromapInputBits);
-        mb_data_.euromapInputBits = ntohl(mb_data_.euromapInputBits);
-        memcpy(&mb_data_.euromapOutputBits, &buf[offset],
-            sizeof(mb_data_.euromapOutputBits));
-        offset += sizeof(mb_data_.euromapOutputBits);
-        mb_data_.euromapOutputBits = ntohl(mb_data_.euromapOutputBits);
-        if (RobotState::getVersion() < 3.0) {
-            int16_t euromap_voltage, euromap_current;
-            memcpy(&euromap_voltage, &buf[offset], sizeof(euromap_voltage));
-            offset += sizeof(euromap_voltage);
-            memcpy(&euromap_current, &buf[offset], sizeof(euromap_current));
-            offset += sizeof(euromap_current);
-            mb_data_.euromapVoltage = ntohs(euromap_voltage);
-            mb_data_.euromapCurrent = ntohs(euromap_current);
-        } else {
-            memcpy(&mb_data_.euromapVoltage, &buf[offset],
-                sizeof(mb_data_.euromapVoltage));
-            offset += sizeof(mb_data_.euromapVoltage);
-            mb_data_.euromapVoltage = ntohl(mb_data_.euromapVoltage);
-            memcpy(&mb_data_.euromapCurrent, &buf[offset],
-                sizeof(mb_data_.euromapCurrent));
-            offset += sizeof(mb_data_.euromapCurrent);
-            mb_data_.euromapCurrent = ntohl(mb_data_.euromapCurrent);
-        }
     }
+  }
 }
 
 double RobotState::getVersion()
 {
-    double ver;
+  double ver;
-    val_lock_.lock();
+  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();
-    val_lock_.unlock();
+  return ver;
-    return ver;
 }
 
 void RobotState::finishedReading()
 {
-    new_data_available_ = false;
+  new_data_available_ = false;
 }
 
 bool RobotState::getNewDataAvailable()
 {
-    return new_data_available_;
+  return new_data_available_;
 }
 
 int RobotState::getDigitalInputBits()
 {
-    return mb_data_.digitalInputBits;
+  return mb_data_.digitalInputBits;
 }
 int RobotState::getDigitalOutputBits()
 {
-    return mb_data_.digitalOutputBits;
+  return mb_data_.digitalOutputBits;
 }
 double RobotState::getAnalogInput0()
 {
-    return mb_data_.analogInput0;
+  return mb_data_.analogInput0;
 }
 double RobotState::getAnalogInput1()
 {
-    return mb_data_.analogInput1;
+  return mb_data_.analogInput1;
 }
 double RobotState::getAnalogOutput0()
 {
-    return mb_data_.analogOutput0;
+  return mb_data_.analogOutput0;
 }
 double RobotState::getAnalogOutput1()
 {
-    return mb_data_.analogOutput1;
+  return mb_data_.analogOutput1;
 }
 bool RobotState::isRobotConnected()
 {
-    return robot_mode_.isRobotConnected;
+  return robot_mode_.isRobotConnected;
 }
 bool RobotState::isRealRobotEnabled()
 {
-    return robot_mode_.isRealRobotEnabled;
+  return robot_mode_.isRealRobotEnabled;
 }
 bool RobotState::isPowerOnRobot()
 {
-    return robot_mode_.isPowerOnRobot;
+  return robot_mode_.isPowerOnRobot;
 }
 bool RobotState::isEmergencyStopped()
 {
-    return robot_mode_.isEmergencyStopped;
+  return robot_mode_.isEmergencyStopped;
 }
 bool RobotState::isProtectiveStopped()
 {
-    return robot_mode_.isProtectiveStopped;
+  return robot_mode_.isProtectiveStopped;
 }
 bool RobotState::isProgramRunning()
 {
-    return robot_mode_.isProgramRunning;
+  return robot_mode_.isProgramRunning;
 }
 bool RobotState::isProgramPaused()
 {
-    return robot_mode_.isProgramPaused;
+  return robot_mode_.isProgramPaused;
 }
 unsigned char RobotState::getRobotMode()
 {
-    return robot_mode_.robotMode;
+  return robot_mode_.robotMode;
 }
 bool RobotState::isReady()
 {
-    if (robot_mode_.robotMode == robot_mode_running_) {
+  if (robot_mode_.robotMode == robot_mode_running_)
-        return true;
+  {
-    }
+    return true;
-    return false;
+  }
+  return false;
 }
 
 void RobotState::setDisconnected()
 {
-    robot_mode_.isRobotConnected = false;
+  robot_mode_.isRobotConnected = false;
-    robot_mode_.isRealRobotEnabled = false;
+  robot_mode_.isRealRobotEnabled = false;
-    robot_mode_.isPowerOnRobot = false;
+  robot_mode_.isPowerOnRobot = false;
 }

src/robot_state_RT.cpp

@@ -20,456 +20,473 @@
 
 RobotStateRT::RobotStateRT(std::condition_variable& msg_cond)
 {
-    version_ = 0.0;
+  version_ = 0.0;
-    time_ = 0.0;
+  time_ = 0.0;
-    q_target_.assign(6, 0.0);
+  q_target_.assign(6, 0.0);
-    qd_target_.assign(6, 0.0);
+  qd_target_.assign(6, 0.0);
-    qdd_target_.assign(6, 0.0);
+  qdd_target_.assign(6, 0.0);
-    i_target_.assign(6, 0.0);
+  i_target_.assign(6, 0.0);
-    m_target_.assign(6, 0.0);
+  m_target_.assign(6, 0.0);
-    q_actual_.assign(6, 0.0);
+  q_actual_.assign(6, 0.0);
-    qd_actual_.assign(6, 0.0);
+  qd_actual_.assign(6, 0.0);
-    i_actual_.assign(6, 0.0);
+  i_actual_.assign(6, 0.0);
-    i_control_.assign(6, 0.0);
+  i_control_.assign(6, 0.0);
-    tool_vector_actual_.assign(6, 0.0);
+  tool_vector_actual_.assign(6, 0.0);
-    tcp_speed_actual_.assign(6, 0.0);
+  tcp_speed_actual_.assign(6, 0.0);
-    tcp_force_.assign(6, 0.0);
+  tcp_force_.assign(6, 0.0);
-    tool_vector_target_.assign(6, 0.0);
+  tool_vector_target_.assign(6, 0.0);
-    tcp_speed_target_.assign(6, 0.0);
+  tcp_speed_target_.assign(6, 0.0);
-    digital_input_bits_.assign(64, false);
+  digital_input_bits_.assign(64, false);
-    motor_temperatures_.assign(6, 0.0);
+  motor_temperatures_.assign(6, 0.0);
-    controller_timer_ = 0.0;
+  controller_timer_ = 0.0;
-    robot_mode_ = 0.0;
+  robot_mode_ = 0.0;
-    joint_modes_.assign(6, 0.0);
+  joint_modes_.assign(6, 0.0);
-    safety_mode_ = 0.0;
+  safety_mode_ = 0.0;
-    tool_accelerometer_values_.assign(3, 0.0);
+  tool_accelerometer_values_.assign(3, 0.0);
-    speed_scaling_ = 0.0;
+  speed_scaling_ = 0.0;
-    linear_momentum_norm_ = 0.0;
+  linear_momentum_norm_ = 0.0;
-    v_main_ = 0.0;
+  v_main_ = 0.0;
-    v_robot_ = 0.0;
+  v_robot_ = 0.0;
-    i_robot_ = 0.0;
+  i_robot_ = 0.0;
-    v_actual_.assign(6, 0.0);
+  v_actual_.assign(6, 0.0);
-    data_published_ = false;
+  data_published_ = false;
-    controller_updated_ = false;
+  controller_updated_ = false;
-    pMsg_cond_ = &msg_cond;
+  pMsg_cond_ = &msg_cond;
 }
 
 RobotStateRT::~RobotStateRT()
 {
-    /* Make sure nobody is waiting after this thread is destroyed */
+  /* Make sure nobody is waiting after this thread is destroyed */
-    data_published_ = true;
+  data_published_ = true;
-    controller_updated_ = true;
+  controller_updated_ = true;
-    pMsg_cond_->notify_all();
+  pMsg_cond_->notify_all();
 }
 
 void RobotStateRT::setDataPublished()
 {
-    data_published_ = false;
+  data_published_ = false;
 }
 bool RobotStateRT::getDataPublished()
 {
-    return data_published_;
+  return data_published_;
 }
 
 void RobotStateRT::setControllerUpdated()
 {
-    controller_updated_ = false;
+  controller_updated_ = false;
 }
 bool RobotStateRT::getControllerUpdated()
 {
-    return controller_updated_;
+  return controller_updated_;
 }
 
 double RobotStateRT::ntohd(uint64_t nf)
 {
-    double x;
+  double x;
-    nf = be64toh(nf);
+  nf = be64toh(nf);
-    memcpy(&x, &nf, sizeof(x));
+  memcpy(&x, &nf, sizeof(x));
-    return x;
+  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;
+  uint64_t q;
-    std::vector<double> ret;
+  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));
+    memcpy(&q, &buf[start_index + i * sizeof(q)], sizeof(q));
-    }
+    ret.push_back(ntohd(q));
-    return ret;
+  }
+  return ret;
 }
 
 std::vector<bool> RobotStateRT::unpackDigitalInputBits(int64_t data)
 {
-    std::vector<bool> ret;
+  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);
+  {
-    }
+    ret.push_back((data & (1 << i)) >> i);
-    return ret;
+  }
+  return ret;
 }
 
 void RobotStateRT::setVersion(double ver)
 {
-    val_lock_.lock();
+  val_lock_.lock();
-    version_ = ver;
+  version_ = ver;
-    val_lock_.unlock();
+  val_lock_.unlock();
 }
 
 double RobotStateRT::getVersion()
 {
-    double ret;
+  double ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = version_;
+  ret = version_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 double RobotStateRT::getTime()
 {
-    double ret;
+  double ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = time_;
+  ret = time_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 std::vector<double> RobotStateRT::getQTarget()
 {
-    std::vector<double> ret;
+  std::vector<double> ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = q_target_;
+  ret = q_target_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 std::vector<double> RobotStateRT::getQdTarget()
 {
-    std::vector<double> ret;
+  std::vector<double> ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = qd_target_;
+  ret = qd_target_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 std::vector<double> RobotStateRT::getQddTarget()
 {
-    std::vector<double> ret;
+  std::vector<double> ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = qdd_target_;
+  ret = qdd_target_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 std::vector<double> RobotStateRT::getITarget()
 {
-    std::vector<double> ret;
+  std::vector<double> ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = i_target_;
+  ret = i_target_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 std::vector<double> RobotStateRT::getMTarget()
 {
-    std::vector<double> ret;
+  std::vector<double> ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = m_target_;
+  ret = m_target_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 std::vector<double> RobotStateRT::getQActual()
 {
-    std::vector<double> ret;
+  std::vector<double> ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = q_actual_;
+  ret = q_actual_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 std::vector<double> RobotStateRT::getQdActual()
 {
-    std::vector<double> ret;
+  std::vector<double> ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = qd_actual_;
+  ret = qd_actual_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 std::vector<double> RobotStateRT::getIActual()
 {
-    std::vector<double> ret;
+  std::vector<double> ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = i_actual_;
+  ret = i_actual_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 std::vector<double> RobotStateRT::getIControl()
 {
-    std::vector<double> ret;
+  std::vector<double> ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = i_control_;
+  ret = i_control_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 std::vector<double> RobotStateRT::getToolVectorActual()
 {
-    std::vector<double> ret;
+  std::vector<double> ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = tool_vector_actual_;
+  ret = tool_vector_actual_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 std::vector<double> RobotStateRT::getTcpSpeedActual()
 {
-    std::vector<double> ret;
+  std::vector<double> ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = tcp_speed_actual_;
+  ret = tcp_speed_actual_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 std::vector<double> RobotStateRT::getTcpForce()
 {
-    std::vector<double> ret;
+  std::vector<double> ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = tcp_force_;
+  ret = tcp_force_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 std::vector<double> RobotStateRT::getToolVectorTarget()
 {
-    std::vector<double> ret;
+  std::vector<double> ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = tool_vector_target_;
+  ret = tool_vector_target_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 std::vector<double> RobotStateRT::getTcpSpeedTarget()
 {
-    std::vector<double> ret;
+  std::vector<double> ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = tcp_speed_target_;
+  ret = tcp_speed_target_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 std::vector<bool> RobotStateRT::getDigitalInputBits()
 {
-    std::vector<bool> ret;
+  std::vector<bool> ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = digital_input_bits_;
+  ret = digital_input_bits_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 std::vector<double> RobotStateRT::getMotorTemperatures()
 {
-    std::vector<double> ret;
+  std::vector<double> ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = motor_temperatures_;
+  ret = motor_temperatures_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 double RobotStateRT::getControllerTimer()
 {
-    double ret;
+  double ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = controller_timer_;
+  ret = controller_timer_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 double RobotStateRT::getRobotMode()
 {
-    double ret;
+  double ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = robot_mode_;
+  ret = robot_mode_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 std::vector<double> RobotStateRT::getJointModes()
 {
-    std::vector<double> ret;
+  std::vector<double> ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = joint_modes_;
+  ret = joint_modes_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 double RobotStateRT::getSafety_mode()
 {
-    double ret;
+  double ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = safety_mode_;
+  ret = safety_mode_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 std::vector<double> RobotStateRT::getToolAccelerometerValues()
 {
-    std::vector<double> ret;
+  std::vector<double> ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = tool_accelerometer_values_;
+  ret = tool_accelerometer_values_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 double RobotStateRT::getSpeedScaling()
 {
-    double ret;
+  double ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = speed_scaling_;
+  ret = speed_scaling_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 double RobotStateRT::getLinearMomentumNorm()
 {
-    double ret;
+  double ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = linear_momentum_norm_;
+  ret = linear_momentum_norm_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 double RobotStateRT::getVMain()
 {
-    double ret;
+  double ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = v_main_;
+  ret = v_main_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 double RobotStateRT::getVRobot()
 {
-    double ret;
+  double ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = v_robot_;
+  ret = v_robot_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 double RobotStateRT::getIRobot()
 {
-    double ret;
+  double ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = i_robot_;
+  ret = i_robot_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 std::vector<double> RobotStateRT::getVActual()
 {
-    std::vector<double> ret;
+  std::vector<double> ret;
-    val_lock_.lock();
+  val_lock_.lock();
-    ret = v_actual_;
+  ret = v_actual_;
-    val_lock_.unlock();
+  val_lock_.unlock();
-    return ret;
+  return ret;
 }
 void RobotStateRT::unpack(uint8_t* buf)
 {
-    int64_t digital_input_bits;
+  int64_t digital_input_bits;
-    uint64_t unpack_to;
+  uint64_t unpack_to;
-    uint16_t offset = 0;
+  uint16_t offset = 0;
-    val_lock_.lock();
+  val_lock_.lock();
-    int len;
+  int len;
-    memcpy(&len, &buf[offset], sizeof(len));
+  memcpy(&len, &buf[offset], sizeof(len));
 
-    offset += sizeof(len);
+  offset += sizeof(len);
-    len = ntohl(len);
+  len = ntohl(len);
 
-    //Check the correct message length is received
+  // Check the correct message length is received
-    bool len_good = true;
+  bool len_good = true;
-    if (version_ >= 1.6 && version_ < 1.7) { //v1.6
+  if (version_ >= 1.6 && version_ < 1.7)
-        if (len != 756)
+  {  // v1.6
-            len_good = false;
+    if (len != 756)
-    } else if (version_ >= 1.7 && version_ < 1.8) { //v1.7
+      len_good = false;
-        if (len != 764)
+  }
-            len_good = false;
+  else if (version_ >= 1.7 && version_ < 1.8)
-    } else if (version_ >= 1.8 && version_ < 1.9) { //v1.8
+  {  // v1.7
-        if (len != 812)
+    if (len != 764)
-            len_good = false;
+      len_good = false;
-    } else if (version_ >= 3.0 && version_ < 3.2) { //v3.0 & v3.1
+  }
-        if (len != 1044)
+  else if (version_ >= 1.8 && version_ < 1.9)
-            len_good = false;
+  {  // v1.8
-    } else if (version_ >= 3.2 && version_ < 3.3) { //v3.2
+    if (len != 812)
-        if (len != 1060)
+      len_good = false;
-            len_good = false;
+  }
-    }
+  else if (version_ >= 3.0 && version_ < 3.2)
+  {  // v3.0 & v3.1
+    if (len != 1044)
+      len_good = false;
+  }
+  else if (version_ >= 3.2 && version_ < 3.3)
+  {  // v3.2
+    if (len != 1060)
+      len_good = false;
+  }
 
-    if (!len_good) {
+  if (!len_good)
-        printf("Wrong length of message on RT interface: %i\n", len);
+  {
-        val_lock_.unlock();
+    printf("Wrong length of message on RT interface: %i\n", len);
-        return;
-    }
-
-    memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
-    time_ = RobotStateRT::ntohd(unpack_to);
-    offset += sizeof(double);
-    q_target_ = unpackVector(buf, offset, 6);
-    offset += sizeof(double) * 6;
-    qd_target_ = unpackVector(buf, offset, 6);
-    offset += sizeof(double) * 6;
-    qdd_target_ = unpackVector(buf, offset, 6);
-    offset += sizeof(double) * 6;
-    i_target_ = unpackVector(buf, offset, 6);
-    offset += sizeof(double) * 6;
-    m_target_ = unpackVector(buf, offset, 6);
-    offset += sizeof(double) * 6;
-    q_actual_ = unpackVector(buf, offset, 6);
-    offset += sizeof(double) * 6;
-    qd_actual_ = unpackVector(buf, offset, 6);
-    offset += sizeof(double) * 6;
-    i_actual_ = unpackVector(buf, offset, 6);
-    offset += sizeof(double) * 6;
-    if (version_ <= 1.9) {
-        if (version_ > 1.6)
-            tool_accelerometer_values_ = unpackVector(buf, offset, 3);
-        offset += sizeof(double) * (3 + 15);
-        tcp_force_ = unpackVector(buf, offset, 6);
-        offset += sizeof(double) * 6;
-        tool_vector_actual_ = unpackVector(buf, offset, 6);
-        offset += sizeof(double) * 6;
-        tcp_speed_actual_ = unpackVector(buf, offset, 6);
-    } else {
-        i_control_ = unpackVector(buf, offset, 6);
-        offset += sizeof(double) * 6;
-        tool_vector_actual_ = unpackVector(buf, offset, 6);
-        offset += sizeof(double) * 6;
-        tcp_speed_actual_ = unpackVector(buf, offset, 6);
-        offset += sizeof(double) * 6;
-        tcp_force_ = unpackVector(buf, offset, 6);
-        offset += sizeof(double) * 6;
-        tool_vector_target_ = unpackVector(buf, offset, 6);
-        offset += sizeof(double) * 6;
-        tcp_speed_target_ = unpackVector(buf, offset, 6);
-    }
-    offset += sizeof(double) * 6;
-
-    memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits));
-    digital_input_bits_ = unpackDigitalInputBits(be64toh(digital_input_bits));
-    offset += sizeof(double);
-    motor_temperatures_ = unpackVector(buf, offset, 6);
-    offset += sizeof(double) * 6;
-    memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
-    controller_timer_ = ntohd(unpack_to);
-    if (version_ > 1.6) {
-        offset += sizeof(double) * 2;
-        memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
-        robot_mode_ = ntohd(unpack_to);
-        if (version_ > 1.7) {
-            offset += sizeof(double);
-            joint_modes_ = unpackVector(buf, offset, 6);
-        }
-    }
-    if (version_ > 1.8) {
-        offset += sizeof(double) * 6;
-        memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
-        safety_mode_ = ntohd(unpack_to);
-        offset += sizeof(double);
-        tool_accelerometer_values_ = unpackVector(buf, offset, 3);
-        offset += sizeof(double) * 3;
-        memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
-        speed_scaling_ = ntohd(unpack_to);
-        offset += sizeof(double);
-        memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
-        linear_momentum_norm_ = ntohd(unpack_to);
-        offset += sizeof(double);
-        memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
-        v_main_ = ntohd(unpack_to);
-        offset += sizeof(double);
-        memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
-        v_robot_ = ntohd(unpack_to);
-        offset += sizeof(double);
-        memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
-        i_robot_ = ntohd(unpack_to);
-        offset += sizeof(double);
-        v_actual_ = unpackVector(buf, offset, 6);
-    }
     val_lock_.unlock();
-    controller_updated_ = true;
+    return;
-    data_published_ = true;
+  }
-    pMsg_cond_->notify_all();
+
+  memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+  time_ = RobotStateRT::ntohd(unpack_to);
+  offset += sizeof(double);
+  q_target_ = unpackVector(buf, offset, 6);
+  offset += sizeof(double) * 6;
+  qd_target_ = unpackVector(buf, offset, 6);
+  offset += sizeof(double) * 6;
+  qdd_target_ = unpackVector(buf, offset, 6);
+  offset += sizeof(double) * 6;
+  i_target_ = unpackVector(buf, offset, 6);
+  offset += sizeof(double) * 6;
+  m_target_ = unpackVector(buf, offset, 6);
+  offset += sizeof(double) * 6;
+  q_actual_ = unpackVector(buf, offset, 6);
+  offset += sizeof(double) * 6;
+  qd_actual_ = unpackVector(buf, offset, 6);
+  offset += sizeof(double) * 6;
+  i_actual_ = unpackVector(buf, offset, 6);
+  offset += sizeof(double) * 6;
+  if (version_ <= 1.9)
+  {
+    if (version_ > 1.6)
+      tool_accelerometer_values_ = unpackVector(buf, offset, 3);
+    offset += sizeof(double) * (3 + 15);
+    tcp_force_ = unpackVector(buf, offset, 6);
+    offset += sizeof(double) * 6;
+    tool_vector_actual_ = unpackVector(buf, offset, 6);
+    offset += sizeof(double) * 6;
+    tcp_speed_actual_ = unpackVector(buf, offset, 6);
+  }
+  else
+  {
+    i_control_ = unpackVector(buf, offset, 6);
+    offset += sizeof(double) * 6;
+    tool_vector_actual_ = unpackVector(buf, offset, 6);
+    offset += sizeof(double) * 6;
+    tcp_speed_actual_ = unpackVector(buf, offset, 6);
+    offset += sizeof(double) * 6;
+    tcp_force_ = unpackVector(buf, offset, 6);
+    offset += sizeof(double) * 6;
+    tool_vector_target_ = unpackVector(buf, offset, 6);
+    offset += sizeof(double) * 6;
+    tcp_speed_target_ = unpackVector(buf, offset, 6);
+  }
+  offset += sizeof(double) * 6;
+
+  memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits));
+  digital_input_bits_ = unpackDigitalInputBits(be64toh(digital_input_bits));
+  offset += sizeof(double);
+  motor_temperatures_ = unpackVector(buf, offset, 6);
+  offset += sizeof(double) * 6;
+  memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+  controller_timer_ = ntohd(unpack_to);
+  if (version_ > 1.6)
+  {
+    offset += sizeof(double) * 2;
+    memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+    robot_mode_ = ntohd(unpack_to);
+    if (version_ > 1.7)
+    {
+      offset += sizeof(double);
+      joint_modes_ = unpackVector(buf, offset, 6);
+    }
+  }
+  if (version_ > 1.8)
+  {
+    offset += sizeof(double) * 6;
+    memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+    safety_mode_ = ntohd(unpack_to);
+    offset += sizeof(double);
+    tool_accelerometer_values_ = unpackVector(buf, offset, 3);
+    offset += sizeof(double) * 3;
+    memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+    speed_scaling_ = ntohd(unpack_to);
+    offset += sizeof(double);
+    memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+    linear_momentum_norm_ = ntohd(unpack_to);
+    offset += sizeof(double);
+    memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+    v_main_ = ntohd(unpack_to);
+    offset += sizeof(double);
+    memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+    v_robot_ = ntohd(unpack_to);
+    offset += sizeof(double);
+    memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+    i_robot_ = ntohd(unpack_to);
+    offset += sizeof(double);
+    v_actual_ = unpackVector(buf, offset, 6);
+  }
+  val_lock_.unlock();
+  controller_updated_ = true;
+  data_published_ = true;
+  pMsg_cond_->notify_all();
 }

src/ros/rt_publisher.cpp

@@ -1,119 +1,122 @@
 #include "ur_modern_driver/ros/rt_publisher.h"
 
-bool RTPublisher::publish_joints(RTShared& packet, Time& t)
+bool RTPublisher::publishJoints(RTShared& packet, Time& t)
 {
-    sensor_msgs::JointState joint_msg;
+  sensor_msgs::JointState joint_msg;
-    joint_msg.header.stamp = t;
+  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);
+  {
-    }
+    joint_msg.position.push_back(q);
-    for (auto const& qd : packet.qd_actual) {
+  }
-        joint_msg.velocity.push_back(qd);
+  for (auto const& qd : packet.qd_actual)
-    }
+  {
-    for (auto const& i : packet.i_actual) {
+    joint_msg.velocity.push_back(qd);
-        joint_msg.effort.push_back(i);
+  }
-    }
+  for (auto const& i : packet.i_actual)
+  {
+    joint_msg.effort.push_back(i);
+  }
 
-    _joint_pub.publish(joint_msg);
+  joint_pub_.publish(joint_msg);
 
-    return true;
+  return true;
 }
 
-bool RTPublisher::publish_wrench(RTShared& packet, Time& t)
+bool RTPublisher::publishWrench(RTShared& packet, Time& t)
 {
-    geometry_msgs::WrenchStamped wrench_msg;
+  geometry_msgs::WrenchStamped wrench_msg;
-    wrench_msg.header.stamp = t;
+  wrench_msg.header.stamp = t;
-    wrench_msg.wrench.force.x = packet.tcp_force[0];
+  wrench_msg.wrench.force.x = packet.tcp_force[0];
-    wrench_msg.wrench.force.y = packet.tcp_force[1];
+  wrench_msg.wrench.force.y = packet.tcp_force[1];
-    wrench_msg.wrench.force.z = packet.tcp_force[2];
+  wrench_msg.wrench.force.z = packet.tcp_force[2];
-    wrench_msg.wrench.torque.x = packet.tcp_force[3];
+  wrench_msg.wrench.torque.x = packet.tcp_force[3];
-    wrench_msg.wrench.torque.y = packet.tcp_force[4];
+  wrench_msg.wrench.torque.y = packet.tcp_force[4];
-    wrench_msg.wrench.torque.z = packet.tcp_force[5];
+  wrench_msg.wrench.torque.z = packet.tcp_force[5];
 
-    _wrench_pub.publish(wrench_msg);
+  wrench_pub_.publish(wrench_msg);
-    return true;
+  return true;
 }
 
-bool RTPublisher::publish_tool(RTShared& packet, Time& t)
+bool RTPublisher::publishTool(RTShared& packet, Time& t)
 {
-    geometry_msgs::TwistStamped tool_twist;
+  geometry_msgs::TwistStamped tool_twist;
-    tool_twist.header.stamp = t;
+  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.x = packet.tcp_speed_actual.position.x;
-    tool_twist.twist.linear.y = packet.tcp_speed_actual.position.y;
+  tool_twist.twist.linear.y = packet.tcp_speed_actual.position.y;
-    tool_twist.twist.linear.z = packet.tcp_speed_actual.position.z;
+  tool_twist.twist.linear.z = packet.tcp_speed_actual.position.z;
-    tool_twist.twist.angular.x = packet.tcp_speed_actual.rotation.x;
+  tool_twist.twist.angular.x = packet.tcp_speed_actual.rotation.x;
-    tool_twist.twist.angular.y = packet.tcp_speed_actual.rotation.y;
+  tool_twist.twist.angular.y = packet.tcp_speed_actual.rotation.y;
-    tool_twist.twist.angular.z = packet.tcp_speed_actual.rotation.z;
+  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;
+  return true;
 }
 
-bool RTPublisher::publish_transform(RTShared& packet, Time& t)
+bool RTPublisher::publishTransform(RTShared& packet, Time& t)
 {
+  auto tv = packet.tool_vector_actual;
 
-    auto tv = packet.tool_vector_actual;
+  Transform transform;
+  transform.setOrigin(Vector3(tv.position.x, tv.position.y, tv.position.z));
 
-    Transform transform;
+  // Create quaternion
-    transform.setOrigin(Vector3(tv.position.x, tv.position.y, tv.position.z));
+  Quaternion quat;
 
-    //Create quaternion
+  double angle = std::sqrt(std::pow(tv.rotation.x, 2) + std::pow(tv.rotation.y, 2) + std::pow(tv.rotation.z, 2));
-    Quaternion quat;
+  if (angle < 1e-16)
+  {
+    quat.setValue(0, 0, 0, 1);
+  }
+  else
+  {
+    quat.setRotation(Vector3(tv.rotation.x / angle, tv.rotation.y / angle, tv.rotation.z / angle), angle);
+  }
 
-    double angle = std::sqrt(std::pow(tv.rotation.x, 2) + std::pow(tv.rotation.y, 2) + std::pow(tv.rotation.z, 2));
+  transform.setRotation(quat);
-    if (angle < 1e-16) {
-        quat.setValue(0, 0, 0, 1);
-    } else {
-        quat.setRotation(Vector3(tv.rotation.x / angle, tv.rotation.y / angle, tv.rotation.z / angle), angle);
-    }
 
-    transform.setRotation(quat);
+  transform_broadcaster_.sendTransform(StampedTransform(transform, t, base_frame_, tool_frame_));
 
-    _transform_broadcaster.sendTransform(StampedTransform(transform, t, _base_frame, _tool_frame));
+  return true;
-
-    return true;
 }
 
-bool RTPublisher::publish_temperature(RTShared& packet, Time& t)
+bool RTPublisher::publishTemperature(RTShared& packet, Time& t)
 {
-    size_t len = _joint_names.size();
+  size_t len = joint_names_.size();
-    for (size_t i = 0; i < len; i++) {
+  for (size_t i = 0; i < len; i++)
-        sensor_msgs::Temperature msg;
+  {
-        msg.header.stamp = t;
+    sensor_msgs::Temperature msg;
-        msg.header.frame_id = _joint_names[i];
+    msg.header.stamp = t;
-        msg.temperature = packet.motor_temperatures[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;
+  return true;
 }
 
 bool RTPublisher::publish(RTShared& packet)
 {
-    Time time = Time::now();
+  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)
 {
-    return publish(state);
+  return publish(state);
 }
 bool RTPublisher::consume(RTState_V1_8& state)
 {
-    return publish(state);
+  return publish(state);
 }
 bool RTPublisher::consume(RTState_V3_0__1& state)
 {
-    return publish(state);
+  return publish(state);
 }
 bool RTPublisher::consume(RTState_V3_2__3& state)
 {
-    return publish(state);
+  return publish(state);
 }

src/ros_main.cpp

@@ -1,6 +1,6 @@
+#include <ros/ros.h>
 #include <chrono>
 #include <cstdlib>
-#include <ros/ros.h>
 #include <string>
 #include <thread>
 
@@ -21,55 +21,59 @@ static const std::string PREFIX_ARG("~prefix");
 static const std::string BASE_FRAME_ARG("~base_frame");
 static const std::string TOOL_FRAME_ARG("~tool_frame");
 
-struct ProgArgs {
+struct ProgArgs
+{
 public:
-    std::string host;
+  std::string host;
-    std::string prefix;
+  std::string prefix;
-    std::string base_frame;
+  std::string base_frame;
-    std::string tool_frame;
+  std::string tool_frame;
-    int32_t reverse_port;
+  int32_t reverse_port;
-    bool use_sim_time;
+  bool use_sim_time;
 };
 
 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;
+    LOG_ERROR("robot_ip_address parameter must be set!");
-    }
+    return false;
-    ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001));
+  }
-    ros::param::param(SIM_TIME_ARG, args.use_sim_time, false);
+  ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001));
-    ros::param::param(PREFIX_ARG, args.prefix, std::string());
+  ros::param::param(SIM_TIME_ARG, args.use_sim_time, false);
-    ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link");
+  ros::param::param(PREFIX_ARG, args.prefix, std::string());
-    ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller");
+  ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link");
-    return true;
+  ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller");
+  return true;
 }
 
 int main(int argc, char** argv)
 {
-    ros::init(argc, argv, "ur_driver");
+  ros::init(argc, argv, "ur_driver");
 
-    ProgArgs args;
+  ProgArgs args;
-    if (!parse_args(args)) {
+  if (!parse_args(args))
-        return EXIT_FAILURE;
+  {
-    }
+    return EXIT_FAILURE;
+  }
 
-    URFactory factory(args.host);
+  URFactory factory(args.host);
-    auto parser = factory.get_rt_parser();
+  auto parser = factory.getRTParser();
 
-    URStream s2(args.host, 30003);
+  URStream s2(args.host, 30003);
-    URProducer<RTPacket> p2(s2, *parser);
+  URProducer<RTPacket> p2(s2, *parser);
-    RTPublisher pub(args.prefix, args.base_frame, args.tool_frame);
+  RTPublisher pub(args.prefix, args.base_frame, args.tool_frame);
 
-    Pipeline<RTPacket> pl(p2, pub);
+  Pipeline<RTPacket> pl(p2, pub);
 
-    pl.run();
+  pl.run();
 
-    while (ros::ok()) {
+  while (ros::ok())
-        std::this_thread::sleep_for(std::chrono::milliseconds(33));
+  {
-    }
+    std::this_thread::sleep_for(std::chrono::milliseconds(33));
+  }
 
-    pl.stop();
+  pl.stop();
 
-    return EXIT_SUCCESS;
+  return EXIT_SUCCESS;
 }

src/ur/master_board.cpp

@@ -1,97 +1,99 @@
 #include "ur_modern_driver/ur/master_board.h"
 #include "ur_modern_driver/ur/consumer.h"
 
-bool SharedMasterBoardData::parse_with(BinParser& bp)
+bool SharedMasterBoardData::parseWith(BinParser& bp)
 {
-    bp.parse(analog_input_range0);
+  bp.parse(analog_input_range0);
-    bp.parse(analog_input_range1);
+  bp.parse(analog_input_range1);
-    bp.parse(analog_input0);
+  bp.parse(analog_input0);
-    bp.parse(analog_input1);
+  bp.parse(analog_input1);
-    bp.parse(analog_output_domain0);
+  bp.parse(analog_output_domain0);
-    bp.parse(analog_output_domain1);
+  bp.parse(analog_output_domain1);
-    bp.parse(analog_output0);
+  bp.parse(analog_output0);
-    bp.parse(analog_output1);
+  bp.parse(analog_output1);
-    bp.parse(master_board_temperature);
+  bp.parse(master_board_temperature);
-    bp.parse(robot_voltage_48V);
+  bp.parse(robot_voltage_48V);
-    bp.parse(robot_current);
+  bp.parse(robot_current);
-    bp.parse(master_IO_current);
+  bp.parse(master_IO_current);
-    return true;
+  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;
+    return false;
 
-    bp.parse(digital_input_bits);
+  bp.parse(digital_input_bits);
-    bp.parse(digital_output_bits);
+  bp.parse(digital_output_bits);
 
-    SharedMasterBoardData::parse_with(bp);
+  SharedMasterBoardData::parseWith(bp);
 
-    bp.parse(master_safety_state);
+  bp.parse(master_safety_state);
-    bp.parse(master_on_off_state);
+  bp.parse(master_on_off_state);
-    bp.parse(euromap67_interface_installed);
+  bp.parse(euromap67_interface_installed);
 
-    if (euromap67_interface_installed) {
+  if (euromap67_interface_installed)
-        if (!bp.check_size(MasterBoardData_V1_X::EURO_SIZE))
+  {
-            return false;
+    if (!bp.checkSize(MasterBoardData_V1_X::EURO_SIZE))
+      return false;
 
-        bp.parse(euromap_voltage);
+    bp.parse(euromap_voltage);
-        bp.parse(euromap_current);
+    bp.parse(euromap_current);
-    }
+  }
 
-    return true;
+  return true;
 }
 
-bool MasterBoardData_V3_0__1::parse_with(BinParser& bp)
+bool MasterBoardData_V3_0__1::parseWith(BinParser& bp)
 {
-    if (!bp.check_size<MasterBoardData_V3_0__1>())
+  if (!bp.checkSize<MasterBoardData_V3_0__1>())
-        return false;
+    return false;
 
-    bp.parse(digital_input_bits);
+  bp.parse(digital_input_bits);
-    bp.parse(digital_output_bits);
+  bp.parse(digital_output_bits);
 
-    SharedMasterBoardData::parse_with(bp);
+  SharedMasterBoardData::parseWith(bp);
 
-    bp.parse(safety_mode);
+  bp.parse(safety_mode);
-    bp.parse(in_reduced_mode);
+  bp.parse(in_reduced_mode);
-    bp.parse(euromap67_interface_installed);
+  bp.parse(euromap67_interface_installed);
 
-    if (euromap67_interface_installed) {
+  if (euromap67_interface_installed)
-        if (!bp.check_size(MasterBoardData_V3_0__1::EURO_SIZE))
+  {
-            return false;
+    if (!bp.checkSize(MasterBoardData_V3_0__1::EURO_SIZE))
+      return false;
 
-        bp.parse(euromap_voltage);
+    bp.parse(euromap_voltage);
-        bp.parse(euromap_current);
+    bp.parse(euromap_current);
-    }
+  }
 
-    bp.consume(sizeof(uint32_t));
+  bp.consume(sizeof(uint32_t));
 
-    return true;
+  return true;
 }
 
-bool MasterBoardData_V3_2::parse_with(BinParser& bp)
+bool MasterBoardData_V3_2::parseWith(BinParser& bp)
 {
-    if (!bp.check_size<MasterBoardData_V3_2>())
+  if (!bp.checkSize<MasterBoardData_V3_2>())
-        return false;
+    return false;
 
-    MasterBoardData_V3_0__1::parse_with(bp);
+  MasterBoardData_V3_0__1::parseWith(bp);
 
-    bp.parse(operational_mode_selector_input);
+  bp.parse(operational_mode_selector_input);
-    bp.parse(three_position_enabling_device_input);
+  bp.parse(three_position_enabling_device_input);
 
-    return true;
+  return true;
 }
 
-bool MasterBoardData_V1_X::consume_with(URStatePacketConsumer& consumer)
+bool MasterBoardData_V1_X::consumeWith(URStatePacketConsumer& consumer)
 {
-    return consumer.consume(*this);
+  return consumer.consume(*this);
 }
-bool MasterBoardData_V3_0__1::consume_with(URStatePacketConsumer& consumer)
+bool MasterBoardData_V3_0__1::consumeWith(URStatePacketConsumer& consumer)
 {
-    return consumer.consume(*this);
+  return consumer.consume(*this);
 }
-bool MasterBoardData_V3_2::consume_with(URStatePacketConsumer& consumer)
+bool MasterBoardData_V3_2::consumeWith(URStatePacketConsumer& consumer)
 {
-    return consumer.consume(*this);
+  return consumer.consume(*this);
 }

src/ur/messages.cpp

@@ -1,20 +1,19 @@
 #include "ur_modern_driver/ur/messages.h"
 #include "ur_modern_driver/ur/consumer.h"
 
-bool VersionMessage::parse_with(BinParser& bp)
+bool VersionMessage::parseWith(BinParser& bp)
 {
+  bp.parse(project_name);
+  bp.parse(major_version);
+  bp.parse(minor_version);
+  bp.parse(svn_version);
+  bp.consume(sizeof(uint32_t));  // undocumented field??
+  bp.parse_remainder(build_date);
 
-    bp.parse(project_name);
+  return true;  // not really possible to check dynamic size packets
-    bp.parse(major_version);
-    bp.parse(minor_version);
-    bp.parse(svn_version);
-    bp.consume(sizeof(uint32_t)); //undocumented field??
-    bp.parse_remainder(build_date);
-
-    return true; //not really possible to check dynamic size packets
 }
 
-bool VersionMessage::consume_with(URMessagePacketConsumer& consumer)
+bool VersionMessage::consumeWith(URMessagePacketConsumer& consumer)
 {
-    return consumer.consume(*this);
+  return consumer.consume(*this);
 }

src/ur/robot_mode.cpp

@@ -1,71 +1,71 @@
 #include "ur_modern_driver/ur/robot_mode.h"
 #include "ur_modern_driver/ur/consumer.h"
 
-bool SharedRobotModeData::parse_with(BinParser& bp)
+bool SharedRobotModeData::parseWith(BinParser& bp)
 {
-    bp.parse(timestamp);
+  bp.parse(timestamp);
-    bp.parse(physical_robot_connected);
+  bp.parse(physical_robot_connected);
-    bp.parse(real_robot_enabled);
+  bp.parse(real_robot_enabled);
-    bp.parse(robot_power_on);
+  bp.parse(robot_power_on);
-    bp.parse(emergency_stopped);
+  bp.parse(emergency_stopped);
-    return true;
+  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;
+    return false;
 
-    SharedRobotModeData::parse_with(bp);
+  SharedRobotModeData::parseWith(bp);
 
-    bp.parse(security_stopped);
+  bp.parse(security_stopped);
-    bp.parse(program_running);
+  bp.parse(program_running);
-    bp.parse(program_paused);
+  bp.parse(program_paused);
-    bp.parse(robot_mode);
+  bp.parse(robot_mode);
-    bp.parse(speed_fraction);
+  bp.parse(speed_fraction);
 
-    return true;
+  return true;
 }
 
-bool RobotModeData_V3_0__1::parse_with(BinParser& bp)
+bool RobotModeData_V3_0__1::parseWith(BinParser& bp)
 {
-    if (!bp.check_size<RobotModeData_V3_0__1>())
+  if (!bp.checkSize<RobotModeData_V3_0__1>())
-        return false;
+    return false;
 
-    SharedRobotModeData::parse_with(bp);
+  SharedRobotModeData::parseWith(bp);
 
-    bp.parse(protective_stopped);
+  bp.parse(protective_stopped);
-    bp.parse(program_running);
+  bp.parse(program_running);
-    bp.parse(program_paused);
+  bp.parse(program_paused);
-    bp.parse(robot_mode);
+  bp.parse(robot_mode);
-    bp.parse(control_mode);
+  bp.parse(control_mode);
-    bp.parse(target_speed_fraction);
+  bp.parse(target_speed_fraction);
-    bp.parse(speed_scaling);
+  bp.parse(speed_scaling);
 
-    return true;
+  return true;
 }
 
-bool RobotModeData_V3_2::parse_with(BinParser& bp)
+bool RobotModeData_V3_2::parseWith(BinParser& bp)
 {
-    if (!bp.check_size<RobotModeData_V3_2>())
+  if (!bp.checkSize<RobotModeData_V3_2>())
-        return false;
+    return false;
 
-    RobotModeData_V3_0__1::parse_with(bp);
+  RobotModeData_V3_0__1::parseWith(bp);
 
-    bp.parse(target_speed_fraction_limit);
+  bp.parse(target_speed_fraction_limit);
 
-    return true;
+  return true;
 }
 
-bool RobotModeData_V1_X::consume_with(URStatePacketConsumer& consumer)
+bool RobotModeData_V1_X::consumeWith(URStatePacketConsumer& consumer)
 {
-    return consumer.consume(*this);
+  return consumer.consume(*this);
 }
-bool RobotModeData_V3_0__1::consume_with(URStatePacketConsumer& consumer)
+bool RobotModeData_V3_0__1::consumeWith(URStatePacketConsumer& consumer)
 {
-    return consumer.consume(*this);
+  return consumer.consume(*this);
 }
-bool RobotModeData_V3_2::consume_with(URStatePacketConsumer& consumer)
+bool RobotModeData_V3_2::consumeWith(URStatePacketConsumer& consumer)
 {
-    return consumer.consume(*this);
+  return consumer.consume(*this);
 }

src/ur/rt_state.cpp

@@ -3,115 +3,115 @@
 
 void RTShared::parse_shared1(BinParser& bp)
 {
-    bp.parse(time);
+  bp.parse(time);
-    bp.parse(q_target);
+  bp.parse(q_target);
-    bp.parse(qd_target);
+  bp.parse(qd_target);
-    bp.parse(qdd_target);
+  bp.parse(qdd_target);
-    bp.parse(i_target);
+  bp.parse(i_target);
-    bp.parse(m_target);
+  bp.parse(m_target);
-    bp.parse(q_actual);
+  bp.parse(q_actual);
-    bp.parse(qd_actual);
+  bp.parse(qd_actual);
-    bp.parse(i_actual);
+  bp.parse(i_actual);
 }
 
 void RTShared::parse_shared2(BinParser& bp)
 {
-    bp.parse(digital_inputs);
+  bp.parse(digital_inputs);
-    bp.parse(motor_temperatures);
+  bp.parse(motor_temperatures);
-    bp.parse(controller_time);
+  bp.parse(controller_time);
-    bp.consume(sizeof(double)); //Unused "Test value" field
+  bp.consume(sizeof(double));  // Unused "Test value" field
-    bp.parse(robot_mode);
+  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;
+    return false;
 
-    parse_shared1(bp);
+  parse_shared1(bp);
 
-    bp.parse(tool_accelerometer_values);
+  bp.parse(tool_accelerometer_values);
-    bp.consume(sizeof(double)*15);
+  bp.consume(sizeof(double) * 15);
-    bp.parse(tcp_force);
+  bp.parse(tcp_force);
-    bp.parse(tool_vector_actual);
+  bp.parse(tool_vector_actual);
-    bp.parse(tcp_speed_actual);
+  bp.parse(tcp_speed_actual);
 
-    parse_shared2(bp);
+  parse_shared2(bp);
 
-    return true;
+  return true;
 }
 
-bool RTState_V1_8::parse_with(BinParser& bp)
+bool RTState_V1_8::parseWith(BinParser& bp)
 {
-    if (!bp.check_size<RTState_V1_8>())
+  if (!bp.checkSize<RTState_V1_8>())
-        return false;
+    return false;
 
-    RTState_V1_6__7::parse_with(bp);
+  RTState_V1_6__7::parseWith(bp);
 
-    bp.parse(joint_modes);
+  bp.parse(joint_modes);
 
-    return true;
+  return true;
 }
 
-bool RTState_V3_0__1::parse_with(BinParser& bp)
+bool RTState_V3_0__1::parseWith(BinParser& bp)
 {
-    if (!bp.check_size<RTState_V3_0__1>())
+  if (!bp.checkSize<RTState_V3_0__1>())
-        return false;
+    return false;
 
-    parse_shared1(bp);
+  parse_shared1(bp);
 
-    bp.parse(i_control);
+  bp.parse(i_control);
-    bp.parse(tool_vector_actual);
+  bp.parse(tool_vector_actual);
-    bp.parse(tcp_speed_actual);
+  bp.parse(tcp_speed_actual);
-    bp.parse(tcp_force);
+  bp.parse(tcp_force);
-    bp.parse(tool_vector_target);
+  bp.parse(tool_vector_target);
-    bp.parse(tcp_speed_target);
+  bp.parse(tcp_speed_target);
 
-    parse_shared2(bp);
+  parse_shared2(bp);
 
-    bp.parse(joint_modes);
+  bp.parse(joint_modes);
-    bp.parse(safety_mode);
+  bp.parse(safety_mode);
-    bp.consume(sizeof(double[6])); //skip undocumented
+  bp.consume(sizeof(double[6]));  // skip undocumented
-    bp.parse(tool_accelerometer_values);
+  bp.parse(tool_accelerometer_values);
-    bp.consume(sizeof(double[6])); //skip undocumented
+  bp.consume(sizeof(double[6]));  // skip undocumented
-    bp.parse(speed_scaling);
+  bp.parse(speed_scaling);
-    bp.parse(linear_momentum_norm);
+  bp.parse(linear_momentum_norm);
-    bp.consume(sizeof(double)); //skip undocumented
+  bp.consume(sizeof(double));  // skip undocumented
-    bp.consume(sizeof(double)); //skip undocumented
+  bp.consume(sizeof(double));  // skip undocumented
-    bp.parse(v_main);
+  bp.parse(v_main);
-    bp.parse(v_robot);
+  bp.parse(v_robot);
-    bp.parse(i_robot);
+  bp.parse(i_robot);
-    bp.parse(v_actual);
+  bp.parse(v_actual);
 
-    return true;
+  return true;
 }
 
-bool RTState_V3_2__3::parse_with(BinParser& bp)
+bool RTState_V3_2__3::parseWith(BinParser& bp)
 {
-    if (!bp.check_size<RTState_V3_2__3>())
+  if (!bp.checkSize<RTState_V3_2__3>())
-        return false;
+    return false;
 
-    RTState_V3_0__1::parse_with(bp);
+  RTState_V3_0__1::parseWith(bp);
 
-    bp.parse(digital_outputs);
+  bp.parse(digital_outputs);
-    bp.parse(program_state);
+  bp.parse(program_state);
 
-    return true;
+  return true;
 }
 
-bool RTState_V1_6__7::consume_with(URRTPacketConsumer& consumer)
+bool RTState_V1_6__7::consumeWith(URRTPacketConsumer& consumer)
 {
-    return consumer.consume(*this);
+  return consumer.consume(*this);
 }
-bool RTState_V1_8::consume_with(URRTPacketConsumer& consumer)
+bool RTState_V1_8::consumeWith(URRTPacketConsumer& consumer)
 {
-    return consumer.consume(*this);
+  return consumer.consume(*this);
 }
-bool RTState_V3_0__1::consume_with(URRTPacketConsumer& consumer)
+bool RTState_V3_0__1::consumeWith(URRTPacketConsumer& consumer)
 {
-    return consumer.consume(*this);
+  return consumer.consume(*this);
 }
-bool RTState_V3_2__3::consume_with(URRTPacketConsumer& consumer)
+bool RTState_V3_2__3::consumeWith(URRTPacketConsumer& consumer)
 {
-    return consumer.consume(*this);
+  return consumer.consume(*this);
 }

src/ur/state.cpp

@@ -1,25 +1,25 @@
 #include "ur_modern_driver/ur/state.h"
 #include "ur_modern_driver/log.h"
 
-//StatePacket::~StatePacket() { }
+// StatePacket::~StatePacket() { }
 
 /*
-bool RobotState::parse_with(BinParser &bp) {
+bool RobotState::parseWith(BinParser &bp) {
     //continue as long as there are bytes to read
     while(!bp.empty()) {
-        
+
-        if(!bp.check_size(sizeof(uint32_t))){
+        if(!bp.checkSize(sizeof(uint32_t))){
             LOG_ERROR("Failed to read sub-package length, there's likely a parsing error");
-            return false;    
+            return false;
         }
 
         uint32_t sub_size = bp.peek<uint32_t>();
-        if(!bp.check_size(static_cast<size_t>(sub_size))) {
+        if(!bp.checkSize(static_cast<size_t>(sub_size))) {
             LOG_WARN("Invalid sub-package size of %" PRIu32 " received!", sub_size);
             return false;
         }
-        
+
-        //deconstruction of a sub parser will increment the position of the parent parser 
+        //deconstruction of a sub parser will increment the position of the parent parser
         BinParser sub_parser(bp, sub_size);
         sub_parser.consume(sizeof(sub_size));
 
@@ -43,19 +43,19 @@ bool parse_base(BinParser &bp, T &pkg) {
         case package_type::ROBOT_MODE_DATA:
             LOG_DEBUG("Parsing robot_mode");
             bp.consume(sizeof(package_type));
-            pkg.robot_mode.parse_with(bp);
+            pkg.robot_mode.parseWith(bp);
             break;
         case package_type::MASTERBOARD_DATA:
             LOG_DEBUG("Parsing master_board");
             bp.consume(sizeof(package_type));
-            pkg.master_board.parse_with(bp);
+            pkg.master_board.parseWith(bp);
             break;
 
         case package_type::TOOL_DATA:
         case package_type::CARTESIAN_INFO:
         case package_type::JOINT_DATA:
-            LOG_DEBUG("Skipping tool, cartesian or joint data");        
+            LOG_DEBUG("Skipping tool, cartesian or joint data");
-            //for unhandled packages we consume the rest of the 
+            //for unhandled packages we consume the rest of the
             //package buffer
             bp.consume();
             break;
@@ -79,8 +79,8 @@ bool parse_advanced(BinParser &bp, T &pkg) {
         case package_type::ADDITIONAL_INFO:
         case package_type::CALIBRATION_DATA:
         case package_type::FORCE_MODE_DATA:
-            LOG_DEBUG("Skipping kinematics, config, additional, calibration or force mode data");        
+            LOG_DEBUG("Skipping kinematics, config, additional, calibration or force mode data");
-            //for unhandled packages we consume the rest of the 
+            //for unhandled packages we consume the rest of the
             //package buffer
             bp.consume();
             break;

src/ur/stream.cpp

@@ -1,126 +1,133 @@
-#include <cstring>
 #include <endian.h>
 #include <netinet/tcp.h>
 #include <unistd.h>
+#include <cstring>
 
 #include "ur_modern_driver/log.h"
 #include "ur_modern_driver/ur/stream.h"
 
 bool URStream::connect()
 {
-    if (_initialized)
+  if (initialized_)
-        return false;
+    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:
+  // gethostbyname() is deprecated so use getadderinfo() as described in:
-    //http://www.beej.us/guide/bgnet/output/html/multipage/syscalls.html#getaddrinfo
+  // 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;
+  struct addrinfo hints, *result;
-    std::memset(&hints, 0, sizeof(hints));
+  std::memset(&hints, 0, sizeof(hints));
 
-    hints.ai_family = AF_UNSPEC;
+  hints.ai_family = AF_UNSPEC;
-    hints.ai_socktype = SOCK_STREAM;
+  hints.ai_socktype = SOCK_STREAM;
-    hints.ai_flags = AI_PASSIVE;
+  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;
+    LOG_ERROR("Failed to get host name");
-    }
+    return false;
+  }
 
-    //loop through the list of addresses untill we find one that's connectable
+  // 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;
+      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)
+    {
-                break;
+      if (stopping_)
-            else
-                continue; //try next addrinfo if connect fails
-        }
-
-        //disable Nagle's algorithm to ensure we sent packets as fast as possible
-        int flag = 1;
-        setsockopt(_socket_fd, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(flag));
-        _initialized = true;
-        LOG_INFO("Connection successfully established");
         break;
+      else
+        continue;  // try next addrinfo if connect fails
     }
 
-    freeaddrinfo(result);
+    // disable Nagle's algorithm to ensure we sent packets as fast as possible
-    if (!_initialized)
+    int flag = 1;
-        LOG_ERROR("Connection failed");
+    setsockopt(socket_fd_, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(flag));
+    initialized_ = true;
+    LOG_INFO("Connection successfully established");
+    break;
+  }
 
-    return _initialized;
+  freeaddrinfo(result);
+  if (!initialized_)
+    LOG_ERROR("Connection failed");
+
+  return initialized_;
 }
 
 void URStream::disconnect()
 {
-    if (!_initialized || _stopping)
+  if (!initialized_ || stopping_)
-        return;
+    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;
+    return -1;
-    if (_stopping)
+  if (stopping_)
-        return 0;
+    return 0;
 
-    size_t total = 0;
+  size_t total = 0;
-    size_t remaining = buf_len;
+  size_t remaining = buf_len;
 
-    //TODO: handle reconnect?
+  // TODO: handle reconnect?
-    //handle partial sends
+  // handle partial sends
-    while (total < buf_len) {
+  while (total < buf_len)
-        ssize_t sent = ::send(_socket_fd, buf + total, remaining, 0);
+  {
-        if (sent <= 0)
+    ssize_t sent = ::send(socket_fd_, buf + total, remaining, 0);
-            return _stopping ? 0 : sent;
+    if (sent <= 0)
-        total += sent;
+      return stopping_ ? 0 : sent;
-        remaining -= sent;
+    total += sent;
-    }
+    remaining -= sent;
+  }
 
-    return total;
+  return total;
 }
 
 ssize_t URStream::receive(uint8_t* buf, size_t buf_len)
 {
-    if (!_initialized)
+  if (!initialized_)
+    return -1;
+  if (stopping_)
+    return 0;
+
+  size_t remainder = sizeof(int32_t);
+  uint8_t* buf_pos = buf;
+  bool initial = true;
+
+  do
+  {
+    ssize_t read = recv(socket_fd_, buf_pos, remainder, 0);
+    if (read <= 0)  // failed reading from socket
+      return stopping_ ? 0 : read;
+
+    if (initial)
+    {
+      remainder = be32toh(*(reinterpret_cast<int32_t*>(buf)));
+      if (remainder >= (buf_len - sizeof(int32_t)))
+      {
+        LOG_ERROR("Packet size %zd is larger than buffer %zu, discarding.", remainder, buf_len);
         return -1;
-    if (_stopping)
+      }
-        return 0;
+      initial = false;
+    }
 
-    size_t remainder = sizeof(int32_t);
+    buf_pos += read;
-    uint8_t* buf_pos = buf;
+    remainder -= read;
-    bool initial = true;
+  } while (remainder > 0);
 
-    do {
+  return buf_pos - buf;
-        ssize_t read = recv(_socket_fd, buf_pos, remainder, 0);
-        if (read <= 0) //failed reading from socket
-            return _stopping ? 0 : read;
-
-        if (initial) {
-            remainder = be32toh(*(reinterpret_cast<int32_t*>(buf)));
-            if (remainder >= (buf_len - sizeof(int32_t))) {
-                LOG_ERROR("Packet size %zd is larger than buffer %zu, discarding.", remainder, buf_len);
-                return -1;
-            }
-            initial = false;
-        }
-
-        buf_pos += read;
-        remainder -= read;
-    } while (remainder > 0);
-
-    return buf_pos - buf;
 }

src/ur_communication.cpp

@@ -18,168 +18,167 @@
 
 #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);
+  robot_state_ = new RobotState(msg_cond);
-    bzero((char*)&pri_serv_addr_, sizeof(pri_serv_addr_));
+  bzero((char*)&pri_serv_addr_, sizeof(pri_serv_addr_));
-    bzero((char*)&sec_serv_addr_, sizeof(sec_serv_addr_));
+  bzero((char*)&sec_serv_addr_, sizeof(sec_serv_addr_));
-    pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
+  pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
-    if (pri_sockfd_ < 0) {
+  if (pri_sockfd_ < 0)
-        print_fatal("ERROR opening socket pri_sockfd");
+  {
-    }
+    print_fatal("ERROR opening socket pri_sockfd");
-    sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
+  }
-    if (sec_sockfd_ < 0) {
+  sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
-        print_fatal("ERROR opening socket sec_sockfd");
+  if (sec_sockfd_ < 0)
-    }
+  {
-    server_ = gethostbyname(host.c_str());
+    print_fatal("ERROR opening socket sec_sockfd");
-    if (server_ == NULL) {
+  }
-        print_fatal("ERROR, unknown host");
+  server_ = gethostbyname(host.c_str());
-    }
+  if (server_ == NULL)
-    pri_serv_addr_.sin_family = AF_INET;
+  {
-    sec_serv_addr_.sin_family = AF_INET;
+    print_fatal("ERROR, unknown host");
-    bcopy((char*)server_->h_addr, (char*)&pri_serv_addr_.sin_addr.s_addr, server_->h_length);
+  }
-    bcopy((char*)server_->h_addr, (char*)&sec_serv_addr_.sin_addr.s_addr, server_->h_length);
+  pri_serv_addr_.sin_family = AF_INET;
-    pri_serv_addr_.sin_port = htons(30001);
+  sec_serv_addr_.sin_family = AF_INET;
-    sec_serv_addr_.sin_port = htons(30002);
+  bcopy((char*)server_->h_addr, (char*)&pri_serv_addr_.sin_addr.s_addr, server_->h_length);
-    flag_ = 1;
+  bcopy((char*)server_->h_addr, (char*)&sec_serv_addr_.sin_addr.s_addr, server_->h_length);
-    setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_,
+  pri_serv_addr_.sin_port = htons(30001);
-        sizeof(int));
+  sec_serv_addr_.sin_port = htons(30002);
-    setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
+  flag_ = 1;
-        sizeof(int));
+  setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
-    setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_,
+  setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
-        sizeof(int));
+  setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
-    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));
+  fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK);
-    setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_,
+  connected_ = false;
-        sizeof(int));
+  keepalive_ = false;
-    fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK);
-    connected_ = false;
-    keepalive_ = false;
 }
 
 bool UrCommunication::start()
 {
-    keepalive_ = true;
+  keepalive_ = true;
-    uint8_t buf[512];
+  uint8_t buf[512];
-    unsigned int bytes_read;
+  unsigned int bytes_read;
-    std::string cmd;
+  std::string cmd;
-    bzero(buf, 512);
+  bzero(buf, 512);
-    print_debug("Acquire firmware version: Connecting...");
+  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");
-        print_fatal("Error connecting to get firmware version");
+    return false;
-        return false;
+  }
-    }
+  print_debug("Acquire firmware version: Got connection");
-    print_debug("Acquire firmware version: Got connection");
+  bytes_read = read(pri_sockfd_, buf, 512);
-    bytes_read = read(pri_sockfd_, buf, 512);
+  setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
-    setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
+  robot_state_->unpack(buf, bytes_read);
-        sizeof(int));
+  // wait for some traffic so the UR socket doesn't die in version 3.1.
-    robot_state_->unpack(buf, bytes_read);
+  std::this_thread::sleep_for(std::chrono::milliseconds(500));
-    //wait for some traffic so the UR socket doesn't die in version 3.1.
+  char tmp[64];
-    std::this_thread::sleep_for(std::chrono::milliseconds(500));
+  sprintf(tmp, "Firmware version detected: %.7f", robot_state_->getVersion());
-    char tmp[64];
+  print_debug(tmp);
-    sprintf(tmp, "Firmware version detected: %.7f", robot_state_->getVersion());
+  close(pri_sockfd_);
-    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;
+  fd_set writefds;
-    struct timeval timeout;
+  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_ZERO(&writefds);
+  FD_SET(sec_sockfd_, &writefds);
-    FD_SET(sec_sockfd_, &writefds);
+  timeout.tv_sec = 10;
-    timeout.tv_sec = 10;
+  timeout.tv_usec = 0;
-    timeout.tv_usec = 0;
+  select(sec_sockfd_ + 1, NULL, &writefds, NULL, &timeout);
-    select(sec_sockfd_ + 1, NULL, &writefds, NULL, &timeout);
+  unsigned int flag_len;
-    unsigned int flag_len;
+  getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &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");
+    print_fatal("Error connecting to secondary interface");
-        return false;
+    return false;
-    }
+  }
-    print_debug("Secondary interface: Got connection");
+  print_debug("Secondary interface: Got connection");
-    comThread_ = std::thread(&UrCommunication::run, this);
+  comThread_ = std::thread(&UrCommunication::run, this);
-    return true;
+  return true;
 }
 
 void UrCommunication::halt()
 {
-    keepalive_ = false;
+  keepalive_ = false;
-    comThread_.join();
+  comThread_.join();
 }
 
 void UrCommunication::run()
 {
-    uint8_t buf[2048];
+  uint8_t buf[2048];
-    int bytes_read;
+  int bytes_read;
-    bzero(buf, 2048);
+  bzero(buf, 2048);
-    struct timeval timeout;
+  struct timeval timeout;
-    fd_set readfds;
+  fd_set readfds;
-    FD_ZERO(&readfds);
+  FD_ZERO(&readfds);
-    FD_SET(sec_sockfd_, &readfds);
+  FD_SET(sec_sockfd_, &readfds);
-    connected_ = true;
+  connected_ = true;
-    while (keepalive_) {
+  while (keepalive_)
-        while (connected_ && keepalive_) {
+  {
-            timeout.tv_sec = 0; //do this each loop as selects modifies timeout
+    while (connected_ && keepalive_)
-            timeout.tv_usec = 500000; // timeout of 0.5 sec
+    {
-            select(sec_sockfd_ + 1, &readfds, NULL, NULL, &timeout);
+      timeout.tv_sec = 0;        // do this each loop as selects modifies timeout
-            bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes
+      timeout.tv_usec = 500000;  // timeout of 0.5 sec
-            if (bytes_read > 0) {
+      select(sec_sockfd_ + 1, &readfds, NULL, NULL, &timeout);
-                setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK,
+      bytes_read = read(sec_sockfd_, buf, 2048);  // usually only up to 1295 bytes
-                    (char*)&flag_, sizeof(int));
+      if (bytes_read > 0)
-                robot_state_->unpack(buf, bytes_read);
+      {
-            } else {
+        setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
-                connected_ = false;
+        robot_state_->unpack(buf, bytes_read);
-                robot_state_->setDisconnected();
+      }
-                close(sec_sockfd_);
+      else
-            }
+      {
-        }
+        connected_ = false;
-        if (keepalive_) {
+        robot_state_->setDisconnected();
-            //reconnect
+        close(sec_sockfd_);
-            print_warning("Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...");
+      }
-            sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
-            if (sec_sockfd_ < 0) {
-                print_fatal("ERROR opening secondary socket");
-            }
-            flag_ = 1;
-            setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_,
-                sizeof(int));
-            setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
-                sizeof(int));
-            setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_,
-                sizeof(int));
-            fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK);
-            while (keepalive_ && !connected_) {
-                std::this_thread::sleep_for(std::chrono::seconds(10));
-                fd_set writefds;
-
-                connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_,
-                    sizeof(sec_serv_addr_));
-                FD_ZERO(&writefds);
-                FD_SET(sec_sockfd_, &writefds);
-                select(sec_sockfd_ + 1, NULL, &writefds, NULL, NULL);
-                unsigned int flag_len;
-                getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_,
-                    &flag_len);
-                if (flag_ < 0) {
-                    print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds...");
-                } else {
-                    connected_ = true;
-                    print_info("Secondary port: Reconnected");
-                }
-            }
-        }
     }
+    if (keepalive_)
+    {
+      // reconnect
+      print_warning("Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...");
+      sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
+      if (sec_sockfd_ < 0)
+      {
+        print_fatal("ERROR opening secondary socket");
+      }
+      flag_ = 1;
+      setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
+      setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
+      setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
+      fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK);
+      while (keepalive_ && !connected_)
+      {
+        std::this_thread::sleep_for(std::chrono::seconds(10));
+        fd_set writefds;
 
-    //wait for some traffic so the UR socket doesn't die in version 3.1.
+        connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_, sizeof(sec_serv_addr_));
-    std::this_thread::sleep_for(std::chrono::milliseconds(500));
+        FD_ZERO(&writefds);
-    close(sec_sockfd_);
+        FD_SET(sec_sockfd_, &writefds);
+        select(sec_sockfd_ + 1, NULL, &writefds, NULL, NULL);
+        unsigned int flag_len;
+        getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
+        if (flag_ < 0)
+        {
+          print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 "
+                      "seconds...");
+        }
+        else
+        {
+          connected_ = true;
+          print_info("Secondary port: Reconnected");
+        }
+      }
+    }
+  }
+
+  // wait for some traffic so the UR socket doesn't die in version 3.1.
+  std::this_thread::sleep_for(std::chrono::milliseconds(500));
+  close(sec_sockfd_);
 }

src/ur_driver.cpp

@@ -18,397 +18,403 @@
 
 #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,
+  : REVERSE_PORT_(reverse_port)
-    double max_payload, double servoj_lookahead_time, double servoj_gain)
+  , maximum_time_step_(max_time_step)
-    : REVERSE_PORT_(reverse_port)
+  , minimum_payload_(min_payload)
-    , maximum_time_step_(max_time_step)
+  , maximum_payload_(max_payload)
-    , minimum_payload_(
+  , servoj_time_(servoj_time)
-          min_payload)
+  , servoj_lookahead_time_(servoj_lookahead_time)
-    , maximum_payload_(max_payload)
+  , servoj_gain_(servoj_gain)
-    , servoj_time_(
-          servoj_time)
-    , servoj_lookahead_time_(servoj_lookahead_time)
-    , servoj_gain_(servoj_gain)
 {
-    char buffer[256];
+  char buffer[256];
-    struct sockaddr_in serv_addr;
+  struct sockaddr_in serv_addr;
-    int n, flag;
+  int n, flag;
 
-    firmware_version_ = 0;
+  firmware_version_ = 0;
-    reverse_connected_ = false;
+  reverse_connected_ = false;
-    executing_traj_ = 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;
-    new_sockfd_ = -1;
+  sec_interface_ = new UrCommunication(msg_cond, host);
-    sec_interface_ = new UrCommunication(msg_cond, host);
 
-    incoming_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
+  incoming_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
-    if (incoming_sockfd_ < 0) {
+  if (incoming_sockfd_ < 0)
-        print_fatal("ERROR opening socket for reverse communication");
+  {
-    }
+    print_fatal("ERROR opening socket for reverse communication");
-    bzero((char*)&serv_addr, sizeof(serv_addr));
+  }
+  bzero((char*)&serv_addr, sizeof(serv_addr));
 
-    serv_addr.sin_family = AF_INET;
+  serv_addr.sin_family = AF_INET;
-    serv_addr.sin_addr.s_addr = INADDR_ANY;
+  serv_addr.sin_addr.s_addr = INADDR_ANY;
-    serv_addr.sin_port = htons(REVERSE_PORT_);
+  serv_addr.sin_port = htons(REVERSE_PORT_);
-    flag = 1;
+  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));
-    setsockopt(incoming_sockfd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int));
+  if (bind(incoming_sockfd_, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0)
-    if (bind(incoming_sockfd_, (struct sockaddr*)&serv_addr,
+  {
-            sizeof(serv_addr))
+    print_fatal("ERROR on binding socket for reverse communication");
-        < 0) {
+  }
-        print_fatal("ERROR on binding socket for reverse communication");
+  listen(incoming_sockfd_, 5);
-    }
-    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)
-    std::vector<double> p0_vel, std::vector<double> p1_vel)
 {
-    /*Returns positions of the joints at time 't' */
+  /*Returns positions of the joints at time 't' */
-    std::vector<double> positions;
+  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 a = p0_pos[i];
-        double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i]
+    double b = p0_vel[i];
-                       - T * p1_vel[i])
+    double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i] - T * p1_vel[i]) / pow(T, 2);
-            / pow(T, 2);
+    double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i] + T * p1_vel[i]) / pow(T, 3);
-        double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i]
+    positions.push_back(a + b * t + c * pow(t, 2) + d * pow(t, 3));
-                       + T * p1_vel[i])
+  }
-            / pow(T, 3);
+  return positions;
-        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::vector<std::vector<double> > inp_velocities)
 {
-    std::chrono::high_resolution_clock::time_point t0, t;
+  std::chrono::high_resolution_clock::time_point t0, t;
-    std::vector<double> positions;
+  std::vector<double> positions;
-    unsigned int j;
+  unsigned int j;
 
-    if (!UrDriver::uploadProg()) {
+  if (!UrDriver::uploadProg())
-        return false;
+  {
+    return false;
+  }
+  executing_traj_ = true;
+  t0 = std::chrono::high_resolution_clock::now();
+  t = t0;
+  j = 0;
+  while ((inp_timestamps[inp_timestamps.size() - 1] >=
+          std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count()) and
+         executing_traj_)
+  {
+    while (inp_timestamps[j] <= std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count() &&
+           j < inp_timestamps.size() - 1)
+    {
+      j += 1;
     }
-    executing_traj_ = true;
+    positions = UrDriver::interp_cubic(std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count() -
-    t0 = std::chrono::high_resolution_clock::now();
+                                           inp_timestamps[j - 1],
-    t = t0;
+                                       inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1],
-    j = 0;
+                                       inp_positions[j], inp_velocities[j - 1], inp_velocities[j]);
-    while ((inp_timestamps[inp_timestamps.size() - 1]
+    UrDriver::servoj(positions);
-               >= std::chrono::duration_cast<std::chrono::duration<double> >(t - t0).count())
-        and executing_traj_) {
-        while (inp_timestamps[j]
-                <= std::chrono::duration_cast<std::chrono::duration<double> >(
-                       t - t0)
-                       .count()
-            && j < inp_timestamps.size() - 1) {
-            j += 1;
-        }
-        positions = UrDriver::interp_cubic(
-            std::chrono::duration_cast<std::chrono::duration<double> >(
-                t - t0)
-                    .count()
-                - inp_timestamps[j - 1],
-            inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1],
-            inp_positions[j], inp_velocities[j - 1], inp_velocities[j]);
-        UrDriver::servoj(positions);
 
-        // oversample with 4 * sample_time
+    // 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();
-        t = std::chrono::high_resolution_clock::now();
+  }
-    }
+  executing_traj_ = false;
-    executing_traj_ = false;
+  // Signal robot to stop driverProg()
-    //Signal robot to stop driverProg()
+  UrDriver::closeServo(positions);
-    UrDriver::closeServo(positions);
+  return true;
-    return true;
 }
 
 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;
+    return;
-    }
+  }
-    unsigned int bytes_written;
+  unsigned int bytes_written;
-    int tmp;
+  int tmp;
-    unsigned char buf[28];
+  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;
+    tmp = htonl((int)(positions[i] * MULT_JOINTSTATE_));
-        buf[i * 4 + 1] = (tmp >> 8) & 0xff;
+    buf[i * 4] = tmp & 0xff;
-        buf[i * 4 + 2] = (tmp >> 16) & 0xff;
+    buf[i * 4 + 1] = (tmp >> 8) & 0xff;
-        buf[i * 4 + 3] = (tmp >> 24) & 0xff;
+    buf[i * 4 + 2] = (tmp >> 16) & 0xff;
-    }
+    buf[i * 4 + 3] = (tmp >> 24) & 0xff;
-    tmp = htonl((int)keepalive);
+  }
-    buf[6 * 4] = tmp & 0xff;
+  tmp = htonl((int)keepalive);
-    buf[6 * 4 + 1] = (tmp >> 8) & 0xff;
+  buf[6 * 4] = tmp & 0xff;
-    buf[6 * 4 + 2] = (tmp >> 16) & 0xff;
+  buf[6 * 4 + 1] = (tmp >> 8) & 0xff;
-    buf[6 * 4 + 3] = (tmp >> 24) & 0xff;
+  buf[6 * 4 + 2] = (tmp >> 16) & 0xff;
-    bytes_written = write(new_sockfd_, buf, 28);
+  buf[6 * 4 + 3] = (tmp >> 24) & 0xff;
+  bytes_written = write(new_sockfd_, buf, 28);
 }
 
 void UrDriver::stopTraj()
 {
-    executing_traj_ = false;
+  executing_traj_ = false;
-    rt_interface_->addCommandToQueue("stopj(10)\n");
+  rt_interface_->addCommandToQueue("stopj(10)\n");
 }
 
 bool UrDriver::uploadProg()
 {
-    std::string cmd_str;
+  std::string cmd_str;
-    char buf[128];
+  char buf[128];
-    cmd_str = "def driverProg():\n";
+  cmd_str = "def driverProg():\n";
 
-    sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_);
+  sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_);
-    cmd_str += buf;
+  cmd_str += buf;
 
-    cmd_str += "\tSERVO_IDLE = 0\n";
+  cmd_str += "\tSERVO_IDLE = 0\n";
-    cmd_str += "\tSERVO_RUNNING = 1\n";
+  cmd_str += "\tSERVO_RUNNING = 1\n";
-    cmd_str += "\tcmd_servo_state = SERVO_IDLE\n";
+  cmd_str += "\tcmd_servo_state = SERVO_IDLE\n";
-    cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n";
+  cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n";
-    cmd_str += "\tdef set_servo_setpoint(q):\n";
+  cmd_str += "\tdef set_servo_setpoint(q):\n";
-    cmd_str += "\t\tenter_critical\n";
+  cmd_str += "\t\tenter_critical\n";
-    cmd_str += "\t\tcmd_servo_state = SERVO_RUNNING\n";
+  cmd_str += "\t\tcmd_servo_state = SERVO_RUNNING\n";
-    cmd_str += "\t\tcmd_servo_q = q\n";
+  cmd_str += "\t\tcmd_servo_q = q\n";
-    cmd_str += "\t\texit_critical\n";
+  cmd_str += "\t\texit_critical\n";
-    cmd_str += "\tend\n";
+  cmd_str += "\tend\n";
-    cmd_str += "\tthread servoThread():\n";
+  cmd_str += "\tthread servoThread():\n";
-    cmd_str += "\t\tstate = SERVO_IDLE\n";
+  cmd_str += "\t\tstate = SERVO_IDLE\n";
-    cmd_str += "\t\twhile True:\n";
+  cmd_str += "\t\twhile True:\n";
-    cmd_str += "\t\t\tenter_critical\n";
+  cmd_str += "\t\t\tenter_critical\n";
-    cmd_str += "\t\t\tq = cmd_servo_q\n";
+  cmd_str += "\t\t\tq = cmd_servo_q\n";
-    cmd_str += "\t\t\tdo_brake = False\n";
+  cmd_str += "\t\t\tdo_brake = False\n";
-    cmd_str += "\t\t\tif (state == SERVO_RUNNING) and ";
+  cmd_str += "\t\t\tif (state == SERVO_RUNNING) and ";
-    cmd_str += "(cmd_servo_state == SERVO_IDLE):\n";
+  cmd_str += "(cmd_servo_state == SERVO_IDLE):\n";
-    cmd_str += "\t\t\t\tdo_brake = True\n";
+  cmd_str += "\t\t\t\tdo_brake = True\n";
-    cmd_str += "\t\t\tend\n";
+  cmd_str += "\t\t\tend\n";
-    cmd_str += "\t\t\tstate = cmd_servo_state\n";
+  cmd_str += "\t\t\tstate = cmd_servo_state\n";
-    cmd_str += "\t\t\tcmd_servo_state = SERVO_IDLE\n";
+  cmd_str += "\t\t\tcmd_servo_state = SERVO_IDLE\n";
-    cmd_str += "\t\t\texit_critical\n";
+  cmd_str += "\t\t\texit_critical\n";
-    cmd_str += "\t\t\tif do_brake:\n";
+  cmd_str += "\t\t\tif do_brake:\n";
-    cmd_str += "\t\t\t\tstopj(1.0)\n";
+  cmd_str += "\t\t\t\tstopj(1.0)\n";
-    cmd_str += "\t\t\t\tsync()\n";
+  cmd_str += "\t\t\t\tsync()\n";
-    cmd_str += "\t\t\telif state == SERVO_RUNNING:\n";
+  cmd_str += "\t\t\telif state == SERVO_RUNNING:\n";
 
-    if (sec_interface_->robot_state_->getVersion() >= 3.1)
+  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
+  else
-        sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_);
+    sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_);
-    cmd_str += buf;
+  cmd_str += buf;
 
-    cmd_str += "\t\t\telse:\n";
+  cmd_str += "\t\t\telse:\n";
-    cmd_str += "\t\t\t\tsync()\n";
+  cmd_str += "\t\t\t\tsync()\n";
-    cmd_str += "\t\t\tend\n";
+  cmd_str += "\t\t\tend\n";
-    cmd_str += "\t\tend\n";
+  cmd_str += "\t\tend\n";
-    cmd_str += "\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 += buf;
 
-    cmd_str += "\tthread_servo = run servoThread()\n";
+  cmd_str += "\tthread_servo = run servoThread()\n";
-    cmd_str += "\tkeepalive = 1\n";
+  cmd_str += "\tkeepalive = 1\n";
-    cmd_str += "\twhile keepalive > 0:\n";
+  cmd_str += "\twhile keepalive > 0:\n";
-    cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1)\n";
+  cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1)\n";
-    cmd_str += "\t\tif params_mult[0] > 0:\n";
+  cmd_str += "\t\tif params_mult[0] > 0:\n";
-    cmd_str += "\t\t\tq = [params_mult[1] / MULT_jointstate, ";
+  cmd_str += "\t\t\tq = [params_mult[1] / MULT_jointstate, ";
-    cmd_str += "params_mult[2] / MULT_jointstate, ";
+  cmd_str += "params_mult[2] / MULT_jointstate, ";
-    cmd_str += "params_mult[3] / MULT_jointstate, ";
+  cmd_str += "params_mult[3] / MULT_jointstate, ";
-    cmd_str += "params_mult[4] / MULT_jointstate, ";
+  cmd_str += "params_mult[4] / MULT_jointstate, ";
-    cmd_str += "params_mult[5] / MULT_jointstate, ";
+  cmd_str += "params_mult[5] / MULT_jointstate, ";
-    cmd_str += "params_mult[6] / MULT_jointstate]\n";
+  cmd_str += "params_mult[6] / MULT_jointstate]\n";
-    cmd_str += "\t\t\tkeepalive = params_mult[7]\n";
+  cmd_str += "\t\t\tkeepalive = params_mult[7]\n";
-    cmd_str += "\t\t\tset_servo_setpoint(q)\n";
+  cmd_str += "\t\t\tset_servo_setpoint(q)\n";
-    cmd_str += "\t\tend\n";
+  cmd_str += "\t\tend\n";
-    cmd_str += "\tend\n";
+  cmd_str += "\tend\n";
-    cmd_str += "\tsleep(.1)\n";
+  cmd_str += "\tsleep(.1)\n";
-    cmd_str += "\tsocket_close()\n";
+  cmd_str += "\tsocket_close()\n";
-    cmd_str += "\tkill thread_servo\n";
+  cmd_str += "\tkill thread_servo\n";
-    cmd_str += "end\n";
+  cmd_str += "end\n";
 
-    rt_interface_->addCommandToQueue(cmd_str);
+  rt_interface_->addCommandToQueue(cmd_str);
-    return UrDriver::openServo();
+  return UrDriver::openServo();
 }
 
 bool UrDriver::openServo()
 {
-    struct sockaddr_in cli_addr;
+  struct sockaddr_in cli_addr;
-    socklen_t clilen;
+  socklen_t clilen;
-    clilen = sizeof(cli_addr);
+  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");
+    print_fatal("ERROR on accepting reverse communication");
-        return false;
+    return false;
-    }
+  }
-    reverse_connected_ = true;
+  reverse_connected_ = true;
-    return true;
+  return true;
 }
 void UrDriver::closeServo(std::vector<double> positions)
 {
-    if (positions.size() != 6)
+  if (positions.size() != 6)
-        UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0);
+    UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0);
-    else
+  else
-        UrDriver::servoj(positions, 0);
+    UrDriver::servoj(positions, 0);
 
-    reverse_connected_ = false;
+  reverse_connected_ = false;
-    close(new_sockfd_);
+  close(new_sockfd_);
 }
 
 bool UrDriver::start()
 {
-    if (!sec_interface_->start())
+  if (!sec_interface_->start())
-        return false;
+    return false;
-    firmware_version_ = sec_interface_->robot_state_->getVersion();
+  firmware_version_ = sec_interface_->robot_state_->getVersion();
-    rt_interface_->robot_state_->setVersion(firmware_version_);
+  rt_interface_->robot_state_->setVersion(firmware_version_);
-    if (!rt_interface_->start())
+  if (!rt_interface_->start())
-        return false;
+    return false;
-    ip_addr_ = rt_interface_->getLocalIp();
+  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_)
+  return true;
-        + "\n");
-    return true;
 }
 
 void UrDriver::halt()
 {
-    if (executing_traj_) {
+  if (executing_traj_)
-        UrDriver::stopTraj();
+  {
-    }
+    UrDriver::stopTraj();
-    sec_interface_->halt();
+  }
-    rt_interface_->halt();
+  sec_interface_->halt();
-    close(incoming_sockfd_);
+  rt_interface_->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);
+  rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc);
 }
 
 std::vector<std::string> UrDriver::getJointNames()
 {
-    return joint_names_;
+  return joint_names_;
 }
 
 void UrDriver::setJointNames(std::vector<std::string> jn)
 {
-    joint_names_ = jn;
+  joint_names_ = jn;
 }
 
 void UrDriver::setToolVoltage(unsigned int v)
 {
-    char buf[256];
+  char buf[256];
-    sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v);
+  sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v);
-    rt_interface_->addCommandToQueue(buf);
+  rt_interface_->addCommandToQueue(buf);
-    print_debug(buf);
+  print_debug(buf);
 }
 void UrDriver::setFlag(unsigned int n, bool b)
 {
-    char buf[256];
+  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);
-    rt_interface_->addCommandToQueue(buf);
+  print_debug(buf);
-    print_debug(buf);
 }
 void UrDriver::setDigitalOut(unsigned int n, bool b)
 {
-    char buf[256];
+  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);
+  rt_interface_->addCommandToQueue(buf);
+  print_debug(buf);
 }
 void UrDriver::setAnalogOut(unsigned int n, double f)
 {
-    char buf[256];
+  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 {
+    sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f);
-        sprintf(buf, "sec setOut():\n\tset_standard_analog_out(%d, %1.4f)\nend\n", n, f);
+  }
-    }
+  else
+  {
+    sprintf(buf, "sec setOut():\n\tset_standard_analog_out(%d, %1.4f)\nend\n", n, f);
+  }
 
-    rt_interface_->addCommandToQueue(buf);
+  rt_interface_->addCommandToQueue(buf);
-    print_debug(buf);
+  print_debug(buf);
 }
 
 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);
+    char buf[256];
-        rt_interface_->addCommandToQueue(buf);
+    sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m);
-        print_debug(buf);
+    rt_interface_->addCommandToQueue(buf);
-        return true;
+    print_debug(buf);
-    } else
+    return true;
-        return false;
+  }
+  else
+    return false;
 }
 
 void UrDriver::setMinPayload(double m)
 {
-    if (m > 0) {
+  if (m > 0)
-        minimum_payload_ = m;
+  {
-    } else {
+    minimum_payload_ = m;
-        minimum_payload_ = 0;
+  }
-    }
+  else
+  {
+    minimum_payload_ = 0;
+  }
 }
 void UrDriver::setMaxPayload(double m)
 {
-    maximum_payload_ = m;
+  maximum_payload_ = m;
 }
 void UrDriver::setServojTime(double t)
 {
-    if (t > 0.008) {
+  if (t > 0.008)
-        servoj_time_ = t;
+  {
-    } else {
+    servoj_time_ = t;
-        servoj_time_ = 0.008;
+  }
-    }
+  else
+  {
+    servoj_time_ = 0.008;
+  }
 }
 void UrDriver::setServojLookahead(double t)
 {
-    if (t > 0.03) {
+  if (t > 0.03)
-        if (t < 0.2) {
+  {
-            servoj_lookahead_time_ = t;
+    if (t < 0.2)
-        } else {
+    {
-            servoj_lookahead_time_ = 0.2;
+      servoj_lookahead_time_ = t;
-        }
-    } else {
-        servoj_lookahead_time_ = 0.03;
     }
+    else
+    {
+      servoj_lookahead_time_ = 0.2;
+    }
+  }
+  else
+  {
+    servoj_lookahead_time_ = 0.03;
+  }
 }
 void UrDriver::setServojGain(double g)
 {
-    if (g > 100) {
+  if (g > 100)
-        if (g < 2000) {
+  {
-            servoj_gain_ = g;
+    if (g < 2000)
-        } else {
+    {
-            servoj_gain_ = 2000;
+      servoj_gain_ = g;
-        }
-    } else {
-        servoj_gain_ = 100;
     }
+    else
+    {
+      servoj_gain_ = 2000;
+    }
+  }
+  else
+  {
+    servoj_gain_ = 100;
+  }
 }

src/ur_hardware_interface.cpp

@@ -57,228 +57,227 @@
 
 #include <ur_modern_driver/ur_hardware_interface.h>
 
-namespace ros_control_ur {
+namespace ros_control_ur
-
-UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot)
-    : nh_(nh)
-    , robot_(robot)
 {
-    // Initialize shared memory and interfaces here
+UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) : nh_(nh), robot_(robot)
-    init(); // this implementation loads from rosparam
+{
+  // Initialize shared memory and interfaces here
+  init();  // this implementation loads from rosparam
 
-    max_vel_change_ = 0.12; // equivalent of an acceleration of 15 rad/sec^2
+  max_vel_change_ = 0.12;  // equivalent of an acceleration of 15 rad/sec^2
 
-    ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface.");
+  ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface.");
 }
 
 void UrHardwareInterface::init()
 {
-    ROS_INFO_STREAM_NAMED("ur_hardware_interface",
+  ROS_INFO_STREAM_NAMED("ur_hardware_interface", "Reading rosparams from namespace: " << nh_.getNamespace());
-        "Reading rosparams from namespace: " << nh_.getNamespace());
 
-    // Get joint names
+  // Get joint names
-    nh_.getParam("hardware_interface/joints", 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?"
+    ROS_FATAL_STREAM_NAMED("ur_hardware_interface",
-                << " Namespace: " << nh_.getNamespace());
+                           "No joints found on parameter server for controller, did you load the proper yaml file?"
-        exit(-1);
+                               << " Namespace: " << nh_.getNamespace());
-    }
+    exit(-1);
-    num_joints_ = joint_names_.size();
+  }
+  num_joints_ = joint_names_.size();
 
-    // Resize vectors
+  // Resize vectors
-    joint_position_.resize(num_joints_);
+  joint_position_.resize(num_joints_);
-    joint_velocity_.resize(num_joints_);
+  joint_velocity_.resize(num_joints_);
-    joint_effort_.resize(num_joints_);
+  joint_effort_.resize(num_joints_);
-    joint_position_command_.resize(num_joints_);
+  joint_position_command_.resize(num_joints_);
-    joint_velocity_command_.resize(num_joints_);
+  joint_velocity_command_.resize(num_joints_);
-    prev_joint_velocity_command_.resize(num_joints_);
+  prev_joint_velocity_command_.resize(num_joints_);
 
-    // Initialize controller
+  // 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
+    // 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
+    // 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
+    // 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]),
+    prev_joint_velocity_command_[i] = 0.;
-                &joint_velocity_command_[i]));
+  }
-        prev_joint_velocity_command_[i] = 0.;
-    }
 
-    // Create force torque interface
+  // Create force torque interface
-    force_torque_interface_.registerHandle(
+  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(&joint_state_interface_);     // From RobotHW base class.
-    registerInterface(&position_joint_interface_); // From RobotHW base class.
+  registerInterface(&position_joint_interface_);  // From RobotHW base class.
-    registerInterface(&velocity_joint_interface_); // From RobotHW base class.
+  registerInterface(&velocity_joint_interface_);  // From RobotHW base class.
-    registerInterface(&force_torque_interface_); // From RobotHW base class.
+  registerInterface(&force_torque_interface_);    // From RobotHW base class.
-    velocity_interface_running_ = false;
+  velocity_interface_running_ = false;
-    position_interface_running_ = false;
+  position_interface_running_ = false;
 }
 
 void UrHardwareInterface::read()
 {
-    std::vector<double> pos, vel, current, tcp;
+  std::vector<double> pos, vel, current, tcp;
-    pos = robot_->rt_interface_->robot_state_->getQActual();
+  pos = robot_->rt_interface_->robot_state_->getQActual();
-    vel = robot_->rt_interface_->robot_state_->getQdActual();
+  vel = robot_->rt_interface_->robot_state_->getQdActual();
-    current = robot_->rt_interface_->robot_state_->getIActual();
+  current = robot_->rt_interface_->robot_state_->getIActual();
-    tcp = robot_->rt_interface_->robot_state_->getTcpForce();
+  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_position_[i] = pos[i];
-        joint_effort_[i] = current[i];
+    joint_velocity_[i] = vel[i];
-    }
+    joint_effort_[i] = current[i];
-    for (std::size_t i = 0; i < 3; ++i) {
+  }
-        robot_force_[i] = tcp[i];
+  for (std::size_t i = 0; i < 3; ++i)
-        robot_torque_[i] = tcp[i + 3];
+  {
-    }
+    robot_force_[i] = tcp[i];
+    robot_torque_[i] = tcp[i + 3];
+  }
 }
 
 void UrHardwareInterface::setMaxVelChange(double inp)
 {
-    max_vel_change_ = inp;
+  max_vel_change_ = inp;
 }
 
 void UrHardwareInterface::write()
 {
-    if (velocity_interface_running_) {
+  if (velocity_interface_running_)
-        std::vector<double> cmd;
+  {
-        //do some rate limiting
+    std::vector<double> cmd;
-        cmd.resize(joint_velocity_command_.size());
+    // do some rate limiting
-        for (unsigned int i = 0; i < joint_velocity_command_.size(); i++) {
+    cmd.resize(joint_velocity_command_.size());
-            cmd[i] = joint_velocity_command_[i];
+    for (unsigned int i = 0; i < joint_velocity_command_.size(); i++)
-            if (cmd[i] > prev_joint_velocity_command_[i] + max_vel_change_) {
+    {
-                cmd[i] = prev_joint_velocity_command_[i] + max_vel_change_;
+      cmd[i] = joint_velocity_command_[i];
-            } else if (cmd[i]
+      if (cmd[i] > prev_joint_velocity_command_[i] + max_vel_change_)
-                < prev_joint_velocity_command_[i] - max_vel_change_) {
+      {
-                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];
+      else if (cmd[i] < prev_joint_velocity_command_[i] - max_vel_change_)
-        }
+      {
-        robot_->setSpeed(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], max_vel_change_ * 125);
+        cmd[i] = prev_joint_velocity_command_[i] - max_vel_change_;
-    } else if (position_interface_running_) {
+      }
-        robot_->servoj(joint_position_command_);
+      prev_joint_velocity_command_[i] = cmd[i];
     }
+    robot_->setSpeed(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], max_vel_change_ * 125);
+  }
+  else if (position_interface_running_)
+  {
+    robot_->servoj(joint_position_command_);
+  }
 }
 
-bool UrHardwareInterface::canSwitch(
+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
-    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());
+                  controller_it->hardware_interface.c_str());
-                return false;
+        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();
+        bool error = true;
-                     stop_controller_it != stop_list.end();
+        for (std::list<hardware_interface::ControllerInfo>::const_iterator stop_controller_it = stop_list.begin();
-                     ++stop_controller_it) {
+             stop_controller_it != stop_list.end(); ++stop_controller_it)
-                    if (stop_controller_it->hardware_interface
+        {
-                        == "hardware_interface::PositionJointInterface") {
+          if (stop_controller_it->hardware_interface == "hardware_interface::PositionJointInterface")
-                        error = false;
+          {
-                        break;
+            error = false;
-                    }
+            break;
-                }
+          }
-                if (error) {
-                    ROS_ERROR(
-                        "%s (type %s) can not be run simultaneously with a PositionJointInterface",
-                        controller_it->name.c_str(),
-                        controller_it->hardware_interface.c_str());
-                    return false;
-                }
-            }
-        } else if (controller_it->hardware_interface
-            == "hardware_interface::PositionJointInterface") {
-            if (position_interface_running_) {
-                ROS_ERROR(
-                    "%s: An interface of that type (%s) is already running",
-                    controller_it->name.c_str(),
-                    controller_it->hardware_interface.c_str());
-                return false;
-            }
-            if (velocity_interface_running_) {
-                bool error = true;
-                for (std::list<hardware_interface::ControllerInfo>::const_iterator stop_controller_it = stop_list.begin();
-                     stop_controller_it != stop_list.end();
-                     ++stop_controller_it) {
-                    if (stop_controller_it->hardware_interface
-                        == "hardware_interface::VelocityJointInterface") {
-                        error = false;
-                        break;
-                    }
-                }
-                if (error) {
-                    ROS_ERROR(
-                        "%s (type %s) can not be run simultaneously with a VelocityJointInterface",
-                        controller_it->name.c_str(),
-                        controller_it->hardware_interface.c_str());
-                    return false;
-                }
-            }
         }
+        if (error)
+        {
+          ROS_ERROR("%s (type %s) can not be run simultaneously with a PositionJointInterface",
+                    controller_it->name.c_str(), controller_it->hardware_interface.c_str());
+          return false;
+        }
+      }
     }
+    else if (controller_it->hardware_interface == "hardware_interface::PositionJointInterface")
+    {
+      if (position_interface_running_)
+      {
+        ROS_ERROR("%s: An interface of that type (%s) is already running", controller_it->name.c_str(),
+                  controller_it->hardware_interface.c_str());
+        return false;
+      }
+      if (velocity_interface_running_)
+      {
+        bool error = true;
+        for (std::list<hardware_interface::ControllerInfo>::const_iterator stop_controller_it = stop_list.begin();
+             stop_controller_it != stop_list.end(); ++stop_controller_it)
+        {
+          if (stop_controller_it->hardware_interface == "hardware_interface::VelocityJointInterface")
+          {
+            error = false;
+            break;
+          }
+        }
+        if (error)
+        {
+          ROS_ERROR("%s (type %s) can not be run simultaneously with a VelocityJointInterface",
+                    controller_it->name.c_str(), controller_it->hardware_interface.c_str());
+          return false;
+        }
+      }
+    }
+  }
 
-    // we can always stop a controller
+  // we can always stop a controller
-    return true;
+  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)
-    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");
+      velocity_interface_running_ = false;
-        }
+      ROS_DEBUG("Stopping velocity interface");
-        if (controller_it->hardware_interface
-            == "hardware_interface::PositionJointInterface") {
-            position_interface_running_ = false;
-            std::vector<double> tmp;
-            robot_->closeServo(tmp);
-            ROS_DEBUG("Stopping position interface");
-        }
     }
-    for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin(); controller_it != start_list.end();
+    if (controller_it->hardware_interface == "hardware_interface::PositionJointInterface")
-         ++controller_it) {
+    {
-        if (controller_it->hardware_interface
+      position_interface_running_ = false;
-            == "hardware_interface::VelocityJointInterface") {
+      std::vector<double> tmp;
-            velocity_interface_running_ = true;
+      robot_->closeServo(tmp);
-            ROS_DEBUG("Starting velocity interface");
+      ROS_DEBUG("Stopping position interface");
-        }
-        if (controller_it->hardware_interface
-            == "hardware_interface::PositionJointInterface") {
-            position_interface_running_ = true;
-            robot_->uploadProg();
-            ROS_DEBUG("Starting position interface");
-        }
     }
+  }
+  for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin();
+       controller_it != start_list.end(); ++controller_it)
+  {
+    if (controller_it->hardware_interface == "hardware_interface::VelocityJointInterface")
+    {
+      velocity_interface_running_ = true;
+      ROS_DEBUG("Starting velocity interface");
+    }
+    if (controller_it->hardware_interface == "hardware_interface::PositionJointInterface")
+    {
+      position_interface_running_ = true;
+      robot_->uploadProg();
+      ROS_DEBUG("Starting position interface");
+    }
+  }
 }
 
-} // namespace
+}  // namespace

src/ur_realtime_communication.cpp

@@ -18,183 +18,194 @@
 
 #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)
-    unsigned int safety_count_max)
 {
-    robot_state_ = new RobotStateRT(msg_cond);
+  robot_state_ = new RobotStateRT(msg_cond);
-    bzero((char*)&serv_addr_, sizeof(serv_addr_));
+  bzero((char*)&serv_addr_, sizeof(serv_addr_));
-    sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
+  sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
-    if (sockfd_ < 0) {
+  if (sockfd_ < 0)
-        print_fatal("ERROR opening socket");
+  {
-    }
+    print_fatal("ERROR opening socket");
-    server_ = gethostbyname(host.c_str());
+  }
-    if (server_ == NULL) {
+  server_ = gethostbyname(host.c_str());
-        print_fatal("ERROR, no such host");
+  if (server_ == NULL)
-    }
+  {
-    serv_addr_.sin_family = AF_INET;
+    print_fatal("ERROR, no such host");
-    bcopy((char*)server_->h_addr, (char*)&serv_addr_.sin_addr.s_addr, server_->h_length);
+  }
-    serv_addr_.sin_port = htons(30003);
+  serv_addr_.sin_family = AF_INET;
-    flag_ = 1;
+  bcopy((char*)server_->h_addr, (char*)&serv_addr_.sin_addr.s_addr, server_->h_length);
-    setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
+  serv_addr_.sin_port = htons(30003);
-    setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
+  flag_ = 1;
-    setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
+  setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
-    fcntl(sockfd_, F_SETFL, O_NONBLOCK);
+  setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
-    connected_ = false;
+  setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
-    keepalive_ = false;
+  fcntl(sockfd_, F_SETFL, O_NONBLOCK);
-    safety_count_ = safety_count_max + 1;
+  connected_ = false;
-    safety_count_max_ = safety_count_max;
+  keepalive_ = false;
+  safety_count_ = safety_count_max + 1;
+  safety_count_max_ = safety_count_max;
 }
 
 bool UrRealtimeCommunication::start()
 {
-    fd_set writefds;
+  fd_set writefds;
-    struct timeval timeout;
+  struct timeval timeout;
 
-    keepalive_ = true;
+  keepalive_ = true;
-    print_debug("Realtime port: Connecting...");
+  print_debug("Realtime port: Connecting...");
 
-    connect(sockfd_, (struct sockaddr*)&serv_addr_, sizeof(serv_addr_));
+  connect(sockfd_, (struct sockaddr*)&serv_addr_, sizeof(serv_addr_));
-    FD_ZERO(&writefds);
+  FD_ZERO(&writefds);
-    FD_SET(sockfd_, &writefds);
+  FD_SET(sockfd_, &writefds);
-    timeout.tv_sec = 10;
+  timeout.tv_sec = 10;
-    timeout.tv_usec = 0;
+  timeout.tv_usec = 0;
-    select(sockfd_ + 1, NULL, &writefds, NULL, &timeout);
+  select(sockfd_ + 1, NULL, &writefds, NULL, &timeout);
-    unsigned int flag_len;
+  unsigned int flag_len;
-    getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &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;
+    print_fatal("Error connecting to RT port 30003");
-    }
+    return false;
-    sockaddr_in name;
+  }
-    socklen_t namelen = sizeof(name);
+  sockaddr_in name;
-    int err = getsockname(sockfd_, (sockaddr*)&name, &namelen);
+  socklen_t namelen = sizeof(name);
-    if (err < 0) {
+  int err = getsockname(sockfd_, (sockaddr*)&name, &namelen);
-        print_fatal("Could not get local IP");
+  if (err < 0)
-        close(sockfd_);
+  {
-        return false;
+    print_fatal("Could not get local IP");
-    }
+    close(sockfd_);
-    char str[18];
+    return false;
-    inet_ntop(AF_INET, &name.sin_addr, str, 18);
+  }
-    local_ip_ = str;
+  char str[18];
-    comThread_ = std::thread(&UrRealtimeCommunication::run, this);
+  inet_ntop(AF_INET, &name.sin_addr, str, 18);
-    return true;
+  local_ip_ = str;
+  comThread_ = std::thread(&UrRealtimeCommunication::run, this);
+  return true;
 }
 
 void UrRealtimeCommunication::halt()
 {
-    keepalive_ = false;
+  keepalive_ = false;
-    comThread_.join();
+  comThread_.join();
 }
 
 void UrRealtimeCommunication::addCommandToQueue(std::string inp)
 {
-    int bytes_written;
+  int bytes_written;
-    if (inp.back() != '\n') {
+  if (inp.back() != '\n')
-        inp.append("\n");
+  {
-    }
+    inp.append("\n");
-    if (connected_)
+  }
-        bytes_written = write(sockfd_, inp.c_str(), inp.length());
+  if (connected_)
-    else
+    bytes_written = write(sockfd_, inp.c_str(), inp.length());
-        print_error("Could not send command \"" + inp + "\". The robot is not connected! Command is discarded");
+  else
+    print_error("Could not send command \"" + inp + "\". The robot is not connected! Command is discarded");
 }
 
-void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2,
+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];
+  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));
-    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
+    // If a joint speed is set, make sure we stop it again after some time if the user doesn't
-        safety_count_ = 0;
+    safety_count_ = 0;
-    }
+  }
 }
 
 void UrRealtimeCommunication::run()
 {
-    uint8_t buf[2048];
+  uint8_t buf[2048];
-    int bytes_read;
+  int bytes_read;
-    bzero(buf, 2048);
+  bzero(buf, 2048);
-    struct timeval timeout;
+  struct timeval timeout;
-    fd_set readfds;
+  fd_set readfds;
-    FD_ZERO(&readfds);
+  FD_ZERO(&readfds);
-    FD_SET(sockfd_, &readfds);
+  FD_SET(sockfd_, &readfds);
-    print_debug("Realtime port: Got connection");
+  print_debug("Realtime port: Got connection");
-    connected_ = true;
+  connected_ = true;
-    while (keepalive_) {
+  while (keepalive_)
-        while (connected_ && keepalive_) {
+  {
-            timeout.tv_sec = 0; //do this each loop as selects modifies timeout
+    while (connected_ && keepalive_)
-            timeout.tv_usec = 500000; // timeout of 0.5 sec
+    {
-            select(sockfd_ + 1, &readfds, NULL, NULL, &timeout);
+      timeout.tv_sec = 0;        // do this each loop as selects modifies timeout
-            bytes_read = read(sockfd_, buf, 2048);
+      timeout.tv_usec = 500000;  // timeout of 0.5 sec
-            if (bytes_read > 0) {
+      select(sockfd_ + 1, &readfds, NULL, NULL, &timeout);
-                setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
+      bytes_read = read(sockfd_, buf, 2048);
-                    sizeof(int));
+      if (bytes_read > 0)
-                robot_state_->unpack(buf);
+      {
-                if (safety_count_ == safety_count_max_) {
+        setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
-                    setSpeed(0., 0., 0., 0., 0., 0.);
+        robot_state_->unpack(buf);
-                }
+        if (safety_count_ == safety_count_max_)
-                safety_count_ += 1;
+        {
-            } else {
+          setSpeed(0., 0., 0., 0., 0., 0.);
-                connected_ = false;
-                close(sockfd_);
-            }
-        }
-        if (keepalive_) {
-            //reconnect
-            print_warning("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...");
-            sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
-            if (sockfd_ < 0) {
-                print_fatal("ERROR opening socket");
-            }
-            flag_ = 1;
-            setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_,
-                sizeof(int));
-            setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
-                sizeof(int));
-
-            setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_,
-                sizeof(int));
-            fcntl(sockfd_, F_SETFL, O_NONBLOCK);
-            while (keepalive_ && !connected_) {
-                std::this_thread::sleep_for(std::chrono::seconds(10));
-                fd_set writefds;
-
-                connect(sockfd_, (struct sockaddr*)&serv_addr_,
-                    sizeof(serv_addr_));
-                FD_ZERO(&writefds);
-                FD_SET(sockfd_, &writefds);
-                select(sockfd_ + 1, NULL, &writefds, NULL, NULL);
-                unsigned int flag_len;
-                getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
-                if (flag_ < 0) {
-                    print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds...");
-                } else {
-                    connected_ = true;
-                    print_info("Realtime port: Reconnected");
-                }
-            }
         }
+        safety_count_ += 1;
+      }
+      else
+      {
+        connected_ = false;
+        close(sockfd_);
+      }
     }
-    setSpeed(0., 0., 0., 0., 0., 0.);
+    if (keepalive_)
-    close(sockfd_);
+    {
+      // reconnect
+      print_warning("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...");
+      sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
+      if (sockfd_ < 0)
+      {
+        print_fatal("ERROR opening socket");
+      }
+      flag_ = 1;
+      setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
+      setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
+
+      setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
+      fcntl(sockfd_, F_SETFL, O_NONBLOCK);
+      while (keepalive_ && !connected_)
+      {
+        std::this_thread::sleep_for(std::chrono::seconds(10));
+        fd_set writefds;
+
+        connect(sockfd_, (struct sockaddr*)&serv_addr_, sizeof(serv_addr_));
+        FD_ZERO(&writefds);
+        FD_SET(sockfd_, &writefds);
+        select(sockfd_ + 1, NULL, &writefds, NULL, NULL);
+        unsigned int flag_len;
+        getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
+        if (flag_ < 0)
+        {
+          print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 "
+                      "seconds...");
+        }
+        else
+        {
+          connected_ = true;
+          print_info("Realtime port: Reconnected");
+        }
+      }
+    }
+  }
+  setSpeed(0., 0., 0., 0., 0., 0.);
+  close(sockfd_);
 }
 
 void UrRealtimeCommunication::setSafetyCountMax(uint inp)
 {
-    safety_count_max_ = inp;
+  safety_count_max_ = inp;
 }
 
 std::string UrRealtimeCommunication::getLocalIp()
 {
-    return local_ip_;
+  return local_ip_;
 }