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

Added clang formatting

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

95
.clang-format Normal file
View File

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

119
include/ur_modern_driver/bin_parser.h
View File

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

1
include/ur_modern_driver/do_output.h
View File

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

22
include/ur_modern_driver/log.h
View File

@@ -2,20 +2,20 @@
#include <inttypes.h>
#ifdef ROS_BUILD
#include <ros/ros.h>
#include <ros/ros.h>
#define LOG_DEBUG ROS_DEBUG
#define LOG_WARN ROS_WARN
#define LOG_INFO ROS_INFO
#define LOG_ERROR ROS_ERROR
#define LOG_FATAL ROS_FATAL
#define LOG_DEBUG ROS_DEBUG
#define LOG_WARN ROS_WARN
#define LOG_INFO ROS_INFO
#define LOG_ERROR ROS_ERROR
#define LOG_FATAL ROS_FATAL
#else
#define LOG_DEBUG(format, ...) printf("[DEBUG]: " format "\n", ##__VA_ARGS__)
#define LOG_WARN(format, ...) printf("[WARNING]: " format "\n", ##__VA_ARGS__)
#define LOG_INFO(format, ...) printf("[INFO]: " format "\n", ##__VA_ARGS__)
#define LOG_ERROR(format, ...) printf("[ERROR]: " format "\n", ##__VA_ARGS__)
#define LOG_FATAL(format, ...) printf("[FATAL]: " format "\n", ##__VA_ARGS__)
#define LOG_DEBUG(format, ...) printf("[DEBUG]: " format "\n", ##__VA_ARGS__)
#define LOG_WARN(format, ...) printf("[WARNING]: " format "\n", ##__VA_ARGS__)
#define LOG_INFO(format, ...) printf("[INFO]: " format "\n", ##__VA_ARGS__)
#define LOG_ERROR(format, ...) printf("[ERROR]: " format "\n", ##__VA_ARGS__)
#define LOG_FATAL(format, ...) printf("[FATAL]: " format "\n", ##__VA_ARGS__)
#endif

2
include/ur_modern_driver/packet.h
View File

@@ -3,5 +3,5 @@
class Packet {
public:
virtual bool parse_with(BinParser &bp) = 0;
virtual bool parse_with(BinParser& bp) = 0;
};

2
include/ur_modern_driver/parser.h
View File

@@ -4,5 +4,5 @@
class Parser {
public:
virtual std::unique_ptr<Packet> parse(BinParser &bp) = 0;
virtual std::unique_ptr<Packet> parse(BinParser& bp) = 0;
};

74
include/ur_modern_driver/pipeline.h
View File

