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

Adopted roscpp code style and naming convention

Browse Source
This commit is contained in:
Simon Rasmussen
2017-03-01 13:59:48 +01:00
parent e4bc40fc09
commit 474f469e97
44 changed files with 5097 additions and 4891 deletions
Download Patch File Download Diff File Expand all files Collapse all files

119
.clang-format
View File

@@ -1,95 +1,48 @@
---
Language: Cpp
# BasedOnStyle: WebKit
AccessModifierOffset: -4
AlignAfterOpenBracket: DontAlign
AlignConsecutiveAssignments: false
AlignConsecutiveDeclarations: false
BasedOnStyle: Google
AccessModifierOffset: -2
ConstructorInitializerIndentWidth: 2
AlignEscapedNewlinesLeft: false
AlignOperands: false
AlignTrailingComments: false
AllowAllParametersOfDeclarationOnNextLine: true
AllowShortBlocksOnASingleLine: false
AllowShortCaseLabelsOnASingleLine: false
AllowShortFunctionsOnASingleLine: All
AlignTrailingComments: true
AllowAllParametersOfDeclarationOnNextLine: false
AllowShortIfStatementsOnASingleLine: false
AllowShortLoopsOnASingleLine: false
AlwaysBreakAfterDefinitionReturnType: None
AlwaysBreakAfterReturnType: None
AllowShortFunctionsOnASingleLine: None
AllowShortLoopsOnASingleLine: false
AlwaysBreakTemplateDeclarations: true
AlwaysBreakBeforeMultilineStrings: false
AlwaysBreakTemplateDeclarations: false
BinPackArguments: true
BinPackParameters: true
BraceWrapping:
AfterClass: false
AfterControlStatement: false
AfterEnum: false
AfterFunction: true
AfterNamespace: false
AfterObjCDeclaration: false
AfterStruct: false
AfterUnion: false
BeforeCatch: false
BeforeElse: false
IndentBraces: false
BreakBeforeBinaryOperators: All
BreakBeforeBraces: WebKit
BreakBeforeTernaryOperators: true
BreakBeforeBinaryOperators: false
BreakBeforeTernaryOperators: false
BreakConstructorInitializersBeforeComma: true
BreakAfterJavaFieldAnnotations: false
BreakStringLiterals: true
ColumnLimit: 0
CommentPragmas: '^ IWYU pragma:'
ConstructorInitializerAllOnOneLineOrOnePerLine: false
ConstructorInitializerIndentWidth: 4
ContinuationIndentWidth: 4
Cpp11BracedListStyle: false
DerivePointerAlignment: false
DisableFormat: false
BinPackParameters: true
ColumnLimit: 120
ConstructorInitializerAllOnOneLineOrOnePerLine: true
DerivePointerBinding: true
ExperimentalAutoDetectBinPacking: false
ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ]
IncludeCategories:
- Regex: '^"(llvm|llvm-c|clang|clang-c)/'
Priority: 2
- Regex: '^(<|"(gtest|isl|json)/)'
Priority: 3
- Regex: '.*'
Priority: 1
IncludeIsMainRegex: '$'
IndentCaseLabels: false
IndentWidth: 4
IndentWrappedFunctionNames: false
JavaScriptQuotes: Leave
JavaScriptWrapImports: true
KeepEmptyLinesAtTheStartOfBlocks: true
MacroBlockBegin: ''
MacroBlockEnd: ''
IndentCaseLabels: true
MaxEmptyLinesToKeep: 1
NamespaceIndentation: Inner
ObjCBlockIndentWidth: 4
ObjCSpaceAfterProperty: true
NamespaceIndentation: None
ObjCSpaceBeforeProtocolList: true
PenaltyBreakBeforeFirstCallParameter: 19
PenaltyBreakComment: 300
PenaltyBreakFirstLessLess: 120
PenaltyBreakString: 1000
PenaltyExcessCharacter: 1000000
PenaltyReturnTypeOnItsOwnLine: 60
PointerAlignment: Left
ReflowComments: true
SortIncludes: true
SpaceAfterCStyleCast: false
SpaceBeforeAssignmentOperators: true
SpaceBeforeParens: ControlStatements
SpaceInEmptyParentheses: false
SpacesBeforeTrailingComments: 1
SpacesInAngles: false
SpacesInContainerLiterals: true
SpacesInCStyleCastParentheses: false
SpacesInParentheses: false
SpacesInSquareBrackets: false
Standard: Cpp03
TabWidth: 8
PenaltyBreakComment: 60
PenaltyBreakString: 1
PenaltyBreakFirstLessLess: 1000
PenaltyExcessCharacter: 1000
PenaltyReturnTypeOnItsOwnLine: 90
PointerBindsToType: false
SpacesBeforeTrailingComments: 2
Cpp11BracedListStyle: false
Standard: Auto
IndentWidth: 2
TabWidth: 2
UseTab: Never
BreakBeforeBraces: Allman
IndentFunctionDeclarationAfterType: false
SpacesInParentheses: false
SpacesInAngles: false
SpaceInEmptyParentheses: false
SpacesInCStyleCastParentheses: false
SpaceAfterControlStatementKeyword: true
SpaceBeforeAssignmentOperators: true
ContinuationIndentWidth: 4
...

61
include/ur_modern_driver/bin_parser.h
View File

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

5
include/ur_modern_driver/packet.h
View File

