diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..95f7eb5 --- /dev/null +++ b/.clang-format @@ -0,0 +1,95 @@ +--- +Language: Cpp +# BasedOnStyle: WebKit +AccessModifierOffset: -4 +AlignAfterOpenBracket: DontAlign +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlinesLeft: false +AlignOperands: false +AlignTrailingComments: false +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: All +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakTemplateDeclarations: false +BinPackArguments: true +BinPackParameters: true +BraceWrapping: + AfterClass: false + AfterControlStatement: false + AfterEnum: false + AfterFunction: true + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + BeforeCatch: false + BeforeElse: false + IndentBraces: false +BreakBeforeBinaryOperators: All +BreakBeforeBraces: WebKit +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: true +BreakAfterJavaFieldAnnotations: false +BreakStringLiterals: true +ColumnLimit: 0 +CommentPragmas: '^ IWYU pragma:' +ConstructorInitializerAllOnOneLineOrOnePerLine: false +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: false +DerivePointerAlignment: false +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] +IncludeCategories: + - Regex: '^"(llvm|llvm-c|clang|clang-c)/' + Priority: 2 + - Regex: '^(<|"(gtest|isl|json)/)' + Priority: 3 + - Regex: '.*' + Priority: 1 +IncludeIsMainRegex: '$' +IndentCaseLabels: false +IndentWidth: 4 +IndentWrappedFunctionNames: false +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: true +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: Inner +ObjCBlockIndentWidth: 4 +ObjCSpaceAfterProperty: true +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 60 +PointerAlignment: Left +ReflowComments: true +SortIncludes: true +SpaceAfterCStyleCast: false +SpaceBeforeAssignmentOperators: true +SpaceBeforeParens: ControlStatements +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 1 +SpacesInAngles: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Cpp03 +TabWidth: 8 +UseTab: Never +... + diff --git a/include/ur_modern_driver/bin_parser.h b/include/ur_modern_driver/bin_parser.h index c7f5d7c..12d49cb 100644 --- a/include/ur_modern_driver/bin_parser.h +++ b/include/ur_modern_driver/bin_parser.h @@ -1,146 +1,171 @@ #pragma once -#include -#include +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/types.h" +#include #include #include +#include +#include #include -#include -#include "ur_modern_driver/types.h" -#include "ur_modern_driver/log.h" class BinParser { private: uint8_t *_buf_pos, *_buf_end; - BinParser &_parent; + BinParser& _parent; public: - BinParser(uint8_t *buffer, size_t buf_len) : - _buf_pos(buffer), - _buf_end(buffer+buf_len), - _parent(*this) { - assert(_buf_pos <= _buf_end); - } - - BinParser(BinParser &parent, size_t sub_len) : - _buf_pos(parent._buf_pos), - _buf_end(parent._buf_pos+sub_len), - _parent(parent) { - assert(_buf_pos <= _buf_end); - } + BinParser(uint8_t* buffer, size_t buf_len) + : _buf_pos(buffer) + , _buf_end(buffer + buf_len) + , _parent(*this) + { + assert(_buf_pos <= _buf_end); + } - ~BinParser() { + BinParser(BinParser& parent, size_t sub_len) + : _buf_pos(parent._buf_pos) + , _buf_end(parent._buf_pos + sub_len) + , _parent(parent) + { + assert(_buf_pos <= _buf_end); + } + + ~BinParser() + { _parent._buf_pos = _buf_pos; - } - + } //Decode from network encoding (big endian) to host encoding - template - T decode(T val) { + template + T decode(T val) + { return val; } - uint16_t decode(uint16_t val) { + uint16_t decode(uint16_t val) + { return be16toh(val); } - uint32_t decode(uint32_t val) { + uint32_t decode(uint32_t val) + { return be32toh(val); } - uint64_t decode(uint64_t val) { + uint64_t decode(uint64_t val) + { return be64toh(val); } - int16_t decode(int16_t val) { + int16_t decode(int16_t val) + { return be16toh(val); } - int32_t decode(int32_t val) { + int32_t decode(int32_t val) + { return be32toh(val); } - int64_t decode(int64_t val) { + int64_t decode(int64_t val) + { return be64toh(val); } - float decode(float val) { + float decode(float val) + { return be32toh(val); } - double decode(double val) { + double decode(double val) + { return be64toh(val); } - - template - T peek() { + template + T peek() + { assert(_buf_pos <= _buf_end); return decode(*(reinterpret_cast(_buf_pos))); } - template - void parse(T &val) { + template + void parse(T& val) + { val = peek(); _buf_pos += sizeof(T); } - // 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 - void parse(bool &val) { + void parse(bool& val) + { uint8_t inner; parse(inner); - val = inner != 0; + val = inner != 0; } // 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.y); parse(val.z); } // 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.rotation); } - void parse_remainder(std::string &val) { + void parse_remainder(std::string& val) + { 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(_buf_pos), len); _buf_pos += len; } - // Special string parse function that assumes uint8_t len followed by chars - void parse(std::string &val) { + // Special string parse function that assumes uint8_t len followed by chars + void parse(std::string& val) + { uint8_t len; parse(len); parse(val, size_t(len)); } - template - void parse(T (&array)[N]) { - for(size_t i = 0; i < N; i++) { + template + void parse(T (&array)[N]) + { + for (size_t i = 0; i < N; i++) { parse(array[i]); } } - void consume() { + void consume() + { _buf_pos = _buf_end; } - void consume(size_t bytes) { + void consume(size_t bytes) + { _buf_pos += bytes; } - bool check_size(size_t bytes) { + bool check_size(size_t bytes) + { return bytes <= size_t(_buf_end - _buf_pos); } - template - bool check_size(void) { + template + bool check_size(void) + { return check_size(T::SIZE); } - bool empty() { + bool empty() + { return _buf_pos == _buf_end; } - void debug() { + void debug() + { LOG_DEBUG("BinParser: %p - %p (%zu bytes)", _buf_pos, _buf_end, _buf_end - _buf_pos); } }; diff --git a/include/ur_modern_driver/do_output.h b/include/ur_modern_driver/do_output.h index d52420e..44c1e63 100644 --- a/include/ur_modern_driver/do_output.h +++ b/include/ur_modern_driver/do_output.h @@ -30,5 +30,4 @@ void print_warning(std::string inp); void print_error(std::string inp); void print_fatal(std::string inp); - #endif /* UR_DO_OUTPUT_H_ */ diff --git a/include/ur_modern_driver/log.h b/include/ur_modern_driver/log.h index db0f5b6..b66267d 100644 --- a/include/ur_modern_driver/log.h +++ b/include/ur_modern_driver/log.h @@ -2,20 +2,20 @@ #include #ifdef ROS_BUILD - #include +#include - #define LOG_DEBUG ROS_DEBUG - #define LOG_WARN ROS_WARN - #define LOG_INFO ROS_INFO - #define LOG_ERROR ROS_ERROR - #define LOG_FATAL ROS_FATAL +#define LOG_DEBUG ROS_DEBUG +#define LOG_WARN ROS_WARN +#define LOG_INFO ROS_INFO +#define LOG_ERROR ROS_ERROR +#define LOG_FATAL ROS_FATAL #else - #define LOG_DEBUG(format, ...) printf("[DEBUG]: " format "\n", ##__VA_ARGS__) - #define LOG_WARN(format, ...) printf("[WARNING]: " format "\n", ##__VA_ARGS__) - #define LOG_INFO(format, ...) printf("[INFO]: " format "\n", ##__VA_ARGS__) - #define LOG_ERROR(format, ...) printf("[ERROR]: " format "\n", ##__VA_ARGS__) - #define LOG_FATAL(format, ...) printf("[FATAL]: " format "\n", ##__VA_ARGS__) +#define LOG_DEBUG(format, ...) printf("[DEBUG]: " format "\n", ##__VA_ARGS__) +#define LOG_WARN(format, ...) printf("[WARNING]: " format "\n", ##__VA_ARGS__) +#define LOG_INFO(format, ...) printf("[INFO]: " format "\n", ##__VA_ARGS__) +#define LOG_ERROR(format, ...) printf("[ERROR]: " format "\n", ##__VA_ARGS__) +#define LOG_FATAL(format, ...) printf("[FATAL]: " format "\n", ##__VA_ARGS__) #endif \ No newline at end of file diff --git a/include/ur_modern_driver/packet.h b/include/ur_modern_driver/packet.h index 7f316dc..541e3e5 100644 --- a/include/ur_modern_driver/packet.h +++ b/include/ur_modern_driver/packet.h @@ -3,5 +3,5 @@ class Packet { public: - virtual bool parse_with(BinParser &bp) = 0; + virtual bool parse_with(BinParser& bp) = 0; }; \ No newline at end of file diff --git a/include/ur_modern_driver/parser.h b/include/ur_modern_driver/parser.h index d7a1222..9997515 100644 --- a/include/ur_modern_driver/parser.h +++ b/include/ur_modern_driver/parser.h @@ -4,5 +4,5 @@ class Parser { public: - virtual std::unique_ptr parse(BinParser &bp) = 0; + virtual std::unique_ptr parse(BinParser& bp) = 0; }; diff --git a/include/ur_modern_driver/pipeline.h b/include/ur_modern_driver/pipeline.h index b718f62..13a9b1b 100644 --- a/include/ur_modern_driver/pipeline.h +++ b/include/ur_modern_driver/pipeline.h @@ -1,21 +1,20 @@ #pragma once -#include -#include -#include #include "ur_modern_driver/log.h" #include "ur_modern_driver/queue/readerwriterqueue.h" +#include +#include +#include -using namespace moodycamel; +using namespace moodycamel; using namespace std; - template class IConsumer { public: - virtual void setup_consumer() { } - virtual void teardown_consumer() { } - virtual void stop_consumer() { } + virtual void setup_consumer() {} + virtual void teardown_consumer() {} + virtual void stop_consumer() {} virtual bool consume(unique_ptr product) = 0; }; @@ -23,33 +22,33 @@ public: template class IProducer { public: - virtual void setup_producer() { } - virtual void teardown_producer() { } - virtual void stop_producer() { } + virtual void setup_producer() {} + virtual void teardown_producer() {} + virtual void stop_producer() {} - virtual bool try_get(std::vector> &products) = 0; + virtual bool try_get(std::vector >& products) = 0; }; - template class Pipeline { private: - IProducer &_producer; - IConsumer &_consumer; - BlockingReaderWriterQueue> _queue; + IProducer& _producer; + IConsumer& _consumer; + BlockingReaderWriterQueue > _queue; atomic _running; thread _pThread, _cThread; - void run_producer() { + void run_producer() + { _producer.setup_producer(); - std::vector> products; - while(_running) { - if(!_producer.try_get(products)) { + std::vector > products; + while (_running) { + if (!_producer.try_get(products)) { break; } - - for(auto &p : products) { - if(!_queue.try_enqueue(std::move(p))) { + + for (auto& p : products) { + if (!_queue.try_enqueue(std::move(p))) { LOG_WARN("Pipeline owerflowed!"); } } @@ -60,27 +59,31 @@ private: //todo cleanup } - void run_consumer() { + void run_consumer() + { _consumer.setup_consumer(); unique_ptr product; - while(_running) { + while (_running) { _queue.wait_dequeue(product); - if(!_consumer.consume(std::move(product))) + if (!_consumer.consume(std::move(product))) break; } _consumer.teardown_consumer(); //todo cleanup } -public: - Pipeline(IProducer &producer, IConsumer &consumer) - : _producer(producer), - _consumer(consumer), - _queue{32}, - _running{false} - { } - void run() { - if(_running) +public: + Pipeline(IProducer& producer, IConsumer& consumer) + : _producer(producer) + , _consumer(consumer) + , _queue{ 32 } + , _running{ false } + { + } + + void run() + { + if (_running) return; _running = true; @@ -88,10 +91,11 @@ public: _cThread = thread(&Pipeline::run_consumer, this); } - void stop() { - if(!_running) + void stop() + { + if (!_running) return; - + _consumer.stop_consumer(); _producer.stop_producer(); diff --git a/include/ur_modern_driver/queue/atomicops.h b/include/ur_modern_driver/queue/atomicops.h index c375710..af0089e 100644 --- a/include/ur_modern_driver/queue/atomicops.h +++ b/include/ur_modern_driver/queue/atomicops.h @@ -12,10 +12,10 @@ // Uses the AE_* prefix for macros (historical reasons), and the "moodycamel" namespace for symbols. #include -#include #include #include #include +#include // Platform detection #if defined(__INTEL_COMPILER) @@ -38,22 +38,19 @@ #define AE_ARCH_UNKNOWN #endif - // AE_UNUSED #define AE_UNUSED(x) ((void)x) - // AE_FORCEINLINE #if defined(AE_VCPP) || defined(AE_ICC) #define AE_FORCEINLINE __forceinline #elif defined(AE_GCC) -//#define AE_FORCEINLINE __attribute__((always_inline)) +//#define AE_FORCEINLINE __attribute__((always_inline)) #define AE_FORCEINLINE inline #else #define AE_FORCEINLINE inline #endif - // AE_ALIGN #if defined(AE_VCPP) || defined(AE_ICC) #define AE_ALIGN(x) __declspec(align(x)) @@ -64,24 +61,23 @@ #define AE_ALIGN(x) __attribute__((aligned(x))) #endif - // Portable atomic fences implemented below: namespace moodycamel { enum memory_order { - memory_order_relaxed, - memory_order_acquire, - memory_order_release, - memory_order_acq_rel, - memory_order_seq_cst, + memory_order_relaxed, + memory_order_acquire, + memory_order_release, + memory_order_acq_rel, + memory_order_seq_cst, - // memory_order_sync: Forces a full sync: - // #LoadLoad, #LoadStore, #StoreStore, and most significantly, #StoreLoad - memory_order_sync = memory_order_seq_cst + // memory_order_sync: Forces a full sync: + // #LoadLoad, #LoadStore, #StoreStore, and most significantly, #StoreLoad + memory_order_sync = memory_order_seq_cst }; -} // end namespace moodycamel +} // end namespace moodycamel #if (defined(AE_VCPP) && (_MSC_VER < 1700 || defined(__cplusplus_cli))) || defined(AE_ICC) // VS2010 and ICC13 don't support std::atomic_*_fence, implement our own fences @@ -100,10 +96,9 @@ enum memory_order { #define AeLiteSync __lwsync #endif - #ifdef AE_VCPP #pragma warning(push) -#pragma warning(disable: 4365) // Disable erroneous 'conversion from long to unsigned int, signed/unsigned mismatch' error when using `assert` +#pragma warning(disable : 4365) // Disable erroneous 'conversion from long to unsigned int, signed/unsigned mismatch' error when using `assert` #ifdef __cplusplus_cli #pragma managed(push, off) #endif @@ -113,14 +108,24 @@ namespace moodycamel { AE_FORCEINLINE void compiler_fence(memory_order order) { - switch (order) { - case memory_order_relaxed: break; - case memory_order_acquire: _ReadBarrier(); break; - case memory_order_release: _WriteBarrier(); break; - case memory_order_acq_rel: _ReadWriteBarrier(); break; - case memory_order_seq_cst: _ReadWriteBarrier(); break; - default: assert(false); - } + switch (order) { + case memory_order_relaxed: + break; + case memory_order_acquire: + _ReadBarrier(); + break; + case memory_order_release: + _WriteBarrier(); + break; + case memory_order_acq_rel: + _ReadWriteBarrier(); + break; + case memory_order_seq_cst: + _ReadWriteBarrier(); + break; + default: + assert(false); + } } // x86/x64 have a strong memory model -- all loads and stores have @@ -129,51 +134,60 @@ AE_FORCEINLINE void compiler_fence(memory_order order) #if defined(AE_ARCH_X86) || defined(AE_ARCH_X64) AE_FORCEINLINE void fence(memory_order order) { - switch (order) { - case memory_order_relaxed: break; - case memory_order_acquire: _ReadBarrier(); break; - case memory_order_release: _WriteBarrier(); break; - case memory_order_acq_rel: _ReadWriteBarrier(); break; - case memory_order_seq_cst: - _ReadWriteBarrier(); - AeFullSync(); - _ReadWriteBarrier(); - break; - default: assert(false); - } + switch (order) { + case memory_order_relaxed: + break; + case memory_order_acquire: + _ReadBarrier(); + break; + case memory_order_release: + _WriteBarrier(); + break; + case memory_order_acq_rel: + _ReadWriteBarrier(); + break; + case memory_order_seq_cst: + _ReadWriteBarrier(); + AeFullSync(); + _ReadWriteBarrier(); + break; + default: + assert(false); + } } #else AE_FORCEINLINE void fence(memory_order order) { - // Non-specialized arch, use heavier memory barriers everywhere just in case :-( - switch (order) { - case memory_order_relaxed: - break; - case memory_order_acquire: - _ReadBarrier(); - AeLiteSync(); - _ReadBarrier(); - break; - case memory_order_release: - _WriteBarrier(); - AeLiteSync(); - _WriteBarrier(); - break; - case memory_order_acq_rel: - _ReadWriteBarrier(); - AeLiteSync(); - _ReadWriteBarrier(); - break; - case memory_order_seq_cst: - _ReadWriteBarrier(); - AeFullSync(); - _ReadWriteBarrier(); - break; - default: assert(false); - } + // Non-specialized arch, use heavier memory barriers everywhere just in case :-( + switch (order) { + case memory_order_relaxed: + break; + case memory_order_acquire: + _ReadBarrier(); + AeLiteSync(); + _ReadBarrier(); + break; + case memory_order_release: + _WriteBarrier(); + AeLiteSync(); + _WriteBarrier(); + break; + case memory_order_acq_rel: + _ReadWriteBarrier(); + AeLiteSync(); + _ReadWriteBarrier(); + break; + case memory_order_seq_cst: + _ReadWriteBarrier(); + AeFullSync(); + _ReadWriteBarrier(); + break; + default: + assert(false); + } } #endif -} // end namespace moodycamel +} // end namespace moodycamel #else // Use standard library of atomics #include @@ -182,33 +196,52 @@ namespace moodycamel { AE_FORCEINLINE void compiler_fence(memory_order order) { - switch (order) { - case memory_order_relaxed: break; - case memory_order_acquire: std::atomic_signal_fence(std::memory_order_acquire); break; - case memory_order_release: std::atomic_signal_fence(std::memory_order_release); break; - case memory_order_acq_rel: std::atomic_signal_fence(std::memory_order_acq_rel); break; - case memory_order_seq_cst: std::atomic_signal_fence(std::memory_order_seq_cst); break; - default: assert(false); - } + switch (order) { + case memory_order_relaxed: + break; + case memory_order_acquire: + std::atomic_signal_fence(std::memory_order_acquire); + break; + case memory_order_release: + std::atomic_signal_fence(std::memory_order_release); + break; + case memory_order_acq_rel: + std::atomic_signal_fence(std::memory_order_acq_rel); + break; + case memory_order_seq_cst: + std::atomic_signal_fence(std::memory_order_seq_cst); + break; + default: + assert(false); + } } AE_FORCEINLINE void fence(memory_order order) { - switch (order) { - case memory_order_relaxed: break; - case memory_order_acquire: std::atomic_thread_fence(std::memory_order_acquire); break; - case memory_order_release: std::atomic_thread_fence(std::memory_order_release); break; - case memory_order_acq_rel: std::atomic_thread_fence(std::memory_order_acq_rel); break; - case memory_order_seq_cst: std::atomic_thread_fence(std::memory_order_seq_cst); break; - default: assert(false); - } + switch (order) { + case memory_order_relaxed: + break; + case memory_order_acquire: + std::atomic_thread_fence(std::memory_order_acquire); + break; + case memory_order_release: + std::atomic_thread_fence(std::memory_order_release); + break; + case memory_order_acq_rel: + std::atomic_thread_fence(std::memory_order_acq_rel); + break; + case memory_order_seq_cst: + std::atomic_thread_fence(std::memory_order_seq_cst); + break; + default: + assert(false); + } } -} // end namespace moodycamel +} // end namespace moodycamel #endif - #if !defined(AE_VCPP) || (_MSC_VER >= 1700 && !defined(__cplusplus_cli)) #define AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC #endif @@ -223,102 +256,126 @@ AE_FORCEINLINE void fence(memory_order order) // The guarantee of atomicity is only made for types that already have atomic load and store guarantees // at the hardware level -- on most platforms this generally means aligned pointers and integers (only). namespace moodycamel { -template -class weak_atomic -{ +template +class weak_atomic { public: - weak_atomic() { } + weak_atomic() {} #ifdef AE_VCPP -#pragma warning(disable: 4100) // Get rid of (erroneous) 'unreferenced formal parameter' warning +#pragma warning(disable : 4100) // Get rid of (erroneous) 'unreferenced formal parameter' warning #endif - template weak_atomic(U&& x) : value(std::forward(x)) { } + template + weak_atomic(U&& x) + : value(std::forward(x)) + { + } #ifdef __cplusplus_cli - // Work around bug with universal reference/nullptr combination that only appears when /clr is on - weak_atomic(nullptr_t) : value(nullptr) { } + // Work around bug with universal reference/nullptr combination that only appears when /clr is on + weak_atomic(nullptr_t) + : value(nullptr) + { + } #endif - weak_atomic(weak_atomic const& other) : value(other.value) { } - weak_atomic(weak_atomic&& other) : value(std::move(other.value)) { } + weak_atomic(weak_atomic const& other) + : value(other.value) + { + } + weak_atomic(weak_atomic&& other) + : value(std::move(other.value)) + { + } #ifdef AE_VCPP -#pragma warning(default: 4100) +#pragma warning(default : 4100) #endif - AE_FORCEINLINE operator T() const { return load(); } + AE_FORCEINLINE operator T() const + { + return load(); + } - #ifndef AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC - template AE_FORCEINLINE weak_atomic const& operator=(U&& x) { value = std::forward(x); return *this; } - AE_FORCEINLINE weak_atomic const& operator=(weak_atomic const& other) { value = other.value; return *this; } - - AE_FORCEINLINE T load() const { return value; } - - AE_FORCEINLINE T fetch_add_acquire(T increment) - { -#if defined(AE_ARCH_X64) || defined(AE_ARCH_X86) - if (sizeof(T) == 4) return _InterlockedExchangeAdd((long volatile*)&value, (long)increment); -#if defined(_M_AMD64) - else if (sizeof(T) == 8) return _InterlockedExchangeAdd64((long long volatile*)&value, (long long)increment); -#endif -#else -#error Unsupported platform -#endif - assert(false && "T must be either a 32 or 64 bit type"); - return value; - } - - AE_FORCEINLINE T fetch_add_release(T increment) - { -#if defined(AE_ARCH_X64) || defined(AE_ARCH_X86) - if (sizeof(T) == 4) return _InterlockedExchangeAdd((long volatile*)&value, (long)increment); -#if defined(_M_AMD64) - else if (sizeof(T) == 8) return _InterlockedExchangeAdd64((long long volatile*)&value, (long long)increment); -#endif -#else -#error Unsupported platform -#endif - assert(false && "T must be either a 32 or 64 bit type"); - return value; - } -#else - template - AE_FORCEINLINE weak_atomic const& operator=(U&& x) - { - value.store(std::forward(x), std::memory_order_relaxed); - return *this; - } - - AE_FORCEINLINE weak_atomic const& operator=(weak_atomic const& other) - { - value.store(other.value.load(std::memory_order_relaxed), std::memory_order_relaxed); - return *this; - } + template + AE_FORCEINLINE weak_atomic const& operator=(U&& x) + { + value = std::forward(x); + return *this; + } + AE_FORCEINLINE weak_atomic const& operator=(weak_atomic const& other) + { + value = other.value; + return *this; + } - AE_FORCEINLINE T load() const { return value.load(std::memory_order_relaxed); } - - AE_FORCEINLINE T fetch_add_acquire(T increment) - { - return value.fetch_add(increment, std::memory_order_acquire); - } - - AE_FORCEINLINE T fetch_add_release(T increment) - { - return value.fetch_add(increment, std::memory_order_release); - } + AE_FORCEINLINE T load() const { return value; } + + AE_FORCEINLINE T fetch_add_acquire(T increment) + { +#if defined(AE_ARCH_X64) || defined(AE_ARCH_X86) + if (sizeof(T) == 4) + return _InterlockedExchangeAdd((long volatile*)&value, (long)increment); +#if defined(_M_AMD64) + else if (sizeof(T) == 8) + return _InterlockedExchangeAdd64((long long volatile*)&value, (long long)increment); +#endif +#else +#error Unsupported platform +#endif + assert(false && "T must be either a 32 or 64 bit type"); + return value; + } + + AE_FORCEINLINE T fetch_add_release(T increment) + { +#if defined(AE_ARCH_X64) || defined(AE_ARCH_X86) + if (sizeof(T) == 4) + return _InterlockedExchangeAdd((long volatile*)&value, (long)increment); +#if defined(_M_AMD64) + else if (sizeof(T) == 8) + return _InterlockedExchangeAdd64((long long volatile*)&value, (long long)increment); +#endif +#else +#error Unsupported platform +#endif + assert(false && "T must be either a 32 or 64 bit type"); + return value; + } +#else + template + AE_FORCEINLINE weak_atomic const& operator=(U&& x) + { + value.store(std::forward(x), std::memory_order_relaxed); + return *this; + } + + AE_FORCEINLINE weak_atomic const& operator=(weak_atomic const& other) + { + value.store(other.value.load(std::memory_order_relaxed), std::memory_order_relaxed); + return *this; + } + + AE_FORCEINLINE T load() const { return value.load(std::memory_order_relaxed); } + + AE_FORCEINLINE T fetch_add_acquire(T increment) + { + return value.fetch_add(increment, std::memory_order_acquire); + } + + AE_FORCEINLINE T fetch_add_release(T increment) + { + return value.fetch_add(increment, std::memory_order_release); + } #endif - private: #ifndef AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC - // No std::atomic support, but still need to circumvent compiler optimizations. - // `volatile` will make memory access slow, but is guaranteed to be reliable. - volatile T value; + // No std::atomic support, but still need to circumvent compiler optimizations. + // `volatile` will make memory access slow, but is guaranteed to be reliable. + volatile T value; #else - std::atomic value; + std::atomic value; #endif }; -} // end namespace moodycamel - - +} // end namespace moodycamel // Portable single-producer, single-consumer semaphore below: @@ -329,11 +386,11 @@ private: // I know this is an ugly hack but it still beats polluting the global // namespace with thousands of generic names or adding a .cpp for nothing. extern "C" { - struct _SECURITY_ATTRIBUTES; - __declspec(dllimport) void* __stdcall CreateSemaphoreW(_SECURITY_ATTRIBUTES* lpSemaphoreAttributes, long lInitialCount, long lMaximumCount, const wchar_t* lpName); - __declspec(dllimport) int __stdcall CloseHandle(void* hObject); - __declspec(dllimport) unsigned long __stdcall WaitForSingleObject(void* hHandle, unsigned long dwMilliseconds); - __declspec(dllimport) int __stdcall ReleaseSemaphore(void* hSemaphore, long lReleaseCount, long* lpPreviousCount); +struct _SECURITY_ATTRIBUTES; +__declspec(dllimport) void* __stdcall CreateSemaphoreW(_SECURITY_ATTRIBUTES* lpSemaphoreAttributes, long lInitialCount, long lMaximumCount, const wchar_t* lpName); +__declspec(dllimport) int __stdcall CloseHandle(void* hObject); +__declspec(dllimport) unsigned long __stdcall WaitForSingleObject(void* hHandle, unsigned long dwMilliseconds); +__declspec(dllimport) int __stdcall ReleaseSemaphore(void* hSemaphore, long lReleaseCount, long* lpPreviousCount); } #elif defined(__MACH__) #include @@ -341,320 +398,305 @@ extern "C" { #include #endif -namespace moodycamel -{ - // Code in the spsc_sema namespace below is an adaptation of Jeff Preshing's - // portable + lightweight semaphore implementations, originally from - // https://github.com/preshing/cpp11-on-multicore/blob/master/common/sema.h - // LICENSE: - // Copyright (c) 2015 Jeff Preshing - // - // This software is provided 'as-is', without any express or implied - // warranty. In no event will the authors be held liable for any damages - // arising from the use of this software. - // - // Permission is granted to anyone to use this software for any purpose, - // including commercial applications, and to alter it and redistribute it - // freely, subject to the following restrictions: - // - // 1. The origin of this software must not be misrepresented; you must not - // claim that you wrote the original software. If you use this software - // in a product, an acknowledgement in the product documentation would be - // appreciated but is not required. - // 2. Altered source versions must be plainly marked as such, and must not be - // misrepresented as being the original software. - // 3. This notice may not be removed or altered from any source distribution. - namespace spsc_sema - { +namespace moodycamel { +// Code in the spsc_sema namespace below is an adaptation of Jeff Preshing's +// portable + lightweight semaphore implementations, originally from +// https://github.com/preshing/cpp11-on-multicore/blob/master/common/sema.h +// LICENSE: +// Copyright (c) 2015 Jeff Preshing +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgement in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +namespace spsc_sema { #if defined(_WIN32) - class Semaphore - { - private: - void* m_hSema; - - Semaphore(const Semaphore& other); - Semaphore& operator=(const Semaphore& other); + class Semaphore { + private: + void* m_hSema; - public: - Semaphore(int initialCount = 0) - { - assert(initialCount >= 0); - const long maxLong = 0x7fffffff; - m_hSema = CreateSemaphoreW(nullptr, initialCount, maxLong, nullptr); - } + Semaphore(const Semaphore& other); + Semaphore& operator=(const Semaphore& other); - ~Semaphore() - { - CloseHandle(m_hSema); - } + public: + Semaphore(int initialCount = 0) + { + assert(initialCount >= 0); + const long maxLong = 0x7fffffff; + m_hSema = CreateSemaphoreW(nullptr, initialCount, maxLong, nullptr); + } - void wait() - { - const unsigned long infinite = 0xffffffff; - WaitForSingleObject(m_hSema, infinite); - } + ~Semaphore() + { + CloseHandle(m_hSema); + } - bool try_wait() - { - const unsigned long RC_WAIT_TIMEOUT = 0x00000102; - return WaitForSingleObject(m_hSema, 0) != RC_WAIT_TIMEOUT; - } + void wait() + { + const unsigned long infinite = 0xffffffff; + WaitForSingleObject(m_hSema, infinite); + } - bool timed_wait(std::uint64_t usecs) - { - const unsigned long RC_WAIT_TIMEOUT = 0x00000102; - return WaitForSingleObject(m_hSema, (unsigned long)(usecs / 1000)) != RC_WAIT_TIMEOUT; - } + bool try_wait() + { + const unsigned long RC_WAIT_TIMEOUT = 0x00000102; + return WaitForSingleObject(m_hSema, 0) != RC_WAIT_TIMEOUT; + } - void signal(int count = 1) - { - ReleaseSemaphore(m_hSema, count, nullptr); - } - }; + bool timed_wait(std::uint64_t usecs) + { + const unsigned long RC_WAIT_TIMEOUT = 0x00000102; + return WaitForSingleObject(m_hSema, (unsigned long)(usecs / 1000)) != RC_WAIT_TIMEOUT; + } + + void signal(int count = 1) + { + ReleaseSemaphore(m_hSema, count, nullptr); + } + }; #elif defined(__MACH__) - //--------------------------------------------------------- - // Semaphore (Apple iOS and OSX) - // Can't use POSIX semaphores due to http://lists.apple.com/archives/darwin-kernel/2009/Apr/msg00010.html - //--------------------------------------------------------- - class Semaphore - { - private: - semaphore_t m_sema; + //--------------------------------------------------------- + // Semaphore (Apple iOS and OSX) + // Can't use POSIX semaphores due to http://lists.apple.com/archives/darwin-kernel/2009/Apr/msg00010.html + //--------------------------------------------------------- + class Semaphore { + private: + semaphore_t m_sema; - Semaphore(const Semaphore& other); - Semaphore& operator=(const Semaphore& other); + Semaphore(const Semaphore& other); + Semaphore& operator=(const Semaphore& other); - public: - Semaphore(int initialCount = 0) - { - assert(initialCount >= 0); - semaphore_create(mach_task_self(), &m_sema, SYNC_POLICY_FIFO, initialCount); - } + public: + Semaphore(int initialCount = 0) + { + assert(initialCount >= 0); + semaphore_create(mach_task_self(), &m_sema, SYNC_POLICY_FIFO, initialCount); + } - ~Semaphore() - { - semaphore_destroy(mach_task_self(), m_sema); - } + ~Semaphore() + { + semaphore_destroy(mach_task_self(), m_sema); + } - void wait() - { - semaphore_wait(m_sema); - } + void wait() + { + semaphore_wait(m_sema); + } - bool try_wait() - { - return timed_wait(0); - } + bool try_wait() + { + return timed_wait(0); + } - bool timed_wait(std::int64_t timeout_usecs) - { - mach_timespec_t ts; - ts.tv_sec = timeout_usecs / 1000000; - ts.tv_nsec = (timeout_usecs % 1000000) * 1000; + bool timed_wait(std::int64_t timeout_usecs) + { + mach_timespec_t ts; + ts.tv_sec = timeout_usecs / 1000000; + ts.tv_nsec = (timeout_usecs % 1000000) * 1000; - // added in OSX 10.10: https://developer.apple.com/library/prerelease/mac/documentation/General/Reference/APIDiffsMacOSX10_10SeedDiff/modules/Darwin.html - kern_return_t rc = semaphore_timedwait(m_sema, ts); + // added in OSX 10.10: https://developer.apple.com/library/prerelease/mac/documentation/General/Reference/APIDiffsMacOSX10_10SeedDiff/modules/Darwin.html + kern_return_t rc = semaphore_timedwait(m_sema, ts); - return rc != KERN_OPERATION_TIMED_OUT; - } + return rc != KERN_OPERATION_TIMED_OUT; + } - void signal() - { - semaphore_signal(m_sema); - } + void signal() + { + semaphore_signal(m_sema); + } - void signal(int count) - { - while (count-- > 0) - { - semaphore_signal(m_sema); - } - } - }; + void signal(int count) + { + while (count-- > 0) { + semaphore_signal(m_sema); + } + } + }; #elif defined(__unix__) - //--------------------------------------------------------- - // Semaphore (POSIX, Linux) - //--------------------------------------------------------- - class Semaphore - { - private: - sem_t m_sema; + //--------------------------------------------------------- + // Semaphore (POSIX, Linux) + //--------------------------------------------------------- + class Semaphore { + private: + sem_t m_sema; - Semaphore(const Semaphore& other); - Semaphore& operator=(const Semaphore& other); + Semaphore(const Semaphore& other); + Semaphore& operator=(const Semaphore& other); - public: - Semaphore(int initialCount = 0) - { - assert(initialCount >= 0); - sem_init(&m_sema, 0, initialCount); - } + public: + Semaphore(int initialCount = 0) + { + assert(initialCount >= 0); + sem_init(&m_sema, 0, initialCount); + } - ~Semaphore() - { - sem_destroy(&m_sema); - } + ~Semaphore() + { + sem_destroy(&m_sema); + } - void wait() - { - // http://stackoverflow.com/questions/2013181/gdb-causes-sem-wait-to-fail-with-eintr-error - int rc; - do - { - rc = sem_wait(&m_sema); - } - while (rc == -1 && errno == EINTR); - } + void wait() + { + // http://stackoverflow.com/questions/2013181/gdb-causes-sem-wait-to-fail-with-eintr-error + int rc; + do { + rc = sem_wait(&m_sema); + } while (rc == -1 && errno == EINTR); + } - bool try_wait() - { - int rc; - do { - rc = sem_trywait(&m_sema); - } while (rc == -1 && errno == EINTR); - return !(rc == -1 && errno == EAGAIN); - } + bool try_wait() + { + int rc; + do { + rc = sem_trywait(&m_sema); + } while (rc == -1 && errno == EINTR); + return !(rc == -1 && errno == EAGAIN); + } - bool timed_wait(std::uint64_t usecs) - { - struct timespec ts; - const int usecs_in_1_sec = 1000000; - const int nsecs_in_1_sec = 1000000000; - clock_gettime(CLOCK_REALTIME, &ts); - ts.tv_sec += usecs / usecs_in_1_sec; - ts.tv_nsec += (usecs % usecs_in_1_sec) * 1000; - // sem_timedwait bombs if you have more than 1e9 in tv_nsec - // so we have to clean things up before passing it in - if (ts.tv_nsec > nsecs_in_1_sec) { - ts.tv_nsec -= nsecs_in_1_sec; - ++ts.tv_sec; - } + bool timed_wait(std::uint64_t usecs) + { + struct timespec ts; + const int usecs_in_1_sec = 1000000; + const int nsecs_in_1_sec = 1000000000; + clock_gettime(CLOCK_REALTIME, &ts); + ts.tv_sec += usecs / usecs_in_1_sec; + ts.tv_nsec += (usecs % usecs_in_1_sec) * 1000; + // sem_timedwait bombs if you have more than 1e9 in tv_nsec + // so we have to clean things up before passing it in + if (ts.tv_nsec > nsecs_in_1_sec) { + ts.tv_nsec -= nsecs_in_1_sec; + ++ts.tv_sec; + } - int rc; - do { - rc = sem_timedwait(&m_sema, &ts); - } while (rc == -1 && errno == EINTR); - return !(rc == -1 && errno == ETIMEDOUT); - } + int rc; + do { + rc = sem_timedwait(&m_sema, &ts); + } while (rc == -1 && errno == EINTR); + return !(rc == -1 && errno == ETIMEDOUT); + } - void signal() - { - sem_post(&m_sema); - } + void signal() + { + sem_post(&m_sema); + } - void signal(int count) - { - while (count-- > 0) - { - sem_post(&m_sema); - } - } - }; + void signal(int count) + { + while (count-- > 0) { + sem_post(&m_sema); + } + } + }; #else #error Unsupported platform! (No semaphore wrapper available) #endif - //--------------------------------------------------------- - // LightweightSemaphore - //--------------------------------------------------------- - class LightweightSemaphore - { - public: - typedef std::make_signed::type ssize_t; - - private: - weak_atomic m_count; - Semaphore m_sema; + //--------------------------------------------------------- + // LightweightSemaphore + //--------------------------------------------------------- + class LightweightSemaphore { + public: + typedef std::make_signed::type ssize_t; - bool waitWithPartialSpinning(std::int64_t timeout_usecs = -1) - { - ssize_t oldCount; - // Is there a better way to set the initial spin count? - // If we lower it to 1000, testBenaphore becomes 15x slower on my Core i7-5930K Windows PC, - // as threads start hitting the kernel semaphore. - int spin = 10000; - while (--spin >= 0) - { - if (m_count.load() > 0) - { - m_count.fetch_add_acquire(-1); - return true; - } - compiler_fence(memory_order_acquire); // Prevent the compiler from collapsing the loop. - } - oldCount = m_count.fetch_add_acquire(-1); - if (oldCount > 0) - return true; - if (timeout_usecs < 0) - { - m_sema.wait(); - return true; - } - if (m_sema.timed_wait(timeout_usecs)) - return true; - // At this point, we've timed out waiting for the semaphore, but the - // count is still decremented indicating we may still be waiting on - // it. So we have to re-adjust the count, but only if the semaphore - // wasn't signaled enough times for us too since then. If it was, we - // need to release the semaphore too. - while (true) - { - oldCount = m_count.fetch_add_release(1); - if (oldCount < 0) - return false; // successfully restored things to the way they were - // Oh, the producer thread just signaled the semaphore after all. Try again: - oldCount = m_count.fetch_add_acquire(-1); - if (oldCount > 0 && m_sema.try_wait()) - return true; - } - } + private: + weak_atomic m_count; + Semaphore m_sema; - public: - LightweightSemaphore(ssize_t initialCount = 0) : m_count(initialCount) - { - assert(initialCount >= 0); - } + bool waitWithPartialSpinning(std::int64_t timeout_usecs = -1) + { + ssize_t oldCount; + // Is there a better way to set the initial spin count? + // If we lower it to 1000, testBenaphore becomes 15x slower on my Core i7-5930K Windows PC, + // as threads start hitting the kernel semaphore. + int spin = 10000; + while (--spin >= 0) { + if (m_count.load() > 0) { + m_count.fetch_add_acquire(-1); + return true; + } + compiler_fence(memory_order_acquire); // Prevent the compiler from collapsing the loop. + } + oldCount = m_count.fetch_add_acquire(-1); + if (oldCount > 0) + return true; + if (timeout_usecs < 0) { + m_sema.wait(); + return true; + } + if (m_sema.timed_wait(timeout_usecs)) + return true; + // At this point, we've timed out waiting for the semaphore, but the + // count is still decremented indicating we may still be waiting on + // it. So we have to re-adjust the count, but only if the semaphore + // wasn't signaled enough times for us too since then. If it was, we + // need to release the semaphore too. + while (true) { + oldCount = m_count.fetch_add_release(1); + if (oldCount < 0) + return false; // successfully restored things to the way they were + // Oh, the producer thread just signaled the semaphore after all. Try again: + oldCount = m_count.fetch_add_acquire(-1); + if (oldCount > 0 && m_sema.try_wait()) + return true; + } + } - bool tryWait() - { - if (m_count.load() > 0) - { - m_count.fetch_add_acquire(-1); - return true; - } - return false; - } + public: + LightweightSemaphore(ssize_t initialCount = 0) + : m_count(initialCount) + { + assert(initialCount >= 0); + } - void wait() - { - if (!tryWait()) - waitWithPartialSpinning(); - } + bool tryWait() + { + if (m_count.load() > 0) { + m_count.fetch_add_acquire(-1); + return true; + } + return false; + } - bool wait(std::int64_t timeout_usecs) - { - return tryWait() || waitWithPartialSpinning(timeout_usecs); - } + void wait() + { + if (!tryWait()) + waitWithPartialSpinning(); + } - void signal(ssize_t count = 1) - { - assert(count >= 0); - ssize_t oldCount = m_count.fetch_add_release(count); - assert(oldCount >= -1); - if (oldCount < 0) - { - m_sema.signal(1); - } - } - - ssize_t availableApprox() const - { - ssize_t count = m_count.load(); - return count > 0 ? count : 0; - } - }; - } // end namespace spsc_sema -} // end namespace moodycamel + bool wait(std::int64_t timeout_usecs) + { + return tryWait() || waitWithPartialSpinning(timeout_usecs); + } + + void signal(ssize_t count = 1) + { + assert(count >= 0); + ssize_t oldCount = m_count.fetch_add_release(count); + assert(oldCount >= -1); + if (oldCount < 0) { + m_sema.signal(1); + } + } + + ssize_t availableApprox() const + { + ssize_t count = m_count.load(); + return count > 0 ? count : 0; + } + }; +} // end namespace spsc_sema +} // end namespace moodycamel #if defined(AE_VCPP) && (_MSC_VER < 1700 || defined(__cplusplus_cli)) #pragma warning(pop) diff --git a/include/ur_modern_driver/queue/readerwriterqueue.h b/include/ur_modern_driver/queue/readerwriterqueue.h index ec465d6..1503dcb 100644 --- a/include/ur_modern_driver/queue/readerwriterqueue.h +++ b/include/ur_modern_driver/queue/readerwriterqueue.h @@ -5,18 +5,17 @@ #pragma once #include "atomicops.h" +#include +#include +#include // For malloc/free/abort & size_t +#include +#include #include #include -#include -#include -#include -#include -#include // For malloc/free/abort & size_t #if __cplusplus > 199711L || _MSC_VER >= 1700 // C++11 or VS2012 #include #endif - // A lock-free queue for a single-consumer, single-producer architecture. // The queue is also wait-free in the common path (except if more memory // needs to be allocated, in which case malloc is called). @@ -42,773 +41,750 @@ #ifdef AE_VCPP #pragma warning(push) -#pragma warning(disable: 4324) // structure was padded due to __declspec(align()) -#pragma warning(disable: 4820) // padding was added -#pragma warning(disable: 4127) // conditional expression is constant +#pragma warning(disable : 4324) // structure was padded due to __declspec(align()) +#pragma warning(disable : 4820) // padding was added +#pragma warning(disable : 4127) // conditional expression is constant #endif namespace moodycamel { -template -class ReaderWriterQueue -{ - // Design: Based on a queue-of-queues. The low-level queues are just - // circular buffers with front and tail indices indicating where the - // next element to dequeue is and where the next element can be enqueued, - // respectively. Each low-level queue is called a "block". Each block - // wastes exactly one element's worth of space to keep the design simple - // (if front == tail then the queue is empty, and can't be full). - // The high-level queue is a circular linked list of blocks; again there - // is a front and tail, but this time they are pointers to the blocks. - // The front block is where the next element to be dequeued is, provided - // the block is not empty. The back block is where elements are to be - // enqueued, provided the block is not full. - // The producer thread owns all the tail indices/pointers. The consumer - // thread owns all the front indices/pointers. Both threads read each - // other's variables, but only the owning thread updates them. E.g. After - // the consumer reads the producer's tail, the tail may change before the - // consumer is done dequeuing an object, but the consumer knows the tail - // will never go backwards, only forwards. - // If there is no room to enqueue an object, an additional block (of - // equal size to the last block) is added. Blocks are never removed. +template +class ReaderWriterQueue { + // Design: Based on a queue-of-queues. The low-level queues are just + // circular buffers with front and tail indices indicating where the + // next element to dequeue is and where the next element can be enqueued, + // respectively. Each low-level queue is called a "block". Each block + // wastes exactly one element's worth of space to keep the design simple + // (if front == tail then the queue is empty, and can't be full). + // The high-level queue is a circular linked list of blocks; again there + // is a front and tail, but this time they are pointers to the blocks. + // The front block is where the next element to be dequeued is, provided + // the block is not empty. The back block is where elements are to be + // enqueued, provided the block is not full. + // The producer thread owns all the tail indices/pointers. The consumer + // thread owns all the front indices/pointers. Both threads read each + // other's variables, but only the owning thread updates them. E.g. After + // the consumer reads the producer's tail, the tail may change before the + // consumer is done dequeuing an object, but the consumer knows the tail + // will never go backwards, only forwards. + // If there is no room to enqueue an object, an additional block (of + // equal size to the last block) is added. Blocks are never removed. public: - // Constructs a queue that can hold maxSize elements without further - // allocations. If more than MAX_BLOCK_SIZE elements are requested, - // then several blocks of MAX_BLOCK_SIZE each are reserved (including - // at least one extra buffer block). - explicit ReaderWriterQueue(size_t maxSize = 15) + // Constructs a queue that can hold maxSize elements without further + // allocations. If more than MAX_BLOCK_SIZE elements are requested, + // then several blocks of MAX_BLOCK_SIZE each are reserved (including + // at least one extra buffer block). + explicit ReaderWriterQueue(size_t maxSize = 15) #ifndef NDEBUG - : enqueuing(false) - ,dequeuing(false) + : enqueuing(false) + , dequeuing(false) #endif - { - assert(maxSize > 0); - assert(MAX_BLOCK_SIZE == ceilToPow2(MAX_BLOCK_SIZE) && "MAX_BLOCK_SIZE must be a power of 2"); - assert(MAX_BLOCK_SIZE >= 2 && "MAX_BLOCK_SIZE must be at least 2"); - - Block* firstBlock = nullptr; - - largestBlockSize = ceilToPow2(maxSize + 1); // We need a spare slot to fit maxSize elements in the block - if (largestBlockSize > MAX_BLOCK_SIZE * 2) { - // We need a spare block in case the producer is writing to a different block the consumer is reading from, and - // wants to enqueue the maximum number of elements. We also need a spare element in each block to avoid the ambiguity - // between front == tail meaning "empty" and "full". - // So the effective number of slots that are guaranteed to be usable at any time is the block size - 1 times the - // number of blocks - 1. Solving for maxSize and applying a ceiling to the division gives us (after simplifying): - size_t initialBlockCount = (maxSize + MAX_BLOCK_SIZE * 2 - 3) / (MAX_BLOCK_SIZE - 1); - largestBlockSize = MAX_BLOCK_SIZE; - Block* lastBlock = nullptr; - for (size_t i = 0; i != initialBlockCount; ++i) { - auto block = make_block(largestBlockSize); - if (block == nullptr) { + { + assert(maxSize > 0); + assert(MAX_BLOCK_SIZE == ceilToPow2(MAX_BLOCK_SIZE) && "MAX_BLOCK_SIZE must be a power of 2"); + assert(MAX_BLOCK_SIZE >= 2 && "MAX_BLOCK_SIZE must be at least 2"); + + Block* firstBlock = nullptr; + + largestBlockSize = ceilToPow2(maxSize + 1); // We need a spare slot to fit maxSize elements in the block + if (largestBlockSize > MAX_BLOCK_SIZE * 2) { + // We need a spare block in case the producer is writing to a different block the consumer is reading from, and + // wants to enqueue the maximum number of elements. We also need a spare element in each block to avoid the ambiguity + // between front == tail meaning "empty" and "full". + // So the effective number of slots that are guaranteed to be usable at any time is the block size - 1 times the + // number of blocks - 1. Solving for maxSize and applying a ceiling to the division gives us (after simplifying): + size_t initialBlockCount = (maxSize + MAX_BLOCK_SIZE * 2 - 3) / (MAX_BLOCK_SIZE - 1); + largestBlockSize = MAX_BLOCK_SIZE; + Block* lastBlock = nullptr; + for (size_t i = 0; i != initialBlockCount; ++i) { + auto block = make_block(largestBlockSize); + if (block == nullptr) { #ifdef MOODYCAMEL_EXCEPTIONS_ENABLED - throw std::bad_alloc(); + throw std::bad_alloc(); #else - abort(); + abort(); #endif - } - if (firstBlock == nullptr) { - firstBlock = block; - } - else { - lastBlock->next = block; - } - lastBlock = block; - block->next = firstBlock; - } - } - else { - firstBlock = make_block(largestBlockSize); - if (firstBlock == nullptr) { + } + if (firstBlock == nullptr) { + firstBlock = block; + } else { + lastBlock->next = block; + } + lastBlock = block; + block->next = firstBlock; + } + } else { + firstBlock = make_block(largestBlockSize); + if (firstBlock == nullptr) { #ifdef MOODYCAMEL_EXCEPTIONS_ENABLED - throw std::bad_alloc(); + throw std::bad_alloc(); #else - abort(); + abort(); #endif - } - firstBlock->next = firstBlock; - } - frontBlock = firstBlock; - tailBlock = firstBlock; - - // Make sure the reader/writer threads will have the initialized memory setup above: - fence(memory_order_sync); - } + } + firstBlock->next = firstBlock; + } + frontBlock = firstBlock; + tailBlock = firstBlock; - // Note: The queue should not be accessed concurrently while it's - // being deleted. It's up to the user to synchronize this. - ~ReaderWriterQueue() - { - // Make sure we get the latest version of all variables from other CPUs: - fence(memory_order_sync); + // Make sure the reader/writer threads will have the initialized memory setup above: + fence(memory_order_sync); + } - // Destroy any remaining objects in queue and free memory - Block* frontBlock_ = frontBlock; - Block* block = frontBlock_; - do { - Block* nextBlock = block->next; - size_t blockFront = block->front; - size_t blockTail = block->tail; + // Note: The queue should not be accessed concurrently while it's + // being deleted. It's up to the user to synchronize this. + ~ReaderWriterQueue() + { + // Make sure we get the latest version of all variables from other CPUs: + fence(memory_order_sync); - for (size_t i = blockFront; i != blockTail; i = (i + 1) & block->sizeMask) { - auto element = reinterpret_cast(block->data + i * sizeof(T)); - element->~T(); - (void)element; - } - - auto rawBlock = block->rawThis; - block->~Block(); - std::free(rawBlock); - block = nextBlock; - } while (block != frontBlock_); - } + // Destroy any remaining objects in queue and free memory + Block* frontBlock_ = frontBlock; + Block* block = frontBlock_; + do { + Block* nextBlock = block->next; + size_t blockFront = block->front; + size_t blockTail = block->tail; + for (size_t i = blockFront; i != blockTail; i = (i + 1) & block->sizeMask) { + auto element = reinterpret_cast(block->data + i * sizeof(T)); + element->~T(); + (void)element; + } - // Enqueues a copy of element if there is room in the queue. - // Returns true if the element was enqueued, false otherwise. - // Does not allocate memory. - AE_FORCEINLINE bool try_enqueue(T const& element) - { - return inner_enqueue(element); - } + auto rawBlock = block->rawThis; + block->~Block(); + std::free(rawBlock); + block = nextBlock; + } while (block != frontBlock_); + } - // Enqueues a moved copy of element if there is room in the queue. - // Returns true if the element was enqueued, false otherwise. - // Does not allocate memory. - AE_FORCEINLINE bool try_enqueue(T&& element) - { - return inner_enqueue(std::forward(element)); - } + // Enqueues a copy of element if there is room in the queue. + // Returns true if the element was enqueued, false otherwise. + // Does not allocate memory. + AE_FORCEINLINE bool try_enqueue(T const& element) + { + return inner_enqueue(element); + } + // Enqueues a moved copy of element if there is room in the queue. + // Returns true if the element was enqueued, false otherwise. + // Does not allocate memory. + AE_FORCEINLINE bool try_enqueue(T&& element) + { + return inner_enqueue(std::forward(element)); + } - // Enqueues a copy of element on the queue. - // Allocates an additional block of memory if needed. - // Only fails (returns false) if memory allocation fails. - AE_FORCEINLINE bool enqueue(T const& element) - { - return inner_enqueue(element); - } + // Enqueues a copy of element on the queue. + // Allocates an additional block of memory if needed. + // Only fails (returns false) if memory allocation fails. + AE_FORCEINLINE bool enqueue(T const& element) + { + return inner_enqueue(element); + } - // Enqueues a moved copy of element on the queue. - // Allocates an additional block of memory if needed. - // Only fails (returns false) if memory allocation fails. - AE_FORCEINLINE bool enqueue(T&& element) - { - return inner_enqueue(std::forward(element)); - } + // Enqueues a moved copy of element on the queue. + // Allocates an additional block of memory if needed. + // Only fails (returns false) if memory allocation fails. + AE_FORCEINLINE bool enqueue(T&& element) + { + return inner_enqueue(std::forward(element)); + } - - // Attempts to dequeue an element; if the queue is empty, - // returns false instead. If the queue has at least one element, - // moves front to result using operator=, then returns true. - template - bool try_dequeue(U& result) - { + // Attempts to dequeue an element; if the queue is empty, + // returns false instead. If the queue has at least one element, + // moves front to result using operator=, then returns true. + template + bool try_dequeue(U& result) + { #ifndef NDEBUG - ReentrantGuard guard(this->dequeuing); + ReentrantGuard guard(this->dequeuing); #endif - // High-level pseudocode: - // Remember where the tail block is - // If the front block has an element in it, dequeue it - // Else - // If front block was the tail block when we entered the function, return false - // Else advance to next block and dequeue the item there + // High-level pseudocode: + // Remember where the tail block is + // If the front block has an element in it, dequeue it + // Else + // If front block was the tail block when we entered the function, return false + // Else advance to next block and dequeue the item there - // Note that we have to use the value of the tail block from before we check if the front - // block is full or not, in case the front block is empty and then, before we check if the - // tail block is at the front block or not, the producer fills up the front block *and - // moves on*, which would make us skip a filled block. Seems unlikely, but was consistently - // reproducible in practice. - // In order to avoid overhead in the common case, though, we do a double-checked pattern - // where we have the fast path if the front block is not empty, then read the tail block, - // then re-read the front block and check if it's not empty again, then check if the tail - // block has advanced. - - Block* frontBlock_ = frontBlock.load(); - size_t blockTail = frontBlock_->localTail; - size_t blockFront = frontBlock_->front.load(); - - if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) { - fence(memory_order_acquire); - - non_empty_front_block: - // Front block not empty, dequeue from here - auto element = reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); - result = std::move(*element); - element->~T(); + // Note that we have to use the value of the tail block from before we check if the front + // block is full or not, in case the front block is empty and then, before we check if the + // tail block is at the front block or not, the producer fills up the front block *and + // moves on*, which would make us skip a filled block. Seems unlikely, but was consistently + // reproducible in practice. + // In order to avoid overhead in the common case, though, we do a double-checked pattern + // where we have the fast path if the front block is not empty, then read the tail block, + // then re-read the front block and check if it's not empty again, then check if the tail + // block has advanced. - blockFront = (blockFront + 1) & frontBlock_->sizeMask; + Block* frontBlock_ = frontBlock.load(); + size_t blockTail = frontBlock_->localTail; + size_t blockFront = frontBlock_->front.load(); - fence(memory_order_release); - frontBlock_->front = blockFront; - } - else if (frontBlock_ != tailBlock.load()) { - fence(memory_order_acquire); + if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) { + fence(memory_order_acquire); - frontBlock_ = frontBlock.load(); - blockTail = frontBlock_->localTail = frontBlock_->tail.load(); - blockFront = frontBlock_->front.load(); - fence(memory_order_acquire); - - if (blockFront != blockTail) { - // Oh look, the front block isn't empty after all - goto non_empty_front_block; - } - - // Front block is empty but there's another block ahead, advance to it - Block* nextBlock = frontBlock_->next; - // Don't need an acquire fence here since next can only ever be set on the tailBlock, - // and we're not the tailBlock, and we did an acquire earlier after reading tailBlock which - // ensures next is up-to-date on this CPU in case we recently were at tailBlock. + non_empty_front_block: + // Front block not empty, dequeue from here + auto element = reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); + result = std::move(*element); + element->~T(); - size_t nextBlockFront = nextBlock->front.load(); - size_t nextBlockTail = nextBlock->localTail = nextBlock->tail.load(); - fence(memory_order_acquire); + blockFront = (blockFront + 1) & frontBlock_->sizeMask; - // Since the tailBlock is only ever advanced after being written to, - // we know there's for sure an element to dequeue on it - assert(nextBlockFront != nextBlockTail); - AE_UNUSED(nextBlockTail); + fence(memory_order_release); + frontBlock_->front = blockFront; + } else if (frontBlock_ != tailBlock.load()) { + fence(memory_order_acquire); - // We're done with this block, let the producer use it if it needs - fence(memory_order_release); // Expose possibly pending changes to frontBlock->front from last dequeue - frontBlock = frontBlock_ = nextBlock; + frontBlock_ = frontBlock.load(); + blockTail = frontBlock_->localTail = frontBlock_->tail.load(); + blockFront = frontBlock_->front.load(); + fence(memory_order_acquire); - compiler_fence(memory_order_release); // Not strictly needed + if (blockFront != blockTail) { + // Oh look, the front block isn't empty after all + goto non_empty_front_block; + } - auto element = reinterpret_cast(frontBlock_->data + nextBlockFront * sizeof(T)); - - result = std::move(*element); - element->~T(); + // Front block is empty but there's another block ahead, advance to it + Block* nextBlock = frontBlock_->next; + // Don't need an acquire fence here since next can only ever be set on the tailBlock, + // and we're not the tailBlock, and we did an acquire earlier after reading tailBlock which + // ensures next is up-to-date on this CPU in case we recently were at tailBlock. - nextBlockFront = (nextBlockFront + 1) & frontBlock_->sizeMask; - - fence(memory_order_release); - frontBlock_->front = nextBlockFront; - } - else { - // No elements in current block and no other block to advance to - return false; - } + size_t nextBlockFront = nextBlock->front.load(); + size_t nextBlockTail = nextBlock->localTail = nextBlock->tail.load(); + fence(memory_order_acquire); - return true; - } + // Since the tailBlock is only ever advanced after being written to, + // we know there's for sure an element to dequeue on it + assert(nextBlockFront != nextBlockTail); + AE_UNUSED(nextBlockTail); + // We're done with this block, let the producer use it if it needs + fence(memory_order_release); // Expose possibly pending changes to frontBlock->front from last dequeue + frontBlock = frontBlock_ = nextBlock; - // Returns a pointer to the front element in the queue (the one that - // would be removed next by a call to `try_dequeue` or `pop`). If the - // queue appears empty at the time the method is called, nullptr is - // returned instead. - // Must be called only from the consumer thread. - T* peek() - { + compiler_fence(memory_order_release); // Not strictly needed + + auto element = reinterpret_cast(frontBlock_->data + nextBlockFront * sizeof(T)); + + result = std::move(*element); + element->~T(); + + nextBlockFront = (nextBlockFront + 1) & frontBlock_->sizeMask; + + fence(memory_order_release); + frontBlock_->front = nextBlockFront; + } else { + // No elements in current block and no other block to advance to + return false; + } + + return true; + } + + // Returns a pointer to the front element in the queue (the one that + // would be removed next by a call to `try_dequeue` or `pop`). If the + // queue appears empty at the time the method is called, nullptr is + // returned instead. + // Must be called only from the consumer thread. + T* peek() + { #ifndef NDEBUG - ReentrantGuard guard(this->dequeuing); + ReentrantGuard guard(this->dequeuing); #endif - // See try_dequeue() for reasoning + // See try_dequeue() for reasoning - Block* frontBlock_ = frontBlock.load(); - size_t blockTail = frontBlock_->localTail; - size_t blockFront = frontBlock_->front.load(); - - if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) { - fence(memory_order_acquire); - non_empty_front_block: - return reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); - } - else if (frontBlock_ != tailBlock.load()) { - fence(memory_order_acquire); - frontBlock_ = frontBlock.load(); - blockTail = frontBlock_->localTail = frontBlock_->tail.load(); - blockFront = frontBlock_->front.load(); - fence(memory_order_acquire); - - if (blockFront != blockTail) { - goto non_empty_front_block; - } - - Block* nextBlock = frontBlock_->next; - - size_t nextBlockFront = nextBlock->front.load(); - fence(memory_order_acquire); + Block* frontBlock_ = frontBlock.load(); + size_t blockTail = frontBlock_->localTail; + size_t blockFront = frontBlock_->front.load(); - assert(nextBlockFront != nextBlock->tail.load()); - return reinterpret_cast(nextBlock->data + nextBlockFront * sizeof(T)); - } - - return nullptr; - } - - // Removes the front element from the queue, if any, without returning it. - // Returns true on success, or false if the queue appeared empty at the time - // `pop` was called. - bool pop() - { + if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) { + fence(memory_order_acquire); + non_empty_front_block: + return reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); + } else if (frontBlock_ != tailBlock.load()) { + fence(memory_order_acquire); + frontBlock_ = frontBlock.load(); + blockTail = frontBlock_->localTail = frontBlock_->tail.load(); + blockFront = frontBlock_->front.load(); + fence(memory_order_acquire); + + if (blockFront != blockTail) { + goto non_empty_front_block; + } + + Block* nextBlock = frontBlock_->next; + + size_t nextBlockFront = nextBlock->front.load(); + fence(memory_order_acquire); + + assert(nextBlockFront != nextBlock->tail.load()); + return reinterpret_cast(nextBlock->data + nextBlockFront * sizeof(T)); + } + + return nullptr; + } + + // Removes the front element from the queue, if any, without returning it. + // Returns true on success, or false if the queue appeared empty at the time + // `pop` was called. + bool pop() + { #ifndef NDEBUG - ReentrantGuard guard(this->dequeuing); + ReentrantGuard guard(this->dequeuing); #endif - // See try_dequeue() for reasoning - - Block* frontBlock_ = frontBlock.load(); - size_t blockTail = frontBlock_->localTail; - size_t blockFront = frontBlock_->front.load(); - - if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) { - fence(memory_order_acquire); - - non_empty_front_block: - auto element = reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); - element->~T(); + // See try_dequeue() for reasoning - blockFront = (blockFront + 1) & frontBlock_->sizeMask; + Block* frontBlock_ = frontBlock.load(); + size_t blockTail = frontBlock_->localTail; + size_t blockFront = frontBlock_->front.load(); - fence(memory_order_release); - frontBlock_->front = blockFront; - } - else if (frontBlock_ != tailBlock.load()) { - fence(memory_order_acquire); - frontBlock_ = frontBlock.load(); - blockTail = frontBlock_->localTail = frontBlock_->tail.load(); - blockFront = frontBlock_->front.load(); - fence(memory_order_acquire); - - if (blockFront != blockTail) { - goto non_empty_front_block; - } - - // Front block is empty but there's another block ahead, advance to it - Block* nextBlock = frontBlock_->next; - - size_t nextBlockFront = nextBlock->front.load(); - size_t nextBlockTail = nextBlock->localTail = nextBlock->tail.load(); - fence(memory_order_acquire); + if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) { + fence(memory_order_acquire); - assert(nextBlockFront != nextBlockTail); - AE_UNUSED(nextBlockTail); + non_empty_front_block: + auto element = reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); + element->~T(); - fence(memory_order_release); - frontBlock = frontBlock_ = nextBlock; + blockFront = (blockFront + 1) & frontBlock_->sizeMask; - compiler_fence(memory_order_release); + fence(memory_order_release); + frontBlock_->front = blockFront; + } else if (frontBlock_ != tailBlock.load()) { + fence(memory_order_acquire); + frontBlock_ = frontBlock.load(); + blockTail = frontBlock_->localTail = frontBlock_->tail.load(); + blockFront = frontBlock_->front.load(); + fence(memory_order_acquire); - auto element = reinterpret_cast(frontBlock_->data + nextBlockFront * sizeof(T)); - element->~T(); + if (blockFront != blockTail) { + goto non_empty_front_block; + } - nextBlockFront = (nextBlockFront + 1) & frontBlock_->sizeMask; - - fence(memory_order_release); - frontBlock_->front = nextBlockFront; - } - else { - // No elements in current block and no other block to advance to - return false; - } + // Front block is empty but there's another block ahead, advance to it + Block* nextBlock = frontBlock_->next; - return true; - } - - // Returns the approximate number of items currently in the queue. - // Safe to call from both the producer and consumer threads. - inline size_t size_approx() const - { - size_t result = 0; - Block* frontBlock_ = frontBlock.load(); - Block* block = frontBlock_; - do { - fence(memory_order_acquire); - size_t blockFront = block->front.load(); - size_t blockTail = block->tail.load(); - result += (blockTail - blockFront) & block->sizeMask; - block = block->next.load(); - } while (block != frontBlock_); - return result; - } + size_t nextBlockFront = nextBlock->front.load(); + size_t nextBlockTail = nextBlock->localTail = nextBlock->tail.load(); + fence(memory_order_acquire); + assert(nextBlockFront != nextBlockTail); + AE_UNUSED(nextBlockTail); + + fence(memory_order_release); + frontBlock = frontBlock_ = nextBlock; + + compiler_fence(memory_order_release); + + auto element = reinterpret_cast(frontBlock_->data + nextBlockFront * sizeof(T)); + element->~T(); + + nextBlockFront = (nextBlockFront + 1) & frontBlock_->sizeMask; + + fence(memory_order_release); + frontBlock_->front = nextBlockFront; + } else { + // No elements in current block and no other block to advance to + return false; + } + + return true; + } + + // Returns the approximate number of items currently in the queue. + // Safe to call from both the producer and consumer threads. + inline size_t size_approx() const + { + size_t result = 0; + Block* frontBlock_ = frontBlock.load(); + Block* block = frontBlock_; + do { + fence(memory_order_acquire); + size_t blockFront = block->front.load(); + size_t blockTail = block->tail.load(); + result += (blockTail - blockFront) & block->sizeMask; + block = block->next.load(); + } while (block != frontBlock_); + return result; + } private: - enum AllocationMode { CanAlloc, CannotAlloc }; + enum AllocationMode { CanAlloc, + CannotAlloc }; - template - bool inner_enqueue(U&& element) - { + template + bool inner_enqueue(U&& element) + { #ifndef NDEBUG - ReentrantGuard guard(this->enqueuing); + ReentrantGuard guard(this->enqueuing); #endif - // High-level pseudocode (assuming we're allowed to alloc a new block): - // If room in tail block, add to tail - // Else check next block - // If next block is not the head block, enqueue on next block - // Else create a new block and enqueue there - // Advance tail to the block we just enqueued to + // High-level pseudocode (assuming we're allowed to alloc a new block): + // If room in tail block, add to tail + // Else check next block + // If next block is not the head block, enqueue on next block + // Else create a new block and enqueue there + // Advance tail to the block we just enqueued to - Block* tailBlock_ = tailBlock.load(); - size_t blockFront = tailBlock_->localFront; - size_t blockTail = tailBlock_->tail.load(); + Block* tailBlock_ = tailBlock.load(); + size_t blockFront = tailBlock_->localFront; + size_t blockTail = tailBlock_->tail.load(); - size_t nextBlockTail = (blockTail + 1) & tailBlock_->sizeMask; - if (nextBlockTail != blockFront || nextBlockTail != (tailBlock_->localFront = tailBlock_->front.load())) { - fence(memory_order_acquire); - // This block has room for at least one more element - char* location = tailBlock_->data + blockTail * sizeof(T); - new (location) T(std::forward(element)); + size_t nextBlockTail = (blockTail + 1) & tailBlock_->sizeMask; + if (nextBlockTail != blockFront || nextBlockTail != (tailBlock_->localFront = tailBlock_->front.load())) { + fence(memory_order_acquire); + // This block has room for at least one more element + char* location = tailBlock_->data + blockTail * sizeof(T); + new (location) T(std::forward(element)); - fence(memory_order_release); - tailBlock_->tail = nextBlockTail; - } - else { - fence(memory_order_acquire); - if (tailBlock_->next.load() != frontBlock) { - // Note that the reason we can't advance to the frontBlock and start adding new entries there - // is because if we did, then dequeue would stay in that block, eventually reading the new values, - // instead of advancing to the next full block (whose values were enqueued first and so should be - // consumed first). - - fence(memory_order_acquire); // Ensure we get latest writes if we got the latest frontBlock + fence(memory_order_release); + tailBlock_->tail = nextBlockTail; + } else { + fence(memory_order_acquire); + if (tailBlock_->next.load() != frontBlock) { + // Note that the reason we can't advance to the frontBlock and start adding new entries there + // is because if we did, then dequeue would stay in that block, eventually reading the new values, + // instead of advancing to the next full block (whose values were enqueued first and so should be + // consumed first). - // tailBlock is full, but there's a free block ahead, use it - Block* tailBlockNext = tailBlock_->next.load(); - size_t nextBlockFront = tailBlockNext->localFront = tailBlockNext->front.load(); - nextBlockTail = tailBlockNext->tail.load(); - fence(memory_order_acquire); + fence(memory_order_acquire); // Ensure we get latest writes if we got the latest frontBlock - // This block must be empty since it's not the head block and we - // go through the blocks in a circle - assert(nextBlockFront == nextBlockTail); - tailBlockNext->localFront = nextBlockFront; + // tailBlock is full, but there's a free block ahead, use it + Block* tailBlockNext = tailBlock_->next.load(); + size_t nextBlockFront = tailBlockNext->localFront = tailBlockNext->front.load(); + nextBlockTail = tailBlockNext->tail.load(); + fence(memory_order_acquire); - char* location = tailBlockNext->data + nextBlockTail * sizeof(T); - new (location) T(std::forward(element)); + // This block must be empty since it's not the head block and we + // go through the blocks in a circle + assert(nextBlockFront == nextBlockTail); + tailBlockNext->localFront = nextBlockFront; - tailBlockNext->tail = (nextBlockTail + 1) & tailBlockNext->sizeMask; + char* location = tailBlockNext->data + nextBlockTail * sizeof(T); + new (location) T(std::forward(element)); - fence(memory_order_release); - tailBlock = tailBlockNext; - } - else if (canAlloc == CanAlloc) { - // tailBlock is full and there's no free block ahead; create a new block - auto newBlockSize = largestBlockSize >= MAX_BLOCK_SIZE ? largestBlockSize : largestBlockSize * 2; - auto newBlock = make_block(newBlockSize); - if (newBlock == nullptr) { - // Could not allocate a block! - return false; - } - largestBlockSize = newBlockSize; + tailBlockNext->tail = (nextBlockTail + 1) & tailBlockNext->sizeMask; - new (newBlock->data) T(std::forward(element)); + fence(memory_order_release); + tailBlock = tailBlockNext; + } else if (canAlloc == CanAlloc) { + // tailBlock is full and there's no free block ahead; create a new block + auto newBlockSize = largestBlockSize >= MAX_BLOCK_SIZE ? largestBlockSize : largestBlockSize * 2; + auto newBlock = make_block(newBlockSize); + if (newBlock == nullptr) { + // Could not allocate a block! + return false; + } + largestBlockSize = newBlockSize; - assert(newBlock->front == 0); - newBlock->tail = newBlock->localTail = 1; + new (newBlock->data) T(std::forward(element)); - newBlock->next = tailBlock_->next.load(); - tailBlock_->next = newBlock; + assert(newBlock->front == 0); + newBlock->tail = newBlock->localTail = 1; - // Might be possible for the dequeue thread to see the new tailBlock->next - // *without* seeing the new tailBlock value, but this is OK since it can't - // advance to the next block until tailBlock is set anyway (because the only - // case where it could try to read the next is if it's already at the tailBlock, - // and it won't advance past tailBlock in any circumstance). - - fence(memory_order_release); - tailBlock = newBlock; - } - else if (canAlloc == CannotAlloc) { - // Would have had to allocate a new block to enqueue, but not allowed - return false; - } - else { - assert(false && "Should be unreachable code"); - return false; - } - } + newBlock->next = tailBlock_->next.load(); + tailBlock_->next = newBlock; - return true; - } + // Might be possible for the dequeue thread to see the new tailBlock->next + // *without* seeing the new tailBlock value, but this is OK since it can't + // advance to the next block until tailBlock is set anyway (because the only + // case where it could try to read the next is if it's already at the tailBlock, + // and it won't advance past tailBlock in any circumstance). + fence(memory_order_release); + tailBlock = newBlock; + } else if (canAlloc == CannotAlloc) { + // Would have had to allocate a new block to enqueue, but not allowed + return false; + } else { + assert(false && "Should be unreachable code"); + return false; + } + } - // Disable copying - ReaderWriterQueue(ReaderWriterQueue const&) { } + return true; + } - // Disable assignment - ReaderWriterQueue& operator=(ReaderWriterQueue const&) { } + // Disable copying + ReaderWriterQueue(ReaderWriterQueue const&) {} + // Disable assignment + ReaderWriterQueue& operator=(ReaderWriterQueue const&) {} + AE_FORCEINLINE static size_t ceilToPow2(size_t x) + { + // From http://graphics.stanford.edu/~seander/bithacks.html#RoundUpPowerOf2 + --x; + x |= x >> 1; + x |= x >> 2; + x |= x >> 4; + for (size_t i = 1; i < sizeof(size_t); i <<= 1) { + x |= x >> (i << 3); + } + ++x; + return x; + } + + template + static AE_FORCEINLINE char* align_for(char* ptr) + { + const std::size_t alignment = std::alignment_of::value; + return ptr + (alignment - (reinterpret_cast(ptr) % alignment)) % alignment; + } - AE_FORCEINLINE static size_t ceilToPow2(size_t x) - { - // From http://graphics.stanford.edu/~seander/bithacks.html#RoundUpPowerOf2 - --x; - x |= x >> 1; - x |= x >> 2; - x |= x >> 4; - for (size_t i = 1; i < sizeof(size_t); i <<= 1) { - x |= x >> (i << 3); - } - ++x; - return x; - } - - template - static AE_FORCEINLINE char* align_for(char* ptr) - { - const std::size_t alignment = std::alignment_of::value; - return ptr + (alignment - (reinterpret_cast(ptr) % alignment)) % alignment; - } private: #ifndef NDEBUG - struct ReentrantGuard - { - ReentrantGuard(bool& _inSection) - : inSection(_inSection) - { - assert(!inSection && "ReaderWriterQueue does not support enqueuing or dequeuing elements from other elements' ctors and dtors"); - inSection = true; - } + struct ReentrantGuard { + ReentrantGuard(bool& _inSection) + : inSection(_inSection) + { + assert(!inSection && "ReaderWriterQueue does not support enqueuing or dequeuing elements from other elements' ctors and dtors"); + inSection = true; + } - ~ReentrantGuard() { inSection = false; } + ~ReentrantGuard() { inSection = false; } - private: - ReentrantGuard& operator=(ReentrantGuard const&); + private: + ReentrantGuard& operator=(ReentrantGuard const&); - private: - bool& inSection; - }; + private: + bool& inSection; + }; #endif - struct Block - { - // Avoid false-sharing by putting highly contended variables on their own cache lines - weak_atomic front; // (Atomic) Elements are read from here - size_t localTail; // An uncontended shadow copy of tail, owned by the consumer - - char cachelineFiller0[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic) - sizeof(size_t)]; - weak_atomic tail; // (Atomic) Elements are enqueued here - size_t localFront; - - char cachelineFiller1[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic) - sizeof(size_t)]; // next isn't very contended, but we don't want it on the same cache line as tail (which is) - weak_atomic next; // (Atomic) - - char* data; // Contents (on heap) are aligned to T's alignment + struct Block { + // Avoid false-sharing by putting highly contended variables on their own cache lines + weak_atomic front; // (Atomic) Elements are read from here + size_t localTail; // An uncontended shadow copy of tail, owned by the consumer - const size_t sizeMask; + char cachelineFiller0[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic) - sizeof(size_t)]; + weak_atomic tail; // (Atomic) Elements are enqueued here + size_t localFront; + char cachelineFiller1[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic) - sizeof(size_t)]; // next isn't very contended, but we don't want it on the same cache line as tail (which is) + weak_atomic next; // (Atomic) - // size must be a power of two (and greater than 0) - Block(size_t const& _size, char* _rawThis, char* _data) - : front(0), localTail(0), tail(0), localFront(0), next(nullptr), data(_data), sizeMask(_size - 1), rawThis(_rawThis) - { - } + char* data; // Contents (on heap) are aligned to T's alignment - private: - // C4512 - Assignment operator could not be generated - Block& operator=(Block const&); + const size_t sizeMask; - public: - char* rawThis; - }; - - - static Block* make_block(size_t capacity) - { - // Allocate enough memory for the block itself, as well as all the elements it will contain - auto size = sizeof(Block) + std::alignment_of::value - 1; - size += sizeof(T) * capacity + std::alignment_of::value - 1; - auto newBlockRaw = static_cast(std::malloc(size)); - if (newBlockRaw == nullptr) { - return nullptr; - } - - auto newBlockAligned = align_for(newBlockRaw); - auto newBlockData = align_for(newBlockAligned + sizeof(Block)); - return new (newBlockAligned) Block(capacity, newBlockRaw, newBlockData); - } + // size must be a power of two (and greater than 0) + Block(size_t const& _size, char* _rawThis, char* _data) + : front(0) + , localTail(0) + , tail(0) + , localFront(0) + , next(nullptr) + , data(_data) + , sizeMask(_size - 1) + , rawThis(_rawThis) + { + } + + private: + // C4512 - Assignment operator could not be generated + Block& operator=(Block const&); + + public: + char* rawThis; + }; + + static Block* make_block(size_t capacity) + { + // Allocate enough memory for the block itself, as well as all the elements it will contain + auto size = sizeof(Block) + std::alignment_of::value - 1; + size += sizeof(T) * capacity + std::alignment_of::value - 1; + auto newBlockRaw = static_cast(std::malloc(size)); + if (newBlockRaw == nullptr) { + return nullptr; + } + + auto newBlockAligned = align_for(newBlockRaw); + auto newBlockData = align_for(newBlockAligned + sizeof(Block)); + return new (newBlockAligned) Block(capacity, newBlockRaw, newBlockData); + } private: - weak_atomic frontBlock; // (Atomic) Elements are enqueued to this block - - char cachelineFiller[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic)]; - weak_atomic tailBlock; // (Atomic) Elements are dequeued from this block + weak_atomic frontBlock; // (Atomic) Elements are enqueued to this block - size_t largestBlockSize; + char cachelineFiller[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic)]; + weak_atomic tailBlock; // (Atomic) Elements are dequeued from this block + + size_t largestBlockSize; #ifndef NDEBUG - bool enqueuing; - bool dequeuing; + bool enqueuing; + bool dequeuing; #endif }; // Like ReaderWriterQueue, but also providees blocking operations -template -class BlockingReaderWriterQueue -{ +template +class BlockingReaderWriterQueue { private: - typedef ::moodycamel::ReaderWriterQueue ReaderWriterQueue; - + typedef ::moodycamel::ReaderWriterQueue ReaderWriterQueue; + public: - explicit BlockingReaderWriterQueue(size_t maxSize = 15) - : inner(maxSize) - { } + explicit BlockingReaderWriterQueue(size_t maxSize = 15) + : inner(maxSize) + { + } - - // Enqueues a copy of element if there is room in the queue. - // Returns true if the element was enqueued, false otherwise. - // Does not allocate memory. - AE_FORCEINLINE bool try_enqueue(T const& element) - { - if (inner.try_enqueue(element)) { - sema.signal(); - return true; - } - return false; - } + // Enqueues a copy of element if there is room in the queue. + // Returns true if the element was enqueued, false otherwise. + // Does not allocate memory. + AE_FORCEINLINE bool try_enqueue(T const& element) + { + if (inner.try_enqueue(element)) { + sema.signal(); + return true; + } + return false; + } - // Enqueues a moved copy of element if there is room in the queue. - // Returns true if the element was enqueued, false otherwise. - // Does not allocate memory. - AE_FORCEINLINE bool try_enqueue(T&& element) - { - if (inner.try_enqueue(std::forward(element))) { - sema.signal(); - return true; - } - return false; - } + // Enqueues a moved copy of element if there is room in the queue. + // Returns true if the element was enqueued, false otherwise. + // Does not allocate memory. + AE_FORCEINLINE bool try_enqueue(T&& element) + { + if (inner.try_enqueue(std::forward(element))) { + sema.signal(); + return true; + } + return false; + } + // Enqueues a copy of element on the queue. + // Allocates an additional block of memory if needed. + // Only fails (returns false) if memory allocation fails. + AE_FORCEINLINE bool enqueue(T const& element) + { + if (inner.enqueue(element)) { + sema.signal(); + return true; + } + return false; + } - // Enqueues a copy of element on the queue. - // Allocates an additional block of memory if needed. - // Only fails (returns false) if memory allocation fails. - AE_FORCEINLINE bool enqueue(T const& element) - { - if (inner.enqueue(element)) { - sema.signal(); - return true; - } - return false; - } + // Enqueues a moved copy of element on the queue. + // Allocates an additional block of memory if needed. + // Only fails (returns false) if memory allocation fails. + AE_FORCEINLINE bool enqueue(T&& element) + { + if (inner.enqueue(std::forward(element))) { + sema.signal(); + return true; + } + return false; + } - // Enqueues a moved copy of element on the queue. - // Allocates an additional block of memory if needed. - // Only fails (returns false) if memory allocation fails. - AE_FORCEINLINE bool enqueue(T&& element) - { - if (inner.enqueue(std::forward(element))) { - sema.signal(); - return true; - } - return false; - } + // Attempts to dequeue an element; if the queue is empty, + // returns false instead. If the queue has at least one element, + // moves front to result using operator=, then returns true. + template + bool try_dequeue(U& result) + { + if (sema.tryWait()) { + bool success = inner.try_dequeue(result); + assert(success); + AE_UNUSED(success); + return true; + } + return false; + } + // Attempts to dequeue an element; if the queue is empty, + // waits until an element is available, then dequeues it. + template + void wait_dequeue(U& result) + { + sema.wait(); + bool success = inner.try_dequeue(result); + AE_UNUSED(result); + assert(success); + AE_UNUSED(success); + } - // Attempts to dequeue an element; if the queue is empty, - // returns false instead. If the queue has at least one element, - // moves front to result using operator=, then returns true. - template - bool try_dequeue(U& result) - { - if (sema.tryWait()) { - bool success = inner.try_dequeue(result); - assert(success); - AE_UNUSED(success); - return true; - } - return false; - } - - - // Attempts to dequeue an element; if the queue is empty, - // waits until an element is available, then dequeues it. - template - void wait_dequeue(U& result) - { - sema.wait(); - bool success = inner.try_dequeue(result); - AE_UNUSED(result); - assert(success); - AE_UNUSED(success); - } - - - // Attempts to dequeue an element; if the queue is empty, - // waits until an element is available up to the specified timeout, - // then dequeues it and returns true, or returns false if the timeout - // expires before an element can be dequeued. - // Using a negative timeout indicates an indefinite timeout, - // and is thus functionally equivalent to calling wait_dequeue. - template - bool wait_dequeue_timed(U& result, std::int64_t timeout_usecs) - { - if (!sema.wait(timeout_usecs)) { - return false; - } - bool success = inner.try_dequeue(result); - AE_UNUSED(result); - assert(success); - AE_UNUSED(success); - return true; - } - + // Attempts to dequeue an element; if the queue is empty, + // waits until an element is available up to the specified timeout, + // then dequeues it and returns true, or returns false if the timeout + // expires before an element can be dequeued. + // Using a negative timeout indicates an indefinite timeout, + // and is thus functionally equivalent to calling wait_dequeue. + template + bool wait_dequeue_timed(U& result, std::int64_t timeout_usecs) + { + if (!sema.wait(timeout_usecs)) { + return false; + } + bool success = inner.try_dequeue(result); + AE_UNUSED(result); + assert(success); + AE_UNUSED(success); + return true; + } #if __cplusplus > 199711L || _MSC_VER >= 1700 - // Attempts to dequeue an element; if the queue is empty, - // waits until an element is available up to the specified timeout, - // then dequeues it and returns true, or returns false if the timeout - // expires before an element can be dequeued. - // Using a negative timeout indicates an indefinite timeout, - // and is thus functionally equivalent to calling wait_dequeue. - template - inline bool wait_dequeue_timed(U& result, std::chrono::duration const& timeout) - { + // Attempts to dequeue an element; if the queue is empty, + // waits until an element is available up to the specified timeout, + // then dequeues it and returns true, or returns false if the timeout + // expires before an element can be dequeued. + // Using a negative timeout indicates an indefinite timeout, + // and is thus functionally equivalent to calling wait_dequeue. + template + inline bool wait_dequeue_timed(U& result, std::chrono::duration const& timeout) + { return wait_dequeue_timed(result, std::chrono::duration_cast(timeout).count()); - } + } #endif + // Returns a pointer to the front element in the queue (the one that + // would be removed next by a call to `try_dequeue` or `pop`). If the + // queue appears empty at the time the method is called, nullptr is + // returned instead. + // Must be called only from the consumer thread. + AE_FORCEINLINE T* peek() + { + return inner.peek(); + } - // Returns a pointer to the front element in the queue (the one that - // would be removed next by a call to `try_dequeue` or `pop`). If the - // queue appears empty at the time the method is called, nullptr is - // returned instead. - // Must be called only from the consumer thread. - AE_FORCEINLINE T* peek() - { - return inner.peek(); - } - - // Removes the front element from the queue, if any, without returning it. - // Returns true on success, or false if the queue appeared empty at the time - // `pop` was called. - AE_FORCEINLINE bool pop() - { - if (sema.tryWait()) { - bool result = inner.pop(); - assert(result); - AE_UNUSED(result); - return true; - } - return false; - } - - // Returns the approximate number of items currently in the queue. - // Safe to call from both the producer and consumer threads. - AE_FORCEINLINE size_t size_approx() const - { - return sema.availableApprox(); - } + // Removes the front element from the queue, if any, without returning it. + // Returns true on success, or false if the queue appeared empty at the time + // `pop` was called. + AE_FORCEINLINE bool pop() + { + if (sema.tryWait()) { + bool result = inner.pop(); + assert(result); + AE_UNUSED(result); + return true; + } + return false; + } + // Returns the approximate number of items currently in the queue. + // Safe to call from both the producer and consumer threads. + AE_FORCEINLINE size_t size_approx() const + { + return sema.availableApprox(); + } private: - // Disable copying & assignment - BlockingReaderWriterQueue(ReaderWriterQueue const&) { } - BlockingReaderWriterQueue& operator=(ReaderWriterQueue const&) { } - + // Disable copying & assignment + BlockingReaderWriterQueue(ReaderWriterQueue const&) {} + BlockingReaderWriterQueue& operator=(ReaderWriterQueue const&) {} + private: - ReaderWriterQueue inner; - spsc_sema::LightweightSemaphore sema; + ReaderWriterQueue inner; + spsc_sema::LightweightSemaphore sema; }; -} // end namespace moodycamel +} // end namespace moodycamel #ifdef AE_VCPP #pragma warning(pop) diff --git a/include/ur_modern_driver/robot_state.h b/include/ur_modern_driver/robot_state.h index 159974b..a35da44 100644 --- a/include/ur_modern_driver/robot_state.h +++ b/include/ur_modern_driver/robot_state.h @@ -19,200 +19,202 @@ #ifndef ROBOT_STATE_H_ #define ROBOT_STATE_H_ +#include #include -#include +#include +#include #include #include -#include -#include -#include +#include namespace message_types { enum message_type { - ROBOT_STATE = 16, 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 { enum package_type { - ROBOT_MODE_DATA = 0, - JOINT_DATA = 1, - TOOL_DATA = 2, - MASTERBOARD_DATA = 3, - CARTESIAN_INFO = 4, - KINEMATICS_INFO = 5, - CONFIGURATION_DATA = 6, - FORCE_MODE_DATA = 7, - ADDITIONAL_INFO = 8, - CALIBRATION_DATA = 9 + ROBOT_MODE_DATA = 0, + JOINT_DATA = 1, + TOOL_DATA = 2, + MASTERBOARD_DATA = 3, + CARTESIAN_INFO = 4, + KINEMATICS_INFO = 5, + CONFIGURATION_DATA = 6, + FORCE_MODE_DATA = 7, + ADDITIONAL_INFO = 8, + CALIBRATION_DATA = 9 }; } typedef package_types::package_type packageType; namespace robot_message_types { enum robot_message_type { - ROBOT_MESSAGE_TEXT = 0, - ROBOT_MESSAGE_PROGRAM_LABEL = 1, - PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2, - ROBOT_MESSAGE_VERSION = 3, - ROBOT_MESSAGE_SAFETY_MODE = 5, - ROBOT_MESSAGE_ERROR_CODE = 6, - ROBOT_MESSAGE_KEY = 7, - ROBOT_MESSAGE_REQUEST_VALUE = 9, - ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10 + ROBOT_MESSAGE_TEXT = 0, + ROBOT_MESSAGE_PROGRAM_LABEL = 1, + PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2, + ROBOT_MESSAGE_VERSION = 3, + ROBOT_MESSAGE_SAFETY_MODE = 5, + ROBOT_MESSAGE_ERROR_CODE = 6, + ROBOT_MESSAGE_KEY = 7, + ROBOT_MESSAGE_REQUEST_VALUE = 9, + ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10 }; } typedef robot_message_types::robot_message_type robotMessageType; namespace robot_state_type_v18 { enum robot_state_type { - ROBOT_RUNNING_MODE = 0, - ROBOT_FREEDRIVE_MODE = 1, - ROBOT_READY_MODE = 2, - ROBOT_INITIALIZING_MODE = 3, - ROBOT_SECURITY_STOPPED_MODE = 4, - ROBOT_EMERGENCY_STOPPED_MODE = 5, - ROBOT_FATAL_ERROR_MODE = 6, - ROBOT_NO_POWER_MODE = 7, - ROBOT_NOT_CONNECTED_MODE = 8, - ROBOT_SHUTDOWN_MODE = 9, - ROBOT_SAFEGUARD_STOP_MODE = 10 + ROBOT_RUNNING_MODE = 0, + ROBOT_FREEDRIVE_MODE = 1, + ROBOT_READY_MODE = 2, + ROBOT_INITIALIZING_MODE = 3, + ROBOT_SECURITY_STOPPED_MODE = 4, + ROBOT_EMERGENCY_STOPPED_MODE = 5, + ROBOT_FATAL_ERROR_MODE = 6, + ROBOT_NO_POWER_MODE = 7, + ROBOT_NOT_CONNECTED_MODE = 8, + ROBOT_SHUTDOWN_MODE = 9, + ROBOT_SAFEGUARD_STOP_MODE = 10 }; } typedef robot_state_type_v18::robot_state_type robotStateTypeV18; namespace robot_state_type_v30 { enum robot_state_type { - ROBOT_MODE_DISCONNECTED = 0, - ROBOT_MODE_CONFIRM_SAFETY = 1, - ROBOT_MODE_BOOTING = 2, - ROBOT_MODE_POWER_OFF = 3, - ROBOT_MODE_POWER_ON = 4, - ROBOT_MODE_IDLE = 5, - ROBOT_MODE_BACKDRIVE = 6, - ROBOT_MODE_RUNNING = 7, - ROBOT_MODE_UPDATING_FIRMWARE = 8 + ROBOT_MODE_DISCONNECTED = 0, + ROBOT_MODE_CONFIRM_SAFETY = 1, + ROBOT_MODE_BOOTING = 2, + ROBOT_MODE_POWER_OFF = 3, + ROBOT_MODE_POWER_ON = 4, + ROBOT_MODE_IDLE = 5, + ROBOT_MODE_BACKDRIVE = 6, + ROBOT_MODE_RUNNING = 7, + ROBOT_MODE_UPDATING_FIRMWARE = 8 }; } typedef robot_state_type_v30::robot_state_type robotStateTypeV30; struct version_message { - uint64_t timestamp; - int8_t source; - int8_t robot_message_type; - int8_t project_name_size; - char project_name[15]; - uint8_t major_version; - uint8_t minor_version; - int svn_revision; - char build_date[25]; + uint64_t timestamp; + int8_t source; + int8_t robot_message_type; + int8_t project_name_size; + char project_name[15]; + uint8_t major_version; + uint8_t minor_version; + int svn_revision; + char build_date[25]; }; struct masterboard_data { - int digitalInputBits; - int digitalOutputBits; - char analogInputRange0; - char analogInputRange1; - double analogInput0; - double analogInput1; - char analogOutputDomain0; - char analogOutputDomain1; - double analogOutput0; - double analogOutput1; - float masterBoardTemperature; - float robotVoltage48V; - float robotCurrent; - float masterIOCurrent; - unsigned char safetyMode; - unsigned char masterOnOffState; - char euromap67InterfaceInstalled; - int euromapInputBits; - int euromapOutputBits; - float euromapVoltage; - float euromapCurrent; + int digitalInputBits; + int digitalOutputBits; + char analogInputRange0; + char analogInputRange1; + double analogInput0; + double analogInput1; + char analogOutputDomain0; + char analogOutputDomain1; + double analogOutput0; + double analogOutput1; + float masterBoardTemperature; + float robotVoltage48V; + float robotCurrent; + float masterIOCurrent; + unsigned char safetyMode; + unsigned char masterOnOffState; + char euromap67InterfaceInstalled; + int euromapInputBits; + int euromapOutputBits; + float euromapVoltage; + float euromapCurrent; }; struct robot_mode_data { - uint64_t timestamp; - bool isRobotConnected; - bool isRealRobotEnabled; - bool isPowerOnRobot; - bool isEmergencyStopped; - bool isProtectiveStopped; - bool isProgramRunning; - bool isProgramPaused; - unsigned char robotMode; - unsigned char controlMode; - double targetSpeedFraction; - double speedScaling; + uint64_t timestamp; + bool isRobotConnected; + bool isRealRobotEnabled; + bool isPowerOnRobot; + bool isEmergencyStopped; + bool isProtectiveStopped; + bool isProgramRunning; + bool isProgramPaused; + unsigned char robotMode; + unsigned char controlMode; + double targetSpeedFraction; + double speedScaling; }; class RobotState { private: - version_message version_msg_; - masterboard_data mb_data_; - robot_mode_data robot_mode_; + version_message version_msg_; + masterboard_data mb_data_; + robot_mode_data robot_mode_; - std::recursive_mutex val_lock_; // Locks the variables while unpack parses data; + std::recursive_mutex val_lock_; // Locks the variables while unpack parses data; - std::condition_variable* pMsg_cond_; //Signals that new vars are available - bool new_data_available_; //to avoid spurious wakes - unsigned char robot_mode_running_; + std::condition_variable* pMsg_cond_; //Signals that new vars are available + bool new_data_available_; //to avoid spurious wakes + unsigned char robot_mode_running_; - double ntohd(uint64_t nf); + double ntohd(uint64_t nf); public: - RobotState(std::condition_variable& msg_cond); - ~RobotState(); - double getVersion(); - double getTime(); - std::vector getQTarget(); - int getDigitalInputBits(); - int getDigitalOutputBits(); - char getAnalogInputRange0(); - char getAnalogInputRange1(); - double getAnalogInput0(); - double getAnalogInput1(); - char getAnalogOutputDomain0(); - char getAnalogOutputDomain1(); - double getAnalogOutput0(); - double getAnalogOutput1(); - std::vector getVActual(); - float getMasterBoardTemperature(); - float getRobotVoltage48V(); - float getRobotCurrent(); - float getMasterIOCurrent(); - unsigned char getSafetyMode(); - unsigned char getInReducedMode(); - char getEuromap67InterfaceInstalled(); - int getEuromapInputBits(); - int getEuromapOutputBits(); - float getEuromapVoltage(); - float getEuromapCurrent(); + RobotState(std::condition_variable& msg_cond); + ~RobotState(); + double getVersion(); + double getTime(); + std::vector getQTarget(); + int getDigitalInputBits(); + int getDigitalOutputBits(); + char getAnalogInputRange0(); + char getAnalogInputRange1(); + double getAnalogInput0(); + double getAnalogInput1(); + char getAnalogOutputDomain0(); + char getAnalogOutputDomain1(); + double getAnalogOutput0(); + double getAnalogOutput1(); + std::vector getVActual(); + float getMasterBoardTemperature(); + float getRobotVoltage48V(); + float getRobotCurrent(); + float getMasterIOCurrent(); + unsigned char getSafetyMode(); + unsigned char getInReducedMode(); + char getEuromap67InterfaceInstalled(); + int getEuromapInputBits(); + int getEuromapOutputBits(); + float getEuromapVoltage(); + float getEuromapCurrent(); - bool isRobotConnected(); - bool isRealRobotEnabled(); - bool isPowerOnRobot(); - bool isEmergencyStopped(); - bool isProtectiveStopped(); - bool isProgramRunning(); - bool isProgramPaused(); - unsigned char getRobotMode(); - bool isReady(); + bool isRobotConnected(); + bool isRealRobotEnabled(); + bool isPowerOnRobot(); + bool isEmergencyStopped(); + bool isProtectiveStopped(); + bool isProgramRunning(); + bool isProgramPaused(); + unsigned char getRobotMode(); + bool isReady(); - void setDisconnected(); + void setDisconnected(); - bool getNewDataAvailable(); - void finishedReading(); + bool getNewDataAvailable(); + void finishedReading(); - void unpack(uint8_t * buf, unsigned int buf_length); - void unpackRobotMessage(uint8_t * buf, unsigned int offset, uint32_t len); - void unpackRobotMessageVersion(uint8_t * buf, unsigned int offset, - uint32_t len); - void unpackRobotState(uint8_t * buf, unsigned int offset, uint32_t len); - void unpackRobotStateMasterboard(uint8_t * buf, unsigned int offset); - void unpackRobotMode(uint8_t * buf, unsigned int offset); + void unpack(uint8_t* buf, unsigned int buf_length); + void unpackRobotMessage(uint8_t* buf, unsigned int offset, uint32_t len); + void unpackRobotMessageVersion(uint8_t* buf, unsigned int offset, + uint32_t len); + void unpackRobotState(uint8_t* buf, unsigned int offset, uint32_t len); + void unpackRobotStateMasterboard(uint8_t* buf, unsigned int offset); + void unpackRobotMode(uint8_t* buf, unsigned int offset); }; #endif /* ROBOT_STATE_H_ */ diff --git a/include/ur_modern_driver/robot_state_RT.h b/include/ur_modern_driver/robot_state_RT.h index 1f5b2af..3d3b2e8 100644 --- a/include/ur_modern_driver/robot_state_RT.h +++ b/include/ur_modern_driver/robot_state_RT.h @@ -19,98 +19,98 @@ #ifndef ROBOT_STATE_RT_H_ #define ROBOT_STATE_RT_H_ +#include #include -#include -#include -#include #include #include -#include +#include +#include +#include class RobotStateRT { private: - double version_; //protocol version + double version_; //protocol version - double time_; //Time elapsed since the controller was started - std::vector q_target_; //Target joint positions - std::vector qd_target_; //Target joint velocities - std::vector qdd_target_; //Target joint accelerations - std::vector i_target_; //Target joint currents - std::vector m_target_; //Target joint moments (torques) - std::vector q_actual_; //Actual joint positions - std::vector qd_actual_; //Actual joint velocities - std::vector i_actual_; //Actual joint currents - std::vector i_control_; //Joint control currents - std::vector 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 tcp_speed_actual_; //Actual speed of the tool given in Cartesian coordinates - std::vector tcp_force_; //Generalised forces in the TC - std::vector 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 tcp_speed_target_; //Target speed of the tool given in Cartesian coordinates - std::vector digital_input_bits_; //Current state of the digital inputs. NOTE: these are bits encoded as int64_t, e.g. a value of 5 corresponds to bit 0 and bit 2 set high - std::vector motor_temperatures_; //Temperature of each joint in degrees celsius - double controller_timer_; //Controller realtime thread execution time - double robot_mode_; //Robot mode - std::vector joint_modes_; //Joint control modes - double safety_mode_; //Safety mode - std::vector tool_accelerometer_values_; //Tool x,y and z accelerometer values (software version 1.7) - double speed_scaling_; //Speed scaling of the trajectory limiter - double linear_momentum_norm_; //Norm of Cartesian linear momentum - double v_main_; //Masterboard: Main voltage - double v_robot_; //Matorborad: Robot voltage (48V) - double i_robot_; //Masterboard: Robot current - std::vector v_actual_; //Actual joint voltages + double time_; //Time elapsed since the controller was started + std::vector q_target_; //Target joint positions + std::vector qd_target_; //Target joint velocities + std::vector qdd_target_; //Target joint accelerations + std::vector i_target_; //Target joint currents + std::vector m_target_; //Target joint moments (torques) + std::vector q_actual_; //Actual joint positions + std::vector qd_actual_; //Actual joint velocities + std::vector i_actual_; //Actual joint currents + std::vector i_control_; //Joint control currents + std::vector 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 tcp_speed_actual_; //Actual speed of the tool given in Cartesian coordinates + std::vector tcp_force_; //Generalised forces in the TC + std::vector 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 tcp_speed_target_; //Target speed of the tool given in Cartesian coordinates + std::vector digital_input_bits_; //Current state of the digital inputs. NOTE: these are bits encoded as int64_t, e.g. a value of 5 corresponds to bit 0 and bit 2 set high + std::vector motor_temperatures_; //Temperature of each joint in degrees celsius + double controller_timer_; //Controller realtime thread execution time + double robot_mode_; //Robot mode + std::vector joint_modes_; //Joint control modes + double safety_mode_; //Safety mode + std::vector tool_accelerometer_values_; //Tool x,y and z accelerometer values (software version 1.7) + double speed_scaling_; //Speed scaling of the trajectory limiter + double linear_momentum_norm_; //Norm of Cartesian linear momentum + double v_main_; //Masterboard: Main voltage + double v_robot_; //Matorborad: Robot voltage (48V) + double i_robot_; //Masterboard: Robot current + std::vector v_actual_; //Actual joint voltages - std::mutex val_lock_; // Locks the variables while unpack parses data; + std::mutex val_lock_; // Locks the variables while unpack parses data; - std::condition_variable* pMsg_cond_; //Signals that new vars are available - bool data_published_; //to avoid spurious wakes - bool controller_updated_; //to avoid spurious wakes + std::condition_variable* pMsg_cond_; //Signals that new vars are available + bool data_published_; //to avoid spurious wakes + bool controller_updated_; //to avoid spurious wakes - std::vector unpackVector(uint8_t * buf, int start_index, - int nr_of_vals); - std::vector unpackDigitalInputBits(int64_t data); - double ntohd(uint64_t nf); + std::vector unpackVector(uint8_t* buf, int start_index, + int nr_of_vals); + std::vector unpackDigitalInputBits(int64_t data); + double ntohd(uint64_t nf); public: - RobotStateRT(std::condition_variable& msg_cond); - ~RobotStateRT(); - double getVersion(); - double getTime(); - std::vector getQTarget(); - std::vector getQdTarget(); - std::vector getQddTarget(); - std::vector getITarget(); - std::vector getMTarget(); - std::vector getQActual(); - std::vector getQdActual(); - std::vector getIActual(); - std::vector getIControl(); - std::vector getToolVectorActual(); - std::vector getTcpSpeedActual(); - std::vector getTcpForce(); - std::vector getToolVectorTarget(); - std::vector getTcpSpeedTarget(); - std::vector getDigitalInputBits(); - std::vector getMotorTemperatures(); - double getControllerTimer(); - double getRobotMode(); - std::vector getJointModes(); - double getSafety_mode(); - std::vector getToolAccelerometerValues(); - double getSpeedScaling(); - double getLinearMomentumNorm(); - double getVMain(); - double getVRobot(); - double getIRobot(); + RobotStateRT(std::condition_variable& msg_cond); + ~RobotStateRT(); + double getVersion(); + double getTime(); + std::vector getQTarget(); + std::vector getQdTarget(); + std::vector getQddTarget(); + std::vector getITarget(); + std::vector getMTarget(); + std::vector getQActual(); + std::vector getQdActual(); + std::vector getIActual(); + std::vector getIControl(); + std::vector getToolVectorActual(); + std::vector getTcpSpeedActual(); + std::vector getTcpForce(); + std::vector getToolVectorTarget(); + std::vector getTcpSpeedTarget(); + std::vector getDigitalInputBits(); + std::vector getMotorTemperatures(); + double getControllerTimer(); + double getRobotMode(); + std::vector getJointModes(); + double getSafety_mode(); + std::vector getToolAccelerometerValues(); + double getSpeedScaling(); + double getLinearMomentumNorm(); + double getVMain(); + double getVRobot(); + double getIRobot(); - void setVersion(double ver); + void setVersion(double ver); - void setDataPublished(); - bool getDataPublished(); - bool getControllerUpdated(); - void setControllerUpdated(); - std::vector getVActual(); - void unpack(uint8_t * buf); + void setDataPublished(); + bool getDataPublished(); + bool getControllerUpdated(); + void setControllerUpdated(); + std::vector getVActual(); + void unpack(uint8_t* buf); }; #endif /* ROBOT_STATE_RT_H_ */ diff --git a/include/ur_modern_driver/ros/converter.h b/include/ur_modern_driver/ros/converter.h index 3f59c93..6f70f09 100644 --- a/include/ur_modern_driver/ros/converter.h +++ b/include/ur_modern_driver/ros/converter.h @@ -1,2 +1 @@ #pragma once - diff --git a/include/ur_modern_driver/ros/rt_publisher.h b/include/ur_modern_driver/ros/rt_publisher.h index 3289174..0737e61 100644 --- a/include/ur_modern_driver/ros/rt_publisher.h +++ b/include/ur_modern_driver/ros/rt_publisher.h @@ -1,12 +1,12 @@ #pragma once #include -#include -#include -#include -#include #include #include +#include +#include +#include +#include #include "ur_modern_driver/ur/consumer.h" @@ -26,34 +26,34 @@ private: NodeHandle _nh; Publisher _joint_pub; Publisher _wrench_pub; - Publisher _tool_vel_pub; + Publisher _tool_vel_pub; std::vector _joint_names; std::string _base_frame; - bool publish_joints(RTShared &packet, ros::Time &t); - bool publish_wrench(RTShared &packet, ros::Time &t); - bool publish_tool(RTShared &packet, ros::Time &t); + bool publish_joints(RTShared& packet, ros::Time& t); + bool publish_wrench(RTShared& packet, ros::Time& t); + bool publish_tool(RTShared& packet, ros::Time& t); - bool publish(RTShared &packet); + bool publish(RTShared& packet); public: - RTPublisher(std::string &joint_prefix, std::string &base_frame) : - _joint_pub(_nh.advertise("joint_states", 1)), - _wrench_pub(_nh.advertise("wrench", 1)), - _tool_vel_pub(_nh.advertise("tool_velocity", 1)), - _base_frame(base_frame) + RTPublisher(std::string& joint_prefix, std::string& base_frame) + : _joint_pub(_nh.advertise("joint_states", 1)) + , _wrench_pub(_nh.advertise("wrench", 1)) + , _tool_vel_pub(_nh.advertise("tool_velocity", 1)) + , _base_frame(base_frame) { - for(auto const& j : JOINTS) { + for (auto const& j : JOINTS) { _joint_names.push_back(joint_prefix + j); } } - virtual bool consume(RTState_V1_6__7 &state); - virtual bool consume(RTState_V1_8 &state); - virtual bool consume(RTState_V3_0__1 &state); - virtual bool consume(RTState_V3_2__3 &state); + virtual bool consume(RTState_V1_6__7& state); + virtual bool consume(RTState_V1_8& state); + virtual bool consume(RTState_V3_0__1& state); + virtual bool consume(RTState_V3_2__3& state); - virtual void setup_consumer() { } - virtual void teardown_consumer() { } - virtual void stop_consumer() { } + virtual void setup_consumer() {} + virtual void teardown_consumer() {} + virtual void stop_consumer() {} }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/consumer.h b/include/ur_modern_driver/ur/consumer.h index 5b2a373..655e8bb 100644 --- a/include/ur_modern_driver/ur/consumer.h +++ b/include/ur_modern_driver/ur/consumer.h @@ -1,46 +1,47 @@ #pragma once #include "ur_modern_driver/pipeline.h" -#include "ur_modern_driver/ur/state.h" #include "ur_modern_driver/ur/master_board.h" +#include "ur_modern_driver/ur/messages.h" #include "ur_modern_driver/ur/robot_mode.h" #include "ur_modern_driver/ur/rt_state.h" -#include "ur_modern_driver/ur/messages.h" +#include "ur_modern_driver/ur/state.h" class URRTPacketConsumer : public IConsumer { public: - virtual bool consume(unique_ptr packet) { + virtual bool consume(unique_ptr packet) + { return packet->consume_with(*this); } - virtual bool consume(RTState_V1_6__7 &state) = 0; - virtual bool consume(RTState_V1_8 &state) = 0; - virtual bool consume(RTState_V3_0__1 &state) = 0; - virtual bool consume(RTState_V3_2__3 &state) = 0; + virtual bool consume(RTState_V1_6__7& state) = 0; + virtual bool consume(RTState_V1_8& state) = 0; + virtual bool consume(RTState_V3_0__1& state) = 0; + virtual bool consume(RTState_V3_2__3& state) = 0; }; - class URStatePacketConsumer : public IConsumer { public: - virtual bool consume(unique_ptr packet) { + virtual bool consume(unique_ptr packet) + { return packet->consume_with(*this); } - virtual bool consume(MasterBoardData_V1_X &data) = 0; - virtual bool consume(MasterBoardData_V3_0__1 &data) = 0; - virtual bool consume(MasterBoardData_V3_2 &data) = 0; - - virtual bool consume(RobotModeData_V1_X &data) = 0; - virtual bool consume(RobotModeData_V3_0__1 &data) = 0; - virtual bool consume(RobotModeData_V3_2 &data) = 0; -}; + virtual bool consume(MasterBoardData_V1_X& data) = 0; + virtual bool consume(MasterBoardData_V3_0__1& data) = 0; + virtual bool consume(MasterBoardData_V3_2& data) = 0; + virtual bool consume(RobotModeData_V1_X& data) = 0; + virtual bool consume(RobotModeData_V3_0__1& data) = 0; + virtual bool consume(RobotModeData_V3_2& data) = 0; +}; class URMessagePacketConsumer : public IConsumer { public: - virtual bool consume(unique_ptr packet) { + virtual bool consume(unique_ptr packet) + { return packet->consume_with(*this); } - virtual bool consume(VersionMessage &message) = 0; + virtual bool consume(VersionMessage& message) = 0; }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/factory.h b/include/ur_modern_driver/ur/factory.h index a09186f..d3266ea 100644 --- a/include/ur_modern_driver/ur/factory.h +++ b/include/ur_modern_driver/ur/factory.h @@ -1,14 +1,13 @@ #pragma once -#include -#include "ur_modern_driver/ur/stream.h" #include "ur_modern_driver/ur/consumer.h" -#include "ur_modern_driver/ur/producer.h" -#include "ur_modern_driver/ur/parser.h" -#include "ur_modern_driver/ur/state_parser.h" -#include "ur_modern_driver/ur/rt_parser.h" #include "ur_modern_driver/ur/messages_parser.h" - +#include "ur_modern_driver/ur/parser.h" +#include "ur_modern_driver/ur/producer.h" +#include "ur_modern_driver/ur/rt_parser.h" +#include "ur_modern_driver/ur/state_parser.h" +#include "ur_modern_driver/ur/stream.h" +#include class URFactory : private URMessagePacketConsumer { private: @@ -18,7 +17,8 @@ private: uint8_t _major_version; uint8_t _minor_version; - bool consume(VersionMessage &vm) { + bool consume(VersionMessage& vm) + { LOG_INFO("Got VersionMessage:"); LOG_INFO("project name: %s", vm.project_name.c_str()); LOG_INFO("version: %u.%u.%d", vm.major_version, vm.minor_version, vm.svn_version); @@ -26,31 +26,33 @@ private: _major_version = vm.major_version; _minor_version = vm.minor_version; - + return true; } - void setup_consumer() { } - void teardown_consumer() { } - void stop_consumer() { } + void setup_consumer() {} + void teardown_consumer() {} + void stop_consumer() {} public: - URFactory(std::string &host) : _stream(host, 30001) { + URFactory(std::string& host) + : _stream(host, 30001) + { URProducer p(_stream, _parser); - std::vector> results; + std::vector > results; p.setup_producer(); - if(!p.try_get(results) || results.size() == 0) { - LOG_FATAL("No version message received, init failed!"); + 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) { + + for (auto const& p : results) { p->consume_with(*this); } - if(_major_version == 0 && _minor_version == 0) { + if (_major_version == 0 && _minor_version == 0) { LOG_FATAL("No version message received, init failed!"); std::exit(EXIT_FAILURE); } @@ -58,28 +60,30 @@ public: p.teardown_producer(); } - std::unique_ptr> get_state_parser() { - if(_major_version == 1) { - return std::unique_ptr>(new URStateParser_V1_X); + std::unique_ptr > get_state_parser() + { + if (_major_version == 1) { + return std::unique_ptr >(new URStateParser_V1_X); } else { - if(_minor_version < 3) - return std::unique_ptr>(new URStateParser_V3_0__1); + if (_minor_version < 3) + return std::unique_ptr >(new URStateParser_V3_0__1); else - return std::unique_ptr>(new URStateParser_V3_2); + return std::unique_ptr >(new URStateParser_V3_2); } } - std::unique_ptr> get_rt_parser() { - if(_major_version == 1) { - if(_minor_version < 8) - return std::unique_ptr>(new URRTStateParser_V1_6__7); + std::unique_ptr > get_rt_parser() + { + if (_major_version == 1) { + if (_minor_version < 8) + return std::unique_ptr >(new URRTStateParser_V1_6__7); else - return std::unique_ptr>(new URRTStateParser_V1_8); + return std::unique_ptr >(new URRTStateParser_V1_8); } else { - if(_minor_version < 3) - return std::unique_ptr>(new URRTStateParser_V3_0__1); - else - return std::unique_ptr>(new URRTStateParser_V3_2__3); + if (_minor_version < 3) + return std::unique_ptr >(new URRTStateParser_V3_0__1); + else + return std::unique_ptr >(new URRTStateParser_V3_2__3); } } }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/master_board.h b/include/ur_modern_driver/ur/master_board.h index 0a7daf1..5f23397 100644 --- a/include/ur_modern_driver/ur/master_board.h +++ b/include/ur_modern_driver/ur/master_board.h @@ -1,23 +1,22 @@ #pragma once +#include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/types.h" +#include "ur_modern_driver/ur/state.h" #include #include -#include "ur_modern_driver/types.h" -#include "ur_modern_driver/bin_parser.h" -#include "ur_modern_driver/ur/state.h" - class SharedMasterBoardData { public: - virtual bool parse_with(BinParser &bp); - - int8_t analog_input_range0; + virtual bool parse_with(BinParser& bp); + + int8_t analog_input_range0; int8_t analog_input_range1; - double analog_input0; + double analog_input0; double analog_input1; - int8_t analog_output_domain0; + int8_t analog_output_domain0; int8_t analog_output_domain1; - double analog_output0; + double analog_output0; double analog_output1; float master_board_temperature; float robot_voltage_48V; @@ -30,7 +29,7 @@ public: int32_t euromap_input_bits; int32_t euromap_output_bits; - static const size_t SIZE = sizeof(double) * 4 + static const size_t SIZE = sizeof(double) * 4 + sizeof(int8_t) * 4 + sizeof(float) * 4 + sizeof(uint8_t); @@ -40,8 +39,8 @@ public: class MasterBoardData_V1_X : public SharedMasterBoardData, public StatePacket { public: - virtual bool parse_with(BinParser &bp); - virtual bool consume_with(URStatePacketConsumer &consumer); + virtual bool parse_with(BinParser& bp); + virtual bool consume_with(URStatePacketConsumer& consumer); int16_t digital_input_bits; int16_t digital_output_bits; @@ -53,7 +52,6 @@ public: int16_t euromap_voltage; int16_t euromap_current; - static const size_t SIZE = SharedMasterBoardData::SIZE + sizeof(int16_t) * 2 + sizeof(uint8_t) * 2; @@ -64,8 +62,8 @@ public: class MasterBoardData_V3_0__1 : public SharedMasterBoardData, public StatePacket { public: - virtual bool parse_with(BinParser &bp); - virtual bool consume_with(URStatePacketConsumer &consumer); + virtual bool parse_with(BinParser& bp); + virtual bool consume_with(URStatePacketConsumer& consumer); int32_t digital_input_bits; int32_t digital_output_bits; @@ -77,7 +75,6 @@ public: float euromap_voltage; float euromap_current; - static const size_t SIZE = SharedMasterBoardData::SIZE + sizeof(int32_t) * 2 + sizeof(uint8_t) * 2 @@ -89,8 +86,8 @@ public: class MasterBoardData_V3_2 : public MasterBoardData_V3_0__1 { public: - virtual bool parse_with(BinParser &bp); - virtual bool consume_with(URStatePacketConsumer &consumer); + virtual bool parse_with(BinParser& bp); + virtual bool consume_with(URStatePacketConsumer& consumer); uint8_t operational_mode_selector_input; uint8_t three_position_enabling_device_input; diff --git a/include/ur_modern_driver/ur/messages.h b/include/ur_modern_driver/ur/messages.h index 97315de..dafad61 100644 --- a/include/ur_modern_driver/ur/messages.h +++ b/include/ur_modern_driver/ur/messages.h @@ -1,30 +1,33 @@ #pragma once +#include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/pipeline.h" #include #include -#include "ur_modern_driver/pipeline.h" -#include "ur_modern_driver/bin_parser.h" - enum class robot_message_type : uint8_t { - ROBOT_MESSAGE_TEXT = 0, - ROBOT_MESSAGE_PROGRAM_LABEL = 1, - PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2, - ROBOT_MESSAGE_VERSION = 3, - ROBOT_MESSAGE_SAFETY_MODE = 5, - ROBOT_MESSAGE_ERROR_CODE = 6, - ROBOT_MESSAGE_KEY = 7, - ROBOT_MESSAGE_REQUEST_VALUE = 9, - ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10 + ROBOT_MESSAGE_TEXT = 0, + ROBOT_MESSAGE_PROGRAM_LABEL = 1, + PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2, + ROBOT_MESSAGE_VERSION = 3, + ROBOT_MESSAGE_SAFETY_MODE = 5, + ROBOT_MESSAGE_ERROR_CODE = 6, + ROBOT_MESSAGE_KEY = 7, + ROBOT_MESSAGE_REQUEST_VALUE = 9, + ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10 }; class URMessagePacketConsumer; class MessagePacket { public: - MessagePacket(uint64_t timestamp, uint8_t source) : timestamp(timestamp), source(source) { } - virtual bool parse_with(BinParser &bp) = 0; - virtual bool consume_with(URMessagePacketConsumer &consumer) = 0; + MessagePacket(uint64_t timestamp, uint8_t source) + : timestamp(timestamp) + , source(source) + { + } + virtual bool parse_with(BinParser& bp) = 0; + virtual bool consume_with(URMessagePacketConsumer& consumer) = 0; uint64_t timestamp; uint8_t source; @@ -32,10 +35,13 @@ public: class VersionMessage : public MessagePacket { public: - VersionMessage(uint64_t timestamp, uint8_t source) : MessagePacket(timestamp, source) { } - - virtual bool parse_with(BinParser &bp); - virtual bool consume_with(URMessagePacketConsumer &consumer); + VersionMessage(uint64_t timestamp, uint8_t source) + : MessagePacket(timestamp, source) + { + } + + virtual bool parse_with(BinParser& bp); + virtual bool consume_with(URMessagePacketConsumer& consumer); std::string project_name; uint8_t major_version; diff --git a/include/ur_modern_driver/ur/messages_parser.h b/include/ur_modern_driver/ur/messages_parser.h index 77dc9b7..b0a781f 100644 --- a/include/ur_modern_driver/ur/messages_parser.h +++ b/include/ur_modern_driver/ur/messages_parser.h @@ -1,20 +1,21 @@ #pragma once -#include -#include "ur_modern_driver/log.h" #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/messages.h" +#include "ur_modern_driver/ur/parser.h" +#include class URMessageParser : public URParser { public: - bool parse(BinParser &bp, std::vector> &results) { + bool parse(BinParser& bp, std::vector >& results) + { int32_t packet_size; message_type type; bp.parse(packet_size); bp.parse(type); - if(type != message_type::ROBOT_MESSAGE) { + if (type != message_type::ROBOT_MESSAGE) { LOG_WARN("Invalid message type recieved: %u", static_cast(type)); return false; } @@ -30,16 +31,16 @@ public: std::unique_ptr 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; - } + 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; + default: + return false; } results.push_back(std::move(result)); diff --git a/include/ur_modern_driver/ur/parser.h b/include/ur_modern_driver/ur/parser.h index 00a0bac..ea53b40 100644 --- a/include/ur_modern_driver/ur/parser.h +++ b/include/ur_modern_driver/ur/parser.h @@ -1,10 +1,10 @@ #pragma once -#include -#include "ur_modern_driver/pipeline.h" #include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/pipeline.h" +#include template class URParser { public: - virtual bool parse(BinParser &bp, std::vector> &results) = 0; + virtual bool parse(BinParser& bp, std::vector >& results) = 0; }; diff --git a/include/ur_modern_driver/ur/producer.h b/include/ur_modern_driver/ur/producer.h index 35be085..cfbe95c 100644 --- a/include/ur_modern_driver/ur/producer.h +++ b/include/ur_modern_driver/ur/producer.h @@ -1,30 +1,36 @@ #pragma once #include "ur_modern_driver/pipeline.h" -#include "ur_modern_driver/ur/stream.h" #include "ur_modern_driver/ur/parser.h" +#include "ur_modern_driver/ur/stream.h" template class URProducer : public IProducer { private: - URStream &_stream; - URParser &_parser; + URStream& _stream; + URParser& _parser; public: - URProducer(URStream &stream, URParser &parser) - : _stream(stream), - _parser(parser) { } - - void setup_producer() { + URProducer(URStream& stream, URParser& parser) + : _stream(stream) + , _parser(parser) + { + } + + void setup_producer() + { _stream.connect(); } - void teardown_producer() { + void teardown_producer() + { _stream.disconnect(); } - void stop_producer() { + void stop_producer() + { _stream.disconnect(); } - - bool try_get(std::vector> &products) { + + bool try_get(std::vector >& products) + { //4KB should be enough to hold any packet received from UR uint8_t buf[4096]; @@ -33,7 +39,7 @@ public: //LOG_DEBUG("Read %d bytes from stream", len); - if(len < 1) { + if (len < 1) { LOG_WARN("Read nothing from stream"); return false; } diff --git a/include/ur_modern_driver/ur/robot_mode.h b/include/ur_modern_driver/ur/robot_mode.h index bce22af..b5307b9 100644 --- a/include/ur_modern_driver/ur/robot_mode.h +++ b/include/ur_modern_driver/ur/robot_mode.h @@ -1,14 +1,14 @@ #pragma once +#include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/types.h" +#include "ur_modern_driver/ur/state.h" #include #include -#include "ur_modern_driver/types.h" -#include "ur_modern_driver/bin_parser.h" -#include "ur_modern_driver/ur/state.h" class SharedRobotModeData { public: - virtual bool parse_with(BinParser &bp); + virtual bool parse_with(BinParser& bp); uint64_t timestamp; bool physical_robot_connected; @@ -23,31 +23,30 @@ public: enum class robot_mode_V1_X : uint8_t { ROBOT_RUNNING_MODE = 0, - ROBOT_FREEDRIVE_MODE = 1, - ROBOT_READY_MODE = 2, - ROBOT_INITIALIZING_MODE = 3, - ROBOT_SECURITY_STOPPED_MODE = 4, - ROBOT_EMERGENCY_STOPPED_MODE = 5, - ROBOT_FATAL_ERROR_MODE = 6, - ROBOT_NO_POWER_MODE = 7, - ROBOT_NOT_CONNECTED_MODE = 8, - ROBOT_SHUTDOWN_MODE = 9, - ROBOT_SAFEGUARD_STOP_MODE = 10 + ROBOT_FREEDRIVE_MODE = 1, + ROBOT_READY_MODE = 2, + ROBOT_INITIALIZING_MODE = 3, + ROBOT_SECURITY_STOPPED_MODE = 4, + ROBOT_EMERGENCY_STOPPED_MODE = 5, + ROBOT_FATAL_ERROR_MODE = 6, + ROBOT_NO_POWER_MODE = 7, + ROBOT_NOT_CONNECTED_MODE = 8, + ROBOT_SHUTDOWN_MODE = 9, + ROBOT_SAFEGUARD_STOP_MODE = 10 }; class RobotModeData_V1_X : public SharedRobotModeData, public StatePacket { public: - virtual bool parse_with(BinParser &bp); - virtual bool consume_with(URStatePacketConsumer &consumer); - + virtual bool parse_with(BinParser& bp); + virtual bool consume_with(URStatePacketConsumer& consumer); bool security_stopped; robot_mode_V1_X robot_mode; double speed_fraction; - static const size_t SIZE = SharedRobotModeData::SIZE + static const size_t SIZE = SharedRobotModeData::SIZE + sizeof(uint8_t) - + sizeof(robot_mode_V1_X) + + sizeof(robot_mode_V1_X) + sizeof(double); static_assert(RobotModeData_V1_X::SIZE == 24, "RobotModeData_V1_X has missmatched size"); @@ -55,14 +54,14 @@ public: enum class robot_mode_V3_X : uint8_t { DISCONNECTED = 0, - CONFIRM_SAFETY = 1, - BOOTING = 2, - POWER_OFF = 3, - POWER_ON = 4, - IDLE = 5, - BACKDRIVE = 6, - RUNNING = 7, - UPDATING_FIRMWARE = 8 + CONFIRM_SAFETY = 1, + BOOTING = 2, + POWER_OFF = 3, + POWER_ON = 4, + IDLE = 5, + BACKDRIVE = 6, + RUNNING = 7, + UPDATING_FIRMWARE = 8 }; enum class robot_control_mode_V3_X : uint8_t { @@ -74,9 +73,8 @@ enum class robot_control_mode_V3_X : uint8_t { class RobotModeData_V3_0__1 : public SharedRobotModeData, public StatePacket { public: - virtual bool parse_with(BinParser &bp); - virtual bool consume_with(URStatePacketConsumer &consumer); - + virtual bool parse_with(BinParser& bp); + virtual bool consume_with(URStatePacketConsumer& consumer); bool protective_stopped; @@ -85,10 +83,10 @@ public: double target_speed_fraction; double speed_scaling; - - static const size_t SIZE = SharedRobotModeData::SIZE - + sizeof(uint8_t) - + sizeof(robot_mode_V3_X) + + static const size_t SIZE = SharedRobotModeData::SIZE + + sizeof(uint8_t) + + sizeof(robot_mode_V3_X) + sizeof(robot_control_mode_V3_X) + sizeof(double) + sizeof(double); @@ -98,14 +96,13 @@ public: class RobotModeData_V3_2 : public RobotModeData_V3_0__1 { public: - virtual bool parse_with(BinParser &bp); - virtual bool consume_with(URStatePacketConsumer &consumer); - + virtual bool parse_with(BinParser& bp); + virtual bool consume_with(URStatePacketConsumer& consumer); double target_speed_fraction_limit; static const size_t SIZE = RobotModeData_V3_0__1::SIZE + sizeof(double); - static_assert(RobotModeData_V3_2::SIZE == 41, "RobotModeData_V3_2 has missmatched size"); + static_assert(RobotModeData_V3_2::SIZE == 41, "RobotModeData_V3_2 has missmatched size"); }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/rt_parser.h b/include/ur_modern_driver/ur/rt_parser.h index 3a3831a..e0cde0d 100644 --- a/include/ur_modern_driver/ur/rt_parser.h +++ b/include/ur_modern_driver/ur/rt_parser.h @@ -1,31 +1,31 @@ #pragma once -#include -#include "ur_modern_driver/log.h" #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 template class URRTStateParser : public URParser { public: - bool parse(BinParser &bp, std::vector> &results) { + bool parse(BinParser& bp, std::vector >& results) + { int32_t packet_size = bp.peek(); - if(!bp.check_size(packet_size)) { + if (!bp.check_size(packet_size)) { LOG_ERROR("Buffer len shorter than expected packet length"); - return false; + return false; } bp.parse(packet_size); //consumes the peeked data std::unique_ptr packet(new T); - if(!packet->parse_with(bp)) + if (!packet->parse_with(bp)) return false; - + results.push_back(std::move(packet)); - + return true; } }; diff --git a/include/ur_modern_driver/ur/rt_state.h b/include/ur_modern_driver/ur/rt_state.h index c749ebb..92cd299 100644 --- a/include/ur_modern_driver/ur/rt_state.h +++ b/include/ur_modern_driver/ur/rt_state.h @@ -1,23 +1,23 @@ #pragma once -#include -#include -#include "ur_modern_driver/types.h" #include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/pipeline.h" +#include "ur_modern_driver/types.h" +#include +#include class URRTPacketConsumer; class RTPacket { public: - virtual bool parse_with(BinParser &bp) = 0; - virtual bool consume_with(URRTPacketConsumer &consumer) = 0; + virtual bool parse_with(BinParser& bp) = 0; + virtual bool consume_with(URRTPacketConsumer& consumer) = 0; }; class RTShared { protected: - bool parse_shared1(BinParser &bp); - bool parse_shared2(BinParser &bp); + bool parse_shared1(BinParser& bp); + bool parse_shared2(BinParser& bp); public: double time; @@ -43,18 +43,16 @@ public: double controller_time; double robot_mode; - - static const size_t SIZE = sizeof(double) * 3 + static const size_t SIZE = sizeof(double) * 3 + sizeof(double[6]) * 10 + sizeof(cartesian_coord_t) * 2 + sizeof(uint64_t); - }; class RTState_V1_6__7 : public RTShared, public RTPacket { public: - bool parse_with(BinParser &bp); - virtual bool consume_with(URRTPacketConsumer &consumer); + bool parse_with(BinParser& bp); + virtual bool consume_with(URRTPacketConsumer& consumer); double3_t tool_accelerometer_values; @@ -66,8 +64,8 @@ public: class RTState_V1_8 : public RTState_V1_6__7 { public: - bool parse_with(BinParser &bp); - virtual bool consume_with(URRTPacketConsumer &consumer); + bool parse_with(BinParser& bp); + virtual bool consume_with(URRTPacketConsumer& consumer); double joint_modes[6]; @@ -79,14 +77,13 @@ public: class RTState_V3_0__1 : public RTShared, public RTPacket { public: - bool parse_with(BinParser &bp); - virtual bool consume_with(URRTPacketConsumer &consumer); + bool parse_with(BinParser& bp); + virtual bool consume_with(URRTPacketConsumer& consumer); double i_control[6]; cartesian_coord_t tool_vector_target; cartesian_coord_t tcp_speed_target; - double joint_modes[6]; double safety_mode; double3_t tool_accelerometer_values; @@ -97,8 +94,7 @@ public: double i_robot; double v_actual[6]; - - static const size_t SIZE = RTShared::SIZE + static const size_t SIZE = RTShared::SIZE + sizeof(double[6]) * 3 + sizeof(double3_t) + sizeof(cartesian_coord_t) * 2 @@ -109,15 +105,15 @@ public: class RTState_V3_2__3 : public RTState_V3_0__1 { public: - bool parse_with(BinParser &bp); - virtual bool consume_with(URRTPacketConsumer &consumer); + bool parse_with(BinParser& bp); + virtual bool consume_with(URRTPacketConsumer& consumer); uint64_t digital_outputs; double program_state; - static const size_t SIZE = RTState_V3_0__1::SIZE + static const size_t SIZE = RTState_V3_0__1::SIZE + sizeof(uint64_t) + sizeof(double); - static_assert(RTState_V3_2__3::SIZE == 936, "RTState_V3_2__3 has mismatched size!"); + static_assert(RTState_V3_2__3::SIZE == 936, "RTState_V3_2__3 has mismatched size!"); }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/state.h b/include/ur_modern_driver/ur/state.h index d37cbf1..0ac8e69 100644 --- a/include/ur_modern_driver/ur/state.h +++ b/include/ur_modern_driver/ur/state.h @@ -1,27 +1,27 @@ #pragma once -#include -#include -#include "ur_modern_driver/pipeline.h" #include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/log.h" +#include "ur_modern_driver/pipeline.h" +#include +#include enum class package_type : uint8_t { - ROBOT_MODE_DATA = 0, - JOINT_DATA = 1, - TOOL_DATA = 2, - MASTERBOARD_DATA = 3, - CARTESIAN_INFO = 4, - KINEMATICS_INFO = 5, - CONFIGURATION_DATA = 6, - FORCE_MODE_DATA = 7, - ADDITIONAL_INFO = 8, - CALIBRATION_DATA = 9 + ROBOT_MODE_DATA = 0, + JOINT_DATA = 1, + TOOL_DATA = 2, + MASTERBOARD_DATA = 3, + CARTESIAN_INFO = 4, + KINEMATICS_INFO = 5, + CONFIGURATION_DATA = 6, + FORCE_MODE_DATA = 7, + ADDITIONAL_INFO = 8, + CALIBRATION_DATA = 9 }; enum class message_type : uint8_t { - ROBOT_STATE = 16, - ROBOT_MESSAGE = 20, + ROBOT_STATE = 16, + ROBOT_MESSAGE = 20, PROGRAM_STATE_MESSAGE = 25 }; @@ -29,6 +29,6 @@ class URStatePacketConsumer; class StatePacket { public: - virtual bool parse_with(BinParser &bp) = 0; - virtual bool consume_with(URStatePacketConsumer &consumer) = 0; + virtual bool parse_with(BinParser& bp) = 0; + virtual bool consume_with(URStatePacketConsumer& consumer) = 0; }; diff --git a/include/ur_modern_driver/ur/state_parser.h b/include/ur_modern_driver/ur/state_parser.h index df5a7d6..a68a611 100644 --- a/include/ur_modern_driver/ur/state_parser.h +++ b/include/ur_modern_driver/ur/state_parser.h @@ -1,73 +1,74 @@ #pragma once -#include -#include "ur_modern_driver/log.h" #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/state.h" #include "ur_modern_driver/ur/master_board.h" +#include "ur_modern_driver/ur/parser.h" #include "ur_modern_driver/ur/robot_mode.h" - +#include "ur_modern_driver/ur/state.h" +#include template class URStateParser : public URParser { private: - StatePacket* from_type(package_type type) { - switch(type) { - case package_type::ROBOT_MODE_DATA: - return new RMD; - case package_type::MASTERBOARD_DATA: - return new MBD; - default: - return nullptr; + StatePacket* from_type(package_type type) + { + switch (type) { + case package_type::ROBOT_MODE_DATA: + return new RMD; + case package_type::MASTERBOARD_DATA: + return new MBD; + default: + return nullptr; } } public: - bool parse(BinParser &bp, std::vector> &results) { + bool parse(BinParser& bp, std::vector >& results) + { int32_t packet_size; message_type type; bp.parse(packet_size); bp.parse(type); - if(type != message_type::ROBOT_STATE) { + if (type != message_type::ROBOT_STATE) { LOG_WARN("Invalid message type recieved: %u", static_cast(type)); return false; } - while(!bp.empty()) { - if(!bp.check_size(sizeof(uint32_t))){ + 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; + return false; } uint32_t sub_size = bp.peek(); - if(!bp.check_size(static_cast(sub_size))) { + if (!bp.check_size(static_cast(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 sbp(bp, sub_size); sbp.consume(sizeof(sub_size)); package_type type; sbp.parse(type); - + std::unique_ptr packet(from_type(type)); - if(packet == nullptr) { + if (packet == nullptr) { sbp.consume(); LOG_INFO("Skipping sub-packet of type %d", type); continue; } - if(!packet->parse_with(sbp)) { + if (!packet->parse_with(sbp)) { LOG_ERROR("Sub-package parsing of type %d failed!", type); return false; } results.push_back(std::move(packet)); - if(!sbp.empty()) { + if (!sbp.empty()) { LOG_ERROR("Sub-package was not parsed completely!"); return false; } @@ -77,6 +78,6 @@ public: } }; -typedef URStateParser URStateParser_V1_X; -typedef URStateParser URStateParser_V3_0__1; -typedef URStateParser URStateParser_V3_2; \ No newline at end of file +typedef URStateParser URStateParser_V1_X; +typedef URStateParser URStateParser_V3_0__1; +typedef URStateParser URStateParser_V3_2; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/stream.h b/include/ur_modern_driver/ur/stream.h index a6ac0d5..7d7363b 100644 --- a/include/ur_modern_driver/ur/stream.h +++ b/include/ur_modern_driver/ur/stream.h @@ -1,11 +1,11 @@ #pragma once -#include #include -#include -#include #include +#include +#include +#include -/// Encapsulates a TCP socket +/// Encapsulates a TCP socket class URStream { private: int _socket_fd = -1; @@ -16,19 +16,22 @@ private: std::atomic _stopping; public: - URStream(std::string &host, int port) - : _host(host), - _port(port), - _initialized(false), - _stopping(false) {} - - ~URStream() { + URStream(std::string& host, int port) + : _host(host) + , _port(port) + , _initialized(false) + , _stopping(false) + { + } + + ~URStream() + { disconnect(); } bool connect(); void disconnect(); - ssize_t send(uint8_t *buf, size_t buf_len); - ssize_t receive(uint8_t *buf, size_t buf_len); + ssize_t send(uint8_t* buf, size_t buf_len); + ssize_t receive(uint8_t* buf, size_t buf_len); }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur_communication.h b/include/ur_modern_driver/ur_communication.h index 5173c45..cad98e1 100644 --- a/include/ur_modern_driver/ur_communication.h +++ b/include/ur_modern_driver/ur_communication.h @@ -19,46 +19,44 @@ #ifndef UR_COMMUNICATION_H_ #define UR_COMMUNICATION_H_ -#include "robot_state.h" #include "do_output.h" -#include -#include -#include -#include -#include -#include -#include +#include "robot_state.h" +#include #include -#include -#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include - +#include +#include +#include +#include class UrCommunication { private: - int pri_sockfd_, sec_sockfd_; - struct sockaddr_in pri_serv_addr_, sec_serv_addr_; - struct hostent *server_; - bool keepalive_; - std::thread comThread_; - int flag_; - void run(); + int pri_sockfd_, sec_sockfd_; + struct sockaddr_in pri_serv_addr_, sec_serv_addr_; + struct hostent* server_; + bool keepalive_; + std::thread comThread_; + int flag_; + void run(); public: - bool connected_; - RobotState* robot_state_; - - UrCommunication(std::condition_variable& msg_cond, std::string host); - bool start(); - void halt(); + bool connected_; + RobotState* robot_state_; + UrCommunication(std::condition_variable& msg_cond, std::string host); + bool start(); + void halt(); }; #endif /* UR_COMMUNICATION_H_ */ diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h index 0f392fa..1d414c1 100644 --- a/include/ur_modern_driver/ur_driver.h +++ b/include/ur_modern_driver/ur_driver.h @@ -19,83 +19,81 @@ #ifndef UR_DRIVER_H_ #define UR_DRIVER_H_ -#include -#include -#include "ur_realtime_communication.h" -#include "ur_communication.h" #include "do_output.h" -#include +#include "ur_communication.h" +#include "ur_realtime_communication.h" +#include #include -#include -#include -#include +#include #include +#include +#include +#include +#include #include - class UrDriver { private: - double maximum_time_step_; - double minimum_payload_; - double maximum_payload_; - std::vector joint_names_; - std::string ip_addr_; - const int MULT_JOINTSTATE_ = 1000000; - const int MULT_TIME_ = 1000000; - const unsigned int REVERSE_PORT_; - int incoming_sockfd_; - int new_sockfd_; - bool reverse_connected_; - double servoj_time_; - bool executing_traj_; - double firmware_version_; - double servoj_lookahead_time_; - double servoj_gain_; + double maximum_time_step_; + double minimum_payload_; + double maximum_payload_; + std::vector joint_names_; + std::string ip_addr_; + const int MULT_JOINTSTATE_ = 1000000; + const int MULT_TIME_ = 1000000; + const unsigned int REVERSE_PORT_; + int incoming_sockfd_; + int new_sockfd_; + bool reverse_connected_; + double servoj_time_; + bool executing_traj_; + double firmware_version_; + double servoj_lookahead_time_; + double servoj_gain_; + public: - UrRealtimeCommunication* rt_interface_; - UrCommunication* sec_interface_; + UrRealtimeCommunication* rt_interface_; + UrCommunication* sec_interface_; - UrDriver(std::condition_variable& rt_msg_cond, - std::condition_variable& msg_cond, std::string host, - unsigned int reverse_port = 50007, double servoj_time = 0.016, unsigned int safety_count_max = - 12, double max_time_step = 0.08, double min_payload = 0., - double max_payload = 1., double servoj_lookahead_time=0.03, double servoj_gain=300.); - bool start(); - void halt(); + UrDriver(std::condition_variable& rt_msg_cond, + std::condition_variable& msg_cond, std::string host, + unsigned int reverse_port = 50007, double servoj_time = 0.016, unsigned int safety_count_max = 12, double max_time_step = 0.08, double min_payload = 0., + double max_payload = 1., double servoj_lookahead_time = 0.03, double servoj_gain = 300.); + bool start(); + void halt(); - void setSpeed(double q0, double q1, double q2, double q3, double q4, - double q5, double acc = 100.); + void setSpeed(double q0, double q1, double q2, double q3, double q4, + double q5, double acc = 100.); - bool doTraj(std::vector inp_timestamps, - std::vector > inp_positions, - std::vector > inp_velocities); - void servoj(std::vector positions, int keepalive = 1); + bool doTraj(std::vector inp_timestamps, + std::vector > inp_positions, + std::vector > inp_velocities); + void servoj(std::vector positions, int keepalive = 1); - void stopTraj(); + void stopTraj(); - bool uploadProg(); - bool openServo(); - void closeServo(std::vector positions); + bool uploadProg(); + bool openServo(); + void closeServo(std::vector positions); - std::vector interp_cubic(double t, double T, - std::vector p0_pos, std::vector p1_pos, - std::vector p0_vel, std::vector p1_vel); + std::vector interp_cubic(double t, double T, + std::vector p0_pos, std::vector p1_pos, + std::vector p0_vel, std::vector p1_vel); - std::vector getJointNames(); - void setJointNames(std::vector jn); - void setToolVoltage(unsigned int v); - void setFlag(unsigned int n, bool b); - void setDigitalOut(unsigned int n, bool b); - void setAnalogOut(unsigned int n, double f); - bool setPayload(double m); - - void setMinPayload(double m); - void setMaxPayload(double m); - void setServojTime(double t); - void setServojLookahead(double t); - void setServojGain(double g); + std::vector getJointNames(); + void setJointNames(std::vector jn); + void setToolVoltage(unsigned int v); + void setFlag(unsigned int n, bool b); + void setDigitalOut(unsigned int n, bool b); + void setAnalogOut(unsigned int n, double f); + bool setPayload(double m); + void setMinPayload(double m); + void setMaxPayload(double m); + void setServojTime(double t); + void setServojLookahead(double t); + void setServojGain(double g); }; #endif /* UR_DRIVER_H_ */ diff --git a/include/ur_modern_driver/ur_hardware_interface.h b/include/ur_modern_driver/ur_hardware_interface.h index ac9f310..0724703 100644 --- a/include/ur_modern_driver/ur_hardware_interface.h +++ b/include/ur_modern_driver/ur_hardware_interface.h @@ -58,16 +58,16 @@ #ifndef UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H #define UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H -#include -#include -#include -#include -#include -#include -#include -#include #include "do_output.h" #include "ur_driver.h" +#include +#include +#include +#include +#include +#include +#include +#include namespace ros_control_ur { @@ -76,64 +76,61 @@ static const double POSITION_STEP_FACTOR = 1; static const double VELOCITY_STEP_FACTOR = 1; /// \brief Hardware interface for a robot -class UrHardwareInterface: public hardware_interface::RobotHW { +class UrHardwareInterface : public hardware_interface::RobotHW { public: - - /** + /** * \brief Constructor * \param nh - Node handle for topics. */ - UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot); + UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot); - /// \brief Initialize the hardware interface - virtual void init(); + /// \brief Initialize the hardware interface + virtual void init(); - /// \brief Read the state from the robot hardware. - virtual void read(); + /// \brief Read the state from the robot hardware. + virtual void read(); - /// \brief write the command to the robot hardware. - virtual void write(); + /// \brief write the command to the robot hardware. + virtual void write(); - void setMaxVelChange(double inp); + void setMaxVelChange(double inp); - bool canSwitch( - const std::list &start_list, - const std::list &stop_list) const; - void doSwitch(const std::list&start_list, - const std::list&stop_list); + bool canSwitch( + const std::list& start_list, + const std::list& stop_list) const; + void doSwitch(const std::list& start_list, + const std::list& stop_list); protected: + // Startup and shutdown of the internal node inside a roscpp program + ros::NodeHandle nh_; - // Startup and shutdown of the internal node inside a roscpp program - ros::NodeHandle nh_; + // Interfaces + hardware_interface::JointStateInterface joint_state_interface_; + hardware_interface::ForceTorqueSensorInterface force_torque_interface_; + hardware_interface::PositionJointInterface position_joint_interface_; + hardware_interface::VelocityJointInterface velocity_joint_interface_; + bool velocity_interface_running_; + bool position_interface_running_; + // Shared memory + std::vector joint_names_; + std::vector joint_position_; + std::vector joint_velocity_; + std::vector joint_effort_; + std::vector joint_position_command_; + std::vector joint_velocity_command_; + std::vector prev_joint_velocity_command_; + std::size_t num_joints_; + double robot_force_[3] = { 0., 0., 0. }; + double robot_torque_[3] = { 0., 0., 0. }; - // Interfaces - hardware_interface::JointStateInterface joint_state_interface_; - hardware_interface::ForceTorqueSensorInterface force_torque_interface_; - hardware_interface::PositionJointInterface position_joint_interface_; - hardware_interface::VelocityJointInterface velocity_joint_interface_; - bool velocity_interface_running_; - bool position_interface_running_; - // Shared memory - std::vector joint_names_; - std::vector joint_position_; - std::vector joint_velocity_; - std::vector joint_effort_; - std::vector joint_position_command_; - std::vector joint_velocity_command_; - std::vector prev_joint_velocity_command_; - std::size_t num_joints_; - double robot_force_[3] = { 0., 0., 0. }; - double robot_torque_[3] = { 0., 0., 0. }; - - double max_vel_change_; - - // Robot API - UrDriver* robot_; + double max_vel_change_; + // Robot API + UrDriver* robot_; }; // class -}// namespace +} // namespace #endif diff --git a/include/ur_modern_driver/ur_realtime_communication.h b/include/ur_modern_driver/ur_realtime_communication.h index 9d6b445..61e5474 100644 --- a/include/ur_modern_driver/ur_realtime_communication.h +++ b/include/ur_modern_driver/ur_realtime_communication.h @@ -19,58 +19,56 @@ #ifndef UR_REALTIME_COMMUNICATION_H_ #define UR_REALTIME_COMMUNICATION_H_ -#include "robot_state_RT.h" #include "do_output.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "robot_state_RT.h" #include +#include #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include +#include +#include +#include +#include class UrRealtimeCommunication { private: - unsigned int safety_count_max_; - int sockfd_; - struct sockaddr_in serv_addr_; - struct hostent *server_; - std::string local_ip_; - bool keepalive_; - std::thread comThread_; - int flag_; - std::recursive_mutex command_string_lock_; - std::string command_; - unsigned int safety_count_; - void run(); - + unsigned int safety_count_max_; + int sockfd_; + struct sockaddr_in serv_addr_; + struct hostent* server_; + std::string local_ip_; + bool keepalive_; + std::thread comThread_; + int flag_; + std::recursive_mutex command_string_lock_; + std::string command_; + unsigned int safety_count_; + void run(); public: - bool connected_; - RobotStateRT* robot_state_; - - UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host, - unsigned int safety_count_max = 12); - bool start(); - void halt(); - void setSpeed(double q0, double q1, double q2, double q3, double q4, - double q5, double acc = 100.); - void addCommandToQueue(std::string inp); - void setSafetyCountMax(uint inp); - std::string getLocalIp(); + bool connected_; + RobotStateRT* robot_state_; + UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host, + unsigned int safety_count_max = 12); + bool start(); + void halt(); + void setSpeed(double q0, double q1, double q2, double q3, double q4, + double q5, double acc = 100.); + void addCommandToQueue(std::string inp); + void setSafetyCountMax(uint inp); + std::string getLocalIp(); }; #endif /* UR_REALTIME_COMMUNICATION_H_ */ diff --git a/src/do_output.cpp b/src/do_output.cpp index 048cce1..f4b0b40 100644 --- a/src/do_output.cpp +++ b/src/do_output.cpp @@ -18,40 +18,45 @@ #include "ur_modern_driver/do_output.h" -void print_debug(std::string inp) { +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) { +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) { +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) { +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) { +void print_fatal(std::string inp) +{ #ifdef ROS_BUILD - ROS_FATAL("%s", inp.c_str()); - ros::shutdown(); + ROS_FATAL("%s", inp.c_str()); + ros::shutdown(); #else - printf("FATAL: %s\n", inp.c_str()); - exit(1); + printf("FATAL: %s\n", inp.c_str()); + exit(1); #endif } diff --git a/src/robot_state.cpp b/src/robot_state.cpp index a7eb589..743c2c2 100644 --- a/src/robot_state.cpp +++ b/src/robot_state.cpp @@ -18,370 +18,391 @@ #include "ur_modern_driver/robot_state.h" -RobotState::RobotState(std::condition_variable& msg_cond) { - version_msg_.major_version = 0; - version_msg_.minor_version = 0; - new_data_available_ = false; - pMsg_cond_ = &msg_cond; - RobotState::setDisconnected(); - robot_mode_running_ = robotStateTypeV30::ROBOT_MODE_RUNNING; +RobotState::RobotState(std::condition_variable& msg_cond) +{ + version_msg_.major_version = 0; + version_msg_.minor_version = 0; + new_data_available_ = false; + pMsg_cond_ = &msg_cond; + RobotState::setDisconnected(); + robot_mode_running_ = robotStateTypeV30::ROBOT_MODE_RUNNING; } -double RobotState::ntohd(uint64_t nf) { - double x; - nf = be64toh(nf); - memcpy(&x, &nf, sizeof(x)); - return x; +double RobotState::ntohd(uint64_t nf) +{ + double x; + nf = be64toh(nf); + memcpy(&x, &nf, sizeof(x)); + return x; } -void RobotState::unpack(uint8_t* buf, unsigned int buf_length) { - /* Returns missing bytes to unpack a message, or 0 if all data was parsed */ - unsigned int offset = 0; - while (buf_length > offset) { - int len; - unsigned char message_type; - memcpy(&len, &buf[offset], sizeof(len)); - len = ntohl(len); - if (len + offset > buf_length) { - return; - } - memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type)); - switch (message_type) { - case messageType::ROBOT_MESSAGE: - RobotState::unpackRobotMessage(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType - break; - case messageType::ROBOT_STATE: - RobotState::unpackRobotState(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType - break; - case messageType::PROGRAM_STATE_MESSAGE: - //Don't do anything atm... - default: - break; - } - offset += len; - - } - return; +void RobotState::unpack(uint8_t* buf, unsigned int buf_length) +{ + /* Returns missing bytes to unpack a message, or 0 if all data was parsed */ + unsigned int offset = 0; + while (buf_length > offset) { + int len; + unsigned char message_type; + memcpy(&len, &buf[offset], sizeof(len)); + len = ntohl(len); + if (len + offset > buf_length) { + return; + } + memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type)); + switch (message_type) { + case messageType::ROBOT_MESSAGE: + RobotState::unpackRobotMessage(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType + break; + case messageType::ROBOT_STATE: + RobotState::unpackRobotState(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType + break; + case messageType::PROGRAM_STATE_MESSAGE: + //Don't do anything atm... + default: + break; + } + offset += len; + } + return; } -void RobotState::unpackRobotMessage(uint8_t * buf, unsigned int offset, - uint32_t len) { - offset += 5; - uint64_t timestamp; - int8_t source, robot_message_type; - memcpy(×tamp, &buf[offset], sizeof(timestamp)); - offset += sizeof(timestamp); - memcpy(&source, &buf[offset], sizeof(source)); - offset += sizeof(source); - memcpy(&robot_message_type, &buf[offset], sizeof(robot_message_type)); - offset += sizeof(robot_message_type); - switch (robot_message_type) { - case robotMessageType::ROBOT_MESSAGE_VERSION: - val_lock_.lock(); - version_msg_.timestamp = timestamp; - version_msg_.source = source; - version_msg_.robot_message_type = robot_message_type; - RobotState::unpackRobotMessageVersion(buf, offset, len); - val_lock_.unlock(); - break; - default: - break; - } - +void RobotState::unpackRobotMessage(uint8_t* buf, unsigned int offset, + uint32_t len) +{ + offset += 5; + uint64_t timestamp; + int8_t source, robot_message_type; + memcpy(×tamp, &buf[offset], sizeof(timestamp)); + offset += sizeof(timestamp); + memcpy(&source, &buf[offset], sizeof(source)); + offset += sizeof(source); + memcpy(&robot_message_type, &buf[offset], sizeof(robot_message_type)); + offset += sizeof(robot_message_type); + switch (robot_message_type) { + case robotMessageType::ROBOT_MESSAGE_VERSION: + val_lock_.lock(); + version_msg_.timestamp = timestamp; + version_msg_.source = source; + version_msg_.robot_message_type = robot_message_type; + RobotState::unpackRobotMessageVersion(buf, offset, len); + val_lock_.unlock(); + break; + default: + break; + } } -void RobotState::unpackRobotState(uint8_t * buf, unsigned int offset, - uint32_t len) { - offset += 5; - while (offset < len) { - int32_t length; - uint8_t package_type; - memcpy(&length, &buf[offset], sizeof(length)); - length = ntohl(length); - memcpy(&package_type, &buf[offset + sizeof(length)], - sizeof(package_type)); - switch (package_type) { - case packageType::ROBOT_MODE_DATA: - val_lock_.lock(); - RobotState::unpackRobotMode(buf, offset + 5); - val_lock_.unlock(); - break; - - case packageType::MASTERBOARD_DATA: - val_lock_.lock(); - RobotState::unpackRobotStateMasterboard(buf, offset + 5); - val_lock_.unlock(); - break; - default: - break; - } - offset += length; - } - new_data_available_ = true; - pMsg_cond_->notify_all(); +void RobotState::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(); + RobotState::unpackRobotMode(buf, offset + 5); + val_lock_.unlock(); + break; + case packageType::MASTERBOARD_DATA: + val_lock_.lock(); + RobotState::unpackRobotStateMasterboard(buf, offset + 5); + val_lock_.unlock(); + break; + default: + break; + } + offset += length; + } + new_data_available_ = true; + pMsg_cond_->notify_all(); } -void RobotState::unpackRobotMessageVersion(uint8_t * buf, unsigned int offset, - uint32_t len) { - memcpy(&version_msg_.project_name_size, &buf[offset], - sizeof(version_msg_.project_name_size)); - offset += sizeof(version_msg_.project_name_size); - memcpy(&version_msg_.project_name, &buf[offset], - sizeof(char) * version_msg_.project_name_size); - offset += version_msg_.project_name_size; - version_msg_.project_name[version_msg_.project_name_size] = '\0'; - memcpy(&version_msg_.major_version, &buf[offset], - sizeof(version_msg_.major_version)); - offset += sizeof(version_msg_.major_version); - memcpy(&version_msg_.minor_version, &buf[offset], - sizeof(version_msg_.minor_version)); - offset += sizeof(version_msg_.minor_version); - memcpy(&version_msg_.svn_revision, &buf[offset], - sizeof(version_msg_.svn_revision)); - offset += sizeof(version_msg_.svn_revision); - version_msg_.svn_revision = ntohl(version_msg_.svn_revision); - memcpy(&version_msg_.build_date, &buf[offset], sizeof(char) * len - offset); - version_msg_.build_date[len - offset] = '\0'; - if (version_msg_.major_version < 2) { - robot_mode_running_ = robotStateTypeV18::ROBOT_RUNNING_MODE; - } +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)); - offset += sizeof(robot_mode_.timestamp); - uint8_t tmp; - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isRobotConnected = true; - else - robot_mode_.isRobotConnected = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isRealRobotEnabled = true; - else - robot_mode_.isRealRobotEnabled = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - //printf("PowerOnRobot: %d\n", tmp); - if (tmp > 0) - robot_mode_.isPowerOnRobot = true; - else - robot_mode_.isPowerOnRobot = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isEmergencyStopped = true; - else - robot_mode_.isEmergencyStopped = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isProtectiveStopped = true; - else - robot_mode_.isProtectiveStopped = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isProgramRunning = true; - else - robot_mode_.isProgramRunning = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isProgramPaused = true; - else - robot_mode_.isProgramPaused = false; - offset += sizeof(tmp); - memcpy(&robot_mode_.robotMode, &buf[offset], sizeof(robot_mode_.robotMode)); - offset += sizeof(robot_mode_.robotMode); - uint64_t temp; - if (RobotState::getVersion() > 2.) { - memcpy(&robot_mode_.controlMode, &buf[offset], - sizeof(robot_mode_.controlMode)); - offset += sizeof(robot_mode_.controlMode); - memcpy(&temp, &buf[offset], sizeof(temp)); - offset += sizeof(temp); - robot_mode_.targetSpeedFraction = RobotState::ntohd(temp); - } - memcpy(&temp, &buf[offset], sizeof(temp)); - offset += sizeof(temp); - robot_mode_.speedScaling = RobotState::ntohd(temp); +void RobotState::unpackRobotMode(uint8_t* buf, unsigned int offset) +{ + memcpy(&robot_mode_.timestamp, &buf[offset], sizeof(robot_mode_.timestamp)); + offset += sizeof(robot_mode_.timestamp); + uint8_t tmp; + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isRobotConnected = true; + else + robot_mode_.isRobotConnected = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isRealRobotEnabled = true; + else + robot_mode_.isRealRobotEnabled = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + //printf("PowerOnRobot: %d\n", tmp); + if (tmp > 0) + robot_mode_.isPowerOnRobot = true; + else + robot_mode_.isPowerOnRobot = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isEmergencyStopped = true; + else + robot_mode_.isEmergencyStopped = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isProtectiveStopped = true; + else + robot_mode_.isProtectiveStopped = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isProgramRunning = true; + else + robot_mode_.isProgramRunning = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isProgramPaused = true; + else + robot_mode_.isProgramPaused = false; + offset += sizeof(tmp); + memcpy(&robot_mode_.robotMode, &buf[offset], sizeof(robot_mode_.robotMode)); + offset += sizeof(robot_mode_.robotMode); + uint64_t temp; + if (RobotState::getVersion() > 2.) { + memcpy(&robot_mode_.controlMode, &buf[offset], + sizeof(robot_mode_.controlMode)); + offset += sizeof(robot_mode_.controlMode); + memcpy(&temp, &buf[offset], sizeof(temp)); + offset += sizeof(temp); + robot_mode_.targetSpeedFraction = RobotState::ntohd(temp); + } + memcpy(&temp, &buf[offset], sizeof(temp)); + offset += sizeof(temp); + robot_mode_.speedScaling = RobotState::ntohd(temp); } -void RobotState::unpackRobotStateMasterboard(uint8_t * buf, - unsigned int offset) { - if (RobotState::getVersion() < 3.0) { - int16_t digital_input_bits, digital_output_bits; - memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits)); - offset += sizeof(digital_input_bits); - memcpy(&digital_output_bits, &buf[offset], sizeof(digital_output_bits)); - offset += sizeof(digital_output_bits); - mb_data_.digitalInputBits = ntohs(digital_input_bits); - mb_data_.digitalOutputBits = ntohs(digital_output_bits); - } else { - memcpy(&mb_data_.digitalInputBits, &buf[offset], - sizeof(mb_data_.digitalInputBits)); - offset += sizeof(mb_data_.digitalInputBits); - mb_data_.digitalInputBits = ntohl(mb_data_.digitalInputBits); - memcpy(&mb_data_.digitalOutputBits, &buf[offset], - sizeof(mb_data_.digitalOutputBits)); - offset += sizeof(mb_data_.digitalOutputBits); - mb_data_.digitalOutputBits = ntohl(mb_data_.digitalOutputBits); - } +void RobotState::unpackRobotStateMasterboard(uint8_t* buf, + unsigned int offset) +{ + if (RobotState::getVersion() < 3.0) { + int16_t digital_input_bits, digital_output_bits; + memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits)); + offset += sizeof(digital_input_bits); + memcpy(&digital_output_bits, &buf[offset], sizeof(digital_output_bits)); + offset += sizeof(digital_output_bits); + mb_data_.digitalInputBits = ntohs(digital_input_bits); + mb_data_.digitalOutputBits = ntohs(digital_output_bits); + } else { + memcpy(&mb_data_.digitalInputBits, &buf[offset], + sizeof(mb_data_.digitalInputBits)); + offset += sizeof(mb_data_.digitalInputBits); + mb_data_.digitalInputBits = ntohl(mb_data_.digitalInputBits); + memcpy(&mb_data_.digitalOutputBits, &buf[offset], + sizeof(mb_data_.digitalOutputBits)); + offset += sizeof(mb_data_.digitalOutputBits); + mb_data_.digitalOutputBits = ntohl(mb_data_.digitalOutputBits); + } - 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_.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_.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_.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); - } - - } + 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; - val_lock_.lock(); - ver = version_msg_.major_version + 0.1 * version_msg_.minor_version - + .0000001 * version_msg_.svn_revision; - val_lock_.unlock(); - return ver; - +double RobotState::getVersion() +{ + double ver; + val_lock_.lock(); + ver = version_msg_.major_version + 0.1 * version_msg_.minor_version + + .0000001 * version_msg_.svn_revision; + val_lock_.unlock(); + return ver; } -void RobotState::finishedReading() { - new_data_available_ = false; +void RobotState::finishedReading() +{ + new_data_available_ = false; } -bool RobotState::getNewDataAvailable() { - return new_data_available_; +bool RobotState::getNewDataAvailable() +{ + return new_data_available_; } -int RobotState::getDigitalInputBits() { - return mb_data_.digitalInputBits; +int RobotState::getDigitalInputBits() +{ + return mb_data_.digitalInputBits; } -int RobotState::getDigitalOutputBits() { - return mb_data_.digitalOutputBits; +int RobotState::getDigitalOutputBits() +{ + return mb_data_.digitalOutputBits; } -double RobotState::getAnalogInput0() { - return mb_data_.analogInput0; +double RobotState::getAnalogInput0() +{ + return mb_data_.analogInput0; } -double RobotState::getAnalogInput1() { - return mb_data_.analogInput1; +double RobotState::getAnalogInput1() +{ + return mb_data_.analogInput1; } -double RobotState::getAnalogOutput0() { - return mb_data_.analogOutput0; - +double RobotState::getAnalogOutput0() +{ + return mb_data_.analogOutput0; } -double RobotState::getAnalogOutput1() { - return mb_data_.analogOutput1; +double RobotState::getAnalogOutput1() +{ + return mb_data_.analogOutput1; } -bool RobotState::isRobotConnected() { - return robot_mode_.isRobotConnected; +bool RobotState::isRobotConnected() +{ + return robot_mode_.isRobotConnected; } -bool RobotState::isRealRobotEnabled() { - return robot_mode_.isRealRobotEnabled; +bool RobotState::isRealRobotEnabled() +{ + return robot_mode_.isRealRobotEnabled; } -bool RobotState::isPowerOnRobot() { - return robot_mode_.isPowerOnRobot; +bool RobotState::isPowerOnRobot() +{ + return robot_mode_.isPowerOnRobot; } -bool RobotState::isEmergencyStopped() { - return robot_mode_.isEmergencyStopped; +bool RobotState::isEmergencyStopped() +{ + return robot_mode_.isEmergencyStopped; } -bool RobotState::isProtectiveStopped() { - return robot_mode_.isProtectiveStopped; +bool RobotState::isProtectiveStopped() +{ + return robot_mode_.isProtectiveStopped; } -bool RobotState::isProgramRunning() { - return robot_mode_.isProgramRunning; +bool RobotState::isProgramRunning() +{ + return robot_mode_.isProgramRunning; } -bool RobotState::isProgramPaused() { - return robot_mode_.isProgramPaused; +bool RobotState::isProgramPaused() +{ + return robot_mode_.isProgramPaused; } -unsigned char RobotState::getRobotMode() { - return robot_mode_.robotMode; +unsigned char RobotState::getRobotMode() +{ + return robot_mode_.robotMode; } -bool RobotState::isReady() { - if (robot_mode_.robotMode == robot_mode_running_) { - return true; - } - return false; +bool RobotState::isReady() +{ + if (robot_mode_.robotMode == robot_mode_running_) { + return true; + } + return false; } -void RobotState::setDisconnected() { - robot_mode_.isRobotConnected = false; - robot_mode_.isRealRobotEnabled = false; - robot_mode_.isPowerOnRobot = false; +void RobotState::setDisconnected() +{ + robot_mode_.isRobotConnected = false; + robot_mode_.isRealRobotEnabled = false; + robot_mode_.isPowerOnRobot = false; } diff --git a/src/robot_state_RT.cpp b/src/robot_state_RT.cpp index b170a04..46467fe 100644 --- a/src/robot_state_RT.cpp +++ b/src/robot_state_RT.cpp @@ -18,420 +18,458 @@ #include "ur_modern_driver/robot_state_RT.h" -RobotStateRT::RobotStateRT(std::condition_variable& msg_cond) { - version_ = 0.0; - time_ = 0.0; - q_target_.assign(6, 0.0); - qd_target_.assign(6, 0.0); - qdd_target_.assign(6, 0.0); - i_target_.assign(6, 0.0); - m_target_.assign(6, 0.0); - q_actual_.assign(6, 0.0); - qd_actual_.assign(6, 0.0); - i_actual_.assign(6, 0.0); - i_control_.assign(6, 0.0); - tool_vector_actual_.assign(6, 0.0); - tcp_speed_actual_.assign(6, 0.0); - tcp_force_.assign(6, 0.0); - tool_vector_target_.assign(6, 0.0); - tcp_speed_target_.assign(6, 0.0); - digital_input_bits_.assign(64, false); - motor_temperatures_.assign(6, 0.0); - controller_timer_ = 0.0; - robot_mode_ = 0.0; - joint_modes_.assign(6, 0.0); - safety_mode_ = 0.0; - tool_accelerometer_values_.assign(3, 0.0); - speed_scaling_ = 0.0; - linear_momentum_norm_ = 0.0; - v_main_ = 0.0; - v_robot_ = 0.0; - i_robot_ = 0.0; - v_actual_.assign(6, 0.0); - data_published_ = false; - controller_updated_ = false; - pMsg_cond_ = &msg_cond; +RobotStateRT::RobotStateRT(std::condition_variable& msg_cond) +{ + version_ = 0.0; + time_ = 0.0; + q_target_.assign(6, 0.0); + qd_target_.assign(6, 0.0); + qdd_target_.assign(6, 0.0); + i_target_.assign(6, 0.0); + m_target_.assign(6, 0.0); + q_actual_.assign(6, 0.0); + qd_actual_.assign(6, 0.0); + i_actual_.assign(6, 0.0); + i_control_.assign(6, 0.0); + tool_vector_actual_.assign(6, 0.0); + tcp_speed_actual_.assign(6, 0.0); + tcp_force_.assign(6, 0.0); + tool_vector_target_.assign(6, 0.0); + tcp_speed_target_.assign(6, 0.0); + digital_input_bits_.assign(64, false); + motor_temperatures_.assign(6, 0.0); + controller_timer_ = 0.0; + robot_mode_ = 0.0; + joint_modes_.assign(6, 0.0); + safety_mode_ = 0.0; + tool_accelerometer_values_.assign(3, 0.0); + speed_scaling_ = 0.0; + linear_momentum_norm_ = 0.0; + v_main_ = 0.0; + v_robot_ = 0.0; + i_robot_ = 0.0; + v_actual_.assign(6, 0.0); + data_published_ = false; + controller_updated_ = false; + pMsg_cond_ = &msg_cond; } -RobotStateRT::~RobotStateRT() { - /* Make sure nobody is waiting after this thread is destroyed */ - data_published_ = true; - controller_updated_ = true; - pMsg_cond_->notify_all(); +RobotStateRT::~RobotStateRT() +{ + /* Make sure nobody is waiting after this thread is destroyed */ + data_published_ = true; + controller_updated_ = true; + pMsg_cond_->notify_all(); } -void RobotStateRT::setDataPublished() { - data_published_ = false; +void RobotStateRT::setDataPublished() +{ + data_published_ = false; } -bool RobotStateRT::getDataPublished() { - return data_published_; +bool RobotStateRT::getDataPublished() +{ + return data_published_; } -void RobotStateRT::setControllerUpdated() { - controller_updated_ = false; +void RobotStateRT::setControllerUpdated() +{ + controller_updated_ = false; } -bool RobotStateRT::getControllerUpdated() { - return controller_updated_; +bool RobotStateRT::getControllerUpdated() +{ + return controller_updated_; } -double RobotStateRT::ntohd(uint64_t nf) { - double x; - nf = be64toh(nf); - memcpy(&x, &nf, sizeof(x)); - return x; +double RobotStateRT::ntohd(uint64_t nf) +{ + double x; + nf = be64toh(nf); + memcpy(&x, &nf, sizeof(x)); + return x; } -std::vector RobotStateRT::unpackVector(uint8_t * buf, int start_index, - int nr_of_vals) { - uint64_t q; - std::vector ret; - for (int i = 0; i < nr_of_vals; i++) { - memcpy(&q, &buf[start_index + i * sizeof(q)], sizeof(q)); - ret.push_back(ntohd(q)); - } - return ret; +std::vector RobotStateRT::unpackVector(uint8_t* buf, int start_index, + int nr_of_vals) +{ + uint64_t q; + std::vector ret; + for (int i = 0; i < nr_of_vals; i++) { + memcpy(&q, &buf[start_index + i * sizeof(q)], sizeof(q)); + ret.push_back(ntohd(q)); + } + return ret; } -std::vector RobotStateRT::unpackDigitalInputBits(int64_t data) { - std::vector ret; - for (int i = 0; i < 64; i++) { - ret.push_back((data & (1 << i)) >> i); - } - return ret; +std::vector RobotStateRT::unpackDigitalInputBits(int64_t data) +{ + std::vector ret; + for (int i = 0; i < 64; i++) { + ret.push_back((data & (1 << i)) >> i); + } + return ret; } -void RobotStateRT::setVersion(double ver) { - val_lock_.lock(); - version_ = ver; - val_lock_.unlock(); +void RobotStateRT::setVersion(double ver) +{ + val_lock_.lock(); + version_ = ver; + val_lock_.unlock(); } -double RobotStateRT::getVersion() { - double ret; - val_lock_.lock(); - ret = version_; - val_lock_.unlock(); - return ret; +double RobotStateRT::getVersion() +{ + double ret; + val_lock_.lock(); + ret = version_; + val_lock_.unlock(); + return ret; } -double RobotStateRT::getTime() { - double ret; - val_lock_.lock(); - ret = time_; - val_lock_.unlock(); - return ret; +double RobotStateRT::getTime() +{ + double ret; + val_lock_.lock(); + ret = time_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getQTarget() { - std::vector ret; - val_lock_.lock(); - ret = q_target_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getQTarget() +{ + std::vector ret; + val_lock_.lock(); + ret = q_target_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getQdTarget() { - std::vector ret; - val_lock_.lock(); - ret = qd_target_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getQdTarget() +{ + std::vector ret; + val_lock_.lock(); + ret = qd_target_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getQddTarget() { - std::vector ret; - val_lock_.lock(); - ret = qdd_target_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getQddTarget() +{ + std::vector ret; + val_lock_.lock(); + ret = qdd_target_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getITarget() { - std::vector ret; - val_lock_.lock(); - ret = i_target_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getITarget() +{ + std::vector ret; + val_lock_.lock(); + ret = i_target_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getMTarget() { - std::vector ret; - val_lock_.lock(); - ret = m_target_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getMTarget() +{ + std::vector ret; + val_lock_.lock(); + ret = m_target_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getQActual() { - std::vector ret; - val_lock_.lock(); - ret = q_actual_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getQActual() +{ + std::vector ret; + val_lock_.lock(); + ret = q_actual_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getQdActual() { - std::vector ret; - val_lock_.lock(); - ret = qd_actual_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getQdActual() +{ + std::vector ret; + val_lock_.lock(); + ret = qd_actual_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getIActual() { - std::vector ret; - val_lock_.lock(); - ret = i_actual_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getIActual() +{ + std::vector ret; + val_lock_.lock(); + ret = i_actual_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getIControl() { - std::vector ret; - val_lock_.lock(); - ret = i_control_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getIControl() +{ + std::vector ret; + val_lock_.lock(); + ret = i_control_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getToolVectorActual() { - std::vector ret; - val_lock_.lock(); - ret = tool_vector_actual_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getToolVectorActual() +{ + std::vector ret; + val_lock_.lock(); + ret = tool_vector_actual_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getTcpSpeedActual() { - std::vector ret; - val_lock_.lock(); - ret = tcp_speed_actual_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getTcpSpeedActual() +{ + std::vector ret; + val_lock_.lock(); + ret = tcp_speed_actual_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getTcpForce() { - std::vector ret; - val_lock_.lock(); - ret = tcp_force_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getTcpForce() +{ + std::vector ret; + val_lock_.lock(); + ret = tcp_force_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getToolVectorTarget() { - std::vector ret; - val_lock_.lock(); - ret = tool_vector_target_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getToolVectorTarget() +{ + std::vector ret; + val_lock_.lock(); + ret = tool_vector_target_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getTcpSpeedTarget() { - std::vector ret; - val_lock_.lock(); - ret = tcp_speed_target_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getTcpSpeedTarget() +{ + std::vector ret; + val_lock_.lock(); + ret = tcp_speed_target_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getDigitalInputBits() { - std::vector ret; - val_lock_.lock(); - ret = digital_input_bits_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getDigitalInputBits() +{ + std::vector ret; + val_lock_.lock(); + ret = digital_input_bits_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getMotorTemperatures() { - std::vector ret; - val_lock_.lock(); - ret = motor_temperatures_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getMotorTemperatures() +{ + std::vector ret; + val_lock_.lock(); + ret = motor_temperatures_; + val_lock_.unlock(); + return ret; } -double RobotStateRT::getControllerTimer() { - double ret; - val_lock_.lock(); - ret = controller_timer_; - val_lock_.unlock(); - return ret; +double RobotStateRT::getControllerTimer() +{ + double ret; + val_lock_.lock(); + ret = controller_timer_; + val_lock_.unlock(); + return ret; } -double RobotStateRT::getRobotMode() { - double ret; - val_lock_.lock(); - ret = robot_mode_; - val_lock_.unlock(); - return ret; +double RobotStateRT::getRobotMode() +{ + double ret; + val_lock_.lock(); + ret = robot_mode_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getJointModes() { - std::vector ret; - val_lock_.lock(); - ret = joint_modes_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getJointModes() +{ + std::vector ret; + val_lock_.lock(); + ret = joint_modes_; + val_lock_.unlock(); + return ret; } -double RobotStateRT::getSafety_mode() { - double ret; - val_lock_.lock(); - ret = safety_mode_; - val_lock_.unlock(); - return ret; +double RobotStateRT::getSafety_mode() +{ + double ret; + val_lock_.lock(); + ret = safety_mode_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getToolAccelerometerValues() { - std::vector ret; - val_lock_.lock(); - ret = tool_accelerometer_values_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getToolAccelerometerValues() +{ + std::vector ret; + val_lock_.lock(); + ret = tool_accelerometer_values_; + val_lock_.unlock(); + return ret; } -double RobotStateRT::getSpeedScaling() { - double ret; - val_lock_.lock(); - ret = speed_scaling_; - val_lock_.unlock(); - return ret; +double RobotStateRT::getSpeedScaling() +{ + double ret; + val_lock_.lock(); + ret = speed_scaling_; + val_lock_.unlock(); + return ret; } -double RobotStateRT::getLinearMomentumNorm() { - double ret; - val_lock_.lock(); - ret = linear_momentum_norm_; - val_lock_.unlock(); - return ret; +double RobotStateRT::getLinearMomentumNorm() +{ + double ret; + val_lock_.lock(); + ret = linear_momentum_norm_; + val_lock_.unlock(); + return ret; } -double RobotStateRT::getVMain() { - double ret; - val_lock_.lock(); - ret = v_main_; - val_lock_.unlock(); - return ret; +double RobotStateRT::getVMain() +{ + double ret; + val_lock_.lock(); + ret = v_main_; + val_lock_.unlock(); + return ret; } -double RobotStateRT::getVRobot() { - double ret; - val_lock_.lock(); - ret = v_robot_; - val_lock_.unlock(); - return ret; +double RobotStateRT::getVRobot() +{ + double ret; + val_lock_.lock(); + ret = v_robot_; + val_lock_.unlock(); + return ret; } -double RobotStateRT::getIRobot() { - double ret; - val_lock_.lock(); - ret = i_robot_; - val_lock_.unlock(); - return ret; +double RobotStateRT::getIRobot() +{ + double ret; + val_lock_.lock(); + ret = i_robot_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getVActual() { - std::vector ret; - val_lock_.lock(); - ret = v_actual_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getVActual() +{ + std::vector ret; + val_lock_.lock(); + ret = v_actual_; + val_lock_.unlock(); + return ret; } -void RobotStateRT::unpack(uint8_t * buf) { - int64_t digital_input_bits; - uint64_t unpack_to; - uint16_t offset = 0; - val_lock_.lock(); - int len; - memcpy(&len, &buf[offset], sizeof(len)); +void RobotStateRT::unpack(uint8_t* buf) +{ + int64_t digital_input_bits; + uint64_t unpack_to; + uint16_t offset = 0; + val_lock_.lock(); + int len; + memcpy(&len, &buf[offset], sizeof(len)); - offset += sizeof(len); - len = ntohl(len); + offset += sizeof(len); + len = ntohl(len); - //Check the correct message length is received - bool len_good = true; - if (version_ >= 1.6 && version_ < 1.7) { //v1.6 - if (len != 756) - len_good = false; - } else if (version_ >= 1.7 && version_ < 1.8) { //v1.7 - if (len != 764) - len_good = false; - } else if (version_ >= 1.8 && version_ < 1.9) { //v1.8 - if (len != 812) - len_good = false; - } else if (version_ >= 3.0 && version_ < 3.2) { //v3.0 & v3.1 - if (len != 1044) - len_good = false; - } else if (version_ >= 3.2 && version_ < 3.3) { //v3.2 - if (len != 1060) - len_good = false; - } + //Check the correct message length is received + bool len_good = true; + if (version_ >= 1.6 && version_ < 1.7) { //v1.6 + if (len != 756) + len_good = false; + } else if (version_ >= 1.7 && version_ < 1.8) { //v1.7 + if (len != 764) + len_good = false; + } else if (version_ >= 1.8 && version_ < 1.9) { //v1.8 + if (len != 812) + len_good = false; + } else if (version_ >= 3.0 && version_ < 3.2) { //v3.0 & v3.1 + if (len != 1044) + len_good = false; + } else if (version_ >= 3.2 && version_ < 3.3) { //v3.2 + if (len != 1060) + len_good = false; + } - if (!len_good) { - printf("Wrong length of message on RT interface: %i\n", len); - val_lock_.unlock(); - return; - } + if (!len_good) { + printf("Wrong length of message on RT interface: %i\n", len); + val_lock_.unlock(); + return; + } - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - time_ = RobotStateRT::ntohd(unpack_to); - offset += sizeof(double); - q_target_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - qd_target_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - qdd_target_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - i_target_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - m_target_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - q_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - qd_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - i_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - if (version_ <= 1.9) { - if (version_ > 1.6) - tool_accelerometer_values_ = unpackVector(buf, offset, 3); - offset += sizeof(double) * (3 + 15); - tcp_force_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tool_vector_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tcp_speed_actual_ = unpackVector(buf, offset, 6); - } else { - i_control_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tool_vector_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tcp_speed_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tcp_force_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tool_vector_target_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tcp_speed_target_ = unpackVector(buf, offset, 6); - } - offset += sizeof(double) * 6; - - memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits)); - digital_input_bits_ = unpackDigitalInputBits(be64toh(digital_input_bits)); - offset += sizeof(double); - motor_temperatures_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - controller_timer_ = ntohd(unpack_to); - if (version_ > 1.6) { - offset += sizeof(double) * 2; - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - robot_mode_ = ntohd(unpack_to); - if (version_ > 1.7) { - offset += sizeof(double); - joint_modes_ = unpackVector(buf, offset, 6); - } - } - if (version_ > 1.8) { - offset += sizeof(double) * 6; - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - safety_mode_ = ntohd(unpack_to); - offset += sizeof(double); - tool_accelerometer_values_ = unpackVector(buf, offset, 3); - offset += sizeof(double) * 3; - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - speed_scaling_ = ntohd(unpack_to); - offset += sizeof(double); - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - linear_momentum_norm_ = ntohd(unpack_to); - offset += sizeof(double); - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - v_main_ = ntohd(unpack_to); - offset += sizeof(double); - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - v_robot_ = ntohd(unpack_to); - offset += sizeof(double); - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - i_robot_ = ntohd(unpack_to); - offset += sizeof(double); - v_actual_ = unpackVector(buf, offset, 6); - } - val_lock_.unlock(); - controller_updated_ = true; - 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(); } - diff --git a/src/ros/rt_publisher.cpp b/src/ros/rt_publisher.cpp index c0db690..af5a4a4 100644 --- a/src/ros/rt_publisher.cpp +++ b/src/ros/rt_publisher.cpp @@ -1,17 +1,18 @@ #include "ur_modern_driver/ros/rt_publisher.h" -bool RTPublisher::publish_joints(RTShared &packet, ros::Time &t) { - sensor_msgs::JointState joint_msg; +bool RTPublisher::publish_joints(RTShared& packet, ros::Time& t) +{ + sensor_msgs::JointState joint_msg; joint_msg.header.stamp = t; joint_msg.name = _joint_names; - for(auto const& q : packet.q_actual) { + for (auto const& q : packet.q_actual) { joint_msg.position.push_back(q); } - for(auto const& qd : packet.qd_actual) { + for (auto const& qd : packet.qd_actual) { joint_msg.velocity.push_back(qd); } - for(auto const& i : packet.i_actual) { + for (auto const& i : packet.i_actual) { joint_msg.effort.push_back(i); } @@ -20,7 +21,8 @@ bool RTPublisher::publish_joints(RTShared &packet, ros::Time &t) { return true; } -bool RTPublisher::publish_wrench(RTShared &packet, ros::Time &t) { +bool RTPublisher::publish_wrench(RTShared& packet, ros::Time& t) +{ geometry_msgs::WrenchStamped wrench_msg; wrench_msg.header.stamp = t; @@ -36,7 +38,8 @@ bool RTPublisher::publish_wrench(RTShared &packet, ros::Time &t) { return true; } -bool RTPublisher::publish_tool(RTShared &packet, ros::Time &t) { +bool RTPublisher::publish_tool(RTShared& packet, ros::Time& t) +{ geometry_msgs::TwistStamped tool_twist; tool_twist.header.stamp = t; @@ -55,20 +58,25 @@ bool RTPublisher::publish_tool(RTShared &packet, ros::Time &t) { return true; } -bool RTPublisher::publish(RTShared &packet) { +bool RTPublisher::publish(RTShared& packet) +{ ros::Time time = ros::Time::now(); return publish_joints(packet, time) && publish_wrench(packet, time) && publish_tool(packet, time); } -bool RTPublisher::consume(RTState_V1_6__7 &state) { +bool RTPublisher::consume(RTState_V1_6__7& state) +{ return publish(state); } -bool RTPublisher::consume(RTState_V1_8 &state) { +bool RTPublisher::consume(RTState_V1_8& state) +{ return publish(state); } -bool RTPublisher::consume(RTState_V3_0__1 &state) { +bool RTPublisher::consume(RTState_V3_0__1& state) +{ return publish(state); } -bool RTPublisher::consume(RTState_V3_2__3 &state) { +bool RTPublisher::consume(RTState_V3_2__3& state) +{ return publish(state); } \ No newline at end of file diff --git a/src/ros_main.cpp b/src/ros_main.cpp index c2dc749..48cf6b6 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -1,18 +1,18 @@ -#include -#include #include -#include +#include #include +#include +#include #include "ur_modern_driver/log.h" #include "ur_modern_driver/pipeline.h" -#include "ur_modern_driver/ur/producer.h" -#include "ur_modern_driver/ur/state.h" -#include "ur_modern_driver/ur/rt_state.h" -#include "ur_modern_driver/ur/parser.h" -#include "ur_modern_driver/ur/messages.h" -#include "ur_modern_driver/ur/factory.h" #include "ur_modern_driver/ros/rt_publisher.h" +#include "ur_modern_driver/ur/factory.h" +#include "ur_modern_driver/ur/messages.h" +#include "ur_modern_driver/ur/parser.h" +#include "ur_modern_driver/ur/producer.h" +#include "ur_modern_driver/ur/rt_state.h" +#include "ur_modern_driver/ur/state.h" static const std::string IP_ADDR_ARG("~robot_ip_address"); static const std::string REVERSE_PORT_ARG("~reverse_port"); @@ -21,48 +21,48 @@ 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 { public: - std::string host; - std::string prefix; - std::string base_frame; - std::string tool_frame; - int32_t reverse_port; - bool use_sim_time; + std::string host; + std::string prefix; + std::string base_frame; + std::string tool_frame; + int32_t reverse_port; + bool use_sim_time; }; -bool parse_args(ProgArgs &args) { - if(!ros::param::get(IP_ADDR_ARG, args.host)) { - LOG_ERROR("robot_ip_address parameter must be set!"); - return false; - } - ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001)); - ros::param::param(SIM_TIME_ARG, args.use_sim_time, false); - ros::param::param(PREFIX_ARG, args.prefix, std::string()); - ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link"); - ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller"); - return true; +bool parse_args(ProgArgs& args) +{ + if (!ros::param::get(IP_ADDR_ARG, args.host)) { + LOG_ERROR("robot_ip_address parameter must be set!"); + return false; + } + ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001)); + ros::param::param(SIM_TIME_ARG, args.use_sim_time, false); + ros::param::param(PREFIX_ARG, args.prefix, std::string()); + ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link"); + ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller"); + return true; } - -int main(int argc, char **argv) { +int main(int argc, char** argv) +{ ros::init(argc, argv, "ur_driver"); - ProgArgs args; - if(!parse_args(args)) { - return EXIT_FAILURE; - } + ProgArgs args; + if (!parse_args(args)) { + return EXIT_FAILURE; + } - URFactory factory(args.host); + URFactory factory(args.host); - auto parser = factory.get_rt_parser(); + auto parser = factory.get_rt_parser(); - URStream s2(args.host, 30003); - URProducer p2(s2, *parser); + URStream s2(args.host, 30003); + URProducer p2(s2, *parser); - /* + /* p2.setup_producer(); while(true) { @@ -77,18 +77,17 @@ int main(int argc, char **argv) { p2.teardown_producer(); */ - RTPublisher pub(args.prefix, args.base_frame); + RTPublisher pub(args.prefix, args.base_frame); - Pipeline pl(p2, pub); + Pipeline pl(p2, pub); - pl.run(); + pl.run(); - while(ros::ok()) { - std::this_thread::sleep_for(std::chrono::milliseconds(33)); - } + while (ros::ok()) { + std::this_thread::sleep_for(std::chrono::milliseconds(33)); + } - pl.stop(); + pl.stop(); - - return EXIT_SUCCESS; + return EXIT_SUCCESS; } \ No newline at end of file diff --git a/src/ur/master_board.cpp b/src/ur/master_board.cpp index c8d947b..046d0ee 100644 --- a/src/ur/master_board.cpp +++ b/src/ur/master_board.cpp @@ -1,11 +1,12 @@ #include "ur_modern_driver/ur/master_board.h" #include "ur_modern_driver/ur/consumer.h" -bool SharedMasterBoardData::parse_with(BinParser &bp) { +bool SharedMasterBoardData::parse_with(BinParser& bp) +{ bp.parse(analog_input_range0); bp.parse(analog_input_range1); bp.parse(analog_input0); - bp.parse(analog_input1); + bp.parse(analog_input1); bp.parse(analog_output_domain0); bp.parse(analog_output_domain1); bp.parse(analog_output0); @@ -17,8 +18,9 @@ bool SharedMasterBoardData::parse_with(BinParser &bp) { return true; } -bool MasterBoardData_V1_X::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool MasterBoardData_V1_X::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; bp.parse(digital_input_bits); @@ -30,8 +32,8 @@ bool MasterBoardData_V1_X::parse_with(BinParser &bp) { bp.parse(master_on_off_state); bp.parse(euromap67_interface_installed); - if(euromap67_interface_installed) { - if(!bp.check_size(MasterBoardData_V1_X::EURO_SIZE)) + if (euromap67_interface_installed) { + if (!bp.check_size(MasterBoardData_V1_X::EURO_SIZE)) return false; bp.parse(euromap_voltage); @@ -41,23 +43,24 @@ bool MasterBoardData_V1_X::parse_with(BinParser &bp) { return true; } -bool MasterBoardData_V3_0__1::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool MasterBoardData_V3_0__1::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; bp.parse(digital_input_bits); bp.parse(digital_output_bits); - + SharedMasterBoardData::parse_with(bp); bp.parse(safety_mode); bp.parse(in_reduced_mode); bp.parse(euromap67_interface_installed); - if(euromap67_interface_installed) { - if(!bp.check_size(MasterBoardData_V3_0__1::EURO_SIZE)) + if (euromap67_interface_installed) { + if (!bp.check_size(MasterBoardData_V3_0__1::EURO_SIZE)) return false; - + bp.parse(euromap_voltage); bp.parse(euromap_current); } @@ -67,10 +70,9 @@ bool MasterBoardData_V3_0__1::parse_with(BinParser &bp) { return true; } - - -bool MasterBoardData_V3_2::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool MasterBoardData_V3_2::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; MasterBoardData_V3_0__1::parse_with(bp); @@ -81,16 +83,15 @@ bool MasterBoardData_V3_2::parse_with(BinParser &bp) { return true; } - - - - -bool MasterBoardData_V1_X::consume_with(URStatePacketConsumer &consumer) { +bool MasterBoardData_V1_X::consume_with(URStatePacketConsumer& consumer) +{ return consumer.consume(*this); } -bool MasterBoardData_V3_0__1::consume_with(URStatePacketConsumer &consumer) { +bool MasterBoardData_V3_0__1::consume_with(URStatePacketConsumer& consumer) +{ return consumer.consume(*this); } -bool MasterBoardData_V3_2::consume_with(URStatePacketConsumer &consumer) { +bool MasterBoardData_V3_2::consume_with(URStatePacketConsumer& consumer) +{ return consumer.consume(*this); } \ No newline at end of file diff --git a/src/ur/messages.cpp b/src/ur/messages.cpp index e079f8d..b532792 100644 --- a/src/ur/messages.cpp +++ b/src/ur/messages.cpp @@ -1,9 +1,9 @@ #include "ur_modern_driver/ur/messages.h" #include "ur_modern_driver/ur/consumer.h" +bool VersionMessage::parse_with(BinParser& bp) +{ -bool VersionMessage::parse_with(BinParser &bp) { - bp.parse(project_name); bp.parse(major_version); bp.parse(minor_version); @@ -11,10 +11,10 @@ bool VersionMessage::parse_with(BinParser &bp) { bp.consume(sizeof(uint32_t)); //undocumented field?? bp.parse_remainder(build_date); - return true; //not really possible to check dynamic size packets + return true; //not really possible to check dynamic size packets } - -bool VersionMessage::consume_with(URMessagePacketConsumer &consumer) { +bool VersionMessage::consume_with(URMessagePacketConsumer& consumer) +{ return consumer.consume(*this); } \ No newline at end of file diff --git a/src/ur/robot_mode.cpp b/src/ur/robot_mode.cpp index 1c62406..be0770f 100644 --- a/src/ur/robot_mode.cpp +++ b/src/ur/robot_mode.cpp @@ -1,8 +1,8 @@ #include "ur_modern_driver/ur/robot_mode.h" #include "ur_modern_driver/ur/consumer.h" - -bool SharedRobotModeData::parse_with(BinParser &bp) { +bool SharedRobotModeData::parse_with(BinParser& bp) +{ bp.parse(timestamp); bp.parse(physical_robot_connected); bp.parse(real_robot_enabled); @@ -11,9 +11,9 @@ bool SharedRobotModeData::parse_with(BinParser &bp) { return true; } - -bool RobotModeData_V1_X::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool RobotModeData_V1_X::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; SharedRobotModeData::parse_with(bp); @@ -27,13 +27,13 @@ bool RobotModeData_V1_X::parse_with(BinParser &bp) { return true; } - -bool RobotModeData_V3_0__1::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool RobotModeData_V3_0__1::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; SharedRobotModeData::parse_with(bp); - + bp.parse(protective_stopped); bp.parse(program_running); bp.parse(program_paused); @@ -45,8 +45,9 @@ bool RobotModeData_V3_0__1::parse_with(BinParser &bp) { return true; } -bool RobotModeData_V3_2::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool RobotModeData_V3_2::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; RobotModeData_V3_0__1::parse_with(bp); @@ -56,15 +57,15 @@ bool RobotModeData_V3_2::parse_with(BinParser &bp) { return true; } - - - -bool RobotModeData_V1_X::consume_with(URStatePacketConsumer &consumer) { +bool RobotModeData_V1_X::consume_with(URStatePacketConsumer& consumer) +{ return consumer.consume(*this); } -bool RobotModeData_V3_0__1::consume_with(URStatePacketConsumer &consumer) { +bool RobotModeData_V3_0__1::consume_with(URStatePacketConsumer& consumer) +{ return consumer.consume(*this); } -bool RobotModeData_V3_2::consume_with(URStatePacketConsumer &consumer) { +bool RobotModeData_V3_2::consume_with(URStatePacketConsumer& consumer) +{ return consumer.consume(*this); } \ No newline at end of file diff --git a/src/ur/rt_state.cpp b/src/ur/rt_state.cpp index 596134d..f13aa73 100644 --- a/src/ur/rt_state.cpp +++ b/src/ur/rt_state.cpp @@ -1,8 +1,8 @@ #include "ur_modern_driver/ur/rt_state.h" #include "ur_modern_driver/ur/consumer.h" - -bool RTShared::parse_shared1(BinParser &bp) { +bool RTShared::parse_shared1(BinParser& bp) +{ bp.parse(time); bp.parse(q_target); bp.parse(qd_target); @@ -15,7 +15,8 @@ bool RTShared::parse_shared1(BinParser &bp) { return true; } -bool RTShared::parse_shared2(BinParser &bp) { +bool RTShared::parse_shared2(BinParser& bp) +{ bp.parse(digital_input); bp.parse(motor_temperatures); bp.parse(controller_time); @@ -23,13 +24,13 @@ bool RTShared::parse_shared2(BinParser &bp) { return true; } - -bool RTState_V1_6__7::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool RTState_V1_6__7::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; parse_shared1(bp); - + bp.parse(tool_accelerometer_values); bp.parse(tcp_force); bp.parse(tool_vector_actual); @@ -40,8 +41,9 @@ bool RTState_V1_6__7::parse_with(BinParser &bp) { return true; } -bool RTState_V1_8::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool RTState_V1_8::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; RTState_V1_6__7::parse_with(bp); @@ -51,8 +53,9 @@ bool RTState_V1_8::parse_with(BinParser &bp) { return true; } -bool RTState_V3_0__1::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool RTState_V3_0__1::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; parse_shared1(bp); @@ -74,7 +77,7 @@ bool RTState_V3_0__1::parse_with(BinParser &bp) { bp.parse(speed_scaling); bp.parse(linear_momentum_norm); 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_robot); bp.parse(i_robot); @@ -83,8 +86,9 @@ bool RTState_V3_0__1::parse_with(BinParser &bp) { return true; } -bool RTState_V3_2__3::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool RTState_V3_2__3::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; RTState_V3_0__1::parse_with(bp); @@ -95,15 +99,19 @@ bool RTState_V3_2__3::parse_with(BinParser &bp) { return true; } -bool RTState_V1_6__7::consume_with(URRTPacketConsumer &consumer) { +bool RTState_V1_6__7::consume_with(URRTPacketConsumer& consumer) +{ return consumer.consume(*this); } -bool RTState_V1_8::consume_with(URRTPacketConsumer &consumer) { +bool RTState_V1_8::consume_with(URRTPacketConsumer& consumer) +{ return consumer.consume(*this); } -bool RTState_V3_0__1::consume_with(URRTPacketConsumer &consumer) { +bool RTState_V3_0__1::consume_with(URRTPacketConsumer& consumer) +{ return consumer.consume(*this); } -bool RTState_V3_2__3::consume_with(URRTPacketConsumer &consumer) { +bool RTState_V3_2__3::consume_with(URRTPacketConsumer& consumer) +{ return consumer.consume(*this); } \ No newline at end of file diff --git a/src/ur/state.cpp b/src/ur/state.cpp index fe5af1b..0b4f4f2 100644 --- a/src/ur/state.cpp +++ b/src/ur/state.cpp @@ -1,7 +1,5 @@ -#include "ur_modern_driver/log.h" #include "ur_modern_driver/ur/state.h" - - +#include "ur_modern_driver/log.h" //StatePacket::~StatePacket() { } diff --git a/src/ur/stream.cpp b/src/ur/stream.cpp index f66482b..d0fd10f 100644 --- a/src/ur/stream.cpp +++ b/src/ur/stream.cpp @@ -1,13 +1,14 @@ #include -#include -#include #include +#include +#include -#include "ur_modern_driver/ur/stream.h" #include "ur_modern_driver/log.h" +#include "ur_modern_driver/ur/stream.h" -bool URStream::connect() { - if(_initialized) +bool URStream::connect() +{ + if (_initialized) return false; LOG_INFO("Connecting to UR @ %s:%d", _host.c_str(), _port); @@ -23,20 +24,20 @@ bool URStream::connect() { hints.ai_socktype = SOCK_STREAM; hints.ai_flags = AI_PASSIVE; - if(getaddrinfo(_host.c_str(), service.c_str(), &hints, &result) != 0) { - LOG_ERROR("Failed to get host name"); + if (getaddrinfo(_host.c_str(), service.c_str(), &hints, &result) != 0) { + LOG_ERROR("Failed to get host name"); return false; } - + //loop through the list of addresses untill we find one that's connectable - for(struct addrinfo *p = result; p != nullptr; p = p->ai_next) { + for (struct addrinfo* p = result; p != nullptr; p = p->ai_next) { _socket_fd = socket(p->ai_family, p->ai_socktype, p->ai_protocol); - if(_socket_fd == -1) //socket error? + if (_socket_fd == -1) //socket error? continue; - if(::connect(_socket_fd, p->ai_addr, p->ai_addrlen) != 0) { - if(_stopping) + if (::connect(_socket_fd, p->ai_addr, p->ai_addrlen) != 0) { + if (_stopping) break; else continue; //try next addrinfo if connect fails @@ -51,14 +52,15 @@ bool URStream::connect() { } freeaddrinfo(result); - if(!_initialized) + if (!_initialized) LOG_ERROR("Connection failed"); return _initialized; } -void URStream::disconnect() { - if(!_initialized || _stopping) +void URStream::disconnect() +{ + if (!_initialized || _stopping) return; _stopping = true; @@ -66,10 +68,11 @@ void URStream::disconnect() { _initialized = false; } -ssize_t URStream::send(uint8_t *buf, size_t buf_len) { - if(!_initialized) +ssize_t URStream::send(uint8_t* buf, size_t buf_len) +{ + if (!_initialized) return -1; - if(_stopping) + if (_stopping) return 0; size_t total = 0; @@ -77,9 +80,9 @@ ssize_t URStream::send(uint8_t *buf, size_t buf_len) { //TODO: handle reconnect? //handle partial sends - while(total < buf_len) { - ssize_t sent = ::send(_socket_fd, buf+total, remaining, 0); - if(sent <= 0) + while (total < buf_len) { + ssize_t sent = ::send(_socket_fd, buf + total, remaining, 0); + if (sent <= 0) return _stopping ? 0 : sent; total += sent; remaining -= sent; @@ -88,24 +91,25 @@ ssize_t URStream::send(uint8_t *buf, size_t buf_len) { return total; } -ssize_t URStream::receive(uint8_t *buf, size_t buf_len) { - if(!_initialized) +ssize_t URStream::receive(uint8_t* buf, size_t buf_len) +{ + if (!_initialized) return -1; - if(_stopping) + if (_stopping) return 0; size_t remainder = sizeof(int32_t); - uint8_t *buf_pos = buf; + 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) { + if (read <= 0) //failed reading from socket + return _stopping ? 0 : read; + + if (initial) { remainder = be32toh(*(reinterpret_cast(buf))); - if(remainder >= (buf_len - sizeof(int32_t))) { + if (remainder >= (buf_len - sizeof(int32_t))) { LOG_ERROR("Packet size %d is larger than buffer %d, discarding.", remainder, buf_len); return -1; } @@ -113,8 +117,8 @@ ssize_t URStream::receive(uint8_t *buf, size_t buf_len) { } buf_pos += read; - remainder -= read; - } while(remainder > 0); + remainder -= read; + } while (remainder > 0); return buf_pos - buf; } \ No newline at end of file diff --git a/src/ur_communication.cpp b/src/ur_communication.cpp index cf04cc3..b883004 100644 --- a/src/ur_communication.cpp +++ b/src/ur_communication.cpp @@ -19,163 +19,167 @@ #include "ur_modern_driver/ur_communication.h" UrCommunication::UrCommunication(std::condition_variable& msg_cond, - std::string host) { - robot_state_ = new RobotState(msg_cond); - bzero((char *) &pri_serv_addr_, sizeof(pri_serv_addr_)); - bzero((char *) &sec_serv_addr_, sizeof(sec_serv_addr_)); - pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (pri_sockfd_ < 0) { - print_fatal("ERROR opening socket pri_sockfd"); - } - sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (sec_sockfd_ < 0) { - print_fatal("ERROR opening socket sec_sockfd"); - } - server_ = gethostbyname(host.c_str()); - if (server_ == NULL) { - print_fatal("ERROR, unknown host"); - } - pri_serv_addr_.sin_family = AF_INET; - sec_serv_addr_.sin_family = AF_INET; - bcopy((char *) server_->h_addr, (char *)&pri_serv_addr_.sin_addr.s_addr, server_->h_length); - bcopy((char *) server_->h_addr, (char *)&sec_serv_addr_.sin_addr.s_addr, server_->h_length); - pri_serv_addr_.sin_port = htons(30001); - sec_serv_addr_.sin_port = htons(30002); - flag_ = 1; - setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, - sizeof(int)); - setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, - sizeof(int)); - setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, - sizeof(int)); - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, - sizeof(int)); - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, - sizeof(int)); - setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, - sizeof(int)); - fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); - connected_ = false; - keepalive_ = false; + std::string host) +{ + robot_state_ = new RobotState(msg_cond); + bzero((char*)&pri_serv_addr_, sizeof(pri_serv_addr_)); + bzero((char*)&sec_serv_addr_, sizeof(sec_serv_addr_)); + pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (pri_sockfd_ < 0) { + print_fatal("ERROR opening socket pri_sockfd"); + } + sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (sec_sockfd_ < 0) { + print_fatal("ERROR opening socket sec_sockfd"); + } + server_ = gethostbyname(host.c_str()); + if (server_ == NULL) { + print_fatal("ERROR, unknown host"); + } + pri_serv_addr_.sin_family = AF_INET; + sec_serv_addr_.sin_family = AF_INET; + bcopy((char*)server_->h_addr, (char*)&pri_serv_addr_.sin_addr.s_addr, server_->h_length); + bcopy((char*)server_->h_addr, (char*)&sec_serv_addr_.sin_addr.s_addr, server_->h_length); + pri_serv_addr_.sin_port = htons(30001); + sec_serv_addr_.sin_port = htons(30002); + flag_ = 1; + setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, + sizeof(int)); + setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, + sizeof(int)); + setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, + sizeof(int)); + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, + sizeof(int)); + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, + sizeof(int)); + setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, + sizeof(int)); + fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); + connected_ = false; + keepalive_ = false; } -bool UrCommunication::start() { - keepalive_ = true; - uint8_t buf[512]; - unsigned int bytes_read; - std::string cmd; - bzero(buf, 512); - print_debug("Acquire firmware version: Connecting..."); - if (connect(pri_sockfd_, (struct sockaddr *) &pri_serv_addr_, - sizeof(pri_serv_addr_)) < 0) { - print_fatal("Error connecting to get firmware version"); - return false; - } - print_debug("Acquire firmware version: Got connection"); - bytes_read = read(pri_sockfd_, buf, 512); - setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, - sizeof(int)); - robot_state_->unpack(buf, bytes_read); - //wait for some traffic so the UR socket doesn't die in version 3.1. - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - char tmp[64]; - sprintf(tmp, "Firmware version detected: %.7f", robot_state_->getVersion()); - print_debug(tmp); - close(pri_sockfd_); +bool UrCommunication::start() +{ + keepalive_ = true; + uint8_t buf[512]; + unsigned int bytes_read; + std::string cmd; + bzero(buf, 512); + print_debug("Acquire firmware version: Connecting..."); + if (connect(pri_sockfd_, (struct sockaddr*)&pri_serv_addr_, + sizeof(pri_serv_addr_)) + < 0) { + print_fatal("Error connecting to get firmware version"); + return false; + } + print_debug("Acquire firmware version: Got connection"); + bytes_read = read(pri_sockfd_, buf, 512); + setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, + sizeof(int)); + robot_state_->unpack(buf, bytes_read); + //wait for some traffic so the UR socket doesn't die in version 3.1. + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + char tmp[64]; + sprintf(tmp, "Firmware version detected: %.7f", robot_state_->getVersion()); + print_debug(tmp); + close(pri_sockfd_); - print_debug( - "Switching to secondary interface for masterboard data: Connecting..."); + print_debug( + "Switching to secondary interface for masterboard data: Connecting..."); - fd_set writefds; - struct timeval timeout; + fd_set writefds; + struct timeval timeout; - connect(sec_sockfd_, (struct sockaddr *) &sec_serv_addr_, - sizeof(sec_serv_addr_)); - FD_ZERO(&writefds); - FD_SET(sec_sockfd_, &writefds); - timeout.tv_sec = 10; - timeout.tv_usec = 0; - select(sec_sockfd_ + 1, NULL, &writefds, NULL, &timeout); - unsigned int flag_len; - getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); - if (flag_ < 0) { - print_fatal("Error connecting to secondary interface"); - return false; - } - print_debug("Secondary interface: Got connection"); - comThread_ = std::thread(&UrCommunication::run, this); - return true; + connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_, + sizeof(sec_serv_addr_)); + FD_ZERO(&writefds); + FD_SET(sec_sockfd_, &writefds); + timeout.tv_sec = 10; + timeout.tv_usec = 0; + select(sec_sockfd_ + 1, NULL, &writefds, NULL, &timeout); + unsigned int flag_len; + getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); + if (flag_ < 0) { + print_fatal("Error connecting to secondary interface"); + return false; + } + print_debug("Secondary interface: Got connection"); + comThread_ = std::thread(&UrCommunication::run, this); + return true; } -void UrCommunication::halt() { - keepalive_ = false; - comThread_.join(); +void UrCommunication::halt() +{ + keepalive_ = false; + comThread_.join(); } -void UrCommunication::run() { - uint8_t buf[2048]; - int bytes_read; - bzero(buf, 2048); - struct timeval timeout; - fd_set readfds; - FD_ZERO(&readfds); - FD_SET(sec_sockfd_, &readfds); - connected_ = true; - while (keepalive_) { - while (connected_ && keepalive_) { - timeout.tv_sec = 0; //do this each loop as selects modifies timeout - timeout.tv_usec = 500000; // timeout of 0.5 sec - select(sec_sockfd_ + 1, &readfds, NULL, NULL, &timeout); - bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes - if (bytes_read > 0) { - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, - (char *) &flag_, sizeof(int)); - robot_state_->unpack(buf, bytes_read); - } else { - connected_ = false; - robot_state_->setDisconnected(); - close(sec_sockfd_); - } - } - if (keepalive_) { - //reconnect - print_warning("Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."); - sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (sec_sockfd_ < 0) { - print_fatal("ERROR opening secondary socket"); - } - flag_ = 1; - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, - sizeof(int)); - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, - sizeof(int)); - setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, - sizeof(int)); - fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); - while (keepalive_ && !connected_) { - std::this_thread::sleep_for(std::chrono::seconds(10)); - fd_set writefds; +void UrCommunication::run() +{ + uint8_t buf[2048]; + int bytes_read; + bzero(buf, 2048); + struct timeval timeout; + fd_set readfds; + FD_ZERO(&readfds); + FD_SET(sec_sockfd_, &readfds); + connected_ = true; + while (keepalive_) { + while (connected_ && keepalive_) { + timeout.tv_sec = 0; //do this each loop as selects modifies timeout + timeout.tv_usec = 500000; // timeout of 0.5 sec + select(sec_sockfd_ + 1, &readfds, NULL, NULL, &timeout); + bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes + if (bytes_read > 0) { + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, + (char*)&flag_, sizeof(int)); + robot_state_->unpack(buf, bytes_read); + } else { + connected_ = false; + robot_state_->setDisconnected(); + close(sec_sockfd_); + } + } + if (keepalive_) { + //reconnect + print_warning("Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."); + sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (sec_sockfd_ < 0) { + print_fatal("ERROR opening secondary socket"); + } + flag_ = 1; + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, + sizeof(int)); + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, + sizeof(int)); + setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, + sizeof(int)); + fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); + while (keepalive_ && !connected_) { + std::this_thread::sleep_for(std::chrono::seconds(10)); + fd_set writefds; - connect(sec_sockfd_, (struct sockaddr *) &sec_serv_addr_, - sizeof(sec_serv_addr_)); - FD_ZERO(&writefds); - FD_SET(sec_sockfd_, &writefds); - select(sec_sockfd_ + 1, NULL, &writefds, NULL, NULL); - unsigned int flag_len; - getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, - &flag_len); - if (flag_ < 0) { - print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds..."); - } else { - connected_ = true; - print_info("Secondary port: Reconnected"); - } - } - } - } + connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_, + sizeof(sec_serv_addr_)); + FD_ZERO(&writefds); + FD_SET(sec_sockfd_, &writefds); + select(sec_sockfd_ + 1, NULL, &writefds, NULL, NULL); + unsigned int flag_len; + getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, + &flag_len); + if (flag_ < 0) { + print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds..."); + } else { + connected_ = true; + print_info("Secondary port: Reconnected"); + } + } + } + } - //wait for some traffic so the UR socket doesn't die in version 3.1. - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - close(sec_sockfd_); + //wait for some traffic so the UR socket doesn't die in version 3.1. + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + close(sec_sockfd_); } - diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index 2208115..08a535a 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -19,364 +19,396 @@ #include "ur_modern_driver/ur_driver.h" UrDriver::UrDriver(std::condition_variable& rt_msg_cond, - std::condition_variable& msg_cond, std::string host, - unsigned int reverse_port, double servoj_time, - unsigned int safety_count_max, double max_time_step, double min_payload, - double max_payload, double servoj_lookahead_time, double servoj_gain) : - REVERSE_PORT_(reverse_port), maximum_time_step_(max_time_step), minimum_payload_( - min_payload), maximum_payload_(max_payload), servoj_time_( - servoj_time), servoj_lookahead_time_(servoj_lookahead_time), servoj_gain_(servoj_gain) { - char buffer[256]; - struct sockaddr_in serv_addr; - int n, flag; + std::condition_variable& msg_cond, std::string host, + unsigned int reverse_port, double servoj_time, + unsigned int safety_count_max, double max_time_step, double min_payload, + double max_payload, double servoj_lookahead_time, double servoj_gain) + : REVERSE_PORT_(reverse_port) + , maximum_time_step_(max_time_step) + , minimum_payload_( + min_payload) + , maximum_payload_(max_payload) + , servoj_time_( + servoj_time) + , servoj_lookahead_time_(servoj_lookahead_time) + , servoj_gain_(servoj_gain) +{ + char buffer[256]; + struct sockaddr_in serv_addr; + int n, flag; - firmware_version_ = 0; - reverse_connected_ = false; - executing_traj_ = false; - rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host, - safety_count_max); - new_sockfd_ = -1; - sec_interface_ = new UrCommunication(msg_cond, host); + firmware_version_ = 0; + reverse_connected_ = false; + executing_traj_ = false; + rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host, + safety_count_max); + new_sockfd_ = -1; + sec_interface_ = new UrCommunication(msg_cond, host); - incoming_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (incoming_sockfd_ < 0) { - print_fatal("ERROR opening socket for reverse communication"); - } - bzero((char *) &serv_addr, sizeof(serv_addr)); + incoming_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (incoming_sockfd_ < 0) { + print_fatal("ERROR opening socket for reverse communication"); + } + bzero((char*)&serv_addr, sizeof(serv_addr)); - serv_addr.sin_family = AF_INET; - serv_addr.sin_addr.s_addr = INADDR_ANY; - serv_addr.sin_port = htons(REVERSE_PORT_); - flag = 1; - setsockopt(incoming_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag, - sizeof(int)); - setsockopt(incoming_sockfd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int)); - if (bind(incoming_sockfd_, (struct sockaddr *) &serv_addr, - sizeof(serv_addr)) < 0) { - print_fatal("ERROR on binding socket for reverse communication"); - } - listen(incoming_sockfd_, 5); + serv_addr.sin_family = AF_INET; + serv_addr.sin_addr.s_addr = INADDR_ANY; + serv_addr.sin_port = htons(REVERSE_PORT_); + flag = 1; + setsockopt(incoming_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag, + sizeof(int)); + setsockopt(incoming_sockfd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int)); + if (bind(incoming_sockfd_, (struct sockaddr*)&serv_addr, + sizeof(serv_addr)) + < 0) { + print_fatal("ERROR on binding socket for reverse communication"); + } + listen(incoming_sockfd_, 5); } std::vector UrDriver::interp_cubic(double t, double T, - std::vector p0_pos, std::vector p1_pos, - std::vector p0_vel, std::vector p1_vel) { - /*Returns positions of the joints at time 't' */ - std::vector positions; - for (unsigned int i = 0; i < p0_pos.size(); i++) { - double a = p0_pos[i]; - double b = p0_vel[i]; - double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i] - - T * p1_vel[i]) / pow(T, 2); - double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i] - + T * p1_vel[i]) / pow(T, 3); - positions.push_back(a + b * t + c * pow(t, 2) + d * pow(t, 3)); - } - return positions; + std::vector p0_pos, std::vector p1_pos, + std::vector p0_vel, std::vector p1_vel) +{ + /*Returns positions of the joints at time 't' */ + std::vector positions; + for (unsigned int i = 0; i < p0_pos.size(); i++) { + double a = p0_pos[i]; + double b = p0_vel[i]; + double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i] + - T * p1_vel[i]) + / pow(T, 2); + double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i] + + T * p1_vel[i]) + / pow(T, 3); + positions.push_back(a + b * t + c * pow(t, 2) + d * pow(t, 3)); + } + return positions; } bool UrDriver::doTraj(std::vector inp_timestamps, - std::vector > inp_positions, - std::vector > inp_velocities) { - std::chrono::high_resolution_clock::time_point t0, t; - std::vector positions; - unsigned int j; + std::vector > inp_positions, + std::vector > inp_velocities) +{ + std::chrono::high_resolution_clock::time_point t0, t; + std::vector positions; + unsigned int j; - if (!UrDriver::uploadProg()) { - return false; - } - executing_traj_ = true; - t0 = std::chrono::high_resolution_clock::now(); - t = t0; - j = 0; - while ((inp_timestamps[inp_timestamps.size() - 1] - >= std::chrono::duration_cast>(t - t0).count()) - and executing_traj_) { - while (inp_timestamps[j] - <= std::chrono::duration_cast>( - t - t0).count() && j < inp_timestamps.size() - 1) { - j += 1; - } - positions = UrDriver::interp_cubic( - std::chrono::duration_cast>( - 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); + if (!UrDriver::uploadProg()) { + return false; + } + executing_traj_ = true; + t0 = std::chrono::high_resolution_clock::now(); + t = t0; + j = 0; + while ((inp_timestamps[inp_timestamps.size() - 1] + >= std::chrono::duration_cast >(t - t0).count()) + and executing_traj_) { + while (inp_timestamps[j] + <= std::chrono::duration_cast >( + t - t0) + .count() + && j < inp_timestamps.size() - 1) { + j += 1; + } + positions = UrDriver::interp_cubic( + std::chrono::duration_cast >( + t - t0) + .count() + - inp_timestamps[j - 1], + inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1], + inp_positions[j], inp_velocities[j - 1], inp_velocities[j]); + UrDriver::servoj(positions); - // oversample with 4 * sample_time - std::this_thread::sleep_for( - std::chrono::milliseconds((int) ((servoj_time_ * 1000) / 4.))); - t = std::chrono::high_resolution_clock::now(); - } - executing_traj_ = false; - //Signal robot to stop driverProg() - UrDriver::closeServo(positions); - return true; + // oversample with 4 * sample_time + std::this_thread::sleep_for( + std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.))); + t = std::chrono::high_resolution_clock::now(); + } + executing_traj_ = false; + //Signal robot to stop driverProg() + UrDriver::closeServo(positions); + return true; } -void UrDriver::servoj(std::vector positions, int keepalive) { - if (!reverse_connected_) { - print_error( - "UrDriver::servoj called without a reverse connection present. Keepalive: " - + std::to_string(keepalive)); - return; - } - unsigned int bytes_written; - int tmp; - unsigned char buf[28]; - for (int i = 0; i < 6; i++) { - tmp = htonl((int) (positions[i] * MULT_JOINTSTATE_)); - buf[i * 4] = tmp & 0xff; - buf[i * 4 + 1] = (tmp >> 8) & 0xff; - buf[i * 4 + 2] = (tmp >> 16) & 0xff; - buf[i * 4 + 3] = (tmp >> 24) & 0xff; - } - tmp = htonl((int) keepalive); - buf[6 * 4] = tmp & 0xff; - buf[6 * 4 + 1] = (tmp >> 8) & 0xff; - buf[6 * 4 + 2] = (tmp >> 16) & 0xff; - buf[6 * 4 + 3] = (tmp >> 24) & 0xff; - bytes_written = write(new_sockfd_, buf, 28); +void UrDriver::servoj(std::vector positions, int keepalive) +{ + if (!reverse_connected_) { + print_error( + "UrDriver::servoj called without a reverse connection present. Keepalive: " + + std::to_string(keepalive)); + return; + } + unsigned int bytes_written; + int tmp; + unsigned char buf[28]; + for (int i = 0; i < 6; i++) { + tmp = htonl((int)(positions[i] * MULT_JOINTSTATE_)); + buf[i * 4] = tmp & 0xff; + buf[i * 4 + 1] = (tmp >> 8) & 0xff; + buf[i * 4 + 2] = (tmp >> 16) & 0xff; + buf[i * 4 + 3] = (tmp >> 24) & 0xff; + } + tmp = htonl((int)keepalive); + buf[6 * 4] = tmp & 0xff; + buf[6 * 4 + 1] = (tmp >> 8) & 0xff; + buf[6 * 4 + 2] = (tmp >> 16) & 0xff; + buf[6 * 4 + 3] = (tmp >> 24) & 0xff; + bytes_written = write(new_sockfd_, buf, 28); } -void UrDriver::stopTraj() { - executing_traj_ = false; - rt_interface_->addCommandToQueue("stopj(10)\n"); +void UrDriver::stopTraj() +{ + executing_traj_ = false; + rt_interface_->addCommandToQueue("stopj(10)\n"); } -bool UrDriver::uploadProg() { - std::string cmd_str; - char buf[128]; - cmd_str = "def driverProg():\n"; +bool UrDriver::uploadProg() +{ + std::string cmd_str; + char buf[128]; + cmd_str = "def driverProg():\n"; - sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_); - cmd_str += buf; + sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_); + cmd_str += buf; - cmd_str += "\tSERVO_IDLE = 0\n"; - cmd_str += "\tSERVO_RUNNING = 1\n"; - cmd_str += "\tcmd_servo_state = SERVO_IDLE\n"; - cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n"; - cmd_str += "\tdef set_servo_setpoint(q):\n"; - cmd_str += "\t\tenter_critical\n"; - cmd_str += "\t\tcmd_servo_state = SERVO_RUNNING\n"; - cmd_str += "\t\tcmd_servo_q = q\n"; - cmd_str += "\t\texit_critical\n"; - cmd_str += "\tend\n"; - cmd_str += "\tthread servoThread():\n"; - cmd_str += "\t\tstate = SERVO_IDLE\n"; - cmd_str += "\t\twhile True:\n"; - cmd_str += "\t\t\tenter_critical\n"; - cmd_str += "\t\t\tq = cmd_servo_q\n"; - cmd_str += "\t\t\tdo_brake = False\n"; - cmd_str += "\t\t\tif (state == SERVO_RUNNING) and "; - cmd_str += "(cmd_servo_state == SERVO_IDLE):\n"; - cmd_str += "\t\t\t\tdo_brake = True\n"; - cmd_str += "\t\t\tend\n"; - cmd_str += "\t\t\tstate = cmd_servo_state\n"; - cmd_str += "\t\t\tcmd_servo_state = SERVO_IDLE\n"; - cmd_str += "\t\t\texit_critical\n"; - cmd_str += "\t\t\tif do_brake:\n"; - cmd_str += "\t\t\t\tstopj(1.0)\n"; - cmd_str += "\t\t\t\tsync()\n"; - cmd_str += "\t\t\telif state == SERVO_RUNNING:\n"; + cmd_str += "\tSERVO_IDLE = 0\n"; + cmd_str += "\tSERVO_RUNNING = 1\n"; + cmd_str += "\tcmd_servo_state = SERVO_IDLE\n"; + cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n"; + cmd_str += "\tdef set_servo_setpoint(q):\n"; + cmd_str += "\t\tenter_critical\n"; + cmd_str += "\t\tcmd_servo_state = SERVO_RUNNING\n"; + cmd_str += "\t\tcmd_servo_q = q\n"; + cmd_str += "\t\texit_critical\n"; + cmd_str += "\tend\n"; + cmd_str += "\tthread servoThread():\n"; + cmd_str += "\t\tstate = SERVO_IDLE\n"; + cmd_str += "\t\twhile True:\n"; + cmd_str += "\t\t\tenter_critical\n"; + cmd_str += "\t\t\tq = cmd_servo_q\n"; + cmd_str += "\t\t\tdo_brake = False\n"; + cmd_str += "\t\t\tif (state == SERVO_RUNNING) and "; + cmd_str += "(cmd_servo_state == SERVO_IDLE):\n"; + cmd_str += "\t\t\t\tdo_brake = True\n"; + cmd_str += "\t\t\tend\n"; + cmd_str += "\t\t\tstate = cmd_servo_state\n"; + cmd_str += "\t\t\tcmd_servo_state = SERVO_IDLE\n"; + cmd_str += "\t\t\texit_critical\n"; + cmd_str += "\t\t\tif do_brake:\n"; + cmd_str += "\t\t\t\tstopj(1.0)\n"; + cmd_str += "\t\t\t\tsync()\n"; + cmd_str += "\t\t\telif state == SERVO_RUNNING:\n"; - if (sec_interface_->robot_state_->getVersion() >= 3.1) - sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=%.4f, gain=%.0f)\n", - servoj_time_, servoj_lookahead_time_, servoj_gain_); - else - sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_); - cmd_str += buf; + if (sec_interface_->robot_state_->getVersion() >= 3.1) + sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=%.4f, gain=%.0f)\n", + servoj_time_, servoj_lookahead_time_, servoj_gain_); + else + sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_); + cmd_str += buf; - cmd_str += "\t\t\telse:\n"; - cmd_str += "\t\t\t\tsync()\n"; - cmd_str += "\t\t\tend\n"; - cmd_str += "\t\tend\n"; - cmd_str += "\tend\n"; + cmd_str += "\t\t\telse:\n"; + cmd_str += "\t\t\t\tsync()\n"; + cmd_str += "\t\t\tend\n"; + cmd_str += "\t\tend\n"; + cmd_str += "\tend\n"; - sprintf(buf, "\tsocket_open(\"%s\", %i)\n", ip_addr_.c_str(), - REVERSE_PORT_); - cmd_str += buf; + sprintf(buf, "\tsocket_open(\"%s\", %i)\n", ip_addr_.c_str(), + REVERSE_PORT_); + cmd_str += buf; - cmd_str += "\tthread_servo = run servoThread()\n"; - cmd_str += "\tkeepalive = 1\n"; - cmd_str += "\twhile keepalive > 0:\n"; - cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1)\n"; - cmd_str += "\t\tif params_mult[0] > 0:\n"; - cmd_str += "\t\t\tq = [params_mult[1] / MULT_jointstate, "; - cmd_str += "params_mult[2] / MULT_jointstate, "; - cmd_str += "params_mult[3] / MULT_jointstate, "; - cmd_str += "params_mult[4] / MULT_jointstate, "; - cmd_str += "params_mult[5] / MULT_jointstate, "; - cmd_str += "params_mult[6] / MULT_jointstate]\n"; - cmd_str += "\t\t\tkeepalive = params_mult[7]\n"; - cmd_str += "\t\t\tset_servo_setpoint(q)\n"; - cmd_str += "\t\tend\n"; - cmd_str += "\tend\n"; - cmd_str += "\tsleep(.1)\n"; - cmd_str += "\tsocket_close()\n"; - cmd_str += "\tkill thread_servo\n"; - cmd_str += "end\n"; + cmd_str += "\tthread_servo = run servoThread()\n"; + cmd_str += "\tkeepalive = 1\n"; + cmd_str += "\twhile keepalive > 0:\n"; + cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1)\n"; + cmd_str += "\t\tif params_mult[0] > 0:\n"; + cmd_str += "\t\t\tq = [params_mult[1] / MULT_jointstate, "; + cmd_str += "params_mult[2] / MULT_jointstate, "; + cmd_str += "params_mult[3] / MULT_jointstate, "; + cmd_str += "params_mult[4] / MULT_jointstate, "; + cmd_str += "params_mult[5] / MULT_jointstate, "; + cmd_str += "params_mult[6] / MULT_jointstate]\n"; + cmd_str += "\t\t\tkeepalive = params_mult[7]\n"; + cmd_str += "\t\t\tset_servo_setpoint(q)\n"; + cmd_str += "\t\tend\n"; + cmd_str += "\tend\n"; + cmd_str += "\tsleep(.1)\n"; + cmd_str += "\tsocket_close()\n"; + cmd_str += "\tkill thread_servo\n"; + cmd_str += "end\n"; - rt_interface_->addCommandToQueue(cmd_str); - return UrDriver::openServo(); + rt_interface_->addCommandToQueue(cmd_str); + return UrDriver::openServo(); } -bool UrDriver::openServo() { - struct sockaddr_in cli_addr; - socklen_t clilen; - clilen = sizeof(cli_addr); - new_sockfd_ = accept(incoming_sockfd_, (struct sockaddr *) &cli_addr, - &clilen); - if (new_sockfd_ < 0) { - print_fatal("ERROR on accepting reverse communication"); - return false; - } - reverse_connected_ = true; - return true; +bool UrDriver::openServo() +{ + struct sockaddr_in cli_addr; + socklen_t clilen; + clilen = sizeof(cli_addr); + new_sockfd_ = accept(incoming_sockfd_, (struct sockaddr*)&cli_addr, + &clilen); + if (new_sockfd_ < 0) { + print_fatal("ERROR on accepting reverse communication"); + return false; + } + reverse_connected_ = true; + return true; } -void UrDriver::closeServo(std::vector positions) { - if (positions.size() != 6) - UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0); - else - UrDriver::servoj(positions, 0); +void UrDriver::closeServo(std::vector positions) +{ + if (positions.size() != 6) + UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0); + else + UrDriver::servoj(positions, 0); - reverse_connected_ = false; - close(new_sockfd_); + reverse_connected_ = false; + close(new_sockfd_); } -bool UrDriver::start() { - if (!sec_interface_->start()) - return false; - firmware_version_ = sec_interface_->robot_state_->getVersion(); - rt_interface_->robot_state_->setVersion(firmware_version_); - if (!rt_interface_->start()) - return false; - ip_addr_ = rt_interface_->getLocalIp(); - print_debug( - "Listening on " + ip_addr_ + ":" + std::to_string(REVERSE_PORT_) - + "\n"); - return true; - +bool UrDriver::start() +{ + if (!sec_interface_->start()) + return false; + firmware_version_ = sec_interface_->robot_state_->getVersion(); + rt_interface_->robot_state_->setVersion(firmware_version_); + if (!rt_interface_->start()) + return false; + ip_addr_ = rt_interface_->getLocalIp(); + print_debug( + "Listening on " + ip_addr_ + ":" + std::to_string(REVERSE_PORT_) + + "\n"); + return true; } -void UrDriver::halt() { - if (executing_traj_) { - UrDriver::stopTraj(); - } - sec_interface_->halt(); - rt_interface_->halt(); - close(incoming_sockfd_); +void UrDriver::halt() +{ + if (executing_traj_) { + UrDriver::stopTraj(); + } + sec_interface_->halt(); + rt_interface_->halt(); + close(incoming_sockfd_); } void UrDriver::setSpeed(double q0, double q1, double q2, double q3, double q4, - double q5, double acc) { - rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc); + double q5, double acc) +{ + rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc); } -std::vector UrDriver::getJointNames() { - return joint_names_; +std::vector UrDriver::getJointNames() +{ + return joint_names_; } -void UrDriver::setJointNames(std::vector jn) { - joint_names_ = jn; +void UrDriver::setJointNames(std::vector jn) +{ + joint_names_ = jn; } -void UrDriver::setToolVoltage(unsigned int v) { - char buf[256]; - sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v); - rt_interface_->addCommandToQueue(buf); - print_debug(buf); +void UrDriver::setToolVoltage(unsigned int v) +{ + char buf[256]; + sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v); + rt_interface_->addCommandToQueue(buf); + print_debug(buf); } -void UrDriver::setFlag(unsigned int n, bool b) { - char buf[256]; - sprintf(buf, "sec setOut():\n\tset_flag(%d, %s)\nend\n", n, - b ? "True" : "False"); - rt_interface_->addCommandToQueue(buf); - print_debug(buf); +void UrDriver::setFlag(unsigned int n, bool b) +{ + char buf[256]; + sprintf(buf, "sec setOut():\n\tset_flag(%d, %s)\nend\n", n, + b ? "True" : "False"); + rt_interface_->addCommandToQueue(buf); + print_debug(buf); } -void UrDriver::setDigitalOut(unsigned int n, bool b) { - char buf[256]; - if (firmware_version_ < 2) { - sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n, - b ? "True" : "False"); +void UrDriver::setDigitalOut(unsigned int n, bool b) +{ + char buf[256]; + if (firmware_version_ < 2) { + sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n, + b ? "True" : "False"); } else if (n > 15) { sprintf(buf, - "sec setOut():\n\tset_tool_digital_out(%d, %s)\nend\n", - n - 16, b ? "True" : "False"); - } else if (n > 7) { + "sec setOut():\n\tset_tool_digital_out(%d, %s)\nend\n", + n - 16, b ? "True" : "False"); + } else if (n > 7) { sprintf(buf, "sec setOut():\n\tset_configurable_digital_out(%d, %s)\nend\n", - n - 8, b ? "True" : "False"); - - } else { - sprintf(buf, "sec setOut():\n\tset_standard_digital_out(%d, %s)\nend\n", - n, b ? "True" : "False"); - - } - rt_interface_->addCommandToQueue(buf); - print_debug(buf); + n - 8, b ? "True" : "False"); + } else { + sprintf(buf, "sec setOut():\n\tset_standard_digital_out(%d, %s)\nend\n", + n, b ? "True" : "False"); + } + rt_interface_->addCommandToQueue(buf); + print_debug(buf); } -void UrDriver::setAnalogOut(unsigned int n, double f) { - char buf[256]; - if (firmware_version_ < 2) { - sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f); - } else { - sprintf(buf, "sec setOut():\n\tset_standard_analog_out(%d, %1.4f)\nend\n", n, f); - } +void UrDriver::setAnalogOut(unsigned int n, double f) +{ + char buf[256]; + if (firmware_version_ < 2) { + sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f); + } else { + sprintf(buf, "sec setOut():\n\tset_standard_analog_out(%d, %1.4f)\nend\n", n, f); + } - rt_interface_->addCommandToQueue(buf); - print_debug(buf); + rt_interface_->addCommandToQueue(buf); + print_debug(buf); } -bool UrDriver::setPayload(double m) { - if ((m < maximum_payload_) && (m > minimum_payload_)) { - char buf[256]; - sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m); - rt_interface_->addCommandToQueue(buf); - print_debug(buf); - return true; - } else - return false; +bool UrDriver::setPayload(double m) +{ + if ((m < maximum_payload_) && (m > minimum_payload_)) { + char buf[256]; + sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m); + rt_interface_->addCommandToQueue(buf); + print_debug(buf); + return true; + } else + return false; } -void UrDriver::setMinPayload(double m) { - if (m > 0) { - minimum_payload_ = m; - } else { - minimum_payload_ = 0; - } - +void UrDriver::setMinPayload(double m) +{ + if (m > 0) { + minimum_payload_ = m; + } else { + minimum_payload_ = 0; + } } -void UrDriver::setMaxPayload(double m) { - maximum_payload_ = m; +void UrDriver::setMaxPayload(double m) +{ + maximum_payload_ = m; } -void UrDriver::setServojTime(double t) { - if (t > 0.008) { - servoj_time_ = t; - } else { - servoj_time_ = 0.008; - } +void UrDriver::setServojTime(double t) +{ + if (t > 0.008) { + servoj_time_ = t; + } else { + servoj_time_ = 0.008; + } } -void UrDriver::setServojLookahead(double t){ - if (t > 0.03) { - if (t < 0.2) { - servoj_lookahead_time_ = t; - } else { - servoj_lookahead_time_ = 0.2; - } - } else { - servoj_lookahead_time_ = 0.03; - } +void UrDriver::setServojLookahead(double t) +{ + if (t > 0.03) { + if (t < 0.2) { + servoj_lookahead_time_ = t; + } else { + servoj_lookahead_time_ = 0.2; + } + } else { + servoj_lookahead_time_ = 0.03; + } } -void UrDriver::setServojGain(double g){ - if (g > 100) { - if (g < 2000) { - servoj_gain_ = g; - } else { - servoj_gain_ = 2000; - } - } else { - servoj_gain_ = 100; - } +void UrDriver::setServojGain(double g) +{ + if (g > 100) { + if (g < 2000) { + servoj_gain_ = g; + } else { + servoj_gain_ = 2000; + } + } else { + servoj_gain_ = 100; + } } diff --git a/src/ur_hardware_interface.cpp b/src/ur_hardware_interface.cpp index ea66395..152452c 100644 --- a/src/ur_hardware_interface.cpp +++ b/src/ur_hardware_interface.cpp @@ -59,225 +59,226 @@ namespace ros_control_ur { -UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) : - nh_(nh), robot_(robot) { - // Initialize shared memory and interfaces here - init(); // this implementation loads from rosparam +UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) + : nh_(nh) + , robot_(robot) +{ + // Initialize shared memory and interfaces here + init(); // this implementation loads from rosparam - max_vel_change_ = 0.12; // equivalent of an acceleration of 15 rad/sec^2 + max_vel_change_ = 0.12; // equivalent of an acceleration of 15 rad/sec^2 - ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface."); + ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface."); } -void UrHardwareInterface::init() { - ROS_INFO_STREAM_NAMED("ur_hardware_interface", - "Reading rosparams from namespace: " << nh_.getNamespace()); +void UrHardwareInterface::init() +{ + ROS_INFO_STREAM_NAMED("ur_hardware_interface", + "Reading rosparams from namespace: " << nh_.getNamespace()); - // Get joint names - nh_.getParam("hardware_interface/joints", joint_names_); - if (joint_names_.size() == 0) { - ROS_FATAL_STREAM_NAMED("ur_hardware_interface", - "No joints found on parameter server for controller, did you load the proper yaml file?" << " Namespace: " << nh_.getNamespace()); - exit(-1); - } - num_joints_ = joint_names_.size(); + // Get joint names + nh_.getParam("hardware_interface/joints", joint_names_); + if (joint_names_.size() == 0) { + ROS_FATAL_STREAM_NAMED("ur_hardware_interface", + "No joints found on parameter server for controller, did you load the proper yaml file?" + << " Namespace: " << nh_.getNamespace()); + exit(-1); + } + num_joints_ = joint_names_.size(); - // Resize vectors - joint_position_.resize(num_joints_); - joint_velocity_.resize(num_joints_); - joint_effort_.resize(num_joints_); - joint_position_command_.resize(num_joints_); - joint_velocity_command_.resize(num_joints_); - prev_joint_velocity_command_.resize(num_joints_); + // Resize vectors + joint_position_.resize(num_joints_); + joint_velocity_.resize(num_joints_); + joint_effort_.resize(num_joints_); + joint_position_command_.resize(num_joints_); + joint_velocity_command_.resize(num_joints_); + prev_joint_velocity_command_.resize(num_joints_); - // Initialize controller - for (std::size_t i = 0; i < num_joints_; ++i) { - ROS_DEBUG_STREAM_NAMED("ur_hardware_interface", - "Loading joint name: " << joint_names_[i]); + // Initialize controller + for (std::size_t i = 0; i < num_joints_; ++i) { + ROS_DEBUG_STREAM_NAMED("ur_hardware_interface", + "Loading joint name: " << joint_names_[i]); - // Create joint state interface - joint_state_interface_.registerHandle( - hardware_interface::JointStateHandle(joint_names_[i], - &joint_position_[i], &joint_velocity_[i], - &joint_effort_[i])); + // Create joint state interface + joint_state_interface_.registerHandle( + hardware_interface::JointStateHandle(joint_names_[i], + &joint_position_[i], &joint_velocity_[i], + &joint_effort_[i])); - // Create position joint interface - position_joint_interface_.registerHandle( - hardware_interface::JointHandle( - joint_state_interface_.getHandle(joint_names_[i]), - &joint_position_command_[i])); + // Create position joint interface + position_joint_interface_.registerHandle( + hardware_interface::JointHandle( + joint_state_interface_.getHandle(joint_names_[i]), + &joint_position_command_[i])); - // Create velocity joint interface - velocity_joint_interface_.registerHandle( - hardware_interface::JointHandle( - joint_state_interface_.getHandle(joint_names_[i]), - &joint_velocity_command_[i])); - prev_joint_velocity_command_[i] = 0.; - } + // Create velocity joint interface + velocity_joint_interface_.registerHandle( + hardware_interface::JointHandle( + joint_state_interface_.getHandle(joint_names_[i]), + &joint_velocity_command_[i])); + prev_joint_velocity_command_[i] = 0.; + } - // Create force torque interface - force_torque_interface_.registerHandle( - hardware_interface::ForceTorqueSensorHandle("wrench", "", - robot_force_, robot_torque_)); + // Create force torque interface + force_torque_interface_.registerHandle( + hardware_interface::ForceTorqueSensorHandle("wrench", "", + robot_force_, robot_torque_)); - registerInterface(&joint_state_interface_); // From RobotHW base class. - registerInterface(&position_joint_interface_); // From RobotHW base class. - registerInterface(&velocity_joint_interface_); // From RobotHW base class. - registerInterface(&force_torque_interface_); // From RobotHW base class. - velocity_interface_running_ = false; - position_interface_running_ = false; + registerInterface(&joint_state_interface_); // From RobotHW base class. + registerInterface(&position_joint_interface_); // From RobotHW base class. + registerInterface(&velocity_joint_interface_); // From RobotHW base class. + registerInterface(&force_torque_interface_); // From RobotHW base class. + velocity_interface_running_ = false; + position_interface_running_ = false; } -void UrHardwareInterface::read() { - std::vector pos, vel, current, tcp; - pos = robot_->rt_interface_->robot_state_->getQActual(); - vel = robot_->rt_interface_->robot_state_->getQdActual(); - current = robot_->rt_interface_->robot_state_->getIActual(); - tcp = robot_->rt_interface_->robot_state_->getTcpForce(); - for (std::size_t i = 0; i < num_joints_; ++i) { - joint_position_[i] = pos[i]; - joint_velocity_[i] = vel[i]; - joint_effort_[i] = current[i]; - } - for (std::size_t i = 0; i < 3; ++i) { - robot_force_[i] = tcp[i]; - robot_torque_[i] = tcp[i + 3]; - } - +void UrHardwareInterface::read() +{ + std::vector pos, vel, current, tcp; + pos = robot_->rt_interface_->robot_state_->getQActual(); + vel = robot_->rt_interface_->robot_state_->getQdActual(); + current = robot_->rt_interface_->robot_state_->getIActual(); + tcp = robot_->rt_interface_->robot_state_->getTcpForce(); + for (std::size_t i = 0; i < num_joints_; ++i) { + joint_position_[i] = pos[i]; + joint_velocity_[i] = vel[i]; + joint_effort_[i] = current[i]; + } + for (std::size_t i = 0; i < 3; ++i) { + robot_force_[i] = tcp[i]; + robot_torque_[i] = tcp[i + 3]; + } } -void UrHardwareInterface::setMaxVelChange(double inp) { - max_vel_change_ = inp; +void UrHardwareInterface::setMaxVelChange(double inp) +{ + max_vel_change_ = inp; } -void UrHardwareInterface::write() { - if (velocity_interface_running_) { - std::vector cmd; - //do some rate limiting - cmd.resize(joint_velocity_command_.size()); - for (unsigned int i = 0; i < joint_velocity_command_.size(); i++) { - cmd[i] = joint_velocity_command_[i]; - if (cmd[i] > prev_joint_velocity_command_[i] + max_vel_change_) { - cmd[i] = prev_joint_velocity_command_[i] + max_vel_change_; - } else if (cmd[i] - < prev_joint_velocity_command_[i] - max_vel_change_) { - cmd[i] = prev_joint_velocity_command_[i] - max_vel_change_; - } - prev_joint_velocity_command_[i] = cmd[i]; - } - robot_->setSpeed(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], max_vel_change_*125); - } else if (position_interface_running_) { - robot_->servoj(joint_position_command_); - } +void UrHardwareInterface::write() +{ + if (velocity_interface_running_) { + std::vector cmd; + //do some rate limiting + cmd.resize(joint_velocity_command_.size()); + for (unsigned int i = 0; i < joint_velocity_command_.size(); i++) { + cmd[i] = joint_velocity_command_[i]; + if (cmd[i] > prev_joint_velocity_command_[i] + max_vel_change_) { + cmd[i] = prev_joint_velocity_command_[i] + max_vel_change_; + } else if (cmd[i] + < prev_joint_velocity_command_[i] - max_vel_change_) { + cmd[i] = prev_joint_velocity_command_[i] - max_vel_change_; + } + prev_joint_velocity_command_[i] = cmd[i]; + } + robot_->setSpeed(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], max_vel_change_ * 125); + } else if (position_interface_running_) { + robot_->servoj(joint_position_command_); + } } bool UrHardwareInterface::canSwitch( - const std::list &start_list, - const std::list &stop_list) const { - for (std::list::const_iterator controller_it = - start_list.begin(); controller_it != start_list.end(); - ++controller_it) { - if (controller_it->hardware_interface - == "hardware_interface::VelocityJointInterface") { - if (velocity_interface_running_) { - ROS_ERROR( - "%s: An interface of that type (%s) is already running", - controller_it->name.c_str(), - controller_it->hardware_interface.c_str()); - return false; - } - if (position_interface_running_) { - bool error = true; - for (std::list::const_iterator stop_controller_it = - stop_list.begin(); - stop_controller_it != stop_list.end(); - ++stop_controller_it) { - if (stop_controller_it->hardware_interface - == "hardware_interface::PositionJointInterface") { - error = false; - break; - } - } - if (error) { - ROS_ERROR( - "%s (type %s) can not be run simultaneously with a PositionJointInterface", - controller_it->name.c_str(), - controller_it->hardware_interface.c_str()); - return false; - } - } - } else if (controller_it->hardware_interface - == "hardware_interface::PositionJointInterface") { - if (position_interface_running_) { - ROS_ERROR( - "%s: An interface of that type (%s) is already running", - controller_it->name.c_str(), - controller_it->hardware_interface.c_str()); - return false; - } - if (velocity_interface_running_) { - bool error = true; - for (std::list::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; - } - } - } - } + const std::list& start_list, + const std::list& stop_list) const +{ + for (std::list::const_iterator controller_it = start_list.begin(); controller_it != start_list.end(); + ++controller_it) { + if (controller_it->hardware_interface + == "hardware_interface::VelocityJointInterface") { + if (velocity_interface_running_) { + ROS_ERROR( + "%s: An interface of that type (%s) is already running", + controller_it->name.c_str(), + controller_it->hardware_interface.c_str()); + return false; + } + if (position_interface_running_) { + bool error = true; + for (std::list::const_iterator stop_controller_it = stop_list.begin(); + stop_controller_it != stop_list.end(); + ++stop_controller_it) { + if (stop_controller_it->hardware_interface + == "hardware_interface::PositionJointInterface") { + error = false; + break; + } + } + if (error) { + ROS_ERROR( + "%s (type %s) can not be run simultaneously with a PositionJointInterface", + controller_it->name.c_str(), + controller_it->hardware_interface.c_str()); + return false; + } + } + } else if (controller_it->hardware_interface + == "hardware_interface::PositionJointInterface") { + if (position_interface_running_) { + ROS_ERROR( + "%s: An interface of that type (%s) is already running", + controller_it->name.c_str(), + controller_it->hardware_interface.c_str()); + return false; + } + if (velocity_interface_running_) { + bool error = true; + for (std::list::const_iterator stop_controller_it = stop_list.begin(); + stop_controller_it != stop_list.end(); + ++stop_controller_it) { + if (stop_controller_it->hardware_interface + == "hardware_interface::VelocityJointInterface") { + error = false; + break; + } + } + if (error) { + ROS_ERROR( + "%s (type %s) can not be run simultaneously with a VelocityJointInterface", + controller_it->name.c_str(), + controller_it->hardware_interface.c_str()); + return false; + } + } + } + } -// we can always stop a controller - return true; + // we can always stop a controller + return true; } void UrHardwareInterface::doSwitch( - const std::list& start_list, - const std::list& stop_list) { - for (std::list::const_iterator controller_it = - stop_list.begin(); controller_it != stop_list.end(); - ++controller_it) { - if (controller_it->hardware_interface - == "hardware_interface::VelocityJointInterface") { - velocity_interface_running_ = false; - ROS_DEBUG("Stopping velocity interface"); - } - if (controller_it->hardware_interface - == "hardware_interface::PositionJointInterface") { - position_interface_running_ = false; - std::vector tmp; - robot_->closeServo(tmp); - ROS_DEBUG("Stopping position interface"); - } - } - for (std::list::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"); - } - } - + const std::list& start_list, + const std::list& stop_list) +{ + for (std::list::const_iterator controller_it = stop_list.begin(); controller_it != stop_list.end(); + ++controller_it) { + if (controller_it->hardware_interface + == "hardware_interface::VelocityJointInterface") { + velocity_interface_running_ = false; + ROS_DEBUG("Stopping velocity interface"); + } + if (controller_it->hardware_interface + == "hardware_interface::PositionJointInterface") { + position_interface_running_ = false; + std::vector tmp; + robot_->closeServo(tmp); + ROS_DEBUG("Stopping position interface"); + } + } + for (std::list::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 - diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp index 90f0cfe..b45224b 100644 --- a/src/ur_realtime_communication.cpp +++ b/src/ur_realtime_communication.cpp @@ -19,175 +19,182 @@ #include "ur_modern_driver/ur_realtime_communication.h" UrRealtimeCommunication::UrRealtimeCommunication( - std::condition_variable& msg_cond, std::string host, - unsigned int safety_count_max) { - robot_state_ = new RobotStateRT(msg_cond); - bzero((char *) &serv_addr_, sizeof(serv_addr_)); - sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (sockfd_ < 0) { - print_fatal("ERROR opening socket"); - } - server_ = gethostbyname(host.c_str()); - if (server_ == NULL) { - print_fatal("ERROR, no such host"); - } - serv_addr_.sin_family = AF_INET; - bcopy((char *) server_->h_addr, (char *)&serv_addr_.sin_addr.s_addr, server_->h_length); - serv_addr_.sin_port = htons(30003); - flag_ = 1; - setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int)); - setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, sizeof(int)); - setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int)); - fcntl(sockfd_, F_SETFL, O_NONBLOCK); - connected_ = false; - keepalive_ = false; - safety_count_ = safety_count_max + 1; - safety_count_max_ = safety_count_max; + std::condition_variable& msg_cond, std::string host, + unsigned int safety_count_max) +{ + robot_state_ = new RobotStateRT(msg_cond); + bzero((char*)&serv_addr_, sizeof(serv_addr_)); + sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (sockfd_ < 0) { + print_fatal("ERROR opening socket"); + } + server_ = gethostbyname(host.c_str()); + if (server_ == NULL) { + print_fatal("ERROR, no such host"); + } + serv_addr_.sin_family = AF_INET; + bcopy((char*)server_->h_addr, (char*)&serv_addr_.sin_addr.s_addr, server_->h_length); + serv_addr_.sin_port = htons(30003); + flag_ = 1; + setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int)); + setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); + setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int)); + fcntl(sockfd_, F_SETFL, O_NONBLOCK); + connected_ = false; + keepalive_ = false; + safety_count_ = safety_count_max + 1; + safety_count_max_ = safety_count_max; } -bool UrRealtimeCommunication::start() { - fd_set writefds; - struct timeval timeout; +bool UrRealtimeCommunication::start() +{ + fd_set writefds; + struct timeval timeout; - keepalive_ = true; - print_debug("Realtime port: Connecting..."); + keepalive_ = true; + print_debug("Realtime port: Connecting..."); - connect(sockfd_, (struct sockaddr *) &serv_addr_, sizeof(serv_addr_)); - FD_ZERO(&writefds); - FD_SET(sockfd_, &writefds); - timeout.tv_sec = 10; - timeout.tv_usec = 0; - select(sockfd_ + 1, NULL, &writefds, NULL, &timeout); - unsigned int flag_len; - getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); - if (flag_ < 0) { - print_fatal("Error connecting to RT port 30003"); - return false; - } - sockaddr_in name; - socklen_t namelen = sizeof(name); - int err = getsockname(sockfd_, (sockaddr*) &name, &namelen); - if (err < 0) { - print_fatal("Could not get local IP"); - close(sockfd_); - return false; - } - char str[18]; - inet_ntop(AF_INET, &name.sin_addr, str, 18); - local_ip_ = str; - comThread_ = std::thread(&UrRealtimeCommunication::run, this); - return true; + connect(sockfd_, (struct sockaddr*)&serv_addr_, sizeof(serv_addr_)); + FD_ZERO(&writefds); + FD_SET(sockfd_, &writefds); + timeout.tv_sec = 10; + timeout.tv_usec = 0; + select(sockfd_ + 1, NULL, &writefds, NULL, &timeout); + unsigned int flag_len; + getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); + if (flag_ < 0) { + print_fatal("Error connecting to RT port 30003"); + return false; + } + sockaddr_in name; + socklen_t namelen = sizeof(name); + int err = getsockname(sockfd_, (sockaddr*)&name, &namelen); + if (err < 0) { + print_fatal("Could not get local IP"); + close(sockfd_); + return false; + } + char str[18]; + inet_ntop(AF_INET, &name.sin_addr, str, 18); + local_ip_ = str; + comThread_ = std::thread(&UrRealtimeCommunication::run, this); + return true; } -void UrRealtimeCommunication::halt() { - keepalive_ = false; - comThread_.join(); +void UrRealtimeCommunication::halt() +{ + keepalive_ = false; + comThread_.join(); } -void UrRealtimeCommunication::addCommandToQueue(std::string inp) { - int bytes_written; - if (inp.back() != '\n') { - inp.append("\n"); - } - if (connected_) - bytes_written = write(sockfd_, inp.c_str(), inp.length()); - else - print_error("Could not send command \"" +inp + "\". The robot is not connected! Command is discarded" ); +void UrRealtimeCommunication::addCommandToQueue(std::string inp) +{ + int bytes_written; + if (inp.back() != '\n') { + inp.append("\n"); + } + if (connected_) + bytes_written = write(sockfd_, inp.c_str(), inp.length()); + else + print_error("Could not send command \"" + inp + "\". The robot is not connected! Command is discarded"); } void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2, - double q3, double q4, double q5, double acc) { - char cmd[1024]; - if( robot_state_->getVersion() >= 3.1 ) { - sprintf(cmd, - "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n", - q0, q1, q2, q3, q4, q5, acc); - } - else { - sprintf(cmd, - "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n", - q0, q1, q2, q3, q4, q5, acc); - } - addCommandToQueue((std::string) (cmd)); - if (q0 != 0. or q1 != 0. or q2 != 0. or q3 != 0. or q4 != 0. or q5 != 0.) { - //If a joint speed is set, make sure we stop it again after some time if the user doesn't - safety_count_ = 0; - } + double q3, double q4, double q5, double acc) +{ + char cmd[1024]; + if (robot_state_->getVersion() >= 3.1) { + sprintf(cmd, + "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n", + q0, q1, q2, q3, q4, q5, acc); + } else { + sprintf(cmd, + "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n", + q0, q1, q2, q3, q4, q5, acc); + } + addCommandToQueue((std::string)(cmd)); + if (q0 != 0. or q1 != 0. or q2 != 0. or q3 != 0. or q4 != 0. or q5 != 0.) { + //If a joint speed is set, make sure we stop it again after some time if the user doesn't + safety_count_ = 0; + } } -void UrRealtimeCommunication::run() { - uint8_t buf[2048]; - int bytes_read; - bzero(buf, 2048); - struct timeval timeout; - fd_set readfds; - FD_ZERO(&readfds); - FD_SET(sockfd_, &readfds); - print_debug("Realtime port: Got connection"); - connected_ = true; - while (keepalive_) { - while (connected_ && keepalive_) { - timeout.tv_sec = 0; //do this each loop as selects modifies timeout - timeout.tv_usec = 500000; // timeout of 0.5 sec - select(sockfd_ + 1, &readfds, NULL, NULL, &timeout); - bytes_read = read(sockfd_, buf, 2048); - if (bytes_read > 0) { - setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, - sizeof(int)); - robot_state_->unpack(buf); - if (safety_count_ == safety_count_max_) { - setSpeed(0., 0., 0., 0., 0., 0.); - } - safety_count_ += 1; - } else { - connected_ = false; - close(sockfd_); - } - } - if (keepalive_) { - //reconnect - print_warning("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."); - sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (sockfd_ < 0) { - print_fatal("ERROR opening socket"); - } - flag_ = 1; - setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, - sizeof(int)); - setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, - sizeof(int)); - - setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, - sizeof(int)); - fcntl(sockfd_, F_SETFL, O_NONBLOCK); - while (keepalive_ && !connected_) { - std::this_thread::sleep_for(std::chrono::seconds(10)); - fd_set writefds; +void UrRealtimeCommunication::run() +{ + uint8_t buf[2048]; + int bytes_read; + bzero(buf, 2048); + struct timeval timeout; + fd_set readfds; + FD_ZERO(&readfds); + FD_SET(sockfd_, &readfds); + print_debug("Realtime port: Got connection"); + connected_ = true; + while (keepalive_) { + while (connected_ && keepalive_) { + timeout.tv_sec = 0; //do this each loop as selects modifies timeout + timeout.tv_usec = 500000; // timeout of 0.5 sec + select(sockfd_ + 1, &readfds, NULL, NULL, &timeout); + bytes_read = read(sockfd_, buf, 2048); + if (bytes_read > 0) { + setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, + sizeof(int)); + robot_state_->unpack(buf); + if (safety_count_ == safety_count_max_) { + setSpeed(0., 0., 0., 0., 0., 0.); + } + safety_count_ += 1; + } else { + connected_ = false; + close(sockfd_); + } + } + if (keepalive_) { + //reconnect + print_warning("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."); + sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (sockfd_ < 0) { + print_fatal("ERROR opening socket"); + } + flag_ = 1; + setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, + sizeof(int)); + setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, + sizeof(int)); - 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_); + 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; +void UrRealtimeCommunication::setSafetyCountMax(uint inp) +{ + safety_count_max_ = inp; } -std::string UrRealtimeCommunication::getLocalIp() { - return local_ip_; +std::string UrRealtimeCommunication::getLocalIp() +{ + return local_ip_; } diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index ab74534..92fe341 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -16,40 +16,40 @@ * limitations under the License. */ +#include "ur_modern_driver/do_output.h" #include "ur_modern_driver/ur_driver.h" #include "ur_modern_driver/ur_hardware_interface.h" -#include "ur_modern_driver/do_output.h" -#include -#include -#include -#include -#include #include -#include #include +#include +#include +#include +#include +#include #include +#include -#include "ros/ros.h" -#include -#include "sensor_msgs/JointState.h" -#include "geometry_msgs/WrenchStamped.h" -#include "geometry_msgs/PoseStamped.h" -#include "control_msgs/FollowJointTrajectoryAction.h" #include "actionlib/server/action_server.h" #include "actionlib/server/server_goal_handle.h" +#include "control_msgs/FollowJointTrajectoryAction.h" +#include "geometry_msgs/PoseStamped.h" +#include "geometry_msgs/WrenchStamped.h" +#include "ros/ros.h" +#include "sensor_msgs/JointState.h" +#include "std_msgs/String.h" #include "trajectory_msgs/JointTrajectoryPoint.h" +#include "ur_msgs/Analog.h" +#include "ur_msgs/Digital.h" +#include "ur_msgs/IOStates.h" #include "ur_msgs/SetIO.h" +#include "ur_msgs/SetIORequest.h" +#include "ur_msgs/SetIOResponse.h" #include "ur_msgs/SetPayload.h" #include "ur_msgs/SetPayloadRequest.h" #include "ur_msgs/SetPayloadResponse.h" -#include "ur_msgs/SetIORequest.h" -#include "ur_msgs/SetIOResponse.h" -#include "ur_msgs/IOStates.h" -#include "ur_msgs/Digital.h" -#include "ur_msgs/Analog.h" -#include "std_msgs/String.h" #include #include +#include /// TF #include @@ -57,125 +57,129 @@ class RosWrapper { protected: - UrDriver robot_; - std::condition_variable rt_msg_cond_; - std::condition_variable msg_cond_; - ros::NodeHandle nh_; - actionlib::ActionServer as_; - actionlib::ServerGoalHandle goal_handle_; - bool has_goal_; - control_msgs::FollowJointTrajectoryFeedback feedback_; - control_msgs::FollowJointTrajectoryResult result_; - ros::Subscriber speed_sub_; - ros::Subscriber urscript_sub_; - ros::ServiceServer io_srv_; - ros::ServiceServer payload_srv_; - std::thread* rt_publish_thread_; - std::thread* mb_publish_thread_; - double io_flag_delay_; - double max_velocity_; - std::vector joint_offsets_; + UrDriver robot_; + std::condition_variable rt_msg_cond_; + std::condition_variable msg_cond_; + ros::NodeHandle nh_; + actionlib::ActionServer as_; + actionlib::ServerGoalHandle goal_handle_; + bool has_goal_; + control_msgs::FollowJointTrajectoryFeedback feedback_; + control_msgs::FollowJointTrajectoryResult result_; + ros::Subscriber speed_sub_; + ros::Subscriber urscript_sub_; + ros::ServiceServer io_srv_; + ros::ServiceServer payload_srv_; + std::thread* rt_publish_thread_; + std::thread* mb_publish_thread_; + double io_flag_delay_; + double max_velocity_; + std::vector joint_offsets_; std::string base_frame_; std::string tool_frame_; - bool use_ros_control_; - std::thread* ros_control_thread_; - boost::shared_ptr hardware_interface_; - boost::shared_ptr controller_manager_; + bool use_ros_control_; + std::thread* ros_control_thread_; + boost::shared_ptr hardware_interface_; + boost::shared_ptr controller_manager_; public: - RosWrapper(std::string host, int reverse_port) : - as_(nh_, "follow_joint_trajectory", - boost::bind(&RosWrapper::goalCB, this, _1), - boost::bind(&RosWrapper::cancelCB, this, _1), false), robot_( - rt_msg_cond_, msg_cond_, host, reverse_port, 0.03, 300), io_flag_delay_(0.05), joint_offsets_( - 6, 0.0) { + RosWrapper(std::string host, int reverse_port) + : as_(nh_, "follow_joint_trajectory", + boost::bind(&RosWrapper::goalCB, this, _1), + boost::bind(&RosWrapper::cancelCB, this, _1), false) + , robot_( + rt_msg_cond_, msg_cond_, host, reverse_port, 0.03, 300) + , io_flag_delay_(0.05) + , joint_offsets_( + 6, 0.0) + { - std::string joint_prefix = ""; - std::vector joint_names; - char buf[256]; + std::string joint_prefix = ""; + std::vector joint_names; + char buf[256]; - if (ros::param::get("~prefix", joint_prefix)) { - if (joint_prefix.length() > 0) { - sprintf(buf, "Setting prefix to %s", joint_prefix.c_str()); - print_info(buf); - } + if (ros::param::get("~prefix", joint_prefix)) { + if (joint_prefix.length() > 0) { + sprintf(buf, "Setting prefix to %s", joint_prefix.c_str()); + print_info(buf); + } } - joint_names.push_back(joint_prefix + "shoulder_pan_joint"); - joint_names.push_back(joint_prefix + "shoulder_lift_joint"); - joint_names.push_back(joint_prefix + "elbow_joint"); - joint_names.push_back(joint_prefix + "wrist_1_joint"); - joint_names.push_back(joint_prefix + "wrist_2_joint"); - joint_names.push_back(joint_prefix + "wrist_3_joint"); - robot_.setJointNames(joint_names); + joint_names.push_back(joint_prefix + "shoulder_pan_joint"); + joint_names.push_back(joint_prefix + "shoulder_lift_joint"); + joint_names.push_back(joint_prefix + "elbow_joint"); + joint_names.push_back(joint_prefix + "wrist_1_joint"); + joint_names.push_back(joint_prefix + "wrist_2_joint"); + joint_names.push_back(joint_prefix + "wrist_3_joint"); + robot_.setJointNames(joint_names); - use_ros_control_ = false; - ros::param::get("~use_ros_control", use_ros_control_); + use_ros_control_ = false; + ros::param::get("~use_ros_control", use_ros_control_); - if (use_ros_control_) { - hardware_interface_.reset( - new ros_control_ur::UrHardwareInterface(nh_, &robot_)); - controller_manager_.reset( - new controller_manager::ControllerManager( - hardware_interface_.get(), nh_)); - double max_vel_change = 0.12; // equivalent of an acceleration of 15 rad/sec^2 - if (ros::param::get("~max_acceleration", max_vel_change)) { - max_vel_change = max_vel_change / 125; - } - sprintf(buf, "Max acceleration set to: %f [rad/sec²]", - max_vel_change * 125); - print_debug(buf); - hardware_interface_->setMaxVelChange(max_vel_change); - } - //Using a very high value in order to not limit execution of trajectories being sent from MoveIt! - max_velocity_ = 10.; - if (ros::param::get("~max_velocity", max_velocity_)) { - sprintf(buf, "Max velocity accepted by ur_driver: %f [rad/s]", - max_velocity_); - print_debug(buf); - } + if (use_ros_control_) { + hardware_interface_.reset( + new ros_control_ur::UrHardwareInterface(nh_, &robot_)); + controller_manager_.reset( + new controller_manager::ControllerManager( + hardware_interface_.get(), nh_)); + double max_vel_change = 0.12; // equivalent of an acceleration of 15 rad/sec^2 + if (ros::param::get("~max_acceleration", max_vel_change)) { + max_vel_change = max_vel_change / 125; + } + sprintf(buf, "Max acceleration set to: %f [rad/sec²]", + max_vel_change * 125); + print_debug(buf); + hardware_interface_->setMaxVelChange(max_vel_change); + } + //Using a very high value in order to not limit execution of trajectories being sent from MoveIt! + max_velocity_ = 10.; + if (ros::param::get("~max_velocity", max_velocity_)) { + sprintf(buf, "Max velocity accepted by ur_driver: %f [rad/s]", + max_velocity_); + print_debug(buf); + } - //Bounds for SetPayload service - //Using a very conservative value as it should be set through the parameter server - double min_payload = 0.; - double max_payload = 1.; - if (ros::param::get("~min_payload", min_payload)) { - sprintf(buf, "Min payload set to: %f [kg]", min_payload); - print_debug(buf); - } - if (ros::param::get("~max_payload", max_payload)) { - sprintf(buf, "Max payload set to: %f [kg]", max_payload); - print_debug(buf); - } - robot_.setMinPayload(min_payload); - robot_.setMaxPayload(max_payload); - sprintf(buf, "Bounds for set_payload service calls: [%f, %f]", - min_payload, max_payload); - print_debug(buf); + //Bounds for SetPayload service + //Using a very conservative value as it should be set through the parameter server + double min_payload = 0.; + double max_payload = 1.; + if (ros::param::get("~min_payload", min_payload)) { + sprintf(buf, "Min payload set to: %f [kg]", min_payload); + print_debug(buf); + } + if (ros::param::get("~max_payload", max_payload)) { + sprintf(buf, "Max payload set to: %f [kg]", max_payload); + print_debug(buf); + } + robot_.setMinPayload(min_payload); + robot_.setMaxPayload(max_payload); + sprintf(buf, "Bounds for set_payload service calls: [%f, %f]", + min_payload, max_payload); + print_debug(buf); - double servoj_time = 0.008; - if (ros::param::get("~servoj_time", servoj_time)) { - sprintf(buf, "Servoj_time set to: %f [sec]", servoj_time); - print_debug(buf); - } - robot_.setServojTime(servoj_time); + double servoj_time = 0.008; + if (ros::param::get("~servoj_time", servoj_time)) { + sprintf(buf, "Servoj_time set to: %f [sec]", servoj_time); + print_debug(buf); + } + robot_.setServojTime(servoj_time); - double servoj_lookahead_time = 0.03; - if (ros::param::get("~servoj_lookahead_time", servoj_lookahead_time)) { - sprintf(buf, "Servoj_lookahead_time set to: %f [sec]", servoj_lookahead_time); - print_debug(buf); - } - robot_.setServojLookahead(servoj_lookahead_time); + double servoj_lookahead_time = 0.03; + if (ros::param::get("~servoj_lookahead_time", servoj_lookahead_time)) { + sprintf(buf, "Servoj_lookahead_time set to: %f [sec]", servoj_lookahead_time); + print_debug(buf); + } + robot_.setServojLookahead(servoj_lookahead_time); - double servoj_gain = 300.; - if (ros::param::get("~servoj_gain", servoj_gain)) { - sprintf(buf, "Servoj_gain set to: %f [sec]", servoj_gain); - print_debug(buf); - } - robot_.setServojGain(servoj_gain); + double servoj_gain = 300.; + if (ros::param::get("~servoj_gain", servoj_gain)) { + sprintf(buf, "Servoj_gain set to: %f [sec]", servoj_gain); + print_debug(buf); + } + robot_.setServojGain(servoj_gain); //Base and tool frames base_frame_ = joint_prefix + "base_link"; - tool_frame_ = joint_prefix + "tool0_controller"; + tool_frame_ = joint_prefix + "tool0_controller"; if (ros::param::get("~base_frame", base_frame_)) { sprintf(buf, "Base frame set to: %s", base_frame_.c_str()); print_debug(buf); @@ -185,506 +189,496 @@ public: print_debug(buf); } - if (robot_.start()) { - if (use_ros_control_) { - ros_control_thread_ = new std::thread( - boost::bind(&RosWrapper::rosControlLoop, this)); - print_debug( - "The control thread for this driver has been started"); - } else { - //start actionserver - has_goal_ = false; - as_.start(); + if (robot_.start()) { + if (use_ros_control_) { + ros_control_thread_ = new std::thread( + boost::bind(&RosWrapper::rosControlLoop, this)); + print_debug( + "The control thread for this driver has been started"); + } else { + //start actionserver + has_goal_ = false; + as_.start(); - //subscribe to the data topic of interest - rt_publish_thread_ = new std::thread( - boost::bind(&RosWrapper::publishRTMsg, this)); - print_debug( - "The action server for this driver has been started"); - } - mb_publish_thread_ = new std::thread( - boost::bind(&RosWrapper::publishMbMsg, this)); - speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1, - &RosWrapper::speedInterface, this); - urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, - &RosWrapper::urscriptInterface, this); + //subscribe to the data topic of interest + rt_publish_thread_ = new std::thread( + boost::bind(&RosWrapper::publishRTMsg, this)); + print_debug( + "The action server for this driver has been started"); + } + mb_publish_thread_ = new std::thread( + boost::bind(&RosWrapper::publishMbMsg, this)); + speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1, + &RosWrapper::speedInterface, this); + urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, + &RosWrapper::urscriptInterface, this); - io_srv_ = nh_.advertiseService("ur_driver/set_io", - &RosWrapper::setIO, this); - payload_srv_ = nh_.advertiseService("ur_driver/set_payload", - &RosWrapper::setPayload, this); - } - } + io_srv_ = nh_.advertiseService("ur_driver/set_io", + &RosWrapper::setIO, this); + payload_srv_ = nh_.advertiseService("ur_driver/set_payload", + &RosWrapper::setPayload, this); + } + } - void halt() { - robot_.halt(); - rt_publish_thread_->join(); + void halt() + { + robot_.halt(); + rt_publish_thread_->join(); + } - } private: - void trajThread(std::vector timestamps, - std::vector > positions, - std::vector > velocities) { + void trajThread(std::vector timestamps, + std::vector > positions, + std::vector > velocities) + { - robot_.doTraj(timestamps, positions, velocities); - if (has_goal_) { - result_.error_code = result_.SUCCESSFUL; - goal_handle_.setSucceeded(result_); - has_goal_ = false; - } - } - void goalCB( - actionlib::ServerGoalHandle< - control_msgs::FollowJointTrajectoryAction> gh) { - std::string buf; - print_info("on_goal"); - if (!robot_.sec_interface_->robot_state_->isReady()) { - result_.error_code = -100; //nothing is defined for this...? + robot_.doTraj(timestamps, positions, velocities); + if (has_goal_) { + result_.error_code = result_.SUCCESSFUL; + goal_handle_.setSucceeded(result_); + has_goal_ = false; + } + } + void goalCB( + actionlib::ServerGoalHandle gh) + { + std::string buf; + print_info("on_goal"); + if (!robot_.sec_interface_->robot_state_->isReady()) { + result_.error_code = -100; //nothing is defined for this...? - if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) { - result_.error_string = - "Cannot accept new trajectories: Robot arm is not powered on"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - if (!robot_.sec_interface_->robot_state_->isRealRobotEnabled()) { - result_.error_string = - "Cannot accept new trajectories: Robot is not enabled"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - result_.error_string = - "Cannot accept new trajectories. (Debug: Robot mode is " - + std::to_string( - robot_.sec_interface_->robot_state_->getRobotMode()) - + ")"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - if (robot_.sec_interface_->robot_state_->isEmergencyStopped()) { - result_.error_code = -100; //nothing is defined for this...? - result_.error_string = - "Cannot accept new trajectories: Robot is emergency stopped"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - if (robot_.sec_interface_->robot_state_->isProtectiveStopped()) { - result_.error_code = -100; //nothing is defined for this...? - result_.error_string = - "Cannot accept new trajectories: Robot is protective stopped"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } + if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) { + result_.error_string = "Cannot accept new trajectories: Robot arm is not powered on"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + if (!robot_.sec_interface_->robot_state_->isRealRobotEnabled()) { + result_.error_string = "Cannot accept new trajectories: Robot is not enabled"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + result_.error_string = "Cannot accept new trajectories. (Debug: Robot mode is " + + std::to_string( + robot_.sec_interface_->robot_state_->getRobotMode()) + + ")"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + if (robot_.sec_interface_->robot_state_->isEmergencyStopped()) { + result_.error_code = -100; //nothing is defined for this...? + result_.error_string = "Cannot accept new trajectories: Robot is emergency stopped"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + if (robot_.sec_interface_->robot_state_->isProtectiveStopped()) { + result_.error_code = -100; //nothing is defined for this...? + result_.error_string = "Cannot accept new trajectories: Robot is protective stopped"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } - actionlib::ActionServer::Goal goal = - *gh.getGoal(); //make a copy that we can modify - if (has_goal_) { - print_warning( - "Received new goal while still executing previous trajectory. Canceling previous trajectory"); - has_goal_ = false; - robot_.stopTraj(); - result_.error_code = -100; //nothing is defined for this...? - result_.error_string = "Received another trajectory"; - goal_handle_.setAborted(result_, result_.error_string); - std::this_thread::sleep_for(std::chrono::milliseconds(250)); - } - goal_handle_ = gh; - if (!validateJointNames()) { - std::string outp_joint_names = ""; - for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); - i++) { - outp_joint_names += goal.trajectory.joint_names[i] + " "; - } - result_.error_code = result_.INVALID_JOINTS; - result_.error_string = - "Received a goal with incorrect joint names: " - + outp_joint_names; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - if (!has_positions()) { - result_.error_code = result_.INVALID_GOAL; - result_.error_string = "Received a goal without positions"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - - if (!has_velocities()) { - result_.error_code = result_.INVALID_GOAL; - result_.error_string = "Received a goal without velocities"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } + actionlib::ActionServer::Goal goal = *gh.getGoal(); //make a copy that we can modify + if (has_goal_) { + print_warning( + "Received new goal while still executing previous trajectory. Canceling previous trajectory"); + has_goal_ = false; + robot_.stopTraj(); + result_.error_code = -100; //nothing is defined for this...? + result_.error_string = "Received another trajectory"; + goal_handle_.setAborted(result_, result_.error_string); + std::this_thread::sleep_for(std::chrono::milliseconds(250)); + } + goal_handle_ = gh; + if (!validateJointNames()) { + std::string outp_joint_names = ""; + for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); + i++) { + outp_joint_names += goal.trajectory.joint_names[i] + " "; + } + result_.error_code = result_.INVALID_JOINTS; + result_.error_string = "Received a goal with incorrect joint names: " + + outp_joint_names; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + if (!has_positions()) { + result_.error_code = result_.INVALID_GOAL; + result_.error_string = "Received a goal without positions"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } - if (!traj_is_finite()) { - result_.error_string = "Received a goal with infinities or NaNs"; - result_.error_code = result_.INVALID_GOAL; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } + if (!has_velocities()) { + result_.error_code = result_.INVALID_GOAL; + result_.error_string = "Received a goal without velocities"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } - if (!has_limited_velocities()) { - result_.error_code = result_.INVALID_GOAL; - result_.error_string = - "Received a goal with velocities that are higher than " - + std::to_string(max_velocity_); - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } + if (!traj_is_finite()) { + result_.error_string = "Received a goal with infinities or NaNs"; + result_.error_code = result_.INVALID_GOAL; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } - reorder_traj_joints(goal.trajectory); - - if (!start_positions_match(goal.trajectory, 0.01)) { - result_.error_code = result_.INVALID_GOAL; - result_.error_string = "Goal start doesn't match current pose"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } + if (!has_limited_velocities()) { + result_.error_code = result_.INVALID_GOAL; + result_.error_string = "Received a goal with velocities that are higher than " + + std::to_string(max_velocity_); + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } - std::vector timestamps; - std::vector > positions, velocities; - if (goal.trajectory.points[0].time_from_start.toSec() != 0.) { - print_warning( - "Trajectory's first point should be the current position, with time_from_start set to 0.0 - Inserting point in malformed trajectory"); - timestamps.push_back(0.0); - positions.push_back( - robot_.rt_interface_->robot_state_->getQActual()); - velocities.push_back( - robot_.rt_interface_->robot_state_->getQdActual()); - } - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { - timestamps.push_back( - goal.trajectory.points[i].time_from_start.toSec()); - positions.push_back(goal.trajectory.points[i].positions); - velocities.push_back(goal.trajectory.points[i].velocities); + reorder_traj_joints(goal.trajectory); - } + if (!start_positions_match(goal.trajectory, 0.01)) { + result_.error_code = result_.INVALID_GOAL; + result_.error_string = "Goal start doesn't match current pose"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } - goal_handle_.setAccepted(); - has_goal_ = true; - std::thread(&RosWrapper::trajThread, this, timestamps, positions, - velocities).detach(); - } + std::vector timestamps; + std::vector > positions, velocities; + if (goal.trajectory.points[0].time_from_start.toSec() != 0.) { + print_warning( + "Trajectory's first point should be the current position, with time_from_start set to 0.0 - Inserting point in malformed trajectory"); + timestamps.push_back(0.0); + positions.push_back( + robot_.rt_interface_->robot_state_->getQActual()); + velocities.push_back( + robot_.rt_interface_->robot_state_->getQdActual()); + } + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { + timestamps.push_back( + goal.trajectory.points[i].time_from_start.toSec()); + positions.push_back(goal.trajectory.points[i].positions); + velocities.push_back(goal.trajectory.points[i].velocities); + } - void cancelCB( - actionlib::ServerGoalHandle< - control_msgs::FollowJointTrajectoryAction> gh) { - // set the action state to preempted - print_info("on_cancel"); - if (has_goal_) { - if (gh == goal_handle_) { - robot_.stopTraj(); - has_goal_ = false; - } - } - result_.error_code = -100; //nothing is defined for this...? - result_.error_string = "Goal cancelled by client"; - gh.setCanceled(result_); - } + goal_handle_.setAccepted(); + has_goal_ = true; + std::thread(&RosWrapper::trajThread, this, timestamps, positions, + velocities) + .detach(); + } - bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp) { - resp.success = true; - //if (req.fun == ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT) { - if (req.fun == 1) { - robot_.setDigitalOut(req.pin, req.state > 0.0 ? true : false); - } else if (req.fun == 2) { - //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_FLAG) { - robot_.setFlag(req.pin, req.state > 0.0 ? true : false); - //According to urdriver.py, set_flag will fail if called to rapidly in succession - ros::Duration(io_flag_delay_).sleep(); - } else if (req.fun == 3) { - //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_ANALOG_OUT) { - robot_.setAnalogOut(req.pin, req.state); - } else if (req.fun == 4) { - //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_TOOL_VOLTAGE) { - robot_.setToolVoltage((int) req.state); - } else { - resp.success = false; - } - return resp.success; - } + void cancelCB( + actionlib::ServerGoalHandle gh) + { + // set the action state to preempted + print_info("on_cancel"); + if (has_goal_) { + if (gh == goal_handle_) { + robot_.stopTraj(); + has_goal_ = false; + } + } + result_.error_code = -100; //nothing is defined for this...? + result_.error_string = "Goal cancelled by client"; + gh.setCanceled(result_); + } - bool setPayload(ur_msgs::SetPayloadRequest& req, - ur_msgs::SetPayloadResponse& resp) { - if (robot_.setPayload(req.payload)) - resp.success = true; - else - resp.success = true; - return resp.success; - } + bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp) + { + resp.success = true; + //if (req.fun == ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT) { + if (req.fun == 1) { + robot_.setDigitalOut(req.pin, req.state > 0.0 ? true : false); + } else if (req.fun == 2) { + //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_FLAG) { + robot_.setFlag(req.pin, req.state > 0.0 ? true : false); + //According to urdriver.py, set_flag will fail if called to rapidly in succession + ros::Duration(io_flag_delay_).sleep(); + } else if (req.fun == 3) { + //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_ANALOG_OUT) { + robot_.setAnalogOut(req.pin, req.state); + } else if (req.fun == 4) { + //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_TOOL_VOLTAGE) { + robot_.setToolVoltage((int)req.state); + } else { + resp.success = false; + } + return resp.success; + } - bool validateJointNames() { - std::vector actual_joint_names = robot_.getJointNames(); - actionlib::ActionServer::Goal goal = - *goal_handle_.getGoal(); - if (goal.trajectory.joint_names.size() != actual_joint_names.size()) - return false; + bool setPayload(ur_msgs::SetPayloadRequest& req, + ur_msgs::SetPayloadResponse& resp) + { + if (robot_.setPayload(req.payload)) + resp.success = true; + else + resp.success = true; + return resp.success; + } - for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) { - unsigned int j; - for (j = 0; j < actual_joint_names.size(); j++) { - if (goal.trajectory.joint_names[i] == actual_joint_names[j]) - break; - } - if (goal.trajectory.joint_names[i] == actual_joint_names[j]) { - actual_joint_names.erase(actual_joint_names.begin() + j); - } else { - return false; - } - } + bool validateJointNames() + { + std::vector actual_joint_names = robot_.getJointNames(); + actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); + if (goal.trajectory.joint_names.size() != actual_joint_names.size()) + return false; - return true; - } + for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) { + unsigned int j; + for (j = 0; j < actual_joint_names.size(); j++) { + if (goal.trajectory.joint_names[i] == actual_joint_names[j]) + break; + } + if (goal.trajectory.joint_names[i] == actual_joint_names[j]) { + actual_joint_names.erase(actual_joint_names.begin() + j); + } else { + return false; + } + } - void reorder_traj_joints(trajectory_msgs::JointTrajectory& traj) { - /* Reorders trajectory - destructive */ - std::vector actual_joint_names = robot_.getJointNames(); - std::vector mapping; - mapping.resize(actual_joint_names.size(), actual_joint_names.size()); - for (unsigned int i = 0; i < traj.joint_names.size(); i++) { - for (unsigned int j = 0; j < actual_joint_names.size(); j++) { - if (traj.joint_names[i] == actual_joint_names[j]) - mapping[j] = i; - } - } - traj.joint_names = actual_joint_names; - std::vector new_traj; - for (unsigned int i = 0; i < traj.points.size(); i++) { - trajectory_msgs::JointTrajectoryPoint new_point; - for (unsigned int j = 0; j < traj.points[i].positions.size(); j++) { - new_point.positions.push_back( - traj.points[i].positions[mapping[j]]); - new_point.velocities.push_back( - traj.points[i].velocities[mapping[j]]); - if (traj.points[i].accelerations.size() != 0) - new_point.accelerations.push_back( - traj.points[i].accelerations[mapping[j]]); - } - new_point.time_from_start = traj.points[i].time_from_start; - new_traj.push_back(new_point); - } - traj.points = new_traj; - } + return true; + } - bool has_velocities() { - actionlib::ActionServer::Goal goal = - *goal_handle_.getGoal(); - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { - if (goal.trajectory.points[i].positions.size() - != goal.trajectory.points[i].velocities.size()) - return false; - } - return true; - } + void reorder_traj_joints(trajectory_msgs::JointTrajectory& traj) + { + /* Reorders trajectory - destructive */ + std::vector actual_joint_names = robot_.getJointNames(); + std::vector mapping; + mapping.resize(actual_joint_names.size(), actual_joint_names.size()); + for (unsigned int i = 0; i < traj.joint_names.size(); i++) { + for (unsigned int j = 0; j < actual_joint_names.size(); j++) { + if (traj.joint_names[i] == actual_joint_names[j]) + mapping[j] = i; + } + } + traj.joint_names = actual_joint_names; + std::vector new_traj; + for (unsigned int i = 0; i < traj.points.size(); i++) { + trajectory_msgs::JointTrajectoryPoint new_point; + for (unsigned int j = 0; j < traj.points[i].positions.size(); j++) { + new_point.positions.push_back( + traj.points[i].positions[mapping[j]]); + new_point.velocities.push_back( + traj.points[i].velocities[mapping[j]]); + if (traj.points[i].accelerations.size() != 0) + new_point.accelerations.push_back( + traj.points[i].accelerations[mapping[j]]); + } + new_point.time_from_start = traj.points[i].time_from_start; + new_traj.push_back(new_point); + } + traj.points = new_traj; + } - bool has_positions() { - actionlib::ActionServer::Goal goal = - *goal_handle_.getGoal(); - if (goal.trajectory.points.size() == 0) - return false; - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { - if (goal.trajectory.points[i].positions.size() - != goal.trajectory.joint_names.size()) - return false; - } - return true; - } + bool has_velocities() + { + actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { + if (goal.trajectory.points[i].positions.size() + != goal.trajectory.points[i].velocities.size()) + return false; + } + return true; + } - bool start_positions_match(const trajectory_msgs::JointTrajectory &traj, double eps) - { - for (unsigned int i = 0; i < traj.points[0].positions.size(); i++) - { - std::vector qActual = robot_.rt_interface_->robot_state_->getQActual(); - if( fabs(traj.points[0].positions[i] - qActual[i]) > eps ) - { - return false; - } - } - return true; - } + bool has_positions() + { + actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); + if (goal.trajectory.points.size() == 0) + return false; + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { + if (goal.trajectory.points[i].positions.size() + != goal.trajectory.joint_names.size()) + return false; + } + return true; + } - bool has_limited_velocities() { - actionlib::ActionServer::Goal goal = - *goal_handle_.getGoal(); - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { - for (unsigned int j = 0; - j < goal.trajectory.points[i].velocities.size(); j++) { - if (fabs(goal.trajectory.points[i].velocities[j]) - > max_velocity_) - return false; - } - } - return true; - } + bool start_positions_match(const trajectory_msgs::JointTrajectory& traj, double eps) + { + for (unsigned int i = 0; i < traj.points[0].positions.size(); i++) { + std::vector qActual = robot_.rt_interface_->robot_state_->getQActual(); + if (fabs(traj.points[0].positions[i] - qActual[i]) > eps) { + return false; + } + } + return true; + } - bool traj_is_finite() { - actionlib::ActionServer::Goal goal = - *goal_handle_.getGoal(); - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { - for (unsigned int j = 0; - j < goal.trajectory.points[i].velocities.size(); j++) { - if (!std::isfinite(goal.trajectory.points[i].positions[j])) - return false; - if (!std::isfinite(goal.trajectory.points[i].velocities[j])) - return false; - } - } - return true; - } + bool has_limited_velocities() + { + actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { + for (unsigned int j = 0; + j < goal.trajectory.points[i].velocities.size(); j++) { + if (fabs(goal.trajectory.points[i].velocities[j]) + > max_velocity_) + return false; + } + } + return true; + } - void speedInterface(const trajectory_msgs::JointTrajectory::Ptr& msg) { - if (msg->points[0].velocities.size() == 6) { - double acc = 100; - if (msg->points[0].accelerations.size() > 0) - acc = *std::max_element(msg->points[0].accelerations.begin(), - msg->points[0].accelerations.end()); - robot_.setSpeed(msg->points[0].velocities[0], - msg->points[0].velocities[1], msg->points[0].velocities[2], - msg->points[0].velocities[3], msg->points[0].velocities[4], - msg->points[0].velocities[5], acc); - } + bool traj_is_finite() + { + actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { + for (unsigned int j = 0; + j < goal.trajectory.points[i].velocities.size(); j++) { + if (!std::isfinite(goal.trajectory.points[i].positions[j])) + return false; + if (!std::isfinite(goal.trajectory.points[i].velocities[j])) + return false; + } + } + return true; + } - } - void urscriptInterface(const std_msgs::String::ConstPtr& msg) { + void speedInterface(const trajectory_msgs::JointTrajectory::Ptr& msg) + { + if (msg->points[0].velocities.size() == 6) { + double acc = 100; + if (msg->points[0].accelerations.size() > 0) + acc = *std::max_element(msg->points[0].accelerations.begin(), + msg->points[0].accelerations.end()); + robot_.setSpeed(msg->points[0].velocities[0], + msg->points[0].velocities[1], msg->points[0].velocities[2], + msg->points[0].velocities[3], msg->points[0].velocities[4], + msg->points[0].velocities[5], acc); + } + } + void urscriptInterface(const std_msgs::String::ConstPtr& msg) + { - robot_.rt_interface_->addCommandToQueue(msg->data); + robot_.rt_interface_->addCommandToQueue(msg->data); + } - } + void rosControlLoop() + { + ros::Duration elapsed_time; + struct timespec last_time, current_time; + static const double BILLION = 1000000000.0; - void rosControlLoop() { - ros::Duration elapsed_time; - struct timespec last_time, current_time; - static const double BILLION = 1000000000.0; + realtime_tools::RealtimePublisher tf_pub(nh_, "/tf", 1); + geometry_msgs::TransformStamped tool_transform; + tool_transform.header.frame_id = base_frame_; + tool_transform.child_frame_id = tool_frame_; + tf_pub.msg_.transforms.push_back(tool_transform); - realtime_tools::RealtimePublisher tf_pub( nh_, "/tf", 1 ); - geometry_msgs::TransformStamped tool_transform; - tool_transform.header.frame_id = base_frame_; - tool_transform.child_frame_id = tool_frame_; - tf_pub.msg_.transforms.push_back( tool_transform ); + realtime_tools::RealtimePublisher tool_vel_pub(nh_, "tool_velocity", 1); + tool_vel_pub.msg_.header.frame_id = base_frame_; - realtime_tools::RealtimePublisher tool_vel_pub( nh_, "tool_velocity", 1 ); - tool_vel_pub.msg_.header.frame_id = base_frame_; + clock_gettime(CLOCK_MONOTONIC, &last_time); + while (ros::ok()) { + std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex + std::unique_lock locker(msg_lock); + while (!robot_.rt_interface_->robot_state_->getControllerUpdated()) { + rt_msg_cond_.wait(locker); + } + // Input + hardware_interface_->read(); + robot_.rt_interface_->robot_state_->setControllerUpdated(); + // Control + clock_gettime(CLOCK_MONOTONIC, ¤t_time); + elapsed_time = ros::Duration(current_time.tv_sec - last_time.tv_sec + (current_time.tv_nsec - last_time.tv_nsec) / BILLION); + ros::Time ros_time = ros::Time::now(); + controller_manager_->update(ros_time, elapsed_time); + last_time = current_time; - clock_gettime(CLOCK_MONOTONIC, &last_time); - while (ros::ok()) { - std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex - std::unique_lock locker(msg_lock); - while (!robot_.rt_interface_->robot_state_->getControllerUpdated()) { - rt_msg_cond_.wait(locker); - } - // Input - hardware_interface_->read(); - robot_.rt_interface_->robot_state_->setControllerUpdated(); + // Output + hardware_interface_->write(); - // Control - clock_gettime(CLOCK_MONOTONIC, ¤t_time); - elapsed_time = ros::Duration(current_time.tv_sec - last_time.tv_sec + (current_time.tv_nsec - last_time.tv_nsec)/ BILLION); - ros::Time ros_time = ros::Time::now(); - controller_manager_->update(ros_time, elapsed_time); - last_time = current_time; + // Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation + std::vector tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual(); - // Output - hardware_interface_->write(); + // Compute rotation angle + double rx = tool_vector_actual[3]; + double ry = tool_vector_actual[4]; + double rz = tool_vector_actual[5]; + double angle = std::sqrt(std::pow(rx, 2) + std::pow(ry, 2) + std::pow(rz, 2)); - // Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation - std::vector tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual(); + // Broadcast transform + if (tf_pub.trylock()) { + tf_pub.msg_.transforms[0].header.stamp = ros_time; + if (angle < 1e-16) { + tf_pub.msg_.transforms[0].transform.rotation.x = 0; + tf_pub.msg_.transforms[0].transform.rotation.y = 0; + tf_pub.msg_.transforms[0].transform.rotation.z = 0; + tf_pub.msg_.transforms[0].transform.rotation.w = 1; + } else { + tf_pub.msg_.transforms[0].transform.rotation.x = (rx / angle) * std::sin(angle * 0.5); + tf_pub.msg_.transforms[0].transform.rotation.y = (ry / angle) * std::sin(angle * 0.5); + tf_pub.msg_.transforms[0].transform.rotation.z = (rz / angle) * std::sin(angle * 0.5); + tf_pub.msg_.transforms[0].transform.rotation.w = std::cos(angle * 0.5); + } + tf_pub.msg_.transforms[0].transform.translation.x = tool_vector_actual[0]; + tf_pub.msg_.transforms[0].transform.translation.y = tool_vector_actual[1]; + tf_pub.msg_.transforms[0].transform.translation.z = tool_vector_actual[2]; - // Compute rotation angle - double rx = tool_vector_actual[3]; - double ry = tool_vector_actual[4]; - double rz = tool_vector_actual[5]; - double angle = std::sqrt(std::pow(rx,2) + std::pow(ry,2) + std::pow(rz,2)); + tf_pub.unlockAndPublish(); + } - // Broadcast transform - if( tf_pub.trylock() ) - { - tf_pub.msg_.transforms[0].header.stamp = ros_time; - if (angle < 1e-16) { - tf_pub.msg_.transforms[0].transform.rotation.x = 0; - tf_pub.msg_.transforms[0].transform.rotation.y = 0; - tf_pub.msg_.transforms[0].transform.rotation.z = 0; - tf_pub.msg_.transforms[0].transform.rotation.w = 1; - } else { - tf_pub.msg_.transforms[0].transform.rotation.x = (rx/angle) * std::sin(angle*0.5); - tf_pub.msg_.transforms[0].transform.rotation.y = (ry/angle) * std::sin(angle*0.5); - tf_pub.msg_.transforms[0].transform.rotation.z = (rz/angle) * std::sin(angle*0.5); - tf_pub.msg_.transforms[0].transform.rotation.w = std::cos(angle*0.5); - } - tf_pub.msg_.transforms[0].transform.translation.x = tool_vector_actual[0]; - tf_pub.msg_.transforms[0].transform.translation.y = tool_vector_actual[1]; - tf_pub.msg_.transforms[0].transform.translation.z = tool_vector_actual[2]; + //Publish tool velocity + std::vector tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual(); - tf_pub.unlockAndPublish(); - } + if (tool_vel_pub.trylock()) { + tool_vel_pub.msg_.header.stamp = ros_time; + tool_vel_pub.msg_.twist.linear.x = tcp_speed[0]; + tool_vel_pub.msg_.twist.linear.y = tcp_speed[1]; + tool_vel_pub.msg_.twist.linear.z = tcp_speed[2]; + tool_vel_pub.msg_.twist.angular.x = tcp_speed[3]; + tool_vel_pub.msg_.twist.angular.y = tcp_speed[4]; + tool_vel_pub.msg_.twist.angular.z = tcp_speed[5]; - //Publish tool velocity - std::vector tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual(); + tool_vel_pub.unlockAndPublish(); + } + } + } - if( tool_vel_pub.trylock() ) - { - tool_vel_pub.msg_.header.stamp = ros_time; - tool_vel_pub.msg_.twist.linear.x = tcp_speed[0]; - tool_vel_pub.msg_.twist.linear.y = tcp_speed[1]; - tool_vel_pub.msg_.twist.linear.z = tcp_speed[2]; - tool_vel_pub.msg_.twist.angular.x = tcp_speed[3]; - tool_vel_pub.msg_.twist.angular.y = tcp_speed[4]; - tool_vel_pub.msg_.twist.angular.z = tcp_speed[5]; - - tool_vel_pub.unlockAndPublish(); - } - - } - } - - void publishRTMsg() { - ros::Publisher joint_pub = nh_.advertise( - "joint_states", 1); - ros::Publisher wrench_pub = nh_.advertise( - "wrench", 1); + void publishRTMsg() + { + ros::Publisher joint_pub = nh_.advertise( + "joint_states", 1); + ros::Publisher wrench_pub = nh_.advertise( + "wrench", 1); ros::Publisher tool_vel_pub = nh_.advertise("tool_velocity", 1); static tf::TransformBroadcaster br; - while (ros::ok()) { - sensor_msgs::JointState joint_msg; - joint_msg.name = robot_.getJointNames(); - geometry_msgs::WrenchStamped wrench_msg; + while (ros::ok()) { + sensor_msgs::JointState joint_msg; + joint_msg.name = robot_.getJointNames(); + geometry_msgs::WrenchStamped wrench_msg; geometry_msgs::PoseStamped tool_pose_msg; - std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex - std::unique_lock locker(msg_lock); - while (!robot_.rt_interface_->robot_state_->getDataPublished()) { - rt_msg_cond_.wait(locker); - } - joint_msg.header.stamp = ros::Time::now(); - joint_msg.position = - robot_.rt_interface_->robot_state_->getQActual(); - for (unsigned int i = 0; i < joint_msg.position.size(); i++) { - joint_msg.position[i] += joint_offsets_[i]; - } - joint_msg.velocity = - robot_.rt_interface_->robot_state_->getQdActual(); - joint_msg.effort = robot_.rt_interface_->robot_state_->getIActual(); - joint_pub.publish(joint_msg); - std::vector tcp_force = - robot_.rt_interface_->robot_state_->getTcpForce(); - wrench_msg.header.stamp = joint_msg.header.stamp; - wrench_msg.wrench.force.x = tcp_force[0]; - wrench_msg.wrench.force.y = tcp_force[1]; - wrench_msg.wrench.force.z = tcp_force[2]; - wrench_msg.wrench.torque.x = tcp_force[3]; - wrench_msg.wrench.torque.y = tcp_force[4]; - wrench_msg.wrench.torque.z = tcp_force[5]; - wrench_pub.publish(wrench_msg); + std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex + std::unique_lock locker(msg_lock); + while (!robot_.rt_interface_->robot_state_->getDataPublished()) { + rt_msg_cond_.wait(locker); + } + joint_msg.header.stamp = ros::Time::now(); + joint_msg.position = robot_.rt_interface_->robot_state_->getQActual(); + for (unsigned int i = 0; i < joint_msg.position.size(); i++) { + joint_msg.position[i] += joint_offsets_[i]; + } + joint_msg.velocity = robot_.rt_interface_->robot_state_->getQdActual(); + joint_msg.effort = robot_.rt_interface_->robot_state_->getIActual(); + joint_pub.publish(joint_msg); + std::vector tcp_force = robot_.rt_interface_->robot_state_->getTcpForce(); + wrench_msg.header.stamp = joint_msg.header.stamp; + wrench_msg.wrench.force.x = tcp_force[0]; + wrench_msg.wrench.force.y = tcp_force[1]; + wrench_msg.wrench.force.z = tcp_force[2]; + wrench_msg.wrench.torque.x = tcp_force[3]; + wrench_msg.wrench.torque.y = tcp_force[4]; + wrench_msg.wrench.torque.z = tcp_force[5]; + wrench_pub.publish(wrench_msg); // Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation std::vector tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual(); @@ -694,11 +688,11 @@ private: double rx = tool_vector_actual[3]; double ry = tool_vector_actual[4]; double rz = tool_vector_actual[5]; - double angle = std::sqrt(std::pow(rx,2) + std::pow(ry,2) + std::pow(rz,2)); + double angle = std::sqrt(std::pow(rx, 2) + std::pow(ry, 2) + std::pow(rz, 2)); if (angle < 1e-16) { quat.setValue(0, 0, 0, 1); } else { - quat.setRotation(tf::Vector3(rx/angle, ry/angle, rz/angle), angle); + quat.setRotation(tf::Vector3(rx / angle, ry / angle, rz / angle), angle); } //Create and broadcast transform @@ -708,8 +702,7 @@ private: br.sendTransform(tf::StampedTransform(transform, joint_msg.header.stamp, base_frame_, tool_frame_)); //Publish tool velocity - std::vector tcp_speed = - robot_.rt_interface_->robot_state_->getTcpSpeedActual(); + std::vector tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual(); geometry_msgs::TwistStamped tool_twist; tool_twist.header.frame_id = base_frame_; tool_twist.header.stamp = joint_msg.header.stamp; @@ -721,119 +714,118 @@ private: tool_twist.twist.angular.z = tcp_speed[5]; tool_vel_pub.publish(tool_twist); - robot_.rt_interface_->robot_state_->setDataPublished(); - } - } + robot_.rt_interface_->robot_state_->setDataPublished(); + } + } - void publishMbMsg() { - bool warned = false; - ros::Publisher io_pub = nh_.advertise( - "ur_driver/io_states", 1); + void publishMbMsg() + { + bool warned = false; + ros::Publisher io_pub = nh_.advertise( + "ur_driver/io_states", 1); - while (ros::ok()) { - ur_msgs::IOStates io_msg; - std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex - std::unique_lock locker(msg_lock); - while (!robot_.sec_interface_->robot_state_->getNewDataAvailable()) { - msg_cond_.wait(locker); - } - int i_max = 10; - if (robot_.sec_interface_->robot_state_->getVersion() > 3.0) - i_max = 18; // From version 3.0, there are up to 18 inputs and outputs - for (unsigned int i = 0; i < i_max; i++) { - ur_msgs::Digital digi; - digi.pin = i; - digi.state = - ((robot_.sec_interface_->robot_state_->getDigitalInputBits() - & (1 << i)) >> i); - io_msg.digital_in_states.push_back(digi); - digi.state = - ((robot_.sec_interface_->robot_state_->getDigitalOutputBits() - & (1 << i)) >> i); - io_msg.digital_out_states.push_back(digi); - } - ur_msgs::Analog ana; - ana.pin = 0; - ana.state = robot_.sec_interface_->robot_state_->getAnalogInput0(); - io_msg.analog_in_states.push_back(ana); - ana.pin = 1; - ana.state = robot_.sec_interface_->robot_state_->getAnalogInput1(); - io_msg.analog_in_states.push_back(ana); + while (ros::ok()) { + ur_msgs::IOStates io_msg; + std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex + std::unique_lock locker(msg_lock); + while (!robot_.sec_interface_->robot_state_->getNewDataAvailable()) { + msg_cond_.wait(locker); + } + int i_max = 10; + if (robot_.sec_interface_->robot_state_->getVersion() > 3.0) + i_max = 18; // From version 3.0, there are up to 18 inputs and outputs + for (unsigned int i = 0; i < i_max; i++) { + ur_msgs::Digital digi; + digi.pin = i; + digi.state = ((robot_.sec_interface_->robot_state_->getDigitalInputBits() + & (1 << i)) + >> i); + io_msg.digital_in_states.push_back(digi); + digi.state = ((robot_.sec_interface_->robot_state_->getDigitalOutputBits() + & (1 << i)) + >> i); + io_msg.digital_out_states.push_back(digi); + } + ur_msgs::Analog ana; + ana.pin = 0; + ana.state = robot_.sec_interface_->robot_state_->getAnalogInput0(); + io_msg.analog_in_states.push_back(ana); + ana.pin = 1; + ana.state = robot_.sec_interface_->robot_state_->getAnalogInput1(); + io_msg.analog_in_states.push_back(ana); - ana.pin = 0; - ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput0(); - io_msg.analog_out_states.push_back(ana); - ana.pin = 1; - ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput1(); - io_msg.analog_out_states.push_back(ana); - io_pub.publish(io_msg); + ana.pin = 0; + ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput0(); + io_msg.analog_out_states.push_back(ana); + ana.pin = 1; + ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput1(); + io_msg.analog_out_states.push_back(ana); + io_pub.publish(io_msg); - if (robot_.sec_interface_->robot_state_->isEmergencyStopped() - or robot_.sec_interface_->robot_state_->isProtectiveStopped()) { - if (robot_.sec_interface_->robot_state_->isEmergencyStopped() - and !warned) { - print_error("Emergency stop pressed!"); - } else if (robot_.sec_interface_->robot_state_->isProtectiveStopped() - and !warned) { - print_error("Robot is protective stopped!"); - } - if (has_goal_) { - print_error("Aborting trajectory"); - robot_.stopTraj(); - result_.error_code = result_.SUCCESSFUL; - result_.error_string = "Robot was halted"; - goal_handle_.setAborted(result_, result_.error_string); - has_goal_ = false; - } - warned = true; - } else - warned = false; - - robot_.sec_interface_->robot_state_->finishedReading(); - - } - } + if (robot_.sec_interface_->robot_state_->isEmergencyStopped() + or robot_.sec_interface_->robot_state_->isProtectiveStopped()) { + if (robot_.sec_interface_->robot_state_->isEmergencyStopped() + and !warned) { + print_error("Emergency stop pressed!"); + } else if (robot_.sec_interface_->robot_state_->isProtectiveStopped() + and !warned) { + print_error("Robot is protective stopped!"); + } + if (has_goal_) { + print_error("Aborting trajectory"); + robot_.stopTraj(); + result_.error_code = result_.SUCCESSFUL; + result_.error_string = "Robot was halted"; + goal_handle_.setAborted(result_, result_.error_string); + has_goal_ = false; + } + warned = true; + } else + warned = false; + robot_.sec_interface_->robot_state_->finishedReading(); + } + } }; -int main(int argc, char **argv) { - bool use_sim_time = false; - std::string host; - int reverse_port = 50001; +int main(int argc, char** argv) +{ + bool use_sim_time = false; + std::string host; + int reverse_port = 50001; - ros::init(argc, argv, "ur_driver"); - ros::NodeHandle nh; - if (ros::param::get("use_sim_time", use_sim_time)) { - print_warning("use_sim_time is set!!"); - } - if (!(ros::param::get("~robot_ip_address", host))) { - if (argc > 1) { - print_warning( - "Please set the parameter robot_ip_address instead of giving it as a command line argument. This method is DEPRECATED"); - host = argv[1]; - } else { - print_fatal( - "Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip"); - exit(1); - } + ros::init(argc, argv, "ur_driver"); + ros::NodeHandle nh; + if (ros::param::get("use_sim_time", use_sim_time)) { + print_warning("use_sim_time is set!!"); + } + if (!(ros::param::get("~robot_ip_address", host))) { + if (argc > 1) { + print_warning( + "Please set the parameter robot_ip_address instead of giving it as a command line argument. This method is DEPRECATED"); + host = argv[1]; + } else { + print_fatal( + "Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip"); + exit(1); + } + } + if ((ros::param::get("~reverse_port", reverse_port))) { + if ((reverse_port <= 0) or (reverse_port >= 65535)) { + print_warning("Reverse port value is not valid (Use number between 1 and 65534. Using default value of 50001"); + reverse_port = 50001; + } + } else + reverse_port = 50001; - } - if ((ros::param::get("~reverse_port", reverse_port))) { - if((reverse_port <= 0) or (reverse_port >= 65535)) { - print_warning("Reverse port value is not valid (Use number between 1 and 65534. Using default value of 50001"); - reverse_port = 50001; - } - } else - reverse_port = 50001; + RosWrapper interface(host, reverse_port); - RosWrapper interface(host, reverse_port); + ros::AsyncSpinner spinner(3); + spinner.start(); - ros::AsyncSpinner spinner(3); - spinner.start(); + ros::waitForShutdown(); - ros::waitForShutdown(); + interface.halt(); - interface.halt(); - - exit(0); + exit(0); }