@@ -1,21 +1,20 @@
#pragma once
#include <thread>
#include <atomic>
#include <vector>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/queue/readerwriterqueue.h"
#include <atomic>
#include <thread>
#include <vector>
using namespace moodycamel;
using namespace std;
template <typename T>
class IConsumer {
public:
virtual void setup_consumer() { }
virtual void teardown_consumer() { }
virtual void stop_consumer() { }
virtual void setup_consumer() {}
virtual void teardown_consumer() {}
virtual void stop_consumer() {}
virtual bool consume(unique_ptr<T> product) = 0;
};
@@ -23,33 +22,33 @@ public:
template <typename T>
class IProducer {
public:
virtual void setup_producer() { }
virtual void teardown_producer() { }
virtual void stop_producer() { }
virtual void setup_producer() {}
virtual void teardown_producer() {}
virtual void stop_producer() {}
virtual bool try_get(std::vector<unique_ptr<T>> &products) = 0;
virtual bool try_get(std::vector<unique_ptr<T> >& products) = 0;
};
template <typename T>
class Pipeline {
private:
IProducer<T> &_producer;
IConsumer<T> &_consumer;
BlockingReaderWriterQueue<unique_ptr<T>> _queue;
IProducer<T>& _producer;
IConsumer<T>& _consumer;
BlockingReaderWriterQueue<unique_ptr<T> > _queue;
atomic<bool> _running;
thread _pThread, _cThread;
void run_producer() {
void run_producer()
{
_producer.setup_producer();
std::vector<unique_ptr<T>> products;
while(_running) {
if(!_producer.try_get(products)) {
std::vector<unique_ptr<T> > products;
while (_running) {
if (!_producer.try_get(products)) {
break;
}
for(auto &p : products) {
if(!_queue.try_enqueue(std::move(p))) {
for (auto& p : products) {
if (!_queue.try_enqueue(std::move(p))) {
LOG_WARN("Pipeline owerflowed!");
}
}
@@ -60,27 +59,31 @@ private:
//todo cleanup
}
void run_consumer() {
void run_consumer()
{
_consumer.setup_consumer();
unique_ptr<T> product;
while(_running) {
while (_running) {
_queue.wait_dequeue(product);
if(!_consumer.consume(std::move(product)))
if (!_consumer.consume(std::move(product)))
break;
}
_consumer.teardown_consumer();
//todo cleanup
}
public:
Pipeline(IProducer<T> &producer, IConsumer<T> &consumer)
: _producer(producer),
_consumer(consumer),
_queue{32},
_running{false}
{ }
void run() {
if(_running)
public:
Pipeline(IProducer<T>& producer, IConsumer<T>& consumer)
: _producer(producer)
, _consumer(consumer)
, _queue{ 32 }
, _running{ false }
{
}
void run()
{
if (_running)
return;
_running = true;
@@ -88,8 +91,9 @@ public:
_cThread = thread(&Pipeline::run_consumer, this);
}
void stop() {
if(!_running)
void stop()
{
if (!_running)
return;
_consumer.stop_consumer();

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

@@ -12,10 +12,10 @@
// Uses the AE_* prefix for macros (historical reasons), and the "moodycamel" namespace for symbols.
#include <cassert>
#include <type_traits>
#include <cerrno>
#include <cstdint>
#include <ctime>
#include <type_traits>
// Platform detection
#if defined(__INTEL_COMPILER)
@@ -38,11 +38,9 @@
#define AE_ARCH_UNKNOWN
#endif
// AE_UNUSED
#define AE_UNUSED(x) ((void)x)
// AE_FORCEINLINE
#if defined(AE_VCPP) || defined(AE_ICC)
#define AE_FORCEINLINE __forceinline
@@ -53,7 +51,6 @@
#define AE_FORCEINLINE inline
#endif
// AE_ALIGN
#if defined(AE_VCPP) || defined(AE_ICC)
#define AE_ALIGN(x) __declspec(align(x))
@@ -64,7 +61,6 @@
#define AE_ALIGN(x) __attribute__((aligned(x)))
#endif
// Portable atomic fences implemented below:
namespace moodycamel {
@@ -100,10 +96,9 @@ enum memory_order {
#define AeLiteSync __lwsync
#endif
#ifdef AE_VCPP
#pragma warning(push)
#pragma warning(disable: 4365) // Disable erroneous 'conversion from long to unsigned int, signed/unsigned mismatch' error when using `assert`
#pragma warning(disable : 4365) // Disable erroneous 'conversion from long to unsigned int, signed/unsigned mismatch' error when using `assert`
#ifdef __cplusplus_cli
#pragma managed(push, off)
#endif
@@ -114,12 +109,22 @@ namespace moodycamel {
AE_FORCEINLINE void compiler_fence(memory_order order)
{
switch (order) {
case memory_order_relaxed: break;
case memory_order_acquire: _ReadBarrier(); break;
case memory_order_release: _WriteBarrier(); break;
case memory_order_acq_rel: _ReadWriteBarrier(); break;
case memory_order_seq_cst: _ReadWriteBarrier(); break;
default: assert(false);
case memory_order_relaxed:
break;
case memory_order_acquire:
_ReadBarrier();
break;
case memory_order_release:
_WriteBarrier();
break;
case memory_order_acq_rel:
_ReadWriteBarrier();
break;
case memory_order_seq_cst:
_ReadWriteBarrier();
break;
default:
assert(false);
}
}
@@ -130,16 +135,24 @@ AE_FORCEINLINE void compiler_fence(memory_order order)
AE_FORCEINLINE void fence(memory_order order)
{
switch (order) {
case memory_order_relaxed: break;
case memory_order_acquire: _ReadBarrier(); break;
case memory_order_release: _WriteBarrier(); break;
case memory_order_acq_rel: _ReadWriteBarrier(); break;
case memory_order_relaxed:
break;
case memory_order_acquire:
_ReadBarrier();
break;
case memory_order_release:
_WriteBarrier();
break;
case memory_order_acq_rel:
_ReadWriteBarrier();
break;
case memory_order_seq_cst:
_ReadWriteBarrier();
AeFullSync();
_ReadWriteBarrier();
break;
default: assert(false);
default:
assert(false);
}
}
#else
@@ -169,7 +182,8 @@ AE_FORCEINLINE void fence(memory_order order)
AeFullSync();
_ReadWriteBarrier();
break;
default: assert(false);
default:
assert(false);
}
}
#endif
@@ -183,24 +197,44 @@ namespace moodycamel {
AE_FORCEINLINE void compiler_fence(memory_order order)
{
switch (order) {
case memory_order_relaxed: break;
case memory_order_acquire: std::atomic_signal_fence(std::memory_order_acquire); break;
case memory_order_release: std::atomic_signal_fence(std::memory_order_release); break;
case memory_order_acq_rel: std::atomic_signal_fence(std::memory_order_acq_rel); break;
case memory_order_seq_cst: std::atomic_signal_fence(std::memory_order_seq_cst); break;
default: assert(false);
case memory_order_relaxed:
break;
case memory_order_acquire:
std::atomic_signal_fence(std::memory_order_acquire);
break;
case memory_order_release:
std::atomic_signal_fence(std::memory_order_release);
break;
case memory_order_acq_rel:
std::atomic_signal_fence(std::memory_order_acq_rel);
break;
case memory_order_seq_cst:
std::atomic_signal_fence(std::memory_order_seq_cst);
break;
default:
assert(false);
}
}
AE_FORCEINLINE void fence(memory_order order)
{
switch (order) {
case memory_order_relaxed: break;
case memory_order_acquire: std::atomic_thread_fence(std::memory_order_acquire); break;
case memory_order_release: std::atomic_thread_fence(std::memory_order_release); break;
case memory_order_acq_rel: std::atomic_thread_fence(std::memory_order_acq_rel); break;
case memory_order_seq_cst: std::atomic_thread_fence(std::memory_order_seq_cst); break;
default: assert(false);
case memory_order_relaxed:
break;
case memory_order_acquire:
std::atomic_thread_fence(std::memory_order_acquire);
break;
case memory_order_release:
std::atomic_thread_fence(std::memory_order_release);
break;
case memory_order_acq_rel:
std::atomic_thread_fence(std::memory_order_acq_rel);
break;
case memory_order_seq_cst:
std::atomic_thread_fence(std::memory_order_seq_cst);
break;
default:
assert(false);
}
}
@@ -208,7 +242,6 @@ AE_FORCEINLINE void fence(memory_order order)
#endif
#if !defined(AE_VCPP) || (_MSC_VER >= 1700 && !defined(__cplusplus_cli))
#define AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC
#endif
@@ -223,40 +256,65 @@ AE_FORCEINLINE void fence(memory_order order)
// The guarantee of atomicity is only made for types that already have atomic load and store guarantees
// at the hardware level -- on most platforms this generally means aligned pointers and integers (only).
namespace moodycamel {
template<typename T>
class weak_atomic
{
template <typename T>
class weak_atomic {
public:
weak_atomic() { }
weak_atomic() {}
#ifdef AE_VCPP
#pragma warning(disable: 4100) // Get rid of (erroneous) 'unreferenced formal parameter' warning
#pragma warning(disable : 4100) // Get rid of (erroneous) 'unreferenced formal parameter' warning
#endif
template<typename U> weak_atomic(U&& x) : value(std::forward<U>(x)) { }
template <typename U>
weak_atomic(U&& x)
: value(std::forward<U>(x))
{
}
#ifdef __cplusplus_cli
// 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&& other) : value(std::move(other.value)) { }
weak_atomic(weak_atomic const& other)
: value(other.value)
{
}
weak_atomic(weak_atomic&& other)
: value(std::move(other.value))
{
}
#ifdef AE_VCPP
#pragma warning(default: 4100)
#pragma warning(default : 4100)
#endif
AE_FORCEINLINE operator T() const { return load(); }
AE_FORCEINLINE operator T() const
{
return load();
}
#ifndef AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC
template<typename U> AE_FORCEINLINE weak_atomic const& operator=(U&& x) { value = std::forward<U>(x); return *this; }
AE_FORCEINLINE weak_atomic const& operator=(weak_atomic const& other) { value = other.value; return *this; }
template <typename U>
AE_FORCEINLINE weak_atomic const& operator=(U&& x)
{
value = std::forward<U>(x);
return *this;
}
AE_FORCEINLINE weak_atomic const& operator=(weak_atomic const& other)
{
value = other.value;
return *this;
}
AE_FORCEINLINE T load() const { return value; }
AE_FORCEINLINE T fetch_add_acquire(T increment)
{
#if defined(AE_ARCH_X64) || defined(AE_ARCH_X86)
if (sizeof(T) == 4) return _InterlockedExchangeAdd((long volatile*)&value, (long)increment);
if (sizeof(T) == 4)
return _InterlockedExchangeAdd((long volatile*)&value, (long)increment);
#if defined(_M_AMD64)
else if (sizeof(T) == 8) return _InterlockedExchangeAdd64((long long volatile*)&value, (long long)increment);
else if (sizeof(T) == 8)
return _InterlockedExchangeAdd64((long long volatile*)&value, (long long)increment);
#endif
#else
#error Unsupported platform
@@ -268,9 +326,11 @@ public:
AE_FORCEINLINE T fetch_add_release(T increment)
{
#if defined(AE_ARCH_X64) || defined(AE_ARCH_X86)
if (sizeof(T) == 4) return _InterlockedExchangeAdd((long volatile*)&value, (long)increment);
if (sizeof(T) == 4)
return _InterlockedExchangeAdd((long volatile*)&value, (long)increment);
#if defined(_M_AMD64)
else if (sizeof(T) == 8) return _InterlockedExchangeAdd64((long long volatile*)&value, (long long)increment);
else if (sizeof(T) == 8)
return _InterlockedExchangeAdd64((long long volatile*)&value, (long long)increment);
#endif
#else
#error Unsupported platform
@@ -279,7 +339,7 @@ public:
return value;
}
#else
template<typename U>
template <typename U>
AE_FORCEINLINE weak_atomic const& operator=(U&& x)
{
value.store(std::forward<U>(x), std::memory_order_relaxed);
@@ -305,7 +365,6 @@ public:
}
#endif
private:
#ifndef AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC
// No std::atomic support, but still need to circumvent compiler optimizations.
@@ -318,8 +377,6 @@ private:
} // end namespace moodycamel
// Portable single-producer, single-consumer semaphore below:
#if defined(_WIN32)
@@ -329,11 +386,11 @@ private:
// I know this is an ugly hack but it still beats polluting the global
// namespace with thousands of generic names or adding a .cpp for nothing.
extern "C" {
struct _SECURITY_ATTRIBUTES;
__declspec(dllimport) void* __stdcall CreateSemaphoreW(_SECURITY_ATTRIBUTES* lpSemaphoreAttributes, long lInitialCount, long lMaximumCount, const wchar_t* lpName);
__declspec(dllimport) int __stdcall CloseHandle(void* hObject);
__declspec(dllimport) unsigned long __stdcall WaitForSingleObject(void* hHandle, unsigned long dwMilliseconds);
__declspec(dllimport) int __stdcall ReleaseSemaphore(void* hSemaphore, long lReleaseCount, long* lpPreviousCount);
struct _SECURITY_ATTRIBUTES;
__declspec(dllimport) void* __stdcall CreateSemaphoreW(_SECURITY_ATTRIBUTES* lpSemaphoreAttributes, long lInitialCount, long lMaximumCount, const wchar_t* lpName);
__declspec(dllimport) int __stdcall CloseHandle(void* hObject);
__declspec(dllimport) unsigned long __stdcall WaitForSingleObject(void* hHandle, unsigned long dwMilliseconds);
__declspec(dllimport) int __stdcall ReleaseSemaphore(void* hSemaphore, long lReleaseCount, long* lpPreviousCount);
}
#elif defined(__MACH__)
#include <mach/mach.h>
@@ -341,34 +398,31 @@ extern "C" {
#include <semaphore.h>
#endif
namespace moodycamel
{
// Code in the spsc_sema namespace below is an adaptation of Jeff Preshing's
// portable + lightweight semaphore implementations, originally from
// https://github.com/preshing/cpp11-on-multicore/blob/master/common/sema.h
// LICENSE:
// Copyright (c) 2015 Jeff Preshing
//
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
//
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
//
// 1. The origin of this software must not be misrepresented; you must not
// claim that you wrote the original software. If you use this software
// in a product, an acknowledgement in the product documentation would be
// appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
namespace spsc_sema
{
namespace moodycamel {
// Code in the spsc_sema namespace below is an adaptation of Jeff Preshing's
// portable + lightweight semaphore implementations, originally from
// https://github.com/preshing/cpp11-on-multicore/blob/master/common/sema.h
// LICENSE:
// Copyright (c) 2015 Jeff Preshing
//
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
//
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
//
// 1. The origin of this software must not be misrepresented; you must not
// claim that you wrote the original software. If you use this software
// in a product, an acknowledgement in the product documentation would be
// appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
namespace spsc_sema {
#if defined(_WIN32)
class Semaphore
{
class Semaphore {
private:
void* m_hSema;
@@ -416,8 +470,7 @@ namespace moodycamel
// 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;
@@ -465,8 +518,7 @@ namespace moodycamel
void signal(int count)
{
while (count-- > 0)
{
while (count-- > 0) {
semaphore_signal(m_sema);
}
}
@@ -475,8 +527,7 @@ namespace moodycamel
//---------------------------------------------------------
// Semaphore (POSIX, Linux)
//---------------------------------------------------------
class Semaphore
{
class Semaphore {
private:
sem_t m_sema;
@@ -499,11 +550,9 @@ namespace moodycamel
{
// 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);
} while (rc == -1 && errno == EINTR);
}
bool try_wait()
@@ -544,8 +593,7 @@ namespace moodycamel
void signal(int count)
{
while (count-- > 0)
{
while (count-- > 0) {
sem_post(&m_sema);
}
}
@@ -557,8 +605,7 @@ namespace moodycamel
//---------------------------------------------------------
// LightweightSemaphore
//---------------------------------------------------------
class LightweightSemaphore
{
class LightweightSemaphore {
public:
typedef std::make_signed<std::size_t>::type ssize_t;
@@ -573,10 +620,8 @@ namespace moodycamel
// If we lower it to 1000, testBenaphore becomes 15x slower on my Core i7-5930K Windows PC,
// 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;
}
@@ -585,8 +630,7 @@ namespace moodycamel
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;
}
@@ -597,8 +641,7 @@ namespace moodycamel
// 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
@@ -610,15 +653,15 @@ namespace moodycamel
}
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;
}
@@ -641,8 +684,7 @@ namespace moodycamel
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);
}
}
@@ -653,7 +695,7 @@ namespace moodycamel
return count > 0 ? count : 0;
}
};
} // end namespace spsc_sema
} // end namespace spsc_sema
} // end namespace moodycamel
#if defined(AE_VCPP) && (_MSC_VER < 1700 || defined(__cplusplus_cli))

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

@@ -5,18 +5,17 @@
#pragma once
#include "atomicops.h"
#include <type_traits>
#include <utility>
#include <cassert>
#include <stdexcept>
#include <new>
#include <cstdint>
#include <cstdlib> // For malloc/free/abort & size_t
#include <new>
#include <stdexcept>
#include <type_traits>
#include <utility>
#if __cplusplus > 199711L || _MSC_VER >= 1700 // C++11 or VS2012
#include <chrono>
#endif
// A lock-free queue for a single-consumer, single-producer architecture.
// The queue is also wait-free in the common path (except if more memory
// needs to be allocated, in which case malloc is called).
@@ -42,16 +41,15 @@
#ifdef AE_VCPP
#pragma warning(push)
#pragma warning(disable: 4324) // structure was padded due to __declspec(align())
#pragma warning(disable: 4820) // padding was added
#pragma warning(disable: 4127) // conditional expression is constant
#pragma warning(disable : 4324) // structure was padded due to __declspec(align())
#pragma warning(disable : 4820) // padding was added
#pragma warning(disable : 4127) // conditional expression is constant
#endif
namespace moodycamel {
template<typename T, size_t MAX_BLOCK_SIZE = 512>
class ReaderWriterQueue
{
template <typename T, size_t MAX_BLOCK_SIZE = 512>
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,
@@ -80,7 +78,7 @@ public:
explicit ReaderWriterQueue(size_t maxSize = 15)
#ifndef NDEBUG
: enqueuing(false)
,dequeuing(false)
, dequeuing(false)
#endif
{
assert(maxSize > 0);
@@ -110,15 +108,13 @@ public:
}
if (firstBlock == nullptr) {
firstBlock = block;
}
else {
} else {
lastBlock->next = block;
}
lastBlock = block;
block->next = firstBlock;
}
}
else {
} else {
firstBlock = make_block(largestBlockSize);
if (firstBlock == nullptr) {
#ifdef MOODYCAMEL_EXCEPTIONS_ENABLED
@@ -164,7 +160,6 @@ public:
} while (block != frontBlock_);
}
// Enqueues a copy of element if there is room in the queue.
// Returns true if the element was enqueued, false otherwise.
// Does not allocate memory.
@@ -181,7 +176,6 @@ public:
return inner_enqueue<CannotAlloc>(std::forward<T>(element));
}
// Enqueues a copy of element on the queue.
// Allocates an additional block of memory if needed.
// Only fails (returns false) if memory allocation fails.
@@ -198,11 +192,10 @@ public:
return inner_enqueue<CanAlloc>(std::forward<T>(element));
}
// Attempts to dequeue an element; if the queue is empty,
// returns false instead. If the queue has at least one element,
// moves front to result using operator=, then returns true.
template<typename U>
template <typename U>
bool try_dequeue(U& result)
{
#ifndef NDEBUG
@@ -243,8 +236,7 @@ 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();
@@ -287,8 +279,7 @@ public:
fence(memory_order_release);
frontBlock_->front = nextBlockFront;
}
else {
} else {
// No elements in current block and no other block to advance to
return false;
}
@@ -296,7 +287,6 @@ public:
return true;
}
// Returns a pointer to the front element in the queue (the one that
// would be removed next by a call to `try_dequeue` or `pop`). If the
// queue appears empty at the time the method is called, nullptr is
@@ -317,8 +307,7 @@ public:
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();
@@ -366,8 +355,7 @@ 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();
@@ -400,8 +388,7 @@ public:
fence(memory_order_release);
frontBlock_->front = nextBlockFront;
}
else {
} else {
// No elements in current block and no other block to advance to
return false;
}
@@ -426,11 +413,11 @@ public:
return result;
}
private:
enum AllocationMode { CanAlloc, CannotAlloc };
enum AllocationMode { CanAlloc,
CannotAlloc };
template<AllocationMode canAlloc, typename U>
template <AllocationMode canAlloc, typename U>
bool inner_enqueue(U&& element)
{
#ifndef NDEBUG
@@ -457,8 +444,7 @@ private:
fence(memory_order_release);
tailBlock_->tail = nextBlockTail;
}
else {
} else {
fence(memory_order_acquire);
if (tailBlock_->next.load() != frontBlock) {
// Note that the reason we can't advance to the frontBlock and start adding new entries there
@@ -486,8 +472,7 @@ 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);
@@ -513,12 +498,10 @@ 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;
}
@@ -527,14 +510,11 @@ private:
return true;
}
// 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)
{
@@ -550,16 +530,16 @@ private:
return x;
}
template<typename U>
template <typename U>
static AE_FORCEINLINE char* align_for(char* ptr)
{
const std::size_t alignment = std::alignment_of<U>::value;
return ptr + (alignment - (reinterpret_cast<std::uintptr_t>(ptr) % alignment)) % alignment;
}
private:
#ifndef NDEBUG
struct ReentrantGuard
{
struct ReentrantGuard {
ReentrantGuard(bool& _inSection)
: inSection(_inSection)
{
@@ -577,8 +557,7 @@ 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
@@ -594,10 +573,16 @@ private:
const size_t sizeMask;
// size must be a power of two (and greater than 0)
Block(size_t const& _size, char* _rawThis, char* _data)
: front(0), localTail(0), tail(0), localFront(0), next(nullptr), data(_data), sizeMask(_size - 1), rawThis(_rawThis)
: front(0)
, localTail(0)
, tail(0)
, localFront(0)
, next(nullptr)
, data(_data)
, sizeMask(_size - 1)
, rawThis(_rawThis)
{
}
@@ -609,7 +594,6 @@ private:
char* rawThis;
};
static Block* make_block(size_t capacity)
{
// Allocate enough memory for the block itself, as well as all the elements it will contain
@@ -640,17 +624,16 @@ private:
};
// Like ReaderWriterQueue, but also providees blocking operations
template<typename T, size_t MAX_BLOCK_SIZE = 512>
class BlockingReaderWriterQueue
{
template <typename T, size_t MAX_BLOCK_SIZE = 512>
class BlockingReaderWriterQueue {
private:
typedef ::moodycamel::ReaderWriterQueue<T, MAX_BLOCK_SIZE> ReaderWriterQueue;
public:
explicit BlockingReaderWriterQueue(size_t maxSize = 15)
: inner(maxSize)
{ }
{
}
// Enqueues a copy of element if there is room in the queue.
// Returns true if the element was enqueued, false otherwise.
@@ -676,7 +659,6 @@ public:
return false;
}
// Enqueues a copy of element on the queue.
// Allocates an additional block of memory if needed.
// Only fails (returns false) if memory allocation fails.
@@ -701,11 +683,10 @@ public:
return false;
}
// Attempts to dequeue an element; if the queue is empty,
// returns false instead. If the queue has at least one element,
// moves front to result using operator=, then returns true.
template<typename U>
template <typename U>
bool try_dequeue(U& result)
{
if (sema.tryWait()) {
@@ -717,10 +698,9 @@ public:
return false;
}
// Attempts to dequeue an element; if the queue is empty,
// waits until an element is available, then dequeues it.
template<typename U>
template <typename U>
void wait_dequeue(U& result)
{
sema.wait();
@@ -730,14 +710,13 @@ public:
AE_UNUSED(success);
}
// Attempts to dequeue an element; if the queue is empty,
// waits until an element is available up to the specified timeout,
// then dequeues it and returns true, or returns false if the timeout
// expires before an element can be dequeued.
// Using a negative timeout indicates an indefinite timeout,
// and is thus functionally equivalent to calling wait_dequeue.
template<typename U>
template <typename U>
bool wait_dequeue_timed(U& result, std::int64_t timeout_usecs)
{
if (!sema.wait(timeout_usecs)) {
@@ -750,7 +729,6 @@ public:
return true;
}
#if __cplusplus > 199711L || _MSC_VER >= 1700
// Attempts to dequeue an element; if the queue is empty,
// waits until an element is available up to the specified timeout,
@@ -758,14 +736,13 @@ public:
// expires before an element can be dequeued.
// Using a negative timeout indicates an indefinite timeout,
// and is thus functionally equivalent to calling wait_dequeue.
template<typename U, typename Rep, typename Period>
template <typename U, typename Rep, typename Period>
inline bool wait_dequeue_timed(U& result, std::chrono::duration<Rep, Period> const& timeout)
{
return wait_dequeue_timed(result, std::chrono::duration_cast<std::chrono::microseconds>(timeout).count());
}
#endif
// Returns a pointer to the front element in the queue (the one that
// would be removed next by a call to `try_dequeue` or `pop`). If the
// queue appears empty at the time the method is called, nullptr is
@@ -797,11 +774,10 @@ public:
return sema.availableApprox();
}
private:
// Disable copying & assignment
BlockingReaderWriterQueue(ReaderWriterQueue const&) { }
BlockingReaderWriterQueue& operator=(ReaderWriterQueue const&) { }
BlockingReaderWriterQueue(ReaderWriterQueue const&) {}
BlockingReaderWriterQueue& operator=(ReaderWriterQueue const&) {}
private:
ReaderWriterQueue inner;

24
include/ur_modern_driver/robot_state.h
View File

@@ -19,17 +19,19 @@
#ifndef ROBOT_STATE_H_
#define ROBOT_STATE_H_
#include <condition_variable>
#include <inttypes.h>
#include <vector>
#include <mutex>
#include <netinet/in.h>
#include <stdlib.h>
#include <string.h>
#include <mutex>
#include <condition_variable>
#include <netinet/in.h>
#include <vector>
namespace message_types {
enum message_type {
ROBOT_STATE = 16, ROBOT_MESSAGE = 20, PROGRAM_STATE_MESSAGE = 25
ROBOT_STATE = 16,
ROBOT_MESSAGE = 20,
PROGRAM_STATE_MESSAGE = 25
};
}
typedef message_types::message_type messageType;
@@ -206,13 +208,13 @@ public:
bool getNewDataAvailable();
void finishedReading();
void unpack(uint8_t * buf, unsigned int buf_length);
void unpackRobotMessage(uint8_t * buf, unsigned int offset, uint32_t len);
void unpackRobotMessageVersion(uint8_t * buf, unsigned int offset,
void unpack(uint8_t* buf, unsigned int buf_length);
void unpackRobotMessage(uint8_t* buf, unsigned int offset, uint32_t len);
void unpackRobotMessageVersion(uint8_t* buf, unsigned int offset,
uint32_t len);
void unpackRobotState(uint8_t * buf, unsigned int offset, uint32_t len);
void unpackRobotStateMasterboard(uint8_t * buf, unsigned int offset);
void unpackRobotMode(uint8_t * buf, unsigned int offset);
void unpackRobotState(uint8_t* buf, unsigned int offset, uint32_t len);
void unpackRobotStateMasterboard(uint8_t* buf, unsigned int offset);
void unpackRobotMode(uint8_t* buf, unsigned int offset);
};
#endif /* ROBOT_STATE_H_ */

12
include/ur_modern_driver/robot_state_RT.h
View File

@@ -19,13 +19,13 @@
#ifndef ROBOT_STATE_RT_H_
#define ROBOT_STATE_RT_H_
#include <condition_variable>
#include <inttypes.h>
#include <vector>
#include <stdlib.h>
#include <string.h>
#include <mutex>
#include <netinet/in.h>
#include <condition_variable>
#include <stdlib.h>
#include <string.h>
#include <vector>
class RobotStateRT {
private:
@@ -66,7 +66,7 @@ private:
bool data_published_; //to avoid spurious wakes
bool controller_updated_; //to avoid spurious wakes
std::vector<double> unpackVector(uint8_t * buf, int start_index,
std::vector<double> unpackVector(uint8_t* buf, int start_index,
int nr_of_vals);
std::vector<bool> unpackDigitalInputBits(int64_t data);
double ntohd(uint64_t nf);
@@ -110,7 +110,7 @@ public:
bool getControllerUpdated();
void setControllerUpdated();
std::vector<double> getVActual();
void unpack(uint8_t * buf);
void unpack(uint8_t* buf);
};
#endif /* ROBOT_STATE_RT_H_ */

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

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

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

@@ -1,12 +1,12 @@
#pragma once
#include <cstdlib>
#include <vector>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/WrenchStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/WrenchStamped.h>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <vector>
#include "ur_modern_driver/ur/consumer.h"
@@ -30,30 +30,30 @@ private:
std::vector<std::string> _joint_names;
std::string _base_frame;
bool publish_joints(RTShared &packet, ros::Time &t);
bool publish_wrench(RTShared &packet, ros::Time &t);
bool publish_tool(RTShared &packet, ros::Time &t);
bool publish_joints(RTShared& packet, ros::Time& t);
bool publish_wrench(RTShared& packet, ros::Time& t);
bool publish_tool(RTShared& packet, ros::Time& t);
bool publish(RTShared &packet);
bool publish(RTShared& packet);
public:
RTPublisher(std::string &joint_prefix, std::string &base_frame) :
_joint_pub(_nh.advertise<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)),
_base_frame(base_frame)
RTPublisher(std::string& joint_prefix, std::string& base_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))
, _base_frame(base_frame)
{
for(auto const& j : JOINTS) {
for (auto const& j : JOINTS) {
_joint_names.push_back(joint_prefix + j);
}
}
virtual bool consume(RTState_V1_6__7 &state);
virtual bool consume(RTState_V1_8 &state);
virtual bool consume(RTState_V3_0__1 &state);
virtual bool consume(RTState_V3_2__3 &state);
virtual bool consume(RTState_V1_6__7& state);
virtual bool consume(RTState_V1_8& state);
virtual bool consume(RTState_V3_0__1& state);
virtual bool consume(RTState_V3_2__3& state);
virtual void setup_consumer() { }
virtual void teardown_consumer() { }
virtual void stop_consumer() { }
virtual void setup_consumer() {}
virtual void teardown_consumer() {}
virtual void stop_consumer() {}
};

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

@@ -1,46 +1,47 @@
#pragma once
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/ur/state.h"
#include "ur_modern_driver/ur/master_board.h"
#include "ur_modern_driver/ur/messages.h"
#include "ur_modern_driver/ur/robot_mode.h"
#include "ur_modern_driver/ur/rt_state.h"
#include "ur_modern_driver/ur/messages.h"
#include "ur_modern_driver/ur/state.h"
class URRTPacketConsumer : public IConsumer<RTPacket> {
public:
virtual bool consume(unique_ptr<RTPacket> packet) {
virtual bool consume(unique_ptr<RTPacket> packet)
{
return packet->consume_with(*this);
}
virtual bool consume(RTState_V1_6__7 &state) = 0;
virtual bool consume(RTState_V1_8 &state) = 0;
virtual bool consume(RTState_V3_0__1 &state) = 0;
virtual bool consume(RTState_V3_2__3 &state) = 0;
virtual bool consume(RTState_V1_6__7& state) = 0;
virtual bool consume(RTState_V1_8& state) = 0;
virtual bool consume(RTState_V3_0__1& state) = 0;
virtual bool consume(RTState_V3_2__3& state) = 0;
};
class URStatePacketConsumer : public IConsumer<StatePacket> {
public:
virtual bool consume(unique_ptr<StatePacket> packet) {
virtual bool consume(unique_ptr<StatePacket> packet)
{
return packet->consume_with(*this);
}
virtual bool consume(MasterBoardData_V1_X &data) = 0;
virtual bool consume(MasterBoardData_V3_0__1 &data) = 0;
virtual bool consume(MasterBoardData_V3_2 &data) = 0;
virtual bool consume(MasterBoardData_V1_X& data) = 0;
virtual bool consume(MasterBoardData_V3_0__1& data) = 0;
virtual bool consume(MasterBoardData_V3_2& data) = 0;
virtual bool consume(RobotModeData_V1_X &data) = 0;
virtual bool consume(RobotModeData_V3_0__1 &data) = 0;
virtual bool consume(RobotModeData_V3_2 &data) = 0;
virtual bool consume(RobotModeData_V1_X& data) = 0;
virtual bool consume(RobotModeData_V3_0__1& data) = 0;
virtual bool consume(RobotModeData_V3_2& data) = 0;
};
class URMessagePacketConsumer : public IConsumer<MessagePacket> {
public:
virtual bool consume(unique_ptr<MessagePacket> packet) {
virtual bool consume(unique_ptr<MessagePacket> packet)
{
return packet->consume_with(*this);
}
virtual bool consume(VersionMessage &message) = 0;
virtual bool consume(VersionMessage& message) = 0;
};

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

@@ -1,14 +1,13 @@
#pragma once
#include <cstdlib>
#include "ur_modern_driver/ur/stream.h"
#include "ur_modern_driver/ur/consumer.h"
#include "ur_modern_driver/ur/producer.h"
#include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/state_parser.h"
#include "ur_modern_driver/ur/rt_parser.h"
#include "ur_modern_driver/ur/messages_parser.h"
#include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/producer.h"
#include "ur_modern_driver/ur/rt_parser.h"
#include "ur_modern_driver/ur/state_parser.h"
#include "ur_modern_driver/ur/stream.h"
#include <cstdlib>
class URFactory : private URMessagePacketConsumer {
private:
@@ -18,7 +17,8 @@ private:
uint8_t _major_version;
uint8_t _minor_version;
bool consume(VersionMessage &vm) {
bool consume(VersionMessage& vm)
{
LOG_INFO("Got VersionMessage:");
LOG_INFO("project name: %s", vm.project_name.c_str());
LOG_INFO("version: %u.%u.%d", vm.major_version, vm.minor_version, vm.svn_version);
@@ -30,27 +30,29 @@ private:
return true;
}
void setup_consumer() { }
void teardown_consumer() { }
void stop_consumer() { }
void setup_consumer() {}
void teardown_consumer() {}
void stop_consumer() {}
public:
URFactory(std::string &host) : _stream(host, 30001) {
URFactory(std::string& host)
: _stream(host, 30001)
{
URProducer<MessagePacket> p(_stream, _parser);
std::vector<unique_ptr<MessagePacket>> results;
std::vector<unique_ptr<MessagePacket> > results;
p.setup_producer();
if(!p.try_get(results) || results.size() == 0) {
if (!p.try_get(results) || results.size() == 0) {
LOG_FATAL("No version message received, init failed!");
std::exit(EXIT_FAILURE);
}
for(auto const& p : results) {
for (auto const& p : results) {
p->consume_with(*this);
}
if(_major_version == 0 && _minor_version == 0) {
if (_major_version == 0 && _minor_version == 0) {
LOG_FATAL("No version message received, init failed!");
std::exit(EXIT_FAILURE);
}
@@ -58,28 +60,30 @@ public:
p.teardown_producer();
}
std::unique_ptr<URParser<StatePacket>> get_state_parser() {
if(_major_version == 1) {
return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V1_X);
std::unique_ptr<URParser<StatePacket> > get_state_parser()
{
if (_major_version == 1) {
return std::unique_ptr<URParser<StatePacket> >(new URStateParser_V1_X);
} else {
if(_minor_version < 3)
return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V3_0__1);
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);
return std::unique_ptr<URParser<StatePacket> >(new URStateParser_V3_2);
}
}
std::unique_ptr<URParser<RTPacket>> get_rt_parser() {
if(_major_version == 1) {
if(_minor_version < 8)
return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V1_6__7);
std::unique_ptr<URParser<RTPacket> > get_rt_parser()
{
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);
return std::unique_ptr<URParser<RTPacket> >(new URRTStateParser_V1_8);
} else {
if(_minor_version < 3)
return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V3_0__1);
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);
return std::unique_ptr<URParser<RTPacket> >(new URRTStateParser_V3_2__3);
}
}
};

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

@@ -1,15 +1,14 @@
#pragma once
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/types.h"
#include "ur_modern_driver/ur/state.h"
#include <cstddef>
#include <inttypes.h>
#include "ur_modern_driver/types.h"
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/ur/state.h"
class SharedMasterBoardData {
public:
virtual bool parse_with(BinParser &bp);
virtual bool parse_with(BinParser& bp);
int8_t analog_input_range0;
int8_t analog_input_range1;
@@ -40,8 +39,8 @@ public:
class MasterBoardData_V1_X : public SharedMasterBoardData, public StatePacket {
public:
virtual bool parse_with(BinParser &bp);
virtual bool consume_with(URStatePacketConsumer &consumer);
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer);
int16_t digital_input_bits;
int16_t digital_output_bits;
@@ -53,7 +52,6 @@ public:
int16_t euromap_voltage;
int16_t euromap_current;
static const size_t SIZE = SharedMasterBoardData::SIZE
+ sizeof(int16_t) * 2
+ sizeof(uint8_t) * 2;
@@ -64,8 +62,8 @@ public:
class MasterBoardData_V3_0__1 : public SharedMasterBoardData, public StatePacket {
public:
virtual bool parse_with(BinParser &bp);
virtual bool consume_with(URStatePacketConsumer &consumer);
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer);
int32_t digital_input_bits;
int32_t digital_output_bits;
@@ -77,7 +75,6 @@ public:
float euromap_voltage;
float euromap_current;
static const size_t SIZE = SharedMasterBoardData::SIZE
+ sizeof(int32_t) * 2
+ sizeof(uint8_t) * 2
@@ -89,8 +86,8 @@ public:
class MasterBoardData_V3_2 : public MasterBoardData_V3_0__1 {
public:
virtual bool parse_with(BinParser &bp);
virtual bool consume_with(URStatePacketConsumer &consumer);
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer);
uint8_t operational_mode_selector_input;
uint8_t three_position_enabling_device_input;

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

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

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

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

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

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

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

@@ -1,30 +1,36 @@
#pragma once
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/ur/stream.h"
#include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/stream.h"
template <typename 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() {
void setup_producer()
{
_stream.connect();
}
void teardown_producer() {
void teardown_producer()
{
_stream.disconnect();
}
void stop_producer() {
void stop_producer()
{
_stream.disconnect();
}
bool try_get(std::vector<unique_ptr<T>> &products) {
bool try_get(std::vector<unique_ptr<T> >& products)
{
//4KB should be enough to hold any packet received from UR
uint8_t buf[4096];
@@ -33,7 +39,7 @@ public:
//LOG_DEBUG("Read %d bytes from stream", len);
if(len < 1) {
if (len < 1) {
LOG_WARN("Read nothing from stream");
return false;
}

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

@@ -1,14 +1,14 @@
#pragma once
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/types.h"
#include "ur_modern_driver/ur/state.h"
#include <cstddef>
#include <inttypes.h>
#include "ur_modern_driver/types.h"
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/ur/state.h"
class SharedRobotModeData {
public:
virtual bool parse_with(BinParser &bp);
virtual bool parse_with(BinParser& bp);
uint64_t timestamp;
bool physical_robot_connected;
@@ -37,9 +37,8 @@ enum class robot_mode_V1_X : uint8_t {
class RobotModeData_V1_X : public SharedRobotModeData, public StatePacket {
public:
virtual bool parse_with(BinParser &bp);
virtual bool consume_with(URStatePacketConsumer &consumer);
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer);
bool security_stopped;
robot_mode_V1_X robot_mode;
@@ -74,9 +73,8 @@ enum class robot_control_mode_V3_X : uint8_t {
class RobotModeData_V3_0__1 : public SharedRobotModeData, public StatePacket {
public:
virtual bool parse_with(BinParser &bp);
virtual bool consume_with(URStatePacketConsumer &consumer);
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer);
bool protective_stopped;
@@ -98,9 +96,8 @@ public:
class RobotModeData_V3_2 : public RobotModeData_V3_0__1 {
public:
virtual bool parse_with(BinParser &bp);
virtual bool consume_with(URStatePacketConsumer &consumer);
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer);
double target_speed_fraction_limit;

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

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

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

@@ -1,23 +1,23 @@
#pragma once
#include <cstddef>
#include <inttypes.h>
#include "ur_modern_driver/types.h"
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/types.h"
#include <cstddef>
#include <inttypes.h>
class URRTPacketConsumer;
class RTPacket {
public:
virtual bool parse_with(BinParser &bp) = 0;
virtual bool consume_with(URRTPacketConsumer &consumer) = 0;
virtual bool parse_with(BinParser& bp) = 0;
virtual bool consume_with(URRTPacketConsumer& consumer) = 0;
};
class RTShared {
protected:
bool parse_shared1(BinParser &bp);
bool parse_shared2(BinParser &bp);
bool parse_shared1(BinParser& bp);
bool parse_shared2(BinParser& bp);
public:
double time;
@@ -43,18 +43,16 @@ public:
double controller_time;
double robot_mode;
static const size_t SIZE = sizeof(double) * 3
+ sizeof(double[6]) * 10
+ sizeof(cartesian_coord_t) * 2
+ sizeof(uint64_t);
};
class RTState_V1_6__7 : public RTShared, public RTPacket {
public:
bool parse_with(BinParser &bp);
virtual bool consume_with(URRTPacketConsumer &consumer);
bool parse_with(BinParser& bp);
virtual bool consume_with(URRTPacketConsumer& consumer);
double3_t tool_accelerometer_values;
@@ -66,8 +64,8 @@ public:
class RTState_V1_8 : public RTState_V1_6__7 {
public:
bool parse_with(BinParser &bp);
virtual bool consume_with(URRTPacketConsumer &consumer);
bool parse_with(BinParser& bp);
virtual bool consume_with(URRTPacketConsumer& consumer);
double joint_modes[6];
@@ -79,14 +77,13 @@ public:
class RTState_V3_0__1 : public RTShared, public RTPacket {
public:
bool parse_with(BinParser &bp);
virtual bool consume_with(URRTPacketConsumer &consumer);
bool parse_with(BinParser& bp);
virtual bool consume_with(URRTPacketConsumer& consumer);
double i_control[6];
cartesian_coord_t tool_vector_target;
cartesian_coord_t tcp_speed_target;
double joint_modes[6];
double safety_mode;
double3_t tool_accelerometer_values;
@@ -97,7 +94,6 @@ public:
double i_robot;
double v_actual[6];
static const size_t SIZE = RTShared::SIZE
+ sizeof(double[6]) * 3
+ sizeof(double3_t)
@@ -109,8 +105,8 @@ public:
class RTState_V3_2__3 : public RTState_V3_0__1 {
public:
bool parse_with(BinParser &bp);
virtual bool consume_with(URRTPacketConsumer &consumer);
bool parse_with(BinParser& bp);
virtual bool consume_with(URRTPacketConsumer& consumer);
uint64_t digital_outputs;
double program_state;

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

@@ -1,10 +1,10 @@
#pragma once
#include <cstddef>
#include <inttypes.h>
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.h"
#include <cstddef>
#include <inttypes.h>
enum class package_type : uint8_t {
ROBOT_MODE_DATA = 0,
@@ -29,6 +29,6 @@ class URStatePacketConsumer;
class StatePacket {
public:
virtual bool parse_with(BinParser &bp) = 0;
virtual bool consume_with(URStatePacketConsumer &consumer) = 0;
virtual bool parse_with(BinParser& bp) = 0;
virtual bool consume_with(URStatePacketConsumer& consumer) = 0;
};

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

@@ -1,19 +1,19 @@
#pragma once
#include <vector>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/state.h"
#include "ur_modern_driver/ur/master_board.h"
#include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/robot_mode.h"
#include "ur_modern_driver/ur/state.h"
#include <vector>
template <typename RMD, typename MBD>
class URStateParser : public URParser<StatePacket> {
private:
StatePacket* from_type(package_type type) {
switch(type) {
StatePacket* from_type(package_type type)
{
switch (type) {
case package_type::ROBOT_MODE_DATA:
return new RMD;
case package_type::MASTERBOARD_DATA:
@@ -24,24 +24,25 @@ private:
}
public:
bool parse(BinParser &bp, std::vector<std::unique_ptr<StatePacket>> &results) {
bool parse(BinParser& bp, std::vector<std::unique_ptr<StatePacket> >& results)
{
int32_t packet_size;
message_type type;
bp.parse(packet_size);
bp.parse(type);
if(type != message_type::ROBOT_STATE) {
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.check_size(sizeof(uint32_t))) {
LOG_ERROR("Failed to read sub-package length, there's likely a parsing error");
return false;
}
uint32_t sub_size = bp.peek<uint32_t>();
if(!bp.check_size(static_cast<size_t>(sub_size))) {
if (!bp.check_size(static_cast<size_t>(sub_size))) {
LOG_WARN("Invalid sub-package size of %" PRIu32 " received!", sub_size);
return false;
}
@@ -54,20 +55,20 @@ 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->parse_with(sbp)) {
LOG_ERROR("Sub-package parsing of type %d failed!", type);
return false;
}
results.push_back(std::move(packet));
if(!sbp.empty()) {
if (!sbp.empty()) {
LOG_ERROR("Sub-package was not parsed completely!");
return false;
}

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

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

34
include/ur_modern_driver/ur_communication.h
View File

@@ -19,33 +19,32 @@
#ifndef UR_COMMUNICATION_H_
#define UR_COMMUNICATION_H_
#include "robot_state.h"
#include "do_output.h"
#include <vector>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <sys/time.h>
#include <thread>
#include <mutex>
#include "robot_state.h"
#include <chrono>
#include <condition_variable>
#include <sys/types.h>
#include <sys/socket.h>
#include <fcntl.h>
#include <iostream>
#include <mutex>
#include <netdb.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <netdb.h>
#include <iostream>
#include <unistd.h>
#include <chrono>
#include <fcntl.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/types.h>
#include <thread>
#include <unistd.h>
#include <vector>
class UrCommunication {
private:
int pri_sockfd_, sec_sockfd_;
struct sockaddr_in pri_serv_addr_, sec_serv_addr_;
struct hostent *server_;
struct hostent* server_;
bool keepalive_;
std::thread comThread_;
int flag_;
@@ -58,7 +57,6 @@ public:
UrCommunication(std::condition_variable& msg_cond, std::string host);
bool start();
void halt();
};
#endif /* UR_COMMUNICATION_H_ */

24
include/ur_modern_driver/ur_driver.h
View File

@@ -19,21 +19,20 @@
#ifndef UR_DRIVER_H_
#define UR_DRIVER_H_
#include <mutex>
#include <condition_variable>
#include "ur_realtime_communication.h"
#include "ur_communication.h"
#include "do_output.h"
#include <vector>
#include "ur_communication.h"
#include "ur_realtime_communication.h"
#include <condition_variable>
#include <math.h>
#include <string>
#include <sys/types.h>
#include <sys/socket.h>
#include <mutex>
#include <netinet/in.h>
#include <string>
#include <sys/socket.h>
#include <sys/types.h>
#include <vector>
#include <chrono>
class UrDriver {
private:
double maximum_time_step_;
@@ -52,15 +51,15 @@ private:
double firmware_version_;
double servoj_lookahead_time_;
double servoj_gain_;
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.);
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();
@@ -95,7 +94,6 @@ public:
void setServojTime(double t);
void setServojLookahead(double t);
void setServojGain(double g);
};
#endif /* UR_DRIVER_H_ */

31
include/ur_modern_driver/ur_hardware_interface.h
View File

@@ -58,16 +58,16 @@
#ifndef UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H
#define UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/force_torque_sensor_interface.h>
#include <hardware_interface/robot_hw.h>
#include <controller_manager/controller_manager.h>
#include <boost/scoped_ptr.hpp>
#include <ros/ros.h>
#include <math.h>
#include "do_output.h"
#include "ur_driver.h"
#include <boost/scoped_ptr.hpp>
#include <controller_manager/controller_manager.h>
#include <hardware_interface/force_torque_sensor_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>
#include <math.h>
#include <ros/ros.h>
namespace ros_control_ur {
@@ -76,9 +76,8 @@ static const double POSITION_STEP_FACTOR = 1;
static const double VELOCITY_STEP_FACTOR = 1;
/// \brief Hardware interface for a robot
class UrHardwareInterface: public hardware_interface::RobotHW {
class UrHardwareInterface : public hardware_interface::RobotHW {
public:
/**
* \brief Constructor
* \param nh - Node handle for topics.
@@ -97,13 +96,12 @@ public:
void setMaxVelChange(double inp);
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);
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);
protected:
// Startup and shutdown of the internal node inside a roscpp program
ros::NodeHandle nh_;
@@ -130,10 +128,9 @@ protected:
// Robot API
UrDriver* robot_;
};
// class
}// namespace
} // namespace
#endif

36
include/ur_modern_driver/ur_realtime_communication.h
View File

@@ -19,34 +19,34 @@
#ifndef UR_REALTIME_COMMUNICATION_H_
#define UR_REALTIME_COMMUNICATION_H_
#include "robot_state_RT.h"
#include "do_output.h"
#include <vector>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <sys/time.h>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <netdb.h>
#include <iostream>
#include <unistd.h>
#include "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>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/types.h>
#include <thread>
#include <unistd.h>
#include <vector>
class UrRealtimeCommunication {
private:
unsigned int safety_count_max_;
int sockfd_;
struct sockaddr_in serv_addr_;
struct hostent *server_;
struct hostent* server_;
std::string local_ip_;
bool keepalive_;
std::thread comThread_;
@@ -56,7 +56,6 @@ private:
unsigned int safety_count_;
void run();
public:
bool connected_;
RobotStateRT* robot_state_;
@@ -70,7 +69,6 @@ public:
void addCommandToQueue(std::string inp);
void setSafetyCountMax(uint inp);
std::string getLocalIp();
};
#endif /* UR_REALTIME_COMMUNICATION_H_ */

15
src/do_output.cpp
View File

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

95
src/robot_state.cpp
View File

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

124
src/robot_state_RT.cpp
View File

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

30
src/ros/rt_publisher.cpp
View File

@@ -1,17 +1,18 @@
#include "ur_modern_driver/ros/rt_publisher.h"
bool RTPublisher::publish_joints(RTShared &packet, ros::Time &t) {
bool RTPublisher::publish_joints(RTShared& packet, ros::Time& t)
{
sensor_msgs::JointState joint_msg;
joint_msg.header.stamp = t;
joint_msg.name = _joint_names;
for(auto const& q : packet.q_actual) {
for (auto const& q : packet.q_actual) {
joint_msg.position.push_back(q);
}
for(auto const& qd : packet.qd_actual) {
for (auto const& qd : packet.qd_actual) {
joint_msg.velocity.push_back(qd);
}
for(auto const& i : packet.i_actual) {
for (auto const& i : packet.i_actual) {
joint_msg.effort.push_back(i);
}
@@ -20,7 +21,8 @@ bool RTPublisher::publish_joints(RTShared &packet, ros::Time &t) {
return true;
}
bool RTPublisher::publish_wrench(RTShared &packet, ros::Time &t) {
bool RTPublisher::publish_wrench(RTShared& packet, ros::Time& t)
{
geometry_msgs::WrenchStamped wrench_msg;
wrench_msg.header.stamp = t;
@@ -36,7 +38,8 @@ bool RTPublisher::publish_wrench(RTShared &packet, ros::Time &t) {
return true;
}
bool RTPublisher::publish_tool(RTShared &packet, ros::Time &t) {
bool RTPublisher::publish_tool(RTShared& packet, ros::Time& t)
{
geometry_msgs::TwistStamped tool_twist;
tool_twist.header.stamp = t;
@@ -55,20 +58,25 @@ bool RTPublisher::publish_tool(RTShared &packet, ros::Time &t) {
return true;
}
bool RTPublisher::publish(RTShared &packet) {
bool RTPublisher::publish(RTShared& packet)
{
ros::Time time = ros::Time::now();
return publish_joints(packet, time) && publish_wrench(packet, time) && publish_tool(packet, time);
}
bool RTPublisher::consume(RTState_V1_6__7 &state) {
bool RTPublisher::consume(RTState_V1_6__7& state)
{
return publish(state);
}
bool RTPublisher::consume(RTState_V1_8 &state) {
bool RTPublisher::consume(RTState_V1_8& state)
{
return publish(state);
}
bool RTPublisher::consume(RTState_V3_0__1 &state) {
bool RTPublisher::consume(RTState_V3_0__1& state)
{
return publish(state);
}
bool RTPublisher::consume(RTState_V3_2__3 &state) {
bool RTPublisher::consume(RTState_V3_2__3& state)
{
return publish(state);
}

33
src/ros_main.cpp
View File

@@ -1,18 +1,18 @@
#include <cstdlib>
#include <string>
#include <chrono>
#include <thread>
#include <cstdlib>
#include <ros/ros.h>
#include <string>
#include <thread>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/ur/producer.h"
#include "ur_modern_driver/ur/state.h"
#include "ur_modern_driver/ur/rt_state.h"
#include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/messages.h"
#include "ur_modern_driver/ur/factory.h"
#include "ur_modern_driver/ros/rt_publisher.h"
#include "ur_modern_driver/ur/factory.h"
#include "ur_modern_driver/ur/messages.h"
#include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/producer.h"
#include "ur_modern_driver/ur/rt_state.h"
#include "ur_modern_driver/ur/state.h"
static const std::string IP_ADDR_ARG("~robot_ip_address");
static const std::string REVERSE_PORT_ARG("~reverse_port");
@@ -21,7 +21,6 @@ static const std::string PREFIX_ARG("~prefix");
static const std::string BASE_FRAME_ARG("~base_frame");
static const std::string TOOL_FRAME_ARG("~tool_frame");
struct ProgArgs {
public:
std::string host;
@@ -32,8 +31,9 @@ public:
bool use_sim_time;
};
bool parse_args(ProgArgs &args) {
if(!ros::param::get(IP_ADDR_ARG, args.host)) {
bool parse_args(ProgArgs& args)
{
if (!ros::param::get(IP_ADDR_ARG, args.host)) {
LOG_ERROR("robot_ip_address parameter must be set!");
return false;
}
@@ -45,13 +45,13 @@ bool parse_args(ProgArgs &args) {
return true;
}
int main(int argc, char **argv) {
int main(int argc, char** argv)
{
ros::init(argc, argv, "ur_driver");
ProgArgs args;
if(!parse_args(args)) {
if (!parse_args(args)) {
return EXIT_FAILURE;
}
@@ -83,12 +83,11 @@ int main(int argc, char **argv) {
pl.run();
while(ros::ok()) {
while (ros::ok()) {
std::this_thread::sleep_for(std::chrono::milliseconds(33));
}
pl.stop();
return EXIT_SUCCESS;
}

41
src/ur/master_board.cpp
View File

@@ -1,7 +1,8 @@
#include "ur_modern_driver/ur/master_board.h"
#include "ur_modern_driver/ur/consumer.h"
bool SharedMasterBoardData::parse_with(BinParser &bp) {
bool SharedMasterBoardData::parse_with(BinParser& bp)
{
bp.parse(analog_input_range0);
bp.parse(analog_input_range1);
bp.parse(analog_input0);
@@ -17,8 +18,9 @@ bool SharedMasterBoardData::parse_with(BinParser &bp) {
return true;
}
bool MasterBoardData_V1_X::parse_with(BinParser &bp) {
if(!bp.check_size<MasterBoardData_V1_X>())
bool MasterBoardData_V1_X::parse_with(BinParser& bp)
{
if (!bp.check_size<MasterBoardData_V1_X>())
return false;
bp.parse(digital_input_bits);
@@ -30,8 +32,8 @@ bool MasterBoardData_V1_X::parse_with(BinParser &bp) {
bp.parse(master_on_off_state);
bp.parse(euromap67_interface_installed);
if(euromap67_interface_installed) {
if(!bp.check_size(MasterBoardData_V1_X::EURO_SIZE))
if (euromap67_interface_installed) {
if (!bp.check_size(MasterBoardData_V1_X::EURO_SIZE))
return false;
bp.parse(euromap_voltage);
@@ -41,8 +43,9 @@ bool MasterBoardData_V1_X::parse_with(BinParser &bp) {
return true;
}
bool MasterBoardData_V3_0__1::parse_with(BinParser &bp) {
if(!bp.check_size<MasterBoardData_V3_0__1>())
bool MasterBoardData_V3_0__1::parse_with(BinParser& bp)
{
if (!bp.check_size<MasterBoardData_V3_0__1>())
return false;
bp.parse(digital_input_bits);
@@ -54,8 +57,8 @@ bool MasterBoardData_V3_0__1::parse_with(BinParser &bp) {
bp.parse(in_reduced_mode);
bp.parse(euromap67_interface_installed);
if(euromap67_interface_installed) {
if(!bp.check_size(MasterBoardData_V3_0__1::EURO_SIZE))
if (euromap67_interface_installed) {
if (!bp.check_size(MasterBoardData_V3_0__1::EURO_SIZE))
return false;
bp.parse(euromap_voltage);
@@ -67,10 +70,9 @@ bool MasterBoardData_V3_0__1::parse_with(BinParser &bp) {
return true;
}
bool MasterBoardData_V3_2::parse_with(BinParser &bp) {
if(!bp.check_size<MasterBoardData_V3_2>())
bool MasterBoardData_V3_2::parse_with(BinParser& bp)
{
if (!bp.check_size<MasterBoardData_V3_2>())
return false;
MasterBoardData_V3_0__1::parse_with(bp);
@@ -81,16 +83,15 @@ bool MasterBoardData_V3_2::parse_with(BinParser &bp) {
return true;
}
bool MasterBoardData_V1_X::consume_with(URStatePacketConsumer &consumer) {
bool MasterBoardData_V1_X::consume_with(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool MasterBoardData_V3_0__1::consume_with(URStatePacketConsumer &consumer) {
bool MasterBoardData_V3_0__1::consume_with(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool MasterBoardData_V3_2::consume_with(URStatePacketConsumer &consumer) {
bool MasterBoardData_V3_2::consume_with(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}

8
src/ur/messages.cpp
View File

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

33
src/ur/robot_mode.cpp
View File

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

40
src/ur/rt_state.cpp
View File

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

4
src/ur/state.cpp
View File

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

58
src/ur/stream.cpp
View File

@@ -1,13 +1,14 @@
#include <cstring>
#include <unistd.h>
#include <netinet/tcp.h>
#include <endian.h>
#include <netinet/tcp.h>
#include <unistd.h>
#include "ur_modern_driver/ur/stream.h"
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ur/stream.h"
bool URStream::connect() {
if(_initialized)
bool URStream::connect()
{
if (_initialized)
return false;
LOG_INFO("Connecting to UR @ %s:%d", _host.c_str(), _port);
@@ -23,20 +24,20 @@ bool URStream::connect() {
hints.ai_socktype = SOCK_STREAM;
hints.ai_flags = AI_PASSIVE;
if(getaddrinfo(_host.c_str(), service.c_str(), &hints, &result) != 0) {
if (getaddrinfo(_host.c_str(), service.c_str(), &hints, &result) != 0) {
LOG_ERROR("Failed to get host name");
return false;
}
//loop through the list of addresses untill we find one that's connectable
for(struct addrinfo *p = result; p != nullptr; p = p->ai_next) {
for (struct addrinfo* p = result; p != nullptr; p = p->ai_next) {
_socket_fd = socket(p->ai_family, p->ai_socktype, p->ai_protocol);
if(_socket_fd == -1) //socket error?
if (_socket_fd == -1) //socket error?
continue;
if(::connect(_socket_fd, p->ai_addr, p->ai_addrlen) != 0) {
if(_stopping)
if (::connect(_socket_fd, p->ai_addr, p->ai_addrlen) != 0) {
if (_stopping)
break;
else
continue; //try next addrinfo if connect fails
@@ -51,14 +52,15 @@ bool URStream::connect() {
}
freeaddrinfo(result);
if(!_initialized)
if (!_initialized)
LOG_ERROR("Connection failed");
return _initialized;
}
void URStream::disconnect() {
if(!_initialized || _stopping)
void URStream::disconnect()
{
if (!_initialized || _stopping)
return;
_stopping = true;
@@ -66,10 +68,11 @@ void URStream::disconnect() {
_initialized = false;
}
ssize_t URStream::send(uint8_t *buf, size_t buf_len) {
if(!_initialized)
ssize_t URStream::send(uint8_t* buf, size_t buf_len)
{
if (!_initialized)
return -1;
if(_stopping)
if (_stopping)
return 0;
size_t total = 0;
@@ -77,9 +80,9 @@ ssize_t URStream::send(uint8_t *buf, size_t buf_len) {
//TODO: handle reconnect?
//handle partial sends
while(total < buf_len) {
ssize_t sent = ::send(_socket_fd, buf+total, remaining, 0);
if(sent <= 0)
while (total < buf_len) {
ssize_t sent = ::send(_socket_fd, buf + total, remaining, 0);
if (sent <= 0)
return _stopping ? 0 : sent;
total += sent;
remaining -= sent;
@@ -88,24 +91,25 @@ ssize_t URStream::send(uint8_t *buf, size_t buf_len) {
return total;
}
ssize_t URStream::receive(uint8_t *buf, size_t buf_len) {
if(!_initialized)
ssize_t URStream::receive(uint8_t* buf, size_t buf_len)
{
if (!_initialized)
return -1;
if(_stopping)
if (_stopping)
return 0;
size_t remainder = sizeof(int32_t);
uint8_t *buf_pos = buf;
uint8_t* buf_pos = buf;
bool initial = true;
do {
ssize_t read = recv(_socket_fd, buf_pos, remainder, 0);
if(read <= 0) //failed reading from socket
if (read <= 0) //failed reading from socket
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 %d is larger than buffer %d, discarding.", remainder, buf_len);
return -1;
}
@@ -114,7 +118,7 @@ ssize_t URStream::receive(uint8_t *buf, size_t buf_len) {
buf_pos += read;
remainder -= read;
} while(remainder > 0);
} while (remainder > 0);
return buf_pos - buf;
}

52
src/ur_communication.cpp
View File

@@ -19,10 +19,11 @@
#include "ur_modern_driver/ur_communication.h"
UrCommunication::UrCommunication(std::condition_variable& msg_cond,
std::string host) {
std::string host)
{
robot_state_ = new RobotState(msg_cond);
bzero((char *) &pri_serv_addr_, sizeof(pri_serv_addr_));
bzero((char *) &sec_serv_addr_, sizeof(sec_serv_addr_));
bzero((char*)&pri_serv_addr_, sizeof(pri_serv_addr_));
bzero((char*)&sec_serv_addr_, sizeof(sec_serv_addr_));
pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (pri_sockfd_ < 0) {
print_fatal("ERROR opening socket pri_sockfd");
@@ -37,43 +38,45 @@ UrCommunication::UrCommunication(std::condition_variable& msg_cond,
}
pri_serv_addr_.sin_family = AF_INET;
sec_serv_addr_.sin_family = AF_INET;
bcopy((char *) server_->h_addr, (char *)&pri_serv_addr_.sin_addr.s_addr, server_->h_length);
bcopy((char *) server_->h_addr, (char *)&sec_serv_addr_.sin_addr.s_addr, server_->h_length);
bcopy((char*)server_->h_addr, (char*)&pri_serv_addr_.sin_addr.s_addr, server_->h_length);
bcopy((char*)server_->h_addr, (char*)&sec_serv_addr_.sin_addr.s_addr, server_->h_length);
pri_serv_addr_.sin_port = htons(30001);
sec_serv_addr_.sin_port = htons(30002);
flag_ = 1;
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_,
sizeof(int));
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_,
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
sizeof(int));
setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_,
setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_,
sizeof(int));
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_,
sizeof(int));
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_,
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
sizeof(int));
setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_,
setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_,
sizeof(int));
fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK);
connected_ = false;
keepalive_ = false;
}
bool UrCommunication::start() {
bool UrCommunication::start()
{
keepalive_ = true;
uint8_t buf[512];
unsigned int bytes_read;
std::string cmd;
bzero(buf, 512);
print_debug("Acquire firmware version: Connecting...");
if (connect(pri_sockfd_, (struct sockaddr *) &pri_serv_addr_,
sizeof(pri_serv_addr_)) < 0) {
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_,
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.
@@ -89,7 +92,7 @@ bool UrCommunication::start() {
fd_set writefds;
struct timeval timeout;
connect(sec_sockfd_, (struct sockaddr *) &sec_serv_addr_,
connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_,
sizeof(sec_serv_addr_));
FD_ZERO(&writefds);
FD_SET(sec_sockfd_, &writefds);
@@ -107,12 +110,14 @@ bool UrCommunication::start() {
return true;
}
void UrCommunication::halt() {
void UrCommunication::halt()
{
keepalive_ = false;
comThread_.join();
}
void UrCommunication::run() {
void UrCommunication::run()
{
uint8_t buf[2048];
int bytes_read;
bzero(buf, 2048);
@@ -129,7 +134,7 @@ void UrCommunication::run() {
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));
(char*)&flag_, sizeof(int));
robot_state_->unpack(buf, bytes_read);
} else {
connected_ = false;
@@ -145,18 +150,18 @@ void UrCommunication::run() {
print_fatal("ERROR opening secondary socket");
}
flag_ = 1;
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_,
sizeof(int));
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_,
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
sizeof(int));
setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_,
setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_,
sizeof(int));
fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK);
while (keepalive_ && !connected_) {
std::this_thread::sleep_for(std::chrono::seconds(10));
fd_set writefds;
connect(sec_sockfd_, (struct sockaddr *) &sec_serv_addr_,
connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_,
sizeof(sec_serv_addr_));
FD_ZERO(&writefds);
FD_SET(sec_sockfd_, &writefds);
@@ -178,4 +183,3 @@ void UrCommunication::run() {
std::this_thread::sleep_for(std::chrono::milliseconds(500));
close(sec_sockfd_);
}

122
src/ur_driver.cpp
View File

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

51
src/ur_hardware_interface.cpp
View File

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

57
src/ur_realtime_communication.cpp
View File

@@ -20,9 +20,10 @@
UrRealtimeCommunication::UrRealtimeCommunication(
std::condition_variable& msg_cond, std::string host,
unsigned int safety_count_max) {
unsigned int safety_count_max)
{
robot_state_ = new RobotStateRT(msg_cond);
bzero((char *) &serv_addr_, sizeof(serv_addr_));
bzero((char*)&serv_addr_, sizeof(serv_addr_));
sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sockfd_ < 0) {
print_fatal("ERROR opening socket");
@@ -32,12 +33,12 @@ UrRealtimeCommunication::UrRealtimeCommunication(
print_fatal("ERROR, no such host");
}
serv_addr_.sin_family = AF_INET;
bcopy((char *) server_->h_addr, (char *)&serv_addr_.sin_addr.s_addr, server_->h_length);
bcopy((char*)server_->h_addr, (char*)&serv_addr_.sin_addr.s_addr, server_->h_length);
serv_addr_.sin_port = htons(30003);
flag_ = 1;
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int));
setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, sizeof(int));
setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int));
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
fcntl(sockfd_, F_SETFL, O_NONBLOCK);
connected_ = false;
keepalive_ = false;
@@ -45,14 +46,15 @@ UrRealtimeCommunication::UrRealtimeCommunication(
safety_count_max_ = safety_count_max;
}
bool UrRealtimeCommunication::start() {
bool UrRealtimeCommunication::start()
{
fd_set writefds;
struct timeval timeout;
keepalive_ = true;
print_debug("Realtime port: Connecting...");
connect(sockfd_, (struct sockaddr *) &serv_addr_, sizeof(serv_addr_));
connect(sockfd_, (struct sockaddr*)&serv_addr_, sizeof(serv_addr_));
FD_ZERO(&writefds);
FD_SET(sockfd_, &writefds);
timeout.tv_sec = 10;
@@ -66,7 +68,7 @@ bool UrRealtimeCommunication::start() {
}
sockaddr_in name;
socklen_t namelen = sizeof(name);
int err = getsockname(sockfd_, (sockaddr*) &name, &namelen);
int err = getsockname(sockfd_, (sockaddr*)&name, &namelen);
if (err < 0) {
print_fatal("Could not get local IP");
close(sockfd_);
@@ -79,12 +81,14 @@ bool UrRealtimeCommunication::start() {
return true;
}
void UrRealtimeCommunication::halt() {
void UrRealtimeCommunication::halt()
{
keepalive_ = false;
comThread_.join();
}
void UrRealtimeCommunication::addCommandToQueue(std::string inp) {
void UrRealtimeCommunication::addCommandToQueue(std::string inp)
{
int bytes_written;
if (inp.back() != '\n') {
inp.append("\n");
@@ -92,30 +96,31 @@ void UrRealtimeCommunication::addCommandToQueue(std::string inp) {
if (connected_)
bytes_written = write(sockfd_, inp.c_str(), inp.length());
else
print_error("Could not send command \"" +inp + "\". The robot is not connected! Command is discarded" );
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) {
double q3, double q4, double q5, double acc)
{
char cmd[1024];
if( robot_state_->getVersion() >= 3.1 ) {
if (robot_state_->getVersion() >= 3.1) {
sprintf(cmd,
"speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n",
q0, q1, q2, q3, q4, q5, acc);
}
else {
} else {
sprintf(cmd,
"speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n",
q0, q1, q2, q3, q4, q5, acc);
}
addCommandToQueue((std::string) (cmd));
addCommandToQueue((std::string)(cmd));
if (q0 != 0. or q1 != 0. or q2 != 0. or q3 != 0. or q4 != 0. or q5 != 0.) {
//If a joint speed is set, make sure we stop it again after some time if the user doesn't
safety_count_ = 0;
}
}
void UrRealtimeCommunication::run() {
void UrRealtimeCommunication::run()
{
uint8_t buf[2048];
int bytes_read;
bzero(buf, 2048);
@@ -132,7 +137,7 @@ void UrRealtimeCommunication::run() {
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_,
setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
sizeof(int));
robot_state_->unpack(buf);
if (safety_count_ == safety_count_max_) {
@@ -152,19 +157,19 @@ void UrRealtimeCommunication::run() {
print_fatal("ERROR opening socket");
}
flag_ = 1;
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_,
sizeof(int));
setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_,
setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
sizeof(int));
setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_,
setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_,
sizeof(int));
fcntl(sockfd_, F_SETFL, O_NONBLOCK);
while (keepalive_ && !connected_) {
std::this_thread::sleep_for(std::chrono::seconds(10));
fd_set writefds;
connect(sockfd_, (struct sockaddr *) &serv_addr_,
connect(sockfd_, (struct sockaddr*)&serv_addr_,
sizeof(serv_addr_));
FD_ZERO(&writefds);
FD_SET(sockfd_, &writefds);
@@ -184,10 +189,12 @@ void UrRealtimeCommunication::run() {
close(sockfd_);
}
void UrRealtimeCommunication::setSafetyCountMax(uint inp) {
void UrRealtimeCommunication::setSafetyCountMax(uint inp)
{
safety_count_max_ = inp;
}
std::string UrRealtimeCommunication::getLocalIp() {
std::string UrRealtimeCommunication::getLocalIp()
{
return local_ip_;
}

224
src/ur_ros_wrapper.cpp
View File

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