@@ -1,7 +1,8 @@
#pragma once
#include "ur_modern_driver/bin_parser.h"
class Packet {
class Packet
{
public:
virtual bool parse_with(BinParser& bp) = 0;
virtual bool parseWith(BinParser& bp) = 0;
};

3
include/ur_modern_driver/parser.h
View File

@@ -2,7 +2,8 @@
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/packet.h"
class Parser {
class Parser
{
public:
virtual std::unique_ptr<Packet> parse(BinParser& bp) = 0;
};

106
include/ur_modern_driver/pipeline.h
View File

@@ -1,116 +1,134 @@
#pragma once
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/queue/readerwriterqueue.h"
#include <atomic>
#include <thread>
#include <vector>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/queue/readerwriterqueue.h"
using namespace moodycamel;
using namespace std;
template <typename T>
class IConsumer {
class IConsumer
{
public:
virtual void setup_consumer() {}
virtual void teardown_consumer() {}
virtual void stop_consumer() {}
virtual void setupConsumer()
{
}
virtual void teardownConsumer()
{
}
virtual void stopConsumer()
{
}
virtual bool consume(unique_ptr<T> product) = 0;
};
template <typename T>
class IProducer {
class IProducer
{
public:
virtual void setup_producer() {}
virtual void teardown_producer() {}
virtual void stop_producer() {}
virtual void setupProducer()
{
}
virtual void teardownProducer()
{
}
virtual void stopProducer()
{
}
virtual bool try_get(std::vector<unique_ptr<T> >& products) = 0;
virtual bool tryGet(std::vector<unique_ptr<T>>& products) = 0;
};
template <typename T>
class Pipeline {
class Pipeline
{
private:
IProducer<T>& _producer;
IConsumer<T>& _consumer;
BlockingReaderWriterQueue<unique_ptr<T> > _queue;
atomic<bool> _running;
thread _pThread, _cThread;
IProducer<T>& producer_;
IConsumer<T>& consumer_;
BlockingReaderWriterQueue<unique_ptr<T>> queue_;
atomic<bool> running_;
thread pThread_, cThread_;
void run_producer()
{
_producer.setup_producer();
producer_.setupProducer();
std::vector<unique_ptr<T>> products;
while (_running) {
if (!_producer.try_get(products)) {
while (running_)
{
if (!producer_.tryGet(products))
{
break;
}
for (auto& p : products) {
if (!_queue.try_enqueue(std::move(p))) {
for (auto& p : products)
{
if (!queue_.try_enqueue(std::move(p)))
{
LOG_ERROR("Pipeline producer owerflowed!");
}
}
products.clear();
}
_producer.teardown_producer();
producer_.teardownProducer();
LOG_DEBUG("Pipline producer ended");
_consumer.stop_consumer();
consumer_.stopConsumer();
}
void run_consumer()
{
_consumer.setup_consumer();
consumer_.setupConsumer();
unique_ptr<T> product;
while (_running) {
while (running_)
{
// 16000us timeout was chosen because we should
// roughly recieve messages at 125hz which is every
// 8ms == 8000us and double it for some error margin
if (!_queue.wait_dequeue_timed(product, 16000)) {
if (!queue_.wait_dequeue_timed(product, 16000))
{
continue;
}
if (!_consumer.consume(std::move(product)))
if (!consumer_.consume(std::move(product)))
break;
}
_consumer.teardown_consumer();
consumer_.teardownConsumer();
LOG_DEBUG("Pipline consumer ended");
_producer.stop_producer();
producer_.stopProducer();
}
public:
Pipeline(IProducer<T>& producer, IConsumer<T>& consumer)
: _producer(producer)
, _consumer(consumer)
, _queue{ 32 }
, _running{ false }
: producer_(producer), consumer_(consumer), queue_{ 32 }, running_{ false }
{
}
void run()
{
if (_running)
if (running_)
return;
_running = true;
_pThread = thread(&Pipeline::run_producer, this);
_cThread = thread(&Pipeline::run_consumer, this);
running_ = true;
pThread_ = thread(&Pipeline::run_producer, this);
cThread_ = thread(&Pipeline::run_consumer, this);
}
void stop()
{
if (!_running)
if (!running_)
return;
LOG_DEBUG("Stopping pipeline");
_consumer.stop_consumer();
_producer.stop_producer();
consumer_.stopConsumer();
producer_.stopProducer();
_running = false;
running_ = false;
_pThread.join();
_cThread.join();
pThread_.join();
cThread_.join();
}
};

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

@@ -63,9 +63,10 @@
// Portable atomic fences implemented below:
namespace moodycamel {
enum memory_order {
namespace moodycamel
{
enum memory_order
{
memory_order_relaxed,
memory_order_acquire,
memory_order_release,
@@ -98,17 +99,19 @@ enum memory_order {
#ifdef AE_VCPP
#pragma warning(push)
#pragma warning(disable : 4365) // Disable erroneous 'conversion from long to unsigned int, signed/unsigned mismatch' error when using `assert`
#pragma warning(disable : 4365) // Disable erroneous 'conversion from long to unsigned int, signed/unsigned mismatch'
// error when using `assert`
#ifdef __cplusplus_cli
#pragma managed(push, off)
#endif
#endif
namespace moodycamel {
namespace moodycamel
{
AE_FORCEINLINE void compiler_fence(memory_order order)
{
switch (order) {
switch (order)
{
case memory_order_relaxed:
break;
case memory_order_acquire:
@@ -134,7 +137,8 @@ AE_FORCEINLINE void compiler_fence(memory_order order)
#if defined(AE_ARCH_X86) || defined(AE_ARCH_X64)
AE_FORCEINLINE void fence(memory_order order)
{
switch (order) {
switch (order)
{
case memory_order_relaxed:
break;
case memory_order_acquire:
@@ -159,7 +163,8 @@ AE_FORCEINLINE void fence(memory_order order)
AE_FORCEINLINE void fence(memory_order order)
{
// Non-specialized arch, use heavier memory barriers everywhere just in case :-(
switch (order) {
switch (order)
{
case memory_order_relaxed:
break;
case memory_order_acquire:
@@ -192,11 +197,12 @@ AE_FORCEINLINE void fence(memory_order order)
// Use standard library of atomics
#include <atomic>
namespace moodycamel {
namespace moodycamel
{
AE_FORCEINLINE void compiler_fence(memory_order order)
{
switch (order) {
switch (order)
{
case memory_order_relaxed:
break;
case memory_order_acquire:
@@ -218,7 +224,8 @@ AE_FORCEINLINE void compiler_fence(memory_order order)
AE_FORCEINLINE void fence(memory_order order)
{
switch (order) {
switch (order)
{
case memory_order_relaxed:
break;
case memory_order_acquire:
@@ -255,32 +262,32 @@ AE_FORCEINLINE void fence(memory_order order)
// Provides basic support for atomic variables -- no memory ordering guarantees are provided.
// The guarantee of atomicity is only made for types that already have atomic load and store guarantees
// at the hardware level -- on most platforms this generally means aligned pointers and integers (only).
namespace moodycamel {
namespace moodycamel
{
template <typename T>
class weak_atomic {
class weak_atomic
{
public:
weak_atomic() {}
weak_atomic()
{
}
#ifdef AE_VCPP
#pragma warning(disable : 4100) // Get rid of (erroneous) 'unreferenced formal parameter' warning
#endif
template <typename U>
weak_atomic(U&& x)
: value(std::forward<U>(x))
weak_atomic(U&& x) : value(std::forward<U>(x))
{
}
#ifdef __cplusplus_cli
// Work around bug with universal reference/nullptr combination that only appears when /clr is on
weak_atomic(nullptr_t)
: value(nullptr)
weak_atomic(nullptr_t) : value(nullptr)
{
}
#endif
weak_atomic(weak_atomic const& other)
: value(other.value)
weak_atomic(weak_atomic const& other) : value(other.value)
{
}
weak_atomic(weak_atomic&& other)
: value(std::move(other.value))
weak_atomic(weak_atomic&& other) : value(std::move(other.value))
{
}
#ifdef AE_VCPP
@@ -305,7 +312,10 @@ public:
return *this;
}
AE_FORCEINLINE T load() const { return value; }
AE_FORCEINLINE T load() const
{
return value;
}
AE_FORCEINLINE T fetch_add_acquire(T increment)
{
@@ -352,7 +362,10 @@ public:
return *this;
}
AE_FORCEINLINE T load() const { return value.load(std::memory_order_relaxed); }
AE_FORCEINLINE T load() const
{
return value.load(std::memory_order_relaxed);
}
AE_FORCEINLINE T fetch_add_acquire(T increment)
{
@@ -387,7 +400,8 @@ private:
// namespace with thousands of generic names or adding a .cpp for nothing.
extern "C" {
struct _SECURITY_ATTRIBUTES;
__declspec(dllimport) void* __stdcall CreateSemaphoreW(_SECURITY_ATTRIBUTES* lpSemaphoreAttributes, long lInitialCount, long lMaximumCount, const wchar_t* lpName);
__declspec(dllimport) void* __stdcall CreateSemaphoreW(_SECURITY_ATTRIBUTES* lpSemaphoreAttributes, long lInitialCount,
long lMaximumCount, const wchar_t* lpName);
__declspec(dllimport) int __stdcall CloseHandle(void* hObject);
__declspec(dllimport) unsigned long __stdcall WaitForSingleObject(void* hHandle, unsigned long dwMilliseconds);
__declspec(dllimport) int __stdcall ReleaseSemaphore(void* hSemaphore, long lReleaseCount, long* lpPreviousCount);
@@ -398,7 +412,8 @@ __declspec(dllimport) int __stdcall ReleaseSemaphore(void* hSemaphore, long lRel
#include <semaphore.h>
#endif
namespace moodycamel {
namespace moodycamel
{
// Code in the spsc_sema namespace below is an adaptation of Jeff Preshing's
// portable + lightweight semaphore implementations, originally from
// https://github.com/preshing/cpp11-on-multicore/blob/master/common/sema.h
@@ -420,9 +435,11 @@ namespace moodycamel {
// 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
namespace spsc_sema {
namespace spsc_sema
{
#if defined(_WIN32)
class Semaphore {
class Semaphore
{
private:
void* m_hSema;
@@ -470,7 +487,8 @@ namespace spsc_sema {
// Semaphore (Apple iOS and OSX)
// Can't use POSIX semaphores due to http://lists.apple.com/archives/darwin-kernel/2009/Apr/msg00010.html
//---------------------------------------------------------
class Semaphore {
class Semaphore
{
private:
semaphore_t m_sema;
@@ -505,7 +523,8 @@ namespace spsc_sema {
ts.tv_sec = timeout_usecs / 1000000;
ts.tv_nsec = (timeout_usecs % 1000000) * 1000;
// added in OSX 10.10: https://developer.apple.com/library/prerelease/mac/documentation/General/Reference/APIDiffsMacOSX10_10SeedDiff/modules/Darwin.html
// added in OSX 10.10:
// https://developer.apple.com/library/prerelease/mac/documentation/General/Reference/APIDiffsMacOSX10_10SeedDiff/modules/Darwin.html
kern_return_t rc = semaphore_timedwait(m_sema, ts);
return rc != KERN_OPERATION_TIMED_OUT;
@@ -518,7 +537,8 @@ namespace spsc_sema {
void signal(int count)
{
while (count-- > 0) {
while (count-- > 0)
{
semaphore_signal(m_sema);
}
}
@@ -527,7 +547,8 @@ namespace spsc_sema {
//---------------------------------------------------------
// Semaphore (POSIX, Linux)
//---------------------------------------------------------
class Semaphore {
class Semaphore
{
private:
sem_t m_sema;
@@ -550,7 +571,8 @@ namespace spsc_sema {
{
// http://stackoverflow.com/questions/2013181/gdb-causes-sem-wait-to-fail-with-eintr-error
int rc;
do {
do
{
rc = sem_wait(&m_sema);
} while (rc == -1 && errno == EINTR);
}
@@ -558,7 +580,8 @@ namespace spsc_sema {
bool try_wait()
{
int rc;
do {
do
{
rc = sem_trywait(&m_sema);
} while (rc == -1 && errno == EINTR);
return !(rc == -1 && errno == EAGAIN);
@@ -574,13 +597,15 @@ namespace spsc_sema {
ts.tv_nsec += (usecs % usecs_in_1_sec) * 1000;
// sem_timedwait bombs if you have more than 1e9 in tv_nsec
// so we have to clean things up before passing it in
if (ts.tv_nsec > nsecs_in_1_sec) {
if (ts.tv_nsec > nsecs_in_1_sec)
{
ts.tv_nsec -= nsecs_in_1_sec;
++ts.tv_sec;
}
int rc;
do {
do
{
rc = sem_timedwait(&m_sema, &ts);
} while (rc == -1 && errno == EINTR);
return !(rc == -1 && errno == ETIMEDOUT);
@@ -593,7 +618,8 @@ namespace spsc_sema {
void signal(int count)
{
while (count-- > 0) {
while (count-- > 0)
{
sem_post(&m_sema);
}
}
@@ -605,7 +631,8 @@ namespace spsc_sema {
//---------------------------------------------------------
// LightweightSemaphore
//---------------------------------------------------------
class LightweightSemaphore {
class LightweightSemaphore
{
public:
typedef std::make_signed<std::size_t>::type ssize_t;
@@ -620,8 +647,10 @@ namespace spsc_sema {
// If we lower it to 1000, testBenaphore becomes 15x slower on my Core i7-5930K Windows PC,
// as threads start hitting the kernel semaphore.
int spin = 10000;
while (--spin >= 0) {
if (m_count.load() > 0) {
while (--spin >= 0)
{
if (m_count.load() > 0)
{
m_count.fetch_add_acquire(-1);
return true;
}
@@ -630,7 +659,8 @@ namespace spsc_sema {
oldCount = m_count.fetch_add_acquire(-1);
if (oldCount > 0)
return true;
if (timeout_usecs < 0) {
if (timeout_usecs < 0)
{
m_sema.wait();
return true;
}
@@ -641,7 +671,8 @@ namespace spsc_sema {
// it. So we have to re-adjust the count, but only if the semaphore
// wasn't signaled enough times for us too since then. If it was, we
// need to release the semaphore too.
while (true) {
while (true)
{
oldCount = m_count.fetch_add_release(1);
if (oldCount < 0)
return false; // successfully restored things to the way they were
@@ -653,15 +684,15 @@ namespace spsc_sema {
}
public:
LightweightSemaphore(ssize_t initialCount = 0)
: m_count(initialCount)
LightweightSemaphore(ssize_t initialCount = 0) : m_count(initialCount)
{
assert(initialCount >= 0);
}
bool tryWait()
{
if (m_count.load() > 0) {
if (m_count.load() > 0)
{
m_count.fetch_add_acquire(-1);
return true;
}
@@ -684,7 +715,8 @@ namespace spsc_sema {
assert(count >= 0);
ssize_t oldCount = m_count.fetch_add_release(count);
assert(oldCount >= -1);
if (oldCount < 0) {
if (oldCount < 0)
{
m_sema.signal(1);
}
}

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

@@ -4,7 +4,6 @@
#pragma once
#include "atomicops.h"
#include <cassert>
#include <cstdint>
#include <cstdlib> // For malloc/free/abort & size_t
@@ -12,6 +11,7 @@
#include <stdexcept>
#include <type_traits>
#include <utility>
#include "atomicops.h"
#if __cplusplus > 199711L || _MSC_VER >= 1700 // C++11 or VS2012
#include <chrono>
#endif
@@ -34,7 +34,8 @@
#endif
#ifndef MOODYCAMEL_EXCEPTIONS_ENABLED
#if (defined(_MSC_VER) && defined(_CPPUNWIND)) || (defined(__GNUC__) && defined(__EXCEPTIONS)) || (!defined(_MSC_VER) && !defined(__GNUC__))
#if (defined(_MSC_VER) && defined(_CPPUNWIND)) || (defined(__GNUC__) && defined(__EXCEPTIONS)) || \
(!defined(_MSC_VER) && !defined(__GNUC__))
#define MOODYCAMEL_EXCEPTIONS_ENABLED
#endif
#endif
@@ -46,10 +47,11 @@
#pragma warning(disable : 4127) // conditional expression is constant
#endif
namespace moodycamel {
namespace moodycamel
{
template <typename T, size_t MAX_BLOCK_SIZE = 512>
class ReaderWriterQueue {
class ReaderWriterQueue
{
// Design: Based on a queue-of-queues. The low-level queues are just
// circular buffers with front and tail indices indicating where the
// next element to dequeue is and where the next element can be enqueued,
@@ -77,8 +79,7 @@ public:
// at least one extra buffer block).
explicit ReaderWriterQueue(size_t maxSize = 15)
#ifndef NDEBUG
: enqueuing(false)
, dequeuing(false)
: enqueuing(false), dequeuing(false)
#endif
{
assert(maxSize > 0);
@@ -88,35 +89,45 @@ public:
Block* firstBlock = nullptr;
largestBlockSize = ceilToPow2(maxSize + 1); // We need a spare slot to fit maxSize elements in the block
if (largestBlockSize > MAX_BLOCK_SIZE * 2) {
if (largestBlockSize > MAX_BLOCK_SIZE * 2)
{
// We need a spare block in case the producer is writing to a different block the consumer is reading from, and
// wants to enqueue the maximum number of elements. We also need a spare element in each block to avoid the ambiguity
// wants to enqueue the maximum number of elements. We also need a spare element in each block to avoid the
// ambiguity
// between front == tail meaning "empty" and "full".
// So the effective number of slots that are guaranteed to be usable at any time is the block size - 1 times the
// number of blocks - 1. Solving for maxSize and applying a ceiling to the division gives us (after simplifying):
size_t initialBlockCount = (maxSize + MAX_BLOCK_SIZE * 2 - 3) / (MAX_BLOCK_SIZE - 1);
largestBlockSize = MAX_BLOCK_SIZE;
Block* lastBlock = nullptr;
for (size_t i = 0; i != initialBlockCount; ++i) {
for (size_t i = 0; i != initialBlockCount; ++i)
{
auto block = make_block(largestBlockSize);
if (block == nullptr) {
if (block == nullptr)
{
#ifdef MOODYCAMEL_EXCEPTIONS_ENABLED
throw std::bad_alloc();
#else
abort();
#endif
}
if (firstBlock == nullptr) {
if (firstBlock == nullptr)
{
firstBlock = block;
} else {
}
else
{
lastBlock->next = block;
}
lastBlock = block;
block->next = firstBlock;
}
} else {
}
else
{
firstBlock = make_block(largestBlockSize);
if (firstBlock == nullptr) {
if (firstBlock == nullptr)
{
#ifdef MOODYCAMEL_EXCEPTIONS_ENABLED
throw std::bad_alloc();
#else
@@ -142,12 +153,14 @@ public:
// Destroy any remaining objects in queue and free memory
Block* frontBlock_ = frontBlock;
Block* block = frontBlock_;
do {
do
{
Block* nextBlock = block->next;
size_t blockFront = block->front;
size_t blockTail = block->tail;
for (size_t i = blockFront; i != blockTail; i = (i + 1) & block->sizeMask) {
for (size_t i = blockFront; i != blockTail; i = (i + 1) & block->sizeMask)
{
auto element = reinterpret_cast<T*>(block->data + i * sizeof(T));
element->~T();
(void)element;
@@ -223,7 +236,8 @@ public:
size_t blockTail = frontBlock_->localTail;
size_t blockFront = frontBlock_->front.load();
if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) {
if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load()))
{
fence(memory_order_acquire);
non_empty_front_block:
@@ -236,7 +250,9 @@ public:
fence(memory_order_release);
frontBlock_->front = blockFront;
} else if (frontBlock_ != tailBlock.load()) {
}
else if (frontBlock_ != tailBlock.load())
{
fence(memory_order_acquire);
frontBlock_ = frontBlock.load();
@@ -244,7 +260,8 @@ public:
blockFront = frontBlock_->front.load();
fence(memory_order_acquire);
if (blockFront != blockTail) {
if (blockFront != blockTail)
{
// Oh look, the front block isn't empty after all
goto non_empty_front_block;
}
@@ -279,7 +296,9 @@ public:
fence(memory_order_release);
frontBlock_->front = nextBlockFront;
} else {
}
else
{
// No elements in current block and no other block to advance to
return false;
}
@@ -303,18 +322,22 @@ public:
size_t blockTail = frontBlock_->localTail;
size_t blockFront = frontBlock_->front.load();
if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) {
if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load()))
{
fence(memory_order_acquire);
non_empty_front_block:
return reinterpret_cast<T*>(frontBlock_->data + blockFront * sizeof(T));
} else if (frontBlock_ != tailBlock.load()) {
}
else if (frontBlock_ != tailBlock.load())
{
fence(memory_order_acquire);
frontBlock_ = frontBlock.load();
blockTail = frontBlock_->localTail = frontBlock_->tail.load();
blockFront = frontBlock_->front.load();
fence(memory_order_acquire);
if (blockFront != blockTail) {
if (blockFront != blockTail)
{
goto non_empty_front_block;
}
@@ -344,7 +367,8 @@ public:
size_t blockTail = frontBlock_->localTail;
size_t blockFront = frontBlock_->front.load();
if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) {
if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load()))
{
fence(memory_order_acquire);
non_empty_front_block:
@@ -355,14 +379,17 @@ public:
fence(memory_order_release);
frontBlock_->front = blockFront;
} else if (frontBlock_ != tailBlock.load()) {
}
else if (frontBlock_ != tailBlock.load())
{
fence(memory_order_acquire);
frontBlock_ = frontBlock.load();
blockTail = frontBlock_->localTail = frontBlock_->tail.load();
blockFront = frontBlock_->front.load();
fence(memory_order_acquire);
if (blockFront != blockTail) {
if (blockFront != blockTail)
{
goto non_empty_front_block;
}
@@ -388,7 +415,9 @@ public:
fence(memory_order_release);
frontBlock_->front = nextBlockFront;
} else {
}
else
{
// No elements in current block and no other block to advance to
return false;
}
@@ -403,7 +432,8 @@ public:
size_t result = 0;
Block* frontBlock_ = frontBlock.load();
Block* block = frontBlock_;
do {
do
{
fence(memory_order_acquire);
size_t blockFront = block->front.load();
size_t blockTail = block->tail.load();
@@ -414,8 +444,11 @@ public:
}
private:
enum AllocationMode { CanAlloc,
CannotAlloc };
enum AllocationMode
{
CanAlloc,
CannotAlloc
};
template <AllocationMode canAlloc, typename U>
bool inner_enqueue(U&& element)
@@ -436,7 +469,8 @@ private:
size_t blockTail = tailBlock_->tail.load();
size_t nextBlockTail = (blockTail + 1) & tailBlock_->sizeMask;
if (nextBlockTail != blockFront || nextBlockTail != (tailBlock_->localFront = tailBlock_->front.load())) {
if (nextBlockTail != blockFront || nextBlockTail != (tailBlock_->localFront = tailBlock_->front.load()))
{
fence(memory_order_acquire);
// This block has room for at least one more element
char* location = tailBlock_->data + blockTail * sizeof(T);
@@ -444,9 +478,12 @@ private:
fence(memory_order_release);
tailBlock_->tail = nextBlockTail;
} else {
}
else
{
fence(memory_order_acquire);
if (tailBlock_->next.load() != frontBlock) {
if (tailBlock_->next.load() != frontBlock)
{
// Note that the reason we can't advance to the frontBlock and start adding new entries there
// is because if we did, then dequeue would stay in that block, eventually reading the new values,
// instead of advancing to the next full block (whose values were enqueued first and so should be
@@ -472,11 +509,14 @@ private:
fence(memory_order_release);
tailBlock = tailBlockNext;
} else if (canAlloc == CanAlloc) {
}
else if (canAlloc == CanAlloc)
{
// tailBlock is full and there's no free block ahead; create a new block
auto newBlockSize = largestBlockSize >= MAX_BLOCK_SIZE ? largestBlockSize : largestBlockSize * 2;
auto newBlock = make_block(newBlockSize);
if (newBlock == nullptr) {
if (newBlock == nullptr)
{
// Could not allocate a block!
return false;
}
@@ -498,10 +538,14 @@ private:
fence(memory_order_release);
tailBlock = newBlock;
} else if (canAlloc == CannotAlloc) {
}
else if (canAlloc == CannotAlloc)
{
// Would have had to allocate a new block to enqueue, but not allowed
return false;
} else {
}
else
{
assert(false && "Should be unreachable code");
return false;
}
@@ -511,10 +555,14 @@ private:
}
// Disable copying
ReaderWriterQueue(ReaderWriterQueue const&) {}
ReaderWriterQueue(ReaderWriterQueue const&)
{
}
// Disable assignment
ReaderWriterQueue& operator=(ReaderWriterQueue const&) {}
ReaderWriterQueue& operator=(ReaderWriterQueue const&)
{
}
AE_FORCEINLINE static size_t ceilToPow2(size_t x)
{
@@ -523,7 +571,8 @@ private:
x |= x >> 1;
x |= x >> 2;
x |= x >> 4;
for (size_t i = 1; i < sizeof(size_t); i <<= 1) {
for (size_t i = 1; i < sizeof(size_t); i <<= 1)
{
x |= x >> (i << 3);
}
++x;
@@ -539,15 +588,19 @@ private:
private:
#ifndef NDEBUG
struct ReentrantGuard {
ReentrantGuard(bool& _inSection)
: inSection(_inSection)
struct ReentrantGuard
{
assert(!inSection && "ReaderWriterQueue does not support enqueuing or dequeuing elements from other elements' ctors and dtors");
ReentrantGuard(bool& _inSection) : inSection(_inSection)
{
assert(!inSection && "ReaderWriterQueue does not support enqueuing or dequeuing elements from other elements' "
"ctors and dtors");
inSection = true;
}
~ReentrantGuard() { inSection = false; }
~ReentrantGuard()
{
inSection = false;
}
private:
ReentrantGuard& operator=(ReentrantGuard const&);
@@ -557,7 +610,8 @@ private:
};
#endif
struct Block {
struct Block
{
// Avoid false-sharing by putting highly contended variables on their own cache lines
weak_atomic<size_t> front; // (Atomic) Elements are read from here
size_t localTail; // An uncontended shadow copy of tail, owned by the consumer
@@ -566,7 +620,14 @@ private:
weak_atomic<size_t> tail; // (Atomic) Elements are enqueued here
size_t localFront;
char cachelineFiller1[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic<size_t>) - sizeof(size_t)]; // next isn't very contended, but we don't want it on the same cache line as tail (which is)
char cachelineFiller1[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic<size_t>) - sizeof(size_t)]; // next isn't
// very
// contended, but
// we don't want
// it on the same
// cache line as
// tail (which
// is)
weak_atomic<Block*> next; // (Atomic)
char* data; // Contents (on heap) are aligned to T's alignment
@@ -600,7 +661,8 @@ private:
auto size = sizeof(Block) + std::alignment_of<Block>::value - 1;
size += sizeof(T) * capacity + std::alignment_of<T>::value - 1;
auto newBlockRaw = static_cast<char*>(std::malloc(size));
if (newBlockRaw == nullptr) {
if (newBlockRaw == nullptr)
{
return nullptr;
}
@@ -625,13 +687,13 @@ private:
// Like ReaderWriterQueue, but also providees blocking operations
template <typename T, size_t MAX_BLOCK_SIZE = 512>
class BlockingReaderWriterQueue {
class BlockingReaderWriterQueue
{
private:
typedef ::moodycamel::ReaderWriterQueue<T, MAX_BLOCK_SIZE> ReaderWriterQueue;
public:
explicit BlockingReaderWriterQueue(size_t maxSize = 15)
: inner(maxSize)
explicit BlockingReaderWriterQueue(size_t maxSize = 15) : inner(maxSize)
{
}
@@ -640,7 +702,8 @@ public:
// Does not allocate memory.
AE_FORCEINLINE bool try_enqueue(T const& element)
{
if (inner.try_enqueue(element)) {
if (inner.try_enqueue(element))
{
sema.signal();
return true;
}
@@ -652,7 +715,8 @@ public:
// Does not allocate memory.
AE_FORCEINLINE bool try_enqueue(T&& element)
{
if (inner.try_enqueue(std::forward<T>(element))) {
if (inner.try_enqueue(std::forward<T>(element)))
{
sema.signal();
return true;
}
@@ -664,7 +728,8 @@ public:
// Only fails (returns false) if memory allocation fails.
AE_FORCEINLINE bool enqueue(T const& element)
{
if (inner.enqueue(element)) {
if (inner.enqueue(element))
{
sema.signal();
return true;
}
@@ -676,7 +741,8 @@ public:
// Only fails (returns false) if memory allocation fails.
AE_FORCEINLINE bool enqueue(T&& element)
{
if (inner.enqueue(std::forward<T>(element))) {
if (inner.enqueue(std::forward<T>(element)))
{
sema.signal();
return true;
}
@@ -689,7 +755,8 @@ public:
template <typename U>
bool try_dequeue(U& result)
{
if (sema.tryWait()) {
if (sema.tryWait())
{
bool success = inner.try_dequeue(result);
assert(success);
AE_UNUSED(success);
@@ -719,7 +786,8 @@ public:
template <typename U>
bool wait_dequeue_timed(U& result, std::int64_t timeout_usecs)
{
if (!sema.wait(timeout_usecs)) {
if (!sema.wait(timeout_usecs))
{
return false;
}
bool success = inner.try_dequeue(result);
@@ -758,7 +826,8 @@ public:
// `pop` was called.
AE_FORCEINLINE bool pop()
{
if (sema.tryWait()) {
if (sema.tryWait())
{
bool result = inner.pop();
assert(result);
AE_UNUSED(result);
@@ -776,8 +845,12 @@ public:
private:
// Disable copying & assignment
BlockingReaderWriterQueue(ReaderWriterQueue const&) {}
BlockingReaderWriterQueue& operator=(ReaderWriterQueue const&) {}
BlockingReaderWriterQueue(ReaderWriterQueue const&)
{
}
BlockingReaderWriterQueue& operator=(ReaderWriterQueue const&)
{
}
private:
ReaderWriterQueue inner;

49
include/ur_modern_driver/robot_state.h
View File

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

19
include/ur_modern_driver/robot_state_RT.h
View File

@@ -19,15 +19,16 @@
#ifndef ROBOT_STATE_RT_H_
#define ROBOT_STATE_RT_H_
#include <condition_variable>
#include <inttypes.h>
#include <mutex>
#include <netinet/in.h>
#include <stdlib.h>
#include <string.h>
#include <condition_variable>
#include <mutex>
#include <vector>
class RobotStateRT {
class RobotStateRT
{
private:
double version_; // protocol version
@@ -41,12 +42,15 @@ private:
std::vector<double> qd_actual_; // Actual joint velocities
std::vector<double> i_actual_; // Actual joint currents
std::vector<double> i_control_; // Joint control currents
std::vector<double> tool_vector_actual_; //Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation
std::vector<double> tool_vector_actual_; // Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry
// and rz is a rotation vector representation of the tool orientation
std::vector<double> tcp_speed_actual_; // Actual speed of the tool given in Cartesian coordinates
std::vector<double> tcp_force_; // Generalised forces in the TC
std::vector<double> tool_vector_target_; //Target Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation
std::vector<double> tool_vector_target_; // Target Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry
// and rz is a rotation vector representation of the tool orientation
std::vector<double> tcp_speed_target_; // Target speed of the tool given in Cartesian coordinates
std::vector<bool> digital_input_bits_; //Current state of the digital inputs. NOTE: these are bits encoded as int64_t, e.g. a value of 5 corresponds to bit 0 and bit 2 set high
std::vector<bool> digital_input_bits_; // Current state of the digital inputs. NOTE: these are bits encoded as
// int64_t, e.g. a value of 5 corresponds to bit 0 and bit 2 set high
std::vector<double> motor_temperatures_; // Temperature of each joint in degrees celsius
double controller_timer_; // Controller realtime thread execution time
double robot_mode_; // Robot mode
@@ -66,8 +70,7 @@ private:
bool data_published_; // to avoid spurious wakes
bool controller_updated_; // to avoid spurious wakes
std::vector<double> unpackVector(uint8_t* buf, int start_index,
int nr_of_vals);
std::vector<double> unpackVector(uint8_t* buf, int start_index, int nr_of_vals);
std::vector<bool> unpackDigitalInputBits(int64_t data);
double ntohd(uint64_t nf);

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

@@ -1,6 +1,5 @@
#pragma once
#include <cstdlib>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/WrenchStamped.h>
@@ -9,6 +8,7 @@
#include <sensor_msgs/Temperature.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <cstdlib>
#include <vector>
#include "ur_modern_driver/ur/consumer.h"
@@ -16,46 +16,42 @@
using namespace ros;
using namespace tf;
const std::string JOINTS[] = {
"shoulder_pan_joint",
"shoulder_lift_joint",
"elbow_joint",
"wrist_1_joint",
"wrist_2_joint",
"wrist_3_joint"
};
const std::string JOINTS[] = { "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint" };
class RTPublisher : public URRTPacketConsumer {
class RTPublisher : public URRTPacketConsumer
{
private:
NodeHandle _nh;
Publisher _joint_pub;
Publisher _wrench_pub;
Publisher _tool_vel_pub;
Publisher _joint_temperature_pub;
TransformBroadcaster _transform_broadcaster;
std::vector<std::string> _joint_names;
std::string _base_frame;
std::string _tool_frame;
NodeHandle nh_;
Publisher joint_pub_;
Publisher wrench_pub_;
Publisher tool_vel_pub_;
Publisher joint_temperature_pub_;
TransformBroadcaster transform_broadcaster_;
std::vector<std::string> joint_names_;
std::string base_frame_;
std::string tool_frame_;
bool publish_joints(RTShared& packet, Time& t);
bool publish_wrench(RTShared& packet, Time& t);
bool publish_tool(RTShared& packet, Time& t);
bool publish_transform(RTShared& packet, Time& t);
bool publish_temperature(RTShared& packet, Time& t);
bool publishJoints(RTShared& packet, Time& t);
bool publishWrench(RTShared& packet, Time& t);
bool publishTool(RTShared& packet, Time& t);
bool publishTransform(RTShared& packet, Time& t);
bool publishTemperature(RTShared& packet, Time& t);
bool publish(RTShared& packet);
public:
RTPublisher(std::string& joint_prefix, std::string& base_frame, std::string& tool_frame)
: _joint_pub(_nh.advertise<sensor_msgs::JointState>("joint_states", 1))
, _wrench_pub(_nh.advertise<geometry_msgs::WrenchStamped>("wrench", 1))
, _tool_vel_pub(_nh.advertise<geometry_msgs::TwistStamped>("tool_velocity", 1))
, _joint_temperature_pub(_nh.advertise<sensor_msgs::Temperature>("joint_temperature", 1))
, _base_frame(base_frame)
, _tool_frame(tool_frame)
: joint_pub_(nh_.advertise<sensor_msgs::JointState>("joint_states", 1))
, wrench_pub_(nh_.advertise<geometry_msgs::WrenchStamped>("wrench", 1))
, tool_vel_pub_(nh_.advertise<geometry_msgs::TwistStamped>("tool_velocity", 1))
, joint_temperature_pub_(nh_.advertise<sensor_msgs::Temperature>("joint_temperature", 1))
, base_frame_(base_frame)
, tool_frame_(tool_frame)
{
for (auto const& j : JOINTS) {
_joint_names.push_back(joint_prefix + j);
for (auto const& j : JOINTS)
{
joint_names_.push_back(joint_prefix + j);
}
}
@@ -64,7 +60,13 @@ public:
virtual bool consume(RTState_V3_0__1& state);
virtual bool consume(RTState_V3_2__3& state);
virtual void setup_consumer() {}
virtual void teardown_consumer() {}
virtual void stop_consumer() {}
virtual void setupConsumer()
{
}
virtual void teardownConsumer()
{
}
virtual void stopConsumer()
{
}
};

6
include/ur_modern_driver/types.h
View File

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

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

@@ -7,11 +7,12 @@
#include "ur_modern_driver/ur/rt_state.h"
#include "ur_modern_driver/ur/state.h"
class URRTPacketConsumer : public IConsumer<RTPacket> {
class URRTPacketConsumer : public IConsumer<RTPacket>
{
public:
virtual bool consume(unique_ptr<RTPacket> packet)
{
return packet->consume_with(*this);
return packet->consumeWith(*this);
}
virtual bool consume(RTState_V1_6__7& state) = 0;
@@ -20,11 +21,12 @@ public:
virtual bool consume(RTState_V3_2__3& state) = 0;
};
class URStatePacketConsumer : public IConsumer<StatePacket> {
class URStatePacketConsumer : public IConsumer<StatePacket>
{
public:
virtual bool consume(unique_ptr<StatePacket> packet)
{
return packet->consume_with(*this);
return packet->consumeWith(*this);
}
virtual bool consume(MasterBoardData_V1_X& data) = 0;
@@ -36,11 +38,12 @@ public:
virtual bool consume(RobotModeData_V3_2& data) = 0;
};
class URMessagePacketConsumer : public IConsumer<MessagePacket> {
class URMessagePacketConsumer : public IConsumer<MessagePacket>
{
public:
virtual bool consume(unique_ptr<MessagePacket> packet)
{
return packet->consume_with(*this);
return packet->consumeWith(*this);
}
virtual bool consume(VersionMessage& message) = 0;

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

@@ -1,5 +1,6 @@
#pragma once
#include <cstdlib>
#include "ur_modern_driver/ur/consumer.h"
#include "ur_modern_driver/ur/messages_parser.h"
#include "ur_modern_driver/ur/parser.h"
@@ -7,15 +8,15 @@
#include "ur_modern_driver/ur/rt_parser.h"
#include "ur_modern_driver/ur/state_parser.h"
#include "ur_modern_driver/ur/stream.h"
#include <cstdlib>
class URFactory : private URMessagePacketConsumer {
class URFactory : private URMessagePacketConsumer
{
private:
URStream _stream;
URMessageParser _parser;
URStream stream_;
URMessageParser parser_;
uint8_t _major_version;
uint8_t _minor_version;
uint8_t major_version_;
uint8_t minor_version_;
bool consume(VersionMessage& vm)
{
@@ -24,63 +25,77 @@ private:
LOG_INFO("version: %u.%u.%d", vm.major_version, vm.minor_version, vm.svn_version);
LOG_INFO("build date: %s", vm.build_date.c_str());
_major_version = vm.major_version;
_minor_version = vm.minor_version;
major_version_ = vm.major_version;
minor_version_ = vm.minor_version;
return true;
}
void setup_consumer() {}
void teardown_consumer() {}
void stop_consumer() {}
void setupConsumer()
{
}
void teardownConsumer()
{
}
void stopConsumer()
{
}
public:
URFactory(std::string& host)
: _stream(host, 30001)
URFactory(std::string& host) : stream_(host, 30001)
{
URProducer<MessagePacket> p(_stream, _parser);
URProducer<MessagePacket> prod(stream_, parser_);
std::vector<unique_ptr<MessagePacket>> results;
p.setup_producer();
prod.setupProducer();
if (!p.try_get(results) || results.size() == 0) {
LOG_FATAL("No version message received, init failed!");
std::exit(EXIT_FAILURE);
}
for (auto const& p : results) {
p->consume_with(*this);
}
if (_major_version == 0 && _minor_version == 0) {
LOG_FATAL("No version message received, init failed!");
std::exit(EXIT_FAILURE);
}
p.teardown_producer();
}
std::unique_ptr<URParser<StatePacket> > get_state_parser()
if (!prod.tryGet(results) || results.size() == 0)
{
LOG_FATAL("No version message received, init failed!");
std::exit(EXIT_FAILURE);
}
for (auto const& p : results)
{
p->consumeWith(*this);
}
if (major_version_ == 0 && minor_version_ == 0)
{
LOG_FATAL("No version message received, init failed!");
std::exit(EXIT_FAILURE);
}
prod.teardownProducer();
}
std::unique_ptr<URParser<StatePacket>> getStateParser()
{
if (major_version_ == 1)
{
if (_major_version == 1) {
return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V1_X);
} else {
if (_minor_version < 3)
}
else
{
if (minor_version_ < 3)
return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V3_0__1);
else
return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V3_2);
}
}
std::unique_ptr<URParser<RTPacket> > get_rt_parser()
std::unique_ptr<URParser<RTPacket>> getRTParser()
{
if (_major_version == 1) {
if (_minor_version < 8)
if (major_version_ == 1)
{
if (minor_version_ < 8)
return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V1_6__7);
else
return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V1_8);
} else {
if (_minor_version < 3)
}
else
{
if (minor_version_ < 3)
return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V3_0__1);
else
return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V3_2__3);

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

@@ -1,14 +1,15 @@
#pragma once
#include <inttypes.h>
#include <cstddef>
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/types.h"
#include "ur_modern_driver/ur/state.h"
#include <cstddef>
#include <inttypes.h>
class SharedMasterBoardData {
class SharedMasterBoardData
{
public:
virtual bool parse_with(BinParser& bp);
virtual bool parseWith(BinParser& bp);
int8_t analog_input_range0;
int8_t analog_input_range1;
@@ -29,18 +30,16 @@ public:
int32_t euromap_input_bits;
int32_t euromap_output_bits;
static const size_t SIZE = sizeof(double) * 4
+ sizeof(int8_t) * 4
+ sizeof(float) * 4
+ sizeof(uint8_t);
static const size_t SIZE = sizeof(double) * 4 + sizeof(int8_t) * 4 + sizeof(float) * 4 + sizeof(uint8_t);
static const size_t EURO_SIZE = sizeof(int32_t) * 2;
};
class MasterBoardData_V1_X : public SharedMasterBoardData, public StatePacket {
class MasterBoardData_V1_X : public SharedMasterBoardData, public StatePacket
{
public:
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer);
virtual bool parseWith(BinParser& bp);
virtual bool consumeWith(URStatePacketConsumer& consumer);
int16_t digital_input_bits;
int16_t digital_output_bits;
@@ -52,18 +51,16 @@ public:
int16_t euromap_voltage;
int16_t euromap_current;
static const size_t SIZE = SharedMasterBoardData::SIZE
+ sizeof(int16_t) * 2
+ sizeof(uint8_t) * 2;
static const size_t SIZE = SharedMasterBoardData::SIZE + sizeof(int16_t) * 2 + sizeof(uint8_t) * 2;
static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE
+ sizeof(int16_t) * 2;
static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE + sizeof(int16_t) * 2;
};
class MasterBoardData_V3_0__1 : public SharedMasterBoardData, public StatePacket {
class MasterBoardData_V3_0__1 : public SharedMasterBoardData, public StatePacket
{
public:
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer);
virtual bool parseWith(BinParser& bp);
virtual bool consumeWith(URStatePacketConsumer& consumer);
int32_t digital_input_bits;
int32_t digital_output_bits;
@@ -75,23 +72,20 @@ public:
float euromap_voltage;
float euromap_current;
static const size_t SIZE = SharedMasterBoardData::SIZE
+ sizeof(int32_t) * 2
+ sizeof(uint8_t) * 2
+ sizeof(uint32_t); //UR internal field we skip
static const size_t SIZE = SharedMasterBoardData::SIZE + sizeof(int32_t) * 2 + sizeof(uint8_t) * 2 +
sizeof(uint32_t); // UR internal field we skip
static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE
+ sizeof(float) * 2;
static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE + sizeof(float) * 2;
};
class MasterBoardData_V3_2 : public MasterBoardData_V3_0__1 {
class MasterBoardData_V3_2 : public MasterBoardData_V3_0__1
{
public:
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer);
virtual bool parseWith(BinParser& bp);
virtual bool consumeWith(URStatePacketConsumer& consumer);
uint8_t operational_mode_selector_input;
uint8_t three_position_enabling_device_input;
static const size_t SIZE = MasterBoardData_V3_0__1::SIZE
+ sizeof(uint8_t) * 2;
static const size_t SIZE = MasterBoardData_V3_0__1::SIZE + sizeof(uint8_t) * 2;
};

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

@@ -1,11 +1,12 @@
#pragma once
#include <inttypes.h>
#include <cstddef>
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/pipeline.h"
#include <cstddef>
#include <inttypes.h>
enum class robot_message_type : uint8_t {
enum class robot_message_type : uint8_t
{
ROBOT_MESSAGE_TEXT = 0,
ROBOT_MESSAGE_PROGRAM_LABEL = 1,
PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2,
@@ -19,29 +20,28 @@ enum class robot_message_type : uint8_t {
class URMessagePacketConsumer;
class MessagePacket {
class MessagePacket
{
public:
MessagePacket(uint64_t timestamp, uint8_t source)
: timestamp(timestamp)
, source(source)
MessagePacket(uint64_t timestamp, uint8_t source) : timestamp(timestamp), source(source)
{
}
virtual bool parse_with(BinParser& bp) = 0;
virtual bool consume_with(URMessagePacketConsumer& consumer) = 0;
virtual bool parseWith(BinParser& bp) = 0;
virtual bool consumeWith(URMessagePacketConsumer& consumer) = 0;
uint64_t timestamp;
uint8_t source;
};
class VersionMessage : public MessagePacket {
class VersionMessage : public MessagePacket
{
public:
VersionMessage(uint64_t timestamp, uint8_t source)
: MessagePacket(timestamp, source)
VersionMessage(uint64_t timestamp, uint8_t source) : MessagePacket(timestamp, source)
{
}
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URMessagePacketConsumer& consumer);
virtual bool parseWith(BinParser& bp);
virtual bool consumeWith(URMessagePacketConsumer& consumer);
std::string project_name;
uint8_t major_version;

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

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

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

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

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

@@ -4,41 +4,52 @@
#include "ur_modern_driver/ur/stream.h"
template <typename T>
class URProducer : public IProducer<T> {
class URProducer : public IProducer<T>
{
private:
URStream& _stream;
URParser<T>& _parser;
URStream& stream_;
URParser<T>& parser_;
public:
URProducer(URStream& stream, URParser<T>& parser)
: _stream(stream)
, _parser(parser)
URProducer(URStream& stream, URParser<T>& parser) : stream_(stream), parser_(parser)
{
}
void setup_producer() { _stream.connect(); }
void teardown_producer() { _stream.disconnect(); }
void stop_producer() { _stream.disconnect(); }
void setupProducer()
{
stream_.connect();
}
void teardownProducer()
{
stream_.disconnect();
}
void stopProducer()
{
stream_.disconnect();
}
bool try_get(std::vector<unique_ptr<T> >& products)
bool tryGet(std::vector<unique_ptr<T>>& products)
{
// 4KB should be enough to hold any packet received from UR
uint8_t buf[4096];
// blocking call
ssize_t len = _stream.receive(buf, sizeof(buf));
ssize_t len = stream_.receive(buf, sizeof(buf));
// LOG_DEBUG("Read %d bytes from stream", len);
if (len == 0) {
if (len == 0)
{
LOG_WARN("Read nothing from stream");
return false;
} else if (len < 0) {
}
else if (len < 0)
{
LOG_WARN("Stream closed");
return false;
}
BinParser bp(buf, static_cast<size_t>(len));
return _parser.parse(bp, products);
return parser_.parse(bp, products);
}
};

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

@@ -1,14 +1,15 @@
#pragma once
#include <inttypes.h>
#include <cstddef>
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/types.h"
#include "ur_modern_driver/ur/state.h"
#include <cstddef>
#include <inttypes.h>
class SharedRobotModeData {
class SharedRobotModeData
{
public:
virtual bool parse_with(BinParser& bp);
virtual bool parseWith(BinParser& bp);
uint64_t timestamp;
bool physical_robot_connected;
@@ -21,7 +22,8 @@ public:
static const size_t SIZE = sizeof(uint64_t) + sizeof(uint8_t) * 6;
};
enum class robot_mode_V1_X : uint8_t {
enum class robot_mode_V1_X : uint8_t
{
ROBOT_RUNNING_MODE = 0,
ROBOT_FREEDRIVE_MODE = 1,
ROBOT_READY_MODE = 2,
@@ -35,24 +37,23 @@ enum class robot_mode_V1_X : uint8_t {
ROBOT_SAFEGUARD_STOP_MODE = 10
};
class RobotModeData_V1_X : public SharedRobotModeData, public StatePacket {
class RobotModeData_V1_X : public SharedRobotModeData, public StatePacket
{
public:
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer);
virtual bool parseWith(BinParser& bp);
virtual bool consumeWith(URStatePacketConsumer& consumer);
bool security_stopped;
robot_mode_V1_X robot_mode;
double speed_fraction;
static const size_t SIZE = SharedRobotModeData::SIZE
+ sizeof(uint8_t)
+ sizeof(robot_mode_V1_X)
+ sizeof(double);
static const size_t SIZE = SharedRobotModeData::SIZE + sizeof(uint8_t) + sizeof(robot_mode_V1_X) + sizeof(double);
static_assert(RobotModeData_V1_X::SIZE == 24, "RobotModeData_V1_X has missmatched size");
};
enum class robot_mode_V3_X : uint8_t {
enum class robot_mode_V3_X : uint8_t
{
DISCONNECTED = 0,
CONFIRM_SAFETY = 1,
BOOTING = 2,
@@ -64,17 +65,19 @@ enum class robot_mode_V3_X : uint8_t {
UPDATING_FIRMWARE = 8
};
enum class robot_control_mode_V3_X : uint8_t {
enum class robot_control_mode_V3_X : uint8_t
{
POSITION = 0,
TEACH = 1,
FORCE = 2,
TORQUE = 3
};
class RobotModeData_V3_0__1 : public SharedRobotModeData, public StatePacket {
class RobotModeData_V3_0__1 : public SharedRobotModeData, public StatePacket
{
public:
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer);
virtual bool parseWith(BinParser& bp);
virtual bool consumeWith(URStatePacketConsumer& consumer);
bool protective_stopped;
@@ -84,25 +87,21 @@ public:
double target_speed_fraction;
double speed_scaling;
static const size_t SIZE = SharedRobotModeData::SIZE
+ sizeof(uint8_t)
+ sizeof(robot_mode_V3_X)
+ sizeof(robot_control_mode_V3_X)
+ sizeof(double)
+ sizeof(double);
static const size_t SIZE = SharedRobotModeData::SIZE + sizeof(uint8_t) + sizeof(robot_mode_V3_X) +
sizeof(robot_control_mode_V3_X) + sizeof(double) + sizeof(double);
static_assert(RobotModeData_V3_0__1::SIZE == 33, "RobotModeData_V3_0__1 has missmatched size");
};
class RobotModeData_V3_2 : public RobotModeData_V3_0__1 {
class RobotModeData_V3_2 : public RobotModeData_V3_0__1
{
public:
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer);
virtual bool parseWith(BinParser& bp);
virtual bool consumeWith(URStatePacketConsumer& consumer);
double target_speed_fraction_limit;
static const size_t SIZE = RobotModeData_V3_0__1::SIZE
+ sizeof(double);
static const size_t SIZE = RobotModeData_V3_0__1::SIZE + sizeof(double);
static_assert(RobotModeData_V3_2::SIZE == 41, "RobotModeData_V3_2 has missmatched size");
};

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

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

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

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

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

@@ -1,12 +1,13 @@
#pragma once
#include <inttypes.h>
#include <cstddef>
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.h"
#include <cstddef>
#include <inttypes.h>
enum class package_type : uint8_t {
enum class package_type : uint8_t
{
ROBOT_MODE_DATA = 0,
JOINT_DATA = 1,
TOOL_DATA = 2,
@@ -19,7 +20,8 @@ enum class package_type : uint8_t {
CALIBRATION_DATA = 9
};
enum class message_type : uint8_t {
enum class message_type : uint8_t
{
ROBOT_STATE = 16,
ROBOT_MESSAGE = 20,
PROGRAM_STATE_MESSAGE = 25
@@ -27,8 +29,9 @@ enum class message_type : uint8_t {
class URStatePacketConsumer;
class StatePacket {
class StatePacket
{
public:
virtual bool parse_with(BinParser& bp) = 0;
virtual bool consume_with(URStatePacketConsumer& consumer) = 0;
virtual bool parseWith(BinParser& bp) = 0;
virtual bool consumeWith(URStatePacketConsumer& consumer) = 0;
};

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

@@ -1,4 +1,5 @@
#pragma once
#include <vector>
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.h"
@@ -6,14 +7,15 @@
#include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/robot_mode.h"
#include "ur_modern_driver/ur/state.h"
#include <vector>
template <typename RMD, typename MBD>
class URStateParser : public URParser<StatePacket> {
class URStateParser : public URParser<StatePacket>
{
private:
StatePacket* from_type(package_type type)
{
switch (type) {
switch (type)
{
case package_type::ROBOT_MODE_DATA:
return new RMD;
case package_type::MASTERBOARD_DATA:
@@ -31,18 +33,22 @@ public:
bp.parse(packet_size);
bp.parse(type);
if (type != message_type::ROBOT_STATE) {
if (type != message_type::ROBOT_STATE)
{
LOG_WARN("Invalid message type recieved: %u", static_cast<uint8_t>(type));
return false;
}
while (!bp.empty()) {
if (!bp.check_size(sizeof(uint32_t))) {
while (!bp.empty())
{
if (!bp.checkSize(sizeof(uint32_t)))
{
LOG_ERROR("Failed to read sub-package length, there's likely a parsing error");
return false;
}
uint32_t sub_size = bp.peek<uint32_t>();
if (!bp.check_size(static_cast<size_t>(sub_size))) {
if (!bp.checkSize(static_cast<size_t>(sub_size)))
{
LOG_WARN("Invalid sub-package size of %" PRIu32 " received!", sub_size);
return false;
}
@@ -55,20 +61,23 @@ public:
std::unique_ptr<StatePacket> packet(from_type(type));
if (packet == nullptr) {
if (packet == nullptr)
{
sbp.consume();
LOG_INFO("Skipping sub-packet of type %d", type);
continue;
}
if (!packet->parse_with(sbp)) {
if (!packet->parseWith(sbp))
{
LOG_ERROR("Sub-package parsing of type %d failed!", type);
return false;
}
results.push_back(std::move(packet));
if (!sbp.empty()) {
if (!sbp.empty())
{
LOG_ERROR("Sub-package was not parsed completely!");
return false;
}

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

@@ -1,26 +1,23 @@
#pragma once
#include <atomic>
#include <netdb.h>
#include <string>
#include <sys/socket.h>
#include <sys/types.h>
#include <atomic>
#include <string>
/// Encapsulates a TCP socket
class URStream {
class URStream
{
private:
int _socket_fd = -1;
std::string _host;
int _port;
int socket_fd_ = -1;
std::string host_;
int port_;
std::atomic<bool> _initialized;
std::atomic<bool> _stopping;
std::atomic<bool> initialized_;
std::atomic<bool> stopping_;
public:
URStream(std::string& host, int port)
: _host(host)
, _port(port)
, _initialized(false)
, _stopping(false)
URStream(std::string& host, int port) : host_(host), port_(port), initialized_(false), stopping_(false)
{
}

17
include/ur_modern_driver/ur_communication.h
View File

@@ -19,13 +19,7 @@
#ifndef UR_COMMUNICATION_H_
#define UR_COMMUNICATION_H_
#include "do_output.h"
#include "robot_state.h"
#include <chrono>
#include <condition_variable>
#include <fcntl.h>
#include <iostream>
#include <mutex>
#include <netdb.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
@@ -36,11 +30,18 @@
#include <sys/time.h>
#include <sys/types.h>
#include <sys/types.h>
#include <thread>
#include <unistd.h>
#include <chrono>
#include <condition_variable>
#include <iostream>
#include <mutex>
#include <thread>
#include <vector>
#include "do_output.h"
#include "robot_state.h"
class UrCommunication {
class UrCommunication
{
private:
int pri_sockfd_, sec_sockfd_;
struct sockaddr_in pri_serv_addr_, sec_serv_addr_;

36
include/ur_modern_driver/ur_driver.h
View File

@@ -19,21 +19,22 @@
#ifndef UR_DRIVER_H_
#define UR_DRIVER_H_
#include <math.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <condition_variable>
#include <mutex>
#include <string>
#include <vector>
#include "do_output.h"
#include "ur_communication.h"
#include "ur_realtime_communication.h"
#include <condition_variable>
#include <math.h>
#include <mutex>
#include <netinet/in.h>
#include <string>
#include <sys/socket.h>
#include <sys/types.h>
#include <vector>
#include <chrono>
class UrDriver {
class UrDriver
{
private:
double maximum_time_step_;
double minimum_payload_;
@@ -56,18 +57,16 @@ public:
UrRealtimeCommunication* rt_interface_;
UrCommunication* sec_interface_;
UrDriver(std::condition_variable& rt_msg_cond,
std::condition_variable& msg_cond, std::string host,
unsigned int reverse_port = 50007, double servoj_time = 0.016, unsigned int safety_count_max = 12, double max_time_step = 0.08, double min_payload = 0.,
double max_payload = 1., double servoj_lookahead_time = 0.03, double servoj_gain = 300.);
UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host,
unsigned int reverse_port = 50007, double servoj_time = 0.016, unsigned int safety_count_max = 12,
double max_time_step = 0.08, double min_payload = 0., double max_payload = 1.,
double servoj_lookahead_time = 0.03, double servoj_gain = 300.);
bool start();
void halt();
void setSpeed(double q0, double q1, double q2, double q3, double q4,
double q5, double acc = 100.);
void setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc = 100.);
bool doTraj(std::vector<double> inp_timestamps,
std::vector<std::vector<double> > inp_positions,
bool doTraj(std::vector<double> inp_timestamps, std::vector<std::vector<double>> inp_positions,
std::vector<std::vector<double>> inp_velocities);
void servoj(std::vector<double> positions, int keepalive = 1);
@@ -77,8 +76,7 @@ public:
bool openServo();
void closeServo(std::vector<double> positions);
std::vector<double> interp_cubic(double t, double T,
std::vector<double> p0_pos, std::vector<double> p1_pos,
std::vector<double> interp_cubic(double t, double T, std::vector<double> p0_pos, std::vector<double> p1_pos,
std::vector<double> p0_vel, std::vector<double> p1_vel);
std::vector<std::string> getJointNames();

16
include/ur_modern_driver/ur_hardware_interface.h
View File

@@ -58,9 +58,6 @@
#ifndef UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H
#define UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H
#include "do_output.h"
#include "ur_driver.h"
#include <boost/scoped_ptr.hpp>
#include <controller_manager/controller_manager.h>
#include <hardware_interface/force_torque_sensor_interface.h>
#include <hardware_interface/joint_command_interface.h>
@@ -68,15 +65,19 @@
#include <hardware_interface/robot_hw.h>
#include <math.h>
#include <ros/ros.h>
#include <boost/scoped_ptr.hpp>
#include "do_output.h"
#include "ur_driver.h"
namespace ros_control_ur {
namespace ros_control_ur
{
// For simulation only - determines how fast a trajectory is followed
static const double POSITION_STEP_FACTOR = 1;
static const double VELOCITY_STEP_FACTOR = 1;
/// \brief Hardware interface for a robot
class UrHardwareInterface : public hardware_interface::RobotHW {
class UrHardwareInterface : public hardware_interface::RobotHW
{
public:
/**
* \brief Constructor
@@ -95,8 +96,7 @@ public:
void setMaxVelChange(double inp);
bool canSwitch(
const std::list<hardware_interface::ControllerInfo>& start_list,
bool canSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list) const;
void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list);

21
include/ur_modern_driver/ur_realtime_communication.h
View File

@@ -19,14 +19,9 @@
#ifndef UR_REALTIME_COMMUNICATION_H_
#define UR_REALTIME_COMMUNICATION_H_
#include "do_output.h"
#include "robot_state_RT.h"
#include <arpa/inet.h>
#include <condition_variable>
#include <errno.h>
#include <fcntl.h>
#include <iostream>
#include <mutex>
#include <netdb.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
@@ -37,11 +32,17 @@
#include <sys/time.h>
#include <sys/types.h>
#include <sys/types.h>
#include <thread>
#include <unistd.h>
#include <condition_variable>
#include <iostream>
#include <mutex>
#include <thread>
#include <vector>
#include "do_output.h"
#include "robot_state_RT.h"
class UrRealtimeCommunication {
class UrRealtimeCommunication
{
private:
unsigned int safety_count_max_;
int sockfd_;
@@ -60,12 +61,10 @@ public:
bool connected_;
RobotStateRT* robot_state_;
UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host,
unsigned int safety_count_max = 12);
UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host, unsigned int safety_count_max = 12);
bool start();
void halt();
void setSpeed(double q0, double q1, double q2, double q3, double q4,
double q5, double acc = 100.);
void setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc = 100.);
void addCommandToQueue(std::string inp);
void setSafetyCountMax(uint inp);
std::string getLocalIp();

131
src/robot_state.cpp
View File

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

47
src/robot_state_RT.cpp
View File

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

55
src/ros/rt_publisher.cpp
View File

@@ -1,27 +1,30 @@
#include "ur_modern_driver/ros/rt_publisher.h"
bool RTPublisher::publish_joints(RTShared& packet, Time& t)
bool RTPublisher::publishJoints(RTShared& packet, Time& t)
{
sensor_msgs::JointState joint_msg;
joint_msg.header.stamp = t;
joint_msg.name = _joint_names;
joint_msg.name = joint_names_;
for (auto const& q : packet.q_actual) {
for (auto const& q : packet.q_actual)
{
joint_msg.position.push_back(q);
}
for (auto const& qd : packet.qd_actual) {
for (auto const& qd : packet.qd_actual)
{
joint_msg.velocity.push_back(qd);
}
for (auto const& i : packet.i_actual) {
for (auto const& i : packet.i_actual)
{
joint_msg.effort.push_back(i);
}
_joint_pub.publish(joint_msg);
joint_pub_.publish(joint_msg);
return true;
}
bool RTPublisher::publish_wrench(RTShared& packet, Time& t)
bool RTPublisher::publishWrench(RTShared& packet, Time& t)
{
geometry_msgs::WrenchStamped wrench_msg;
wrench_msg.header.stamp = t;
@@ -32,15 +35,15 @@ bool RTPublisher::publish_wrench(RTShared& packet, Time& t)
wrench_msg.wrench.torque.y = packet.tcp_force[4];
wrench_msg.wrench.torque.z = packet.tcp_force[5];
_wrench_pub.publish(wrench_msg);
wrench_pub_.publish(wrench_msg);
return true;
}
bool RTPublisher::publish_tool(RTShared& packet, Time& t)
bool RTPublisher::publishTool(RTShared& packet, Time& t)
{
geometry_msgs::TwistStamped tool_twist;
tool_twist.header.stamp = t;
tool_twist.header.frame_id = _base_frame;
tool_twist.header.frame_id = base_frame_;
tool_twist.twist.linear.x = packet.tcp_speed_actual.position.x;
tool_twist.twist.linear.y = packet.tcp_speed_actual.position.y;
tool_twist.twist.linear.z = packet.tcp_speed_actual.position.z;
@@ -48,13 +51,12 @@ bool RTPublisher::publish_tool(RTShared& packet, Time& t)
tool_twist.twist.angular.y = packet.tcp_speed_actual.rotation.y;
tool_twist.twist.angular.z = packet.tcp_speed_actual.rotation.z;
_tool_vel_pub.publish(tool_twist);
tool_vel_pub_.publish(tool_twist);
return true;
}
bool RTPublisher::publish_transform(RTShared& packet, Time& t)
bool RTPublisher::publishTransform(RTShared& packet, Time& t)
{
auto tv = packet.tool_vector_actual;
Transform transform;
@@ -64,29 +66,33 @@ bool RTPublisher::publish_transform(RTShared& packet, Time& t)
Quaternion quat;
double angle = std::sqrt(std::pow(tv.rotation.x, 2) + std::pow(tv.rotation.y, 2) + std::pow(tv.rotation.z, 2));
if (angle < 1e-16) {
if (angle < 1e-16)
{
quat.setValue(0, 0, 0, 1);
} else {
}
else
{
quat.setRotation(Vector3(tv.rotation.x / angle, tv.rotation.y / angle, tv.rotation.z / angle), angle);
}
transform.setRotation(quat);
_transform_broadcaster.sendTransform(StampedTransform(transform, t, _base_frame, _tool_frame));
transform_broadcaster_.sendTransform(StampedTransform(transform, t, base_frame_, tool_frame_));
return true;
}
bool RTPublisher::publish_temperature(RTShared& packet, Time& t)
bool RTPublisher::publishTemperature(RTShared& packet, Time& t)
{
size_t len = joint_names_.size();
for (size_t i = 0; i < len; i++)
{
size_t len = _joint_names.size();
for (size_t i = 0; i < len; i++) {
sensor_msgs::Temperature msg;
msg.header.stamp = t;
msg.header.frame_id = _joint_names[i];
msg.header.frame_id = joint_names_[i];
msg.temperature = packet.motor_temperatures[i];
_joint_temperature_pub.publish(msg);
joint_temperature_pub_.publish(msg);
}
return true;
}
@@ -94,11 +100,8 @@ bool RTPublisher::publish_temperature(RTShared& packet, Time& t)
bool RTPublisher::publish(RTShared& packet)
{
Time time = Time::now();
return publish_joints(packet, time)
&& publish_wrench(packet, time)
&& publish_tool(packet, time)
&& publish_transform(packet, time)
&& publish_temperature(packet, time);
return publishJoints(packet, time) && publishWrench(packet, time) && publishTool(packet, time) &&
publishTransform(packet, time) && publishTemperature(packet, time);
}
bool RTPublisher::consume(RTState_V1_6__7& state)

16
src/ros_main.cpp
View File

@@ -1,6 +1,6 @@
#include <ros/ros.h>
#include <chrono>
#include <cstdlib>
#include <ros/ros.h>
#include <string>
#include <thread>
@@ -21,7 +21,8 @@ static const std::string PREFIX_ARG("~prefix");
static const std::string BASE_FRAME_ARG("~base_frame");
static const std::string TOOL_FRAME_ARG("~tool_frame");
struct ProgArgs {
struct ProgArgs
{
public:
std::string host;
std::string prefix;
@@ -33,7 +34,8 @@ public:
bool parse_args(ProgArgs& args)
{
if (!ros::param::get(IP_ADDR_ARG, args.host)) {
if (!ros::param::get(IP_ADDR_ARG, args.host))
{
LOG_ERROR("robot_ip_address parameter must be set!");
return false;
}
@@ -50,12 +52,13 @@ int main(int argc, char** argv)
ros::init(argc, argv, "ur_driver");
ProgArgs args;
if (!parse_args(args)) {
if (!parse_args(args))
{
return EXIT_FAILURE;
}
URFactory factory(args.host);
auto parser = factory.get_rt_parser();
auto parser = factory.getRTParser();
URStream s2(args.host, 30003);
URProducer<RTPacket> p2(s2, *parser);
@@ -65,7 +68,8 @@ int main(int argc, char** argv)
pl.run();
while (ros::ok()) {
while (ros::ok())
{
std::this_thread::sleep_for(std::chrono::milliseconds(33));
}

36
src/ur/master_board.cpp
View File

@@ -1,7 +1,7 @@
#include "ur_modern_driver/ur/master_board.h"
#include "ur_modern_driver/ur/consumer.h"
bool SharedMasterBoardData::parse_with(BinParser& bp)
bool SharedMasterBoardData::parseWith(BinParser& bp)
{
bp.parse(analog_input_range0);
bp.parse(analog_input_range1);
@@ -18,22 +18,23 @@ bool SharedMasterBoardData::parse_with(BinParser& bp)
return true;
}
bool MasterBoardData_V1_X::parse_with(BinParser& bp)
bool MasterBoardData_V1_X::parseWith(BinParser& bp)
{
if (!bp.check_size<MasterBoardData_V1_X>())
if (!bp.checkSize<MasterBoardData_V1_X>())
return false;
bp.parse(digital_input_bits);
bp.parse(digital_output_bits);
SharedMasterBoardData::parse_with(bp);
SharedMasterBoardData::parseWith(bp);
bp.parse(master_safety_state);
bp.parse(master_on_off_state);
bp.parse(euromap67_interface_installed);
if (euromap67_interface_installed) {
if (!bp.check_size(MasterBoardData_V1_X::EURO_SIZE))
if (euromap67_interface_installed)
{
if (!bp.checkSize(MasterBoardData_V1_X::EURO_SIZE))
return false;
bp.parse(euromap_voltage);
@@ -43,22 +44,23 @@ bool MasterBoardData_V1_X::parse_with(BinParser& bp)
return true;
}
bool MasterBoardData_V3_0__1::parse_with(BinParser& bp)
bool MasterBoardData_V3_0__1::parseWith(BinParser& bp)
{
if (!bp.check_size<MasterBoardData_V3_0__1>())
if (!bp.checkSize<MasterBoardData_V3_0__1>())
return false;
bp.parse(digital_input_bits);
bp.parse(digital_output_bits);
SharedMasterBoardData::parse_with(bp);
SharedMasterBoardData::parseWith(bp);
bp.parse(safety_mode);
bp.parse(in_reduced_mode);
bp.parse(euromap67_interface_installed);
if (euromap67_interface_installed) {
if (!bp.check_size(MasterBoardData_V3_0__1::EURO_SIZE))
if (euromap67_interface_installed)
{
if (!bp.checkSize(MasterBoardData_V3_0__1::EURO_SIZE))
return false;
bp.parse(euromap_voltage);
@@ -70,12 +72,12 @@ bool MasterBoardData_V3_0__1::parse_with(BinParser& bp)
return true;
}
bool MasterBoardData_V3_2::parse_with(BinParser& bp)
bool MasterBoardData_V3_2::parseWith(BinParser& bp)
{
if (!bp.check_size<MasterBoardData_V3_2>())
if (!bp.checkSize<MasterBoardData_V3_2>())
return false;
MasterBoardData_V3_0__1::parse_with(bp);
MasterBoardData_V3_0__1::parseWith(bp);
bp.parse(operational_mode_selector_input);
bp.parse(three_position_enabling_device_input);
@@ -83,15 +85,15 @@ bool MasterBoardData_V3_2::parse_with(BinParser& bp)
return true;
}
bool MasterBoardData_V1_X::consume_with(URStatePacketConsumer& consumer)
bool MasterBoardData_V1_X::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool MasterBoardData_V3_0__1::consume_with(URStatePacketConsumer& consumer)
bool MasterBoardData_V3_0__1::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool MasterBoardData_V3_2::consume_with(URStatePacketConsumer& consumer)
bool MasterBoardData_V3_2::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}

5
src/ur/messages.cpp
View File

@@ -1,9 +1,8 @@
#include "ur_modern_driver/ur/messages.h"
#include "ur_modern_driver/ur/consumer.h"
bool VersionMessage::parse_with(BinParser& bp)
bool VersionMessage::parseWith(BinParser& bp)
{
bp.parse(project_name);
bp.parse(major_version);
bp.parse(minor_version);
@@ -14,7 +13,7 @@ bool VersionMessage::parse_with(BinParser& bp)
return true; // not really possible to check dynamic size packets
}
bool VersionMessage::consume_with(URMessagePacketConsumer& consumer)
bool VersionMessage::consumeWith(URMessagePacketConsumer& consumer)
{
return consumer.consume(*this);
}

26
src/ur/robot_mode.cpp
View File

@@ -1,7 +1,7 @@
#include "ur_modern_driver/ur/robot_mode.h"
#include "ur_modern_driver/ur/consumer.h"
bool SharedRobotModeData::parse_with(BinParser& bp)
bool SharedRobotModeData::parseWith(BinParser& bp)
{
bp.parse(timestamp);
bp.parse(physical_robot_connected);
@@ -11,12 +11,12 @@ bool SharedRobotModeData::parse_with(BinParser& bp)
return true;
}
bool RobotModeData_V1_X::parse_with(BinParser& bp)
bool RobotModeData_V1_X::parseWith(BinParser& bp)
{
if (!bp.check_size<RobotModeData_V1_X>())
if (!bp.checkSize<RobotModeData_V1_X>())
return false;
SharedRobotModeData::parse_with(bp);
SharedRobotModeData::parseWith(bp);
bp.parse(security_stopped);
bp.parse(program_running);
@@ -27,12 +27,12 @@ bool RobotModeData_V1_X::parse_with(BinParser& bp)
return true;
}
bool RobotModeData_V3_0__1::parse_with(BinParser& bp)
bool RobotModeData_V3_0__1::parseWith(BinParser& bp)
{
if (!bp.check_size<RobotModeData_V3_0__1>())
if (!bp.checkSize<RobotModeData_V3_0__1>())
return false;
SharedRobotModeData::parse_with(bp);
SharedRobotModeData::parseWith(bp);
bp.parse(protective_stopped);
bp.parse(program_running);
@@ -45,27 +45,27 @@ bool RobotModeData_V3_0__1::parse_with(BinParser& bp)
return true;
}
bool RobotModeData_V3_2::parse_with(BinParser& bp)
bool RobotModeData_V3_2::parseWith(BinParser& bp)
{
if (!bp.check_size<RobotModeData_V3_2>())
if (!bp.checkSize<RobotModeData_V3_2>())
return false;
RobotModeData_V3_0__1::parse_with(bp);
RobotModeData_V3_0__1::parseWith(bp);
bp.parse(target_speed_fraction_limit);
return true;
}
bool RobotModeData_V1_X::consume_with(URStatePacketConsumer& consumer)
bool RobotModeData_V1_X::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool RobotModeData_V3_0__1::consume_with(URStatePacketConsumer& consumer)
bool RobotModeData_V3_0__1::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool RobotModeData_V3_2::consume_with(URStatePacketConsumer& consumer)
bool RobotModeData_V3_2::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}

28
src/ur/rt_state.cpp
View File

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

10
src/ur/state.cpp
View File

@@ -4,17 +4,17 @@
// StatePacket::~StatePacket() { }
/*
bool RobotState::parse_with(BinParser &bp) {
bool RobotState::parseWith(BinParser &bp) {
//continue as long as there are bytes to read
while(!bp.empty()) {
if(!bp.check_size(sizeof(uint32_t))){
if(!bp.checkSize(sizeof(uint32_t))){
LOG_ERROR("Failed to read sub-package length, there's likely a parsing error");
return false;
}
uint32_t sub_size = bp.peek<uint32_t>();
if(!bp.check_size(static_cast<size_t>(sub_size))) {
if(!bp.checkSize(static_cast<size_t>(sub_size))) {
LOG_WARN("Invalid sub-package size of %" PRIu32 " received!", sub_size);
return false;
}
@@ -43,12 +43,12 @@ bool parse_base(BinParser &bp, T &pkg) {
case package_type::ROBOT_MODE_DATA:
LOG_DEBUG("Parsing robot_mode");
bp.consume(sizeof(package_type));
pkg.robot_mode.parse_with(bp);
pkg.robot_mode.parseWith(bp);
break;
case package_type::MASTERBOARD_DATA:
LOG_DEBUG("Parsing master_board");
bp.consume(sizeof(package_type));
pkg.master_board.parse_with(bp);
pkg.master_board.parseWith(bp);
break;
case package_type::TOOL_DATA:

69
src/ur/stream.cpp
View File

@@ -1,22 +1,22 @@
#include <cstring>
#include <endian.h>
#include <netinet/tcp.h>
#include <unistd.h>
#include <cstring>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ur/stream.h"
bool URStream::connect()
{
if (_initialized)
if (initialized_)
return false;
LOG_INFO("Connecting to UR @ %s:%d", _host.c_str(), _port);
LOG_INFO("Connecting to UR @ %s:%d", host_.c_str(), port_);
// gethostbyname() is deprecated so use getadderinfo() as described in:
// http://www.beej.us/guide/bgnet/output/html/multipage/syscalls.html#getaddrinfo
std::string service = std::to_string(_port);
std::string service = std::to_string(port_);
struct addrinfo hints, *result;
std::memset(&hints, 0, sizeof(hints));
@@ -24,20 +24,23 @@ bool URStream::connect()
hints.ai_socktype = SOCK_STREAM;
hints.ai_flags = AI_PASSIVE;
if (getaddrinfo(_host.c_str(), service.c_str(), &hints, &result) != 0) {
if (getaddrinfo(host_.c_str(), service.c_str(), &hints, &result) != 0)
{
LOG_ERROR("Failed to get host name");
return false;
}
// loop through the list of addresses untill we find one that's connectable
for (struct addrinfo* p = result; p != nullptr; p = p->ai_next) {
_socket_fd = socket(p->ai_family, p->ai_socktype, p->ai_protocol);
for (struct addrinfo* p = result; p != nullptr; p = p->ai_next)
{
socket_fd_ = socket(p->ai_family, p->ai_socktype, p->ai_protocol);
if (_socket_fd == -1) //socket error?
if (socket_fd_ == -1) // socket error?
continue;
if (::connect(_socket_fd, p->ai_addr, p->ai_addrlen) != 0) {
if (_stopping)
if (::connect(socket_fd_, p->ai_addr, p->ai_addrlen) != 0)
{
if (stopping_)
break;
else
continue; // try next addrinfo if connect fails
@@ -45,36 +48,36 @@ bool URStream::connect()
// disable Nagle's algorithm to ensure we sent packets as fast as possible
int flag = 1;
setsockopt(_socket_fd, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(flag));
_initialized = true;
setsockopt(socket_fd_, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(flag));
initialized_ = true;
LOG_INFO("Connection successfully established");
break;
}
freeaddrinfo(result);
if (!_initialized)
if (!initialized_)
LOG_ERROR("Connection failed");
return _initialized;
return initialized_;
}
void URStream::disconnect()
{
if (!_initialized || _stopping)
if (!initialized_ || stopping_)
return;
LOG_INFO("Disconnecting from %s:%d", _host.c_str(), _port);
LOG_INFO("Disconnecting from %s:%d", host_.c_str(), port_);
_stopping = true;
close(_socket_fd);
_initialized = false;
stopping_ = true;
close(socket_fd_);
initialized_ = false;
}
ssize_t URStream::send(uint8_t* buf, size_t buf_len)
{
if (!_initialized)
if (!initialized_)
return -1;
if (_stopping)
if (stopping_)
return 0;
size_t total = 0;
@@ -82,10 +85,11 @@ ssize_t URStream::send(uint8_t* buf, size_t buf_len)
// TODO: handle reconnect?
// handle partial sends
while (total < buf_len) {
ssize_t sent = ::send(_socket_fd, buf + total, remaining, 0);
while (total < buf_len)
{
ssize_t sent = ::send(socket_fd_, buf + total, remaining, 0);
if (sent <= 0)
return _stopping ? 0 : sent;
return stopping_ ? 0 : sent;
total += sent;
remaining -= sent;
}
@@ -95,23 +99,26 @@ ssize_t URStream::send(uint8_t* buf, size_t buf_len)
ssize_t URStream::receive(uint8_t* buf, size_t buf_len)
{
if (!_initialized)
if (!initialized_)
return -1;
if (_stopping)
if (stopping_)
return 0;
size_t remainder = sizeof(int32_t);
uint8_t* buf_pos = buf;
bool initial = true;
do {
ssize_t read = recv(_socket_fd, buf_pos, remainder, 0);
do
{
ssize_t read = recv(socket_fd_, buf_pos, remainder, 0);
if (read <= 0) // failed reading from socket
return _stopping ? 0 : read;
return stopping_ ? 0 : read;
if (initial) {
if (initial)
{
remainder = be32toh(*(reinterpret_cast<int32_t*>(buf)));
if (remainder >= (buf_len - sizeof(int32_t))) {
if (remainder >= (buf_len - sizeof(int32_t)))
{
LOG_ERROR("Packet size %zd is larger than buffer %zu, discarding.", remainder, buf_len);
return -1;
}

97
src/ur_communication.cpp
View File

@@ -18,22 +18,24 @@
#include "ur_modern_driver/ur_communication.h"
UrCommunication::UrCommunication(std::condition_variable& msg_cond,
std::string host)
UrCommunication::UrCommunication(std::condition_variable& msg_cond, std::string host)
{
robot_state_ = new RobotState(msg_cond);
bzero((char*)&pri_serv_addr_, sizeof(pri_serv_addr_));
bzero((char*)&sec_serv_addr_, sizeof(sec_serv_addr_));
pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (pri_sockfd_ < 0) {
if (pri_sockfd_ < 0)
{
print_fatal("ERROR opening socket pri_sockfd");
}
sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sec_sockfd_ < 0) {
if (sec_sockfd_ < 0)
{
print_fatal("ERROR opening socket sec_sockfd");
}
server_ = gethostbyname(host.c_str());
if (server_ == NULL) {
if (server_ == NULL)
{
print_fatal("ERROR, unknown host");
}
pri_serv_addr_.sin_family = AF_INET;
@@ -43,18 +45,12 @@ UrCommunication::UrCommunication(std::condition_variable& msg_cond,
pri_serv_addr_.sin_port = htons(30001);
sec_serv_addr_.sin_port = htons(30002);
flag_ = 1;
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_,
sizeof(int));
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
sizeof(int));
setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_,
sizeof(int));
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_,
sizeof(int));
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
sizeof(int));
setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_,
sizeof(int));
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK);
connected_ = false;
keepalive_ = false;
@@ -68,16 +64,14 @@ bool UrCommunication::start()
std::string cmd;
bzero(buf, 512);
print_debug("Acquire firmware version: Connecting...");
if (connect(pri_sockfd_, (struct sockaddr*)&pri_serv_addr_,
sizeof(pri_serv_addr_))
< 0) {
if (connect(pri_sockfd_, (struct sockaddr*)&pri_serv_addr_, sizeof(pri_serv_addr_)) < 0)
{
print_fatal("Error connecting to get firmware version");
return false;
}
print_debug("Acquire firmware version: Got connection");
bytes_read = read(pri_sockfd_, buf, 512);
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
sizeof(int));
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
robot_state_->unpack(buf, bytes_read);
// wait for some traffic so the UR socket doesn't die in version 3.1.
std::this_thread::sleep_for(std::chrono::milliseconds(500));
@@ -86,14 +80,12 @@ bool UrCommunication::start()
print_debug(tmp);
close(pri_sockfd_);
print_debug(
"Switching to secondary interface for masterboard data: Connecting...");
print_debug("Switching to secondary interface for masterboard data: Connecting...");
fd_set writefds;
struct timeval timeout;
connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_,
sizeof(sec_serv_addr_));
connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_, sizeof(sec_serv_addr_));
FD_ZERO(&writefds);
FD_SET(sec_sockfd_, &writefds);
timeout.tv_sec = 10;
@@ -101,7 +93,8 @@ bool UrCommunication::start()
select(sec_sockfd_ + 1, NULL, &writefds, NULL, &timeout);
unsigned int flag_len;
getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
if (flag_ < 0) {
if (flag_ < 0)
{
print_fatal("Error connecting to secondary interface");
return false;
}
@@ -126,52 +119,58 @@ void UrCommunication::run()
FD_ZERO(&readfds);
FD_SET(sec_sockfd_, &readfds);
connected_ = true;
while (keepalive_) {
while (connected_ && keepalive_) {
while (keepalive_)
{
while (connected_ && keepalive_)
{
timeout.tv_sec = 0; // do this each loop as selects modifies timeout
timeout.tv_usec = 500000; // timeout of 0.5 sec
select(sec_sockfd_ + 1, &readfds, NULL, NULL, &timeout);
bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes
if (bytes_read > 0) {
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK,
(char*)&flag_, sizeof(int));
if (bytes_read > 0)
{
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
robot_state_->unpack(buf, bytes_read);
} else {
}
else
{
connected_ = false;
robot_state_->setDisconnected();
close(sec_sockfd_);
}
}
if (keepalive_) {
if (keepalive_)
{
// reconnect
print_warning("Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...");
sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sec_sockfd_ < 0) {
if (sec_sockfd_ < 0)
{
print_fatal("ERROR opening secondary socket");
}
flag_ = 1;
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_,
sizeof(int));
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
sizeof(int));
setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_,
sizeof(int));
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK);
while (keepalive_ && !connected_) {
while (keepalive_ && !connected_)
{
std::this_thread::sleep_for(std::chrono::seconds(10));
fd_set writefds;
connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_,
sizeof(sec_serv_addr_));
connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_, sizeof(sec_serv_addr_));
FD_ZERO(&writefds);
FD_SET(sec_sockfd_, &writefds);
select(sec_sockfd_ + 1, NULL, &writefds, NULL, NULL);
unsigned int flag_len;
getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_,
&flag_len);
if (flag_ < 0) {
print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds...");
} else {
getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
if (flag_ < 0)
{
print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 "
"seconds...");
}
else
{
connected_ = true;
print_info("Secondary port: Reconnected");
}

194
src/ur_driver.cpp
View File

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

165
src/ur_hardware_interface.cpp
View File

@@ -57,11 +57,9 @@
#include <ur_modern_driver/ur_hardware_interface.h>
namespace ros_control_ur {
UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot)
: nh_(nh)
, robot_(robot)
namespace ros_control_ur
{
UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) : nh_(nh), robot_(robot)
{
// Initialize shared memory and interfaces here
init(); // this implementation loads from rosparam
@@ -73,12 +71,12 @@ UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot)
void UrHardwareInterface::init()
{
ROS_INFO_STREAM_NAMED("ur_hardware_interface",
"Reading rosparams from namespace: " << nh_.getNamespace());
ROS_INFO_STREAM_NAMED("ur_hardware_interface", "Reading rosparams from namespace: " << nh_.getNamespace());
// Get joint names
nh_.getParam("hardware_interface/joints", joint_names_);
if (joint_names_.size() == 0) {
if (joint_names_.size() == 0)
{
ROS_FATAL_STREAM_NAMED("ur_hardware_interface",
"No joints found on parameter server for controller, did you load the proper yaml file?"
<< " Namespace: " << nh_.getNamespace());
@@ -95,34 +93,27 @@ void UrHardwareInterface::init()
prev_joint_velocity_command_.resize(num_joints_);
// Initialize controller
for (std::size_t i = 0; i < num_joints_; ++i) {
ROS_DEBUG_STREAM_NAMED("ur_hardware_interface",
"Loading joint name: " << joint_names_[i]);
for (std::size_t i = 0; i < num_joints_; ++i)
{
ROS_DEBUG_STREAM_NAMED("ur_hardware_interface", "Loading joint name: " << joint_names_[i]);
// Create joint state interface
joint_state_interface_.registerHandle(
hardware_interface::JointStateHandle(joint_names_[i],
&joint_position_[i], &joint_velocity_[i],
&joint_effort_[i]));
joint_state_interface_.registerHandle(hardware_interface::JointStateHandle(joint_names_[i], &joint_position_[i],
&joint_velocity_[i], &joint_effort_[i]));
// Create position joint interface
position_joint_interface_.registerHandle(
hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[i]),
&joint_position_command_[i]));
position_joint_interface_.registerHandle(hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[i]), &joint_position_command_[i]));
// Create velocity joint interface
velocity_joint_interface_.registerHandle(
hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[i]),
&joint_velocity_command_[i]));
velocity_joint_interface_.registerHandle(hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[i]), &joint_velocity_command_[i]));
prev_joint_velocity_command_[i] = 0.;
}
// Create force torque interface
force_torque_interface_.registerHandle(
hardware_interface::ForceTorqueSensorHandle("wrench", "",
robot_force_, robot_torque_));
hardware_interface::ForceTorqueSensorHandle("wrench", "", robot_force_, robot_torque_));
registerInterface(&joint_state_interface_); // From RobotHW base class.
registerInterface(&position_joint_interface_); // From RobotHW base class.
@@ -139,12 +130,14 @@ void UrHardwareInterface::read()
vel = robot_->rt_interface_->robot_state_->getQdActual();
current = robot_->rt_interface_->robot_state_->getIActual();
tcp = robot_->rt_interface_->robot_state_->getTcpForce();
for (std::size_t i = 0; i < num_joints_; ++i) {
for (std::size_t i = 0; i < num_joints_; ++i)
{
joint_position_[i] = pos[i];
joint_velocity_[i] = vel[i];
joint_effort_[i] = current[i];
}
for (std::size_t i = 0; i < 3; ++i) {
for (std::size_t i = 0; i < 3; ++i)
{
robot_force_[i] = tcp[i];
robot_torque_[i] = tcp[i + 3];
}
@@ -157,85 +150,90 @@ void UrHardwareInterface::setMaxVelChange(double inp)
void UrHardwareInterface::write()
{
if (velocity_interface_running_) {
if (velocity_interface_running_)
{
std::vector<double> cmd;
// do some rate limiting
cmd.resize(joint_velocity_command_.size());
for (unsigned int i = 0; i < joint_velocity_command_.size(); i++) {
for (unsigned int i = 0; i < joint_velocity_command_.size(); i++)
{
cmd[i] = joint_velocity_command_[i];
if (cmd[i] > prev_joint_velocity_command_[i] + max_vel_change_) {
if (cmd[i] > prev_joint_velocity_command_[i] + max_vel_change_)
{
cmd[i] = prev_joint_velocity_command_[i] + max_vel_change_;
} else if (cmd[i]
< prev_joint_velocity_command_[i] - max_vel_change_) {
}
else if (cmd[i] < prev_joint_velocity_command_[i] - max_vel_change_)
{
cmd[i] = prev_joint_velocity_command_[i] - max_vel_change_;
}
prev_joint_velocity_command_[i] = cmd[i];
}
robot_->setSpeed(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], max_vel_change_ * 125);
} else if (position_interface_running_) {
}
else if (position_interface_running_)
{
robot_->servoj(joint_position_command_);
}
}
bool UrHardwareInterface::canSwitch(
const std::list<hardware_interface::ControllerInfo>& start_list,
bool UrHardwareInterface::canSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list) const
{
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin(); controller_it != start_list.end();
++controller_it) {
if (controller_it->hardware_interface
== "hardware_interface::VelocityJointInterface") {
if (velocity_interface_running_) {
ROS_ERROR(
"%s: An interface of that type (%s) is already running",
controller_it->name.c_str(),
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin();
controller_it != start_list.end(); ++controller_it)
{
if (controller_it->hardware_interface == "hardware_interface::VelocityJointInterface")
{
if (velocity_interface_running_)
{
ROS_ERROR("%s: An interface of that type (%s) is already running", controller_it->name.c_str(),
controller_it->hardware_interface.c_str());
return false;
}
if (position_interface_running_) {
if (position_interface_running_)
{
bool error = true;
for (std::list<hardware_interface::ControllerInfo>::const_iterator stop_controller_it = stop_list.begin();
stop_controller_it != stop_list.end();
++stop_controller_it) {
if (stop_controller_it->hardware_interface
== "hardware_interface::PositionJointInterface") {
stop_controller_it != stop_list.end(); ++stop_controller_it)
{
if (stop_controller_it->hardware_interface == "hardware_interface::PositionJointInterface")
{
error = false;
break;
}
}
if (error) {
ROS_ERROR(
"%s (type %s) can not be run simultaneously with a PositionJointInterface",
controller_it->name.c_str(),
controller_it->hardware_interface.c_str());
if (error)
{
ROS_ERROR("%s (type %s) can not be run simultaneously with a PositionJointInterface",
controller_it->name.c_str(), controller_it->hardware_interface.c_str());
return false;
}
}
} else if (controller_it->hardware_interface
== "hardware_interface::PositionJointInterface") {
if (position_interface_running_) {
ROS_ERROR(
"%s: An interface of that type (%s) is already running",
controller_it->name.c_str(),
}
else if (controller_it->hardware_interface == "hardware_interface::PositionJointInterface")
{
if (position_interface_running_)
{
ROS_ERROR("%s: An interface of that type (%s) is already running", controller_it->name.c_str(),
controller_it->hardware_interface.c_str());
return false;
}
if (velocity_interface_running_) {
if (velocity_interface_running_)
{
bool error = true;
for (std::list<hardware_interface::ControllerInfo>::const_iterator stop_controller_it = stop_list.begin();
stop_controller_it != stop_list.end();
++stop_controller_it) {
if (stop_controller_it->hardware_interface
== "hardware_interface::VelocityJointInterface") {
stop_controller_it != stop_list.end(); ++stop_controller_it)
{
if (stop_controller_it->hardware_interface == "hardware_interface::VelocityJointInterface")
{
error = false;
break;
}
}
if (error) {
ROS_ERROR(
"%s (type %s) can not be run simultaneously with a VelocityJointInterface",
controller_it->name.c_str(),
controller_it->hardware_interface.c_str());
if (error)
{
ROS_ERROR("%s (type %s) can not be run simultaneously with a VelocityJointInterface",
controller_it->name.c_str(), controller_it->hardware_interface.c_str());
return false;
}
}
@@ -246,34 +244,35 @@ bool UrHardwareInterface::canSwitch(
return true;
}
void UrHardwareInterface::doSwitch(
const std::list<hardware_interface::ControllerInfo>& start_list,
void UrHardwareInterface::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list)
{
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = stop_list.begin(); controller_it != stop_list.end();
++controller_it) {
if (controller_it->hardware_interface
== "hardware_interface::VelocityJointInterface") {
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = stop_list.begin();
controller_it != stop_list.end(); ++controller_it)
{
if (controller_it->hardware_interface == "hardware_interface::VelocityJointInterface")
{
velocity_interface_running_ = false;
ROS_DEBUG("Stopping velocity interface");
}
if (controller_it->hardware_interface
== "hardware_interface::PositionJointInterface") {
if (controller_it->hardware_interface == "hardware_interface::PositionJointInterface")
{
position_interface_running_ = false;
std::vector<double> tmp;
robot_->closeServo(tmp);
ROS_DEBUG("Stopping position interface");
}
}
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin(); controller_it != start_list.end();
++controller_it) {
if (controller_it->hardware_interface
== "hardware_interface::VelocityJointInterface") {
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin();
controller_it != start_list.end(); ++controller_it)
{
if (controller_it->hardware_interface == "hardware_interface::VelocityJointInterface")
{
velocity_interface_running_ = true;
ROS_DEBUG("Starting velocity interface");
}
if (controller_it->hardware_interface
== "hardware_interface::PositionJointInterface") {
if (controller_it->hardware_interface == "hardware_interface::PositionJointInterface")
{
position_interface_running_ = true;
robot_->uploadProg();
ROS_DEBUG("Starting position interface");

89
src/ur_realtime_communication.cpp
View File

@@ -18,18 +18,19 @@
#include "ur_modern_driver/ur_realtime_communication.h"
UrRealtimeCommunication::UrRealtimeCommunication(
std::condition_variable& msg_cond, std::string host,
UrRealtimeCommunication::UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host,
unsigned int safety_count_max)
{
robot_state_ = new RobotStateRT(msg_cond);
bzero((char*)&serv_addr_, sizeof(serv_addr_));
sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sockfd_ < 0) {
if (sockfd_ < 0)
{
print_fatal("ERROR opening socket");
}
server_ = gethostbyname(host.c_str());
if (server_ == NULL) {
if (server_ == NULL)
{
print_fatal("ERROR, no such host");
}
serv_addr_.sin_family = AF_INET;
@@ -62,14 +63,16 @@ bool UrRealtimeCommunication::start()
select(sockfd_ + 1, NULL, &writefds, NULL, &timeout);
unsigned int flag_len;
getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
if (flag_ < 0) {
if (flag_ < 0)
{
print_fatal("Error connecting to RT port 30003");
return false;
}
sockaddr_in name;
socklen_t namelen = sizeof(name);
int err = getsockname(sockfd_, (sockaddr*)&name, &namelen);
if (err < 0) {
if (err < 0)
{
print_fatal("Could not get local IP");
close(sockfd_);
return false;
@@ -90,7 +93,8 @@ void UrRealtimeCommunication::halt()
void UrRealtimeCommunication::addCommandToQueue(std::string inp)
{
int bytes_written;
if (inp.back() != '\n') {
if (inp.back() != '\n')
{
inp.append("\n");
}
if (connected_)
@@ -99,21 +103,20 @@ void UrRealtimeCommunication::addCommandToQueue(std::string inp)
print_error("Could not send command \"" + inp + "\". The robot is not connected! Command is discarded");
}
void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2,
double q3, double q4, double q5, double acc)
void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc)
{
char cmd[1024];
if (robot_state_->getVersion() >= 3.1) {
sprintf(cmd,
"speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n",
q0, q1, q2, q3, q4, q5, acc);
} else {
sprintf(cmd,
"speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n",
q0, q1, q2, q3, q4, q5, acc);
if (robot_state_->getVersion() >= 3.1)
{
sprintf(cmd, "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n", q0, q1, q2, q3, q4, q5, acc);
}
else
{
sprintf(cmd, "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n", q0, q1, q2, q3, q4, q5, acc);
}
addCommandToQueue((std::string)(cmd));
if (q0 != 0. or q1 != 0. or q2 != 0. or q3 != 0. or q4 != 0. or q5 != 0.) {
if (q0 != 0. or q1 != 0. or q2 != 0. or q3 != 0. or q4 != 0. or q5 != 0.)
{
// If a joint speed is set, make sure we stop it again after some time if the user doesn't
safety_count_ = 0;
}
@@ -130,55 +133,63 @@ void UrRealtimeCommunication::run()
FD_SET(sockfd_, &readfds);
print_debug("Realtime port: Got connection");
connected_ = true;
while (keepalive_) {
while (connected_ && keepalive_) {
while (keepalive_)
{
while (connected_ && keepalive_)
{
timeout.tv_sec = 0; // do this each loop as selects modifies timeout
timeout.tv_usec = 500000; // timeout of 0.5 sec
select(sockfd_ + 1, &readfds, NULL, NULL, &timeout);
bytes_read = read(sockfd_, buf, 2048);
if (bytes_read > 0) {
setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
sizeof(int));
if (bytes_read > 0)
{
setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
robot_state_->unpack(buf);
if (safety_count_ == safety_count_max_) {
if (safety_count_ == safety_count_max_)
{
setSpeed(0., 0., 0., 0., 0., 0.);
}
safety_count_ += 1;
} else {
}
else
{
connected_ = false;
close(sockfd_);
}
}
if (keepalive_) {
if (keepalive_)
{
// reconnect
print_warning("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...");
sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sockfd_ < 0) {
if (sockfd_ < 0)
{
print_fatal("ERROR opening socket");
}
flag_ = 1;
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_,
sizeof(int));
setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
sizeof(int));
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_,
sizeof(int));
setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
fcntl(sockfd_, F_SETFL, O_NONBLOCK);
while (keepalive_ && !connected_) {
while (keepalive_ && !connected_)
{
std::this_thread::sleep_for(std::chrono::seconds(10));
fd_set writefds;
connect(sockfd_, (struct sockaddr*)&serv_addr_,
sizeof(serv_addr_));
connect(sockfd_, (struct sockaddr*)&serv_addr_, sizeof(serv_addr_));
FD_ZERO(&writefds);
FD_SET(sockfd_, &writefds);
select(sockfd_ + 1, NULL, &writefds, NULL, NULL);
unsigned int flag_len;
getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
if (flag_ < 0) {
print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds...");
} else {
if (flag_ < 0)
{
print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 "
"seconds...");
}
else
{
connected_ = true;
print_info("Realtime port: Reconnected");
}

442
src/ur_ros_wrapper.cpp
View File

@@ -16,19 +16,22 @@
* limitations under the License.
*/
#include "ur_modern_driver/do_output.h"
#include "ur_modern_driver/ur_driver.h"
#include "ur_modern_driver/ur_hardware_interface.h"
#include <string.h>
#include <time.h>
#include <algorithm>
#include <chrono>
#include <cmath>
#include <condition_variable>
#include <mutex>
#include <string.h>
#include <thread>
#include <time.h>
#include <vector>
#include "ur_modern_driver/do_output.h"
#include "ur_modern_driver/ur_driver.h"
#include "ur_modern_driver/ur_hardware_interface.h"
#include <controller_manager/controller_manager.h>
#include <realtime_tools/realtime_publisher.h>
#include <ros/console.h>
#include "actionlib/server/action_server.h"
#include "actionlib/server/server_goal_handle.h"
#include "control_msgs/FollowJointTrajectoryAction.h"
@@ -47,15 +50,13 @@
#include "ur_msgs/SetPayload.h"
#include "ur_msgs/SetPayloadRequest.h"
#include "ur_msgs/SetPayloadResponse.h"
#include <controller_manager/controller_manager.h>
#include <realtime_tools/realtime_publisher.h>
#include <ros/console.h>
/// TF
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
class RosWrapper {
class RosWrapper
{
protected:
UrDriver robot_;
std::condition_variable rt_msg_cond_;
@@ -84,22 +85,20 @@ protected:
public:
RosWrapper(std::string host, int reverse_port)
: as_(nh_, "follow_joint_trajectory",
boost::bind(&RosWrapper::goalCB, this, _1),
: as_(nh_, "follow_joint_trajectory", boost::bind(&RosWrapper::goalCB, this, _1),
boost::bind(&RosWrapper::cancelCB, this, _1), false)
, robot_(
rt_msg_cond_, msg_cond_, host, reverse_port, 0.03, 300)
, robot_(rt_msg_cond_, msg_cond_, host, reverse_port, 0.03, 300)
, io_flag_delay_(0.05)
, joint_offsets_(
6, 0.0)
, joint_offsets_(6, 0.0)
{
std::string joint_prefix = "";
std::vector<std::string> joint_names;
char buf[256];
if (ros::param::get("~prefix", joint_prefix)) {
if (joint_prefix.length() > 0) {
if (ros::param::get("~prefix", joint_prefix))
{
if (joint_prefix.length() > 0)
{
sprintf(buf, "Setting prefix to %s", joint_prefix.c_str());
print_info(buf);
}
@@ -115,26 +114,24 @@ public:
use_ros_control_ = false;
ros::param::get("~use_ros_control", use_ros_control_);
if (use_ros_control_) {
hardware_interface_.reset(
new ros_control_ur::UrHardwareInterface(nh_, &robot_));
controller_manager_.reset(
new controller_manager::ControllerManager(
hardware_interface_.get(), nh_));
if (use_ros_control_)
{
hardware_interface_.reset(new ros_control_ur::UrHardwareInterface(nh_, &robot_));
controller_manager_.reset(new controller_manager::ControllerManager(hardware_interface_.get(), nh_));
double max_vel_change = 0.12; // equivalent of an acceleration of 15 rad/sec^2
if (ros::param::get("~max_acceleration", max_vel_change)) {
if (ros::param::get("~max_acceleration", max_vel_change))
{
max_vel_change = max_vel_change / 125;
}
sprintf(buf, "Max acceleration set to: %f [rad/sec²]",
max_vel_change * 125);
sprintf(buf, "Max acceleration set to: %f [rad/sec²]", max_vel_change * 125);
print_debug(buf);
hardware_interface_->setMaxVelChange(max_vel_change);
}
// Using a very high value in order to not limit execution of trajectories being sent from MoveIt!
max_velocity_ = 10.;
if (ros::param::get("~max_velocity", max_velocity_)) {
sprintf(buf, "Max velocity accepted by ur_driver: %f [rad/s]",
max_velocity_);
if (ros::param::get("~max_velocity", max_velocity_))
{
sprintf(buf, "Max velocity accepted by ur_driver: %f [rad/s]", max_velocity_);
print_debug(buf);
}
@@ -142,36 +139,40 @@ public:
// Using a very conservative value as it should be set through the parameter server
double min_payload = 0.;
double max_payload = 1.;
if (ros::param::get("~min_payload", min_payload)) {
if (ros::param::get("~min_payload", min_payload))
{
sprintf(buf, "Min payload set to: %f [kg]", min_payload);
print_debug(buf);
}
if (ros::param::get("~max_payload", max_payload)) {
if (ros::param::get("~max_payload", max_payload))
{
sprintf(buf, "Max payload set to: %f [kg]", max_payload);
print_debug(buf);
}
robot_.setMinPayload(min_payload);
robot_.setMaxPayload(max_payload);
sprintf(buf, "Bounds for set_payload service calls: [%f, %f]",
min_payload, max_payload);
sprintf(buf, "Bounds for set_payload service calls: [%f, %f]", min_payload, max_payload);
print_debug(buf);
double servoj_time = 0.008;
if (ros::param::get("~servoj_time", servoj_time)) {
if (ros::param::get("~servoj_time", servoj_time))
{
sprintf(buf, "Servoj_time set to: %f [sec]", servoj_time);
print_debug(buf);
}
robot_.setServojTime(servoj_time);
double servoj_lookahead_time = 0.03;
if (ros::param::get("~servoj_lookahead_time", servoj_lookahead_time)) {
if (ros::param::get("~servoj_lookahead_time", servoj_lookahead_time))
{
sprintf(buf, "Servoj_lookahead_time set to: %f [sec]", servoj_lookahead_time);
print_debug(buf);
}
robot_.setServojLookahead(servoj_lookahead_time);
double servoj_gain = 300.;
if (ros::param::get("~servoj_gain", servoj_gain)) {
if (ros::param::get("~servoj_gain", servoj_gain))
{
sprintf(buf, "Servoj_gain set to: %f [sec]", servoj_gain);
print_debug(buf);
}
@@ -180,43 +181,40 @@ public:
// Base and tool frames
base_frame_ = joint_prefix + "base_link";
tool_frame_ = joint_prefix + "tool0_controller";
if (ros::param::get("~base_frame", base_frame_)) {
if (ros::param::get("~base_frame", base_frame_))
{
sprintf(buf, "Base frame set to: %s", base_frame_.c_str());
print_debug(buf);
}
if (ros::param::get("~tool_frame", tool_frame_)) {
if (ros::param::get("~tool_frame", tool_frame_))
{
sprintf(buf, "Tool frame set to: %s", tool_frame_.c_str());
print_debug(buf);
}
if (robot_.start()) {
if (use_ros_control_) {
ros_control_thread_ = new std::thread(
boost::bind(&RosWrapper::rosControlLoop, this));
print_debug(
"The control thread for this driver has been started");
} else {
if (robot_.start())
{
if (use_ros_control_)
{
ros_control_thread_ = new std::thread(boost::bind(&RosWrapper::rosControlLoop, this));
print_debug("The control thread for this driver has been started");
}
else
{
// start actionserver
has_goal_ = false;
as_.start();
// subscribe to the data topic of interest
rt_publish_thread_ = new std::thread(
boost::bind(&RosWrapper::publishRTMsg, this));
print_debug(
"The action server for this driver has been started");
rt_publish_thread_ = new std::thread(boost::bind(&RosWrapper::publishRTMsg, this));
print_debug("The action server for this driver has been started");
}
mb_publish_thread_ = new std::thread(
boost::bind(&RosWrapper::publishMbMsg, this));
speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1,
&RosWrapper::speedInterface, this);
urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1,
&RosWrapper::urscriptInterface, this);
mb_publish_thread_ = new std::thread(boost::bind(&RosWrapper::publishMbMsg, this));
speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1, &RosWrapper::speedInterface, this);
urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, &RosWrapper::urscriptInterface, this);
io_srv_ = nh_.advertiseService("ur_driver/set_io",
&RosWrapper::setIO, this);
payload_srv_ = nh_.advertiseService("ur_driver/set_payload",
&RosWrapper::setPayload, this);
io_srv_ = nh_.advertiseService("ur_driver/set_io", &RosWrapper::setIO, this);
payload_srv_ = nh_.advertiseService("ur_driver/set_payload", &RosWrapper::setPayload, this);
}
}
@@ -227,54 +225,55 @@ public:
}
private:
void trajThread(std::vector<double> timestamps,
std::vector<std::vector<double> > positions,
void trajThread(std::vector<double> timestamps, std::vector<std::vector<double>> positions,
std::vector<std::vector<double>> velocities)
{
robot_.doTraj(timestamps, positions, velocities);
if (has_goal_) {
if (has_goal_)
{
result_.error_code = result_.SUCCESSFUL;
goal_handle_.setSucceeded(result_);
has_goal_ = false;
}
}
void goalCB(
actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> gh)
void goalCB(actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> gh)
{
std::string buf;
print_info("on_goal");
if (!robot_.sec_interface_->robot_state_->isReady()) {
if (!robot_.sec_interface_->robot_state_->isReady())
{
result_.error_code = -100; // nothing is defined for this...?
if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) {
if (!robot_.sec_interface_->robot_state_->isPowerOnRobot())
{
result_.error_string = "Cannot accept new trajectories: Robot arm is not powered on";
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
if (!robot_.sec_interface_->robot_state_->isRealRobotEnabled()) {
if (!robot_.sec_interface_->robot_state_->isRealRobotEnabled())
{
result_.error_string = "Cannot accept new trajectories: Robot is not enabled";
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
result_.error_string = "Cannot accept new trajectories. (Debug: Robot mode is "
+ std::to_string(
robot_.sec_interface_->robot_state_->getRobotMode())
+ ")";
result_.error_string = "Cannot accept new trajectories. (Debug: Robot mode is " +
std::to_string(robot_.sec_interface_->robot_state_->getRobotMode()) + ")";
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
if (robot_.sec_interface_->robot_state_->isEmergencyStopped()) {
if (robot_.sec_interface_->robot_state_->isEmergencyStopped())
{
result_.error_code = -100; // nothing is defined for this...?
result_.error_string = "Cannot accept new trajectories: Robot is emergency stopped";
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
if (robot_.sec_interface_->robot_state_->isProtectiveStopped()) {
if (robot_.sec_interface_->robot_state_->isProtectiveStopped())
{
result_.error_code = -100; // nothing is defined for this...?
result_.error_string = "Cannot accept new trajectories: Robot is protective stopped";
gh.setRejected(result_, result_.error_string);
@@ -282,10 +281,11 @@ private:
return;
}
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *gh.getGoal(); //make a copy that we can modify
if (has_goal_) {
print_warning(
"Received new goal while still executing previous trajectory. Canceling previous trajectory");
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
*gh.getGoal(); // make a copy that we can modify
if (has_goal_)
{
print_warning("Received new goal while still executing previous trajectory. Canceling previous trajectory");
has_goal_ = false;
robot_.stopTraj();
result_.error_code = -100; // nothing is defined for this...?
@@ -294,20 +294,21 @@ private:
std::this_thread::sleep_for(std::chrono::milliseconds(250));
}
goal_handle_ = gh;
if (!validateJointNames()) {
if (!validateJointNames())
{
std::string outp_joint_names = "";
for (unsigned int i = 0; i < goal.trajectory.joint_names.size();
i++) {
for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++)
{
outp_joint_names += goal.trajectory.joint_names[i] + " ";
}
result_.error_code = result_.INVALID_JOINTS;
result_.error_string = "Received a goal with incorrect joint names: "
+ outp_joint_names;
result_.error_string = "Received a goal with incorrect joint names: " + outp_joint_names;
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
if (!has_positions()) {
if (!has_positions())
{
result_.error_code = result_.INVALID_GOAL;
result_.error_string = "Received a goal without positions";
gh.setRejected(result_, result_.error_string);
@@ -315,7 +316,8 @@ private:
return;
}
if (!has_velocities()) {
if (!has_velocities())
{
result_.error_code = result_.INVALID_GOAL;
result_.error_string = "Received a goal without velocities";
gh.setRejected(result_, result_.error_string);
@@ -323,7 +325,8 @@ private:
return;
}
if (!traj_is_finite()) {
if (!traj_is_finite())
{
result_.error_string = "Received a goal with infinities or NaNs";
result_.error_code = result_.INVALID_GOAL;
gh.setRejected(result_, result_.error_string);
@@ -331,10 +334,10 @@ private:
return;
}
if (!has_limited_velocities()) {
if (!has_limited_velocities())
{
result_.error_code = result_.INVALID_GOAL;
result_.error_string = "Received a goal with velocities that are higher than "
+ std::to_string(max_velocity_);
result_.error_string = "Received a goal with velocities that are higher than " + std::to_string(max_velocity_);
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
@@ -342,7 +345,8 @@ private:
reorder_traj_joints(goal.trajectory);
if (!start_positions_match(goal.trajectory, 0.01)) {
if (!start_positions_match(goal.trajectory, 0.01))
{
result_.error_code = result_.INVALID_GOAL;
result_.error_string = "Goal start doesn't match current pose";
gh.setRejected(result_, result_.error_string);
@@ -352,36 +356,34 @@ private:
std::vector<double> timestamps;
std::vector<std::vector<double>> positions, velocities;
if (goal.trajectory.points[0].time_from_start.toSec() != 0.) {
print_warning(
"Trajectory's first point should be the current position, with time_from_start set to 0.0 - Inserting point in malformed trajectory");
if (goal.trajectory.points[0].time_from_start.toSec() != 0.)
{
print_warning("Trajectory's first point should be the current position, with time_from_start set to 0.0 - "
"Inserting point in malformed trajectory");
timestamps.push_back(0.0);
positions.push_back(
robot_.rt_interface_->robot_state_->getQActual());
velocities.push_back(
robot_.rt_interface_->robot_state_->getQdActual());
positions.push_back(robot_.rt_interface_->robot_state_->getQActual());
velocities.push_back(robot_.rt_interface_->robot_state_->getQdActual());
}
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
timestamps.push_back(
goal.trajectory.points[i].time_from_start.toSec());
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++)
{
timestamps.push_back(goal.trajectory.points[i].time_from_start.toSec());
positions.push_back(goal.trajectory.points[i].positions);
velocities.push_back(goal.trajectory.points[i].velocities);
}
goal_handle_.setAccepted();
has_goal_ = true;
std::thread(&RosWrapper::trajThread, this, timestamps, positions,
velocities)
.detach();
std::thread(&RosWrapper::trajThread, this, timestamps, positions, velocities).detach();
}
void cancelCB(
actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> gh)
void cancelCB(actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> gh)
{
// set the action state to preempted
print_info("on_cancel");
if (has_goal_) {
if (gh == goal_handle_) {
if (has_goal_)
{
if (gh == goal_handle_)
{
robot_.stopTraj();
has_goal_ = false;
}
@@ -395,27 +397,35 @@ private:
{
resp.success = true;
// if (req.fun == ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT) {
if (req.fun == 1) {
if (req.fun == 1)
{
robot_.setDigitalOut(req.pin, req.state > 0.0 ? true : false);
} else if (req.fun == 2) {
}
else if (req.fun == 2)
{
//} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_FLAG) {
robot_.setFlag(req.pin, req.state > 0.0 ? true : false);
// According to urdriver.py, set_flag will fail if called to rapidly in succession
ros::Duration(io_flag_delay_).sleep();
} else if (req.fun == 3) {
}
else if (req.fun == 3)
{
//} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_ANALOG_OUT) {
robot_.setAnalogOut(req.pin, req.state);
} else if (req.fun == 4) {
}
else if (req.fun == 4)
{
//} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_TOOL_VOLTAGE) {
robot_.setToolVoltage((int)req.state);
} else {
}
else
{
resp.success = false;
}
return resp.success;
}
bool setPayload(ur_msgs::SetPayloadRequest& req,
ur_msgs::SetPayloadResponse& resp)
bool setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::SetPayloadResponse& resp)
{
if (robot_.setPayload(req.payload))
resp.success = true;
@@ -431,15 +441,20 @@ private:
if (goal.trajectory.joint_names.size() != actual_joint_names.size())
return false;
for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) {
for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++)
{
unsigned int j;
for (j = 0; j < actual_joint_names.size(); j++) {
for (j = 0; j < actual_joint_names.size(); j++)
{
if (goal.trajectory.joint_names[i] == actual_joint_names[j])
break;
}
if (goal.trajectory.joint_names[i] == actual_joint_names[j]) {
if (goal.trajectory.joint_names[i] == actual_joint_names[j])
{
actual_joint_names.erase(actual_joint_names.begin() + j);
} else {
}
else
{
return false;
}
}
@@ -453,24 +468,25 @@ private:
std::vector<std::string> actual_joint_names = robot_.getJointNames();
std::vector<unsigned int> mapping;
mapping.resize(actual_joint_names.size(), actual_joint_names.size());
for (unsigned int i = 0; i < traj.joint_names.size(); i++) {
for (unsigned int j = 0; j < actual_joint_names.size(); j++) {
for (unsigned int i = 0; i < traj.joint_names.size(); i++)
{
for (unsigned int j = 0; j < actual_joint_names.size(); j++)
{
if (traj.joint_names[i] == actual_joint_names[j])
mapping[j] = i;
}
}
traj.joint_names = actual_joint_names;
std::vector<trajectory_msgs::JointTrajectoryPoint> new_traj;
for (unsigned int i = 0; i < traj.points.size(); i++) {
for (unsigned int i = 0; i < traj.points.size(); i++)
{
trajectory_msgs::JointTrajectoryPoint new_point;
for (unsigned int j = 0; j < traj.points[i].positions.size(); j++) {
new_point.positions.push_back(
traj.points[i].positions[mapping[j]]);
new_point.velocities.push_back(
traj.points[i].velocities[mapping[j]]);
for (unsigned int j = 0; j < traj.points[i].positions.size(); j++)
{
new_point.positions.push_back(traj.points[i].positions[mapping[j]]);
new_point.velocities.push_back(traj.points[i].velocities[mapping[j]]);
if (traj.points[i].accelerations.size() != 0)
new_point.accelerations.push_back(
traj.points[i].accelerations[mapping[j]]);
new_point.accelerations.push_back(traj.points[i].accelerations[mapping[j]]);
}
new_point.time_from_start = traj.points[i].time_from_start;
new_traj.push_back(new_point);
@@ -481,9 +497,9 @@ private:
bool has_velocities()
{
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_.getGoal();
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
if (goal.trajectory.points[i].positions.size()
!= goal.trajectory.points[i].velocities.size())
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++)
{
if (goal.trajectory.points[i].positions.size() != goal.trajectory.points[i].velocities.size())
return false;
}
return true;
@@ -494,9 +510,9 @@ private:
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_.getGoal();
if (goal.trajectory.points.size() == 0)
return false;
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
if (goal.trajectory.points[i].positions.size()
!= goal.trajectory.joint_names.size())
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++)
{
if (goal.trajectory.points[i].positions.size() != goal.trajectory.joint_names.size())
return false;
}
return true;
@@ -504,9 +520,11 @@ private:
bool start_positions_match(const trajectory_msgs::JointTrajectory& traj, double eps)
{
for (unsigned int i = 0; i < traj.points[0].positions.size(); i++) {
for (unsigned int i = 0; i < traj.points[0].positions.size(); i++)
{
std::vector<double> qActual = robot_.rt_interface_->robot_state_->getQActual();
if (fabs(traj.points[0].positions[i] - qActual[i]) > eps) {
if (fabs(traj.points[0].positions[i] - qActual[i]) > eps)
{
return false;
}
}
@@ -516,11 +534,11 @@ private:
bool has_limited_velocities()
{
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_.getGoal();
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
for (unsigned int j = 0;
j < goal.trajectory.points[i].velocities.size(); j++) {
if (fabs(goal.trajectory.points[i].velocities[j])
> max_velocity_)
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++)
{
for (unsigned int j = 0; j < goal.trajectory.points[i].velocities.size(); j++)
{
if (fabs(goal.trajectory.points[i].velocities[j]) > max_velocity_)
return false;
}
}
@@ -530,9 +548,10 @@ private:
bool traj_is_finite()
{
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_.getGoal();
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
for (unsigned int j = 0;
j < goal.trajectory.points[i].velocities.size(); j++) {
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++)
{
for (unsigned int j = 0; j < goal.trajectory.points[i].velocities.size(); j++)
{
if (!std::isfinite(goal.trajectory.points[i].positions[j]))
return false;
if (!std::isfinite(goal.trajectory.points[i].velocities[j]))
@@ -544,20 +563,17 @@ private:
void speedInterface(const trajectory_msgs::JointTrajectory::Ptr& msg)
{
if (msg->points[0].velocities.size() == 6) {
if (msg->points[0].velocities.size() == 6)
{
double acc = 100;
if (msg->points[0].accelerations.size() > 0)
acc = *std::max_element(msg->points[0].accelerations.begin(),
msg->points[0].accelerations.end());
robot_.setSpeed(msg->points[0].velocities[0],
msg->points[0].velocities[1], msg->points[0].velocities[2],
msg->points[0].velocities[3], msg->points[0].velocities[4],
msg->points[0].velocities[5], acc);
acc = *std::max_element(msg->points[0].accelerations.begin(), msg->points[0].accelerations.end());
robot_.setSpeed(msg->points[0].velocities[0], msg->points[0].velocities[1], msg->points[0].velocities[2],
msg->points[0].velocities[3], msg->points[0].velocities[4], msg->points[0].velocities[5], acc);
}
}
void urscriptInterface(const std_msgs::String::ConstPtr& msg)
{
robot_.rt_interface_->addCommandToQueue(msg->data);
}
@@ -577,10 +593,12 @@ private:
tool_vel_pub.msg_.header.frame_id = base_frame_;
clock_gettime(CLOCK_MONOTONIC, &last_time);
while (ros::ok()) {
while (ros::ok())
{
std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex
std::unique_lock<std::mutex> locker(msg_lock);
while (!robot_.rt_interface_->robot_state_->getControllerUpdated()) {
while (!robot_.rt_interface_->robot_state_->getControllerUpdated())
{
rt_msg_cond_.wait(locker);
}
// Input
@@ -589,7 +607,8 @@ private:
// Control
clock_gettime(CLOCK_MONOTONIC, &current_time);
elapsed_time = ros::Duration(current_time.tv_sec - last_time.tv_sec + (current_time.tv_nsec - last_time.tv_nsec) / BILLION);
elapsed_time =
ros::Duration(current_time.tv_sec - last_time.tv_sec + (current_time.tv_nsec - last_time.tv_nsec) / BILLION);
ros::Time ros_time = ros::Time::now();
controller_manager_->update(ros_time, elapsed_time);
last_time = current_time;
@@ -597,7 +616,8 @@ private:
// Output
hardware_interface_->write();
// Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation
// Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation
// vector representation of the tool orientation
std::vector<double> tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual();
// Compute rotation angle
@@ -607,14 +627,18 @@ private:
double angle = std::sqrt(std::pow(rx, 2) + std::pow(ry, 2) + std::pow(rz, 2));
// Broadcast transform
if (tf_pub.trylock()) {
if (tf_pub.trylock())
{
tf_pub.msg_.transforms[0].header.stamp = ros_time;
if (angle < 1e-16) {
if (angle < 1e-16)
{
tf_pub.msg_.transforms[0].transform.rotation.x = 0;
tf_pub.msg_.transforms[0].transform.rotation.y = 0;
tf_pub.msg_.transforms[0].transform.rotation.z = 0;
tf_pub.msg_.transforms[0].transform.rotation.w = 1;
} else {
}
else
{
tf_pub.msg_.transforms[0].transform.rotation.x = (rx / angle) * std::sin(angle * 0.5);
tf_pub.msg_.transforms[0].transform.rotation.y = (ry / angle) * std::sin(angle * 0.5);
tf_pub.msg_.transforms[0].transform.rotation.z = (rz / angle) * std::sin(angle * 0.5);
@@ -630,7 +654,8 @@ private:
// Publish tool velocity
std::vector<double> tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual();
if (tool_vel_pub.trylock()) {
if (tool_vel_pub.trylock())
{
tool_vel_pub.msg_.header.stamp = ros_time;
tool_vel_pub.msg_.twist.linear.x = tcp_speed[0];
tool_vel_pub.msg_.twist.linear.y = tcp_speed[1];
@@ -646,25 +671,26 @@ private:
void publishRTMsg()
{
ros::Publisher joint_pub = nh_.advertise<sensor_msgs::JointState>(
"joint_states", 1);
ros::Publisher wrench_pub = nh_.advertise<geometry_msgs::WrenchStamped>(
"wrench", 1);
ros::Publisher joint_pub = nh_.advertise<sensor_msgs::JointState>("joint_states", 1);
ros::Publisher wrench_pub = nh_.advertise<geometry_msgs::WrenchStamped>("wrench", 1);
ros::Publisher tool_vel_pub = nh_.advertise<geometry_msgs::TwistStamped>("tool_velocity", 1);
static tf::TransformBroadcaster br;
while (ros::ok()) {
while (ros::ok())
{
sensor_msgs::JointState joint_msg;
joint_msg.name = robot_.getJointNames();
geometry_msgs::WrenchStamped wrench_msg;
geometry_msgs::PoseStamped tool_pose_msg;
std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex
std::unique_lock<std::mutex> locker(msg_lock);
while (!robot_.rt_interface_->robot_state_->getDataPublished()) {
while (!robot_.rt_interface_->robot_state_->getDataPublished())
{
rt_msg_cond_.wait(locker);
}
joint_msg.header.stamp = ros::Time::now();
joint_msg.position = robot_.rt_interface_->robot_state_->getQActual();
for (unsigned int i = 0; i < joint_msg.position.size(); i++) {
for (unsigned int i = 0; i < joint_msg.position.size(); i++)
{
joint_msg.position[i] += joint_offsets_[i];
}
joint_msg.velocity = robot_.rt_interface_->robot_state_->getQdActual();
@@ -680,7 +706,8 @@ private:
wrench_msg.wrench.torque.z = tcp_force[5];
wrench_pub.publish(wrench_msg);
// Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation
// Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation
// vector representation of the tool orientation
std::vector<double> tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual();
// Create quaternion
@@ -689,9 +716,12 @@ private:
double ry = tool_vector_actual[4];
double rz = tool_vector_actual[5];
double angle = std::sqrt(std::pow(rx, 2) + std::pow(ry, 2) + std::pow(rz, 2));
if (angle < 1e-16) {
if (angle < 1e-16)
{
quat.setValue(0, 0, 0, 1);
} else {
}
else
{
quat.setRotation(tf::Vector3(rx / angle, ry / angle, rz / angle), angle);
}
@@ -721,29 +751,27 @@ private:
void publishMbMsg()
{
bool warned = false;
ros::Publisher io_pub = nh_.advertise<ur_msgs::IOStates>(
"ur_driver/io_states", 1);
ros::Publisher io_pub = nh_.advertise<ur_msgs::IOStates>("ur_driver/io_states", 1);
while (ros::ok()) {
while (ros::ok())
{
ur_msgs::IOStates io_msg;
std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex
std::unique_lock<std::mutex> locker(msg_lock);
while (!robot_.sec_interface_->robot_state_->getNewDataAvailable()) {
while (!robot_.sec_interface_->robot_state_->getNewDataAvailable())
{
msg_cond_.wait(locker);
}
int i_max = 10;
if (robot_.sec_interface_->robot_state_->getVersion() > 3.0)
i_max = 18; // From version 3.0, there are up to 18 inputs and outputs
for (unsigned int i = 0; i < i_max; i++) {
for (unsigned int i = 0; i < i_max; i++)
{
ur_msgs::Digital digi;
digi.pin = i;
digi.state = ((robot_.sec_interface_->robot_state_->getDigitalInputBits()
& (1 << i))
>> i);
digi.state = ((robot_.sec_interface_->robot_state_->getDigitalInputBits() & (1 << i)) >> i);
io_msg.digital_in_states.push_back(digi);
digi.state = ((robot_.sec_interface_->robot_state_->getDigitalOutputBits()
& (1 << i))
>> i);
digi.state = ((robot_.sec_interface_->robot_state_->getDigitalOutputBits() & (1 << i)) >> i);
io_msg.digital_out_states.push_back(digi);
}
ur_msgs::Analog ana;
@@ -762,16 +790,19 @@ private:
io_msg.analog_out_states.push_back(ana);
io_pub.publish(io_msg);
if (robot_.sec_interface_->robot_state_->isEmergencyStopped()
or robot_.sec_interface_->robot_state_->isProtectiveStopped()) {
if (robot_.sec_interface_->robot_state_->isEmergencyStopped()
and !warned) {
if (robot_.sec_interface_->robot_state_->isEmergencyStopped() or
robot_.sec_interface_->robot_state_->isProtectiveStopped())
{
if (robot_.sec_interface_->robot_state_->isEmergencyStopped() and !warned)
{
print_error("Emergency stop pressed!");
} else if (robot_.sec_interface_->robot_state_->isProtectiveStopped()
and !warned) {
}
else if (robot_.sec_interface_->robot_state_->isProtectiveStopped() and !warned)
{
print_error("Robot is protective stopped!");
}
if (has_goal_) {
if (has_goal_)
{
print_error("Aborting trajectory");
robot_.stopTraj();
result_.error_code = result_.SUCCESSFUL;
@@ -780,7 +811,8 @@ private:
has_goal_ = false;
}
warned = true;
} else
}
else
warned = false;
robot_.sec_interface_->robot_state_->finishedReading();
@@ -796,26 +828,34 @@ int main(int argc, char** argv)
ros::init(argc, argv, "ur_driver");
ros::NodeHandle nh;
if (ros::param::get("use_sim_time", use_sim_time)) {
if (ros::param::get("use_sim_time", use_sim_time))
{
print_warning("use_sim_time is set!!");
}
if (!(ros::param::get("~robot_ip_address", host))) {
if (argc > 1) {
print_warning(
"Please set the parameter robot_ip_address instead of giving it as a command line argument. This method is DEPRECATED");
if (!(ros::param::get("~robot_ip_address", host)))
{
if (argc > 1)
{
print_warning("Please set the parameter robot_ip_address instead of giving it as a command line argument. This "
"method is DEPRECATED");
host = argv[1];
} else {
print_fatal(
"Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip");
}
else
{
print_fatal("Could not get robot ip. Please supply it as command line parameter or on the parameter server as "
"robot_ip");
exit(1);
}
}
if ((ros::param::get("~reverse_port", reverse_port))) {
if ((reverse_port <= 0) or (reverse_port >= 65535)) {
if ((ros::param::get("~reverse_port", reverse_port)))
{
if ((reverse_port <= 0) or (reverse_port >= 65535))
{
print_warning("Reverse port value is not valid (Use number between 1 and 65534. Using default value of 50001");
reverse_port = 50001;
}
} else
}
else
reverse_port = 50001;
RosWrapper interface(host, reverse_port);