Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-14 20:10:47 +02:00
Code Releases Activity

Added clang formatting

Browse Source
This commit is contained in:
Simon Rasmussen
2017-02-16 02:03:40 +01:00
parent e00cfac0ee
commit a78d3eadf3
46 changed files with 4476 additions and 4212 deletions
Download Patch File Download Diff File Expand all files Collapse all files

95
.clang-format Normal file
View File

@@ -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
...

105
include/ur_modern_driver/bin_parser.h
View File

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

1
include/ur_modern_driver/do_output.h
View File

@@ -30,5 +30,4 @@ void print_warning(std::string inp);
void print_error(std::string inp); void print_error(std::string inp);
void print_fatal(std::string inp); void print_fatal(std::string inp);
#endif /* UR_DO_OUTPUT_H_ */ #endif /* UR_DO_OUTPUT_H_ */

32
include/ur_modern_driver/pipeline.h
View File

@@ -1,15 +1,14 @@
#pragma once #pragma once
#include <thread>
#include <atomic>
#include <vector>
#include "ur_modern_driver/log.h" #include "ur_modern_driver/log.h"
#include "ur_modern_driver/queue/readerwriterqueue.h" #include "ur_modern_driver/queue/readerwriterqueue.h"
#include <atomic>
#include <thread>
#include <vector>
using namespace moodycamel; using namespace moodycamel;
using namespace std; using namespace std;
template <typename T> template <typename T>
class IConsumer { class IConsumer {
public: public:
@@ -30,7 +29,6 @@ public:
virtual bool try_get(std::vector<unique_ptr<T> >& products) = 0; virtual bool try_get(std::vector<unique_ptr<T> >& products) = 0;
}; };
template <typename T> template <typename T>
class Pipeline { class Pipeline {
private: private:
@@ -40,7 +38,8 @@ private:
atomic<bool> _running; atomic<bool> _running;
thread _pThread, _cThread; thread _pThread, _cThread;
void run_producer() { void run_producer()
{
_producer.setup_producer(); _producer.setup_producer();
std::vector<unique_ptr<T> > products; std::vector<unique_ptr<T> > products;
while (_running) { while (_running) {
@@ -60,7 +59,8 @@ private:
//todo cleanup //todo cleanup
} }
void run_consumer() { void run_consumer()
{
_consumer.setup_consumer(); _consumer.setup_consumer();
unique_ptr<T> product; unique_ptr<T> product;
while (_running) { while (_running) {
@@ -71,15 +71,18 @@ private:
_consumer.teardown_consumer(); _consumer.teardown_consumer();
//todo cleanup //todo cleanup
} }
public: public:
Pipeline(IProducer<T>& producer, IConsumer<T>& consumer) Pipeline(IProducer<T>& producer, IConsumer<T>& consumer)
: _producer(producer), : _producer(producer)
_consumer(consumer), , _consumer(consumer)
_queue{32}, , _queue{ 32 }
_running{false} , _running{ false }
{ } {
}
void run() { void run()
{
if (_running) if (_running)
return; return;
@@ -88,7 +91,8 @@ public:
_cThread = thread(&Pipeline::run_consumer, this); _cThread = thread(&Pipeline::run_consumer, this);
} }
void stop() { void stop()
{
if (!_running) if (!_running)
return; return;

204
include/ur_modern_driver/queue/atomicops.h
View File

@@ -12,10 +12,10 @@
// Uses the AE_* prefix for macros (historical reasons), and the "moodycamel" namespace for symbols. // Uses the AE_* prefix for macros (historical reasons), and the "moodycamel" namespace for symbols.
#include <cassert> #include <cassert>
#include <type_traits>
#include <cerrno> #include <cerrno>
#include <cstdint> #include <cstdint>
#include <ctime> #include <ctime>
#include <type_traits>
// Platform detection // Platform detection
#if defined(__INTEL_COMPILER) #if defined(__INTEL_COMPILER)
@@ -38,11 +38,9 @@
#define AE_ARCH_UNKNOWN #define AE_ARCH_UNKNOWN
#endif #endif
// AE_UNUSED // AE_UNUSED
#define AE_UNUSED(x) ((void)x) #define AE_UNUSED(x) ((void)x)
// AE_FORCEINLINE // AE_FORCEINLINE
#if defined(AE_VCPP) || defined(AE_ICC) #if defined(AE_VCPP) || defined(AE_ICC)
#define AE_FORCEINLINE __forceinline #define AE_FORCEINLINE __forceinline
@@ -53,7 +51,6 @@
#define AE_FORCEINLINE inline #define AE_FORCEINLINE inline
#endif #endif
// AE_ALIGN // AE_ALIGN
#if defined(AE_VCPP) || defined(AE_ICC) #if defined(AE_VCPP) || defined(AE_ICC)
#define AE_ALIGN(x) __declspec(align(x)) #define AE_ALIGN(x) __declspec(align(x))
@@ -64,7 +61,6 @@
#define AE_ALIGN(x) __attribute__((aligned(x))) #define AE_ALIGN(x) __attribute__((aligned(x)))
#endif #endif
// Portable atomic fences implemented below: // Portable atomic fences implemented below:
namespace moodycamel { namespace moodycamel {
@@ -100,7 +96,6 @@ enum memory_order {
#define AeLiteSync __lwsync #define AeLiteSync __lwsync
#endif #endif
#ifdef AE_VCPP #ifdef AE_VCPP
#pragma warning(push) #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`
@@ -114,12 +109,22 @@ namespace moodycamel {
AE_FORCEINLINE void compiler_fence(memory_order order) AE_FORCEINLINE void compiler_fence(memory_order order)
{ {
switch (order) { switch (order) {
case memory_order_relaxed: break; case memory_order_relaxed:
case memory_order_acquire: _ReadBarrier(); break; break;
case memory_order_release: _WriteBarrier(); break; case memory_order_acquire:
case memory_order_acq_rel: _ReadWriteBarrier(); break; _ReadBarrier();
case memory_order_seq_cst: _ReadWriteBarrier(); break; break;
default: assert(false); case memory_order_release:
_WriteBarrier();
break;
case memory_order_acq_rel:
_ReadWriteBarrier();
break;
case memory_order_seq_cst:
_ReadWriteBarrier();
break;
default:
assert(false);
} }
} }
@@ -130,16 +135,24 @@ AE_FORCEINLINE void compiler_fence(memory_order order)
AE_FORCEINLINE void fence(memory_order order) AE_FORCEINLINE void fence(memory_order order)
{ {
switch (order) { switch (order) {
case memory_order_relaxed: break; case memory_order_relaxed:
case memory_order_acquire: _ReadBarrier(); break; break;
case memory_order_release: _WriteBarrier(); break; case memory_order_acquire:
case memory_order_acq_rel: _ReadWriteBarrier(); break; _ReadBarrier();
break;
case memory_order_release:
_WriteBarrier();
break;
case memory_order_acq_rel:
_ReadWriteBarrier();
break;
case memory_order_seq_cst: case memory_order_seq_cst:
_ReadWriteBarrier(); _ReadWriteBarrier();
AeFullSync(); AeFullSync();
_ReadWriteBarrier(); _ReadWriteBarrier();
break; break;
default: assert(false); default:
assert(false);
} }
} }
#else #else
@@ -169,7 +182,8 @@ AE_FORCEINLINE void fence(memory_order order)
AeFullSync(); AeFullSync();
_ReadWriteBarrier(); _ReadWriteBarrier();
break; break;
default: assert(false); default:
assert(false);
} }
} }
#endif #endif
@@ -183,24 +197,44 @@ namespace moodycamel {
AE_FORCEINLINE void compiler_fence(memory_order order) AE_FORCEINLINE void compiler_fence(memory_order order)
{ {
switch (order) { switch (order) {
case memory_order_relaxed: break; case memory_order_relaxed:
case memory_order_acquire: std::atomic_signal_fence(std::memory_order_acquire); break; break;
case memory_order_release: std::atomic_signal_fence(std::memory_order_release); break; case memory_order_acquire:
case memory_order_acq_rel: std::atomic_signal_fence(std::memory_order_acq_rel); break; std::atomic_signal_fence(std::memory_order_acquire);
case memory_order_seq_cst: std::atomic_signal_fence(std::memory_order_seq_cst); break; break;
default: assert(false); 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) AE_FORCEINLINE void fence(memory_order order)
{ {
switch (order) { switch (order) {
case memory_order_relaxed: break; case memory_order_relaxed:
case memory_order_acquire: std::atomic_thread_fence(std::memory_order_acquire); break; break;
case memory_order_release: std::atomic_thread_fence(std::memory_order_release); break; case memory_order_acquire:
case memory_order_acq_rel: std::atomic_thread_fence(std::memory_order_acq_rel); break; std::atomic_thread_fence(std::memory_order_acquire);
case memory_order_seq_cst: std::atomic_thread_fence(std::memory_order_seq_cst); break; break;
default: assert(false); 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);
} }
} }
@@ -208,7 +242,6 @@ AE_FORCEINLINE void fence(memory_order order)
#endif #endif
#if !defined(AE_VCPP) || (_MSC_VER >= 1700 && !defined(__cplusplus_cli)) #if !defined(AE_VCPP) || (_MSC_VER >= 1700 && !defined(__cplusplus_cli))
#define AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC #define AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC
#endif #endif
@@ -224,39 +257,64 @@ AE_FORCEINLINE void fence(memory_order order)
// at the hardware level -- on most platforms this generally means aligned pointers and integers (only). // at the hardware level -- on most platforms this generally means aligned pointers and integers (only).
namespace moodycamel { namespace moodycamel {
template <typename T> template <typename T>
class weak_atomic class weak_atomic {
{
public: public:
weak_atomic() {} weak_atomic() {}
#ifdef AE_VCPP #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 #endif
template<typename U> weak_atomic(U&& x) : value(std::forward<U>(x)) { } template <typename U>
weak_atomic(U&& x)
: value(std::forward<U>(x))
{
}
#ifdef __cplusplus_cli #ifdef __cplusplus_cli
// Work around bug with universal reference/nullptr combination that only appears when /clr is on // Work around bug with universal reference/nullptr combination that only appears when /clr is on
weak_atomic(nullptr_t) : value(nullptr) { } weak_atomic(nullptr_t)
: value(nullptr)
{
}
#endif #endif
weak_atomic(weak_atomic const& other) : value(other.value) { } weak_atomic(weak_atomic const& other)
weak_atomic(weak_atomic&& other) : value(std::move(other.value)) { } : value(other.value)
{
}
weak_atomic(weak_atomic&& other)
: value(std::move(other.value))
{
}
#ifdef AE_VCPP #ifdef AE_VCPP
#pragma warning(default : 4100) #pragma warning(default : 4100)
#endif #endif
AE_FORCEINLINE operator T() const { return load(); } AE_FORCEINLINE operator T() const
{
return load();
}
#ifndef AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC #ifndef AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC
template<typename U> AE_FORCEINLINE weak_atomic const& operator=(U&& x) { value = std::forward<U>(x); return *this; } template <typename U>
AE_FORCEINLINE weak_atomic const& operator=(weak_atomic const& other) { value = other.value; return *this; } AE_FORCEINLINE weak_atomic const& operator=(U&& x)
{
value = std::forward<U>(x);
return *this;
}
AE_FORCEINLINE weak_atomic const& operator=(weak_atomic const& other)
{
value = other.value;
return *this;
}
AE_FORCEINLINE T load() const { return value; } AE_FORCEINLINE T load() const { return value; }
AE_FORCEINLINE T fetch_add_acquire(T increment) AE_FORCEINLINE T fetch_add_acquire(T increment)
{ {
#if defined(AE_ARCH_X64) || defined(AE_ARCH_X86) #if defined(AE_ARCH_X64) || defined(AE_ARCH_X86)
if (sizeof(T) == 4) return _InterlockedExchangeAdd((long volatile*)&value, (long)increment); if (sizeof(T) == 4)
return _InterlockedExchangeAdd((long volatile*)&value, (long)increment);
#if defined(_M_AMD64) #if defined(_M_AMD64)
else if (sizeof(T) == 8) return _InterlockedExchangeAdd64((long long volatile*)&value, (long long)increment); else if (sizeof(T) == 8)
return _InterlockedExchangeAdd64((long long volatile*)&value, (long long)increment);
#endif #endif
#else #else
#error Unsupported platform #error Unsupported platform
@@ -268,9 +326,11 @@ public:
AE_FORCEINLINE T fetch_add_release(T increment) AE_FORCEINLINE T fetch_add_release(T increment)
{ {
#if defined(AE_ARCH_X64) || defined(AE_ARCH_X86) #if defined(AE_ARCH_X64) || defined(AE_ARCH_X86)
if (sizeof(T) == 4) return _InterlockedExchangeAdd((long volatile*)&value, (long)increment); if (sizeof(T) == 4)
return _InterlockedExchangeAdd((long volatile*)&value, (long)increment);
#if defined(_M_AMD64) #if defined(_M_AMD64)
else if (sizeof(T) == 8) return _InterlockedExchangeAdd64((long long volatile*)&value, (long long)increment); else if (sizeof(T) == 8)
return _InterlockedExchangeAdd64((long long volatile*)&value, (long long)increment);
#endif #endif
#else #else
#error Unsupported platform #error Unsupported platform
@@ -305,7 +365,6 @@ public:
} }
#endif #endif
private: private:
#ifndef AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC #ifndef AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC
// No std::atomic support, but still need to circumvent compiler optimizations. // No std::atomic support, but still need to circumvent compiler optimizations.
@@ -318,8 +377,6 @@ private:
} // end namespace moodycamel } // end namespace moodycamel
// Portable single-producer, single-consumer semaphore below: // Portable single-producer, single-consumer semaphore below:
#if defined(_WIN32) #if defined(_WIN32)
@@ -341,8 +398,7 @@ extern "C" {
#include <semaphore.h> #include <semaphore.h>
#endif #endif
namespace moodycamel namespace moodycamel {
{
// Code in the spsc_sema namespace below is an adaptation of Jeff Preshing's // Code in the spsc_sema namespace below is an adaptation of Jeff Preshing's
// portable + lightweight semaphore implementations, originally from // portable + lightweight semaphore implementations, originally from
// https://github.com/preshing/cpp11-on-multicore/blob/master/common/sema.h // https://github.com/preshing/cpp11-on-multicore/blob/master/common/sema.h
@@ -364,11 +420,9 @@ namespace moodycamel
// 2. Altered source versions must be plainly marked as such, and must not be // 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software. // misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution. // 3. This notice may not be removed or altered from any source distribution.
namespace spsc_sema namespace spsc_sema {
{
#if defined(_WIN32) #if defined(_WIN32)
class Semaphore class Semaphore {
{
private: private:
void* m_hSema; void* m_hSema;
@@ -416,8 +470,7 @@ namespace moodycamel
// Semaphore (Apple iOS and OSX) // Semaphore (Apple iOS and OSX)
// Can't use POSIX semaphores due to http://lists.apple.com/archives/darwin-kernel/2009/Apr/msg00010.html // Can't use POSIX semaphores due to http://lists.apple.com/archives/darwin-kernel/2009/Apr/msg00010.html
//--------------------------------------------------------- //---------------------------------------------------------
class Semaphore class Semaphore {
{
private: private:
semaphore_t m_sema; semaphore_t m_sema;
@@ -465,8 +518,7 @@ namespace moodycamel
void signal(int count) void signal(int count)
{ {
while (count-- > 0) while (count-- > 0) {
{
semaphore_signal(m_sema); semaphore_signal(m_sema);
} }
} }
@@ -475,8 +527,7 @@ namespace moodycamel
//--------------------------------------------------------- //---------------------------------------------------------
// Semaphore (POSIX, Linux) // Semaphore (POSIX, Linux)
//--------------------------------------------------------- //---------------------------------------------------------
class Semaphore class Semaphore {
{
private: private:
sem_t m_sema; sem_t m_sema;
@@ -499,11 +550,9 @@ namespace moodycamel
{ {
// http://stackoverflow.com/questions/2013181/gdb-causes-sem-wait-to-fail-with-eintr-error // http://stackoverflow.com/questions/2013181/gdb-causes-sem-wait-to-fail-with-eintr-error
int rc; int rc;
do do {
{
rc = sem_wait(&m_sema); rc = sem_wait(&m_sema);
} } while (rc == -1 && errno == EINTR);
while (rc == -1 && errno == EINTR);
} }
bool try_wait() bool try_wait()
@@ -544,8 +593,7 @@ namespace moodycamel
void signal(int count) void signal(int count)
{ {
while (count-- > 0) while (count-- > 0) {
{
sem_post(&m_sema); sem_post(&m_sema);
} }
} }
@@ -557,8 +605,7 @@ namespace moodycamel
//--------------------------------------------------------- //---------------------------------------------------------
// LightweightSemaphore // LightweightSemaphore
//--------------------------------------------------------- //---------------------------------------------------------
class LightweightSemaphore class LightweightSemaphore {
{
public: public:
typedef std::make_signed<std::size_t>::type ssize_t; typedef std::make_signed<std::size_t>::type ssize_t;
@@ -573,10 +620,8 @@ namespace moodycamel
// If we lower it to 1000, testBenaphore becomes 15x slower on my Core i7-5930K Windows PC, // If we lower it to 1000, testBenaphore becomes 15x slower on my Core i7-5930K Windows PC,
// as threads start hitting the kernel semaphore. // as threads start hitting the kernel semaphore.
int spin = 10000; int spin = 10000;
while (--spin >= 0) while (--spin >= 0) {
{ if (m_count.load() > 0) {
if (m_count.load() > 0)
{
m_count.fetch_add_acquire(-1); m_count.fetch_add_acquire(-1);
return true; return true;
} }
@@ -585,8 +630,7 @@ namespace moodycamel
oldCount = m_count.fetch_add_acquire(-1); oldCount = m_count.fetch_add_acquire(-1);
if (oldCount > 0) if (oldCount > 0)
return true; return true;
if (timeout_usecs < 0) if (timeout_usecs < 0) {
{
m_sema.wait(); m_sema.wait();
return true; return true;
} }
@@ -597,8 +641,7 @@ namespace moodycamel
// it. So we have to re-adjust the count, but only if the semaphore // 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 // wasn't signaled enough times for us too since then. If it was, we
// need to release the semaphore too. // need to release the semaphore too.
while (true) while (true) {
{
oldCount = m_count.fetch_add_release(1); oldCount = m_count.fetch_add_release(1);
if (oldCount < 0) if (oldCount < 0)
return false; // successfully restored things to the way they were return false; // successfully restored things to the way they were
@@ -610,15 +653,15 @@ namespace moodycamel
} }
public: public:
LightweightSemaphore(ssize_t initialCount = 0) : m_count(initialCount) LightweightSemaphore(ssize_t initialCount = 0)
: m_count(initialCount)
{ {
assert(initialCount >= 0); assert(initialCount >= 0);
} }
bool tryWait() bool tryWait()
{ {
if (m_count.load() > 0) if (m_count.load() > 0) {
{
m_count.fetch_add_acquire(-1); m_count.fetch_add_acquire(-1);
return true; return true;
} }
@@ -641,8 +684,7 @@ namespace moodycamel
assert(count >= 0); assert(count >= 0);
ssize_t oldCount = m_count.fetch_add_release(count); ssize_t oldCount = m_count.fetch_add_release(count);
assert(oldCount >= -1); assert(oldCount >= -1);
if (oldCount < 0) if (oldCount < 0) {
{
m_sema.signal(1); m_sema.signal(1);
} }
} }

88
include/ur_modern_driver/queue/readerwriterqueue.h
View File

@@ -5,18 +5,17 @@
#pragma once #pragma once
#include "atomicops.h" #include "atomicops.h"
#include <type_traits>
#include <utility>
#include <cassert> #include <cassert>
#include <stdexcept>
#include <new>
#include <cstdint> #include <cstdint>
#include <cstdlib> // For malloc/free/abort & size_t #include <cstdlib> // For malloc/free/abort & size_t
#include <new>
#include <stdexcept>
#include <type_traits>
#include <utility>
#if __cplusplus > 199711L || _MSC_VER >= 1700 // C++11 or VS2012 #if __cplusplus > 199711L || _MSC_VER >= 1700 // C++11 or VS2012
#include <chrono> #include <chrono>
#endif #endif
// A lock-free queue for a single-consumer, single-producer architecture. // 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 // The queue is also wait-free in the common path (except if more memory
// needs to be allocated, in which case malloc is called). // needs to be allocated, in which case malloc is called).
@@ -50,8 +49,7 @@
namespace moodycamel { namespace moodycamel {
template <typename T, size_t MAX_BLOCK_SIZE = 512> template <typename T, size_t MAX_BLOCK_SIZE = 512>
class ReaderWriterQueue class ReaderWriterQueue {
{
// Design: Based on a queue-of-queues. The low-level queues are just // Design: Based on a queue-of-queues. The low-level queues are just
// circular buffers with front and tail indices indicating where the // circular buffers with front and tail indices indicating where the
// next element to dequeue is and where the next element can be enqueued, // next element to dequeue is and where the next element can be enqueued,
@@ -110,15 +108,13 @@ public:
} }
if (firstBlock == nullptr) { if (firstBlock == nullptr) {
firstBlock = block; firstBlock = block;
} } else {
else {
lastBlock->next = block; lastBlock->next = block;
} }
lastBlock = block; lastBlock = block;
block->next = firstBlock; block->next = firstBlock;
} }
} } else {
else {
firstBlock = make_block(largestBlockSize); firstBlock = make_block(largestBlockSize);
if (firstBlock == nullptr) { if (firstBlock == nullptr) {
#ifdef MOODYCAMEL_EXCEPTIONS_ENABLED #ifdef MOODYCAMEL_EXCEPTIONS_ENABLED
@@ -164,7 +160,6 @@ public:
} while (block != frontBlock_); } while (block != frontBlock_);
} }
// Enqueues a copy of element if there is room in the queue. // Enqueues a copy of element if there is room in the queue.
// Returns true if the element was enqueued, false otherwise. // Returns true if the element was enqueued, false otherwise.
// Does not allocate memory. // Does not allocate memory.
@@ -181,7 +176,6 @@ public:
return inner_enqueue<CannotAlloc>(std::forward<T>(element)); return inner_enqueue<CannotAlloc>(std::forward<T>(element));
} }
// Enqueues a copy of element on the queue. // Enqueues a copy of element on the queue.
// Allocates an additional block of memory if needed. // Allocates an additional block of memory if needed.
// Only fails (returns false) if memory allocation fails. // Only fails (returns false) if memory allocation fails.
@@ -198,7 +192,6 @@ public:
return inner_enqueue<CanAlloc>(std::forward<T>(element)); return inner_enqueue<CanAlloc>(std::forward<T>(element));
} }
// Attempts to dequeue an element; if the queue is empty, // Attempts to dequeue an element; if the queue is empty,
// returns false instead. If the queue has at least one element, // returns false instead. If the queue has at least one element,
// moves front to result using operator=, then returns true. // moves front to result using operator=, then returns true.
@@ -243,8 +236,7 @@ public:
fence(memory_order_release); fence(memory_order_release);
frontBlock_->front = blockFront; frontBlock_->front = blockFront;
} } else if (frontBlock_ != tailBlock.load()) {
else if (frontBlock_ != tailBlock.load()) {
fence(memory_order_acquire); fence(memory_order_acquire);
frontBlock_ = frontBlock.load(); frontBlock_ = frontBlock.load();
@@ -287,8 +279,7 @@ public:
fence(memory_order_release); fence(memory_order_release);
frontBlock_->front = nextBlockFront; frontBlock_->front = nextBlockFront;
} } else {
else {
// No elements in current block and no other block to advance to // No elements in current block and no other block to advance to
return false; return false;
} }
@@ -296,7 +287,6 @@ public:
return true; return true;
} }
// Returns a pointer to the front element in the queue (the one that // 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 // 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 // queue appears empty at the time the method is called, nullptr is
@@ -317,8 +307,7 @@ public:
fence(memory_order_acquire); fence(memory_order_acquire);
non_empty_front_block: non_empty_front_block:
return reinterpret_cast<T*>(frontBlock_->data + blockFront * sizeof(T)); return reinterpret_cast<T*>(frontBlock_->data + blockFront * sizeof(T));
} } else if (frontBlock_ != tailBlock.load()) {
else if (frontBlock_ != tailBlock.load()) {
fence(memory_order_acquire); fence(memory_order_acquire);
frontBlock_ = frontBlock.load(); frontBlock_ = frontBlock.load();
blockTail = frontBlock_->localTail = frontBlock_->tail.load(); blockTail = frontBlock_->localTail = frontBlock_->tail.load();
@@ -366,8 +355,7 @@ public:
fence(memory_order_release); fence(memory_order_release);
frontBlock_->front = blockFront; frontBlock_->front = blockFront;
} } else if (frontBlock_ != tailBlock.load()) {
else if (frontBlock_ != tailBlock.load()) {
fence(memory_order_acquire); fence(memory_order_acquire);
frontBlock_ = frontBlock.load(); frontBlock_ = frontBlock.load();
blockTail = frontBlock_->localTail = frontBlock_->tail.load(); blockTail = frontBlock_->localTail = frontBlock_->tail.load();
@@ -400,8 +388,7 @@ public:
fence(memory_order_release); fence(memory_order_release);
frontBlock_->front = nextBlockFront; frontBlock_->front = nextBlockFront;
} } else {
else {
// No elements in current block and no other block to advance to // No elements in current block and no other block to advance to
return false; return false;
} }
@@ -426,9 +413,9 @@ public:
return result; return result;
} }
private: private:
enum AllocationMode { CanAlloc, CannotAlloc }; enum AllocationMode { CanAlloc,
CannotAlloc };
template <AllocationMode canAlloc, typename U> template <AllocationMode canAlloc, typename U>
bool inner_enqueue(U&& element) bool inner_enqueue(U&& element)
@@ -457,8 +444,7 @@ private:
fence(memory_order_release); fence(memory_order_release);
tailBlock_->tail = nextBlockTail; tailBlock_->tail = nextBlockTail;
} } else {
else {
fence(memory_order_acquire); fence(memory_order_acquire);
if (tailBlock_->next.load() != frontBlock) { if (tailBlock_->next.load() != frontBlock) {
// Note that the reason we can't advance to the frontBlock and start adding new entries there // Note that the reason we can't advance to the frontBlock and start adding new entries there
@@ -486,8 +472,7 @@ private:
fence(memory_order_release); fence(memory_order_release);
tailBlock = tailBlockNext; tailBlock = tailBlockNext;
} } else if (canAlloc == CanAlloc) {
else if (canAlloc == CanAlloc) {
// tailBlock is full and there's no free block ahead; create a new block // tailBlock is full and there's no free block ahead; create a new block
auto newBlockSize = largestBlockSize >= MAX_BLOCK_SIZE ? largestBlockSize : largestBlockSize * 2; auto newBlockSize = largestBlockSize >= MAX_BLOCK_SIZE ? largestBlockSize : largestBlockSize * 2;
auto newBlock = make_block(newBlockSize); auto newBlock = make_block(newBlockSize);
@@ -513,12 +498,10 @@ private:
fence(memory_order_release); fence(memory_order_release);
tailBlock = newBlock; tailBlock = newBlock;
} } else if (canAlloc == CannotAlloc) {
else if (canAlloc == CannotAlloc) {
// Would have had to allocate a new block to enqueue, but not allowed // Would have had to allocate a new block to enqueue, but not allowed
return false; return false;
} } else {
else {
assert(false && "Should be unreachable code"); assert(false && "Should be unreachable code");
return false; return false;
} }
@@ -527,15 +510,12 @@ private:
return true; return true;
} }
// Disable copying // Disable copying
ReaderWriterQueue(ReaderWriterQueue const&) {} ReaderWriterQueue(ReaderWriterQueue const&) {}
// Disable assignment // Disable assignment
ReaderWriterQueue& operator=(ReaderWriterQueue const&) {} ReaderWriterQueue& operator=(ReaderWriterQueue const&) {}
AE_FORCEINLINE static size_t ceilToPow2(size_t x) AE_FORCEINLINE static size_t ceilToPow2(size_t x)
{ {
// From http://graphics.stanford.edu/~seander/bithacks.html#RoundUpPowerOf2 // From http://graphics.stanford.edu/~seander/bithacks.html#RoundUpPowerOf2
@@ -556,10 +536,10 @@ private:
const std::size_t alignment = std::alignment_of<U>::value; const std::size_t alignment = std::alignment_of<U>::value;
return ptr + (alignment - (reinterpret_cast<std::uintptr_t>(ptr) % alignment)) % alignment; return ptr + (alignment - (reinterpret_cast<std::uintptr_t>(ptr) % alignment)) % alignment;
} }
private: private:
#ifndef NDEBUG #ifndef NDEBUG
struct ReentrantGuard struct ReentrantGuard {
{
ReentrantGuard(bool& _inSection) ReentrantGuard(bool& _inSection)
: inSection(_inSection) : inSection(_inSection)
{ {
@@ -577,8 +557,7 @@ private:
}; };
#endif #endif
struct Block struct Block {
{
// Avoid false-sharing by putting highly contended variables on their own cache lines // Avoid false-sharing by putting highly contended variables on their own cache lines
weak_atomic<size_t> front; // (Atomic) Elements are read from here weak_atomic<size_t> front; // (Atomic) Elements are read from here
size_t localTail; // An uncontended shadow copy of tail, owned by the consumer size_t localTail; // An uncontended shadow copy of tail, owned by the consumer
@@ -594,10 +573,16 @@ private:
const size_t sizeMask; const size_t sizeMask;
// size must be a power of two (and greater than 0) // size must be a power of two (and greater than 0)
Block(size_t const& _size, char* _rawThis, char* _data) 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) : front(0)
, localTail(0)
, tail(0)
, localFront(0)
, next(nullptr)
, data(_data)
, sizeMask(_size - 1)
, rawThis(_rawThis)
{ {
} }
@@ -609,7 +594,6 @@ private:
char* rawThis; char* rawThis;
}; };
static Block* make_block(size_t capacity) static Block* make_block(size_t capacity)
{ {
// Allocate enough memory for the block itself, as well as all the elements it will contain // Allocate enough memory for the block itself, as well as all the elements it will contain
@@ -641,16 +625,15 @@ private:
// Like ReaderWriterQueue, but also providees blocking operations // Like ReaderWriterQueue, but also providees blocking operations
template <typename T, size_t MAX_BLOCK_SIZE = 512> template <typename T, size_t MAX_BLOCK_SIZE = 512>
class BlockingReaderWriterQueue class BlockingReaderWriterQueue {
{
private: private:
typedef ::moodycamel::ReaderWriterQueue<T, MAX_BLOCK_SIZE> ReaderWriterQueue; typedef ::moodycamel::ReaderWriterQueue<T, MAX_BLOCK_SIZE> ReaderWriterQueue;
public: public:
explicit BlockingReaderWriterQueue(size_t maxSize = 15) explicit BlockingReaderWriterQueue(size_t maxSize = 15)
: inner(maxSize) : inner(maxSize)
{ } {
}
// Enqueues a copy of element if there is room in the queue. // Enqueues a copy of element if there is room in the queue.
// Returns true if the element was enqueued, false otherwise. // Returns true if the element was enqueued, false otherwise.
@@ -676,7 +659,6 @@ public:
return false; return false;
} }
// Enqueues a copy of element on the queue. // Enqueues a copy of element on the queue.
// Allocates an additional block of memory if needed. // Allocates an additional block of memory if needed.
// Only fails (returns false) if memory allocation fails. // Only fails (returns false) if memory allocation fails.
@@ -701,7 +683,6 @@ public:
return false; return false;
} }
// Attempts to dequeue an element; if the queue is empty, // Attempts to dequeue an element; if the queue is empty,
// returns false instead. If the queue has at least one element, // returns false instead. If the queue has at least one element,
// moves front to result using operator=, then returns true. // moves front to result using operator=, then returns true.
@@ -717,7 +698,6 @@ public:
return false; return false;
} }
// Attempts to dequeue an element; if the queue is empty, // Attempts to dequeue an element; if the queue is empty,
// waits until an element is available, then dequeues it. // waits until an element is available, then dequeues it.
template <typename U> template <typename U>
@@ -730,7 +710,6 @@ public:
AE_UNUSED(success); AE_UNUSED(success);
} }
// Attempts to dequeue an element; if the queue is empty, // Attempts to dequeue an element; if the queue is empty,
// waits until an element is available up to the specified timeout, // waits until an element is available up to the specified timeout,
// then dequeues it and returns true, or returns false if the timeout // then dequeues it and returns true, or returns false if the timeout
@@ -750,7 +729,6 @@ public:
return true; return true;
} }
#if __cplusplus > 199711L || _MSC_VER >= 1700 #if __cplusplus > 199711L || _MSC_VER >= 1700
// Attempts to dequeue an element; if the queue is empty, // Attempts to dequeue an element; if the queue is empty,
// waits until an element is available up to the specified timeout, // waits until an element is available up to the specified timeout,
@@ -765,7 +743,6 @@ public:
} }
#endif #endif
// Returns a pointer to the front element in the queue (the one that // 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 // 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 // queue appears empty at the time the method is called, nullptr is
@@ -797,7 +774,6 @@ public:
return sema.availableApprox(); return sema.availableApprox();
} }
private: private:
// Disable copying & assignment // Disable copying & assignment
BlockingReaderWriterQueue(ReaderWriterQueue const&) {} BlockingReaderWriterQueue(ReaderWriterQueue const&) {}

12
include/ur_modern_driver/robot_state.h
View File

@@ -19,17 +19,19 @@
#ifndef ROBOT_STATE_H_ #ifndef ROBOT_STATE_H_
#define ROBOT_STATE_H_ #define ROBOT_STATE_H_
#include <condition_variable>
#include <inttypes.h> #include <inttypes.h>
#include <vector> #include <mutex>
#include <netinet/in.h>
#include <stdlib.h> #include <stdlib.h>
#include <string.h> #include <string.h>
#include <mutex> #include <vector>
#include <condition_variable>
#include <netinet/in.h>
namespace message_types { namespace message_types {
enum message_type { 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; typedef message_types::message_type messageType;

8
include/ur_modern_driver/robot_state_RT.h
View File

@@ -19,13 +19,13 @@
#ifndef ROBOT_STATE_RT_H_ #ifndef ROBOT_STATE_RT_H_
#define ROBOT_STATE_RT_H_ #define ROBOT_STATE_RT_H_
#include <condition_variable>
#include <inttypes.h> #include <inttypes.h>
#include <vector>
#include <stdlib.h>
#include <string.h>
#include <mutex> #include <mutex>
#include <netinet/in.h> #include <netinet/in.h>
#include <condition_variable> #include <stdlib.h>
#include <string.h>
#include <vector>
class RobotStateRT { class RobotStateRT {
private: private:

1
include/ur_modern_driver/ros/converter.h
View File

@@ -1,2 +1 @@
#pragma once #pragma once

18
include/ur_modern_driver/ros/rt_publisher.h
View File

@@ -1,12 +1,12 @@
#pragma once #pragma once
#include <cstdlib> #include <cstdlib>
#include <vector>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/WrenchStamped.h>
#include <geometry_msgs/PoseStamped.h> #include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h> #include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/WrenchStamped.h>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <vector>
#include "ur_modern_driver/ur/consumer.h" #include "ur_modern_driver/ur/consumer.h"
@@ -37,11 +37,11 @@ private:
bool publish(RTShared& packet); bool publish(RTShared& packet);
public: public:
RTPublisher(std::string &joint_prefix, std::string &base_frame) : RTPublisher(std::string& joint_prefix, std::string& base_frame)
_joint_pub(_nh.advertise<sensor_msgs::JointState>("joint_states", 1)), : _joint_pub(_nh.advertise<sensor_msgs::JointState>("joint_states", 1))
_wrench_pub(_nh.advertise<geometry_msgs::WrenchStamped>("wrench", 1)), , _wrench_pub(_nh.advertise<geometry_msgs::WrenchStamped>("wrench", 1))
_tool_vel_pub(_nh.advertise<geometry_msgs::TwistStamped>("tool_velocity", 1)), , _tool_vel_pub(_nh.advertise<geometry_msgs::TwistStamped>("tool_velocity", 1))
_base_frame(base_frame) , _base_frame(base_frame)
{ {
for (auto const& j : JOINTS) { for (auto const& j : JOINTS) {
_joint_names.push_back(joint_prefix + j); _joint_names.push_back(joint_prefix + j);

15
include/ur_modern_driver/ur/consumer.h
View File

@@ -1,15 +1,16 @@
#pragma once #pragma once
#include "ur_modern_driver/pipeline.h" #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/master_board.h"
#include "ur_modern_driver/ur/messages.h"
#include "ur_modern_driver/ur/robot_mode.h" #include "ur_modern_driver/ur/robot_mode.h"
#include "ur_modern_driver/ur/rt_state.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<RTPacket> { class URRTPacketConsumer : public IConsumer<RTPacket> {
public: public:
virtual bool consume(unique_ptr<RTPacket> packet) { virtual bool consume(unique_ptr<RTPacket> packet)
{
return packet->consume_with(*this); return packet->consume_with(*this);
} }
@@ -19,10 +20,10 @@ public:
virtual bool consume(RTState_V3_2__3& state) = 0; virtual bool consume(RTState_V3_2__3& state) = 0;
}; };
class URStatePacketConsumer : public IConsumer<StatePacket> { class URStatePacketConsumer : public IConsumer<StatePacket> {
public: public:
virtual bool consume(unique_ptr<StatePacket> packet) { virtual bool consume(unique_ptr<StatePacket> packet)
{
return packet->consume_with(*this); return packet->consume_with(*this);
} }
@@ -35,10 +36,10 @@ public:
virtual bool consume(RobotModeData_V3_2& data) = 0; virtual bool consume(RobotModeData_V3_2& data) = 0;
}; };
class URMessagePacketConsumer : public IConsumer<MessagePacket> { class URMessagePacketConsumer : public IConsumer<MessagePacket> {
public: public:
virtual bool consume(unique_ptr<MessagePacket> packet) { virtual bool consume(unique_ptr<MessagePacket> packet)
{
return packet->consume_with(*this); return packet->consume_with(*this);
} }

26
include/ur_modern_driver/ur/factory.h
View File

@@ -1,14 +1,13 @@
#pragma once #pragma once
#include <cstdlib>
#include "ur_modern_driver/ur/stream.h"
#include "ur_modern_driver/ur/consumer.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/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 <cstdlib>
class URFactory : private URMessagePacketConsumer { class URFactory : private URMessagePacketConsumer {
private: private:
@@ -18,7 +17,8 @@ private:
uint8_t _major_version; uint8_t _major_version;
uint8_t _minor_version; uint8_t _minor_version;
bool consume(VersionMessage &vm) { bool consume(VersionMessage& vm)
{
LOG_INFO("Got VersionMessage:"); LOG_INFO("Got VersionMessage:");
LOG_INFO("project name: %s", vm.project_name.c_str()); LOG_INFO("project name: %s", vm.project_name.c_str());
LOG_INFO("version: %u.%u.%d", vm.major_version, vm.minor_version, vm.svn_version); LOG_INFO("version: %u.%u.%d", vm.major_version, vm.minor_version, vm.svn_version);
@@ -35,7 +35,9 @@ private:
void stop_consumer() {} void stop_consumer() {}
public: public:
URFactory(std::string &host) : _stream(host, 30001) { URFactory(std::string& host)
: _stream(host, 30001)
{
URProducer<MessagePacket> p(_stream, _parser); URProducer<MessagePacket> p(_stream, _parser);
std::vector<unique_ptr<MessagePacket> > results; std::vector<unique_ptr<MessagePacket> > results;
@@ -58,7 +60,8 @@ public:
p.teardown_producer(); p.teardown_producer();
} }
std::unique_ptr<URParser<StatePacket>> get_state_parser() { std::unique_ptr<URParser<StatePacket> > get_state_parser()
{
if (_major_version == 1) { if (_major_version == 1) {
return std::unique_ptr<URParser<StatePacket> >(new URStateParser_V1_X); return std::unique_ptr<URParser<StatePacket> >(new URStateParser_V1_X);
} else { } else {
@@ -69,7 +72,8 @@ public:
} }
} }
std::unique_ptr<URParser<RTPacket>> get_rt_parser() { std::unique_ptr<URParser<RTPacket> > get_rt_parser()
{
if (_major_version == 1) { if (_major_version == 1) {
if (_minor_version < 8) if (_minor_version < 8)
return std::unique_ptr<URParser<RTPacket> >(new URRTStateParser_V1_6__7); return std::unique_ptr<URParser<RTPacket> >(new URRTStateParser_V1_6__7);

9
include/ur_modern_driver/ur/master_board.h
View File

@@ -1,11 +1,10 @@
#pragma once #pragma once
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/types.h"
#include "ur_modern_driver/ur/state.h"
#include <cstddef> #include <cstddef>
#include <inttypes.h> #include <inttypes.h>
#include "ur_modern_driver/types.h"
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/ur/state.h"
class SharedMasterBoardData { class SharedMasterBoardData {
public: public:
@@ -53,7 +52,6 @@ public:
int16_t euromap_voltage; int16_t euromap_voltage;
int16_t euromap_current; int16_t euromap_current;
static const size_t SIZE = SharedMasterBoardData::SIZE static const size_t SIZE = SharedMasterBoardData::SIZE
+ sizeof(int16_t) * 2 + sizeof(int16_t) * 2
+ sizeof(uint8_t) * 2; + sizeof(uint8_t) * 2;
@@ -77,7 +75,6 @@ public:
float euromap_voltage; float euromap_voltage;
float euromap_current; float euromap_current;
static const size_t SIZE = SharedMasterBoardData::SIZE static const size_t SIZE = SharedMasterBoardData::SIZE
+ sizeof(int32_t) * 2 + sizeof(int32_t) * 2
+ sizeof(uint8_t) * 2 + sizeof(uint8_t) * 2

16
include/ur_modern_driver/ur/messages.h
View File

@@ -1,10 +1,9 @@
#pragma once #pragma once
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/pipeline.h"
#include <cstddef> #include <cstddef>
#include <inttypes.h> #include <inttypes.h>
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/bin_parser.h"
enum class robot_message_type : uint8_t { enum class robot_message_type : uint8_t {
ROBOT_MESSAGE_TEXT = 0, ROBOT_MESSAGE_TEXT = 0,
@@ -22,7 +21,11 @@ class URMessagePacketConsumer;
class MessagePacket { class MessagePacket {
public: public:
MessagePacket(uint64_t timestamp, uint8_t source) : timestamp(timestamp), source(source) { } MessagePacket(uint64_t timestamp, uint8_t source)
: timestamp(timestamp)
, source(source)
{
}
virtual bool parse_with(BinParser& bp) = 0; virtual bool parse_with(BinParser& bp) = 0;
virtual bool consume_with(URMessagePacketConsumer& consumer) = 0; virtual bool consume_with(URMessagePacketConsumer& consumer) = 0;
@@ -32,7 +35,10 @@ public:
class VersionMessage : public MessagePacket { class VersionMessage : public MessagePacket {
public: public:
VersionMessage(uint64_t timestamp, uint8_t source) : MessagePacket(timestamp, source) { } VersionMessage(uint64_t timestamp, uint8_t source)
: MessagePacket(timestamp, source)
{
}
virtual bool parse_with(BinParser& bp); virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URMessagePacketConsumer& consumer); virtual bool consume_with(URMessagePacketConsumer& consumer);

9
include/ur_modern_driver/ur/messages_parser.h
View File

@@ -1,14 +1,15 @@
#pragma once #pragma once
#include <vector>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.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/messages.h"
#include "ur_modern_driver/ur/parser.h"
#include <vector>
class URMessageParser : public URParser<MessagePacket> { class URMessageParser : public URParser<MessagePacket> {
public: public:
bool parse(BinParser &bp, std::vector<unique_ptr<MessagePacket>> &results) { bool parse(BinParser& bp, std::vector<unique_ptr<MessagePacket> >& results)
{
int32_t packet_size; int32_t packet_size;
message_type type; message_type type;
bp.parse(packet_size); bp.parse(packet_size);

4
include/ur_modern_driver/ur/parser.h
View File

@@ -1,7 +1,7 @@
#pragma once #pragma once
#include <vector>
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/pipeline.h"
#include <vector>
template <typename T> template <typename T>
class URParser { class URParser {

20
include/ur_modern_driver/ur/producer.h
View File

@@ -1,7 +1,7 @@
#pragma once #pragma once
#include "ur_modern_driver/pipeline.h" #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/parser.h"
#include "ur_modern_driver/ur/stream.h"
template <typename T> template <typename T>
class URProducer : public IProducer<T> { class URProducer : public IProducer<T> {
@@ -11,20 +11,26 @@ private:
public: public:
URProducer(URStream& stream, URParser<T>& parser) URProducer(URStream& stream, URParser<T>& parser)
: _stream(stream), : _stream(stream)
_parser(parser) { } , _parser(parser)
{
}
void setup_producer() { void setup_producer()
{
_stream.connect(); _stream.connect();
} }
void teardown_producer() { void teardown_producer()
{
_stream.disconnect(); _stream.disconnect();
} }
void stop_producer() { void stop_producer()
{
_stream.disconnect(); _stream.disconnect();
} }
bool try_get(std::vector<unique_ptr<T>> &products) { bool try_get(std::vector<unique_ptr<T> >& products)
{
//4KB should be enough to hold any packet received from UR //4KB should be enough to hold any packet received from UR
uint8_t buf[4096]; uint8_t buf[4096];

9
include/ur_modern_driver/ur/robot_mode.h
View File

@@ -1,10 +1,10 @@
#pragma once #pragma once
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/types.h"
#include "ur_modern_driver/ur/state.h"
#include <cstddef> #include <cstddef>
#include <inttypes.h> #include <inttypes.h>
#include "ur_modern_driver/types.h"
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/ur/state.h"
class SharedRobotModeData { class SharedRobotModeData {
public: public:
@@ -40,7 +40,6 @@ public:
virtual bool parse_with(BinParser& bp); virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer); virtual bool consume_with(URStatePacketConsumer& consumer);
bool security_stopped; bool security_stopped;
robot_mode_V1_X robot_mode; robot_mode_V1_X robot_mode;
double speed_fraction; double speed_fraction;
@@ -77,7 +76,6 @@ public:
virtual bool parse_with(BinParser& bp); virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer); virtual bool consume_with(URStatePacketConsumer& consumer);
bool protective_stopped; bool protective_stopped;
robot_mode_V3_X robot_mode; robot_mode_V3_X robot_mode;
@@ -101,7 +99,6 @@ public:
virtual bool parse_with(BinParser& bp); virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer); virtual bool consume_with(URStatePacketConsumer& consumer);
double target_speed_fraction_limit; double target_speed_fraction_limit;
static const size_t SIZE = RobotModeData_V3_0__1::SIZE static const size_t SIZE = RobotModeData_V3_0__1::SIZE

8
include/ur_modern_driver/ur/rt_parser.h
View File

@@ -1,16 +1,16 @@
#pragma once #pragma once
#include <vector>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.h" #include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/ur/parser.h" #include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/rt_state.h" #include "ur_modern_driver/ur/rt_state.h"
#include <vector>
template <typename T> template <typename T>
class URRTStateParser : public URParser<RTPacket> { class URRTStateParser : public URParser<RTPacket> {
public: public:
bool parse(BinParser &bp, std::vector<std::unique_ptr<RTPacket>> &results) { bool parse(BinParser& bp, std::vector<std::unique_ptr<RTPacket> >& results)
{
int32_t packet_size = bp.peek<int32_t>(); int32_t packet_size = bp.peek<int32_t>();
if (!bp.check_size(packet_size)) { if (!bp.check_size(packet_size)) {

10
include/ur_modern_driver/ur/rt_state.h
View File

@@ -1,10 +1,10 @@
#pragma once #pragma once
#include <cstddef>
#include <inttypes.h>
#include "ur_modern_driver/types.h"
#include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/pipeline.h" #include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/types.h"
#include <cstddef>
#include <inttypes.h>
class URRTPacketConsumer; class URRTPacketConsumer;
@@ -43,12 +43,10 @@ public:
double controller_time; double controller_time;
double robot_mode; double robot_mode;
static const size_t SIZE = sizeof(double) * 3 static const size_t SIZE = sizeof(double) * 3
+ sizeof(double[6]) * 10 + sizeof(double[6]) * 10
+ sizeof(cartesian_coord_t) * 2 + sizeof(cartesian_coord_t) * 2
+ sizeof(uint64_t); + sizeof(uint64_t);
}; };
class RTState_V1_6__7 : public RTShared, public RTPacket { class RTState_V1_6__7 : public RTShared, public RTPacket {
@@ -86,7 +84,6 @@ public:
cartesian_coord_t tool_vector_target; cartesian_coord_t tool_vector_target;
cartesian_coord_t tcp_speed_target; cartesian_coord_t tcp_speed_target;
double joint_modes[6]; double joint_modes[6];
double safety_mode; double safety_mode;
double3_t tool_accelerometer_values; double3_t tool_accelerometer_values;
@@ -97,7 +94,6 @@ public:
double i_robot; double i_robot;
double v_actual[6]; double v_actual[6];
static const size_t SIZE = RTShared::SIZE static const size_t SIZE = RTShared::SIZE
+ sizeof(double[6]) * 3 + sizeof(double[6]) * 3
+ sizeof(double3_t) + sizeof(double3_t)

6
include/ur_modern_driver/ur/state.h
View File

@@ -1,10 +1,10 @@
#pragma once #pragma once
#include <cstddef>
#include <inttypes.h>
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/log.h" #include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.h"
#include <cstddef>
#include <inttypes.h>
enum class package_type : uint8_t { enum class package_type : uint8_t {
ROBOT_MODE_DATA = 0, ROBOT_MODE_DATA = 0,

15
include/ur_modern_driver/ur/state_parser.h
View File

@@ -1,18 +1,18 @@
#pragma once #pragma once
#include <vector>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.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/master_board.h"
#include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/robot_mode.h" #include "ur_modern_driver/ur/robot_mode.h"
#include "ur_modern_driver/ur/state.h"
#include <vector>
template <typename RMD, typename MBD> template <typename RMD, typename MBD>
class URStateParser : public URParser<StatePacket> { class URStateParser : public URParser<StatePacket> {
private: private:
StatePacket* from_type(package_type type) { StatePacket* from_type(package_type type)
{
switch (type) { switch (type) {
case package_type::ROBOT_MODE_DATA: case package_type::ROBOT_MODE_DATA:
return new RMD; return new RMD;
@@ -24,7 +24,8 @@ private:
} }
public: public:
bool parse(BinParser &bp, std::vector<std::unique_ptr<StatePacket>> &results) { bool parse(BinParser& bp, std::vector<std::unique_ptr<StatePacket> >& results)
{
int32_t packet_size; int32_t packet_size;
message_type type; message_type type;
bp.parse(packet_size); bp.parse(packet_size);

19
include/ur_modern_driver/ur/stream.h
View File

@@ -1,9 +1,9 @@
#pragma once #pragma once
#include <string>
#include <atomic> #include <atomic>
#include <sys/types.h>
#include <sys/socket.h>
#include <netdb.h> #include <netdb.h>
#include <string>
#include <sys/socket.h>
#include <sys/types.h>
/// Encapsulates a TCP socket /// Encapsulates a TCP socket
class URStream { class URStream {
@@ -17,12 +17,15 @@ private:
public: public:
URStream(std::string& host, int port) URStream(std::string& host, int port)
: _host(host), : _host(host)
_port(port), , _port(port)
_initialized(false), , _initialized(false)
_stopping(false) {} , _stopping(false)
{
}
~URStream() { ~URStream()
{
disconnect(); disconnect();
} }

32
include/ur_modern_driver/ur_communication.h
View File

@@ -19,27 +19,26 @@
#ifndef UR_COMMUNICATION_H_ #ifndef UR_COMMUNICATION_H_
#define UR_COMMUNICATION_H_ #define UR_COMMUNICATION_H_
#include "robot_state.h"
#include "do_output.h" #include "do_output.h"
#include <vector> #include "robot_state.h"
#include <stdlib.h> #include <chrono>
#include <stdio.h>
#include <string.h>
#include <sys/time.h>
#include <thread>
#include <mutex>
#include <condition_variable> #include <condition_variable>
#include <sys/types.h> #include <fcntl.h>
#include <sys/socket.h> #include <iostream>
#include <mutex>
#include <netdb.h>
#include <netinet/in.h> #include <netinet/in.h>
#include <netinet/tcp.h> #include <netinet/tcp.h>
#include <netdb.h> #include <stdio.h>
#include <iostream> #include <stdlib.h>
#include <unistd.h> #include <string.h>
#include <chrono> #include <sys/socket.h>
#include <fcntl.h> #include <sys/time.h>
#include <sys/types.h> #include <sys/types.h>
#include <sys/types.h>
#include <thread>
#include <unistd.h>
#include <vector>
class UrCommunication { class UrCommunication {
private: private:
@@ -58,7 +57,6 @@ public:
UrCommunication(std::condition_variable& msg_cond, std::string host); UrCommunication(std::condition_variable& msg_cond, std::string host);
bool start(); bool start();
void halt(); void halt();
}; };
#endif /* UR_COMMUNICATION_H_ */ #endif /* UR_COMMUNICATION_H_ */

22
include/ur_modern_driver/ur_driver.h
View File

@@ -19,21 +19,20 @@
#ifndef UR_DRIVER_H_ #ifndef UR_DRIVER_H_
#define UR_DRIVER_H_ #define UR_DRIVER_H_
#include <mutex>
#include <condition_variable>
#include "ur_realtime_communication.h"
#include "ur_communication.h"
#include "do_output.h" #include "do_output.h"
#include <vector> #include "ur_communication.h"
#include "ur_realtime_communication.h"
#include <condition_variable>
#include <math.h> #include <math.h>
#include <string> #include <mutex>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h> #include <netinet/in.h>
#include <string>
#include <sys/socket.h>
#include <sys/types.h>
#include <vector>
#include <chrono> #include <chrono>
class UrDriver { class UrDriver {
private: private:
double maximum_time_step_; double maximum_time_step_;
@@ -52,14 +51,14 @@ private:
double firmware_version_; double firmware_version_;
double servoj_lookahead_time_; double servoj_lookahead_time_;
double servoj_gain_; double servoj_gain_;
public: public:
UrRealtimeCommunication* rt_interface_; UrRealtimeCommunication* rt_interface_;
UrCommunication* sec_interface_; UrCommunication* sec_interface_;
UrDriver(std::condition_variable& rt_msg_cond, UrDriver(std::condition_variable& rt_msg_cond,
std::condition_variable& msg_cond, std::string host, std::condition_variable& msg_cond, std::string host,
unsigned int reverse_port = 50007, double servoj_time = 0.016, unsigned int safety_count_max = 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.,
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.); double max_payload = 1., double servoj_lookahead_time = 0.03, double servoj_gain = 300.);
bool start(); bool start();
void halt(); void halt();
@@ -95,7 +94,6 @@ public:
void setServojTime(double t); void setServojTime(double t);
void setServojLookahead(double t); void setServojLookahead(double t);
void setServojGain(double g); void setServojGain(double g);
}; };
#endif /* UR_DRIVER_H_ */ #endif /* UR_DRIVER_H_ */

19
include/ur_modern_driver/ur_hardware_interface.h
View File

@@ -58,16 +58,16 @@
#ifndef UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H #ifndef UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H
#define UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H #define UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/force_torque_sensor_interface.h>
#include <hardware_interface/robot_hw.h>
#include <controller_manager/controller_manager.h>
#include <boost/scoped_ptr.hpp>
#include <ros/ros.h>
#include <math.h>
#include "do_output.h" #include "do_output.h"
#include "ur_driver.h" #include "ur_driver.h"
#include <boost/scoped_ptr.hpp>
#include <controller_manager/controller_manager.h>
#include <hardware_interface/force_torque_sensor_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>
#include <math.h>
#include <ros/ros.h>
namespace ros_control_ur { namespace ros_control_ur {
@@ -78,7 +78,6 @@ static const double VELOCITY_STEP_FACTOR = 1;
/// \brief Hardware interface for a robot /// \brief Hardware interface for a robot
class UrHardwareInterface : public hardware_interface::RobotHW { class UrHardwareInterface : public hardware_interface::RobotHW {
public: public:
/** /**
* \brief Constructor * \brief Constructor
* \param nh - Node handle for topics. * \param nh - Node handle for topics.
@@ -103,7 +102,6 @@ public:
const std::list<hardware_interface::ControllerInfo>& stop_list); const std::list<hardware_interface::ControllerInfo>& stop_list);
protected: protected:
// Startup and shutdown of the internal node inside a roscpp program // Startup and shutdown of the internal node inside a roscpp program
ros::NodeHandle nh_; ros::NodeHandle nh_;
@@ -130,7 +128,6 @@ protected:
// Robot API // Robot API
UrDriver* robot_; UrDriver* robot_;
}; };
// class // class

34
include/ur_modern_driver/ur_realtime_communication.h
View File

@@ -19,27 +19,27 @@
#ifndef UR_REALTIME_COMMUNICATION_H_ #ifndef UR_REALTIME_COMMUNICATION_H_
#define UR_REALTIME_COMMUNICATION_H_ #define UR_REALTIME_COMMUNICATION_H_
#include "robot_state_RT.h"
#include "do_output.h" #include "do_output.h"
#include <vector> #include "robot_state_RT.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <sys/time.h>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <netdb.h>
#include <iostream>
#include <unistd.h>
#include <arpa/inet.h> #include <arpa/inet.h>
#include <condition_variable>
#include <errno.h> #include <errno.h>
#include <fcntl.h> #include <fcntl.h>
#include <iostream>
#include <mutex>
#include <netdb.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <sys/types.h> #include <sys/types.h>
#include <sys/types.h>
#include <thread>
#include <unistd.h>
#include <vector>
class UrRealtimeCommunication { class UrRealtimeCommunication {
private: private:
@@ -56,7 +56,6 @@ private:
unsigned int safety_count_; unsigned int safety_count_;
void run(); void run();
public: public:
bool connected_; bool connected_;
RobotStateRT* robot_state_; RobotStateRT* robot_state_;
@@ -70,7 +69,6 @@ public:
void addCommandToQueue(std::string inp); void addCommandToQueue(std::string inp);
void setSafetyCountMax(uint inp); void setSafetyCountMax(uint inp);
std::string getLocalIp(); std::string getLocalIp();
}; };
#endif /* UR_REALTIME_COMMUNICATION_H_ */ #endif /* UR_REALTIME_COMMUNICATION_H_ */

15
src/do_output.cpp
View File

@@ -18,35 +18,40 @@
#include "ur_modern_driver/do_output.h" #include "ur_modern_driver/do_output.h"
void print_debug(std::string inp) { void print_debug(std::string inp)
{
#ifdef ROS_BUILD #ifdef ROS_BUILD
ROS_DEBUG("%s", inp.c_str()); ROS_DEBUG("%s", inp.c_str());
#else #else
printf("DEBUG: %s\n", inp.c_str()); printf("DEBUG: %s\n", inp.c_str());
#endif #endif
} }
void print_info(std::string inp) { void print_info(std::string inp)
{
#ifdef ROS_BUILD #ifdef ROS_BUILD
ROS_INFO("%s", inp.c_str()); ROS_INFO("%s", inp.c_str());
#else #else
printf("INFO: %s\n", inp.c_str()); printf("INFO: %s\n", inp.c_str());
#endif #endif
} }
void print_warning(std::string inp) { void print_warning(std::string inp)
{
#ifdef ROS_BUILD #ifdef ROS_BUILD
ROS_WARN("%s", inp.c_str()); ROS_WARN("%s", inp.c_str());
#else #else
printf("WARNING: %s\n", inp.c_str()); printf("WARNING: %s\n", inp.c_str());
#endif #endif
} }
void print_error(std::string inp) { void print_error(std::string inp)
{
#ifdef ROS_BUILD #ifdef ROS_BUILD
ROS_ERROR("%s", inp.c_str()); ROS_ERROR("%s", inp.c_str());
#else #else
printf("ERROR: %s\n", inp.c_str()); printf("ERROR: %s\n", inp.c_str());
#endif #endif
} }
void print_fatal(std::string inp) { void print_fatal(std::string inp)
{
#ifdef ROS_BUILD #ifdef ROS_BUILD
ROS_FATAL("%s", inp.c_str()); ROS_FATAL("%s", inp.c_str());
ros::shutdown(); ros::shutdown();

87
src/robot_state.cpp
View File

@@ -18,7 +18,8 @@
#include "ur_modern_driver/robot_state.h" #include "ur_modern_driver/robot_state.h"
RobotState::RobotState(std::condition_variable& msg_cond) { RobotState::RobotState(std::condition_variable& msg_cond)
{
version_msg_.major_version = 0; version_msg_.major_version = 0;
version_msg_.minor_version = 0; version_msg_.minor_version = 0;
new_data_available_ = false; new_data_available_ = false;
@@ -26,13 +27,15 @@ RobotState::RobotState(std::condition_variable& msg_cond) {
RobotState::setDisconnected(); RobotState::setDisconnected();
robot_mode_running_ = robotStateTypeV30::ROBOT_MODE_RUNNING; robot_mode_running_ = robotStateTypeV30::ROBOT_MODE_RUNNING;
} }
double RobotState::ntohd(uint64_t nf) { double RobotState::ntohd(uint64_t nf)
{
double x; double x;
nf = be64toh(nf); nf = be64toh(nf);
memcpy(&x, &nf, sizeof(x)); memcpy(&x, &nf, sizeof(x));
return x; return x;
} }
void RobotState::unpack(uint8_t* buf, unsigned int buf_length) { void RobotState::unpack(uint8_t* buf, unsigned int buf_length)
{
/* Returns missing bytes to unpack a message, or 0 if all data was parsed */ /* Returns missing bytes to unpack a message, or 0 if all data was parsed */
unsigned int offset = 0; unsigned int offset = 0;
while (buf_length > offset) { while (buf_length > offset) {
@@ -57,13 +60,13 @@ void RobotState::unpack(uint8_t* buf, unsigned int buf_length) {
break; break;
} }
offset += len; offset += len;
} }
return; return;
} }
void RobotState::unpackRobotMessage(uint8_t* buf, unsigned int offset, void RobotState::unpackRobotMessage(uint8_t* buf, unsigned int offset,
uint32_t len) { uint32_t len)
{
offset += 5; offset += 5;
uint64_t timestamp; uint64_t timestamp;
int8_t source, robot_message_type; int8_t source, robot_message_type;
@@ -85,11 +88,11 @@ void RobotState::unpackRobotMessage(uint8_t * buf, unsigned int offset,
default: default:
break; break;
} }
} }
void RobotState::unpackRobotState(uint8_t* buf, unsigned int offset, void RobotState::unpackRobotState(uint8_t* buf, unsigned int offset,
uint32_t len) { uint32_t len)
{
offset += 5; offset += 5;
while (offset < len) { while (offset < len) {
int32_t length; int32_t length;
@@ -117,11 +120,11 @@ void RobotState::unpackRobotState(uint8_t * buf, unsigned int offset,
} }
new_data_available_ = true; new_data_available_ = true;
pMsg_cond_->notify_all(); pMsg_cond_->notify_all();
} }
void RobotState::unpackRobotMessageVersion(uint8_t* buf, unsigned int offset, void RobotState::unpackRobotMessageVersion(uint8_t* buf, unsigned int offset,
uint32_t len) { uint32_t len)
{
memcpy(&version_msg_.project_name_size, &buf[offset], memcpy(&version_msg_.project_name_size, &buf[offset],
sizeof(version_msg_.project_name_size)); sizeof(version_msg_.project_name_size));
offset += sizeof(version_msg_.project_name_size); offset += sizeof(version_msg_.project_name_size);
@@ -146,7 +149,8 @@ void RobotState::unpackRobotMessageVersion(uint8_t * buf, unsigned int offset,
} }
} }
void RobotState::unpackRobotMode(uint8_t * buf, unsigned int offset) { void RobotState::unpackRobotMode(uint8_t* buf, unsigned int offset)
{
memcpy(&robot_mode_.timestamp, &buf[offset], sizeof(robot_mode_.timestamp)); memcpy(&robot_mode_.timestamp, &buf[offset], sizeof(robot_mode_.timestamp));
offset += sizeof(robot_mode_.timestamp); offset += sizeof(robot_mode_.timestamp);
uint8_t tmp; uint8_t tmp;
@@ -210,7 +214,8 @@ void RobotState::unpackRobotMode(uint8_t * buf, unsigned int offset) {
} }
void RobotState::unpackRobotStateMasterboard(uint8_t* buf, void RobotState::unpackRobotStateMasterboard(uint8_t* buf,
unsigned int offset) { unsigned int offset)
{
if (RobotState::getVersion() < 3.0) { if (RobotState::getVersion() < 3.0) {
int16_t digital_input_bits, digital_output_bits; int16_t digital_input_bits, digital_output_bits;
memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits)); memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits));
@@ -308,79 +313,95 @@ void RobotState::unpackRobotStateMasterboard(uint8_t * buf,
offset += sizeof(mb_data_.euromapCurrent); offset += sizeof(mb_data_.euromapCurrent);
mb_data_.euromapCurrent = ntohl(mb_data_.euromapCurrent); mb_data_.euromapCurrent = ntohl(mb_data_.euromapCurrent);
} }
} }
} }
double RobotState::getVersion() { double RobotState::getVersion()
{
double ver; double ver;
val_lock_.lock(); val_lock_.lock();
ver = version_msg_.major_version + 0.1 * version_msg_.minor_version ver = version_msg_.major_version + 0.1 * version_msg_.minor_version
+ .0000001 * version_msg_.svn_revision; + .0000001 * version_msg_.svn_revision;
val_lock_.unlock(); val_lock_.unlock();
return ver; return ver;
} }
void RobotState::finishedReading() { void RobotState::finishedReading()
{
new_data_available_ = false; new_data_available_ = false;
} }
bool RobotState::getNewDataAvailable() { bool RobotState::getNewDataAvailable()
{
return new_data_available_; return new_data_available_;
} }
int RobotState::getDigitalInputBits() { int RobotState::getDigitalInputBits()
{
return mb_data_.digitalInputBits; return mb_data_.digitalInputBits;
} }
int RobotState::getDigitalOutputBits() { int RobotState::getDigitalOutputBits()
{
return mb_data_.digitalOutputBits; return mb_data_.digitalOutputBits;
} }
double RobotState::getAnalogInput0() { double RobotState::getAnalogInput0()
{
return mb_data_.analogInput0; return mb_data_.analogInput0;
} }
double RobotState::getAnalogInput1() { double RobotState::getAnalogInput1()
{
return mb_data_.analogInput1; return mb_data_.analogInput1;
} }
double RobotState::getAnalogOutput0() { double RobotState::getAnalogOutput0()
{
return mb_data_.analogOutput0; return mb_data_.analogOutput0;
} }
double RobotState::getAnalogOutput1() { double RobotState::getAnalogOutput1()
{
return mb_data_.analogOutput1; return mb_data_.analogOutput1;
} }
bool RobotState::isRobotConnected() { bool RobotState::isRobotConnected()
{
return robot_mode_.isRobotConnected; return robot_mode_.isRobotConnected;
} }
bool RobotState::isRealRobotEnabled() { bool RobotState::isRealRobotEnabled()
{
return robot_mode_.isRealRobotEnabled; return robot_mode_.isRealRobotEnabled;
} }
bool RobotState::isPowerOnRobot() { bool RobotState::isPowerOnRobot()
{
return robot_mode_.isPowerOnRobot; return robot_mode_.isPowerOnRobot;
} }
bool RobotState::isEmergencyStopped() { bool RobotState::isEmergencyStopped()
{
return robot_mode_.isEmergencyStopped; return robot_mode_.isEmergencyStopped;
} }
bool RobotState::isProtectiveStopped() { bool RobotState::isProtectiveStopped()
{
return robot_mode_.isProtectiveStopped; return robot_mode_.isProtectiveStopped;
} }
bool RobotState::isProgramRunning() { bool RobotState::isProgramRunning()
{
return robot_mode_.isProgramRunning; return robot_mode_.isProgramRunning;
} }
bool RobotState::isProgramPaused() { bool RobotState::isProgramPaused()
{
return robot_mode_.isProgramPaused; return robot_mode_.isProgramPaused;
} }
unsigned char RobotState::getRobotMode() { unsigned char RobotState::getRobotMode()
{
return robot_mode_.robotMode; return robot_mode_.robotMode;
} }
bool RobotState::isReady() { bool RobotState::isReady()
{
if (robot_mode_.robotMode == robot_mode_running_) { if (robot_mode_.robotMode == robot_mode_running_) {
return true; return true;
} }
return false; return false;
} }
void RobotState::setDisconnected() { void RobotState::setDisconnected()
{
robot_mode_.isRobotConnected = false; robot_mode_.isRobotConnected = false;
robot_mode_.isRealRobotEnabled = false; robot_mode_.isRealRobotEnabled = false;
robot_mode_.isPowerOnRobot = false; robot_mode_.isPowerOnRobot = false;

122
src/robot_state_RT.cpp
View File

@@ -18,7 +18,8 @@
#include "ur_modern_driver/robot_state_RT.h" #include "ur_modern_driver/robot_state_RT.h"
RobotStateRT::RobotStateRT(std::condition_variable& msg_cond) { RobotStateRT::RobotStateRT(std::condition_variable& msg_cond)
{
version_ = 0.0; version_ = 0.0;
time_ = 0.0; time_ = 0.0;
q_target_.assign(6, 0.0); q_target_.assign(6, 0.0);
@@ -53,28 +54,34 @@ RobotStateRT::RobotStateRT(std::condition_variable& msg_cond) {
pMsg_cond_ = &msg_cond; pMsg_cond_ = &msg_cond;
} }
RobotStateRT::~RobotStateRT() { RobotStateRT::~RobotStateRT()
{
/* Make sure nobody is waiting after this thread is destroyed */ /* Make sure nobody is waiting after this thread is destroyed */
data_published_ = true; data_published_ = true;
controller_updated_ = true; controller_updated_ = true;
pMsg_cond_->notify_all(); pMsg_cond_->notify_all();
} }
void RobotStateRT::setDataPublished() { void RobotStateRT::setDataPublished()
{
data_published_ = false; data_published_ = false;
} }
bool RobotStateRT::getDataPublished() { bool RobotStateRT::getDataPublished()
{
return data_published_; return data_published_;
} }
void RobotStateRT::setControllerUpdated() { void RobotStateRT::setControllerUpdated()
{
controller_updated_ = false; controller_updated_ = false;
} }
bool RobotStateRT::getControllerUpdated() { bool RobotStateRT::getControllerUpdated()
{
return controller_updated_; return controller_updated_;
} }
double RobotStateRT::ntohd(uint64_t nf) { double RobotStateRT::ntohd(uint64_t nf)
{
double x; double x;
nf = be64toh(nf); nf = be64toh(nf);
memcpy(&x, &nf, sizeof(x)); memcpy(&x, &nf, sizeof(x));
@@ -82,7 +89,8 @@ double RobotStateRT::ntohd(uint64_t nf) {
} }
std::vector<double> RobotStateRT::unpackVector(uint8_t* buf, int start_index, std::vector<double> RobotStateRT::unpackVector(uint8_t* buf, int start_index,
int nr_of_vals) { int nr_of_vals)
{
uint64_t q; uint64_t q;
std::vector<double> ret; std::vector<double> ret;
for (int i = 0; i < nr_of_vals; i++) { for (int i = 0; i < nr_of_vals; i++) {
@@ -92,7 +100,8 @@ std::vector<double> RobotStateRT::unpackVector(uint8_t * buf, int start_index,
return ret; return ret;
} }
std::vector<bool> RobotStateRT::unpackDigitalInputBits(int64_t data) { std::vector<bool> RobotStateRT::unpackDigitalInputBits(int64_t data)
{
std::vector<bool> ret; std::vector<bool> ret;
for (int i = 0; i < 64; i++) { for (int i = 0; i < 64; i++) {
ret.push_back((data & (1 << i)) >> i); ret.push_back((data & (1 << i)) >> i);
@@ -100,216 +109,247 @@ std::vector<bool> RobotStateRT::unpackDigitalInputBits(int64_t data) {
return ret; return ret;
} }
void RobotStateRT::setVersion(double ver) { void RobotStateRT::setVersion(double ver)
{
val_lock_.lock(); val_lock_.lock();
version_ = ver; version_ = ver;
val_lock_.unlock(); val_lock_.unlock();
} }
double RobotStateRT::getVersion() { double RobotStateRT::getVersion()
{
double ret; double ret;
val_lock_.lock(); val_lock_.lock();
ret = version_; ret = version_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
double RobotStateRT::getTime() { double RobotStateRT::getTime()
{
double ret; double ret;
val_lock_.lock(); val_lock_.lock();
ret = time_; ret = time_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
std::vector<double> RobotStateRT::getQTarget() { std::vector<double> RobotStateRT::getQTarget()
{
std::vector<double> ret; std::vector<double> ret;
val_lock_.lock(); val_lock_.lock();
ret = q_target_; ret = q_target_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
std::vector<double> RobotStateRT::getQdTarget() { std::vector<double> RobotStateRT::getQdTarget()
{
std::vector<double> ret; std::vector<double> ret;
val_lock_.lock(); val_lock_.lock();
ret = qd_target_; ret = qd_target_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
std::vector<double> RobotStateRT::getQddTarget() { std::vector<double> RobotStateRT::getQddTarget()
{
std::vector<double> ret; std::vector<double> ret;
val_lock_.lock(); val_lock_.lock();
ret = qdd_target_; ret = qdd_target_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
std::vector<double> RobotStateRT::getITarget() { std::vector<double> RobotStateRT::getITarget()
{
std::vector<double> ret; std::vector<double> ret;
val_lock_.lock(); val_lock_.lock();
ret = i_target_; ret = i_target_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
std::vector<double> RobotStateRT::getMTarget() { std::vector<double> RobotStateRT::getMTarget()
{
std::vector<double> ret; std::vector<double> ret;
val_lock_.lock(); val_lock_.lock();
ret = m_target_; ret = m_target_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
std::vector<double> RobotStateRT::getQActual() { std::vector<double> RobotStateRT::getQActual()
{
std::vector<double> ret; std::vector<double> ret;
val_lock_.lock(); val_lock_.lock();
ret = q_actual_; ret = q_actual_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
std::vector<double> RobotStateRT::getQdActual() { std::vector<double> RobotStateRT::getQdActual()
{
std::vector<double> ret; std::vector<double> ret;
val_lock_.lock(); val_lock_.lock();
ret = qd_actual_; ret = qd_actual_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
std::vector<double> RobotStateRT::getIActual() { std::vector<double> RobotStateRT::getIActual()
{
std::vector<double> ret; std::vector<double> ret;
val_lock_.lock(); val_lock_.lock();
ret = i_actual_; ret = i_actual_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
std::vector<double> RobotStateRT::getIControl() { std::vector<double> RobotStateRT::getIControl()
{
std::vector<double> ret; std::vector<double> ret;
val_lock_.lock(); val_lock_.lock();
ret = i_control_; ret = i_control_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
std::vector<double> RobotStateRT::getToolVectorActual() { std::vector<double> RobotStateRT::getToolVectorActual()
{
std::vector<double> ret; std::vector<double> ret;
val_lock_.lock(); val_lock_.lock();
ret = tool_vector_actual_; ret = tool_vector_actual_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
std::vector<double> RobotStateRT::getTcpSpeedActual() { std::vector<double> RobotStateRT::getTcpSpeedActual()
{
std::vector<double> ret; std::vector<double> ret;
val_lock_.lock(); val_lock_.lock();
ret = tcp_speed_actual_; ret = tcp_speed_actual_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
std::vector<double> RobotStateRT::getTcpForce() { std::vector<double> RobotStateRT::getTcpForce()
{
std::vector<double> ret; std::vector<double> ret;
val_lock_.lock(); val_lock_.lock();
ret = tcp_force_; ret = tcp_force_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
std::vector<double> RobotStateRT::getToolVectorTarget() { std::vector<double> RobotStateRT::getToolVectorTarget()
{
std::vector<double> ret; std::vector<double> ret;
val_lock_.lock(); val_lock_.lock();
ret = tool_vector_target_; ret = tool_vector_target_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
std::vector<double> RobotStateRT::getTcpSpeedTarget() { std::vector<double> RobotStateRT::getTcpSpeedTarget()
{
std::vector<double> ret; std::vector<double> ret;
val_lock_.lock(); val_lock_.lock();
ret = tcp_speed_target_; ret = tcp_speed_target_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
std::vector<bool> RobotStateRT::getDigitalInputBits() { std::vector<bool> RobotStateRT::getDigitalInputBits()
{
std::vector<bool> ret; std::vector<bool> ret;
val_lock_.lock(); val_lock_.lock();
ret = digital_input_bits_; ret = digital_input_bits_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
std::vector<double> RobotStateRT::getMotorTemperatures() { std::vector<double> RobotStateRT::getMotorTemperatures()
{
std::vector<double> ret; std::vector<double> ret;
val_lock_.lock(); val_lock_.lock();
ret = motor_temperatures_; ret = motor_temperatures_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
double RobotStateRT::getControllerTimer() { double RobotStateRT::getControllerTimer()
{
double ret; double ret;
val_lock_.lock(); val_lock_.lock();
ret = controller_timer_; ret = controller_timer_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
double RobotStateRT::getRobotMode() { double RobotStateRT::getRobotMode()
{
double ret; double ret;
val_lock_.lock(); val_lock_.lock();
ret = robot_mode_; ret = robot_mode_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
std::vector<double> RobotStateRT::getJointModes() { std::vector<double> RobotStateRT::getJointModes()
{
std::vector<double> ret; std::vector<double> ret;
val_lock_.lock(); val_lock_.lock();
ret = joint_modes_; ret = joint_modes_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
double RobotStateRT::getSafety_mode() { double RobotStateRT::getSafety_mode()
{
double ret; double ret;
val_lock_.lock(); val_lock_.lock();
ret = safety_mode_; ret = safety_mode_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
std::vector<double> RobotStateRT::getToolAccelerometerValues() { std::vector<double> RobotStateRT::getToolAccelerometerValues()
{
std::vector<double> ret; std::vector<double> ret;
val_lock_.lock(); val_lock_.lock();
ret = tool_accelerometer_values_; ret = tool_accelerometer_values_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
double RobotStateRT::getSpeedScaling() { double RobotStateRT::getSpeedScaling()
{
double ret; double ret;
val_lock_.lock(); val_lock_.lock();
ret = speed_scaling_; ret = speed_scaling_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
double RobotStateRT::getLinearMomentumNorm() { double RobotStateRT::getLinearMomentumNorm()
{
double ret; double ret;
val_lock_.lock(); val_lock_.lock();
ret = linear_momentum_norm_; ret = linear_momentum_norm_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
double RobotStateRT::getVMain() { double RobotStateRT::getVMain()
{
double ret; double ret;
val_lock_.lock(); val_lock_.lock();
ret = v_main_; ret = v_main_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
double RobotStateRT::getVRobot() { double RobotStateRT::getVRobot()
{
double ret; double ret;
val_lock_.lock(); val_lock_.lock();
ret = v_robot_; ret = v_robot_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
double RobotStateRT::getIRobot() { double RobotStateRT::getIRobot()
{
double ret; double ret;
val_lock_.lock(); val_lock_.lock();
ret = i_robot_; ret = i_robot_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
std::vector<double> RobotStateRT::getVActual() { std::vector<double> RobotStateRT::getVActual()
{
std::vector<double> ret; std::vector<double> ret;
val_lock_.lock(); val_lock_.lock();
ret = v_actual_; ret = v_actual_;
val_lock_.unlock(); val_lock_.unlock();
return ret; return ret;
} }
void RobotStateRT::unpack(uint8_t * buf) { void RobotStateRT::unpack(uint8_t* buf)
{
int64_t digital_input_bits; int64_t digital_input_bits;
uint64_t unpack_to; uint64_t unpack_to;
uint16_t offset = 0; uint16_t offset = 0;
@@ -432,6 +472,4 @@ void RobotStateRT::unpack(uint8_t * buf) {
controller_updated_ = true; controller_updated_ = true;
data_published_ = true; data_published_ = true;
pMsg_cond_->notify_all(); pMsg_cond_->notify_all();
} }

24
src/ros/rt_publisher.cpp
View File

@@ -1,6 +1,7 @@
#include "ur_modern_driver/ros/rt_publisher.h" #include "ur_modern_driver/ros/rt_publisher.h"
bool RTPublisher::publish_joints(RTShared &packet, ros::Time &t) { bool RTPublisher::publish_joints(RTShared& packet, ros::Time& t)
{
sensor_msgs::JointState joint_msg; sensor_msgs::JointState joint_msg;
joint_msg.header.stamp = t; joint_msg.header.stamp = t;
joint_msg.name = _joint_names; joint_msg.name = _joint_names;
@@ -20,7 +21,8 @@ bool RTPublisher::publish_joints(RTShared &packet, ros::Time &t) {
return true; 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; geometry_msgs::WrenchStamped wrench_msg;
wrench_msg.header.stamp = t; wrench_msg.header.stamp = t;
@@ -36,7 +38,8 @@ bool RTPublisher::publish_wrench(RTShared &packet, ros::Time &t) {
return true; 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; geometry_msgs::TwistStamped tool_twist;
tool_twist.header.stamp = t; tool_twist.header.stamp = t;
@@ -55,20 +58,25 @@ bool RTPublisher::publish_tool(RTShared &packet, ros::Time &t) {
return true; return true;
} }
bool RTPublisher::publish(RTShared &packet) { bool RTPublisher::publish(RTShared& packet)
{
ros::Time time = ros::Time::now(); ros::Time time = ros::Time::now();
return publish_joints(packet, time) && publish_wrench(packet, time) && publish_tool(packet, time); 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); return publish(state);
} }
bool RTPublisher::consume(RTState_V1_8 &state) { bool RTPublisher::consume(RTState_V1_8& state)
{
return publish(state); return publish(state);
} }
bool RTPublisher::consume(RTState_V3_0__1 &state) { bool RTPublisher::consume(RTState_V3_0__1& state)
{
return publish(state); return publish(state);
} }
bool RTPublisher::consume(RTState_V3_2__3 &state) { bool RTPublisher::consume(RTState_V3_2__3& state)
{
return publish(state); return publish(state);
} }

27
src/ros_main.cpp
View File

@@ -1,18 +1,18 @@
#include <cstdlib>
#include <string>
#include <chrono> #include <chrono>
#include <thread> #include <cstdlib>
#include <ros/ros.h> #include <ros/ros.h>
#include <string>
#include <thread>
#include "ur_modern_driver/log.h" #include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.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/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 IP_ADDR_ARG("~robot_ip_address");
static const std::string REVERSE_PORT_ARG("~reverse_port"); static const std::string REVERSE_PORT_ARG("~reverse_port");
@@ -21,7 +21,6 @@ static const std::string PREFIX_ARG("~prefix");
static const std::string BASE_FRAME_ARG("~base_frame"); static const std::string BASE_FRAME_ARG("~base_frame");
static const std::string TOOL_FRAME_ARG("~tool_frame"); static const std::string TOOL_FRAME_ARG("~tool_frame");
struct ProgArgs { struct ProgArgs {
public: public:
std::string host; std::string host;
@@ -32,7 +31,8 @@ public:
bool use_sim_time; bool use_sim_time;
}; };
bool parse_args(ProgArgs &args) { bool parse_args(ProgArgs& args)
{
if (!ros::param::get(IP_ADDR_ARG, args.host)) { if (!ros::param::get(IP_ADDR_ARG, args.host)) {
LOG_ERROR("robot_ip_address parameter must be set!"); LOG_ERROR("robot_ip_address parameter must be set!");
return false; return false;
@@ -45,8 +45,8 @@ bool parse_args(ProgArgs &args) {
return true; return true;
} }
int main(int argc, char** argv)
int main(int argc, char **argv) { {
ros::init(argc, argv, "ur_driver"); ros::init(argc, argv, "ur_driver");
@@ -89,6 +89,5 @@ int main(int argc, char **argv) {
pl.stop(); pl.stop();
return EXIT_SUCCESS; return EXIT_SUCCESS;
} }

27
src/ur/master_board.cpp
View File

@@ -1,7 +1,8 @@
#include "ur_modern_driver/ur/master_board.h" #include "ur_modern_driver/ur/master_board.h"
#include "ur_modern_driver/ur/consumer.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_range0);
bp.parse(analog_input_range1); bp.parse(analog_input_range1);
bp.parse(analog_input0); bp.parse(analog_input0);
@@ -17,7 +18,8 @@ bool SharedMasterBoardData::parse_with(BinParser &bp) {
return true; return true;
} }
bool MasterBoardData_V1_X::parse_with(BinParser &bp) { bool MasterBoardData_V1_X::parse_with(BinParser& bp)
{
if (!bp.check_size<MasterBoardData_V1_X>()) if (!bp.check_size<MasterBoardData_V1_X>())
return false; return false;
@@ -41,7 +43,8 @@ bool MasterBoardData_V1_X::parse_with(BinParser &bp) {
return true; return true;
} }
bool MasterBoardData_V3_0__1::parse_with(BinParser &bp) { bool MasterBoardData_V3_0__1::parse_with(BinParser& bp)
{
if (!bp.check_size<MasterBoardData_V3_0__1>()) if (!bp.check_size<MasterBoardData_V3_0__1>())
return false; return false;
@@ -67,9 +70,8 @@ bool MasterBoardData_V3_0__1::parse_with(BinParser &bp) {
return true; return true;
} }
bool MasterBoardData_V3_2::parse_with(BinParser& bp)
{
bool MasterBoardData_V3_2::parse_with(BinParser &bp) {
if (!bp.check_size<MasterBoardData_V3_2>()) if (!bp.check_size<MasterBoardData_V3_2>())
return false; return false;
@@ -81,16 +83,15 @@ bool MasterBoardData_V3_2::parse_with(BinParser &bp) {
return true; return true;
} }
bool MasterBoardData_V1_X::consume_with(URStatePacketConsumer& consumer)
{
bool MasterBoardData_V1_X::consume_with(URStatePacketConsumer &consumer) {
return consumer.consume(*this); 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); return consumer.consume(*this);
} }
bool MasterBoardData_V3_2::consume_with(URStatePacketConsumer &consumer) { bool MasterBoardData_V3_2::consume_with(URStatePacketConsumer& consumer)
{
return consumer.consume(*this); return consumer.consume(*this);
} }

8
src/ur/messages.cpp
View File

@@ -1,8 +1,8 @@
#include "ur_modern_driver/ur/messages.h" #include "ur_modern_driver/ur/messages.h"
#include "ur_modern_driver/ur/consumer.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(project_name);
bp.parse(major_version); bp.parse(major_version);
@@ -14,7 +14,7 @@ bool VersionMessage::parse_with(BinParser &bp) {
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); return consumer.consume(*this);
} }

27
src/ur/robot_mode.cpp
View File

@@ -1,8 +1,8 @@
#include "ur_modern_driver/ur/robot_mode.h" #include "ur_modern_driver/ur/robot_mode.h"
#include "ur_modern_driver/ur/consumer.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(timestamp);
bp.parse(physical_robot_connected); bp.parse(physical_robot_connected);
bp.parse(real_robot_enabled); bp.parse(real_robot_enabled);
@@ -11,8 +11,8 @@ bool SharedRobotModeData::parse_with(BinParser &bp) {
return true; return true;
} }
bool RobotModeData_V1_X::parse_with(BinParser& bp)
bool RobotModeData_V1_X::parse_with(BinParser &bp) { {
if (!bp.check_size<RobotModeData_V1_X>()) if (!bp.check_size<RobotModeData_V1_X>())
return false; return false;
@@ -27,8 +27,8 @@ bool RobotModeData_V1_X::parse_with(BinParser &bp) {
return true; return true;
} }
bool RobotModeData_V3_0__1::parse_with(BinParser& bp)
bool RobotModeData_V3_0__1::parse_with(BinParser &bp) { {
if (!bp.check_size<RobotModeData_V3_0__1>()) if (!bp.check_size<RobotModeData_V3_0__1>())
return false; return false;
@@ -45,7 +45,8 @@ bool RobotModeData_V3_0__1::parse_with(BinParser &bp) {
return true; return true;
} }
bool RobotModeData_V3_2::parse_with(BinParser &bp) { bool RobotModeData_V3_2::parse_with(BinParser& bp)
{
if (!bp.check_size<RobotModeData_V3_2>()) if (!bp.check_size<RobotModeData_V3_2>())
return false; return false;
@@ -56,15 +57,15 @@ bool RobotModeData_V3_2::parse_with(BinParser &bp) {
return true; return true;
} }
bool RobotModeData_V1_X::consume_with(URStatePacketConsumer& consumer)
{
bool RobotModeData_V1_X::consume_with(URStatePacketConsumer &consumer) {
return consumer.consume(*this); 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); return consumer.consume(*this);
} }
bool RobotModeData_V3_2::consume_with(URStatePacketConsumer &consumer) { bool RobotModeData_V3_2::consume_with(URStatePacketConsumer& consumer)
{
return consumer.consume(*this); return consumer.consume(*this);
} }

32
src/ur/rt_state.cpp
View File

@@ -1,8 +1,8 @@
#include "ur_modern_driver/ur/rt_state.h" #include "ur_modern_driver/ur/rt_state.h"
#include "ur_modern_driver/ur/consumer.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(time);
bp.parse(q_target); bp.parse(q_target);
bp.parse(qd_target); bp.parse(qd_target);
@@ -15,7 +15,8 @@ bool RTShared::parse_shared1(BinParser &bp) {
return true; return true;
} }
bool RTShared::parse_shared2(BinParser &bp) { bool RTShared::parse_shared2(BinParser& bp)
{
bp.parse(digital_input); bp.parse(digital_input);
bp.parse(motor_temperatures); bp.parse(motor_temperatures);
bp.parse(controller_time); bp.parse(controller_time);
@@ -23,8 +24,8 @@ bool RTShared::parse_shared2(BinParser &bp) {
return true; return true;
} }
bool RTState_V1_6__7::parse_with(BinParser& bp)
bool RTState_V1_6__7::parse_with(BinParser &bp) { {
if (!bp.check_size<RTState_V1_6__7>()) if (!bp.check_size<RTState_V1_6__7>())
return false; return false;
@@ -40,7 +41,8 @@ bool RTState_V1_6__7::parse_with(BinParser &bp) {
return true; return true;
} }
bool RTState_V1_8::parse_with(BinParser &bp) { bool RTState_V1_8::parse_with(BinParser& bp)
{
if (!bp.check_size<RTState_V1_8>()) if (!bp.check_size<RTState_V1_8>())
return false; return false;
@@ -51,7 +53,8 @@ bool RTState_V1_8::parse_with(BinParser &bp) {
return true; return true;
} }
bool RTState_V3_0__1::parse_with(BinParser &bp) { bool RTState_V3_0__1::parse_with(BinParser& bp)
{
if (!bp.check_size<RTState_V3_0__1>()) if (!bp.check_size<RTState_V3_0__1>())
return false; return false;
@@ -83,7 +86,8 @@ bool RTState_V3_0__1::parse_with(BinParser &bp) {
return true; return true;
} }
bool RTState_V3_2__3::parse_with(BinParser &bp) { bool RTState_V3_2__3::parse_with(BinParser& bp)
{
if (!bp.check_size<RTState_V3_2__3>()) if (!bp.check_size<RTState_V3_2__3>())
return false; return false;
@@ -95,15 +99,19 @@ bool RTState_V3_2__3::parse_with(BinParser &bp) {
return true; return true;
} }
bool RTState_V1_6__7::consume_with(URRTPacketConsumer &consumer) { bool RTState_V1_6__7::consume_with(URRTPacketConsumer& consumer)
{
return consumer.consume(*this); return consumer.consume(*this);
} }
bool RTState_V1_8::consume_with(URRTPacketConsumer &consumer) { bool RTState_V1_8::consume_with(URRTPacketConsumer& consumer)
{
return consumer.consume(*this); 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); 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); return consumer.consume(*this);
} }

4
src/ur/state.cpp
View File

@@ -1,7 +1,5 @@
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ur/state.h" #include "ur_modern_driver/ur/state.h"
#include "ur_modern_driver/log.h"
//StatePacket::~StatePacket() { } //StatePacket::~StatePacket() { }

18
src/ur/stream.cpp
View File

@@ -1,12 +1,13 @@
#include <cstring> #include <cstring>
#include <unistd.h>
#include <netinet/tcp.h>
#include <endian.h> #include <endian.h>
#include <netinet/tcp.h>
#include <unistd.h>
#include "ur_modern_driver/ur/stream.h"
#include "ur_modern_driver/log.h" #include "ur_modern_driver/log.h"
#include "ur_modern_driver/ur/stream.h"
bool URStream::connect() { bool URStream::connect()
{
if (_initialized) if (_initialized)
return false; return false;
@@ -57,7 +58,8 @@ bool URStream::connect() {
return _initialized; return _initialized;
} }
void URStream::disconnect() { void URStream::disconnect()
{
if (!_initialized || _stopping) if (!_initialized || _stopping)
return; return;
@@ -66,7 +68,8 @@ void URStream::disconnect() {
_initialized = false; _initialized = false;
} }
ssize_t URStream::send(uint8_t *buf, size_t buf_len) { ssize_t URStream::send(uint8_t* buf, size_t buf_len)
{
if (!_initialized) if (!_initialized)
return -1; return -1;
if (_stopping) if (_stopping)
@@ -88,7 +91,8 @@ ssize_t URStream::send(uint8_t *buf, size_t buf_len) {
return total; return total;
} }
ssize_t URStream::receive(uint8_t *buf, size_t buf_len) { ssize_t URStream::receive(uint8_t* buf, size_t buf_len)
{
if (!_initialized) if (!_initialized)
return -1; return -1;
if (_stopping) if (_stopping)

16
src/ur_communication.cpp
View File

@@ -19,7 +19,8 @@
#include "ur_modern_driver/ur_communication.h" #include "ur_modern_driver/ur_communication.h"
UrCommunication::UrCommunication(std::condition_variable& msg_cond, UrCommunication::UrCommunication(std::condition_variable& msg_cond,
std::string host) { std::string host)
{
robot_state_ = new RobotState(msg_cond); robot_state_ = new RobotState(msg_cond);
bzero((char*)&pri_serv_addr_, sizeof(pri_serv_addr_)); bzero((char*)&pri_serv_addr_, sizeof(pri_serv_addr_));
bzero((char*)&sec_serv_addr_, sizeof(sec_serv_addr_)); bzero((char*)&sec_serv_addr_, sizeof(sec_serv_addr_));
@@ -59,7 +60,8 @@ UrCommunication::UrCommunication(std::condition_variable& msg_cond,
keepalive_ = false; keepalive_ = false;
} }
bool UrCommunication::start() { bool UrCommunication::start()
{
keepalive_ = true; keepalive_ = true;
uint8_t buf[512]; uint8_t buf[512];
unsigned int bytes_read; unsigned int bytes_read;
@@ -67,7 +69,8 @@ bool UrCommunication::start() {
bzero(buf, 512); bzero(buf, 512);
print_debug("Acquire firmware version: Connecting..."); print_debug("Acquire firmware version: Connecting...");
if (connect(pri_sockfd_, (struct sockaddr*)&pri_serv_addr_, if (connect(pri_sockfd_, (struct sockaddr*)&pri_serv_addr_,
sizeof(pri_serv_addr_)) < 0) { sizeof(pri_serv_addr_))
< 0) {
print_fatal("Error connecting to get firmware version"); print_fatal("Error connecting to get firmware version");
return false; return false;
} }
@@ -107,12 +110,14 @@ bool UrCommunication::start() {
return true; return true;
} }
void UrCommunication::halt() { void UrCommunication::halt()
{
keepalive_ = false; keepalive_ = false;
comThread_.join(); comThread_.join();
} }
void UrCommunication::run() { void UrCommunication::run()
{
uint8_t buf[2048]; uint8_t buf[2048];
int bytes_read; int bytes_read;
bzero(buf, 2048); bzero(buf, 2048);
@@ -178,4 +183,3 @@ void UrCommunication::run() {
std::this_thread::sleep_for(std::chrono::milliseconds(500)); std::this_thread::sleep_for(std::chrono::milliseconds(500));
close(sec_sockfd_); close(sec_sockfd_);
} }

102
src/ur_driver.cpp
View File

@@ -22,10 +22,17 @@ UrDriver::UrDriver(std::condition_variable& rt_msg_cond,
std::condition_variable& msg_cond, std::string host, std::condition_variable& msg_cond, std::string host,
unsigned int reverse_port, double servoj_time, unsigned int reverse_port, double servoj_time,
unsigned int safety_count_max, double max_time_step, double min_payload, unsigned int safety_count_max, double max_time_step, double min_payload,
double max_payload, double servoj_lookahead_time, double servoj_gain) : double max_payload, double servoj_lookahead_time, double servoj_gain)
REVERSE_PORT_(reverse_port), maximum_time_step_(max_time_step), minimum_payload_( : REVERSE_PORT_(reverse_port)
min_payload), maximum_payload_(max_payload), servoj_time_( , maximum_time_step_(max_time_step)
servoj_time), servoj_lookahead_time_(servoj_lookahead_time), servoj_gain_(servoj_gain) { , 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]; char buffer[256];
struct sockaddr_in serv_addr; struct sockaddr_in serv_addr;
int n, flag; int n, flag;
@@ -52,7 +59,8 @@ UrDriver::UrDriver(std::condition_variable& rt_msg_cond,
sizeof(int)); sizeof(int));
setsockopt(incoming_sockfd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int)); setsockopt(incoming_sockfd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int));
if (bind(incoming_sockfd_, (struct sockaddr*)&serv_addr, if (bind(incoming_sockfd_, (struct sockaddr*)&serv_addr,
sizeof(serv_addr)) < 0) { sizeof(serv_addr))
< 0) {
print_fatal("ERROR on binding socket for reverse communication"); print_fatal("ERROR on binding socket for reverse communication");
} }
listen(incoming_sockfd_, 5); listen(incoming_sockfd_, 5);
@@ -60,16 +68,19 @@ UrDriver::UrDriver(std::condition_variable& rt_msg_cond,
std::vector<double> UrDriver::interp_cubic(double t, double T, std::vector<double> UrDriver::interp_cubic(double t, double T,
std::vector<double> p0_pos, std::vector<double> p1_pos, std::vector<double> p0_pos, std::vector<double> p1_pos,
std::vector<double> p0_vel, std::vector<double> p1_vel) { std::vector<double> p0_vel, std::vector<double> p1_vel)
{
/*Returns positions of the joints at time 't' */ /*Returns positions of the joints at time 't' */
std::vector<double> positions; std::vector<double> positions;
for (unsigned int i = 0; i < p0_pos.size(); i++) { for (unsigned int i = 0; i < p0_pos.size(); i++) {
double a = p0_pos[i]; double a = p0_pos[i];
double b = p0_vel[i]; double b = p0_vel[i];
double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i] double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i]
- T * p1_vel[i]) / pow(T, 2); - T * p1_vel[i])
/ pow(T, 2);
double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i] double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i]
+ T * p1_vel[i]) / pow(T, 3); + T * p1_vel[i])
/ pow(T, 3);
positions.push_back(a + b * t + c * pow(t, 2) + d * pow(t, 3)); positions.push_back(a + b * t + c * pow(t, 2) + d * pow(t, 3));
} }
return positions; return positions;
@@ -77,7 +88,8 @@ std::vector<double> UrDriver::interp_cubic(double t, double T,
bool UrDriver::doTraj(std::vector<double> inp_timestamps, bool UrDriver::doTraj(std::vector<double> inp_timestamps,
std::vector<std::vector<double> > inp_positions, std::vector<std::vector<double> > inp_positions,
std::vector<std::vector<double> > inp_velocities) { std::vector<std::vector<double> > inp_velocities)
{
std::chrono::high_resolution_clock::time_point t0, t; std::chrono::high_resolution_clock::time_point t0, t;
std::vector<double> positions; std::vector<double> positions;
unsigned int j; unsigned int j;
@@ -94,12 +106,16 @@ bool UrDriver::doTraj(std::vector<double> inp_timestamps,
and executing_traj_) { and executing_traj_) {
while (inp_timestamps[j] while (inp_timestamps[j]
<= std::chrono::duration_cast<std::chrono::duration<double> >( <= std::chrono::duration_cast<std::chrono::duration<double> >(
t - t0).count() && j < inp_timestamps.size() - 1) { t - t0)
.count()
&& j < inp_timestamps.size() - 1) {
j += 1; j += 1;
} }
positions = UrDriver::interp_cubic( positions = UrDriver::interp_cubic(
std::chrono::duration_cast<std::chrono::duration<double> >( std::chrono::duration_cast<std::chrono::duration<double> >(
t - t0).count() - inp_timestamps[j - 1], t - t0)
.count()
- inp_timestamps[j - 1],
inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1], inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1],
inp_positions[j], inp_velocities[j - 1], inp_velocities[j]); inp_positions[j], inp_velocities[j - 1], inp_velocities[j]);
UrDriver::servoj(positions); UrDriver::servoj(positions);
@@ -115,7 +131,8 @@ bool UrDriver::doTraj(std::vector<double> inp_timestamps,
return true; return true;
} }
void UrDriver::servoj(std::vector<double> positions, int keepalive) { void UrDriver::servoj(std::vector<double> positions, int keepalive)
{
if (!reverse_connected_) { if (!reverse_connected_) {
print_error( print_error(
"UrDriver::servoj called without a reverse connection present. Keepalive: " "UrDriver::servoj called without a reverse connection present. Keepalive: "
@@ -140,12 +157,14 @@ void UrDriver::servoj(std::vector<double> positions, int keepalive) {
bytes_written = write(new_sockfd_, buf, 28); bytes_written = write(new_sockfd_, buf, 28);
} }
void UrDriver::stopTraj() { void UrDriver::stopTraj()
{
executing_traj_ = false; executing_traj_ = false;
rt_interface_->addCommandToQueue("stopj(10)\n"); rt_interface_->addCommandToQueue("stopj(10)\n");
} }
bool UrDriver::uploadProg() { bool UrDriver::uploadProg()
{
std::string cmd_str; std::string cmd_str;
char buf[128]; char buf[128];
cmd_str = "def driverProg():\n"; cmd_str = "def driverProg():\n";
@@ -222,7 +241,8 @@ bool UrDriver::uploadProg() {
return UrDriver::openServo(); return UrDriver::openServo();
} }
bool UrDriver::openServo() { bool UrDriver::openServo()
{
struct sockaddr_in cli_addr; struct sockaddr_in cli_addr;
socklen_t clilen; socklen_t clilen;
clilen = sizeof(cli_addr); clilen = sizeof(cli_addr);
@@ -235,7 +255,8 @@ bool UrDriver::openServo() {
reverse_connected_ = true; reverse_connected_ = true;
return true; return true;
} }
void UrDriver::closeServo(std::vector<double> positions) { void UrDriver::closeServo(std::vector<double> positions)
{
if (positions.size() != 6) if (positions.size() != 6)
UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0); UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0);
else else
@@ -245,7 +266,8 @@ void UrDriver::closeServo(std::vector<double> positions) {
close(new_sockfd_); close(new_sockfd_);
} }
bool UrDriver::start() { bool UrDriver::start()
{
if (!sec_interface_->start()) if (!sec_interface_->start())
return false; return false;
firmware_version_ = sec_interface_->robot_state_->getVersion(); firmware_version_ = sec_interface_->robot_state_->getVersion();
@@ -257,10 +279,10 @@ bool UrDriver::start() {
"Listening on " + ip_addr_ + ":" + std::to_string(REVERSE_PORT_) "Listening on " + ip_addr_ + ":" + std::to_string(REVERSE_PORT_)
+ "\n"); + "\n");
return true; return true;
} }
void UrDriver::halt() { void UrDriver::halt()
{
if (executing_traj_) { if (executing_traj_) {
UrDriver::stopTraj(); UrDriver::stopTraj();
} }
@@ -270,32 +292,38 @@ void UrDriver::halt() {
} }
void UrDriver::setSpeed(double q0, double q1, double q2, double q3, double q4, void UrDriver::setSpeed(double q0, double q1, double q2, double q3, double q4,
double q5, double acc) { double q5, double acc)
{
rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc); rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc);
} }
std::vector<std::string> UrDriver::getJointNames() { std::vector<std::string> UrDriver::getJointNames()
{
return joint_names_; return joint_names_;
} }
void UrDriver::setJointNames(std::vector<std::string> jn) { void UrDriver::setJointNames(std::vector<std::string> jn)
{
joint_names_ = jn; joint_names_ = jn;
} }
void UrDriver::setToolVoltage(unsigned int v) { void UrDriver::setToolVoltage(unsigned int v)
{
char buf[256]; char buf[256];
sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v); sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v);
rt_interface_->addCommandToQueue(buf); rt_interface_->addCommandToQueue(buf);
print_debug(buf); print_debug(buf);
} }
void UrDriver::setFlag(unsigned int n, bool b) { void UrDriver::setFlag(unsigned int n, bool b)
{
char buf[256]; char buf[256];
sprintf(buf, "sec setOut():\n\tset_flag(%d, %s)\nend\n", n, sprintf(buf, "sec setOut():\n\tset_flag(%d, %s)\nend\n", n,
b ? "True" : "False"); b ? "True" : "False");
rt_interface_->addCommandToQueue(buf); rt_interface_->addCommandToQueue(buf);
print_debug(buf); print_debug(buf);
} }
void UrDriver::setDigitalOut(unsigned int n, bool b) { void UrDriver::setDigitalOut(unsigned int n, bool b)
{
char buf[256]; char buf[256];
if (firmware_version_ < 2) { if (firmware_version_ < 2) {
sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n, sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n,
@@ -311,13 +339,12 @@ void UrDriver::setDigitalOut(unsigned int n, bool b) {
} else { } else {
sprintf(buf, "sec setOut():\n\tset_standard_digital_out(%d, %s)\nend\n", sprintf(buf, "sec setOut():\n\tset_standard_digital_out(%d, %s)\nend\n",
n, b ? "True" : "False"); n, b ? "True" : "False");
} }
rt_interface_->addCommandToQueue(buf); rt_interface_->addCommandToQueue(buf);
print_debug(buf); print_debug(buf);
} }
void UrDriver::setAnalogOut(unsigned int n, double f) { void UrDriver::setAnalogOut(unsigned int n, double f)
{
char buf[256]; char buf[256];
if (firmware_version_ < 2) { if (firmware_version_ < 2) {
sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f); sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f);
@@ -329,7 +356,8 @@ void UrDriver::setAnalogOut(unsigned int n, double f) {
print_debug(buf); print_debug(buf);
} }
bool UrDriver::setPayload(double m) { bool UrDriver::setPayload(double m)
{
if ((m < maximum_payload_) && (m > minimum_payload_)) { if ((m < maximum_payload_) && (m > minimum_payload_)) {
char buf[256]; char buf[256];
sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m); sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m);
@@ -340,25 +368,28 @@ bool UrDriver::setPayload(double m) {
return false; return false;
} }
void UrDriver::setMinPayload(double m) { void UrDriver::setMinPayload(double m)
{
if (m > 0) { if (m > 0) {
minimum_payload_ = m; minimum_payload_ = m;
} else { } else {
minimum_payload_ = 0; minimum_payload_ = 0;
} }
} }
void UrDriver::setMaxPayload(double m) { void UrDriver::setMaxPayload(double m)
{
maximum_payload_ = m; maximum_payload_ = m;
} }
void UrDriver::setServojTime(double t) { void UrDriver::setServojTime(double t)
{
if (t > 0.008) { if (t > 0.008) {
servoj_time_ = t; servoj_time_ = t;
} else { } else {
servoj_time_ = 0.008; servoj_time_ = 0.008;
} }
} }
void UrDriver::setServojLookahead(double t){ void UrDriver::setServojLookahead(double t)
{
if (t > 0.03) { if (t > 0.03) {
if (t < 0.2) { if (t < 0.2) {
servoj_lookahead_time_ = t; servoj_lookahead_time_ = t;
@@ -369,7 +400,8 @@ void UrDriver::setServojLookahead(double t){
servoj_lookahead_time_ = 0.03; servoj_lookahead_time_ = 0.03;
} }
} }
void UrDriver::setServojGain(double g){ void UrDriver::setServojGain(double g)
{
if (g > 100) { if (g > 100) {
if (g < 2000) { if (g < 2000) {
servoj_gain_ = g; servoj_gain_ = g;

45
src/ur_hardware_interface.cpp
View File

@@ -59,8 +59,10 @@
namespace ros_control_ur { namespace ros_control_ur {
UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) : UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot)
nh_(nh), robot_(robot) { : nh_(nh)
, robot_(robot)
{
// Initialize shared memory and interfaces here // Initialize shared memory and interfaces here
init(); // this implementation loads from rosparam init(); // this implementation loads from rosparam
@@ -69,7 +71,8 @@ UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) :
ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface."); ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface.");
} }
void UrHardwareInterface::init() { void UrHardwareInterface::init()
{
ROS_INFO_STREAM_NAMED("ur_hardware_interface", ROS_INFO_STREAM_NAMED("ur_hardware_interface",
"Reading rosparams from namespace: " << nh_.getNamespace()); "Reading rosparams from namespace: " << nh_.getNamespace());
@@ -77,7 +80,8 @@ void UrHardwareInterface::init() {
nh_.getParam("hardware_interface/joints", joint_names_); nh_.getParam("hardware_interface/joints", joint_names_);
if (joint_names_.size() == 0) { if (joint_names_.size() == 0) {
ROS_FATAL_STREAM_NAMED("ur_hardware_interface", 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()); "No joints found on parameter server for controller, did you load the proper yaml file?"
<< " Namespace: " << nh_.getNamespace());
exit(-1); exit(-1);
} }
num_joints_ = joint_names_.size(); num_joints_ = joint_names_.size();
@@ -128,7 +132,8 @@ void UrHardwareInterface::init() {
position_interface_running_ = false; position_interface_running_ = false;
} }
void UrHardwareInterface::read() { void UrHardwareInterface::read()
{
std::vector<double> pos, vel, current, tcp; std::vector<double> pos, vel, current, tcp;
pos = robot_->rt_interface_->robot_state_->getQActual(); pos = robot_->rt_interface_->robot_state_->getQActual();
vel = robot_->rt_interface_->robot_state_->getQdActual(); vel = robot_->rt_interface_->robot_state_->getQdActual();
@@ -143,14 +148,15 @@ void UrHardwareInterface::read() {
robot_force_[i] = tcp[i]; robot_force_[i] = tcp[i];
robot_torque_[i] = tcp[i + 3]; robot_torque_[i] = tcp[i + 3];
} }
} }
void UrHardwareInterface::setMaxVelChange(double inp) { void UrHardwareInterface::setMaxVelChange(double inp)
{
max_vel_change_ = inp; max_vel_change_ = inp;
} }
void UrHardwareInterface::write() { void UrHardwareInterface::write()
{
if (velocity_interface_running_) { if (velocity_interface_running_) {
std::vector<double> cmd; std::vector<double> cmd;
//do some rate limiting //do some rate limiting
@@ -173,9 +179,9 @@ void UrHardwareInterface::write() {
bool UrHardwareInterface::canSwitch( bool UrHardwareInterface::canSwitch(
const std::list<hardware_interface::ControllerInfo>& start_list, const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo> &stop_list) const { const std::list<hardware_interface::ControllerInfo>& stop_list) const
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = {
start_list.begin(); controller_it != start_list.end(); for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin(); controller_it != start_list.end();
++controller_it) { ++controller_it) {
if (controller_it->hardware_interface if (controller_it->hardware_interface
== "hardware_interface::VelocityJointInterface") { == "hardware_interface::VelocityJointInterface") {
@@ -188,8 +194,7 @@ bool UrHardwareInterface::canSwitch(
} }
if (position_interface_running_) { if (position_interface_running_) {
bool error = true; bool error = true;
for (std::list<hardware_interface::ControllerInfo>::const_iterator stop_controller_it = for (std::list<hardware_interface::ControllerInfo>::const_iterator stop_controller_it = stop_list.begin();
stop_list.begin();
stop_controller_it != stop_list.end(); stop_controller_it != stop_list.end();
++stop_controller_it) { ++stop_controller_it) {
if (stop_controller_it->hardware_interface if (stop_controller_it->hardware_interface
@@ -217,8 +222,7 @@ bool UrHardwareInterface::canSwitch(
} }
if (velocity_interface_running_) { if (velocity_interface_running_) {
bool error = true; bool error = true;
for (std::list<hardware_interface::ControllerInfo>::const_iterator stop_controller_it = for (std::list<hardware_interface::ControllerInfo>::const_iterator stop_controller_it = stop_list.begin();
stop_list.begin();
stop_controller_it != stop_list.end(); stop_controller_it != stop_list.end();
++stop_controller_it) { ++stop_controller_it) {
if (stop_controller_it->hardware_interface if (stop_controller_it->hardware_interface
@@ -244,9 +248,9 @@ bool UrHardwareInterface::canSwitch(
void UrHardwareInterface::doSwitch( void UrHardwareInterface::doSwitch(
const std::list<hardware_interface::ControllerInfo>& start_list, const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list) { const std::list<hardware_interface::ControllerInfo>& stop_list)
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = {
stop_list.begin(); controller_it != stop_list.end(); for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = stop_list.begin(); controller_it != stop_list.end();
++controller_it) { ++controller_it) {
if (controller_it->hardware_interface if (controller_it->hardware_interface
== "hardware_interface::VelocityJointInterface") { == "hardware_interface::VelocityJointInterface") {
@@ -261,8 +265,7 @@ void UrHardwareInterface::doSwitch(
ROS_DEBUG("Stopping position interface"); ROS_DEBUG("Stopping position interface");
} }
} }
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin(); controller_it != start_list.end();
start_list.begin(); controller_it != start_list.end();
++controller_it) { ++controller_it) {
if (controller_it->hardware_interface if (controller_it->hardware_interface
== "hardware_interface::VelocityJointInterface") { == "hardware_interface::VelocityJointInterface") {
@@ -276,8 +279,6 @@ void UrHardwareInterface::doSwitch(
ROS_DEBUG("Starting position interface"); ROS_DEBUG("Starting position interface");
} }
} }
} }
} // namespace } // namespace

27
src/ur_realtime_communication.cpp
View File

@@ -20,7 +20,8 @@
UrRealtimeCommunication::UrRealtimeCommunication( UrRealtimeCommunication::UrRealtimeCommunication(
std::condition_variable& msg_cond, std::string host, std::condition_variable& msg_cond, std::string host,
unsigned int safety_count_max) { unsigned int safety_count_max)
{
robot_state_ = new RobotStateRT(msg_cond); robot_state_ = new RobotStateRT(msg_cond);
bzero((char*)&serv_addr_, sizeof(serv_addr_)); bzero((char*)&serv_addr_, sizeof(serv_addr_));
sockfd_ = socket(AF_INET, SOCK_STREAM, 0); sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
@@ -45,7 +46,8 @@ UrRealtimeCommunication::UrRealtimeCommunication(
safety_count_max_ = safety_count_max; safety_count_max_ = safety_count_max;
} }
bool UrRealtimeCommunication::start() { bool UrRealtimeCommunication::start()
{
fd_set writefds; fd_set writefds;
struct timeval timeout; struct timeval timeout;
@@ -79,12 +81,14 @@ bool UrRealtimeCommunication::start() {
return true; return true;
} }
void UrRealtimeCommunication::halt() { void UrRealtimeCommunication::halt()
{
keepalive_ = false; keepalive_ = false;
comThread_.join(); comThread_.join();
} }
void UrRealtimeCommunication::addCommandToQueue(std::string inp) { void UrRealtimeCommunication::addCommandToQueue(std::string inp)
{
int bytes_written; int bytes_written;
if (inp.back() != '\n') { if (inp.back() != '\n') {
inp.append("\n"); inp.append("\n");
@@ -96,14 +100,14 @@ void UrRealtimeCommunication::addCommandToQueue(std::string inp) {
} }
void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2, void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2,
double q3, double q4, double q5, double acc) { double q3, double q4, double q5, double acc)
{
char cmd[1024]; char cmd[1024];
if (robot_state_->getVersion() >= 3.1) { if (robot_state_->getVersion() >= 3.1) {
sprintf(cmd, sprintf(cmd,
"speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n", "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n",
q0, q1, q2, q3, q4, q5, acc); q0, q1, q2, q3, q4, q5, acc);
} } else {
else {
sprintf(cmd, sprintf(cmd,
"speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n", "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n",
q0, q1, q2, q3, q4, q5, acc); q0, q1, q2, q3, q4, q5, acc);
@@ -115,7 +119,8 @@ void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2,
} }
} }
void UrRealtimeCommunication::run() { void UrRealtimeCommunication::run()
{
uint8_t buf[2048]; uint8_t buf[2048];
int bytes_read; int bytes_read;
bzero(buf, 2048); bzero(buf, 2048);
@@ -184,10 +189,12 @@ void UrRealtimeCommunication::run() {
close(sockfd_); close(sockfd_);
} }
void UrRealtimeCommunication::setSafetyCountMax(uint inp) { void UrRealtimeCommunication::setSafetyCountMax(uint inp)
{
safety_count_max_ = inp; safety_count_max_ = inp;
} }
std::string UrRealtimeCommunication::getLocalIp() { std::string UrRealtimeCommunication::getLocalIp()
{
return local_ip_; return local_ip_;
} }

196
src/ur_ros_wrapper.cpp
View File

@@ -16,40 +16,40 @@
* limitations under the License. * limitations under the License.
*/ */
#include "ur_modern_driver/do_output.h"
#include "ur_modern_driver/ur_driver.h" #include "ur_modern_driver/ur_driver.h"
#include "ur_modern_driver/ur_hardware_interface.h" #include "ur_modern_driver/ur_hardware_interface.h"
#include "ur_modern_driver/do_output.h"
#include <string.h>
#include <vector>
#include <mutex>
#include <condition_variable>
#include <thread>
#include <algorithm> #include <algorithm>
#include <cmath>
#include <chrono> #include <chrono>
#include <cmath>
#include <condition_variable>
#include <mutex>
#include <string.h>
#include <thread>
#include <time.h> #include <time.h>
#include <vector>
#include "ros/ros.h"
#include <ros/console.h>
#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/action_server.h"
#include "actionlib/server/server_goal_handle.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 "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/SetIO.h"
#include "ur_msgs/SetIORequest.h"
#include "ur_msgs/SetIOResponse.h"
#include "ur_msgs/SetPayload.h" #include "ur_msgs/SetPayload.h"
#include "ur_msgs/SetPayloadRequest.h" #include "ur_msgs/SetPayloadRequest.h"
#include "ur_msgs/SetPayloadResponse.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 <controller_manager/controller_manager.h> #include <controller_manager/controller_manager.h>
#include <realtime_tools/realtime_publisher.h> #include <realtime_tools/realtime_publisher.h>
#include <ros/console.h>
/// TF /// TF
#include <tf/tf.h> #include <tf/tf.h>
@@ -83,12 +83,16 @@ protected:
boost::shared_ptr<controller_manager::ControllerManager> controller_manager_; boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;
public: public:
RosWrapper(std::string host, int reverse_port) : RosWrapper(std::string host, int reverse_port)
as_(nh_, "follow_joint_trajectory", : as_(nh_, "follow_joint_trajectory",
boost::bind(&RosWrapper::goalCB, this, _1), boost::bind(&RosWrapper::goalCB, this, _1),
boost::bind(&RosWrapper::cancelCB, this, _1), false), robot_( boost::bind(&RosWrapper::cancelCB, this, _1), false)
rt_msg_cond_, msg_cond_, host, reverse_port, 0.03, 300), io_flag_delay_(0.05), joint_offsets_( , robot_(
6, 0.0) { 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::string joint_prefix = "";
std::vector<std::string> joint_names; std::vector<std::string> joint_names;
@@ -216,15 +220,17 @@ public:
} }
} }
void halt() { void halt()
{
robot_.halt(); robot_.halt();
rt_publish_thread_->join(); rt_publish_thread_->join();
} }
private: private:
void trajThread(std::vector<double> timestamps, void trajThread(std::vector<double> timestamps,
std::vector<std::vector<double> > positions, std::vector<std::vector<double> > positions,
std::vector<std::vector<double> > velocities) { std::vector<std::vector<double> > velocities)
{
robot_.doTraj(timestamps, positions, velocities); robot_.doTraj(timestamps, positions, velocities);
if (has_goal_) { if (has_goal_) {
@@ -234,29 +240,26 @@ private:
} }
} }
void goalCB( void goalCB(
actionlib::ServerGoalHandle< actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> gh)
control_msgs::FollowJointTrajectoryAction> gh) { {
std::string buf; std::string buf;
print_info("on_goal"); print_info("on_goal");
if (!robot_.sec_interface_->robot_state_->isReady()) { if (!robot_.sec_interface_->robot_state_->isReady()) {
result_.error_code = -100; //nothing is defined for this...? result_.error_code = -100; //nothing is defined for this...?
if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) { if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) {
result_.error_string = result_.error_string = "Cannot accept new trajectories: Robot arm is not powered on";
"Cannot accept new trajectories: Robot arm is not powered on";
gh.setRejected(result_, result_.error_string); gh.setRejected(result_, result_.error_string);
print_error(result_.error_string); print_error(result_.error_string);
return; return;
} }
if (!robot_.sec_interface_->robot_state_->isRealRobotEnabled()) { if (!robot_.sec_interface_->robot_state_->isRealRobotEnabled()) {
result_.error_string = result_.error_string = "Cannot accept new trajectories: Robot is not enabled";
"Cannot accept new trajectories: Robot is not enabled";
gh.setRejected(result_, result_.error_string); gh.setRejected(result_, result_.error_string);
print_error(result_.error_string); print_error(result_.error_string);
return; return;
} }
result_.error_string = result_.error_string = "Cannot accept new trajectories. (Debug: Robot mode is "
"Cannot accept new trajectories. (Debug: Robot mode is "
+ std::to_string( + std::to_string(
robot_.sec_interface_->robot_state_->getRobotMode()) robot_.sec_interface_->robot_state_->getRobotMode())
+ ")"; + ")";
@@ -266,23 +269,20 @@ private:
} }
if (robot_.sec_interface_->robot_state_->isEmergencyStopped()) { if (robot_.sec_interface_->robot_state_->isEmergencyStopped()) {
result_.error_code = -100; //nothing is defined for this...? result_.error_code = -100; //nothing is defined for this...?
result_.error_string = result_.error_string = "Cannot accept new trajectories: Robot is emergency stopped";
"Cannot accept new trajectories: Robot is emergency stopped";
gh.setRejected(result_, result_.error_string); gh.setRejected(result_, result_.error_string);
print_error(result_.error_string); print_error(result_.error_string);
return; return;
} }
if (robot_.sec_interface_->robot_state_->isProtectiveStopped()) { if (robot_.sec_interface_->robot_state_->isProtectiveStopped()) {
result_.error_code = -100; //nothing is defined for this...? result_.error_code = -100; //nothing is defined for this...?
result_.error_string = result_.error_string = "Cannot accept new trajectories: Robot is protective stopped";
"Cannot accept new trajectories: Robot is protective stopped";
gh.setRejected(result_, result_.error_string); gh.setRejected(result_, result_.error_string);
print_error(result_.error_string); print_error(result_.error_string);
return; return;
} }
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *gh.getGoal(); //make a copy that we can modify
*gh.getGoal(); //make a copy that we can modify
if (has_goal_) { if (has_goal_) {
print_warning( print_warning(
"Received new goal while still executing previous trajectory. Canceling previous trajectory"); "Received new goal while still executing previous trajectory. Canceling previous trajectory");
@@ -301,8 +301,7 @@ private:
outp_joint_names += goal.trajectory.joint_names[i] + " "; outp_joint_names += goal.trajectory.joint_names[i] + " ";
} }
result_.error_code = result_.INVALID_JOINTS; result_.error_code = result_.INVALID_JOINTS;
result_.error_string = result_.error_string = "Received a goal with incorrect joint names: "
"Received a goal with incorrect joint names: "
+ outp_joint_names; + outp_joint_names;
gh.setRejected(result_, result_.error_string); gh.setRejected(result_, result_.error_string);
print_error(result_.error_string); print_error(result_.error_string);
@@ -334,8 +333,7 @@ private:
if (!has_limited_velocities()) { if (!has_limited_velocities()) {
result_.error_code = result_.INVALID_GOAL; result_.error_code = result_.INVALID_GOAL;
result_.error_string = result_.error_string = "Received a goal with velocities that are higher than "
"Received a goal with velocities that are higher than "
+ std::to_string(max_velocity_); + std::to_string(max_velocity_);
gh.setRejected(result_, result_.error_string); gh.setRejected(result_, result_.error_string);
print_error(result_.error_string); print_error(result_.error_string);
@@ -368,18 +366,18 @@ private:
goal.trajectory.points[i].time_from_start.toSec()); goal.trajectory.points[i].time_from_start.toSec());
positions.push_back(goal.trajectory.points[i].positions); positions.push_back(goal.trajectory.points[i].positions);
velocities.push_back(goal.trajectory.points[i].velocities); velocities.push_back(goal.trajectory.points[i].velocities);
} }
goal_handle_.setAccepted(); goal_handle_.setAccepted();
has_goal_ = true; has_goal_ = true;
std::thread(&RosWrapper::trajThread, this, timestamps, positions, std::thread(&RosWrapper::trajThread, this, timestamps, positions,
velocities).detach(); velocities)
.detach();
} }
void cancelCB( void cancelCB(
actionlib::ServerGoalHandle< actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> gh)
control_msgs::FollowJointTrajectoryAction> gh) { {
// set the action state to preempted // set the action state to preempted
print_info("on_cancel"); print_info("on_cancel");
if (has_goal_) { if (has_goal_) {
@@ -393,7 +391,8 @@ private:
gh.setCanceled(result_); gh.setCanceled(result_);
} }
bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp) { bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp)
{
resp.success = true; resp.success = true;
//if (req.fun == ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT) { //if (req.fun == ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT) {
if (req.fun == 1) { if (req.fun == 1) {
@@ -416,7 +415,8 @@ private:
} }
bool setPayload(ur_msgs::SetPayloadRequest& req, bool setPayload(ur_msgs::SetPayloadRequest& req,
ur_msgs::SetPayloadResponse& resp) { ur_msgs::SetPayloadResponse& resp)
{
if (robot_.setPayload(req.payload)) if (robot_.setPayload(req.payload))
resp.success = true; resp.success = true;
else else
@@ -424,10 +424,10 @@ private:
return resp.success; return resp.success;
} }
bool validateJointNames() { bool validateJointNames()
{
std::vector<std::string> actual_joint_names = robot_.getJointNames(); std::vector<std::string> actual_joint_names = robot_.getJointNames();
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_.getGoal();
*goal_handle_.getGoal();
if (goal.trajectory.joint_names.size() != actual_joint_names.size()) if (goal.trajectory.joint_names.size() != actual_joint_names.size())
return false; return false;
@@ -447,7 +447,8 @@ private:
return true; return true;
} }
void reorder_traj_joints(trajectory_msgs::JointTrajectory& traj) { void reorder_traj_joints(trajectory_msgs::JointTrajectory& traj)
{
/* Reorders trajectory - destructive */ /* Reorders trajectory - destructive */
std::vector<std::string> actual_joint_names = robot_.getJointNames(); std::vector<std::string> actual_joint_names = robot_.getJointNames();
std::vector<unsigned int> mapping; std::vector<unsigned int> mapping;
@@ -477,9 +478,9 @@ private:
traj.points = new_traj; traj.points = new_traj;
} }
bool has_velocities() { bool has_velocities()
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = {
*goal_handle_.getGoal(); actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_.getGoal();
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
if (goal.trajectory.points[i].positions.size() if (goal.trajectory.points[i].positions.size()
!= goal.trajectory.points[i].velocities.size()) != goal.trajectory.points[i].velocities.size())
@@ -488,9 +489,9 @@ private:
return true; return true;
} }
bool has_positions() { bool has_positions()
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = {
*goal_handle_.getGoal(); actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_.getGoal();
if (goal.trajectory.points.size() == 0) if (goal.trajectory.points.size() == 0)
return false; return false;
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
@@ -503,20 +504,18 @@ private:
bool start_positions_match(const trajectory_msgs::JointTrajectory& traj, double eps) bool start_positions_match(const trajectory_msgs::JointTrajectory& traj, double eps)
{ {
for (unsigned int i = 0; i < traj.points[0].positions.size(); i++) for (unsigned int i = 0; i < traj.points[0].positions.size(); i++) {
{
std::vector<double> qActual = robot_.rt_interface_->robot_state_->getQActual(); std::vector<double> qActual = robot_.rt_interface_->robot_state_->getQActual();
if( fabs(traj.points[0].positions[i] - qActual[i]) > eps ) if (fabs(traj.points[0].positions[i] - qActual[i]) > eps) {
{
return false; return false;
} }
} }
return true; return true;
} }
bool has_limited_velocities() { bool has_limited_velocities()
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = {
*goal_handle_.getGoal(); actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_.getGoal();
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
for (unsigned int j = 0; for (unsigned int j = 0;
j < goal.trajectory.points[i].velocities.size(); j++) { j < goal.trajectory.points[i].velocities.size(); j++) {
@@ -528,9 +527,9 @@ private:
return true; return true;
} }
bool traj_is_finite() { bool traj_is_finite()
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = {
*goal_handle_.getGoal(); actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_.getGoal();
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
for (unsigned int j = 0; for (unsigned int j = 0;
j < goal.trajectory.points[i].velocities.size(); j++) { j < goal.trajectory.points[i].velocities.size(); j++) {
@@ -543,7 +542,8 @@ private:
return true; return true;
} }
void speedInterface(const trajectory_msgs::JointTrajectory::Ptr& msg) { void speedInterface(const trajectory_msgs::JointTrajectory::Ptr& msg)
{
if (msg->points[0].velocities.size() == 6) { if (msg->points[0].velocities.size() == 6) {
double acc = 100; double acc = 100;
if (msg->points[0].accelerations.size() > 0) if (msg->points[0].accelerations.size() > 0)
@@ -554,15 +554,15 @@ private:
msg->points[0].velocities[3], msg->points[0].velocities[4], msg->points[0].velocities[3], msg->points[0].velocities[4],
msg->points[0].velocities[5], acc); msg->points[0].velocities[5], acc);
} }
} }
void urscriptInterface(const std_msgs::String::ConstPtr& msg) { void urscriptInterface(const std_msgs::String::ConstPtr& msg)
{
robot_.rt_interface_->addCommandToQueue(msg->data); robot_.rt_interface_->addCommandToQueue(msg->data);
} }
void rosControlLoop() { void rosControlLoop()
{
ros::Duration elapsed_time; ros::Duration elapsed_time;
struct timespec last_time, current_time; struct timespec last_time, current_time;
static const double BILLION = 1000000000.0; static const double BILLION = 1000000000.0;
@@ -576,7 +576,6 @@ private:
realtime_tools::RealtimePublisher<geometry_msgs::TwistStamped> tool_vel_pub(nh_, "tool_velocity", 1); realtime_tools::RealtimePublisher<geometry_msgs::TwistStamped> tool_vel_pub(nh_, "tool_velocity", 1);
tool_vel_pub.msg_.header.frame_id = base_frame_; tool_vel_pub.msg_.header.frame_id = base_frame_;
clock_gettime(CLOCK_MONOTONIC, &last_time); clock_gettime(CLOCK_MONOTONIC, &last_time);
while (ros::ok()) { while (ros::ok()) {
std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex
@@ -608,8 +607,7 @@ private:
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));
// Broadcast transform // Broadcast transform
if( tf_pub.trylock() ) if (tf_pub.trylock()) {
{
tf_pub.msg_.transforms[0].header.stamp = ros_time; tf_pub.msg_.transforms[0].header.stamp = ros_time;
if (angle < 1e-16) { if (angle < 1e-16) {
tf_pub.msg_.transforms[0].transform.rotation.x = 0; tf_pub.msg_.transforms[0].transform.rotation.x = 0;
@@ -632,8 +630,7 @@ private:
//Publish tool velocity //Publish tool velocity
std::vector<double> tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual(); std::vector<double> tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual();
if( tool_vel_pub.trylock() ) if (tool_vel_pub.trylock()) {
{
tool_vel_pub.msg_.header.stamp = ros_time; tool_vel_pub.msg_.header.stamp = ros_time;
tool_vel_pub.msg_.twist.linear.x = tcp_speed[0]; 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.y = tcp_speed[1];
@@ -644,11 +641,11 @@ private:
tool_vel_pub.unlockAndPublish(); tool_vel_pub.unlockAndPublish();
} }
} }
} }
void publishRTMsg() { void publishRTMsg()
{
ros::Publisher joint_pub = nh_.advertise<sensor_msgs::JointState>( ros::Publisher joint_pub = nh_.advertise<sensor_msgs::JointState>(
"joint_states", 1); "joint_states", 1);
ros::Publisher wrench_pub = nh_.advertise<geometry_msgs::WrenchStamped>( ros::Publisher wrench_pub = nh_.advertise<geometry_msgs::WrenchStamped>(
@@ -666,17 +663,14 @@ private:
rt_msg_cond_.wait(locker); rt_msg_cond_.wait(locker);
} }
joint_msg.header.stamp = ros::Time::now(); joint_msg.header.stamp = ros::Time::now();
joint_msg.position = joint_msg.position = robot_.rt_interface_->robot_state_->getQActual();
robot_.rt_interface_->robot_state_->getQActual();
for (unsigned int i = 0; i < joint_msg.position.size(); i++) { for (unsigned int i = 0; i < joint_msg.position.size(); i++) {
joint_msg.position[i] += joint_offsets_[i]; joint_msg.position[i] += joint_offsets_[i];
} }
joint_msg.velocity = joint_msg.velocity = robot_.rt_interface_->robot_state_->getQdActual();
robot_.rt_interface_->robot_state_->getQdActual();
joint_msg.effort = robot_.rt_interface_->robot_state_->getIActual(); joint_msg.effort = robot_.rt_interface_->robot_state_->getIActual();
joint_pub.publish(joint_msg); joint_pub.publish(joint_msg);
std::vector<double> tcp_force = std::vector<double> tcp_force = robot_.rt_interface_->robot_state_->getTcpForce();
robot_.rt_interface_->robot_state_->getTcpForce();
wrench_msg.header.stamp = joint_msg.header.stamp; wrench_msg.header.stamp = joint_msg.header.stamp;
wrench_msg.wrench.force.x = tcp_force[0]; wrench_msg.wrench.force.x = tcp_force[0];
wrench_msg.wrench.force.y = tcp_force[1]; wrench_msg.wrench.force.y = tcp_force[1];
@@ -708,8 +702,7 @@ private:
br.sendTransform(tf::StampedTransform(transform, joint_msg.header.stamp, base_frame_, tool_frame_)); br.sendTransform(tf::StampedTransform(transform, joint_msg.header.stamp, base_frame_, tool_frame_));
//Publish tool velocity //Publish tool velocity
std::vector<double> tcp_speed = std::vector<double> tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual();
robot_.rt_interface_->robot_state_->getTcpSpeedActual();
geometry_msgs::TwistStamped tool_twist; geometry_msgs::TwistStamped tool_twist;
tool_twist.header.frame_id = base_frame_; tool_twist.header.frame_id = base_frame_;
tool_twist.header.stamp = joint_msg.header.stamp; tool_twist.header.stamp = joint_msg.header.stamp;
@@ -725,7 +718,8 @@ private:
} }
} }
void publishMbMsg() { void publishMbMsg()
{
bool warned = false; bool warned = false;
ros::Publisher io_pub = nh_.advertise<ur_msgs::IOStates>( ros::Publisher io_pub = nh_.advertise<ur_msgs::IOStates>(
"ur_driver/io_states", 1); "ur_driver/io_states", 1);
@@ -743,13 +737,13 @@ private:
for (unsigned int i = 0; i < i_max; i++) { for (unsigned int i = 0; i < i_max; i++) {
ur_msgs::Digital digi; ur_msgs::Digital digi;
digi.pin = i; digi.pin = i;
digi.state = digi.state = ((robot_.sec_interface_->robot_state_->getDigitalInputBits()
((robot_.sec_interface_->robot_state_->getDigitalInputBits() & (1 << i))
& (1 << i)) >> i); >> i);
io_msg.digital_in_states.push_back(digi); io_msg.digital_in_states.push_back(digi);
digi.state = digi.state = ((robot_.sec_interface_->robot_state_->getDigitalOutputBits()
((robot_.sec_interface_->robot_state_->getDigitalOutputBits() & (1 << i))
& (1 << i)) >> i); >> i);
io_msg.digital_out_states.push_back(digi); io_msg.digital_out_states.push_back(digi);
} }
ur_msgs::Analog ana; ur_msgs::Analog ana;
@@ -790,13 +784,12 @@ private:
warned = false; warned = false;
robot_.sec_interface_->robot_state_->finishedReading(); robot_.sec_interface_->robot_state_->finishedReading();
} }
} }
}; };
int main(int argc, char **argv) { int main(int argc, char** argv)
{
bool use_sim_time = false; bool use_sim_time = false;
std::string host; std::string host;
int reverse_port = 50001; int reverse_port = 50001;
@@ -816,7 +809,6 @@ int main(int argc, char **argv) {
"Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip"); "Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip");
exit(1); exit(1);
} }
} }
if ((ros::param::get("~reverse_port", reverse_port))) { if ((ros::param::get("~reverse_port", reverse_port))) {
if ((reverse_port <= 0) or (reverse_port >= 65535)) { if ((reverse_port <= 0) or (reverse_port >= 65535)) {