From c42bdc4075070e0350e4e601f58be01d870ffc8e Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Sun, 29 Jan 2017 04:18:13 +0100 Subject: [PATCH 001/114] Implemented abstract pipeline design --- include/ur_modern_driver/pipeline.h | 92 ++ include/ur_modern_driver/queue/LICENSE.md | 28 + include/ur_modern_driver/queue/atomicops.h | 664 ++++++++++++++ .../queue/readerwriterqueue.h | 815 ++++++++++++++++++ src/pipeline.cpp | 23 + 5 files changed, 1622 insertions(+) create mode 100644 include/ur_modern_driver/pipeline.h create mode 100644 include/ur_modern_driver/queue/LICENSE.md create mode 100644 include/ur_modern_driver/queue/atomicops.h create mode 100644 include/ur_modern_driver/queue/readerwriterqueue.h create mode 100644 src/pipeline.cpp diff --git a/include/ur_modern_driver/pipeline.h b/include/ur_modern_driver/pipeline.h new file mode 100644 index 000000000..ac2fe535d --- /dev/null +++ b/include/ur_modern_driver/pipeline.h @@ -0,0 +1,92 @@ +#pragma once + +#include +#include +#include "ur_modern_driver/queue/readerwriterqueue.h" + +using namespace moodycamel; +using namespace std; + + +template +class IProducer { +public: + virtual void setup_producer() = 0; + virtual void teardown_producer() = 0; + virtual void stop_producer() = 0; + virtual unique_ptr try_get() = 0; +}; + +template +class IConsumer { +public: + virtual void setup_consumer() = 0; + virtual void teardown_consumer() = 0; + virtual void stop_consumer() = 0; + virtual bool push(unique_ptr product) = 0; +}; + +template +class Pipeline { +private: + IProducer &_producer; + IConsumer &_consumer; + BlockingReaderWriterQueue> _queue; + atomic _running; + thread _pThread, _cThread; + + void run_producer() { + _producer.setup_producer(); + while(_running) { + unique_ptr product(_producer.try_get()); + + if(product == nullptr) + break; + + if(!_queue.try_enqueue(std::move(product))) { + //log dropped product + } + } + _producer.teardown_producer(); + //todo cleanup + } + + void run_consumer() { + _consumer.setup_consumer(); + while(_running) { + unique_ptr product; + _queue.wait_dequeue(product); + if(!_consumer.push(std::move(product))) + break; + } + _consumer.teardown_consumer(); + //todo cleanup + } +public: + Pipeline(IProducer &producer, IConsumer &consumer) + : _producer(producer), + _consumer(consumer), + _queue{32}, + _running{false} + { } + + void run() { + if(_running) + return; + + _running = true; + _pThread = thread(&Pipeline::run_producer, this); + _cThread = thread(&Pipeline::run_consumer, this); + } + + void stop() { + if(!_running) + return; + + _consumer.stop_consumer(); + _producer.stop_producer(); + + _pThread.join(); + _cThread.join(); + } +}; \ No newline at end of file diff --git a/include/ur_modern_driver/queue/LICENSE.md b/include/ur_modern_driver/queue/LICENSE.md new file mode 100644 index 000000000..76d802ecc --- /dev/null +++ b/include/ur_modern_driver/queue/LICENSE.md @@ -0,0 +1,28 @@ +This license applies to all the code in this repository except that written by third +parties, namely the files in benchmarks/ext, which have their own licenses, and Jeff +Preshing's semaphore implementation (used in the blocking queue) which has a zlib +license (embedded in atomicops.h). + +Simplified BSD License: + +Copyright (c) 2013-2015, Cameron Desrochers +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +- Redistributions of source code must retain the above copyright notice, this list of +conditions and the following disclaimer. +- Redistributions in binary form must reproduce the above copyright notice, this list of +conditions and the following disclaimer in the documentation and/or other materials +provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL +THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, +EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/include/ur_modern_driver/queue/atomicops.h b/include/ur_modern_driver/queue/atomicops.h new file mode 100644 index 000000000..c375710cd --- /dev/null +++ b/include/ur_modern_driver/queue/atomicops.h @@ -0,0 +1,664 @@ +// ©2013-2016 Cameron Desrochers. +// Distributed under the simplified BSD license (see the license file that +// should have come with this header). +// Uses Jeff Preshing's semaphore implementation (under the terms of its +// separate zlib license, embedded below). + +#pragma once + +// Provides portable (VC++2010+, Intel ICC 13, GCC 4.7+, and anything C++11 compliant) implementation +// of low-level memory barriers, plus a few semi-portable utility macros (for inlining and alignment). +// Also has a basic atomic type (limited to hardware-supported atomics with no memory ordering guarantees). +// Uses the AE_* prefix for macros (historical reasons), and the "moodycamel" namespace for symbols. + +#include +#include +#include +#include +#include + +// Platform detection +#if defined(__INTEL_COMPILER) +#define AE_ICC +#elif defined(_MSC_VER) +#define AE_VCPP +#elif defined(__GNUC__) +#define AE_GCC +#endif + +#if defined(_M_IA64) || defined(__ia64__) +#define AE_ARCH_IA64 +#elif defined(_WIN64) || defined(__amd64__) || defined(_M_X64) || defined(__x86_64__) +#define AE_ARCH_X64 +#elif defined(_M_IX86) || defined(__i386__) +#define AE_ARCH_X86 +#elif defined(_M_PPC) || defined(__powerpc__) +#define AE_ARCH_PPC +#else +#define AE_ARCH_UNKNOWN +#endif + + +// AE_UNUSED +#define AE_UNUSED(x) ((void)x) + + +// AE_FORCEINLINE +#if defined(AE_VCPP) || defined(AE_ICC) +#define AE_FORCEINLINE __forceinline +#elif defined(AE_GCC) +//#define AE_FORCEINLINE __attribute__((always_inline)) +#define AE_FORCEINLINE inline +#else +#define AE_FORCEINLINE inline +#endif + + +// AE_ALIGN +#if defined(AE_VCPP) || defined(AE_ICC) +#define AE_ALIGN(x) __declspec(align(x)) +#elif defined(AE_GCC) +#define AE_ALIGN(x) __attribute__((aligned(x))) +#else +// Assume GCC compliant syntax... +#define AE_ALIGN(x) __attribute__((aligned(x))) +#endif + + +// Portable atomic fences implemented below: + +namespace moodycamel { + +enum memory_order { + memory_order_relaxed, + memory_order_acquire, + memory_order_release, + memory_order_acq_rel, + memory_order_seq_cst, + + // memory_order_sync: Forces a full sync: + // #LoadLoad, #LoadStore, #StoreStore, and most significantly, #StoreLoad + memory_order_sync = memory_order_seq_cst +}; + +} // end namespace moodycamel + +#if (defined(AE_VCPP) && (_MSC_VER < 1700 || defined(__cplusplus_cli))) || defined(AE_ICC) +// VS2010 and ICC13 don't support std::atomic_*_fence, implement our own fences + +#include + +#if defined(AE_ARCH_X64) || defined(AE_ARCH_X86) +#define AeFullSync _mm_mfence +#define AeLiteSync _mm_mfence +#elif defined(AE_ARCH_IA64) +#define AeFullSync __mf +#define AeLiteSync __mf +#elif defined(AE_ARCH_PPC) +#include +#define AeFullSync __sync +#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` +#ifdef __cplusplus_cli +#pragma managed(push, off) +#endif +#endif + +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); + } +} + +// x86/x64 have a strong memory model -- all loads and stores have +// acquire and release semantics automatically (so only need compiler +// barriers for those). +#if defined(AE_ARCH_X86) || defined(AE_ARCH_X64) +AE_FORCEINLINE void fence(memory_order order) +{ + switch (order) { + case memory_order_relaxed: break; + case memory_order_acquire: _ReadBarrier(); break; + case memory_order_release: _WriteBarrier(); break; + case memory_order_acq_rel: _ReadWriteBarrier(); break; + case memory_order_seq_cst: + _ReadWriteBarrier(); + AeFullSync(); + _ReadWriteBarrier(); + break; + default: assert(false); + } +} +#else +AE_FORCEINLINE void fence(memory_order order) +{ + // Non-specialized arch, use heavier memory barriers everywhere just in case :-( + switch (order) { + case memory_order_relaxed: + break; + case memory_order_acquire: + _ReadBarrier(); + AeLiteSync(); + _ReadBarrier(); + break; + case memory_order_release: + _WriteBarrier(); + AeLiteSync(); + _WriteBarrier(); + break; + case memory_order_acq_rel: + _ReadWriteBarrier(); + AeLiteSync(); + _ReadWriteBarrier(); + break; + case memory_order_seq_cst: + _ReadWriteBarrier(); + AeFullSync(); + _ReadWriteBarrier(); + break; + default: assert(false); + } +} +#endif +} // end namespace moodycamel +#else +// Use standard library of atomics +#include + +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); + } +} + +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); + } +} + +} // end namespace moodycamel + +#endif + + +#if !defined(AE_VCPP) || (_MSC_VER >= 1700 && !defined(__cplusplus_cli)) +#define AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC +#endif + +#ifdef AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC +#include +#endif +#include + +// WARNING: *NOT* A REPLACEMENT FOR std::atomic. READ CAREFULLY: +// Provides basic support for atomic variables -- no memory ordering guarantees are provided. +// The guarantee of atomicity is only made for types that already have atomic load and store guarantees +// at the hardware level -- on most platforms this generally means aligned pointers and integers (only). +namespace moodycamel { +template +class weak_atomic +{ +public: + weak_atomic() { } +#ifdef AE_VCPP +#pragma warning(disable: 4100) // Get rid of (erroneous) 'unreferenced formal parameter' warning +#endif + template weak_atomic(U&& x) : value(std::forward(x)) { } +#ifdef __cplusplus_cli + // Work around bug with universal reference/nullptr combination that only appears when /clr is on + weak_atomic(nullptr_t) : value(nullptr) { } +#endif + 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) +#endif + + AE_FORCEINLINE operator T() const { return load(); } + + +#ifndef AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC + template AE_FORCEINLINE weak_atomic const& operator=(U&& x) { value = std::forward(x); return *this; } + AE_FORCEINLINE weak_atomic const& operator=(weak_atomic const& other) { value = other.value; return *this; } + + AE_FORCEINLINE T load() const { return value; } + + AE_FORCEINLINE T fetch_add_acquire(T increment) + { +#if defined(AE_ARCH_X64) || defined(AE_ARCH_X86) + if (sizeof(T) == 4) return _InterlockedExchangeAdd((long volatile*)&value, (long)increment); +#if defined(_M_AMD64) + else if (sizeof(T) == 8) return _InterlockedExchangeAdd64((long long volatile*)&value, (long long)increment); +#endif +#else +#error Unsupported platform +#endif + assert(false && "T must be either a 32 or 64 bit type"); + return value; + } + + AE_FORCEINLINE T fetch_add_release(T increment) + { +#if defined(AE_ARCH_X64) || defined(AE_ARCH_X86) + if (sizeof(T) == 4) return _InterlockedExchangeAdd((long volatile*)&value, (long)increment); +#if defined(_M_AMD64) + else if (sizeof(T) == 8) return _InterlockedExchangeAdd64((long long volatile*)&value, (long long)increment); +#endif +#else +#error Unsupported platform +#endif + assert(false && "T must be either a 32 or 64 bit type"); + return value; + } +#else + template + AE_FORCEINLINE weak_atomic const& operator=(U&& x) + { + value.store(std::forward(x), std::memory_order_relaxed); + return *this; + } + + AE_FORCEINLINE weak_atomic const& operator=(weak_atomic const& other) + { + value.store(other.value.load(std::memory_order_relaxed), std::memory_order_relaxed); + return *this; + } + + AE_FORCEINLINE T load() const { return value.load(std::memory_order_relaxed); } + + AE_FORCEINLINE T fetch_add_acquire(T increment) + { + return value.fetch_add(increment, std::memory_order_acquire); + } + + AE_FORCEINLINE T fetch_add_release(T increment) + { + return value.fetch_add(increment, std::memory_order_release); + } +#endif + + +private: +#ifndef AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC + // No std::atomic support, but still need to circumvent compiler optimizations. + // `volatile` will make memory access slow, but is guaranteed to be reliable. + volatile T value; +#else + std::atomic value; +#endif +}; + +} // end namespace moodycamel + + + +// Portable single-producer, single-consumer semaphore below: + +#if defined(_WIN32) +// Avoid including windows.h in a header; we only need a handful of +// items, so we'll redeclare them here (this is relatively safe since +// the API generally has to remain stable between Windows versions). +// 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); +} +#elif defined(__MACH__) +#include +#elif defined(__unix__) +#include +#endif + +namespace moodycamel +{ + // Code in the spsc_sema namespace below is an adaptation of Jeff Preshing's + // portable + lightweight semaphore implementations, originally from + // https://github.com/preshing/cpp11-on-multicore/blob/master/common/sema.h + // LICENSE: + // Copyright (c) 2015 Jeff Preshing + // + // This software is provided 'as-is', without any express or implied + // warranty. In no event will the authors be held liable for any damages + // arising from the use of this software. + // + // Permission is granted to anyone to use this software for any purpose, + // including commercial applications, and to alter it and redistribute it + // freely, subject to the following restrictions: + // + // 1. The origin of this software must not be misrepresented; you must not + // claim that you wrote the original software. If you use this software + // in a product, an acknowledgement in the product documentation would be + // appreciated but is not required. + // 2. Altered source versions must be plainly marked as such, and must not be + // misrepresented as being the original software. + // 3. This notice may not be removed or altered from any source distribution. + namespace spsc_sema + { +#if defined(_WIN32) + class Semaphore + { + private: + void* m_hSema; + + Semaphore(const Semaphore& other); + Semaphore& operator=(const Semaphore& other); + + public: + Semaphore(int initialCount = 0) + { + assert(initialCount >= 0); + const long maxLong = 0x7fffffff; + m_hSema = CreateSemaphoreW(nullptr, initialCount, maxLong, nullptr); + } + + ~Semaphore() + { + CloseHandle(m_hSema); + } + + void wait() + { + const unsigned long infinite = 0xffffffff; + WaitForSingleObject(m_hSema, infinite); + } + + bool try_wait() + { + const unsigned long RC_WAIT_TIMEOUT = 0x00000102; + return WaitForSingleObject(m_hSema, 0) != RC_WAIT_TIMEOUT; + } + + bool timed_wait(std::uint64_t usecs) + { + const unsigned long RC_WAIT_TIMEOUT = 0x00000102; + return WaitForSingleObject(m_hSema, (unsigned long)(usecs / 1000)) != RC_WAIT_TIMEOUT; + } + + void signal(int count = 1) + { + ReleaseSemaphore(m_hSema, count, nullptr); + } + }; +#elif defined(__MACH__) + //--------------------------------------------------------- + // Semaphore (Apple iOS and OSX) + // Can't use POSIX semaphores due to http://lists.apple.com/archives/darwin-kernel/2009/Apr/msg00010.html + //--------------------------------------------------------- + class Semaphore + { + private: + semaphore_t m_sema; + + Semaphore(const Semaphore& other); + Semaphore& operator=(const Semaphore& other); + + public: + Semaphore(int initialCount = 0) + { + assert(initialCount >= 0); + semaphore_create(mach_task_self(), &m_sema, SYNC_POLICY_FIFO, initialCount); + } + + ~Semaphore() + { + semaphore_destroy(mach_task_self(), m_sema); + } + + void wait() + { + semaphore_wait(m_sema); + } + + bool try_wait() + { + return timed_wait(0); + } + + bool timed_wait(std::int64_t timeout_usecs) + { + mach_timespec_t ts; + ts.tv_sec = timeout_usecs / 1000000; + ts.tv_nsec = (timeout_usecs % 1000000) * 1000; + + // added in OSX 10.10: https://developer.apple.com/library/prerelease/mac/documentation/General/Reference/APIDiffsMacOSX10_10SeedDiff/modules/Darwin.html + kern_return_t rc = semaphore_timedwait(m_sema, ts); + + return rc != KERN_OPERATION_TIMED_OUT; + } + + void signal() + { + semaphore_signal(m_sema); + } + + void signal(int count) + { + while (count-- > 0) + { + semaphore_signal(m_sema); + } + } + }; +#elif defined(__unix__) + //--------------------------------------------------------- + // Semaphore (POSIX, Linux) + //--------------------------------------------------------- + class Semaphore + { + private: + sem_t m_sema; + + Semaphore(const Semaphore& other); + Semaphore& operator=(const Semaphore& other); + + public: + Semaphore(int initialCount = 0) + { + assert(initialCount >= 0); + sem_init(&m_sema, 0, initialCount); + } + + ~Semaphore() + { + sem_destroy(&m_sema); + } + + void wait() + { + // http://stackoverflow.com/questions/2013181/gdb-causes-sem-wait-to-fail-with-eintr-error + int rc; + do + { + rc = sem_wait(&m_sema); + } + while (rc == -1 && errno == EINTR); + } + + bool try_wait() + { + int rc; + do { + rc = sem_trywait(&m_sema); + } while (rc == -1 && errno == EINTR); + return !(rc == -1 && errno == EAGAIN); + } + + bool timed_wait(std::uint64_t usecs) + { + struct timespec ts; + const int usecs_in_1_sec = 1000000; + const int nsecs_in_1_sec = 1000000000; + clock_gettime(CLOCK_REALTIME, &ts); + ts.tv_sec += usecs / usecs_in_1_sec; + ts.tv_nsec += (usecs % usecs_in_1_sec) * 1000; + // sem_timedwait bombs if you have more than 1e9 in tv_nsec + // so we have to clean things up before passing it in + if (ts.tv_nsec > nsecs_in_1_sec) { + ts.tv_nsec -= nsecs_in_1_sec; + ++ts.tv_sec; + } + + int rc; + do { + rc = sem_timedwait(&m_sema, &ts); + } while (rc == -1 && errno == EINTR); + return !(rc == -1 && errno == ETIMEDOUT); + } + + void signal() + { + sem_post(&m_sema); + } + + void signal(int count) + { + while (count-- > 0) + { + sem_post(&m_sema); + } + } + }; +#else +#error Unsupported platform! (No semaphore wrapper available) +#endif + + //--------------------------------------------------------- + // LightweightSemaphore + //--------------------------------------------------------- + class LightweightSemaphore + { + public: + typedef std::make_signed::type ssize_t; + + private: + weak_atomic m_count; + Semaphore m_sema; + + bool waitWithPartialSpinning(std::int64_t timeout_usecs = -1) + { + ssize_t oldCount; + // Is there a better way to set the initial spin count? + // If we lower it to 1000, testBenaphore becomes 15x slower on my Core i7-5930K Windows PC, + // as threads start hitting the kernel semaphore. + int spin = 10000; + while (--spin >= 0) + { + if (m_count.load() > 0) + { + m_count.fetch_add_acquire(-1); + return true; + } + compiler_fence(memory_order_acquire); // Prevent the compiler from collapsing the loop. + } + oldCount = m_count.fetch_add_acquire(-1); + if (oldCount > 0) + return true; + if (timeout_usecs < 0) + { + m_sema.wait(); + return true; + } + if (m_sema.timed_wait(timeout_usecs)) + return true; + // At this point, we've timed out waiting for the semaphore, but the + // count is still decremented indicating we may still be waiting on + // it. So we have to re-adjust the count, but only if the semaphore + // wasn't signaled enough times for us too since then. If it was, we + // need to release the semaphore too. + while (true) + { + oldCount = m_count.fetch_add_release(1); + if (oldCount < 0) + return false; // successfully restored things to the way they were + // Oh, the producer thread just signaled the semaphore after all. Try again: + oldCount = m_count.fetch_add_acquire(-1); + if (oldCount > 0 && m_sema.try_wait()) + return true; + } + } + + public: + LightweightSemaphore(ssize_t initialCount = 0) : m_count(initialCount) + { + assert(initialCount >= 0); + } + + bool tryWait() + { + if (m_count.load() > 0) + { + m_count.fetch_add_acquire(-1); + return true; + } + return false; + } + + void wait() + { + if (!tryWait()) + waitWithPartialSpinning(); + } + + bool wait(std::int64_t timeout_usecs) + { + return tryWait() || waitWithPartialSpinning(timeout_usecs); + } + + void signal(ssize_t count = 1) + { + assert(count >= 0); + ssize_t oldCount = m_count.fetch_add_release(count); + assert(oldCount >= -1); + if (oldCount < 0) + { + m_sema.signal(1); + } + } + + ssize_t availableApprox() const + { + ssize_t count = m_count.load(); + return count > 0 ? count : 0; + } + }; + } // end namespace spsc_sema +} // end namespace moodycamel + +#if defined(AE_VCPP) && (_MSC_VER < 1700 || defined(__cplusplus_cli)) +#pragma warning(pop) +#ifdef __cplusplus_cli +#pragma managed(pop) +#endif +#endif diff --git a/include/ur_modern_driver/queue/readerwriterqueue.h b/include/ur_modern_driver/queue/readerwriterqueue.h new file mode 100644 index 000000000..ec465d694 --- /dev/null +++ b/include/ur_modern_driver/queue/readerwriterqueue.h @@ -0,0 +1,815 @@ +// ©2013-2016 Cameron Desrochers. +// Distributed under the simplified BSD license (see the license file that +// should have come with this header). + +#pragma once + +#include "atomicops.h" +#include +#include +#include +#include +#include +#include +#include // For malloc/free/abort & size_t +#if __cplusplus > 199711L || _MSC_VER >= 1700 // C++11 or VS2012 +#include +#endif + + +// A lock-free queue for a single-consumer, single-producer architecture. +// The queue is also wait-free in the common path (except if more memory +// needs to be allocated, in which case malloc is called). +// Allocates memory sparingly (O(lg(n) times, amortized), and only once if +// the original maximum size estimate is never exceeded. +// Tested on x86/x64 processors, but semantics should be correct for all +// architectures (given the right implementations in atomicops.h), provided +// that aligned integer and pointer accesses are naturally atomic. +// Note that there should only be one consumer thread and producer thread; +// Switching roles of the threads, or using multiple consecutive threads for +// one role, is not safe unless properly synchronized. +// Using the queue exclusively from one thread is fine, though a bit silly. + +#ifndef MOODYCAMEL_CACHE_LINE_SIZE +#define MOODYCAMEL_CACHE_LINE_SIZE 64 +#endif + +#ifndef MOODYCAMEL_EXCEPTIONS_ENABLED +#if (defined(_MSC_VER) && defined(_CPPUNWIND)) || (defined(__GNUC__) && defined(__EXCEPTIONS)) || (!defined(_MSC_VER) && !defined(__GNUC__)) +#define MOODYCAMEL_EXCEPTIONS_ENABLED +#endif +#endif + +#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 +#endif + +namespace moodycamel { + +template +class ReaderWriterQueue +{ + // Design: Based on a queue-of-queues. The low-level queues are just + // circular buffers with front and tail indices indicating where the + // next element to dequeue is and where the next element can be enqueued, + // respectively. Each low-level queue is called a "block". Each block + // wastes exactly one element's worth of space to keep the design simple + // (if front == tail then the queue is empty, and can't be full). + // The high-level queue is a circular linked list of blocks; again there + // is a front and tail, but this time they are pointers to the blocks. + // The front block is where the next element to be dequeued is, provided + // the block is not empty. The back block is where elements are to be + // enqueued, provided the block is not full. + // The producer thread owns all the tail indices/pointers. The consumer + // thread owns all the front indices/pointers. Both threads read each + // other's variables, but only the owning thread updates them. E.g. After + // the consumer reads the producer's tail, the tail may change before the + // consumer is done dequeuing an object, but the consumer knows the tail + // will never go backwards, only forwards. + // If there is no room to enqueue an object, an additional block (of + // equal size to the last block) is added. Blocks are never removed. + +public: + // Constructs a queue that can hold maxSize elements without further + // allocations. If more than MAX_BLOCK_SIZE elements are requested, + // then several blocks of MAX_BLOCK_SIZE each are reserved (including + // at least one extra buffer block). + explicit ReaderWriterQueue(size_t maxSize = 15) +#ifndef NDEBUG + : enqueuing(false) + ,dequeuing(false) +#endif + { + assert(maxSize > 0); + assert(MAX_BLOCK_SIZE == ceilToPow2(MAX_BLOCK_SIZE) && "MAX_BLOCK_SIZE must be a power of 2"); + assert(MAX_BLOCK_SIZE >= 2 && "MAX_BLOCK_SIZE must be at least 2"); + + Block* firstBlock = nullptr; + + largestBlockSize = ceilToPow2(maxSize + 1); // We need a spare slot to fit maxSize elements in the block + if (largestBlockSize > MAX_BLOCK_SIZE * 2) { + // We need a spare block in case the producer is writing to a different block the consumer is reading from, and + // wants to enqueue the maximum number of elements. We also need a spare element in each block to avoid the ambiguity + // between front == tail meaning "empty" and "full". + // So the effective number of slots that are guaranteed to be usable at any time is the block size - 1 times the + // number of blocks - 1. Solving for maxSize and applying a ceiling to the division gives us (after simplifying): + size_t initialBlockCount = (maxSize + MAX_BLOCK_SIZE * 2 - 3) / (MAX_BLOCK_SIZE - 1); + largestBlockSize = MAX_BLOCK_SIZE; + Block* lastBlock = nullptr; + for (size_t i = 0; i != initialBlockCount; ++i) { + auto block = make_block(largestBlockSize); + if (block == nullptr) { +#ifdef MOODYCAMEL_EXCEPTIONS_ENABLED + throw std::bad_alloc(); +#else + abort(); +#endif + } + if (firstBlock == nullptr) { + firstBlock = block; + } + else { + lastBlock->next = block; + } + lastBlock = block; + block->next = firstBlock; + } + } + else { + firstBlock = make_block(largestBlockSize); + if (firstBlock == nullptr) { +#ifdef MOODYCAMEL_EXCEPTIONS_ENABLED + throw std::bad_alloc(); +#else + abort(); +#endif + } + firstBlock->next = firstBlock; + } + frontBlock = firstBlock; + tailBlock = firstBlock; + + // Make sure the reader/writer threads will have the initialized memory setup above: + fence(memory_order_sync); + } + + // Note: The queue should not be accessed concurrently while it's + // being deleted. It's up to the user to synchronize this. + ~ReaderWriterQueue() + { + // Make sure we get the latest version of all variables from other CPUs: + fence(memory_order_sync); + + // Destroy any remaining objects in queue and free memory + Block* frontBlock_ = frontBlock; + Block* block = frontBlock_; + do { + Block* nextBlock = block->next; + size_t blockFront = block->front; + size_t blockTail = block->tail; + + for (size_t i = blockFront; i != blockTail; i = (i + 1) & block->sizeMask) { + auto element = reinterpret_cast(block->data + i * sizeof(T)); + element->~T(); + (void)element; + } + + auto rawBlock = block->rawThis; + block->~Block(); + std::free(rawBlock); + block = nextBlock; + } 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. + AE_FORCEINLINE bool try_enqueue(T const& element) + { + return inner_enqueue(element); + } + + // Enqueues a moved copy of element if there is room in the queue. + // Returns true if the element was enqueued, false otherwise. + // Does not allocate memory. + AE_FORCEINLINE bool try_enqueue(T&& element) + { + return inner_enqueue(std::forward(element)); + } + + + // Enqueues a copy of element on the queue. + // Allocates an additional block of memory if needed. + // Only fails (returns false) if memory allocation fails. + AE_FORCEINLINE bool enqueue(T const& element) + { + return inner_enqueue(element); + } + + // Enqueues a moved copy of element on the queue. + // Allocates an additional block of memory if needed. + // Only fails (returns false) if memory allocation fails. + AE_FORCEINLINE bool enqueue(T&& element) + { + return inner_enqueue(std::forward(element)); + } + + + // Attempts to dequeue an element; if the queue is empty, + // returns false instead. If the queue has at least one element, + // moves front to result using operator=, then returns true. + template + bool try_dequeue(U& result) + { +#ifndef NDEBUG + ReentrantGuard guard(this->dequeuing); +#endif + + // High-level pseudocode: + // Remember where the tail block is + // If the front block has an element in it, dequeue it + // Else + // If front block was the tail block when we entered the function, return false + // Else advance to next block and dequeue the item there + + // Note that we have to use the value of the tail block from before we check if the front + // block is full or not, in case the front block is empty and then, before we check if the + // tail block is at the front block or not, the producer fills up the front block *and + // moves on*, which would make us skip a filled block. Seems unlikely, but was consistently + // reproducible in practice. + // In order to avoid overhead in the common case, though, we do a double-checked pattern + // where we have the fast path if the front block is not empty, then read the tail block, + // then re-read the front block and check if it's not empty again, then check if the tail + // block has advanced. + + Block* frontBlock_ = frontBlock.load(); + size_t blockTail = frontBlock_->localTail; + size_t blockFront = frontBlock_->front.load(); + + if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) { + fence(memory_order_acquire); + + non_empty_front_block: + // Front block not empty, dequeue from here + auto element = reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); + result = std::move(*element); + element->~T(); + + blockFront = (blockFront + 1) & frontBlock_->sizeMask; + + fence(memory_order_release); + frontBlock_->front = blockFront; + } + else if (frontBlock_ != tailBlock.load()) { + fence(memory_order_acquire); + + frontBlock_ = frontBlock.load(); + blockTail = frontBlock_->localTail = frontBlock_->tail.load(); + blockFront = frontBlock_->front.load(); + fence(memory_order_acquire); + + if (blockFront != blockTail) { + // Oh look, the front block isn't empty after all + goto non_empty_front_block; + } + + // Front block is empty but there's another block ahead, advance to it + Block* nextBlock = frontBlock_->next; + // Don't need an acquire fence here since next can only ever be set on the tailBlock, + // and we're not the tailBlock, and we did an acquire earlier after reading tailBlock which + // ensures next is up-to-date on this CPU in case we recently were at tailBlock. + + size_t nextBlockFront = nextBlock->front.load(); + size_t nextBlockTail = nextBlock->localTail = nextBlock->tail.load(); + fence(memory_order_acquire); + + // Since the tailBlock is only ever advanced after being written to, + // we know there's for sure an element to dequeue on it + assert(nextBlockFront != nextBlockTail); + AE_UNUSED(nextBlockTail); + + // We're done with this block, let the producer use it if it needs + fence(memory_order_release); // Expose possibly pending changes to frontBlock->front from last dequeue + frontBlock = frontBlock_ = nextBlock; + + compiler_fence(memory_order_release); // Not strictly needed + + auto element = reinterpret_cast(frontBlock_->data + nextBlockFront * sizeof(T)); + + result = std::move(*element); + element->~T(); + + nextBlockFront = (nextBlockFront + 1) & frontBlock_->sizeMask; + + fence(memory_order_release); + frontBlock_->front = nextBlockFront; + } + else { + // No elements in current block and no other block to advance to + return false; + } + + return true; + } + + + // Returns a pointer to the front element in the queue (the one that + // would be removed next by a call to `try_dequeue` or `pop`). If the + // queue appears empty at the time the method is called, nullptr is + // returned instead. + // Must be called only from the consumer thread. + T* peek() + { +#ifndef NDEBUG + ReentrantGuard guard(this->dequeuing); +#endif + // See try_dequeue() for reasoning + + Block* frontBlock_ = frontBlock.load(); + size_t blockTail = frontBlock_->localTail; + size_t blockFront = frontBlock_->front.load(); + + if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) { + fence(memory_order_acquire); + non_empty_front_block: + return reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); + } + else if (frontBlock_ != tailBlock.load()) { + fence(memory_order_acquire); + frontBlock_ = frontBlock.load(); + blockTail = frontBlock_->localTail = frontBlock_->tail.load(); + blockFront = frontBlock_->front.load(); + fence(memory_order_acquire); + + if (blockFront != blockTail) { + goto non_empty_front_block; + } + + Block* nextBlock = frontBlock_->next; + + size_t nextBlockFront = nextBlock->front.load(); + fence(memory_order_acquire); + + assert(nextBlockFront != nextBlock->tail.load()); + return reinterpret_cast(nextBlock->data + nextBlockFront * sizeof(T)); + } + + return nullptr; + } + + // Removes the front element from the queue, if any, without returning it. + // Returns true on success, or false if the queue appeared empty at the time + // `pop` was called. + bool pop() + { +#ifndef NDEBUG + ReentrantGuard guard(this->dequeuing); +#endif + // See try_dequeue() for reasoning + + Block* frontBlock_ = frontBlock.load(); + size_t blockTail = frontBlock_->localTail; + size_t blockFront = frontBlock_->front.load(); + + if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) { + fence(memory_order_acquire); + + non_empty_front_block: + auto element = reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); + element->~T(); + + blockFront = (blockFront + 1) & frontBlock_->sizeMask; + + fence(memory_order_release); + frontBlock_->front = blockFront; + } + else if (frontBlock_ != tailBlock.load()) { + fence(memory_order_acquire); + frontBlock_ = frontBlock.load(); + blockTail = frontBlock_->localTail = frontBlock_->tail.load(); + blockFront = frontBlock_->front.load(); + fence(memory_order_acquire); + + if (blockFront != blockTail) { + goto non_empty_front_block; + } + + // Front block is empty but there's another block ahead, advance to it + Block* nextBlock = frontBlock_->next; + + size_t nextBlockFront = nextBlock->front.load(); + size_t nextBlockTail = nextBlock->localTail = nextBlock->tail.load(); + fence(memory_order_acquire); + + assert(nextBlockFront != nextBlockTail); + AE_UNUSED(nextBlockTail); + + fence(memory_order_release); + frontBlock = frontBlock_ = nextBlock; + + compiler_fence(memory_order_release); + + auto element = reinterpret_cast(frontBlock_->data + nextBlockFront * sizeof(T)); + element->~T(); + + nextBlockFront = (nextBlockFront + 1) & frontBlock_->sizeMask; + + fence(memory_order_release); + frontBlock_->front = nextBlockFront; + } + else { + // No elements in current block and no other block to advance to + return false; + } + + return true; + } + + // Returns the approximate number of items currently in the queue. + // Safe to call from both the producer and consumer threads. + inline size_t size_approx() const + { + size_t result = 0; + Block* frontBlock_ = frontBlock.load(); + Block* block = frontBlock_; + do { + fence(memory_order_acquire); + size_t blockFront = block->front.load(); + size_t blockTail = block->tail.load(); + result += (blockTail - blockFront) & block->sizeMask; + block = block->next.load(); + } while (block != frontBlock_); + return result; + } + + +private: + enum AllocationMode { CanAlloc, CannotAlloc }; + + template + bool inner_enqueue(U&& element) + { +#ifndef NDEBUG + ReentrantGuard guard(this->enqueuing); +#endif + + // High-level pseudocode (assuming we're allowed to alloc a new block): + // If room in tail block, add to tail + // Else check next block + // If next block is not the head block, enqueue on next block + // Else create a new block and enqueue there + // Advance tail to the block we just enqueued to + + Block* tailBlock_ = tailBlock.load(); + size_t blockFront = tailBlock_->localFront; + size_t blockTail = tailBlock_->tail.load(); + + size_t nextBlockTail = (blockTail + 1) & tailBlock_->sizeMask; + if (nextBlockTail != blockFront || nextBlockTail != (tailBlock_->localFront = tailBlock_->front.load())) { + fence(memory_order_acquire); + // This block has room for at least one more element + char* location = tailBlock_->data + blockTail * sizeof(T); + new (location) T(std::forward(element)); + + fence(memory_order_release); + tailBlock_->tail = nextBlockTail; + } + else { + fence(memory_order_acquire); + if (tailBlock_->next.load() != frontBlock) { + // Note that the reason we can't advance to the frontBlock and start adding new entries there + // is because if we did, then dequeue would stay in that block, eventually reading the new values, + // instead of advancing to the next full block (whose values were enqueued first and so should be + // consumed first). + + fence(memory_order_acquire); // Ensure we get latest writes if we got the latest frontBlock + + // tailBlock is full, but there's a free block ahead, use it + Block* tailBlockNext = tailBlock_->next.load(); + size_t nextBlockFront = tailBlockNext->localFront = tailBlockNext->front.load(); + nextBlockTail = tailBlockNext->tail.load(); + fence(memory_order_acquire); + + // This block must be empty since it's not the head block and we + // go through the blocks in a circle + assert(nextBlockFront == nextBlockTail); + tailBlockNext->localFront = nextBlockFront; + + char* location = tailBlockNext->data + nextBlockTail * sizeof(T); + new (location) T(std::forward(element)); + + tailBlockNext->tail = (nextBlockTail + 1) & tailBlockNext->sizeMask; + + fence(memory_order_release); + tailBlock = tailBlockNext; + } + else if (canAlloc == CanAlloc) { + // tailBlock is full and there's no free block ahead; create a new block + auto newBlockSize = largestBlockSize >= MAX_BLOCK_SIZE ? largestBlockSize : largestBlockSize * 2; + auto newBlock = make_block(newBlockSize); + if (newBlock == nullptr) { + // Could not allocate a block! + return false; + } + largestBlockSize = newBlockSize; + + new (newBlock->data) T(std::forward(element)); + + assert(newBlock->front == 0); + newBlock->tail = newBlock->localTail = 1; + + newBlock->next = tailBlock_->next.load(); + tailBlock_->next = newBlock; + + // Might be possible for the dequeue thread to see the new tailBlock->next + // *without* seeing the new tailBlock value, but this is OK since it can't + // advance to the next block until tailBlock is set anyway (because the only + // case where it could try to read the next is if it's already at the tailBlock, + // and it won't advance past tailBlock in any circumstance). + + fence(memory_order_release); + tailBlock = newBlock; + } + else if (canAlloc == CannotAlloc) { + // Would have had to allocate a new block to enqueue, but not allowed + return false; + } + else { + assert(false && "Should be unreachable code"); + return false; + } + } + + return true; + } + + + // Disable copying + ReaderWriterQueue(ReaderWriterQueue const&) { } + + // Disable assignment + ReaderWriterQueue& operator=(ReaderWriterQueue const&) { } + + + + AE_FORCEINLINE static size_t ceilToPow2(size_t x) + { + // From http://graphics.stanford.edu/~seander/bithacks.html#RoundUpPowerOf2 + --x; + x |= x >> 1; + x |= x >> 2; + x |= x >> 4; + for (size_t i = 1; i < sizeof(size_t); i <<= 1) { + x |= x >> (i << 3); + } + ++x; + return x; + } + + template + static AE_FORCEINLINE char* align_for(char* ptr) + { + const std::size_t alignment = std::alignment_of::value; + return ptr + (alignment - (reinterpret_cast(ptr) % alignment)) % alignment; + } +private: +#ifndef NDEBUG + struct ReentrantGuard + { + ReentrantGuard(bool& _inSection) + : inSection(_inSection) + { + assert(!inSection && "ReaderWriterQueue does not support enqueuing or dequeuing elements from other elements' ctors and dtors"); + inSection = true; + } + + ~ReentrantGuard() { inSection = false; } + + private: + ReentrantGuard& operator=(ReentrantGuard const&); + + private: + bool& inSection; + }; +#endif + + struct Block + { + // Avoid false-sharing by putting highly contended variables on their own cache lines + weak_atomic front; // (Atomic) Elements are read from here + size_t localTail; // An uncontended shadow copy of tail, owned by the consumer + + char cachelineFiller0[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic) - sizeof(size_t)]; + weak_atomic tail; // (Atomic) Elements are enqueued here + size_t localFront; + + char cachelineFiller1[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic) - sizeof(size_t)]; // next isn't very contended, but we don't want it on the same cache line as tail (which is) + weak_atomic next; // (Atomic) + + char* data; // Contents (on heap) are aligned to T's alignment + + 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) + { + } + + private: + // C4512 - Assignment operator could not be generated + Block& operator=(Block const&); + + public: + char* rawThis; + }; + + + static Block* make_block(size_t capacity) + { + // Allocate enough memory for the block itself, as well as all the elements it will contain + auto size = sizeof(Block) + std::alignment_of::value - 1; + size += sizeof(T) * capacity + std::alignment_of::value - 1; + auto newBlockRaw = static_cast(std::malloc(size)); + if (newBlockRaw == nullptr) { + return nullptr; + } + + auto newBlockAligned = align_for(newBlockRaw); + auto newBlockData = align_for(newBlockAligned + sizeof(Block)); + return new (newBlockAligned) Block(capacity, newBlockRaw, newBlockData); + } + +private: + weak_atomic frontBlock; // (Atomic) Elements are enqueued to this block + + char cachelineFiller[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic)]; + weak_atomic tailBlock; // (Atomic) Elements are dequeued from this block + + size_t largestBlockSize; + +#ifndef NDEBUG + bool enqueuing; + bool dequeuing; +#endif +}; + +// Like ReaderWriterQueue, but also providees blocking operations +template +class BlockingReaderWriterQueue +{ +private: + typedef ::moodycamel::ReaderWriterQueue 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. + // Does not allocate memory. + AE_FORCEINLINE bool try_enqueue(T const& element) + { + if (inner.try_enqueue(element)) { + sema.signal(); + return true; + } + return false; + } + + // Enqueues a moved copy of element if there is room in the queue. + // Returns true if the element was enqueued, false otherwise. + // Does not allocate memory. + AE_FORCEINLINE bool try_enqueue(T&& element) + { + if (inner.try_enqueue(std::forward(element))) { + sema.signal(); + return true; + } + return false; + } + + + // Enqueues a copy of element on the queue. + // Allocates an additional block of memory if needed. + // Only fails (returns false) if memory allocation fails. + AE_FORCEINLINE bool enqueue(T const& element) + { + if (inner.enqueue(element)) { + sema.signal(); + return true; + } + return false; + } + + // Enqueues a moved copy of element on the queue. + // Allocates an additional block of memory if needed. + // Only fails (returns false) if memory allocation fails. + AE_FORCEINLINE bool enqueue(T&& element) + { + if (inner.enqueue(std::forward(element))) { + sema.signal(); + return true; + } + return false; + } + + + // Attempts to dequeue an element; if the queue is empty, + // returns false instead. If the queue has at least one element, + // moves front to result using operator=, then returns true. + template + bool try_dequeue(U& result) + { + if (sema.tryWait()) { + bool success = inner.try_dequeue(result); + assert(success); + AE_UNUSED(success); + return true; + } + return false; + } + + + // Attempts to dequeue an element; if the queue is empty, + // waits until an element is available, then dequeues it. + template + void wait_dequeue(U& result) + { + sema.wait(); + bool success = inner.try_dequeue(result); + AE_UNUSED(result); + assert(success); + AE_UNUSED(success); + } + + + // Attempts to dequeue an element; if the queue is empty, + // waits until an element is available up to the specified timeout, + // then dequeues it and returns true, or returns false if the timeout + // expires before an element can be dequeued. + // Using a negative timeout indicates an indefinite timeout, + // and is thus functionally equivalent to calling wait_dequeue. + template + bool wait_dequeue_timed(U& result, std::int64_t timeout_usecs) + { + if (!sema.wait(timeout_usecs)) { + return false; + } + bool success = inner.try_dequeue(result); + AE_UNUSED(result); + assert(success); + AE_UNUSED(success); + return true; + } + + +#if __cplusplus > 199711L || _MSC_VER >= 1700 + // Attempts to dequeue an element; if the queue is empty, + // waits until an element is available up to the specified timeout, + // then dequeues it and returns true, or returns false if the timeout + // expires before an element can be dequeued. + // Using a negative timeout indicates an indefinite timeout, + // and is thus functionally equivalent to calling wait_dequeue. + template + inline bool wait_dequeue_timed(U& result, std::chrono::duration const& timeout) + { + return wait_dequeue_timed(result, std::chrono::duration_cast(timeout).count()); + } +#endif + + + // Returns a pointer to the front element in the queue (the one that + // would be removed next by a call to `try_dequeue` or `pop`). If the + // queue appears empty at the time the method is called, nullptr is + // returned instead. + // Must be called only from the consumer thread. + AE_FORCEINLINE T* peek() + { + return inner.peek(); + } + + // Removes the front element from the queue, if any, without returning it. + // Returns true on success, or false if the queue appeared empty at the time + // `pop` was called. + AE_FORCEINLINE bool pop() + { + if (sema.tryWait()) { + bool result = inner.pop(); + assert(result); + AE_UNUSED(result); + return true; + } + return false; + } + + // Returns the approximate number of items currently in the queue. + // Safe to call from both the producer and consumer threads. + AE_FORCEINLINE size_t size_approx() const + { + return sema.availableApprox(); + } + + +private: + // Disable copying & assignment + BlockingReaderWriterQueue(ReaderWriterQueue const&) { } + BlockingReaderWriterQueue& operator=(ReaderWriterQueue const&) { } + +private: + ReaderWriterQueue inner; + spsc_sema::LightweightSemaphore sema; +}; + +} // end namespace moodycamel + +#ifdef AE_VCPP +#pragma warning(pop) +#endif diff --git a/src/pipeline.cpp b/src/pipeline.cpp new file mode 100644 index 000000000..53b5232a1 --- /dev/null +++ b/src/pipeline.cpp @@ -0,0 +1,23 @@ +#include "ur_modern_driver/queue/readerwriterqueue.h" + +using namespace moodycamel; +using namespace std; + +class UR2ROSRelay { +private: + URReciever reciever; + URParser parser; + + BlockingReaderWriterQueue> queue; + + ROSConverter converter; + ROSPublisher publisher; + + +public: + UR2ROSRelay(string &host, int port) : reciever{host, port} { } + + void run() { + + } +}; From efcb7383251e2ddd72cbba5110d86dfa28375aeb Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Mon, 30 Jan 2017 02:29:53 +0100 Subject: [PATCH 002/114] Implemented URStream --- include/ur_modern_driver/ur/stream.h | 30 +++++++++ src/ur/stream.cpp | 93 ++++++++++++++++++++++++++++ 2 files changed, 123 insertions(+) create mode 100644 include/ur_modern_driver/ur/stream.h create mode 100644 src/ur/stream.cpp diff --git a/include/ur_modern_driver/ur/stream.h b/include/ur_modern_driver/ur/stream.h new file mode 100644 index 000000000..88a5ed642 --- /dev/null +++ b/include/ur_modern_driver/ur/stream.h @@ -0,0 +1,30 @@ +#pragma once +#include +#include +#include +#include +#include + +/// Encapsulates a TCP socket +class URStream { +private: + int _socket_fd = -1; + std::string _host; + int _port; + + std::atomic _initialized; + std::atomic _stopping; + +public: + URStream(std::string &host, int port) + : _host(host), + _port(port), + _initialized(false), + _stopping(false) {} + + bool connect(); + void disconnect(); + + ssize_t send(uint8_t *buf, size_t buf_len); + ssize_t receive(uint8_t *buf, size_t buf_len); +}; \ No newline at end of file diff --git a/src/ur/stream.cpp b/src/ur/stream.cpp new file mode 100644 index 000000000..a7c53a8b9 --- /dev/null +++ b/src/ur/stream.cpp @@ -0,0 +1,93 @@ +#include +#include +#include + +#include "ur_modern_driver/ur/stream.h" +#include "ur_modern_driver/log.h" + +bool URStream::connect() { + if(_initialized) + return false; + + LOG_INFO("Connecting to UR @ %s:%d\n", _host.c_str(), _port); + + //gethostbyname() is deprecated so use getadderinfo() as described in: + //http://www.beej.us/guide/bgnet/output/html/multipage/syscalls.html#getaddrinfo + + std::string service = std::to_string(_port); + struct addrinfo hints, *result; + std::memset(&hints, 0, sizeof(hints)); + + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_STREAM; + hints.ai_flags = AI_PASSIVE; + + if(getaddrinfo(_host.c_str(), service.c_str(), &hints, &result) != 0) { + LOG_ERROR("Failed to get host name\n"); + return false; + } + + //loop through the list of addresses untill we find one that's connectable + for(struct addrinfo *p = result; p != nullptr; p = p->ai_next) { + _socket_fd = socket(p->ai_family, p->ai_socktype, p->ai_protocol); + + if(_socket_fd == -1) //socket error? + continue; + + if(::connect(_socket_fd, p->ai_addr, p->ai_addrlen) != 0) { + if(_stopping) + break; + else + continue; //try next addrinfo if connect fails + } + + //disable Nagle's algorithm to ensure we sent packets as fast as possible + int flag = 1; + setsockopt(_socket_fd, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(flag)); + _initialized = true; + LOG_INFO("Connection successfully established\n"); + break; + } + + freeaddrinfo(result); + if(!_initialized) + LOG_ERROR("Connection failed\n"); + + return _initialized; +} + +void URStream::disconnect() { + if(!_initialized || _stopping) + return; + + _stopping = true; + close(_socket_fd); + _initialized = false; +} + +ssize_t URStream::send(uint8_t *buf, size_t buf_len) { + if(!_initialized) + return -1; + if(_stopping) + return 0; + + size_t total = 0; + size_t remaining = buf_len; + + //TODO: handle reconnect? + //handle partial sends + while(total < buf_len) { + ssize_t sent = ::send(_socket_fd, buf+total, remaining, 0); + if(sent == -1) + return _stopping ? 0 : -1; + total += sent; + remaining -= sent; + } + + return total; +} + +ssize_t URStream::receive(uint8_t *buf, size_t buf_len) { + //TODO: handle reconnect? + return recv(_socket_fd, buf, buf_len, 0); +} \ No newline at end of file From cd639339c71fc143035e7f3db918e1438c4663c4 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Mon, 6 Feb 2017 14:15:46 +0100 Subject: [PATCH 003/114] Improved BinParser --- include/ur_modern_driver/bin_parser.h | 92 +++++++++++++++++++++++++++ 1 file changed, 92 insertions(+) create mode 100644 include/ur_modern_driver/bin_parser.h diff --git a/include/ur_modern_driver/bin_parser.h b/include/ur_modern_driver/bin_parser.h new file mode 100644 index 000000000..f44d57fd4 --- /dev/null +++ b/include/ur_modern_driver/bin_parser.h @@ -0,0 +1,92 @@ +#pragma once + +#include +#include +#include +#include +#include "ur_modern_driver/types.h" + +class BinParser { +private: + uint8_t *_buf_pos, *_buf_end; + BinParser &_parent; + +public: + BinParser(uint8_t *buffer, size_t buf_len) : + _buf_pos(buffer), + _buf_end(buffer+buf_len), + _parent(*this) + { } + + BinParser(BinParser &parent, size_t sub_len) : + _buf_pos(parent._buf_pos), + _buf_end(parent._buf_pos+sub_len), + _parent(parent) + { } + + ~BinParser() { + _parent._buf_pos = _buf_pos; + } + + template + T peek() { + return *(reinterpret_cast(_buf_pos)); + } + + template + void parse(T &val) { + val = peek(); + _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) { + uint8_t inner; + parse(inner); + val = inner != 0; + } + + // Explicit parsing order of fields to avoid issues with struct layout + 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) { + parse(val.position); + parse(val.rotation); + } + + void parse(std::string &val, size_t len) { + val = val.assign(reinterpret_cast(_buf_pos), len); + _buf_pos += len; + } + + // Special string parse function that assumes uint8_t len followed by chars + void parse(std::string &val) { + uint8_t len; + parse(len); + parse(val, size_t(len)); + } + + template + void parse(T (&array)[N]) { + std::memcpy(array, _buf_pos, sizeof(T)*N); + _buf_pos += (sizeof(T)*N); + } + + void skip(size_t bytes) { + _buf_pos += bytes; + } + + bool check_size(size_t bytes) { + return bytes <= size_t(_buf_end - _buf_pos); + } + template + bool check_size(void) { + return check_size(T::SIZE); + } +}; From ead41d205ce6bf94c902edb4377007c137c308d8 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Mon, 6 Feb 2017 14:16:14 +0100 Subject: [PATCH 004/114] Refactored logging --- include/ur_modern_driver/log.h | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) create mode 100644 include/ur_modern_driver/log.h diff --git a/include/ur_modern_driver/log.h b/include/ur_modern_driver/log.h new file mode 100644 index 000000000..0d96b88c9 --- /dev/null +++ b/include/ur_modern_driver/log.h @@ -0,0 +1,21 @@ +#pragma once +#include + +#ifdef ROS_BUILD + #include + + #define LOG_DEBUG ROS_DEBUG + #define LOG_WARN ROS_WARN + #define LOG_INFO ROS_INFO + #define LOG_ERROR ROS_ERROR + #define LOG_FATAL ROS_FATAL + +#else + + #define LOG_DEBUG(format, ...) printf("DEBUG: " format, ##__VA_ARGS__) + #define LOG_WARN(format, ...) printf("WARNING: " format, ##__VA_ARGS__) + #define LOG_INFO(format, ...) printf("INFO: " format, ##__VA_ARGS__) + #define LOG_ERROR(format, ...) printf("ERROR: " format, ##__VA_ARGS__) + #define LOG_FATAL(format, ...) printf("FATAL: " format, ##__VA_ARGS__) + +#endif \ No newline at end of file From c4c613c5e7838121519028e6da7a535d0ddf52e7 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Mon, 6 Feb 2017 14:16:48 +0100 Subject: [PATCH 005/114] Added basic vector and cartesian types --- include/ur_modern_driver/types.h | 12 ++++++++++++ 1 file changed, 12 insertions(+) create mode 100644 include/ur_modern_driver/types.h diff --git a/include/ur_modern_driver/types.h b/include/ur_modern_driver/types.h new file mode 100644 index 000000000..8c5144334 --- /dev/null +++ b/include/ur_modern_driver/types.h @@ -0,0 +1,12 @@ +#pragma once + +#include + +typedef struct { + double x, y, z; +} double3_t; + +typedef struct { + double3_t position; + double3_t rotation; +} cartesian_coord_t; \ No newline at end of file From c7bee00cc19522c6eed62fed6f5705872705e287 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Mon, 6 Feb 2017 14:20:34 +0100 Subject: [PATCH 006/114] Removed uneeded pipeline.cpp --- src/pipeline.cpp | 23 ----------------------- 1 file changed, 23 deletions(-) delete mode 100644 src/pipeline.cpp diff --git a/src/pipeline.cpp b/src/pipeline.cpp deleted file mode 100644 index 53b5232a1..000000000 --- a/src/pipeline.cpp +++ /dev/null @@ -1,23 +0,0 @@ -#include "ur_modern_driver/queue/readerwriterqueue.h" - -using namespace moodycamel; -using namespace std; - -class UR2ROSRelay { -private: - URReciever reciever; - URParser parser; - - BlockingReaderWriterQueue> queue; - - ROSConverter converter; - ROSPublisher publisher; - - -public: - UR2ROSRelay(string &host, int port) : reciever{host, port} { } - - void run() { - - } -}; From ffe2bbe96a00c130a1327c0467042f98955b317f Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Mon, 6 Feb 2017 14:22:28 +0100 Subject: [PATCH 007/114] Implemented initial robot state and parsing --- include/ur_modern_driver/packet.h | 7 ++ include/ur_modern_driver/ur/master_board.h | 84 +++++++++++++++++ include/ur_modern_driver/ur/messages.h | 20 ++++ include/ur_modern_driver/ur/robot_mode.h | 104 +++++++++++++++++++++ include/ur_modern_driver/ur/rt_state.h | 58 ++++++++++++ include/ur_modern_driver/ur/state.h | 59 ++++++++++++ src/ur/master_board.cpp | 66 +++++++++++++ src/ur/messages.cpp | 22 +++++ src/ur/robot_mode.cpp | 56 +++++++++++ src/ur/rt_state.cpp | 37 ++++++++ src/ur/state.cpp | 73 +++++++++++++++ 11 files changed, 586 insertions(+) create mode 100644 include/ur_modern_driver/packet.h create mode 100644 include/ur_modern_driver/ur/master_board.h create mode 100644 include/ur_modern_driver/ur/messages.h create mode 100644 include/ur_modern_driver/ur/robot_mode.h create mode 100644 include/ur_modern_driver/ur/rt_state.h create mode 100644 include/ur_modern_driver/ur/state.h create mode 100644 src/ur/master_board.cpp create mode 100644 src/ur/messages.cpp create mode 100644 src/ur/robot_mode.cpp create mode 100644 src/ur/rt_state.cpp create mode 100644 src/ur/state.cpp diff --git a/include/ur_modern_driver/packet.h b/include/ur_modern_driver/packet.h new file mode 100644 index 000000000..7f316dc4b --- /dev/null +++ b/include/ur_modern_driver/packet.h @@ -0,0 +1,7 @@ +#pragma once +#include "ur_modern_driver/bin_parser.h" + +class Packet { +public: + virtual bool parse_with(BinParser &bp) = 0; +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/master_board.h b/include/ur_modern_driver/ur/master_board.h new file mode 100644 index 000000000..e85b2ddb6 --- /dev/null +++ b/include/ur_modern_driver/ur/master_board.h @@ -0,0 +1,84 @@ +#pragma once + +#include +#include +#include "ur_modern_driver/types.h" +#include "ur_modern_driver/bin_parser.h" + + +class SharedMasterBoardData { +public: + virtual bool parse_with(BinParser &bp); + + int8_t analog_input_range0; + int8_t analog_input_range1; + double analog_input0; + double analog_input1; + int8_t analog_output_domain0; + int8_t analog_output_domain1; + double analog_output0; + double analog_output1; + float master_board_temperature; + float robot_voltage_48V; + float robot_current; + float master_IO_current; + + bool euromap67_interface_installed; + + //euromap fields are dynamic so don't include in SIZE + int32_t euromap_input_bits; + int32_t euromap_output_bits; + + static const size_t SIZE = sizeof(double) * 4 + + sizeof(int8_t) * 4 + + sizeof(float) * 4 + + sizeof(uint8_t); + + static const size_t EURO_SIZE = sizeof(int32_t) * 2; +}; + +class MasterBoardData_V1_X : public SharedMasterBoardData { +public: + virtual bool parse_with(BinParser &bp); + + int16_t digital_input_bits; + int16_t digital_output_bits; + + uint8_t master_safety_state; + bool master_on_off_state; + + //euromap fields are dynamic so don't include in SIZE + int16_t euromap_voltage; + int16_t euromap_current; + + + static const size_t SIZE = SharedMasterBoardData::SIZE + + sizeof(int16_t) * 2 + + sizeof(uint8_t) * 2; + + static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE + + sizeof(int16_t) * 2; +}; + +class MasterBoardData_V3_X : public SharedMasterBoardData { +public: + virtual bool parse_with(BinParser &bp); + + int32_t digital_input_bits; + int32_t digital_output_bits; + + uint8_t safety_mode; + bool in_reduced_mode; + + //euromap fields are dynamic so don't include in SIZE + float euromap_voltage; + float euromap_current; + + + static const size_t SIZE = SharedMasterBoardData::SIZE + + sizeof(int32_t) * 2 + + sizeof(uint8_t) * 2; + + static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE + + sizeof(float) * 2; +}; diff --git a/include/ur_modern_driver/ur/messages.h b/include/ur_modern_driver/ur/messages.h new file mode 100644 index 000000000..31ec39192 --- /dev/null +++ b/include/ur_modern_driver/ur/messages.h @@ -0,0 +1,20 @@ +#pragma once + +class MessageBase { +public: + virtual bool parse_with(BinParser &bp) = 0; + + uint64_t timestamp; + uint8_t source; +}; + +class VersionMessage : public MessageBase { +public: + bool parse_with(BinParser &bp); + + std::string project_name; + uint8_t major_version; + uint8_t minor_version; + int32_t svn_version; + std::string build_date; +} \ No newline at end of file diff --git a/include/ur_modern_driver/ur/robot_mode.h b/include/ur_modern_driver/ur/robot_mode.h new file mode 100644 index 000000000..13b195293 --- /dev/null +++ b/include/ur_modern_driver/ur/robot_mode.h @@ -0,0 +1,104 @@ +#pragma once + +#include +#include +#include "ur_modern_driver/types.h" +#include "ur_modern_driver/bin_parser.h" + +class SharedRobotModeData { +public: + bool parse_with(BinParser &bp); + + uint64_t timestamp; + bool physical_robot_connected; + bool real_robot_enabled; + bool robot_power_on; + bool emergency_stopped; + bool program_running; + bool program_paused; + + static const size_t SIZE = sizeof(uint64_t) + sizeof(uint8_t) * 6; +}; + +enum class robot_mode_V1_X : uint8_t { + ROBOT_RUNNING_MODE = 0, + ROBOT_FREEDRIVE_MODE = 1, + ROBOT_READY_MODE = 2, + ROBOT_INITIALIZING_MODE = 3, + ROBOT_SECURITY_STOPPED_MODE = 4, + ROBOT_EMERGENCY_STOPPED_MODE = 5, + ROBOT_FATAL_ERROR_MODE = 6, + ROBOT_NO_POWER_MODE = 7, + ROBOT_NOT_CONNECTED_MODE = 8, + ROBOT_SHUTDOWN_MODE = 9, + ROBOT_SAFEGUARD_STOP_MODE = 10 +}; + +class RobotModeData_V1_X : public SharedRobotModeData { +public: + bool parse_with(BinParser &bp); + + bool security_stopped; + robot_mode_V1_X robot_mode; + double speed_fraction; + + static const size_t SIZE = SharedRobotModeData::SIZE + + sizeof(uint8_t) + + sizeof(robot_mode_V1_X) + + sizeof(double); + + static_assert(RobotModeData_V1_X::SIZE == 24, "RobotModeData_V1_X has missmatched size"); +}; + +enum class robot_mode_V3_X : uint8_t { + DISCONNECTED = 0, + CONFIRM_SAFETY = 1, + BOOTING = 2, + POWER_OFF = 3, + POWER_ON = 4, + IDLE = 5, + BACKDRIVE = 6, + RUNNING = 7, + UPDATING_FIRMWARE = 8 +}; + +enum class robot_control_mode_V3_X : uint8_t { + POSITION = 0, + TEACH = 1, + FORCE = 2, + TORQUE = 3 +}; + +class RobotModeData_V3_0__1 : public SharedRobotModeData { +public: + bool parse_with(BinParser &bp); + + bool protective_stopped; + + robot_mode_V3_X robot_mode; + robot_control_mode_V3_X control_mode; + + double target_speed_fraction; + double speed_scaling; + + static const size_t SIZE = SharedRobotModeData::SIZE + + sizeof(uint8_t) + + sizeof(robot_mode_V3_X) + + sizeof(robot_control_mode_V3_X) + + sizeof(double) + + sizeof(double); + + static_assert(RobotModeData_V3_0__1::SIZE == 33, "RobotModeData_V3_0__1 has missmatched size"); +}; + +class RobotModeData_V3_2 : public RobotModeData_V3_0__1 { +public: + bool parse_with(BinParser &bp); + + double target_speed_fraction_limit; + + static const size_t SIZE = RobotModeData_V3_0__1::SIZE + + sizeof(double); + + static_assert(RobotModeData_V3_2::SIZE == 41, "RobotModeData_V3_2 has missmatched size"); +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/rt_state.h b/include/ur_modern_driver/ur/rt_state.h new file mode 100644 index 000000000..bc56aa919 --- /dev/null +++ b/include/ur_modern_driver/ur/rt_state.h @@ -0,0 +1,58 @@ +#pragma once + +#include +#include +#include "ur_modern_driver/types.h" +#include "ur_modern_driver/bin_parser.h" + +class RTState_V1_6__7 { +public: + bool parse_with(BinParser &bp); + + double time; + double q_target[6]; + double qd_target[6]; + double qdd_target[6]; + double i_target[6]; + double m_target[6]; + double q_actual[6]; + double qd_actual[6]; + double i_actual[6]; + double3_t tool_accelerometer_values; + double tcp_force[6]; + cartesian_coord_t tool_vector; + double tcp_speed[6]; + uint64_t digital_input; + double motor_temperatures[6]; + double controller_time; + double robot_mode; + + static const size_t SIZE = sizeof(double) * 3 + + sizeof(double[6]) * 11 + + sizeof(double3_t) + + sizeof(cartesian_coord_t) + + sizeof(uint64_t); + + + static_assert(RTState_V1_6__7::SIZE == 632, "RTState_V1_6__7 has mismatched size!"); +}; + +class RTState_V1_8 : public RTState_V1_6__7 { +public: + bool parse_with(BinParser &bp); + + double joint_modes[6]; + + static const size_t SIZE = RTState_V1_6__7::SIZE + + sizeof(double[6]); + + static_assert(RTState_V1_8::SIZE == 680, "RTState_V1_8 has mismatched size!"); +}; + +class RTState_V3_0__1 { +public: + //bool parse_with(BinParser &bp); + + double m_actual[6]; + double i_control[6]; +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/state.h b/include/ur_modern_driver/ur/state.h new file mode 100644 index 000000000..f76e8a673 --- /dev/null +++ b/include/ur_modern_driver/ur/state.h @@ -0,0 +1,59 @@ +#pragma once +#include +#include +#include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/packet.h" +#include "ur_modern_driver/ur/master_board.h" +#include "ur_modern_driver/ur/robot_mode.h" + + + +enum class package_type : uint8_t { + ROBOT_MODE_DATA = 0, + JOINT_DATA = 1, + TOOL_DATA = 2, + MASTERBOARD_DATA = 3, + CARTESIAN_INFO = 4, + KINEMATICS_INFO = 5, + CONFIGURATION_DATA = 6, + FORCE_MODE_DATA = 7, + ADDITIONAL_INFO = 8, + CALIBRATION_DATA = 9 +}; + +enum class message_type : uint8_t { + ROBOT_STATE = 16, + ROBOT_MESSAGE = 20, + PROGRAM_STATE_MESSAGE = 25 +}; + + +class RobotState : public Packet { +public: + bool parse_with(BinParser &bp); +protected: + virtual bool parse_package(BinParser &bp) = 0; +}; + +class RobotState_V1_6__7 : public RobotState { +protected: + virtual bool parse_package(BinParser &bp) = 0; +public: + RobotModeData_V1_X robot_mode; + //JointData + //ToolData + MasterBoardData_V1_X master_board; + //CartesianInfo +}; + +class RobotState_V1_8 : public RobotState_V1_6__7 { +protected: + virtual bool parse_package(BinParser &bp) = 0; +public: + + //KinematicsInfo + //ConfigurationData + //ForceModeData + //AdditionalInfo + //CalibrationData +}; \ No newline at end of file diff --git a/src/ur/master_board.cpp b/src/ur/master_board.cpp new file mode 100644 index 000000000..084211a18 --- /dev/null +++ b/src/ur/master_board.cpp @@ -0,0 +1,66 @@ +#include "ur_modern_driver/ur/master_board.h" + +bool SharedMasterBoardData::parse_with(BinParser &bp) { + bp.parse(analog_input_range0); + bp.parse(analog_input_range1); + bp.parse(analog_input0); + bp.parse(analog_input1); + bp.parse(analog_output_domain0); + bp.parse(analog_output_domain1); + bp.parse(analog_output0); + bp.parse(analog_output1); + bp.parse(master_board_temperature); + bp.parse(robot_voltage_48V); + bp.parse(robot_current); + bp.parse(master_IO_current); + return true; +} + +bool MasterBoardData_V1_X::parse_with(BinParser &bp) { + if(!bp.check_size()) + return false; + + bp.parse(digital_input_bits); + bp.parse(digital_output_bits); + + SharedMasterBoardData::parse_with(bp); + + bp.parse(master_safety_state); + bp.parse(master_on_off_state); + bp.parse(euromap67_interface_installed); + + if(euromap67_interface_installed) { + if(!bp.check_size(MasterBoardData_V1_X::EURO_SIZE)) + return false; + + bp.parse(euromap_voltage); + bp.parse(euromap_current); + } + + return true; +} + +bool MasterBoardData_V3_X::parse_with(BinParser &bp) { + if(!bp.check_size()) + return false; + + bp.parse(digital_input_bits); + bp.parse(digital_output_bits); + + SharedMasterBoardData::parse_with(bp); + + bp.parse(safety_mode); + bp.parse(in_reduced_mode); + bp.parse(euromap67_interface_installed); + + if(euromap67_interface_installed) { + if(!bp.check_size(MasterBoardData_V3_X::EURO_SIZE)) + return false; + + bp.parse(euromap_voltage); + bp.parse(euromap_current); + } + + return true; +} + diff --git a/src/ur/messages.cpp b/src/ur/messages.cpp new file mode 100644 index 000000000..0e7ceea56 --- /dev/null +++ b/src/ur/messages.cpp @@ -0,0 +1,22 @@ +#include "ur_modern_driver/ur/messages.h" + +bool MessageBase::parse_with(BinParser &bp) { + bp.parse(timestamp); + bp.parse(source); + + return true; //not really possible to check dynamic size packets +} + + +bool VersionMessage::parse_with(BinParser &bp) { + + bp.parse(project_name); + bp.parse(major_version); + bp.parse(minor_version); + bp.parse(svn_version); //net to host? + + // how to parse this without length?? + //bp.parse(build_date); + + return true; //not really possible to check dynamic size packets +} \ No newline at end of file diff --git a/src/ur/robot_mode.cpp b/src/ur/robot_mode.cpp new file mode 100644 index 000000000..4fea01d83 --- /dev/null +++ b/src/ur/robot_mode.cpp @@ -0,0 +1,56 @@ +#include "ur_modern_driver/ur/robot_mode.h" + + +bool SharedRobotModeData::parse_with(BinParser &bp) { + bp.parse(timestamp); + bp.parse(physical_robot_connected); + bp.parse(real_robot_enabled); + bp.parse(robot_power_on); + bp.parse(emergency_stopped); + return true; +} + + +bool RobotModeData_V1_X::parse_with(BinParser &bp) { + if(!bp.check_size()) + return false; + + SharedRobotModeData::parse_with(bp); + + bp.parse(security_stopped); + bp.parse(program_running); + bp.parse(program_paused); + bp.parse(robot_mode); + bp.parse(speed_fraction); + + return true; +} + + +bool RobotModeData_V3_0__1::parse_with(BinParser &bp) { + if(!bp.check_size()) + return false; + + SharedRobotModeData::parse_with(bp); + + bp.parse(protective_stopped); + bp.parse(program_running); + bp.parse(program_paused); + bp.parse(robot_mode); + bp.parse(control_mode); + bp.parse(target_speed_fraction); + bp.parse(speed_scaling); + + return true; +} + +bool RobotModeData_V3_2::parse_with(BinParser &bp) { + if(!bp.check_size()) + return false; + + RobotModeData_V3_0__1::parse_with(bp); + + bp.parse(target_speed_fraction_limit); + + return true; +} diff --git a/src/ur/rt_state.cpp b/src/ur/rt_state.cpp new file mode 100644 index 000000000..aad336d36 --- /dev/null +++ b/src/ur/rt_state.cpp @@ -0,0 +1,37 @@ +#include "ur_modern_driver/ur/rt_state.h" + +bool RTState_V1_6__7::parse_with(BinParser &bp) { + if(!bp.check_size()) + return false; + + bp.parse(time); + bp.parse(q_target); + bp.parse(qd_target); + bp.parse(qdd_target); + bp.parse(i_target); + bp.parse(m_target); + bp.parse(q_actual); + bp.parse(qd_actual); + bp.parse(i_actual); + bp.parse(tool_accelerometer_values); + bp.parse(tcp_force); + bp.parse(tool_vector); + bp.parse(tcp_speed); + bp.parse(digital_input); + bp.parse(motor_temperatures); + bp.parse(controller_time); + bp.parse(robot_mode); + + return true; +} + +bool RTState_V1_8::parse_with(BinParser &bp) { + if(!bp.check_size()) + return false; + + RTState_V1_6__7::parse_with(bp); + + bp.parse(joint_modes); + + return true; +} \ No newline at end of file diff --git a/src/ur/state.cpp b/src/ur/state.cpp new file mode 100644 index 000000000..460bdf1f0 --- /dev/null +++ b/src/ur/state.cpp @@ -0,0 +1,73 @@ +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/ur/state.h" + + +bool RobotState::parse_with(BinParser &bp) { + //continue as long as there are bytes to read + while(bp.check_size(sizeof(uint8_t))) { + if(!bp.check_size(sizeof(uint32_t))){ + LOG_ERROR("Failed to read sub-package length, there's likely a parsing error\n"); + return false; + } + + uint32_t sub_size = bp.peek(); + if(!bp.check_size(static_cast(sub_size))) { + LOG_WARN("Invalid sub-package size of %" PRIu32 " received!\n", sub_size); + return false; + } + + //deconstruction of a sub parser will increment the position of the parent parser + BinParser sub_parser(bp, sub_size); + sub_parser.parse(sub_size); + + if(!parse_package(sub_parser)) + return false; + } + return true; +} + +bool RobotState_V1_6__7::parse_package(BinParser &bp) { + //todo: size checks + package_type type; + bp.parse(type); + + switch(type) { + case package_type::ROBOT_MODE_DATA: + robot_mode.parse_with(bp); + break; + case package_type::JOINT_DATA: + //TODO + break; + case package_type::TOOL_DATA: + //TODO + break; + case package_type::MASTERBOARD_DATA: + master_board.parse_with(bp); + break; + case package_type::CARTESIAN_INFO: + //TODO + break; + default: + LOG_ERROR("Invalid package type parsed: %" PRIu8 "\n", type); + return false; + } + + return true; +} + +bool RobotState_V1_8::parse_package(BinParser &bp) { + package_type type = bp.peek(); + switch(type) { + case package_type::KINEMATICS_INFO: + break; + case package_type::CONFIGURATION_DATA: + break; + case package_type::ADDITIONAL_INFO: + break; + case package_type::CALIBRATION_DATA: + break; + default: + return RobotState_V1_6__7::parse_package(bp); + } + return true; +} \ No newline at end of file From b6eb109cd32378760e918f8c060c23f5d8a34c32 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Mon, 6 Feb 2017 14:27:34 +0100 Subject: [PATCH 008/114] Implemented URStateParser, URRTStateParser and URProducer --- include/ur_modern_driver/parser.h | 8 ++++ include/ur_modern_driver/ur/parser.h | 52 +++++++++++++++++++++++++ include/ur_modern_driver/ur/producer.h | 21 ++++++++++ src/ur/producer.cpp | 54 ++++++++++++++++++++++++++ 4 files changed, 135 insertions(+) create mode 100644 include/ur_modern_driver/parser.h create mode 100644 include/ur_modern_driver/ur/parser.h create mode 100644 include/ur_modern_driver/ur/producer.h create mode 100644 src/ur/producer.cpp diff --git a/include/ur_modern_driver/parser.h b/include/ur_modern_driver/parser.h new file mode 100644 index 000000000..d7a122270 --- /dev/null +++ b/include/ur_modern_driver/parser.h @@ -0,0 +1,8 @@ +#pragma once +#include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/packet.h" + +class Parser { +public: + virtual std::unique_ptr parse(BinParser &bp) = 0; +}; diff --git a/include/ur_modern_driver/ur/parser.h b/include/ur_modern_driver/ur/parser.h new file mode 100644 index 000000000..573b3d4cb --- /dev/null +++ b/include/ur_modern_driver/ur/parser.h @@ -0,0 +1,52 @@ +#pragma once +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/parser.h" +#include "ur_modern_driver/ur/state.h" + +template +class URStateParser : public Parser { + std::unique_ptr parse(BinParser &bp) { + int32_t packet_size = bp.peek(); + message_type type; + + if(!bp.check_size(packet_size)) { + LOG_ERROR("Buffer len shorter than expected packet length\n"); + return std::unique_ptr(nullptr); + } + + bp.parse(packet_size); //consumes the peeked data + bp.parse(type); + + if(type != message_type::ROBOT_STATE) { + LOG_ERROR("Invalid message type recieved: %u\n", static_cast(type)); + return std::unique_ptr(nullptr); + } + + std::unique_ptr obj(new T); + if(obj->parse_with(bp)) + return obj; + + return std::unique_ptr(nullptr); + } +}; + + +template +class URRTStateParser : public Parser { + std::unique_ptr parse(BinParser &bp) { + int32_t packet_size = bp.peek(); + + if(!bp.check_size(packet_size)) { + LOG_ERROR("Buffer len shorter than expected packet length\n"); + return std::unique_ptr(nullptr); + } + + bp.parse(packet_size); //consumes the peeked data + + std::unique_ptr obj(new T); + if(obj->parse_with(bp)) + return obj; + + return std::unique_ptr(nullptr); + } +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/producer.h b/include/ur_modern_driver/ur/producer.h new file mode 100644 index 000000000..b86b51907 --- /dev/null +++ b/include/ur_modern_driver/ur/producer.h @@ -0,0 +1,21 @@ +#pragma once +#include "ur_modern_driver/pipeline.h" +#include "ur_modern_driver/ur/stream.h" +#include "ur_modern_driver/packet.h" +#include "ur_modern_driver/parser.h" + +class URProducer : public IProducer { +private: + URStream &_stream; + Parser &_parser; + +public: + URProducer(URStream &stream, Parser &parser) + : _stream(stream), + _parser(parser) { } + + void setup_producer(); + void teardown_producer(); + void stop_producer(); + std::unique_ptr try_get(); +}; \ No newline at end of file diff --git a/src/ur/producer.cpp b/src/ur/producer.cpp new file mode 100644 index 000000000..f1ca009ba --- /dev/null +++ b/src/ur/producer.cpp @@ -0,0 +1,54 @@ +#include "ur_modern_driver/ur/producer.h" +#include "ur_modern_driver/log.h" + +void URProducer::setup_producer() { + _stream.connect(); +} + +void URProducer::teardown_producer() { + +} + +void URProducer::stop_producer() { + _stream.disconnect(); +} + +std::unique_ptr URProducer::try_get() { + //4KB should be enough to hold any packet received from UR + uint8_t buf[4096]; + + ssize_t total = 0; + int32_t packet_size = 0; + + //deal with partial recieves + while(total <= sizeof(buf)) { + uint8_t *pos = buf + total; + size_t size = sizeof(buf) - total; + + //blocking call + ssize_t len = _stream.receive(pos, size); + + if(len < 1) { + LOG_DEBUG("Read nothing from stream\n"); + return std::unique_ptr(nullptr); + } + + total += len; + BinParser bp(buf, static_cast(total)); + + if(packet_size == 0) { + packet_size = bp.peek(); + //TODO: check other wrong packet sizes? + if(packet_size > sizeof(buf)) { + LOG_ERROR("A packet with 'len' larger than buffer was received, discarding...\n"); + return std::unique_ptr(nullptr); + } + } + + if(total < packet_size){ + LOG_DEBUG("Partial packet recieved\n"); + continue; + } + return std::move(_parser.parse(bp)); + } +} \ No newline at end of file From 113b32d5af444e16e4f10d8356b0f32585a6203b Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Wed, 8 Feb 2017 10:30:52 +0100 Subject: [PATCH 009/114] Implemented network byte order decoding --- include/ur_modern_driver/bin_parser.h | 42 ++++++++++++++++++++++++--- 1 file changed, 38 insertions(+), 4 deletions(-) diff --git a/include/ur_modern_driver/bin_parser.h b/include/ur_modern_driver/bin_parser.h index f44d57fd4..02083d5c7 100644 --- a/include/ur_modern_driver/bin_parser.h +++ b/include/ur_modern_driver/bin_parser.h @@ -1,9 +1,10 @@ #pragma once #include +#include #include #include -#include +#include #include "ur_modern_driver/types.h" class BinParser { @@ -28,9 +29,41 @@ class BinParser { _parent._buf_pos = _buf_pos; } + + //Decode from network encoding (big endian) to host encoding + template + T decode(T val) { + return val; + } + uint16_t decode(uint16_t val) { + return be16toh(val); + } + uint32_t decode(uint32_t val) { + return be32toh(val); + } + uint64_t decode(uint64_t val) { + return be64toh(val); + } + int16_t decode(int16_t val) { + return be16toh(val); + } + int32_t decode(int32_t val) { + return be32toh(val); + } + int64_t decode(int64_t val) { + return be64toh(val); + } + float decode(float val) { + return be32toh(val); + } + double decode(double val) { + return be64toh(val); + } + + template T peek() { - return *(reinterpret_cast(_buf_pos)); + return decode(*(reinterpret_cast(_buf_pos))); } template @@ -74,8 +107,9 @@ class BinParser { template void parse(T (&array)[N]) { - std::memcpy(array, _buf_pos, sizeof(T)*N); - _buf_pos += (sizeof(T)*N); + for(size_t i = 0; i < N; i++) { + parse(array[i]); + } } void skip(size_t bytes) { From e523a5d466cbe6d75a66881e74f2f3d359abcdc0 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Wed, 8 Feb 2017 10:32:01 +0100 Subject: [PATCH 010/114] Newline parity between printf and ros logging --- include/ur_modern_driver/log.h | 10 +++++----- src/ur/producer.cpp | 6 +++--- src/ur/state.cpp | 6 +++--- src/ur/stream.cpp | 8 ++++---- 4 files changed, 15 insertions(+), 15 deletions(-) diff --git a/include/ur_modern_driver/log.h b/include/ur_modern_driver/log.h index 0d96b88c9..383740207 100644 --- a/include/ur_modern_driver/log.h +++ b/include/ur_modern_driver/log.h @@ -12,10 +12,10 @@ #else - #define LOG_DEBUG(format, ...) printf("DEBUG: " format, ##__VA_ARGS__) - #define LOG_WARN(format, ...) printf("WARNING: " format, ##__VA_ARGS__) - #define LOG_INFO(format, ...) printf("INFO: " format, ##__VA_ARGS__) - #define LOG_ERROR(format, ...) printf("ERROR: " format, ##__VA_ARGS__) - #define LOG_FATAL(format, ...) printf("FATAL: " format, ##__VA_ARGS__) + #define LOG_DEBUG(format, ...) printf("DEBUG: " format "\n", ##__VA_ARGS__) + #define LOG_WARN(format, ...) printf("WARNING: " format "\n", ##__VA_ARGS__) + #define LOG_INFO(format, ...) printf("INFO: " format "\n", ##__VA_ARGS__) + #define LOG_ERROR(format, ...) printf("ERROR: " format "\n", ##__VA_ARGS__) + #define LOG_FATAL(format, ...) printf("FATAL: " format "\n", ##__VA_ARGS__) #endif \ No newline at end of file diff --git a/src/ur/producer.cpp b/src/ur/producer.cpp index f1ca009ba..bd51daea0 100644 --- a/src/ur/producer.cpp +++ b/src/ur/producer.cpp @@ -29,7 +29,7 @@ std::unique_ptr URProducer::try_get() { ssize_t len = _stream.receive(pos, size); if(len < 1) { - LOG_DEBUG("Read nothing from stream\n"); + LOG_DEBUG("Read nothing from stream"); return std::unique_ptr(nullptr); } @@ -40,13 +40,13 @@ std::unique_ptr URProducer::try_get() { packet_size = bp.peek(); //TODO: check other wrong packet sizes? if(packet_size > sizeof(buf)) { - LOG_ERROR("A packet with 'len' larger than buffer was received, discarding...\n"); + LOG_ERROR("A packet with 'len' (%d) larger than buffer was received, discarding...", packet_size); return std::unique_ptr(nullptr); } } if(total < packet_size){ - LOG_DEBUG("Partial packet recieved\n"); + LOG_DEBUG("Partial packet recieved"); continue; } return std::move(_parser.parse(bp)); diff --git a/src/ur/state.cpp b/src/ur/state.cpp index 460bdf1f0..363199230 100644 --- a/src/ur/state.cpp +++ b/src/ur/state.cpp @@ -6,13 +6,13 @@ bool RobotState::parse_with(BinParser &bp) { //continue as long as there are bytes to read while(bp.check_size(sizeof(uint8_t))) { if(!bp.check_size(sizeof(uint32_t))){ - LOG_ERROR("Failed to read sub-package length, there's likely a parsing error\n"); + LOG_ERROR("Failed to read sub-package length, there's likely a parsing error"); return false; } uint32_t sub_size = bp.peek(); if(!bp.check_size(static_cast(sub_size))) { - LOG_WARN("Invalid sub-package size of %" PRIu32 " received!\n", sub_size); + LOG_WARN("Invalid sub-package size of %" PRIu32 " received!", sub_size); return false; } @@ -48,7 +48,7 @@ bool RobotState_V1_6__7::parse_package(BinParser &bp) { //TODO break; default: - LOG_ERROR("Invalid package type parsed: %" PRIu8 "\n", type); + LOG_ERROR("Invalid package type parsed: %" PRIu8 "", type); return false; } diff --git a/src/ur/stream.cpp b/src/ur/stream.cpp index a7c53a8b9..fad5ef96d 100644 --- a/src/ur/stream.cpp +++ b/src/ur/stream.cpp @@ -9,7 +9,7 @@ bool URStream::connect() { if(_initialized) return false; - LOG_INFO("Connecting to UR @ %s:%d\n", _host.c_str(), _port); + LOG_INFO("Connecting to UR @ %s:%d", _host.c_str(), _port); //gethostbyname() is deprecated so use getadderinfo() as described in: //http://www.beej.us/guide/bgnet/output/html/multipage/syscalls.html#getaddrinfo @@ -23,7 +23,7 @@ bool URStream::connect() { hints.ai_flags = AI_PASSIVE; if(getaddrinfo(_host.c_str(), service.c_str(), &hints, &result) != 0) { - LOG_ERROR("Failed to get host name\n"); + LOG_ERROR("Failed to get host name"); return false; } @@ -45,13 +45,13 @@ bool URStream::connect() { int flag = 1; setsockopt(_socket_fd, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(flag)); _initialized = true; - LOG_INFO("Connection successfully established\n"); + LOG_INFO("Connection successfully established"); break; } freeaddrinfo(result); if(!_initialized) - LOG_ERROR("Connection failed\n"); + LOG_ERROR("Connection failed"); return _initialized; } From e94bbb55361e2c02088bdca7467412493a24f08e Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Thu, 9 Feb 2017 12:15:54 +0100 Subject: [PATCH 011/114] BinParser improvements --- include/ur_modern_driver/bin_parser.h | 34 +++++++++++++++++++++------ 1 file changed, 27 insertions(+), 7 deletions(-) diff --git a/include/ur_modern_driver/bin_parser.h b/include/ur_modern_driver/bin_parser.h index 02083d5c7..d8cde1b41 100644 --- a/include/ur_modern_driver/bin_parser.h +++ b/include/ur_modern_driver/bin_parser.h @@ -4,8 +4,10 @@ #include #include #include -#include +#include +#include #include "ur_modern_driver/types.h" +#include "ur_modern_driver/log.h" class BinParser { private: @@ -16,14 +18,16 @@ class BinParser { BinParser(uint8_t *buffer, size_t buf_len) : _buf_pos(buffer), _buf_end(buffer+buf_len), - _parent(*this) - { } + _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) - { } + _parent(parent) { + assert(_buf_pos <= _buf_end); + } ~BinParser() { _parent._buf_pos = _buf_pos; @@ -63,6 +67,7 @@ class BinParser { template T peek() { + assert(_buf_pos <= _buf_end); return decode(*(reinterpret_cast(_buf_pos))); } @@ -93,8 +98,12 @@ class BinParser { parse(val.rotation); } + void parse_remainder(std::string &val) { + parse(val, size_t(_buf_end - _buf_pos)); + } + void parse(std::string &val, size_t len) { - val = val.assign(reinterpret_cast(_buf_pos), len); + val.assign(reinterpret_cast(_buf_pos), len); _buf_pos += len; } @@ -112,7 +121,10 @@ class BinParser { } } - void skip(size_t bytes) { + void consume() { + _buf_pos = _buf_end; + } + void consume(size_t bytes) { _buf_pos += bytes; } @@ -123,4 +135,12 @@ class BinParser { bool check_size(void) { return check_size(T::SIZE); } + + bool empty() { + return _buf_pos == _buf_end; + } + + void debug() { + LOG_DEBUG("BinParser: %zx - %zx (%zu bytes)", _buf_pos, _buf_end, _buf_end - _buf_pos); + } }; From 43974dcbf262734923cd8494831799c7e09477ff Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Thu, 9 Feb 2017 12:16:51 +0100 Subject: [PATCH 012/114] Refactoring, improvements and fixes --- include/ur_modern_driver/ur/master_board.h | 16 +++- include/ur_modern_driver/ur/messages.h | 19 ++++- include/ur_modern_driver/ur/parser.h | 60 ++++++++++++--- include/ur_modern_driver/ur/state.h | 41 +++++++++- include/ur_modern_driver/ur/stream.h | 4 + src/ur/master_board.cpp | 22 +++++- src/ur/messages.cpp | 7 +- src/ur/producer.cpp | 46 +++--------- src/ur/state.cpp | 87 ++++++++++++++++------ src/ur/stream.cpp | 35 ++++++++- 10 files changed, 249 insertions(+), 88 deletions(-) diff --git a/include/ur_modern_driver/ur/master_board.h b/include/ur_modern_driver/ur/master_board.h index e85b2ddb6..39317ebb8 100644 --- a/include/ur_modern_driver/ur/master_board.h +++ b/include/ur_modern_driver/ur/master_board.h @@ -60,7 +60,7 @@ class MasterBoardData_V1_X : public SharedMasterBoardData { + sizeof(int16_t) * 2; }; -class MasterBoardData_V3_X : public SharedMasterBoardData { +class MasterBoardData_V3_0__1 : public SharedMasterBoardData { public: virtual bool parse_with(BinParser &bp); @@ -77,8 +77,20 @@ class MasterBoardData_V3_X : public SharedMasterBoardData { static const size_t SIZE = SharedMasterBoardData::SIZE + sizeof(int32_t) * 2 - + sizeof(uint8_t) * 2; + + sizeof(uint8_t) * 2 + + sizeof(uint32_t); //UR internal field we skip static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE + sizeof(float) * 2; }; + +class MasterBoardData_V3_2 : public MasterBoardData_V3_0__1 { +public: + virtual bool parse_with(BinParser &bp); + + uint8_t operational_mode_selector_input; + uint8_t three_position_enabling_device_input; + + static const size_t SIZE = MasterBoardData_V3_0__1::SIZE + + sizeof(uint8_t) * 2; +}; diff --git a/include/ur_modern_driver/ur/messages.h b/include/ur_modern_driver/ur/messages.h index 31ec39192..838c31219 100644 --- a/include/ur_modern_driver/ur/messages.h +++ b/include/ur_modern_driver/ur/messages.h @@ -1,6 +1,21 @@ #pragma once +#include +#include +#include "ur_modern_driver/packet.h" -class MessageBase { +enum class robot_message_type : uint8_t { + ROBOT_MESSAGE_TEXT = 0, + ROBOT_MESSAGE_PROGRAM_LABEL = 1, + PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2, + ROBOT_MESSAGE_VERSION = 3, + ROBOT_MESSAGE_SAFETY_MODE = 5, + ROBOT_MESSAGE_ERROR_CODE = 6, + ROBOT_MESSAGE_KEY = 7, + ROBOT_MESSAGE_REQUEST_VALUE = 9, + ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10 +}; + +class MessageBase : public Packet { public: virtual bool parse_with(BinParser &bp) = 0; @@ -17,4 +32,4 @@ class VersionMessage : public MessageBase { uint8_t minor_version; int32_t svn_version; std::string build_date; -} \ No newline at end of file +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/parser.h b/include/ur_modern_driver/ur/parser.h index 573b3d4cb..9cd80677f 100644 --- a/include/ur_modern_driver/ur/parser.h +++ b/include/ur_modern_driver/ur/parser.h @@ -2,23 +2,19 @@ #include "ur_modern_driver/log.h" #include "ur_modern_driver/parser.h" #include "ur_modern_driver/ur/state.h" +#include "ur_modern_driver/ur/rt_state.h" +#include "ur_modern_driver/ur/messages.h" template class URStateParser : public Parser { - std::unique_ptr parse(BinParser &bp) { - int32_t packet_size = bp.peek(); + std::unique_ptr parse(BinParser &bp) { + int32_t packet_size; message_type type; - - if(!bp.check_size(packet_size)) { - LOG_ERROR("Buffer len shorter than expected packet length\n"); - return std::unique_ptr(nullptr); - } - - bp.parse(packet_size); //consumes the peeked data + bp.parse(packet_size); bp.parse(type); if(type != message_type::ROBOT_STATE) { - LOG_ERROR("Invalid message type recieved: %u\n", static_cast(type)); + LOG_ERROR("Invalid message type recieved: %u", static_cast(type)); return std::unique_ptr(nullptr); } @@ -33,11 +29,11 @@ class URStateParser : public Parser { template class URRTStateParser : public Parser { - std::unique_ptr parse(BinParser &bp) { + std::unique_ptr parse(BinParser &bp) { int32_t packet_size = bp.peek(); if(!bp.check_size(packet_size)) { - LOG_ERROR("Buffer len shorter than expected packet length\n"); + LOG_ERROR("Buffer len shorter than expected packet length"); return std::unique_ptr(nullptr); } @@ -49,4 +45,44 @@ class URRTStateParser : public Parser { return std::unique_ptr(nullptr); } +}; + +class URMessageParser : public Parser { + std::unique_ptr parse(BinParser &bp) { + int32_t packet_size = bp.peek(); + message_type type; + + if(!bp.check_size(packet_size)) { + LOG_ERROR("Buffer len shorter than expected packet length"); + return std::unique_ptr(nullptr); + } + + bp.parse(packet_size); //consumes the peeked data + bp.parse(type); + + if(type != message_type::ROBOT_MESSAGE) { + LOG_ERROR("Invalid message type recieved: %u", static_cast(type)); + return std::unique_ptr(nullptr); + } + + uint64_t timestamp; + uint8_t source; + robot_message_type message_type; + + bp.parse(timestamp); + bp.parse(source); + bp.parse(message_type); + + std::unique_ptr obj(nullptr); + + switch(message_type) { + case robot_message_type::ROBOT_MESSAGE_VERSION: + VersionMessage *vm = new VersionMessage(); + if(vm->parse_with(bp)) + obj.reset(vm); + break; + } + + return obj; + } }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/state.h b/include/ur_modern_driver/ur/state.h index f76e8a673..085d42f95 100644 --- a/include/ur_modern_driver/ur/state.h +++ b/include/ur_modern_driver/ur/state.h @@ -37,7 +37,7 @@ class RobotState : public Packet { class RobotState_V1_6__7 : public RobotState { protected: - virtual bool parse_package(BinParser &bp) = 0; + bool parse_package(BinParser &bp); public: RobotModeData_V1_X robot_mode; //JointData @@ -48,7 +48,7 @@ class RobotState_V1_6__7 : public RobotState { class RobotState_V1_8 : public RobotState_V1_6__7 { protected: - virtual bool parse_package(BinParser &bp) = 0; + bool parse_package(BinParser &bp); public: //KinematicsInfo @@ -56,4 +56,39 @@ class RobotState_V1_8 : public RobotState_V1_6__7 { //ForceModeData //AdditionalInfo //CalibrationData -}; \ No newline at end of file +}; + + +class RobotState_V3_0__1 : public RobotState { +protected: + bool parse_package(BinParser &bp); +public: + RobotModeData_V3_0__1 robot_mode; + //JointData + //ToolData + MasterBoardData_V3_0__1 master_board; + //CartesianInfo + + //KinematicsInfo + //ConfigurationData + //ForceModeData + //AdditionalInfo + //CalibrationData +}; + +class RobotState_V3_2 : public RobotState { +protected: + bool parse_package(BinParser &bp); +public: + RobotModeData_V3_2 robot_mode; + //JointData + //ToolData + MasterBoardData_V3_2 master_board; + //CartesianInfo + + //KinematicsInfo + //ConfigurationData + //ForceModeData + //AdditionalInfo + //CalibrationData +}; diff --git a/include/ur_modern_driver/ur/stream.h b/include/ur_modern_driver/ur/stream.h index 88a5ed642..a6ac0d58c 100644 --- a/include/ur_modern_driver/ur/stream.h +++ b/include/ur_modern_driver/ur/stream.h @@ -21,6 +21,10 @@ class URStream { _port(port), _initialized(false), _stopping(false) {} + + ~URStream() { + disconnect(); + } bool connect(); void disconnect(); diff --git a/src/ur/master_board.cpp b/src/ur/master_board.cpp index 084211a18..9c0f8655d 100644 --- a/src/ur/master_board.cpp +++ b/src/ur/master_board.cpp @@ -40,8 +40,8 @@ bool MasterBoardData_V1_X::parse_with(BinParser &bp) { return true; } -bool MasterBoardData_V3_X::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool MasterBoardData_V3_0__1::parse_with(BinParser &bp) { + if(!bp.check_size()) return false; bp.parse(digital_input_bits); @@ -54,13 +54,29 @@ bool MasterBoardData_V3_X::parse_with(BinParser &bp) { bp.parse(euromap67_interface_installed); if(euromap67_interface_installed) { - if(!bp.check_size(MasterBoardData_V3_X::EURO_SIZE)) + if(!bp.check_size(MasterBoardData_V3_0__1::EURO_SIZE)) return false; bp.parse(euromap_voltage); bp.parse(euromap_current); } + bp.consume(sizeof(uint32_t)); + + return true; +} + + + +bool MasterBoardData_V3_2::parse_with(BinParser &bp) { + if(!bp.check_size()) + return false; + + MasterBoardData_V3_0__1::parse_with(bp); + + bp.parse(operational_mode_selector_input); + bp.parse(three_position_enabling_device_input); + return true; } diff --git a/src/ur/messages.cpp b/src/ur/messages.cpp index 0e7ceea56..42ceb8ab0 100644 --- a/src/ur/messages.cpp +++ b/src/ur/messages.cpp @@ -13,10 +13,9 @@ bool VersionMessage::parse_with(BinParser &bp) { bp.parse(project_name); bp.parse(major_version); bp.parse(minor_version); - bp.parse(svn_version); //net to host? - - // how to parse this without length?? - //bp.parse(build_date); + bp.parse(svn_version); + bp.consume(sizeof(uint32_t)); //undocumented field?? + bp.parse_remainder(build_date); return true; //not really possible to check dynamic size packets } \ No newline at end of file diff --git a/src/ur/producer.cpp b/src/ur/producer.cpp index bd51daea0..87fb6ed75 100644 --- a/src/ur/producer.cpp +++ b/src/ur/producer.cpp @@ -16,39 +16,17 @@ void URProducer::stop_producer() { std::unique_ptr URProducer::try_get() { //4KB should be enough to hold any packet received from UR uint8_t buf[4096]; - - ssize_t total = 0; - int32_t packet_size = 0; - - //deal with partial recieves - while(total <= sizeof(buf)) { - uint8_t *pos = buf + total; - size_t size = sizeof(buf) - total; - - //blocking call - ssize_t len = _stream.receive(pos, size); - - if(len < 1) { - LOG_DEBUG("Read nothing from stream"); - return std::unique_ptr(nullptr); - } - - total += len; - BinParser bp(buf, static_cast(total)); - - if(packet_size == 0) { - packet_size = bp.peek(); - //TODO: check other wrong packet sizes? - if(packet_size > sizeof(buf)) { - LOG_ERROR("A packet with 'len' (%d) larger than buffer was received, discarding...", packet_size); - return std::unique_ptr(nullptr); - } - } - - if(total < packet_size){ - LOG_DEBUG("Partial packet recieved"); - continue; - } - return std::move(_parser.parse(bp)); + + //blocking call + ssize_t len = _stream.receive(buf, sizeof(buf)); + + LOG_DEBUG("Read %d bytes from stream", len); + + if(len < 1) { + LOG_WARN("Read nothing from stream"); + return std::unique_ptr(nullptr); } + + BinParser bp(buf, static_cast(len)); + return std::move(_parser.parse(bp)); } \ No newline at end of file diff --git a/src/ur/state.cpp b/src/ur/state.cpp index 363199230..851c906b0 100644 --- a/src/ur/state.cpp +++ b/src/ur/state.cpp @@ -4,7 +4,8 @@ bool RobotState::parse_with(BinParser &bp) { //continue as long as there are bytes to read - while(bp.check_size(sizeof(uint8_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; @@ -18,56 +19,94 @@ bool RobotState::parse_with(BinParser &bp) { //deconstruction of a sub parser will increment the position of the parent parser BinParser sub_parser(bp, sub_size); - sub_parser.parse(sub_size); + sub_parser.consume(sizeof(sub_size)); - if(!parse_package(sub_parser)) + if(!parse_package(sub_parser)) { + LOG_ERROR("Failed to parse sub-package"); return false; + } + + if(!sub_parser.empty()) { + LOG_ERROR("Sub-package was not parsed completely!"); + return false; + } } return true; } -bool RobotState_V1_6__7::parse_package(BinParser &bp) { - //todo: size checks - package_type type; - bp.parse(type); - +template +bool parse_base(BinParser &bp, T &pkg) { + package_type type = bp.peek(); switch(type) { case package_type::ROBOT_MODE_DATA: - robot_mode.parse_with(bp); - break; - case package_type::JOINT_DATA: - //TODO - break; - case package_type::TOOL_DATA: - //TODO + LOG_DEBUG("Parsing robot_mode"); + bp.consume(sizeof(package_type)); + pkg.robot_mode.parse_with(bp); break; case package_type::MASTERBOARD_DATA: - master_board.parse_with(bp); + LOG_DEBUG("Parsing master_board"); + bp.consume(sizeof(package_type)); + pkg.master_board.parse_with(bp); break; + + case package_type::TOOL_DATA: case package_type::CARTESIAN_INFO: - //TODO + case package_type::JOINT_DATA: + LOG_DEBUG("Skipping tool, cartesian or joint data"); + //for unhandled packages we consume the rest of the + //package buffer + bp.consume(); break; default: - LOG_ERROR("Invalid package type parsed: %" PRIu8 "", type); return false; } - return true; } -bool RobotState_V1_8::parse_package(BinParser &bp) { + +template +bool parse_advanced(BinParser &bp, T &pkg) { + if(parse_base(bp, pkg)) + return true; + package_type type = bp.peek(); + switch(type) { case package_type::KINEMATICS_INFO: - break; case package_type::CONFIGURATION_DATA: - break; case package_type::ADDITIONAL_INFO: - break; case package_type::CALIBRATION_DATA: + case package_type::FORCE_MODE_DATA: + LOG_DEBUG("Skipping kinematics, config, additional, calibration or force mode data"); + //for unhandled packages we consume the rest of the + //package buffer + bp.consume(); break; default: - return RobotState_V1_6__7::parse_package(bp); + LOG_ERROR("Invalid sub-package type parsed: %" PRIu8 "", type); + return false; } + return true; +} + + +bool RobotState_V1_6__7::parse_package(BinParser &bp) { + if(!parse_base(bp, *this)) { + LOG_ERROR("Invalid sub-package type parsed: %" PRIu8 "", bp.peek()); + return false; + } + return true; +} + +bool RobotState_V1_8::parse_package(BinParser &bp) { + return parse_advanced(bp, *this); +} + +bool RobotState_V3_0__1::parse_package(BinParser &bp) { + return parse_advanced(bp, *this); +} + +bool RobotState_V3_2::parse_package(BinParser &bp) { + return parse_advanced(bp, *this); } \ No newline at end of file diff --git a/src/ur/stream.cpp b/src/ur/stream.cpp index fad5ef96d..f66482b96 100644 --- a/src/ur/stream.cpp +++ b/src/ur/stream.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include "ur_modern_driver/ur/stream.h" #include "ur_modern_driver/log.h" @@ -78,8 +79,8 @@ ssize_t URStream::send(uint8_t *buf, size_t buf_len) { //handle partial sends while(total < buf_len) { ssize_t sent = ::send(_socket_fd, buf+total, remaining, 0); - if(sent == -1) - return _stopping ? 0 : -1; + if(sent <= 0) + return _stopping ? 0 : sent; total += sent; remaining -= sent; } @@ -88,6 +89,32 @@ ssize_t URStream::send(uint8_t *buf, size_t buf_len) { } ssize_t URStream::receive(uint8_t *buf, size_t buf_len) { - //TODO: handle reconnect? - return recv(_socket_fd, buf, buf_len, 0); + if(!_initialized) + return -1; + if(_stopping) + return 0; + + size_t remainder = sizeof(int32_t); + uint8_t *buf_pos = buf; + bool initial = true; + + do { + ssize_t read = recv(_socket_fd, buf_pos, remainder, 0); + if(read <= 0) //failed reading from socket + return _stopping ? 0 : read; + + if(initial) { + remainder = be32toh(*(reinterpret_cast(buf))); + if(remainder >= (buf_len - sizeof(int32_t))) { + LOG_ERROR("Packet size %d is larger than buffer %d, discarding.", remainder, buf_len); + return -1; + } + initial = false; + } + + buf_pos += read; + remainder -= read; + } while(remainder > 0); + + return buf_pos - buf; } \ No newline at end of file From 32cfed93b305a736053a3e4811540a35a05c7196 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Wed, 15 Feb 2017 23:05:52 +0100 Subject: [PATCH 013/114] Better non ros output logging --- include/ur_modern_driver/log.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/include/ur_modern_driver/log.h b/include/ur_modern_driver/log.h index 383740207..db0f5b698 100644 --- a/include/ur_modern_driver/log.h +++ b/include/ur_modern_driver/log.h @@ -12,10 +12,10 @@ #else - #define LOG_DEBUG(format, ...) printf("DEBUG: " format "\n", ##__VA_ARGS__) - #define LOG_WARN(format, ...) printf("WARNING: " format "\n", ##__VA_ARGS__) - #define LOG_INFO(format, ...) printf("INFO: " format "\n", ##__VA_ARGS__) - #define LOG_ERROR(format, ...) printf("ERROR: " format "\n", ##__VA_ARGS__) - #define LOG_FATAL(format, ...) printf("FATAL: " format "\n", ##__VA_ARGS__) + #define LOG_DEBUG(format, ...) printf("[DEBUG]: " format "\n", ##__VA_ARGS__) + #define LOG_WARN(format, ...) printf("[WARNING]: " format "\n", ##__VA_ARGS__) + #define LOG_INFO(format, ...) printf("[INFO]: " format "\n", ##__VA_ARGS__) + #define LOG_ERROR(format, ...) printf("[ERROR]: " format "\n", ##__VA_ARGS__) + #define LOG_FATAL(format, ...) printf("[FATAL]: " format "\n", ##__VA_ARGS__) #endif \ No newline at end of file From d0fa801cad2c313ff2a06bfa01cf41c6759b6282 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Wed, 15 Feb 2017 23:06:08 +0100 Subject: [PATCH 014/114] Improved bin parser debugging --- include/ur_modern_driver/bin_parser.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ur_modern_driver/bin_parser.h b/include/ur_modern_driver/bin_parser.h index d8cde1b41..c7f5d7cdf 100644 --- a/include/ur_modern_driver/bin_parser.h +++ b/include/ur_modern_driver/bin_parser.h @@ -141,6 +141,6 @@ class BinParser { } void debug() { - LOG_DEBUG("BinParser: %zx - %zx (%zu bytes)", _buf_pos, _buf_end, _buf_end - _buf_pos); + LOG_DEBUG("BinParser: %p - %p (%zu bytes)", _buf_pos, _buf_end, _buf_end - _buf_pos); } }; From c282c961f74d99b0adf2cac28d3b379124566bbb Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Thu, 16 Feb 2017 01:52:22 +0100 Subject: [PATCH 015/114] Completed parsing of UR state, messages and RT --- include/ur_modern_driver/pipeline.h | 47 ++++++---- include/ur_modern_driver/ur/consumer.h | 46 ++++++++++ include/ur_modern_driver/ur/factory.h | 85 ++++++++++++++++++ include/ur_modern_driver/ur/master_board.h | 8 +- include/ur_modern_driver/ur/messages.h | 18 +++- include/ur_modern_driver/ur/messages_parser.h | 48 ++++++++++ include/ur_modern_driver/ur/parser.h | 90 ++----------------- include/ur_modern_driver/ur/producer.h | 41 +++++++-- include/ur_modern_driver/ur/robot_mode.h | 19 ++-- include/ur_modern_driver/ur/rt_parser.h | 36 ++++++++ include/ur_modern_driver/ur/rt_state.h | 87 +++++++++++++++--- include/ur_modern_driver/ur/state.h | 74 ++------------- include/ur_modern_driver/ur/state_parser.h | 82 +++++++++++++++++ src/ur/master_board.cpp | 14 +++ src/ur/messages.cpp | 13 ++- src/ur/producer.cpp | 32 ------- src/ur/robot_mode.cpp | 14 +++ src/ur/rt_state.cpp | 86 ++++++++++++++++-- src/ur/state.cpp | 7 +- 19 files changed, 599 insertions(+), 248 deletions(-) create mode 100644 include/ur_modern_driver/ur/consumer.h create mode 100644 include/ur_modern_driver/ur/factory.h create mode 100644 include/ur_modern_driver/ur/messages_parser.h create mode 100644 include/ur_modern_driver/ur/rt_parser.h create mode 100644 include/ur_modern_driver/ur/state_parser.h delete mode 100644 src/ur/producer.cpp diff --git a/include/ur_modern_driver/pipeline.h b/include/ur_modern_driver/pipeline.h index ac2fe535d..b718f6239 100644 --- a/include/ur_modern_driver/pipeline.h +++ b/include/ur_modern_driver/pipeline.h @@ -2,6 +2,8 @@ #include #include +#include +#include "ur_modern_driver/log.h" #include "ur_modern_driver/queue/readerwriterqueue.h" using namespace moodycamel; @@ -9,23 +11,26 @@ using namespace std; template -class IProducer { +class IConsumer { public: - virtual void setup_producer() = 0; - virtual void teardown_producer() = 0; - virtual void stop_producer() = 0; - virtual unique_ptr try_get() = 0; + virtual void setup_consumer() { } + virtual void teardown_consumer() { } + virtual void stop_consumer() { } + + virtual bool consume(unique_ptr product) = 0; }; template -class IConsumer { +class IProducer { public: - virtual void setup_consumer() = 0; - virtual void teardown_consumer() = 0; - virtual void stop_consumer() = 0; - virtual bool push(unique_ptr product) = 0; + virtual void setup_producer() { } + virtual void teardown_producer() { } + virtual void stop_producer() { } + + virtual bool try_get(std::vector> &products) = 0; }; + template class Pipeline { private: @@ -37,15 +42,19 @@ class Pipeline { void run_producer() { _producer.setup_producer(); + std::vector> products; while(_running) { - unique_ptr product(_producer.try_get()); - - if(product == nullptr) + if(!_producer.try_get(products)) { break; - - if(!_queue.try_enqueue(std::move(product))) { - //log dropped product } + + for(auto &p : products) { + if(!_queue.try_enqueue(std::move(p))) { + LOG_WARN("Pipeline owerflowed!"); + } + } + + products.clear(); } _producer.teardown_producer(); //todo cleanup @@ -53,10 +62,10 @@ class Pipeline { void run_consumer() { _consumer.setup_consumer(); + unique_ptr product; while(_running) { - unique_ptr product; _queue.wait_dequeue(product); - if(!_consumer.push(std::move(product))) + if(!_consumer.consume(std::move(product))) break; } _consumer.teardown_consumer(); @@ -86,6 +95,8 @@ class Pipeline { _consumer.stop_consumer(); _producer.stop_producer(); + _running = false; + _pThread.join(); _cThread.join(); } diff --git a/include/ur_modern_driver/ur/consumer.h b/include/ur_modern_driver/ur/consumer.h new file mode 100644 index 000000000..5b2a373e8 --- /dev/null +++ b/include/ur_modern_driver/ur/consumer.h @@ -0,0 +1,46 @@ +#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/robot_mode.h" +#include "ur_modern_driver/ur/rt_state.h" +#include "ur_modern_driver/ur/messages.h" + +class URRTPacketConsumer : public IConsumer { +public: + virtual bool consume(unique_ptr packet) { + return packet->consume_with(*this); + } + + virtual bool consume(RTState_V1_6__7 &state) = 0; + virtual bool consume(RTState_V1_8 &state) = 0; + virtual bool consume(RTState_V3_0__1 &state) = 0; + virtual bool consume(RTState_V3_2__3 &state) = 0; +}; + + +class URStatePacketConsumer : public IConsumer { +public: + virtual bool consume(unique_ptr packet) { + return packet->consume_with(*this); + } + + virtual bool consume(MasterBoardData_V1_X &data) = 0; + virtual bool consume(MasterBoardData_V3_0__1 &data) = 0; + virtual bool consume(MasterBoardData_V3_2 &data) = 0; + + virtual bool consume(RobotModeData_V1_X &data) = 0; + virtual bool consume(RobotModeData_V3_0__1 &data) = 0; + virtual bool consume(RobotModeData_V3_2 &data) = 0; +}; + + +class URMessagePacketConsumer : public IConsumer { +public: + virtual bool consume(unique_ptr packet) { + return packet->consume_with(*this); + } + + virtual bool consume(VersionMessage &message) = 0; +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/factory.h b/include/ur_modern_driver/ur/factory.h new file mode 100644 index 000000000..a09186f40 --- /dev/null +++ b/include/ur_modern_driver/ur/factory.h @@ -0,0 +1,85 @@ +#pragma once + +#include +#include "ur_modern_driver/ur/stream.h" +#include "ur_modern_driver/ur/consumer.h" +#include "ur_modern_driver/ur/producer.h" +#include "ur_modern_driver/ur/parser.h" +#include "ur_modern_driver/ur/state_parser.h" +#include "ur_modern_driver/ur/rt_parser.h" +#include "ur_modern_driver/ur/messages_parser.h" + + +class URFactory : private URMessagePacketConsumer { +private: + URStream _stream; + URMessageParser _parser; + + uint8_t _major_version; + uint8_t _minor_version; + + 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); + LOG_INFO("build date: %s", vm.build_date.c_str()); + + _major_version = vm.major_version; + _minor_version = vm.minor_version; + + return true; + } + + void setup_consumer() { } + void teardown_consumer() { } + void stop_consumer() { } + +public: + URFactory(std::string &host) : _stream(host, 30001) { + URProducer p(_stream, _parser); + std::vector> results; + + p.setup_producer(); + + if(!p.try_get(results) || results.size() == 0) { + LOG_FATAL("No version message received, init failed!"); + std::exit(EXIT_FAILURE); + } + + for(auto const& p : results) { + p->consume_with(*this); + } + + if(_major_version == 0 && _minor_version == 0) { + LOG_FATAL("No version message received, init failed!"); + std::exit(EXIT_FAILURE); + } + + p.teardown_producer(); + } + + std::unique_ptr> get_state_parser() { + if(_major_version == 1) { + return std::unique_ptr>(new URStateParser_V1_X); + } else { + if(_minor_version < 3) + return std::unique_ptr>(new URStateParser_V3_0__1); + else + return std::unique_ptr>(new URStateParser_V3_2); + } + } + + std::unique_ptr> get_rt_parser() { + if(_major_version == 1) { + if(_minor_version < 8) + return std::unique_ptr>(new URRTStateParser_V1_6__7); + else + return std::unique_ptr>(new URRTStateParser_V1_8); + } else { + if(_minor_version < 3) + return std::unique_ptr>(new URRTStateParser_V3_0__1); + else + return std::unique_ptr>(new URRTStateParser_V3_2__3); + } + } +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/master_board.h b/include/ur_modern_driver/ur/master_board.h index 39317ebb8..0a7daf135 100644 --- a/include/ur_modern_driver/ur/master_board.h +++ b/include/ur_modern_driver/ur/master_board.h @@ -4,6 +4,7 @@ #include #include "ur_modern_driver/types.h" #include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/ur/state.h" class SharedMasterBoardData { @@ -37,9 +38,10 @@ class SharedMasterBoardData { static const size_t EURO_SIZE = sizeof(int32_t) * 2; }; -class MasterBoardData_V1_X : public SharedMasterBoardData { +class MasterBoardData_V1_X : public SharedMasterBoardData, public StatePacket { public: virtual bool parse_with(BinParser &bp); + virtual bool consume_with(URStatePacketConsumer &consumer); int16_t digital_input_bits; int16_t digital_output_bits; @@ -60,9 +62,10 @@ class MasterBoardData_V1_X : public SharedMasterBoardData { + sizeof(int16_t) * 2; }; -class MasterBoardData_V3_0__1 : public SharedMasterBoardData { +class MasterBoardData_V3_0__1 : public SharedMasterBoardData, public StatePacket { public: virtual bool parse_with(BinParser &bp); + virtual bool consume_with(URStatePacketConsumer &consumer); int32_t digital_input_bits; int32_t digital_output_bits; @@ -87,6 +90,7 @@ class MasterBoardData_V3_0__1 : public SharedMasterBoardData { class MasterBoardData_V3_2 : public MasterBoardData_V3_0__1 { public: virtual bool parse_with(BinParser &bp); + virtual bool consume_with(URStatePacketConsumer &consumer); uint8_t operational_mode_selector_input; uint8_t three_position_enabling_device_input; diff --git a/include/ur_modern_driver/ur/messages.h b/include/ur_modern_driver/ur/messages.h index 838c31219..97315debd 100644 --- a/include/ur_modern_driver/ur/messages.h +++ b/include/ur_modern_driver/ur/messages.h @@ -1,7 +1,10 @@ #pragma once + #include #include -#include "ur_modern_driver/packet.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, @@ -15,17 +18,24 @@ enum class robot_message_type : uint8_t { ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10 }; -class MessageBase : public Packet { +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; uint64_t timestamp; uint8_t source; }; -class VersionMessage : public MessageBase { +class VersionMessage : public MessagePacket { public: - bool parse_with(BinParser &bp); + VersionMessage(uint64_t timestamp, uint8_t source) : MessagePacket(timestamp, source) { } + + virtual bool parse_with(BinParser &bp); + virtual bool consume_with(URMessagePacketConsumer &consumer); std::string project_name; uint8_t major_version; diff --git a/include/ur_modern_driver/ur/messages_parser.h b/include/ur_modern_driver/ur/messages_parser.h new file mode 100644 index 000000000..77dc9b705 --- /dev/null +++ b/include/ur_modern_driver/ur/messages_parser.h @@ -0,0 +1,48 @@ +#pragma once +#include +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/pipeline.h" +#include "ur_modern_driver/ur/parser.h" +#include "ur_modern_driver/ur/messages.h" + +class URMessageParser : public URParser { +public: + bool parse(BinParser &bp, std::vector> &results) { + int32_t packet_size; + message_type type; + bp.parse(packet_size); + bp.parse(type); + + if(type != message_type::ROBOT_MESSAGE) { + LOG_WARN("Invalid message type recieved: %u", static_cast(type)); + return false; + } + + uint64_t timestamp; + uint8_t source; + robot_message_type message_type; + + bp.parse(timestamp); + bp.parse(source); + bp.parse(message_type); + + std::unique_ptr result; + bool parsed = false; + + switch(message_type) { + case robot_message_type::ROBOT_MESSAGE_VERSION: { + VersionMessage *vm = new VersionMessage(timestamp, source); + parsed = vm->parse_with(bp); + result.reset(vm); + break; + } + + default: + return false; + } + + results.push_back(std::move(result)); + return parsed; + } +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/parser.h b/include/ur_modern_driver/ur/parser.h index 9cd80677f..00a0bac6f 100644 --- a/include/ur_modern_driver/ur/parser.h +++ b/include/ur_modern_driver/ur/parser.h @@ -1,88 +1,10 @@ #pragma once -#include "ur_modern_driver/log.h" -#include "ur_modern_driver/parser.h" -#include "ur_modern_driver/ur/state.h" -#include "ur_modern_driver/ur/rt_state.h" -#include "ur_modern_driver/ur/messages.h" +#include +#include "ur_modern_driver/pipeline.h" +#include "ur_modern_driver/bin_parser.h" template -class URStateParser : public Parser { - std::unique_ptr parse(BinParser &bp) { - int32_t packet_size; - message_type type; - bp.parse(packet_size); - bp.parse(type); - - if(type != message_type::ROBOT_STATE) { - LOG_ERROR("Invalid message type recieved: %u", static_cast(type)); - return std::unique_ptr(nullptr); - } - - std::unique_ptr obj(new T); - if(obj->parse_with(bp)) - return obj; - - return std::unique_ptr(nullptr); - } -}; - - -template -class URRTStateParser : public Parser { - std::unique_ptr parse(BinParser &bp) { - int32_t packet_size = bp.peek(); - - if(!bp.check_size(packet_size)) { - LOG_ERROR("Buffer len shorter than expected packet length"); - return std::unique_ptr(nullptr); - } - - bp.parse(packet_size); //consumes the peeked data - - std::unique_ptr obj(new T); - if(obj->parse_with(bp)) - return obj; - - return std::unique_ptr(nullptr); - } +class URParser { +public: + virtual bool parse(BinParser &bp, std::vector> &results) = 0; }; - -class URMessageParser : public Parser { - std::unique_ptr parse(BinParser &bp) { - int32_t packet_size = bp.peek(); - message_type type; - - if(!bp.check_size(packet_size)) { - LOG_ERROR("Buffer len shorter than expected packet length"); - return std::unique_ptr(nullptr); - } - - bp.parse(packet_size); //consumes the peeked data - bp.parse(type); - - if(type != message_type::ROBOT_MESSAGE) { - LOG_ERROR("Invalid message type recieved: %u", static_cast(type)); - return std::unique_ptr(nullptr); - } - - uint64_t timestamp; - uint8_t source; - robot_message_type message_type; - - bp.parse(timestamp); - bp.parse(source); - bp.parse(message_type); - - std::unique_ptr obj(nullptr); - - switch(message_type) { - case robot_message_type::ROBOT_MESSAGE_VERSION: - VersionMessage *vm = new VersionMessage(); - if(vm->parse_with(bp)) - obj.reset(vm); - break; - } - - return obj; - } -}; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/producer.h b/include/ur_modern_driver/ur/producer.h index b86b51907..35be085f3 100644 --- a/include/ur_modern_driver/ur/producer.h +++ b/include/ur_modern_driver/ur/producer.h @@ -1,21 +1,44 @@ #pragma once #include "ur_modern_driver/pipeline.h" #include "ur_modern_driver/ur/stream.h" -#include "ur_modern_driver/packet.h" -#include "ur_modern_driver/parser.h" +#include "ur_modern_driver/ur/parser.h" -class URProducer : public IProducer { +template +class URProducer : public IProducer { private: URStream &_stream; - Parser &_parser; + URParser &_parser; public: - URProducer(URStream &stream, Parser &parser) + URProducer(URStream &stream, URParser &parser) : _stream(stream), _parser(parser) { } - void setup_producer(); - void teardown_producer(); - void stop_producer(); - std::unique_ptr try_get(); + void setup_producer() { + _stream.connect(); + } + void teardown_producer() { + _stream.disconnect(); + } + void stop_producer() { + _stream.disconnect(); + } + + bool try_get(std::vector> &products) { + //4KB should be enough to hold any packet received from UR + uint8_t buf[4096]; + + //blocking call + ssize_t len = _stream.receive(buf, sizeof(buf)); + + //LOG_DEBUG("Read %d bytes from stream", len); + + if(len < 1) { + LOG_WARN("Read nothing from stream"); + return false; + } + + BinParser bp(buf, static_cast(len)); + return _parser.parse(bp, products); + } }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/robot_mode.h b/include/ur_modern_driver/ur/robot_mode.h index 13b195293..bce22af57 100644 --- a/include/ur_modern_driver/ur/robot_mode.h +++ b/include/ur_modern_driver/ur/robot_mode.h @@ -4,10 +4,11 @@ #include #include "ur_modern_driver/types.h" #include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/ur/state.h" class SharedRobotModeData { public: - bool parse_with(BinParser &bp); + virtual bool parse_with(BinParser &bp); uint64_t timestamp; bool physical_robot_connected; @@ -34,9 +35,11 @@ enum class robot_mode_V1_X : uint8_t { ROBOT_SAFEGUARD_STOP_MODE = 10 }; -class RobotModeData_V1_X : public SharedRobotModeData { +class RobotModeData_V1_X : public SharedRobotModeData, public StatePacket { public: - bool parse_with(BinParser &bp); + virtual bool parse_with(BinParser &bp); + virtual bool consume_with(URStatePacketConsumer &consumer); + bool security_stopped; robot_mode_V1_X robot_mode; @@ -69,9 +72,11 @@ enum class robot_control_mode_V3_X : uint8_t { TORQUE = 3 }; -class RobotModeData_V3_0__1 : public SharedRobotModeData { +class RobotModeData_V3_0__1 : public SharedRobotModeData, public StatePacket { public: - bool parse_with(BinParser &bp); + virtual bool parse_with(BinParser &bp); + virtual bool consume_with(URStatePacketConsumer &consumer); + bool protective_stopped; @@ -93,7 +98,9 @@ class RobotModeData_V3_0__1 : public SharedRobotModeData { class RobotModeData_V3_2 : public RobotModeData_V3_0__1 { public: - bool parse_with(BinParser &bp); + virtual bool parse_with(BinParser &bp); + virtual bool consume_with(URStatePacketConsumer &consumer); + double target_speed_fraction_limit; diff --git a/include/ur_modern_driver/ur/rt_parser.h b/include/ur_modern_driver/ur/rt_parser.h new file mode 100644 index 000000000..3a3831a5c --- /dev/null +++ b/include/ur_modern_driver/ur/rt_parser.h @@ -0,0 +1,36 @@ +#pragma once +#include +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/pipeline.h" +#include "ur_modern_driver/ur/parser.h" +#include "ur_modern_driver/ur/rt_state.h" + + +template +class URRTStateParser : public URParser { +public: + bool parse(BinParser &bp, std::vector> &results) { + int32_t packet_size = bp.peek(); + + if(!bp.check_size(packet_size)) { + LOG_ERROR("Buffer len shorter than expected packet length"); + return false; + } + + bp.parse(packet_size); //consumes the peeked data + + std::unique_ptr packet(new T); + if(!packet->parse_with(bp)) + return false; + + results.push_back(std::move(packet)); + + return true; + } +}; + +typedef URRTStateParser URRTStateParser_V1_6__7; +typedef URRTStateParser URRTStateParser_V1_8; +typedef URRTStateParser URRTStateParser_V3_0__1; +typedef URRTStateParser URRTStateParser_V3_2__3; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/rt_state.h b/include/ur_modern_driver/ur/rt_state.h index bc56aa919..c749ebbd8 100644 --- a/include/ur_modern_driver/ur/rt_state.h +++ b/include/ur_modern_driver/ur/rt_state.h @@ -4,11 +4,22 @@ #include #include "ur_modern_driver/types.h" #include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/pipeline.h" -class RTState_V1_6__7 { +class URRTPacketConsumer; + +class RTPacket { public: - bool parse_with(BinParser &bp); + 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); + +public: double time; double q_target[6]; double qd_target[6]; @@ -18,21 +29,37 @@ class RTState_V1_6__7 { double q_actual[6]; double qd_actual[6]; double i_actual[6]; - double3_t tool_accelerometer_values; + + //gap here depending on version + double tcp_force[6]; - cartesian_coord_t tool_vector; - double tcp_speed[6]; + cartesian_coord_t tool_vector_actual; + cartesian_coord_t tcp_speed_actual; + + //gap here depending on version + uint64_t digital_input; double motor_temperatures[6]; double controller_time; double robot_mode; + static const size_t SIZE = sizeof(double) * 3 - + sizeof(double[6]) * 11 - + sizeof(double3_t) - + sizeof(cartesian_coord_t) + + 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); + + double3_t tool_accelerometer_values; + + static const size_t SIZE = RTShared::SIZE + + sizeof(double3_t); static_assert(RTState_V1_6__7::SIZE == 632, "RTState_V1_6__7 has mismatched size!"); }; @@ -40,6 +67,7 @@ class RTState_V1_6__7 { class RTState_V1_8 : public RTState_V1_6__7 { public: bool parse_with(BinParser &bp); + virtual bool consume_with(URRTPacketConsumer &consumer); double joint_modes[6]; @@ -49,10 +77,47 @@ class RTState_V1_8 : public RTState_V1_6__7 { static_assert(RTState_V1_8::SIZE == 680, "RTState_V1_8 has mismatched size!"); }; -class RTState_V3_0__1 { +class RTState_V3_0__1 : public RTShared, public RTPacket { public: - //bool parse_with(BinParser &bp); + bool parse_with(BinParser &bp); + virtual bool consume_with(URRTPacketConsumer &consumer); - double m_actual[6]; 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; + double speed_scaling; + double linear_momentum_norm; + double v_main; + double v_robot; + double i_robot; + double v_actual[6]; + + + static const size_t SIZE = RTShared::SIZE + + sizeof(double[6]) * 3 + + sizeof(double3_t) + + sizeof(cartesian_coord_t) * 2 + + sizeof(double) * 6; + + static_assert(RTState_V3_0__1::SIZE == 920, "RTState_V3_0__1 has mismatched size!"); +}; + +class RTState_V3_2__3 : public RTState_V3_0__1 { +public: + bool parse_with(BinParser &bp); + virtual bool consume_with(URRTPacketConsumer &consumer); + + uint64_t digital_outputs; + double program_state; + + static const size_t SIZE = RTState_V3_0__1::SIZE + + sizeof(uint64_t) + + sizeof(double); + + static_assert(RTState_V3_2__3::SIZE == 936, "RTState_V3_2__3 has mismatched size!"); }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/state.h b/include/ur_modern_driver/ur/state.h index 085d42f95..d37cbf195 100644 --- a/include/ur_modern_driver/ur/state.h +++ b/include/ur_modern_driver/ur/state.h @@ -1,12 +1,10 @@ #pragma once + #include #include +#include "ur_modern_driver/pipeline.h" #include "ur_modern_driver/bin_parser.h" -#include "ur_modern_driver/packet.h" -#include "ur_modern_driver/ur/master_board.h" -#include "ur_modern_driver/ur/robot_mode.h" - - +#include "ur_modern_driver/log.h" enum class package_type : uint8_t { ROBOT_MODE_DATA = 0, @@ -27,68 +25,10 @@ enum class message_type : uint8_t { PROGRAM_STATE_MESSAGE = 25 }; +class URStatePacketConsumer; -class RobotState : public Packet { -public: - bool parse_with(BinParser &bp); -protected: - virtual bool parse_package(BinParser &bp) = 0; -}; - -class RobotState_V1_6__7 : public RobotState { -protected: - bool parse_package(BinParser &bp); -public: - RobotModeData_V1_X robot_mode; - //JointData - //ToolData - MasterBoardData_V1_X master_board; - //CartesianInfo -}; - -class RobotState_V1_8 : public RobotState_V1_6__7 { -protected: - bool parse_package(BinParser &bp); -public: - - //KinematicsInfo - //ConfigurationData - //ForceModeData - //AdditionalInfo - //CalibrationData -}; - - -class RobotState_V3_0__1 : public RobotState { -protected: - bool parse_package(BinParser &bp); +class StatePacket { public: - RobotModeData_V3_0__1 robot_mode; - //JointData - //ToolData - MasterBoardData_V3_0__1 master_board; - //CartesianInfo - - //KinematicsInfo - //ConfigurationData - //ForceModeData - //AdditionalInfo - //CalibrationData -}; - -class RobotState_V3_2 : public RobotState { -protected: - bool parse_package(BinParser &bp); -public: - RobotModeData_V3_2 robot_mode; - //JointData - //ToolData - MasterBoardData_V3_2 master_board; - //CartesianInfo - - //KinematicsInfo - //ConfigurationData - //ForceModeData - //AdditionalInfo - //CalibrationData + virtual bool parse_with(BinParser &bp) = 0; + virtual bool consume_with(URStatePacketConsumer &consumer) = 0; }; diff --git a/include/ur_modern_driver/ur/state_parser.h b/include/ur_modern_driver/ur/state_parser.h new file mode 100644 index 000000000..df5a7d60a --- /dev/null +++ b/include/ur_modern_driver/ur/state_parser.h @@ -0,0 +1,82 @@ +#pragma once +#include +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/bin_parser.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/robot_mode.h" + + +template +class URStateParser : public URParser { +private: + StatePacket* from_type(package_type type) { + switch(type) { + case package_type::ROBOT_MODE_DATA: + return new RMD; + case package_type::MASTERBOARD_DATA: + return new MBD; + default: + return nullptr; + } + } + +public: + bool parse(BinParser &bp, std::vector> &results) { + int32_t packet_size; + message_type type; + bp.parse(packet_size); + bp.parse(type); + + if(type != message_type::ROBOT_STATE) { + LOG_WARN("Invalid message type recieved: %u", static_cast(type)); + return false; + } + + while(!bp.empty()) { + if(!bp.check_size(sizeof(uint32_t))){ + LOG_ERROR("Failed to read sub-package length, there's likely a parsing error"); + return false; + } + uint32_t sub_size = bp.peek(); + if(!bp.check_size(static_cast(sub_size))) { + LOG_WARN("Invalid sub-package size of %" PRIu32 " received!", sub_size); + return false; + } + + //deconstruction of a sub parser will increment the position of the parent parser + BinParser sbp(bp, sub_size); + sbp.consume(sizeof(sub_size)); + package_type type; + sbp.parse(type); + + std::unique_ptr packet(from_type(type)); + + if(packet == nullptr) { + sbp.consume(); + LOG_INFO("Skipping sub-packet of type %d", type); + continue; + } + + if(!packet->parse_with(sbp)) { + LOG_ERROR("Sub-package parsing of type %d failed!", type); + return false; + } + + results.push_back(std::move(packet)); + + if(!sbp.empty()) { + LOG_ERROR("Sub-package was not parsed completely!"); + return false; + } + } + + return true; + } +}; + +typedef URStateParser URStateParser_V1_X; +typedef URStateParser URStateParser_V3_0__1; +typedef URStateParser URStateParser_V3_2; \ No newline at end of file diff --git a/src/ur/master_board.cpp b/src/ur/master_board.cpp index 9c0f8655d..c8d947b49 100644 --- a/src/ur/master_board.cpp +++ b/src/ur/master_board.cpp @@ -1,4 +1,5 @@ #include "ur_modern_driver/ur/master_board.h" +#include "ur_modern_driver/ur/consumer.h" bool SharedMasterBoardData::parse_with(BinParser &bp) { bp.parse(analog_input_range0); @@ -80,3 +81,16 @@ bool MasterBoardData_V3_2::parse_with(BinParser &bp) { return true; } + + + + +bool MasterBoardData_V1_X::consume_with(URStatePacketConsumer &consumer) { + return consumer.consume(*this); +} +bool MasterBoardData_V3_0__1::consume_with(URStatePacketConsumer &consumer) { + return consumer.consume(*this); +} +bool MasterBoardData_V3_2::consume_with(URStatePacketConsumer &consumer) { + return consumer.consume(*this); +} \ No newline at end of file diff --git a/src/ur/messages.cpp b/src/ur/messages.cpp index 42ceb8ab0..e079f8d7c 100644 --- a/src/ur/messages.cpp +++ b/src/ur/messages.cpp @@ -1,11 +1,5 @@ #include "ur_modern_driver/ur/messages.h" - -bool MessageBase::parse_with(BinParser &bp) { - bp.parse(timestamp); - bp.parse(source); - - return true; //not really possible to check dynamic size packets -} +#include "ur_modern_driver/ur/consumer.h" bool VersionMessage::parse_with(BinParser &bp) { @@ -18,4 +12,9 @@ bool VersionMessage::parse_with(BinParser &bp) { bp.parse_remainder(build_date); return true; //not really possible to check dynamic size packets +} + + +bool VersionMessage::consume_with(URMessagePacketConsumer &consumer) { + return consumer.consume(*this); } \ No newline at end of file diff --git a/src/ur/producer.cpp b/src/ur/producer.cpp deleted file mode 100644 index 87fb6ed75..000000000 --- a/src/ur/producer.cpp +++ /dev/null @@ -1,32 +0,0 @@ -#include "ur_modern_driver/ur/producer.h" -#include "ur_modern_driver/log.h" - -void URProducer::setup_producer() { - _stream.connect(); -} - -void URProducer::teardown_producer() { - -} - -void URProducer::stop_producer() { - _stream.disconnect(); -} - -std::unique_ptr URProducer::try_get() { - //4KB should be enough to hold any packet received from UR - uint8_t buf[4096]; - - //blocking call - ssize_t len = _stream.receive(buf, sizeof(buf)); - - LOG_DEBUG("Read %d bytes from stream", len); - - if(len < 1) { - LOG_WARN("Read nothing from stream"); - return std::unique_ptr(nullptr); - } - - BinParser bp(buf, static_cast(len)); - return std::move(_parser.parse(bp)); -} \ No newline at end of file diff --git a/src/ur/robot_mode.cpp b/src/ur/robot_mode.cpp index 4fea01d83..1c6240633 100644 --- a/src/ur/robot_mode.cpp +++ b/src/ur/robot_mode.cpp @@ -1,4 +1,5 @@ #include "ur_modern_driver/ur/robot_mode.h" +#include "ur_modern_driver/ur/consumer.h" bool SharedRobotModeData::parse_with(BinParser &bp) { @@ -54,3 +55,16 @@ bool RobotModeData_V3_2::parse_with(BinParser &bp) { return true; } + + + + +bool RobotModeData_V1_X::consume_with(URStatePacketConsumer &consumer) { + return consumer.consume(*this); +} +bool RobotModeData_V3_0__1::consume_with(URStatePacketConsumer &consumer) { + return consumer.consume(*this); +} +bool RobotModeData_V3_2::consume_with(URStatePacketConsumer &consumer) { + return consumer.consume(*this); +} \ No newline at end of file diff --git a/src/ur/rt_state.cpp b/src/ur/rt_state.cpp index aad336d36..596134dec 100644 --- a/src/ur/rt_state.cpp +++ b/src/ur/rt_state.cpp @@ -1,9 +1,8 @@ #include "ur_modern_driver/ur/rt_state.h" +#include "ur_modern_driver/ur/consumer.h" -bool RTState_V1_6__7::parse_with(BinParser &bp) { - if(!bp.check_size()) - return false; +bool RTShared::parse_shared1(BinParser &bp) { bp.parse(time); bp.parse(q_target); bp.parse(qd_target); @@ -13,14 +12,30 @@ bool RTState_V1_6__7::parse_with(BinParser &bp) { bp.parse(q_actual); bp.parse(qd_actual); bp.parse(i_actual); - bp.parse(tool_accelerometer_values); - bp.parse(tcp_force); - bp.parse(tool_vector); - bp.parse(tcp_speed); + return true; +} + +bool RTShared::parse_shared2(BinParser &bp) { bp.parse(digital_input); bp.parse(motor_temperatures); bp.parse(controller_time); bp.parse(robot_mode); + return true; +} + + +bool RTState_V1_6__7::parse_with(BinParser &bp) { + if(!bp.check_size()) + return false; + + parse_shared1(bp); + + bp.parse(tool_accelerometer_values); + bp.parse(tcp_force); + bp.parse(tool_vector_actual); + bp.parse(tcp_speed_actual); + + parse_shared2(bp); return true; } @@ -34,4 +49,61 @@ bool RTState_V1_8::parse_with(BinParser &bp) { bp.parse(joint_modes); return true; +} + +bool RTState_V3_0__1::parse_with(BinParser &bp) { + if(!bp.check_size()) + return false; + + parse_shared1(bp); + + bp.parse(i_control); + bp.parse(tool_vector_actual); + bp.parse(tcp_speed_actual); + bp.parse(tcp_force); + bp.parse(tool_vector_target); + bp.parse(tcp_speed_target); + + parse_shared2(bp); + + bp.parse(joint_modes); + bp.parse(safety_mode); + bp.consume(sizeof(double[6])); //skip undocumented + bp.parse(tool_accelerometer_values); + bp.consume(sizeof(double[6])); //skip undocumented + bp.parse(speed_scaling); + bp.parse(linear_momentum_norm); + bp.consume(sizeof(double)); //skip undocumented + bp.consume(sizeof(double)); //skip undocumented + bp.parse(v_main); + bp.parse(v_robot); + bp.parse(i_robot); + bp.parse(v_actual); + + return true; +} + +bool RTState_V3_2__3::parse_with(BinParser &bp) { + if(!bp.check_size()) + return false; + + RTState_V3_0__1::parse_with(bp); + + bp.parse(digital_outputs); + bp.parse(program_state); + + return true; +} + +bool RTState_V1_6__7::consume_with(URRTPacketConsumer &consumer) { + return consumer.consume(*this); +} +bool RTState_V1_8::consume_with(URRTPacketConsumer &consumer) { + return consumer.consume(*this); +} +bool RTState_V3_0__1::consume_with(URRTPacketConsumer &consumer) { + return consumer.consume(*this); +} +bool RTState_V3_2__3::consume_with(URRTPacketConsumer &consumer) { + return consumer.consume(*this); } \ No newline at end of file diff --git a/src/ur/state.cpp b/src/ur/state.cpp index 851c906b0..fe5af1b83 100644 --- a/src/ur/state.cpp +++ b/src/ur/state.cpp @@ -2,6 +2,10 @@ #include "ur_modern_driver/ur/state.h" + +//StatePacket::~StatePacket() { } + +/* bool RobotState::parse_with(BinParser &bp) { //continue as long as there are bytes to read while(!bp.empty()) { @@ -109,4 +113,5 @@ bool RobotState_V3_0__1::parse_package(BinParser &bp) { bool RobotState_V3_2::parse_package(BinParser &bp) { return parse_advanced(bp, *this); -} \ No newline at end of file +} +*/ \ No newline at end of file From c824c373038d4fab87b4cb5c5720adaf4d227a78 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Thu, 16 Feb 2017 01:52:59 +0100 Subject: [PATCH 016/114] Implemented RT publishing --- include/ur_modern_driver/ros/converter.h | 2 + include/ur_modern_driver/ros/rt_publisher.h | 59 ++++++++++++++++ src/ros/rt_publisher.cpp | 74 +++++++++++++++++++++ 3 files changed, 135 insertions(+) create mode 100644 include/ur_modern_driver/ros/converter.h create mode 100644 include/ur_modern_driver/ros/rt_publisher.h create mode 100644 src/ros/rt_publisher.cpp diff --git a/include/ur_modern_driver/ros/converter.h b/include/ur_modern_driver/ros/converter.h new file mode 100644 index 000000000..3f59c932d --- /dev/null +++ b/include/ur_modern_driver/ros/converter.h @@ -0,0 +1,2 @@ +#pragma once + diff --git a/include/ur_modern_driver/ros/rt_publisher.h b/include/ur_modern_driver/ros/rt_publisher.h new file mode 100644 index 000000000..3289174d5 --- /dev/null +++ b/include/ur_modern_driver/ros/rt_publisher.h @@ -0,0 +1,59 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "ur_modern_driver/ur/consumer.h" + +using namespace ros; + +const std::string JOINTS[] = { + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint" +}; + +class RTPublisher : public URRTPacketConsumer { +private: + NodeHandle _nh; + Publisher _joint_pub; + Publisher _wrench_pub; + Publisher _tool_vel_pub; + std::vector _joint_names; + std::string _base_frame; + + bool publish_joints(RTShared &packet, ros::Time &t); + bool publish_wrench(RTShared &packet, ros::Time &t); + bool publish_tool(RTShared &packet, ros::Time &t); + + bool publish(RTShared &packet); + +public: + RTPublisher(std::string &joint_prefix, std::string &base_frame) : + _joint_pub(_nh.advertise("joint_states", 1)), + _wrench_pub(_nh.advertise("wrench", 1)), + _tool_vel_pub(_nh.advertise("tool_velocity", 1)), + _base_frame(base_frame) + { + 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 void setup_consumer() { } + virtual void teardown_consumer() { } + virtual void stop_consumer() { } +}; \ No newline at end of file diff --git a/src/ros/rt_publisher.cpp b/src/ros/rt_publisher.cpp new file mode 100644 index 000000000..c0db69060 --- /dev/null +++ b/src/ros/rt_publisher.cpp @@ -0,0 +1,74 @@ +#include "ur_modern_driver/ros/rt_publisher.h" + +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) { + joint_msg.position.push_back(q); + } + for(auto const& qd : packet.qd_actual) { + joint_msg.velocity.push_back(qd); + } + for(auto const& i : packet.i_actual) { + joint_msg.effort.push_back(i); + } + + _joint_pub.publish(joint_msg); + + return true; +} + +bool RTPublisher::publish_wrench(RTShared &packet, ros::Time &t) { + geometry_msgs::WrenchStamped wrench_msg; + wrench_msg.header.stamp = t; + + wrench_msg.wrench.force.x = packet.tcp_force[0]; + wrench_msg.wrench.force.y = packet.tcp_force[1]; + wrench_msg.wrench.force.z = packet.tcp_force[2]; + wrench_msg.wrench.torque.x = packet.tcp_force[3]; + wrench_msg.wrench.torque.y = packet.tcp_force[4]; + wrench_msg.wrench.torque.z = packet.tcp_force[5]; + + _wrench_pub.publish(wrench_msg); + + return true; +} + +bool RTPublisher::publish_tool(RTShared &packet, ros::Time &t) { + geometry_msgs::TwistStamped tool_twist; + + tool_twist.header.stamp = t; + + tool_twist.header.frame_id = _base_frame; + + tool_twist.twist.linear.x = packet.tcp_speed_actual.position.x; + tool_twist.twist.linear.y = packet.tcp_speed_actual.position.y; + tool_twist.twist.linear.z = packet.tcp_speed_actual.position.z; + tool_twist.twist.angular.x = packet.tcp_speed_actual.rotation.x; + tool_twist.twist.angular.y = packet.tcp_speed_actual.rotation.y; + tool_twist.twist.angular.z = packet.tcp_speed_actual.rotation.z; + + _tool_vel_pub.publish(tool_twist); + + return true; +} + +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) { + return publish(state); +} +bool RTPublisher::consume(RTState_V1_8 &state) { + return publish(state); +} +bool RTPublisher::consume(RTState_V3_0__1 &state) { + return publish(state); +} +bool RTPublisher::consume(RTState_V3_2__3 &state) { + return publish(state); +} \ No newline at end of file From e00cfac0ee8feab406283baa9a5be031bec360f5 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Thu, 16 Feb 2017 01:54:21 +0100 Subject: [PATCH 017/114] new entry point WIP --- src/ros_main.cpp | 94 ++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 94 insertions(+) create mode 100644 src/ros_main.cpp diff --git a/src/ros_main.cpp b/src/ros_main.cpp new file mode 100644 index 000000000..c2dc749a6 --- /dev/null +++ b/src/ros_main.cpp @@ -0,0 +1,94 @@ +#include +#include +#include +#include +#include + +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/pipeline.h" +#include "ur_modern_driver/ur/producer.h" +#include "ur_modern_driver/ur/state.h" +#include "ur_modern_driver/ur/rt_state.h" +#include "ur_modern_driver/ur/parser.h" +#include "ur_modern_driver/ur/messages.h" +#include "ur_modern_driver/ur/factory.h" +#include "ur_modern_driver/ros/rt_publisher.h" + +static const std::string IP_ADDR_ARG("~robot_ip_address"); +static const std::string REVERSE_PORT_ARG("~reverse_port"); +static const std::string SIM_TIME_ARG("~use_sim_time"); +static const std::string PREFIX_ARG("~prefix"); +static const std::string BASE_FRAME_ARG("~base_frame"); +static const std::string TOOL_FRAME_ARG("~tool_frame"); + + +struct ProgArgs { +public: + std::string host; + std::string prefix; + std::string base_frame; + std::string tool_frame; + int32_t reverse_port; + bool use_sim_time; +}; + +bool parse_args(ProgArgs &args) { + if(!ros::param::get(IP_ADDR_ARG, args.host)) { + LOG_ERROR("robot_ip_address parameter must be set!"); + return false; + } + ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001)); + ros::param::param(SIM_TIME_ARG, args.use_sim_time, false); + ros::param::param(PREFIX_ARG, args.prefix, std::string()); + ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link"); + ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller"); + return true; +} + + +int main(int argc, char **argv) { + + ros::init(argc, argv, "ur_driver"); + + ProgArgs args; + if(!parse_args(args)) { + return EXIT_FAILURE; + } + + URFactory factory(args.host); + + auto parser = factory.get_rt_parser(); + + URStream s2(args.host, 30003); + URProducer p2(s2, *parser); + + /* + p2.setup_producer(); + + while(true) { + std::vector>> products; + p2.try_get(products); + for(auto const& p : products) { + LOG_INFO("Got packet! %x", p.get()); + } + products.clear(); + } + + p2.teardown_producer(); + */ + + RTPublisher pub(args.prefix, args.base_frame); + + Pipeline pl(p2, pub); + + pl.run(); + + while(ros::ok()) { + std::this_thread::sleep_for(std::chrono::milliseconds(33)); + } + + pl.stop(); + + + return EXIT_SUCCESS; +} \ No newline at end of file From a78d3eadf3b0c07fc4b94a774005230ce3da2a3c Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Thu, 16 Feb 2017 02:03:40 +0100 Subject: [PATCH 018/114] Added clang formatting --- .clang-format | 95 ++ include/ur_modern_driver/bin_parser.h | 137 +- include/ur_modern_driver/do_output.h | 1 - include/ur_modern_driver/log.h | 22 +- include/ur_modern_driver/packet.h | 2 +- include/ur_modern_driver/parser.h | 2 +- include/ur_modern_driver/pipeline.h | 80 +- include/ur_modern_driver/queue/atomicops.h | 962 ++++++----- .../queue/readerwriterqueue.h | 1418 ++++++++-------- include/ur_modern_driver/robot_state.h | 290 ++-- include/ur_modern_driver/robot_state_RT.h | 156 +- include/ur_modern_driver/ros/converter.h | 1 - include/ur_modern_driver/ros/rt_publisher.h | 44 +- include/ur_modern_driver/ur/consumer.h | 39 +- include/ur_modern_driver/ur/factory.h | 70 +- include/ur_modern_driver/ur/master_board.h | 35 +- include/ur_modern_driver/ur/messages.h | 44 +- include/ur_modern_driver/ur/messages_parser.h | 29 +- include/ur_modern_driver/ur/parser.h | 6 +- include/ur_modern_driver/ur/producer.h | 32 +- include/ur_modern_driver/ur/robot_mode.h | 73 +- include/ur_modern_driver/ur/rt_parser.h | 18 +- include/ur_modern_driver/ur/rt_state.h | 42 +- include/ur_modern_driver/ur/state.h | 34 +- include/ur_modern_driver/ur/state_parser.h | 57 +- include/ur_modern_driver/ur/stream.h | 29 +- include/ur_modern_driver/ur_communication.h | 54 +- include/ur_modern_driver/ur_driver.h | 116 +- .../ur_modern_driver/ur_hardware_interface.h | 101 +- .../ur_realtime_communication.h | 78 +- src/do_output.cpp | 39 +- src/robot_state.cpp | 671 ++++---- src/robot_state_RT.cpp | 824 ++++----- src/ros/rt_publisher.cpp | 32 +- src/ros_main.cpp | 89 +- src/ur/master_board.cpp | 47 +- src/ur/messages.cpp | 10 +- src/ur/robot_mode.cpp | 35 +- src/ur/rt_state.cpp | 44 +- src/ur/state.cpp | 4 +- src/ur/stream.cpp | 68 +- src/ur_communication.cpp | 300 ++-- src/ur_driver.cpp | 668 ++++---- src/ur_hardware_interface.cpp | 389 ++--- src/ur_realtime_communication.cpp | 313 ++-- src/ur_ros_wrapper.cpp | 1480 ++++++++--------- 46 files changed, 4672 insertions(+), 4408 deletions(-) create mode 100644 .clang-format diff --git a/.clang-format b/.clang-format new file mode 100644 index 000000000..95f7eb56b --- /dev/null +++ b/.clang-format @@ -0,0 +1,95 @@ +--- +Language: Cpp +# BasedOnStyle: WebKit +AccessModifierOffset: -4 +AlignAfterOpenBracket: DontAlign +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlinesLeft: false +AlignOperands: false +AlignTrailingComments: false +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: All +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakTemplateDeclarations: false +BinPackArguments: true +BinPackParameters: true +BraceWrapping: + AfterClass: false + AfterControlStatement: false + AfterEnum: false + AfterFunction: true + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + BeforeCatch: false + BeforeElse: false + IndentBraces: false +BreakBeforeBinaryOperators: All +BreakBeforeBraces: WebKit +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: true +BreakAfterJavaFieldAnnotations: false +BreakStringLiterals: true +ColumnLimit: 0 +CommentPragmas: '^ IWYU pragma:' +ConstructorInitializerAllOnOneLineOrOnePerLine: false +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: false +DerivePointerAlignment: false +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] +IncludeCategories: + - Regex: '^"(llvm|llvm-c|clang|clang-c)/' + Priority: 2 + - Regex: '^(<|"(gtest|isl|json)/)' + Priority: 3 + - Regex: '.*' + Priority: 1 +IncludeIsMainRegex: '$' +IndentCaseLabels: false +IndentWidth: 4 +IndentWrappedFunctionNames: false +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: true +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: Inner +ObjCBlockIndentWidth: 4 +ObjCSpaceAfterProperty: true +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 60 +PointerAlignment: Left +ReflowComments: true +SortIncludes: true +SpaceAfterCStyleCast: false +SpaceBeforeAssignmentOperators: true +SpaceBeforeParens: ControlStatements +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 1 +SpacesInAngles: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Cpp03 +TabWidth: 8 +UseTab: Never +... + diff --git a/include/ur_modern_driver/bin_parser.h b/include/ur_modern_driver/bin_parser.h index c7f5d7cdf..12d49cbbc 100644 --- a/include/ur_modern_driver/bin_parser.h +++ b/include/ur_modern_driver/bin_parser.h @@ -1,146 +1,171 @@ #pragma once -#include -#include +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/types.h" +#include #include #include +#include +#include #include -#include -#include "ur_modern_driver/types.h" -#include "ur_modern_driver/log.h" class BinParser { private: uint8_t *_buf_pos, *_buf_end; - BinParser &_parent; + BinParser& _parent; public: - BinParser(uint8_t *buffer, size_t buf_len) : - _buf_pos(buffer), - _buf_end(buffer+buf_len), - _parent(*this) { - assert(_buf_pos <= _buf_end); - } - - BinParser(BinParser &parent, size_t sub_len) : - _buf_pos(parent._buf_pos), - _buf_end(parent._buf_pos+sub_len), - _parent(parent) { - assert(_buf_pos <= _buf_end); - } + BinParser(uint8_t* buffer, size_t buf_len) + : _buf_pos(buffer) + , _buf_end(buffer + buf_len) + , _parent(*this) + { + assert(_buf_pos <= _buf_end); + } - ~BinParser() { - _parent._buf_pos = _buf_pos; - } + BinParser(BinParser& parent, size_t sub_len) + : _buf_pos(parent._buf_pos) + , _buf_end(parent._buf_pos + sub_len) + , _parent(parent) + { + assert(_buf_pos <= _buf_end); + } + ~BinParser() + { + _parent._buf_pos = _buf_pos; + } //Decode from network encoding (big endian) to host encoding - template - T decode(T val) { + template + T decode(T val) + { return val; } - uint16_t decode(uint16_t val) { + uint16_t decode(uint16_t val) + { return be16toh(val); } - uint32_t decode(uint32_t val) { + uint32_t decode(uint32_t val) + { return be32toh(val); } - uint64_t decode(uint64_t val) { + uint64_t decode(uint64_t val) + { return be64toh(val); } - int16_t decode(int16_t val) { + int16_t decode(int16_t val) + { return be16toh(val); } - int32_t decode(int32_t val) { + int32_t decode(int32_t val) + { return be32toh(val); } - int64_t decode(int64_t val) { + int64_t decode(int64_t val) + { return be64toh(val); } - float decode(float val) { + float decode(float val) + { return be32toh(val); } - double decode(double val) { + double decode(double val) + { return be64toh(val); } - - template - T peek() { + template + T peek() + { assert(_buf_pos <= _buf_end); return decode(*(reinterpret_cast(_buf_pos))); } - template - void parse(T &val) { + template + void parse(T& val) + { val = peek(); _buf_pos += sizeof(T); } - // UR uses 1 byte for boolean values but sizeof(bool) is implementation + // UR uses 1 byte for boolean values but sizeof(bool) is implementation // defined so we must ensure they're parsed as uint8_t on all compilers - void parse(bool &val) { + void parse(bool& val) + { uint8_t inner; parse(inner); - val = inner != 0; + val = inner != 0; } // Explicit parsing order of fields to avoid issues with struct layout - void parse(double3_t &val) { + void parse(double3_t& val) + { parse(val.x); parse(val.y); parse(val.z); } // Explicit parsing order of fields to avoid issues with struct layout - void parse(cartesian_coord_t &val) { + void parse(cartesian_coord_t& val) + { parse(val.position); parse(val.rotation); } - void parse_remainder(std::string &val) { + void parse_remainder(std::string& val) + { parse(val, size_t(_buf_end - _buf_pos)); } - void parse(std::string &val, size_t len) { + void parse(std::string& val, size_t len) + { val.assign(reinterpret_cast(_buf_pos), len); _buf_pos += len; } - // Special string parse function that assumes uint8_t len followed by chars - void parse(std::string &val) { + // Special string parse function that assumes uint8_t len followed by chars + void parse(std::string& val) + { uint8_t len; parse(len); parse(val, size_t(len)); } - template - void parse(T (&array)[N]) { - for(size_t i = 0; i < N; i++) { + template + void parse(T (&array)[N]) + { + for (size_t i = 0; i < N; i++) { parse(array[i]); } } - void consume() { + void consume() + { _buf_pos = _buf_end; } - void consume(size_t bytes) { + void consume(size_t bytes) + { _buf_pos += bytes; } - bool check_size(size_t bytes) { + bool check_size(size_t bytes) + { return bytes <= size_t(_buf_end - _buf_pos); } - template - bool check_size(void) { + template + bool check_size(void) + { return check_size(T::SIZE); } - bool empty() { + bool empty() + { return _buf_pos == _buf_end; } - void debug() { + void debug() + { LOG_DEBUG("BinParser: %p - %p (%zu bytes)", _buf_pos, _buf_end, _buf_end - _buf_pos); } }; diff --git a/include/ur_modern_driver/do_output.h b/include/ur_modern_driver/do_output.h index d52420e11..44c1e63bf 100644 --- a/include/ur_modern_driver/do_output.h +++ b/include/ur_modern_driver/do_output.h @@ -30,5 +30,4 @@ void print_warning(std::string inp); void print_error(std::string inp); void print_fatal(std::string inp); - #endif /* UR_DO_OUTPUT_H_ */ diff --git a/include/ur_modern_driver/log.h b/include/ur_modern_driver/log.h index db0f5b698..b66267d44 100644 --- a/include/ur_modern_driver/log.h +++ b/include/ur_modern_driver/log.h @@ -2,20 +2,20 @@ #include #ifdef ROS_BUILD - #include +#include - #define LOG_DEBUG ROS_DEBUG - #define LOG_WARN ROS_WARN - #define LOG_INFO ROS_INFO - #define LOG_ERROR ROS_ERROR - #define LOG_FATAL ROS_FATAL +#define LOG_DEBUG ROS_DEBUG +#define LOG_WARN ROS_WARN +#define LOG_INFO ROS_INFO +#define LOG_ERROR ROS_ERROR +#define LOG_FATAL ROS_FATAL #else - #define LOG_DEBUG(format, ...) printf("[DEBUG]: " format "\n", ##__VA_ARGS__) - #define LOG_WARN(format, ...) printf("[WARNING]: " format "\n", ##__VA_ARGS__) - #define LOG_INFO(format, ...) printf("[INFO]: " format "\n", ##__VA_ARGS__) - #define LOG_ERROR(format, ...) printf("[ERROR]: " format "\n", ##__VA_ARGS__) - #define LOG_FATAL(format, ...) printf("[FATAL]: " format "\n", ##__VA_ARGS__) +#define LOG_DEBUG(format, ...) printf("[DEBUG]: " format "\n", ##__VA_ARGS__) +#define LOG_WARN(format, ...) printf("[WARNING]: " format "\n", ##__VA_ARGS__) +#define LOG_INFO(format, ...) printf("[INFO]: " format "\n", ##__VA_ARGS__) +#define LOG_ERROR(format, ...) printf("[ERROR]: " format "\n", ##__VA_ARGS__) +#define LOG_FATAL(format, ...) printf("[FATAL]: " format "\n", ##__VA_ARGS__) #endif \ No newline at end of file diff --git a/include/ur_modern_driver/packet.h b/include/ur_modern_driver/packet.h index 7f316dc4b..541e3e5ae 100644 --- a/include/ur_modern_driver/packet.h +++ b/include/ur_modern_driver/packet.h @@ -3,5 +3,5 @@ class Packet { public: - virtual bool parse_with(BinParser &bp) = 0; + virtual bool parse_with(BinParser& bp) = 0; }; \ No newline at end of file diff --git a/include/ur_modern_driver/parser.h b/include/ur_modern_driver/parser.h index d7a122270..99975156b 100644 --- a/include/ur_modern_driver/parser.h +++ b/include/ur_modern_driver/parser.h @@ -4,5 +4,5 @@ class Parser { public: - virtual std::unique_ptr parse(BinParser &bp) = 0; + virtual std::unique_ptr parse(BinParser& bp) = 0; }; diff --git a/include/ur_modern_driver/pipeline.h b/include/ur_modern_driver/pipeline.h index b718f6239..13a9b1bcd 100644 --- a/include/ur_modern_driver/pipeline.h +++ b/include/ur_modern_driver/pipeline.h @@ -1,21 +1,20 @@ #pragma once -#include -#include -#include #include "ur_modern_driver/log.h" #include "ur_modern_driver/queue/readerwriterqueue.h" +#include +#include +#include -using namespace moodycamel; +using namespace moodycamel; using namespace std; - template class IConsumer { public: - virtual void setup_consumer() { } - virtual void teardown_consumer() { } - virtual void stop_consumer() { } + virtual void setup_consumer() {} + virtual void teardown_consumer() {} + virtual void stop_consumer() {} virtual bool consume(unique_ptr product) = 0; }; @@ -23,33 +22,33 @@ class IConsumer { template class IProducer { public: - virtual void setup_producer() { } - virtual void teardown_producer() { } - virtual void stop_producer() { } + virtual void setup_producer() {} + virtual void teardown_producer() {} + virtual void stop_producer() {} - virtual bool try_get(std::vector> &products) = 0; + virtual bool try_get(std::vector >& products) = 0; }; - template class Pipeline { private: - IProducer &_producer; - IConsumer &_consumer; - BlockingReaderWriterQueue> _queue; + IProducer& _producer; + IConsumer& _consumer; + BlockingReaderWriterQueue > _queue; atomic _running; thread _pThread, _cThread; - void run_producer() { + void run_producer() + { _producer.setup_producer(); - std::vector> products; - while(_running) { - if(!_producer.try_get(products)) { + std::vector > products; + while (_running) { + if (!_producer.try_get(products)) { break; } - - for(auto &p : products) { - if(!_queue.try_enqueue(std::move(p))) { + + for (auto& p : products) { + if (!_queue.try_enqueue(std::move(p))) { LOG_WARN("Pipeline owerflowed!"); } } @@ -60,27 +59,31 @@ class Pipeline { //todo cleanup } - void run_consumer() { + void run_consumer() + { _consumer.setup_consumer(); unique_ptr product; - while(_running) { + while (_running) { _queue.wait_dequeue(product); - if(!_consumer.consume(std::move(product))) + if (!_consumer.consume(std::move(product))) break; } _consumer.teardown_consumer(); //todo cleanup } + public: - Pipeline(IProducer &producer, IConsumer &consumer) - : _producer(producer), - _consumer(consumer), - _queue{32}, - _running{false} - { } - - void run() { - if(_running) + Pipeline(IProducer& producer, IConsumer& consumer) + : _producer(producer) + , _consumer(consumer) + , _queue{ 32 } + , _running{ false } + { + } + + void run() + { + if (_running) return; _running = true; @@ -88,10 +91,11 @@ class Pipeline { _cThread = thread(&Pipeline::run_consumer, this); } - void stop() { - if(!_running) + void stop() + { + if (!_running) return; - + _consumer.stop_consumer(); _producer.stop_producer(); diff --git a/include/ur_modern_driver/queue/atomicops.h b/include/ur_modern_driver/queue/atomicops.h index c375710cd..af0089e43 100644 --- a/include/ur_modern_driver/queue/atomicops.h +++ b/include/ur_modern_driver/queue/atomicops.h @@ -12,10 +12,10 @@ // Uses the AE_* prefix for macros (historical reasons), and the "moodycamel" namespace for symbols. #include -#include #include #include #include +#include // Platform detection #if defined(__INTEL_COMPILER) @@ -38,22 +38,19 @@ #define AE_ARCH_UNKNOWN #endif - // AE_UNUSED #define AE_UNUSED(x) ((void)x) - // AE_FORCEINLINE #if defined(AE_VCPP) || defined(AE_ICC) #define AE_FORCEINLINE __forceinline #elif defined(AE_GCC) -//#define AE_FORCEINLINE __attribute__((always_inline)) +//#define AE_FORCEINLINE __attribute__((always_inline)) #define AE_FORCEINLINE inline #else #define AE_FORCEINLINE inline #endif - // AE_ALIGN #if defined(AE_VCPP) || defined(AE_ICC) #define AE_ALIGN(x) __declspec(align(x)) @@ -64,24 +61,23 @@ #define AE_ALIGN(x) __attribute__((aligned(x))) #endif - // Portable atomic fences implemented below: namespace moodycamel { enum memory_order { - memory_order_relaxed, - memory_order_acquire, - memory_order_release, - memory_order_acq_rel, - memory_order_seq_cst, - - // memory_order_sync: Forces a full sync: - // #LoadLoad, #LoadStore, #StoreStore, and most significantly, #StoreLoad - memory_order_sync = memory_order_seq_cst + memory_order_relaxed, + memory_order_acquire, + memory_order_release, + memory_order_acq_rel, + memory_order_seq_cst, + + // memory_order_sync: Forces a full sync: + // #LoadLoad, #LoadStore, #StoreStore, and most significantly, #StoreLoad + memory_order_sync = memory_order_seq_cst }; -} // end namespace moodycamel +} // end namespace moodycamel #if (defined(AE_VCPP) && (_MSC_VER < 1700 || defined(__cplusplus_cli))) || defined(AE_ICC) // VS2010 and ICC13 don't support std::atomic_*_fence, implement our own fences @@ -100,10 +96,9 @@ enum memory_order { #define AeLiteSync __lwsync #endif - #ifdef AE_VCPP #pragma warning(push) -#pragma warning(disable: 4365) // Disable erroneous 'conversion from long to unsigned int, signed/unsigned mismatch' error when using `assert` +#pragma warning(disable : 4365) // Disable erroneous 'conversion from long to unsigned int, signed/unsigned mismatch' error when using `assert` #ifdef __cplusplus_cli #pragma managed(push, off) #endif @@ -113,14 +108,24 @@ namespace moodycamel { AE_FORCEINLINE void compiler_fence(memory_order order) { - switch (order) { - case memory_order_relaxed: break; - case memory_order_acquire: _ReadBarrier(); break; - case memory_order_release: _WriteBarrier(); break; - case memory_order_acq_rel: _ReadWriteBarrier(); break; - case memory_order_seq_cst: _ReadWriteBarrier(); break; - default: assert(false); - } + switch (order) { + case memory_order_relaxed: + break; + case memory_order_acquire: + _ReadBarrier(); + break; + case memory_order_release: + _WriteBarrier(); + break; + case memory_order_acq_rel: + _ReadWriteBarrier(); + break; + case memory_order_seq_cst: + _ReadWriteBarrier(); + break; + default: + assert(false); + } } // x86/x64 have a strong memory model -- all loads and stores have @@ -129,51 +134,60 @@ AE_FORCEINLINE void compiler_fence(memory_order order) #if defined(AE_ARCH_X86) || defined(AE_ARCH_X64) AE_FORCEINLINE void fence(memory_order order) { - switch (order) { - case memory_order_relaxed: break; - case memory_order_acquire: _ReadBarrier(); break; - case memory_order_release: _WriteBarrier(); break; - case memory_order_acq_rel: _ReadWriteBarrier(); break; - case memory_order_seq_cst: - _ReadWriteBarrier(); - AeFullSync(); - _ReadWriteBarrier(); - break; - default: assert(false); - } + switch (order) { + case memory_order_relaxed: + break; + case memory_order_acquire: + _ReadBarrier(); + break; + case memory_order_release: + _WriteBarrier(); + break; + case memory_order_acq_rel: + _ReadWriteBarrier(); + break; + case memory_order_seq_cst: + _ReadWriteBarrier(); + AeFullSync(); + _ReadWriteBarrier(); + break; + default: + assert(false); + } } #else AE_FORCEINLINE void fence(memory_order order) { - // Non-specialized arch, use heavier memory barriers everywhere just in case :-( - switch (order) { - case memory_order_relaxed: - break; - case memory_order_acquire: - _ReadBarrier(); - AeLiteSync(); - _ReadBarrier(); - break; - case memory_order_release: - _WriteBarrier(); - AeLiteSync(); - _WriteBarrier(); - break; - case memory_order_acq_rel: - _ReadWriteBarrier(); - AeLiteSync(); - _ReadWriteBarrier(); - break; - case memory_order_seq_cst: - _ReadWriteBarrier(); - AeFullSync(); - _ReadWriteBarrier(); - break; - default: assert(false); - } + // Non-specialized arch, use heavier memory barriers everywhere just in case :-( + switch (order) { + case memory_order_relaxed: + break; + case memory_order_acquire: + _ReadBarrier(); + AeLiteSync(); + _ReadBarrier(); + break; + case memory_order_release: + _WriteBarrier(); + AeLiteSync(); + _WriteBarrier(); + break; + case memory_order_acq_rel: + _ReadWriteBarrier(); + AeLiteSync(); + _ReadWriteBarrier(); + break; + case memory_order_seq_cst: + _ReadWriteBarrier(); + AeFullSync(); + _ReadWriteBarrier(); + break; + default: + assert(false); + } } #endif -} // end namespace moodycamel +} // end namespace moodycamel #else // Use standard library of atomics #include @@ -182,33 +196,52 @@ namespace moodycamel { AE_FORCEINLINE void compiler_fence(memory_order order) { - switch (order) { - case memory_order_relaxed: break; - case memory_order_acquire: std::atomic_signal_fence(std::memory_order_acquire); break; - case memory_order_release: std::atomic_signal_fence(std::memory_order_release); break; - case memory_order_acq_rel: std::atomic_signal_fence(std::memory_order_acq_rel); break; - case memory_order_seq_cst: std::atomic_signal_fence(std::memory_order_seq_cst); break; - default: assert(false); - } + switch (order) { + case memory_order_relaxed: + break; + case memory_order_acquire: + std::atomic_signal_fence(std::memory_order_acquire); + break; + case memory_order_release: + std::atomic_signal_fence(std::memory_order_release); + break; + case memory_order_acq_rel: + std::atomic_signal_fence(std::memory_order_acq_rel); + break; + case memory_order_seq_cst: + std::atomic_signal_fence(std::memory_order_seq_cst); + break; + default: + assert(false); + } } AE_FORCEINLINE void fence(memory_order order) { - switch (order) { - case memory_order_relaxed: break; - case memory_order_acquire: std::atomic_thread_fence(std::memory_order_acquire); break; - case memory_order_release: std::atomic_thread_fence(std::memory_order_release); break; - case memory_order_acq_rel: std::atomic_thread_fence(std::memory_order_acq_rel); break; - case memory_order_seq_cst: std::atomic_thread_fence(std::memory_order_seq_cst); break; - default: assert(false); - } + switch (order) { + case memory_order_relaxed: + break; + case memory_order_acquire: + std::atomic_thread_fence(std::memory_order_acquire); + break; + case memory_order_release: + std::atomic_thread_fence(std::memory_order_release); + break; + case memory_order_acq_rel: + std::atomic_thread_fence(std::memory_order_acq_rel); + break; + case memory_order_seq_cst: + std::atomic_thread_fence(std::memory_order_seq_cst); + break; + default: + assert(false); + } } -} // end namespace moodycamel +} // end namespace moodycamel #endif - #if !defined(AE_VCPP) || (_MSC_VER >= 1700 && !defined(__cplusplus_cli)) #define AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC #endif @@ -223,102 +256,126 @@ AE_FORCEINLINE void fence(memory_order order) // The guarantee of atomicity is only made for types that already have atomic load and store guarantees // at the hardware level -- on most platforms this generally means aligned pointers and integers (only). namespace moodycamel { -template -class weak_atomic -{ +template +class weak_atomic { public: - weak_atomic() { } + weak_atomic() {} #ifdef AE_VCPP -#pragma warning(disable: 4100) // Get rid of (erroneous) 'unreferenced formal parameter' warning +#pragma warning(disable : 4100) // Get rid of (erroneous) 'unreferenced formal parameter' warning #endif - template weak_atomic(U&& x) : value(std::forward(x)) { } + template + weak_atomic(U&& x) + : value(std::forward(x)) + { + } #ifdef __cplusplus_cli - // Work around bug with universal reference/nullptr combination that only appears when /clr is on - weak_atomic(nullptr_t) : value(nullptr) { } + // Work around bug with universal reference/nullptr combination that only appears when /clr is on + weak_atomic(nullptr_t) + : value(nullptr) + { + } #endif - weak_atomic(weak_atomic const& other) : value(other.value) { } - weak_atomic(weak_atomic&& other) : value(std::move(other.value)) { } + weak_atomic(weak_atomic const& other) + : value(other.value) + { + } + weak_atomic(weak_atomic&& other) + : value(std::move(other.value)) + { + } #ifdef AE_VCPP -#pragma warning(default: 4100) +#pragma warning(default : 4100) #endif - AE_FORCEINLINE operator T() const { return load(); } + AE_FORCEINLINE operator T() const + { + return load(); + } - #ifndef AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC - template AE_FORCEINLINE weak_atomic const& operator=(U&& x) { value = std::forward(x); return *this; } - AE_FORCEINLINE weak_atomic const& operator=(weak_atomic const& other) { value = other.value; return *this; } - - AE_FORCEINLINE T load() const { return value; } - - AE_FORCEINLINE T fetch_add_acquire(T increment) - { + template + AE_FORCEINLINE weak_atomic const& operator=(U&& x) + { + value = std::forward(x); + return *this; + } + AE_FORCEINLINE weak_atomic const& operator=(weak_atomic const& other) + { + value = other.value; + return *this; + } + + AE_FORCEINLINE T load() const { return value; } + + AE_FORCEINLINE T fetch_add_acquire(T increment) + { #if defined(AE_ARCH_X64) || defined(AE_ARCH_X86) - if (sizeof(T) == 4) return _InterlockedExchangeAdd((long volatile*)&value, (long)increment); + if (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 #endif - assert(false && "T must be either a 32 or 64 bit type"); - return value; - } - - AE_FORCEINLINE T fetch_add_release(T increment) - { + assert(false && "T must be either a 32 or 64 bit type"); + return value; + } + + AE_FORCEINLINE T fetch_add_release(T increment) + { #if defined(AE_ARCH_X64) || defined(AE_ARCH_X86) - if (sizeof(T) == 4) return _InterlockedExchangeAdd((long volatile*)&value, (long)increment); + if (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 #endif - assert(false && "T must be either a 32 or 64 bit type"); - return value; - } + assert(false && "T must be either a 32 or 64 bit type"); + return value; + } #else - template - AE_FORCEINLINE weak_atomic const& operator=(U&& x) - { - value.store(std::forward(x), std::memory_order_relaxed); - return *this; - } - - AE_FORCEINLINE weak_atomic const& operator=(weak_atomic const& other) - { - value.store(other.value.load(std::memory_order_relaxed), std::memory_order_relaxed); - return *this; - } - - AE_FORCEINLINE T load() const { return value.load(std::memory_order_relaxed); } - - AE_FORCEINLINE T fetch_add_acquire(T increment) - { - return value.fetch_add(increment, std::memory_order_acquire); - } - - AE_FORCEINLINE T fetch_add_release(T increment) - { - return value.fetch_add(increment, std::memory_order_release); - } + template + AE_FORCEINLINE weak_atomic const& operator=(U&& x) + { + value.store(std::forward(x), std::memory_order_relaxed); + return *this; + } + + AE_FORCEINLINE weak_atomic const& operator=(weak_atomic const& other) + { + value.store(other.value.load(std::memory_order_relaxed), std::memory_order_relaxed); + return *this; + } + + AE_FORCEINLINE T load() const { return value.load(std::memory_order_relaxed); } + + AE_FORCEINLINE T fetch_add_acquire(T increment) + { + return value.fetch_add(increment, std::memory_order_acquire); + } + + AE_FORCEINLINE T fetch_add_release(T increment) + { + return value.fetch_add(increment, std::memory_order_release); + } #endif - private: #ifndef AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC - // No std::atomic support, but still need to circumvent compiler optimizations. - // `volatile` will make memory access slow, but is guaranteed to be reliable. - volatile T value; + // No std::atomic support, but still need to circumvent compiler optimizations. + // `volatile` will make memory access slow, but is guaranteed to be reliable. + volatile T value; #else - std::atomic value; + std::atomic value; #endif }; -} // end namespace moodycamel - - +} // end namespace moodycamel // Portable single-producer, single-consumer semaphore below: @@ -329,11 +386,11 @@ class weak_atomic // I know this is an ugly hack but it still beats polluting the global // namespace with thousands of generic names or adding a .cpp for nothing. extern "C" { - struct _SECURITY_ATTRIBUTES; - __declspec(dllimport) void* __stdcall CreateSemaphoreW(_SECURITY_ATTRIBUTES* lpSemaphoreAttributes, long lInitialCount, long lMaximumCount, const wchar_t* lpName); - __declspec(dllimport) int __stdcall CloseHandle(void* hObject); - __declspec(dllimport) unsigned long __stdcall WaitForSingleObject(void* hHandle, unsigned long dwMilliseconds); - __declspec(dllimport) int __stdcall ReleaseSemaphore(void* hSemaphore, long lReleaseCount, long* lpPreviousCount); +struct _SECURITY_ATTRIBUTES; +__declspec(dllimport) void* __stdcall CreateSemaphoreW(_SECURITY_ATTRIBUTES* lpSemaphoreAttributes, long lInitialCount, long lMaximumCount, const wchar_t* lpName); +__declspec(dllimport) int __stdcall CloseHandle(void* hObject); +__declspec(dllimport) unsigned long __stdcall WaitForSingleObject(void* hHandle, unsigned long dwMilliseconds); +__declspec(dllimport) int __stdcall ReleaseSemaphore(void* hSemaphore, long lReleaseCount, long* lpPreviousCount); } #elif defined(__MACH__) #include @@ -341,320 +398,305 @@ extern "C" { #include #endif -namespace moodycamel -{ - // Code in the spsc_sema namespace below is an adaptation of Jeff Preshing's - // portable + lightweight semaphore implementations, originally from - // https://github.com/preshing/cpp11-on-multicore/blob/master/common/sema.h - // LICENSE: - // Copyright (c) 2015 Jeff Preshing - // - // This software is provided 'as-is', without any express or implied - // warranty. In no event will the authors be held liable for any damages - // arising from the use of this software. - // - // Permission is granted to anyone to use this software for any purpose, - // including commercial applications, and to alter it and redistribute it - // freely, subject to the following restrictions: - // - // 1. The origin of this software must not be misrepresented; you must not - // claim that you wrote the original software. If you use this software - // in a product, an acknowledgement in the product documentation would be - // appreciated but is not required. - // 2. Altered source versions must be plainly marked as such, and must not be - // misrepresented as being the original software. - // 3. This notice may not be removed or altered from any source distribution. - namespace spsc_sema - { +namespace moodycamel { +// Code in the spsc_sema namespace below is an adaptation of Jeff Preshing's +// portable + lightweight semaphore implementations, originally from +// https://github.com/preshing/cpp11-on-multicore/blob/master/common/sema.h +// LICENSE: +// Copyright (c) 2015 Jeff Preshing +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgement in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +namespace spsc_sema { #if defined(_WIN32) - class Semaphore - { - private: - void* m_hSema; - - Semaphore(const Semaphore& other); - Semaphore& operator=(const Semaphore& other); - - public: - Semaphore(int initialCount = 0) - { - assert(initialCount >= 0); - const long maxLong = 0x7fffffff; - m_hSema = CreateSemaphoreW(nullptr, initialCount, maxLong, nullptr); - } - - ~Semaphore() - { - CloseHandle(m_hSema); - } - - void wait() - { - const unsigned long infinite = 0xffffffff; - WaitForSingleObject(m_hSema, infinite); - } - - bool try_wait() - { - const unsigned long RC_WAIT_TIMEOUT = 0x00000102; - return WaitForSingleObject(m_hSema, 0) != RC_WAIT_TIMEOUT; - } - - bool timed_wait(std::uint64_t usecs) - { - const unsigned long RC_WAIT_TIMEOUT = 0x00000102; - return WaitForSingleObject(m_hSema, (unsigned long)(usecs / 1000)) != RC_WAIT_TIMEOUT; - } - - void signal(int count = 1) - { - ReleaseSemaphore(m_hSema, count, nullptr); - } - }; + class Semaphore { + private: + void* m_hSema; + + Semaphore(const Semaphore& other); + Semaphore& operator=(const Semaphore& other); + + public: + Semaphore(int initialCount = 0) + { + assert(initialCount >= 0); + const long maxLong = 0x7fffffff; + m_hSema = CreateSemaphoreW(nullptr, initialCount, maxLong, nullptr); + } + + ~Semaphore() + { + CloseHandle(m_hSema); + } + + void wait() + { + const unsigned long infinite = 0xffffffff; + WaitForSingleObject(m_hSema, infinite); + } + + bool try_wait() + { + const unsigned long RC_WAIT_TIMEOUT = 0x00000102; + return WaitForSingleObject(m_hSema, 0) != RC_WAIT_TIMEOUT; + } + + bool timed_wait(std::uint64_t usecs) + { + const unsigned long RC_WAIT_TIMEOUT = 0x00000102; + return WaitForSingleObject(m_hSema, (unsigned long)(usecs / 1000)) != RC_WAIT_TIMEOUT; + } + + void signal(int count = 1) + { + ReleaseSemaphore(m_hSema, count, nullptr); + } + }; #elif defined(__MACH__) - //--------------------------------------------------------- - // Semaphore (Apple iOS and OSX) - // Can't use POSIX semaphores due to http://lists.apple.com/archives/darwin-kernel/2009/Apr/msg00010.html - //--------------------------------------------------------- - class Semaphore - { - private: - semaphore_t m_sema; - - Semaphore(const Semaphore& other); - Semaphore& operator=(const Semaphore& other); - - public: - Semaphore(int initialCount = 0) - { - assert(initialCount >= 0); - semaphore_create(mach_task_self(), &m_sema, SYNC_POLICY_FIFO, initialCount); - } - - ~Semaphore() - { - semaphore_destroy(mach_task_self(), m_sema); - } - - void wait() - { - semaphore_wait(m_sema); - } - - bool try_wait() - { - return timed_wait(0); - } - - bool timed_wait(std::int64_t timeout_usecs) - { - mach_timespec_t ts; - ts.tv_sec = timeout_usecs / 1000000; - ts.tv_nsec = (timeout_usecs % 1000000) * 1000; - - // added in OSX 10.10: https://developer.apple.com/library/prerelease/mac/documentation/General/Reference/APIDiffsMacOSX10_10SeedDiff/modules/Darwin.html - kern_return_t rc = semaphore_timedwait(m_sema, ts); - - return rc != KERN_OPERATION_TIMED_OUT; - } - - void signal() - { - semaphore_signal(m_sema); - } - - void signal(int count) - { - while (count-- > 0) - { - semaphore_signal(m_sema); - } - } - }; + //--------------------------------------------------------- + // Semaphore (Apple iOS and OSX) + // Can't use POSIX semaphores due to http://lists.apple.com/archives/darwin-kernel/2009/Apr/msg00010.html + //--------------------------------------------------------- + class Semaphore { + private: + semaphore_t m_sema; + + Semaphore(const Semaphore& other); + Semaphore& operator=(const Semaphore& other); + + public: + Semaphore(int initialCount = 0) + { + assert(initialCount >= 0); + semaphore_create(mach_task_self(), &m_sema, SYNC_POLICY_FIFO, initialCount); + } + + ~Semaphore() + { + semaphore_destroy(mach_task_self(), m_sema); + } + + void wait() + { + semaphore_wait(m_sema); + } + + bool try_wait() + { + return timed_wait(0); + } + + bool timed_wait(std::int64_t timeout_usecs) + { + mach_timespec_t ts; + ts.tv_sec = timeout_usecs / 1000000; + ts.tv_nsec = (timeout_usecs % 1000000) * 1000; + + // added in OSX 10.10: https://developer.apple.com/library/prerelease/mac/documentation/General/Reference/APIDiffsMacOSX10_10SeedDiff/modules/Darwin.html + kern_return_t rc = semaphore_timedwait(m_sema, ts); + + return rc != KERN_OPERATION_TIMED_OUT; + } + + void signal() + { + semaphore_signal(m_sema); + } + + void signal(int count) + { + while (count-- > 0) { + semaphore_signal(m_sema); + } + } + }; #elif defined(__unix__) - //--------------------------------------------------------- - // Semaphore (POSIX, Linux) - //--------------------------------------------------------- - class Semaphore - { - private: - sem_t m_sema; - - Semaphore(const Semaphore& other); - Semaphore& operator=(const Semaphore& other); - - public: - Semaphore(int initialCount = 0) - { - assert(initialCount >= 0); - sem_init(&m_sema, 0, initialCount); - } - - ~Semaphore() - { - sem_destroy(&m_sema); - } - - void wait() - { - // http://stackoverflow.com/questions/2013181/gdb-causes-sem-wait-to-fail-with-eintr-error - int rc; - do - { - rc = sem_wait(&m_sema); - } - while (rc == -1 && errno == EINTR); - } - - bool try_wait() - { - int rc; - do { - rc = sem_trywait(&m_sema); - } while (rc == -1 && errno == EINTR); - return !(rc == -1 && errno == EAGAIN); - } - - bool timed_wait(std::uint64_t usecs) - { - struct timespec ts; - const int usecs_in_1_sec = 1000000; - const int nsecs_in_1_sec = 1000000000; - clock_gettime(CLOCK_REALTIME, &ts); - ts.tv_sec += usecs / usecs_in_1_sec; - ts.tv_nsec += (usecs % usecs_in_1_sec) * 1000; - // sem_timedwait bombs if you have more than 1e9 in tv_nsec - // so we have to clean things up before passing it in - if (ts.tv_nsec > nsecs_in_1_sec) { - ts.tv_nsec -= nsecs_in_1_sec; - ++ts.tv_sec; - } - - int rc; - do { - rc = sem_timedwait(&m_sema, &ts); - } while (rc == -1 && errno == EINTR); - return !(rc == -1 && errno == ETIMEDOUT); - } - - void signal() - { - sem_post(&m_sema); - } - - void signal(int count) - { - while (count-- > 0) - { - sem_post(&m_sema); - } - } - }; + //--------------------------------------------------------- + // Semaphore (POSIX, Linux) + //--------------------------------------------------------- + class Semaphore { + private: + sem_t m_sema; + + Semaphore(const Semaphore& other); + Semaphore& operator=(const Semaphore& other); + + public: + Semaphore(int initialCount = 0) + { + assert(initialCount >= 0); + sem_init(&m_sema, 0, initialCount); + } + + ~Semaphore() + { + sem_destroy(&m_sema); + } + + void wait() + { + // http://stackoverflow.com/questions/2013181/gdb-causes-sem-wait-to-fail-with-eintr-error + int rc; + do { + rc = sem_wait(&m_sema); + } while (rc == -1 && errno == EINTR); + } + + bool try_wait() + { + int rc; + do { + rc = sem_trywait(&m_sema); + } while (rc == -1 && errno == EINTR); + return !(rc == -1 && errno == EAGAIN); + } + + bool timed_wait(std::uint64_t usecs) + { + struct timespec ts; + const int usecs_in_1_sec = 1000000; + const int nsecs_in_1_sec = 1000000000; + clock_gettime(CLOCK_REALTIME, &ts); + ts.tv_sec += usecs / usecs_in_1_sec; + ts.tv_nsec += (usecs % usecs_in_1_sec) * 1000; + // sem_timedwait bombs if you have more than 1e9 in tv_nsec + // so we have to clean things up before passing it in + if (ts.tv_nsec > nsecs_in_1_sec) { + ts.tv_nsec -= nsecs_in_1_sec; + ++ts.tv_sec; + } + + int rc; + do { + rc = sem_timedwait(&m_sema, &ts); + } while (rc == -1 && errno == EINTR); + return !(rc == -1 && errno == ETIMEDOUT); + } + + void signal() + { + sem_post(&m_sema); + } + + void signal(int count) + { + while (count-- > 0) { + sem_post(&m_sema); + } + } + }; #else #error Unsupported platform! (No semaphore wrapper available) #endif - //--------------------------------------------------------- - // LightweightSemaphore - //--------------------------------------------------------- - class LightweightSemaphore - { - public: - typedef std::make_signed::type ssize_t; - - private: - weak_atomic m_count; - Semaphore m_sema; - - bool waitWithPartialSpinning(std::int64_t timeout_usecs = -1) - { - ssize_t oldCount; - // Is there a better way to set the initial spin count? - // If we lower it to 1000, testBenaphore becomes 15x slower on my Core i7-5930K Windows PC, - // as threads start hitting the kernel semaphore. - int spin = 10000; - while (--spin >= 0) - { - if (m_count.load() > 0) - { - m_count.fetch_add_acquire(-1); - return true; - } - compiler_fence(memory_order_acquire); // Prevent the compiler from collapsing the loop. - } - oldCount = m_count.fetch_add_acquire(-1); - if (oldCount > 0) - return true; - if (timeout_usecs < 0) - { - m_sema.wait(); - return true; - } - if (m_sema.timed_wait(timeout_usecs)) - return true; - // At this point, we've timed out waiting for the semaphore, but the - // count is still decremented indicating we may still be waiting on - // it. So we have to re-adjust the count, but only if the semaphore - // wasn't signaled enough times for us too since then. If it was, we - // need to release the semaphore too. - while (true) - { - oldCount = m_count.fetch_add_release(1); - if (oldCount < 0) - return false; // successfully restored things to the way they were - // Oh, the producer thread just signaled the semaphore after all. Try again: - oldCount = m_count.fetch_add_acquire(-1); - if (oldCount > 0 && m_sema.try_wait()) - return true; - } - } - - public: - LightweightSemaphore(ssize_t initialCount = 0) : m_count(initialCount) - { - assert(initialCount >= 0); - } - - bool tryWait() - { - if (m_count.load() > 0) - { - m_count.fetch_add_acquire(-1); - return true; - } - return false; - } - - void wait() - { - if (!tryWait()) - waitWithPartialSpinning(); - } - - bool wait(std::int64_t timeout_usecs) - { - return tryWait() || waitWithPartialSpinning(timeout_usecs); - } - - void signal(ssize_t count = 1) - { - assert(count >= 0); - ssize_t oldCount = m_count.fetch_add_release(count); - assert(oldCount >= -1); - if (oldCount < 0) - { - m_sema.signal(1); - } - } - - ssize_t availableApprox() const - { - ssize_t count = m_count.load(); - return count > 0 ? count : 0; - } - }; - } // end namespace spsc_sema -} // end namespace moodycamel + //--------------------------------------------------------- + // LightweightSemaphore + //--------------------------------------------------------- + class LightweightSemaphore { + public: + typedef std::make_signed::type ssize_t; + + private: + weak_atomic m_count; + Semaphore m_sema; + + bool waitWithPartialSpinning(std::int64_t timeout_usecs = -1) + { + ssize_t oldCount; + // Is there a better way to set the initial spin count? + // If we lower it to 1000, testBenaphore becomes 15x slower on my Core i7-5930K Windows PC, + // as threads start hitting the kernel semaphore. + int spin = 10000; + while (--spin >= 0) { + if (m_count.load() > 0) { + m_count.fetch_add_acquire(-1); + return true; + } + compiler_fence(memory_order_acquire); // Prevent the compiler from collapsing the loop. + } + oldCount = m_count.fetch_add_acquire(-1); + if (oldCount > 0) + return true; + if (timeout_usecs < 0) { + m_sema.wait(); + return true; + } + if (m_sema.timed_wait(timeout_usecs)) + return true; + // At this point, we've timed out waiting for the semaphore, but the + // count is still decremented indicating we may still be waiting on + // it. So we have to re-adjust the count, but only if the semaphore + // wasn't signaled enough times for us too since then. If it was, we + // need to release the semaphore too. + while (true) { + oldCount = m_count.fetch_add_release(1); + if (oldCount < 0) + return false; // successfully restored things to the way they were + // Oh, the producer thread just signaled the semaphore after all. Try again: + oldCount = m_count.fetch_add_acquire(-1); + if (oldCount > 0 && m_sema.try_wait()) + return true; + } + } + + public: + LightweightSemaphore(ssize_t initialCount = 0) + : m_count(initialCount) + { + assert(initialCount >= 0); + } + + bool tryWait() + { + if (m_count.load() > 0) { + m_count.fetch_add_acquire(-1); + return true; + } + return false; + } + + void wait() + { + if (!tryWait()) + waitWithPartialSpinning(); + } + + bool wait(std::int64_t timeout_usecs) + { + return tryWait() || waitWithPartialSpinning(timeout_usecs); + } + + void signal(ssize_t count = 1) + { + assert(count >= 0); + ssize_t oldCount = m_count.fetch_add_release(count); + assert(oldCount >= -1); + if (oldCount < 0) { + m_sema.signal(1); + } + } + + ssize_t availableApprox() const + { + ssize_t count = m_count.load(); + return count > 0 ? count : 0; + } + }; +} // end namespace spsc_sema +} // end namespace moodycamel #if defined(AE_VCPP) && (_MSC_VER < 1700 || defined(__cplusplus_cli)) #pragma warning(pop) diff --git a/include/ur_modern_driver/queue/readerwriterqueue.h b/include/ur_modern_driver/queue/readerwriterqueue.h index ec465d694..1503dcbe7 100644 --- a/include/ur_modern_driver/queue/readerwriterqueue.h +++ b/include/ur_modern_driver/queue/readerwriterqueue.h @@ -5,18 +5,17 @@ #pragma once #include "atomicops.h" -#include -#include #include -#include -#include #include -#include // For malloc/free/abort & size_t +#include // For malloc/free/abort & size_t +#include +#include +#include +#include #if __cplusplus > 199711L || _MSC_VER >= 1700 // C++11 or VS2012 #include #endif - // A lock-free queue for a single-consumer, single-producer architecture. // The queue is also wait-free in the common path (except if more memory // needs to be allocated, in which case malloc is called). @@ -42,773 +41,750 @@ #ifdef AE_VCPP #pragma warning(push) -#pragma warning(disable: 4324) // structure was padded due to __declspec(align()) -#pragma warning(disable: 4820) // padding was added -#pragma warning(disable: 4127) // conditional expression is constant +#pragma warning(disable : 4324) // structure was padded due to __declspec(align()) +#pragma warning(disable : 4820) // padding was added +#pragma warning(disable : 4127) // conditional expression is constant #endif namespace moodycamel { -template -class ReaderWriterQueue -{ - // Design: Based on a queue-of-queues. The low-level queues are just - // circular buffers with front and tail indices indicating where the - // next element to dequeue is and where the next element can be enqueued, - // respectively. Each low-level queue is called a "block". Each block - // wastes exactly one element's worth of space to keep the design simple - // (if front == tail then the queue is empty, and can't be full). - // The high-level queue is a circular linked list of blocks; again there - // is a front and tail, but this time they are pointers to the blocks. - // The front block is where the next element to be dequeued is, provided - // the block is not empty. The back block is where elements are to be - // enqueued, provided the block is not full. - // The producer thread owns all the tail indices/pointers. The consumer - // thread owns all the front indices/pointers. Both threads read each - // other's variables, but only the owning thread updates them. E.g. After - // the consumer reads the producer's tail, the tail may change before the - // consumer is done dequeuing an object, but the consumer knows the tail - // will never go backwards, only forwards. - // If there is no room to enqueue an object, an additional block (of - // equal size to the last block) is added. Blocks are never removed. +template +class ReaderWriterQueue { + // Design: Based on a queue-of-queues. The low-level queues are just + // circular buffers with front and tail indices indicating where the + // next element to dequeue is and where the next element can be enqueued, + // respectively. Each low-level queue is called a "block". Each block + // wastes exactly one element's worth of space to keep the design simple + // (if front == tail then the queue is empty, and can't be full). + // The high-level queue is a circular linked list of blocks; again there + // is a front and tail, but this time they are pointers to the blocks. + // The front block is where the next element to be dequeued is, provided + // the block is not empty. The back block is where elements are to be + // enqueued, provided the block is not full. + // The producer thread owns all the tail indices/pointers. The consumer + // thread owns all the front indices/pointers. Both threads read each + // other's variables, but only the owning thread updates them. E.g. After + // the consumer reads the producer's tail, the tail may change before the + // consumer is done dequeuing an object, but the consumer knows the tail + // will never go backwards, only forwards. + // If there is no room to enqueue an object, an additional block (of + // equal size to the last block) is added. Blocks are never removed. public: - // Constructs a queue that can hold maxSize elements without further - // allocations. If more than MAX_BLOCK_SIZE elements are requested, - // then several blocks of MAX_BLOCK_SIZE each are reserved (including - // at least one extra buffer block). - explicit ReaderWriterQueue(size_t maxSize = 15) + // Constructs a queue that can hold maxSize elements without further + // allocations. If more than MAX_BLOCK_SIZE elements are requested, + // then several blocks of MAX_BLOCK_SIZE each are reserved (including + // at least one extra buffer block). + explicit ReaderWriterQueue(size_t maxSize = 15) #ifndef NDEBUG - : enqueuing(false) - ,dequeuing(false) + : enqueuing(false) + , dequeuing(false) #endif - { - assert(maxSize > 0); - assert(MAX_BLOCK_SIZE == ceilToPow2(MAX_BLOCK_SIZE) && "MAX_BLOCK_SIZE must be a power of 2"); - assert(MAX_BLOCK_SIZE >= 2 && "MAX_BLOCK_SIZE must be at least 2"); - - Block* firstBlock = nullptr; - - largestBlockSize = ceilToPow2(maxSize + 1); // We need a spare slot to fit maxSize elements in the block - if (largestBlockSize > MAX_BLOCK_SIZE * 2) { - // We need a spare block in case the producer is writing to a different block the consumer is reading from, and - // wants to enqueue the maximum number of elements. We also need a spare element in each block to avoid the ambiguity - // between front == tail meaning "empty" and "full". - // So the effective number of slots that are guaranteed to be usable at any time is the block size - 1 times the - // number of blocks - 1. Solving for maxSize and applying a ceiling to the division gives us (after simplifying): - size_t initialBlockCount = (maxSize + MAX_BLOCK_SIZE * 2 - 3) / (MAX_BLOCK_SIZE - 1); - largestBlockSize = MAX_BLOCK_SIZE; - Block* lastBlock = nullptr; - for (size_t i = 0; i != initialBlockCount; ++i) { - auto block = make_block(largestBlockSize); - if (block == nullptr) { + { + assert(maxSize > 0); + assert(MAX_BLOCK_SIZE == ceilToPow2(MAX_BLOCK_SIZE) && "MAX_BLOCK_SIZE must be a power of 2"); + assert(MAX_BLOCK_SIZE >= 2 && "MAX_BLOCK_SIZE must be at least 2"); + + Block* firstBlock = nullptr; + + largestBlockSize = ceilToPow2(maxSize + 1); // We need a spare slot to fit maxSize elements in the block + if (largestBlockSize > MAX_BLOCK_SIZE * 2) { + // We need a spare block in case the producer is writing to a different block the consumer is reading from, and + // wants to enqueue the maximum number of elements. We also need a spare element in each block to avoid the ambiguity + // between front == tail meaning "empty" and "full". + // So the effective number of slots that are guaranteed to be usable at any time is the block size - 1 times the + // number of blocks - 1. Solving for maxSize and applying a ceiling to the division gives us (after simplifying): + size_t initialBlockCount = (maxSize + MAX_BLOCK_SIZE * 2 - 3) / (MAX_BLOCK_SIZE - 1); + largestBlockSize = MAX_BLOCK_SIZE; + Block* lastBlock = nullptr; + for (size_t i = 0; i != initialBlockCount; ++i) { + auto block = make_block(largestBlockSize); + if (block == nullptr) { #ifdef MOODYCAMEL_EXCEPTIONS_ENABLED - throw std::bad_alloc(); + throw std::bad_alloc(); #else - abort(); + abort(); #endif - } - if (firstBlock == nullptr) { - firstBlock = block; - } - else { - lastBlock->next = block; - } - lastBlock = block; - block->next = firstBlock; - } - } - else { - firstBlock = make_block(largestBlockSize); - if (firstBlock == nullptr) { + } + if (firstBlock == nullptr) { + firstBlock = block; + } else { + lastBlock->next = block; + } + lastBlock = block; + block->next = firstBlock; + } + } else { + firstBlock = make_block(largestBlockSize); + if (firstBlock == nullptr) { #ifdef MOODYCAMEL_EXCEPTIONS_ENABLED - throw std::bad_alloc(); + throw std::bad_alloc(); #else - abort(); + abort(); #endif - } - firstBlock->next = firstBlock; - } - frontBlock = firstBlock; - tailBlock = firstBlock; - - // Make sure the reader/writer threads will have the initialized memory setup above: - fence(memory_order_sync); - } - - // Note: The queue should not be accessed concurrently while it's - // being deleted. It's up to the user to synchronize this. - ~ReaderWriterQueue() - { - // Make sure we get the latest version of all variables from other CPUs: - fence(memory_order_sync); - - // Destroy any remaining objects in queue and free memory - Block* frontBlock_ = frontBlock; - Block* block = frontBlock_; - do { - Block* nextBlock = block->next; - size_t blockFront = block->front; - size_t blockTail = block->tail; - - for (size_t i = blockFront; i != blockTail; i = (i + 1) & block->sizeMask) { - auto element = reinterpret_cast(block->data + i * sizeof(T)); - element->~T(); - (void)element; - } - - auto rawBlock = block->rawThis; - block->~Block(); - std::free(rawBlock); - block = nextBlock; - } 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. - AE_FORCEINLINE bool try_enqueue(T const& element) - { - return inner_enqueue(element); - } - - // Enqueues a moved copy of element if there is room in the queue. - // Returns true if the element was enqueued, false otherwise. - // Does not allocate memory. - AE_FORCEINLINE bool try_enqueue(T&& element) - { - return inner_enqueue(std::forward(element)); - } - - - // Enqueues a copy of element on the queue. - // Allocates an additional block of memory if needed. - // Only fails (returns false) if memory allocation fails. - AE_FORCEINLINE bool enqueue(T const& element) - { - return inner_enqueue(element); - } - - // Enqueues a moved copy of element on the queue. - // Allocates an additional block of memory if needed. - // Only fails (returns false) if memory allocation fails. - AE_FORCEINLINE bool enqueue(T&& element) - { - return inner_enqueue(std::forward(element)); - } - - - // Attempts to dequeue an element; if the queue is empty, - // returns false instead. If the queue has at least one element, - // moves front to result using operator=, then returns true. - template - bool try_dequeue(U& result) - { + } + firstBlock->next = firstBlock; + } + frontBlock = firstBlock; + tailBlock = firstBlock; + + // Make sure the reader/writer threads will have the initialized memory setup above: + fence(memory_order_sync); + } + + // Note: The queue should not be accessed concurrently while it's + // being deleted. It's up to the user to synchronize this. + ~ReaderWriterQueue() + { + // Make sure we get the latest version of all variables from other CPUs: + fence(memory_order_sync); + + // Destroy any remaining objects in queue and free memory + Block* frontBlock_ = frontBlock; + Block* block = frontBlock_; + do { + Block* nextBlock = block->next; + size_t blockFront = block->front; + size_t blockTail = block->tail; + + for (size_t i = blockFront; i != blockTail; i = (i + 1) & block->sizeMask) { + auto element = reinterpret_cast(block->data + i * sizeof(T)); + element->~T(); + (void)element; + } + + auto rawBlock = block->rawThis; + block->~Block(); + std::free(rawBlock); + block = nextBlock; + } 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. + AE_FORCEINLINE bool try_enqueue(T const& element) + { + return inner_enqueue(element); + } + + // Enqueues a moved copy of element if there is room in the queue. + // Returns true if the element was enqueued, false otherwise. + // Does not allocate memory. + AE_FORCEINLINE bool try_enqueue(T&& element) + { + return inner_enqueue(std::forward(element)); + } + + // Enqueues a copy of element on the queue. + // Allocates an additional block of memory if needed. + // Only fails (returns false) if memory allocation fails. + AE_FORCEINLINE bool enqueue(T const& element) + { + return inner_enqueue(element); + } + + // Enqueues a moved copy of element on the queue. + // Allocates an additional block of memory if needed. + // Only fails (returns false) if memory allocation fails. + AE_FORCEINLINE bool enqueue(T&& element) + { + return inner_enqueue(std::forward(element)); + } + + // Attempts to dequeue an element; if the queue is empty, + // returns false instead. If the queue has at least one element, + // moves front to result using operator=, then returns true. + template + bool try_dequeue(U& result) + { #ifndef NDEBUG - ReentrantGuard guard(this->dequeuing); + ReentrantGuard guard(this->dequeuing); #endif - // High-level pseudocode: - // Remember where the tail block is - // If the front block has an element in it, dequeue it - // Else - // If front block was the tail block when we entered the function, return false - // Else advance to next block and dequeue the item there - - // Note that we have to use the value of the tail block from before we check if the front - // block is full or not, in case the front block is empty and then, before we check if the - // tail block is at the front block or not, the producer fills up the front block *and - // moves on*, which would make us skip a filled block. Seems unlikely, but was consistently - // reproducible in practice. - // In order to avoid overhead in the common case, though, we do a double-checked pattern - // where we have the fast path if the front block is not empty, then read the tail block, - // then re-read the front block and check if it's not empty again, then check if the tail - // block has advanced. - - Block* frontBlock_ = frontBlock.load(); - size_t blockTail = frontBlock_->localTail; - size_t blockFront = frontBlock_->front.load(); - - if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) { - fence(memory_order_acquire); - - non_empty_front_block: - // Front block not empty, dequeue from here - auto element = reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); - result = std::move(*element); - element->~T(); - - blockFront = (blockFront + 1) & frontBlock_->sizeMask; - - fence(memory_order_release); - frontBlock_->front = blockFront; - } - else if (frontBlock_ != tailBlock.load()) { - fence(memory_order_acquire); - - frontBlock_ = frontBlock.load(); - blockTail = frontBlock_->localTail = frontBlock_->tail.load(); - blockFront = frontBlock_->front.load(); - fence(memory_order_acquire); - - if (blockFront != blockTail) { - // Oh look, the front block isn't empty after all - goto non_empty_front_block; - } - - // Front block is empty but there's another block ahead, advance to it - Block* nextBlock = frontBlock_->next; - // Don't need an acquire fence here since next can only ever be set on the tailBlock, - // and we're not the tailBlock, and we did an acquire earlier after reading tailBlock which - // ensures next is up-to-date on this CPU in case we recently were at tailBlock. - - size_t nextBlockFront = nextBlock->front.load(); - size_t nextBlockTail = nextBlock->localTail = nextBlock->tail.load(); - fence(memory_order_acquire); - - // Since the tailBlock is only ever advanced after being written to, - // we know there's for sure an element to dequeue on it - assert(nextBlockFront != nextBlockTail); - AE_UNUSED(nextBlockTail); - - // We're done with this block, let the producer use it if it needs - fence(memory_order_release); // Expose possibly pending changes to frontBlock->front from last dequeue - frontBlock = frontBlock_ = nextBlock; - - compiler_fence(memory_order_release); // Not strictly needed - - auto element = reinterpret_cast(frontBlock_->data + nextBlockFront * sizeof(T)); - - result = std::move(*element); - element->~T(); - - nextBlockFront = (nextBlockFront + 1) & frontBlock_->sizeMask; - - fence(memory_order_release); - frontBlock_->front = nextBlockFront; - } - else { - // No elements in current block and no other block to advance to - return false; - } - - return true; - } - - - // Returns a pointer to the front element in the queue (the one that - // would be removed next by a call to `try_dequeue` or `pop`). If the - // queue appears empty at the time the method is called, nullptr is - // returned instead. - // Must be called only from the consumer thread. - T* peek() - { + // High-level pseudocode: + // Remember where the tail block is + // If the front block has an element in it, dequeue it + // Else + // If front block was the tail block when we entered the function, return false + // Else advance to next block and dequeue the item there + + // Note that we have to use the value of the tail block from before we check if the front + // block is full or not, in case the front block is empty and then, before we check if the + // tail block is at the front block or not, the producer fills up the front block *and + // moves on*, which would make us skip a filled block. Seems unlikely, but was consistently + // reproducible in practice. + // In order to avoid overhead in the common case, though, we do a double-checked pattern + // where we have the fast path if the front block is not empty, then read the tail block, + // then re-read the front block and check if it's not empty again, then check if the tail + // block has advanced. + + Block* frontBlock_ = frontBlock.load(); + size_t blockTail = frontBlock_->localTail; + size_t blockFront = frontBlock_->front.load(); + + if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) { + fence(memory_order_acquire); + + non_empty_front_block: + // Front block not empty, dequeue from here + auto element = reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); + result = std::move(*element); + element->~T(); + + blockFront = (blockFront + 1) & frontBlock_->sizeMask; + + fence(memory_order_release); + frontBlock_->front = blockFront; + } else if (frontBlock_ != tailBlock.load()) { + fence(memory_order_acquire); + + frontBlock_ = frontBlock.load(); + blockTail = frontBlock_->localTail = frontBlock_->tail.load(); + blockFront = frontBlock_->front.load(); + fence(memory_order_acquire); + + if (blockFront != blockTail) { + // Oh look, the front block isn't empty after all + goto non_empty_front_block; + } + + // Front block is empty but there's another block ahead, advance to it + Block* nextBlock = frontBlock_->next; + // Don't need an acquire fence here since next can only ever be set on the tailBlock, + // and we're not the tailBlock, and we did an acquire earlier after reading tailBlock which + // ensures next is up-to-date on this CPU in case we recently were at tailBlock. + + size_t nextBlockFront = nextBlock->front.load(); + size_t nextBlockTail = nextBlock->localTail = nextBlock->tail.load(); + fence(memory_order_acquire); + + // Since the tailBlock is only ever advanced after being written to, + // we know there's for sure an element to dequeue on it + assert(nextBlockFront != nextBlockTail); + AE_UNUSED(nextBlockTail); + + // We're done with this block, let the producer use it if it needs + fence(memory_order_release); // Expose possibly pending changes to frontBlock->front from last dequeue + frontBlock = frontBlock_ = nextBlock; + + compiler_fence(memory_order_release); // Not strictly needed + + auto element = reinterpret_cast(frontBlock_->data + nextBlockFront * sizeof(T)); + + result = std::move(*element); + element->~T(); + + nextBlockFront = (nextBlockFront + 1) & frontBlock_->sizeMask; + + fence(memory_order_release); + frontBlock_->front = nextBlockFront; + } else { + // No elements in current block and no other block to advance to + return false; + } + + return true; + } + + // Returns a pointer to the front element in the queue (the one that + // would be removed next by a call to `try_dequeue` or `pop`). If the + // queue appears empty at the time the method is called, nullptr is + // returned instead. + // Must be called only from the consumer thread. + T* peek() + { #ifndef NDEBUG - ReentrantGuard guard(this->dequeuing); + ReentrantGuard guard(this->dequeuing); #endif - // See try_dequeue() for reasoning - - Block* frontBlock_ = frontBlock.load(); - size_t blockTail = frontBlock_->localTail; - size_t blockFront = frontBlock_->front.load(); - - if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) { - fence(memory_order_acquire); - non_empty_front_block: - return reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); - } - else if (frontBlock_ != tailBlock.load()) { - fence(memory_order_acquire); - frontBlock_ = frontBlock.load(); - blockTail = frontBlock_->localTail = frontBlock_->tail.load(); - blockFront = frontBlock_->front.load(); - fence(memory_order_acquire); - - if (blockFront != blockTail) { - goto non_empty_front_block; - } - - Block* nextBlock = frontBlock_->next; - - size_t nextBlockFront = nextBlock->front.load(); - fence(memory_order_acquire); - - assert(nextBlockFront != nextBlock->tail.load()); - return reinterpret_cast(nextBlock->data + nextBlockFront * sizeof(T)); - } - - return nullptr; - } - - // Removes the front element from the queue, if any, without returning it. - // Returns true on success, or false if the queue appeared empty at the time - // `pop` was called. - bool pop() - { + // See try_dequeue() for reasoning + + Block* frontBlock_ = frontBlock.load(); + size_t blockTail = frontBlock_->localTail; + size_t blockFront = frontBlock_->front.load(); + + if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) { + fence(memory_order_acquire); + non_empty_front_block: + return reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); + } else if (frontBlock_ != tailBlock.load()) { + fence(memory_order_acquire); + frontBlock_ = frontBlock.load(); + blockTail = frontBlock_->localTail = frontBlock_->tail.load(); + blockFront = frontBlock_->front.load(); + fence(memory_order_acquire); + + if (blockFront != blockTail) { + goto non_empty_front_block; + } + + Block* nextBlock = frontBlock_->next; + + size_t nextBlockFront = nextBlock->front.load(); + fence(memory_order_acquire); + + assert(nextBlockFront != nextBlock->tail.load()); + return reinterpret_cast(nextBlock->data + nextBlockFront * sizeof(T)); + } + + return nullptr; + } + + // Removes the front element from the queue, if any, without returning it. + // Returns true on success, or false if the queue appeared empty at the time + // `pop` was called. + bool pop() + { #ifndef NDEBUG - ReentrantGuard guard(this->dequeuing); + ReentrantGuard guard(this->dequeuing); #endif - // See try_dequeue() for reasoning - - Block* frontBlock_ = frontBlock.load(); - size_t blockTail = frontBlock_->localTail; - size_t blockFront = frontBlock_->front.load(); - - if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) { - fence(memory_order_acquire); - - non_empty_front_block: - auto element = reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); - element->~T(); - - blockFront = (blockFront + 1) & frontBlock_->sizeMask; - - fence(memory_order_release); - frontBlock_->front = blockFront; - } - else if (frontBlock_ != tailBlock.load()) { - fence(memory_order_acquire); - frontBlock_ = frontBlock.load(); - blockTail = frontBlock_->localTail = frontBlock_->tail.load(); - blockFront = frontBlock_->front.load(); - fence(memory_order_acquire); - - if (blockFront != blockTail) { - goto non_empty_front_block; - } - - // Front block is empty but there's another block ahead, advance to it - Block* nextBlock = frontBlock_->next; - - size_t nextBlockFront = nextBlock->front.load(); - size_t nextBlockTail = nextBlock->localTail = nextBlock->tail.load(); - fence(memory_order_acquire); - - assert(nextBlockFront != nextBlockTail); - AE_UNUSED(nextBlockTail); - - fence(memory_order_release); - frontBlock = frontBlock_ = nextBlock; - - compiler_fence(memory_order_release); - - auto element = reinterpret_cast(frontBlock_->data + nextBlockFront * sizeof(T)); - element->~T(); - - nextBlockFront = (nextBlockFront + 1) & frontBlock_->sizeMask; - - fence(memory_order_release); - frontBlock_->front = nextBlockFront; - } - else { - // No elements in current block and no other block to advance to - return false; - } - - return true; - } - - // Returns the approximate number of items currently in the queue. - // Safe to call from both the producer and consumer threads. - inline size_t size_approx() const - { - size_t result = 0; - Block* frontBlock_ = frontBlock.load(); - Block* block = frontBlock_; - do { - fence(memory_order_acquire); - size_t blockFront = block->front.load(); - size_t blockTail = block->tail.load(); - result += (blockTail - blockFront) & block->sizeMask; - block = block->next.load(); - } while (block != frontBlock_); - return result; - } - + // See try_dequeue() for reasoning + + Block* frontBlock_ = frontBlock.load(); + size_t blockTail = frontBlock_->localTail; + size_t blockFront = frontBlock_->front.load(); + + if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) { + fence(memory_order_acquire); + + non_empty_front_block: + auto element = reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); + element->~T(); + + blockFront = (blockFront + 1) & frontBlock_->sizeMask; + + fence(memory_order_release); + frontBlock_->front = blockFront; + } else if (frontBlock_ != tailBlock.load()) { + fence(memory_order_acquire); + frontBlock_ = frontBlock.load(); + blockTail = frontBlock_->localTail = frontBlock_->tail.load(); + blockFront = frontBlock_->front.load(); + fence(memory_order_acquire); + + if (blockFront != blockTail) { + goto non_empty_front_block; + } + + // Front block is empty but there's another block ahead, advance to it + Block* nextBlock = frontBlock_->next; + + size_t nextBlockFront = nextBlock->front.load(); + size_t nextBlockTail = nextBlock->localTail = nextBlock->tail.load(); + fence(memory_order_acquire); + + assert(nextBlockFront != nextBlockTail); + AE_UNUSED(nextBlockTail); + + fence(memory_order_release); + frontBlock = frontBlock_ = nextBlock; + + compiler_fence(memory_order_release); + + auto element = reinterpret_cast(frontBlock_->data + nextBlockFront * sizeof(T)); + element->~T(); + + nextBlockFront = (nextBlockFront + 1) & frontBlock_->sizeMask; + + fence(memory_order_release); + frontBlock_->front = nextBlockFront; + } else { + // No elements in current block and no other block to advance to + return false; + } + + return true; + } + + // Returns the approximate number of items currently in the queue. + // Safe to call from both the producer and consumer threads. + inline size_t size_approx() const + { + size_t result = 0; + Block* frontBlock_ = frontBlock.load(); + Block* block = frontBlock_; + do { + fence(memory_order_acquire); + size_t blockFront = block->front.load(); + size_t blockTail = block->tail.load(); + result += (blockTail - blockFront) & block->sizeMask; + block = block->next.load(); + } while (block != frontBlock_); + return result; + } private: - enum AllocationMode { CanAlloc, CannotAlloc }; + enum AllocationMode { CanAlloc, + CannotAlloc }; - template - bool inner_enqueue(U&& element) - { + template + bool inner_enqueue(U&& element) + { #ifndef NDEBUG - ReentrantGuard guard(this->enqueuing); + ReentrantGuard guard(this->enqueuing); #endif - // High-level pseudocode (assuming we're allowed to alloc a new block): - // If room in tail block, add to tail - // Else check next block - // If next block is not the head block, enqueue on next block - // Else create a new block and enqueue there - // Advance tail to the block we just enqueued to - - Block* tailBlock_ = tailBlock.load(); - size_t blockFront = tailBlock_->localFront; - size_t blockTail = tailBlock_->tail.load(); - - size_t nextBlockTail = (blockTail + 1) & tailBlock_->sizeMask; - if (nextBlockTail != blockFront || nextBlockTail != (tailBlock_->localFront = tailBlock_->front.load())) { - fence(memory_order_acquire); - // This block has room for at least one more element - char* location = tailBlock_->data + blockTail * sizeof(T); - new (location) T(std::forward(element)); - - fence(memory_order_release); - tailBlock_->tail = nextBlockTail; - } - else { - fence(memory_order_acquire); - if (tailBlock_->next.load() != frontBlock) { - // Note that the reason we can't advance to the frontBlock and start adding new entries there - // is because if we did, then dequeue would stay in that block, eventually reading the new values, - // instead of advancing to the next full block (whose values were enqueued first and so should be - // consumed first). - - fence(memory_order_acquire); // Ensure we get latest writes if we got the latest frontBlock - - // tailBlock is full, but there's a free block ahead, use it - Block* tailBlockNext = tailBlock_->next.load(); - size_t nextBlockFront = tailBlockNext->localFront = tailBlockNext->front.load(); - nextBlockTail = tailBlockNext->tail.load(); - fence(memory_order_acquire); - - // This block must be empty since it's not the head block and we - // go through the blocks in a circle - assert(nextBlockFront == nextBlockTail); - tailBlockNext->localFront = nextBlockFront; - - char* location = tailBlockNext->data + nextBlockTail * sizeof(T); - new (location) T(std::forward(element)); - - tailBlockNext->tail = (nextBlockTail + 1) & tailBlockNext->sizeMask; - - fence(memory_order_release); - tailBlock = tailBlockNext; - } - else if (canAlloc == CanAlloc) { - // tailBlock is full and there's no free block ahead; create a new block - auto newBlockSize = largestBlockSize >= MAX_BLOCK_SIZE ? largestBlockSize : largestBlockSize * 2; - auto newBlock = make_block(newBlockSize); - if (newBlock == nullptr) { - // Could not allocate a block! - return false; - } - largestBlockSize = newBlockSize; - - new (newBlock->data) T(std::forward(element)); - - assert(newBlock->front == 0); - newBlock->tail = newBlock->localTail = 1; - - newBlock->next = tailBlock_->next.load(); - tailBlock_->next = newBlock; - - // Might be possible for the dequeue thread to see the new tailBlock->next - // *without* seeing the new tailBlock value, but this is OK since it can't - // advance to the next block until tailBlock is set anyway (because the only - // case where it could try to read the next is if it's already at the tailBlock, - // and it won't advance past tailBlock in any circumstance). - - fence(memory_order_release); - tailBlock = newBlock; - } - else if (canAlloc == CannotAlloc) { - // Would have had to allocate a new block to enqueue, but not allowed - return false; - } - else { - assert(false && "Should be unreachable code"); - return false; - } - } - - return true; - } - - - // Disable copying - ReaderWriterQueue(ReaderWriterQueue const&) { } - - // Disable assignment - ReaderWriterQueue& operator=(ReaderWriterQueue const&) { } - - - - AE_FORCEINLINE static size_t ceilToPow2(size_t x) - { - // From http://graphics.stanford.edu/~seander/bithacks.html#RoundUpPowerOf2 - --x; - x |= x >> 1; - x |= x >> 2; - x |= x >> 4; - for (size_t i = 1; i < sizeof(size_t); i <<= 1) { - x |= x >> (i << 3); - } - ++x; - return x; - } - - template - static AE_FORCEINLINE char* align_for(char* ptr) - { - const std::size_t alignment = std::alignment_of::value; - return ptr + (alignment - (reinterpret_cast(ptr) % alignment)) % alignment; - } + // High-level pseudocode (assuming we're allowed to alloc a new block): + // If room in tail block, add to tail + // Else check next block + // If next block is not the head block, enqueue on next block + // Else create a new block and enqueue there + // Advance tail to the block we just enqueued to + + Block* tailBlock_ = tailBlock.load(); + size_t blockFront = tailBlock_->localFront; + size_t blockTail = tailBlock_->tail.load(); + + size_t nextBlockTail = (blockTail + 1) & tailBlock_->sizeMask; + if (nextBlockTail != blockFront || nextBlockTail != (tailBlock_->localFront = tailBlock_->front.load())) { + fence(memory_order_acquire); + // This block has room for at least one more element + char* location = tailBlock_->data + blockTail * sizeof(T); + new (location) T(std::forward(element)); + + fence(memory_order_release); + tailBlock_->tail = nextBlockTail; + } else { + fence(memory_order_acquire); + if (tailBlock_->next.load() != frontBlock) { + // Note that the reason we can't advance to the frontBlock and start adding new entries there + // is because if we did, then dequeue would stay in that block, eventually reading the new values, + // instead of advancing to the next full block (whose values were enqueued first and so should be + // consumed first). + + fence(memory_order_acquire); // Ensure we get latest writes if we got the latest frontBlock + + // tailBlock is full, but there's a free block ahead, use it + Block* tailBlockNext = tailBlock_->next.load(); + size_t nextBlockFront = tailBlockNext->localFront = tailBlockNext->front.load(); + nextBlockTail = tailBlockNext->tail.load(); + fence(memory_order_acquire); + + // This block must be empty since it's not the head block and we + // go through the blocks in a circle + assert(nextBlockFront == nextBlockTail); + tailBlockNext->localFront = nextBlockFront; + + char* location = tailBlockNext->data + nextBlockTail * sizeof(T); + new (location) T(std::forward(element)); + + tailBlockNext->tail = (nextBlockTail + 1) & tailBlockNext->sizeMask; + + fence(memory_order_release); + tailBlock = tailBlockNext; + } else if (canAlloc == CanAlloc) { + // tailBlock is full and there's no free block ahead; create a new block + auto newBlockSize = largestBlockSize >= MAX_BLOCK_SIZE ? largestBlockSize : largestBlockSize * 2; + auto newBlock = make_block(newBlockSize); + if (newBlock == nullptr) { + // Could not allocate a block! + return false; + } + largestBlockSize = newBlockSize; + + new (newBlock->data) T(std::forward(element)); + + assert(newBlock->front == 0); + newBlock->tail = newBlock->localTail = 1; + + newBlock->next = tailBlock_->next.load(); + tailBlock_->next = newBlock; + + // Might be possible for the dequeue thread to see the new tailBlock->next + // *without* seeing the new tailBlock value, but this is OK since it can't + // advance to the next block until tailBlock is set anyway (because the only + // case where it could try to read the next is if it's already at the tailBlock, + // and it won't advance past tailBlock in any circumstance). + + fence(memory_order_release); + tailBlock = newBlock; + } else if (canAlloc == CannotAlloc) { + // Would have had to allocate a new block to enqueue, but not allowed + return false; + } else { + assert(false && "Should be unreachable code"); + return false; + } + } + + return true; + } + + // Disable copying + ReaderWriterQueue(ReaderWriterQueue const&) {} + + // Disable assignment + ReaderWriterQueue& operator=(ReaderWriterQueue const&) {} + + AE_FORCEINLINE static size_t ceilToPow2(size_t x) + { + // From http://graphics.stanford.edu/~seander/bithacks.html#RoundUpPowerOf2 + --x; + x |= x >> 1; + x |= x >> 2; + x |= x >> 4; + for (size_t i = 1; i < sizeof(size_t); i <<= 1) { + x |= x >> (i << 3); + } + ++x; + return x; + } + + template + static AE_FORCEINLINE char* align_for(char* ptr) + { + const std::size_t alignment = std::alignment_of::value; + return ptr + (alignment - (reinterpret_cast(ptr) % alignment)) % alignment; + } + private: #ifndef NDEBUG - struct ReentrantGuard - { - ReentrantGuard(bool& _inSection) - : inSection(_inSection) - { - assert(!inSection && "ReaderWriterQueue does not support enqueuing or dequeuing elements from other elements' ctors and dtors"); - inSection = true; - } - - ~ReentrantGuard() { inSection = false; } - - private: - ReentrantGuard& operator=(ReentrantGuard const&); - - private: - bool& inSection; - }; + struct ReentrantGuard { + ReentrantGuard(bool& _inSection) + : inSection(_inSection) + { + assert(!inSection && "ReaderWriterQueue does not support enqueuing or dequeuing elements from other elements' ctors and dtors"); + inSection = true; + } + + ~ReentrantGuard() { inSection = false; } + + private: + ReentrantGuard& operator=(ReentrantGuard const&); + + private: + bool& inSection; + }; #endif - struct Block - { - // Avoid false-sharing by putting highly contended variables on their own cache lines - weak_atomic front; // (Atomic) Elements are read from here - size_t localTail; // An uncontended shadow copy of tail, owned by the consumer - - char cachelineFiller0[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic) - sizeof(size_t)]; - weak_atomic tail; // (Atomic) Elements are enqueued here - size_t localFront; - - char cachelineFiller1[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic) - sizeof(size_t)]; // next isn't very contended, but we don't want it on the same cache line as tail (which is) - weak_atomic next; // (Atomic) - - char* data; // Contents (on heap) are aligned to T's alignment - - 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) - { - } - - private: - // C4512 - Assignment operator could not be generated - Block& operator=(Block const&); - - public: - char* rawThis; - }; - - - static Block* make_block(size_t capacity) - { - // Allocate enough memory for the block itself, as well as all the elements it will contain - auto size = sizeof(Block) + std::alignment_of::value - 1; - size += sizeof(T) * capacity + std::alignment_of::value - 1; - auto newBlockRaw = static_cast(std::malloc(size)); - if (newBlockRaw == nullptr) { - return nullptr; - } - - auto newBlockAligned = align_for(newBlockRaw); - auto newBlockData = align_for(newBlockAligned + sizeof(Block)); - return new (newBlockAligned) Block(capacity, newBlockRaw, newBlockData); - } + struct Block { + // Avoid false-sharing by putting highly contended variables on their own cache lines + weak_atomic front; // (Atomic) Elements are read from here + size_t localTail; // An uncontended shadow copy of tail, owned by the consumer + + char cachelineFiller0[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic) - sizeof(size_t)]; + weak_atomic tail; // (Atomic) Elements are enqueued here + size_t localFront; + + char cachelineFiller1[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic) - sizeof(size_t)]; // next isn't very contended, but we don't want it on the same cache line as tail (which is) + weak_atomic next; // (Atomic) + + char* data; // Contents (on heap) are aligned to T's alignment + + 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) + { + } + + private: + // C4512 - Assignment operator could not be generated + Block& operator=(Block const&); + + public: + char* rawThis; + }; + + static Block* make_block(size_t capacity) + { + // Allocate enough memory for the block itself, as well as all the elements it will contain + auto size = sizeof(Block) + std::alignment_of::value - 1; + size += sizeof(T) * capacity + std::alignment_of::value - 1; + auto newBlockRaw = static_cast(std::malloc(size)); + if (newBlockRaw == nullptr) { + return nullptr; + } + + auto newBlockAligned = align_for(newBlockRaw); + auto newBlockData = align_for(newBlockAligned + sizeof(Block)); + return new (newBlockAligned) Block(capacity, newBlockRaw, newBlockData); + } private: - weak_atomic frontBlock; // (Atomic) Elements are enqueued to this block - - char cachelineFiller[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic)]; - weak_atomic tailBlock; // (Atomic) Elements are dequeued from this block + weak_atomic frontBlock; // (Atomic) Elements are enqueued to this block - size_t largestBlockSize; + char cachelineFiller[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic)]; + weak_atomic tailBlock; // (Atomic) Elements are dequeued from this block + + size_t largestBlockSize; #ifndef NDEBUG - bool enqueuing; - bool dequeuing; + bool enqueuing; + bool dequeuing; #endif }; // Like ReaderWriterQueue, but also providees blocking operations -template -class BlockingReaderWriterQueue -{ +template +class BlockingReaderWriterQueue { private: - typedef ::moodycamel::ReaderWriterQueue ReaderWriterQueue; - -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. - // Does not allocate memory. - AE_FORCEINLINE bool try_enqueue(T const& element) - { - if (inner.try_enqueue(element)) { - sema.signal(); - return true; - } - return false; - } - - // Enqueues a moved copy of element if there is room in the queue. - // Returns true if the element was enqueued, false otherwise. - // Does not allocate memory. - AE_FORCEINLINE bool try_enqueue(T&& element) - { - if (inner.try_enqueue(std::forward(element))) { - sema.signal(); - return true; - } - return false; - } - - - // Enqueues a copy of element on the queue. - // Allocates an additional block of memory if needed. - // Only fails (returns false) if memory allocation fails. - AE_FORCEINLINE bool enqueue(T const& element) - { - if (inner.enqueue(element)) { - sema.signal(); - return true; - } - return false; - } - - // Enqueues a moved copy of element on the queue. - // Allocates an additional block of memory if needed. - // Only fails (returns false) if memory allocation fails. - AE_FORCEINLINE bool enqueue(T&& element) - { - if (inner.enqueue(std::forward(element))) { - sema.signal(); - return true; - } - return false; - } - - - // Attempts to dequeue an element; if the queue is empty, - // returns false instead. If the queue has at least one element, - // moves front to result using operator=, then returns true. - template - bool try_dequeue(U& result) - { - if (sema.tryWait()) { - bool success = inner.try_dequeue(result); - assert(success); - AE_UNUSED(success); - return true; - } - return false; - } - - - // Attempts to dequeue an element; if the queue is empty, - // waits until an element is available, then dequeues it. - template - void wait_dequeue(U& result) - { - sema.wait(); - bool success = inner.try_dequeue(result); - AE_UNUSED(result); - assert(success); - AE_UNUSED(success); - } - - - // Attempts to dequeue an element; if the queue is empty, - // waits until an element is available up to the specified timeout, - // then dequeues it and returns true, or returns false if the timeout - // expires before an element can be dequeued. - // Using a negative timeout indicates an indefinite timeout, - // and is thus functionally equivalent to calling wait_dequeue. - template - bool wait_dequeue_timed(U& result, std::int64_t timeout_usecs) - { - if (!sema.wait(timeout_usecs)) { - return false; - } - bool success = inner.try_dequeue(result); - AE_UNUSED(result); - assert(success); - AE_UNUSED(success); - return true; - } + typedef ::moodycamel::ReaderWriterQueue 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. + // Does not allocate memory. + AE_FORCEINLINE bool try_enqueue(T const& element) + { + if (inner.try_enqueue(element)) { + sema.signal(); + return true; + } + return false; + } + + // Enqueues a moved copy of element if there is room in the queue. + // Returns true if the element was enqueued, false otherwise. + // Does not allocate memory. + AE_FORCEINLINE bool try_enqueue(T&& element) + { + if (inner.try_enqueue(std::forward(element))) { + sema.signal(); + return true; + } + return false; + } + + // Enqueues a copy of element on the queue. + // Allocates an additional block of memory if needed. + // Only fails (returns false) if memory allocation fails. + AE_FORCEINLINE bool enqueue(T const& element) + { + if (inner.enqueue(element)) { + sema.signal(); + return true; + } + return false; + } + + // Enqueues a moved copy of element on the queue. + // Allocates an additional block of memory if needed. + // Only fails (returns false) if memory allocation fails. + AE_FORCEINLINE bool enqueue(T&& element) + { + if (inner.enqueue(std::forward(element))) { + sema.signal(); + return true; + } + return false; + } + + // Attempts to dequeue an element; if the queue is empty, + // returns false instead. If the queue has at least one element, + // moves front to result using operator=, then returns true. + template + bool try_dequeue(U& result) + { + if (sema.tryWait()) { + bool success = inner.try_dequeue(result); + assert(success); + AE_UNUSED(success); + return true; + } + return false; + } + + // Attempts to dequeue an element; if the queue is empty, + // waits until an element is available, then dequeues it. + template + void wait_dequeue(U& result) + { + sema.wait(); + bool success = inner.try_dequeue(result); + AE_UNUSED(result); + assert(success); + AE_UNUSED(success); + } + + // Attempts to dequeue an element; if the queue is empty, + // waits until an element is available up to the specified timeout, + // then dequeues it and returns true, or returns false if the timeout + // expires before an element can be dequeued. + // Using a negative timeout indicates an indefinite timeout, + // and is thus functionally equivalent to calling wait_dequeue. + template + bool wait_dequeue_timed(U& result, std::int64_t timeout_usecs) + { + if (!sema.wait(timeout_usecs)) { + return false; + } + bool success = inner.try_dequeue(result); + AE_UNUSED(result); + assert(success); + AE_UNUSED(success); + return true; + } #if __cplusplus > 199711L || _MSC_VER >= 1700 - // Attempts to dequeue an element; if the queue is empty, - // waits until an element is available up to the specified timeout, - // then dequeues it and returns true, or returns false if the timeout - // expires before an element can be dequeued. - // Using a negative timeout indicates an indefinite timeout, - // and is thus functionally equivalent to calling wait_dequeue. - template - inline bool wait_dequeue_timed(U& result, std::chrono::duration const& timeout) - { + // Attempts to dequeue an element; if the queue is empty, + // waits until an element is available up to the specified timeout, + // then dequeues it and returns true, or returns false if the timeout + // expires before an element can be dequeued. + // Using a negative timeout indicates an indefinite timeout, + // and is thus functionally equivalent to calling wait_dequeue. + template + inline bool wait_dequeue_timed(U& result, std::chrono::duration const& timeout) + { return wait_dequeue_timed(result, std::chrono::duration_cast(timeout).count()); - } + } #endif - - // Returns a pointer to the front element in the queue (the one that - // would be removed next by a call to `try_dequeue` or `pop`). If the - // queue appears empty at the time the method is called, nullptr is - // returned instead. - // Must be called only from the consumer thread. - AE_FORCEINLINE T* peek() - { - return inner.peek(); - } - - // Removes the front element from the queue, if any, without returning it. - // Returns true on success, or false if the queue appeared empty at the time - // `pop` was called. - AE_FORCEINLINE bool pop() - { - if (sema.tryWait()) { - bool result = inner.pop(); - assert(result); - AE_UNUSED(result); - return true; - } - return false; - } - - // Returns the approximate number of items currently in the queue. - // Safe to call from both the producer and consumer threads. - AE_FORCEINLINE size_t size_approx() const - { - return sema.availableApprox(); - } - + // Returns a pointer to the front element in the queue (the one that + // would be removed next by a call to `try_dequeue` or `pop`). If the + // queue appears empty at the time the method is called, nullptr is + // returned instead. + // Must be called only from the consumer thread. + AE_FORCEINLINE T* peek() + { + return inner.peek(); + } + + // Removes the front element from the queue, if any, without returning it. + // Returns true on success, or false if the queue appeared empty at the time + // `pop` was called. + AE_FORCEINLINE bool pop() + { + if (sema.tryWait()) { + bool result = inner.pop(); + assert(result); + AE_UNUSED(result); + return true; + } + return false; + } + + // Returns the approximate number of items currently in the queue. + // Safe to call from both the producer and consumer threads. + AE_FORCEINLINE size_t size_approx() const + { + return sema.availableApprox(); + } private: - // Disable copying & assignment - BlockingReaderWriterQueue(ReaderWriterQueue const&) { } - BlockingReaderWriterQueue& operator=(ReaderWriterQueue const&) { } - + // Disable copying & assignment + BlockingReaderWriterQueue(ReaderWriterQueue const&) {} + BlockingReaderWriterQueue& operator=(ReaderWriterQueue const&) {} + private: - ReaderWriterQueue inner; - spsc_sema::LightweightSemaphore sema; + ReaderWriterQueue inner; + spsc_sema::LightweightSemaphore sema; }; -} // end namespace moodycamel +} // end namespace moodycamel #ifdef AE_VCPP #pragma warning(pop) diff --git a/include/ur_modern_driver/robot_state.h b/include/ur_modern_driver/robot_state.h index 159974bd1..a35da4486 100644 --- a/include/ur_modern_driver/robot_state.h +++ b/include/ur_modern_driver/robot_state.h @@ -19,200 +19,202 @@ #ifndef ROBOT_STATE_H_ #define ROBOT_STATE_H_ +#include #include -#include -#include -#include #include -#include #include +#include +#include +#include namespace message_types { enum message_type { - ROBOT_STATE = 16, ROBOT_MESSAGE = 20, PROGRAM_STATE_MESSAGE = 25 + ROBOT_STATE = 16, + ROBOT_MESSAGE = 20, + PROGRAM_STATE_MESSAGE = 25 }; } typedef message_types::message_type messageType; namespace package_types { enum package_type { - ROBOT_MODE_DATA = 0, - JOINT_DATA = 1, - TOOL_DATA = 2, - MASTERBOARD_DATA = 3, - CARTESIAN_INFO = 4, - KINEMATICS_INFO = 5, - CONFIGURATION_DATA = 6, - FORCE_MODE_DATA = 7, - ADDITIONAL_INFO = 8, - CALIBRATION_DATA = 9 + ROBOT_MODE_DATA = 0, + JOINT_DATA = 1, + TOOL_DATA = 2, + MASTERBOARD_DATA = 3, + CARTESIAN_INFO = 4, + KINEMATICS_INFO = 5, + CONFIGURATION_DATA = 6, + FORCE_MODE_DATA = 7, + ADDITIONAL_INFO = 8, + CALIBRATION_DATA = 9 }; } typedef package_types::package_type packageType; namespace robot_message_types { enum robot_message_type { - ROBOT_MESSAGE_TEXT = 0, - ROBOT_MESSAGE_PROGRAM_LABEL = 1, - PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2, - ROBOT_MESSAGE_VERSION = 3, - ROBOT_MESSAGE_SAFETY_MODE = 5, - ROBOT_MESSAGE_ERROR_CODE = 6, - ROBOT_MESSAGE_KEY = 7, - ROBOT_MESSAGE_REQUEST_VALUE = 9, - ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10 + ROBOT_MESSAGE_TEXT = 0, + ROBOT_MESSAGE_PROGRAM_LABEL = 1, + PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2, + ROBOT_MESSAGE_VERSION = 3, + ROBOT_MESSAGE_SAFETY_MODE = 5, + ROBOT_MESSAGE_ERROR_CODE = 6, + ROBOT_MESSAGE_KEY = 7, + ROBOT_MESSAGE_REQUEST_VALUE = 9, + ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10 }; } typedef robot_message_types::robot_message_type robotMessageType; namespace robot_state_type_v18 { enum robot_state_type { - ROBOT_RUNNING_MODE = 0, - ROBOT_FREEDRIVE_MODE = 1, - ROBOT_READY_MODE = 2, - ROBOT_INITIALIZING_MODE = 3, - ROBOT_SECURITY_STOPPED_MODE = 4, - ROBOT_EMERGENCY_STOPPED_MODE = 5, - ROBOT_FATAL_ERROR_MODE = 6, - ROBOT_NO_POWER_MODE = 7, - ROBOT_NOT_CONNECTED_MODE = 8, - ROBOT_SHUTDOWN_MODE = 9, - ROBOT_SAFEGUARD_STOP_MODE = 10 + ROBOT_RUNNING_MODE = 0, + ROBOT_FREEDRIVE_MODE = 1, + ROBOT_READY_MODE = 2, + ROBOT_INITIALIZING_MODE = 3, + ROBOT_SECURITY_STOPPED_MODE = 4, + ROBOT_EMERGENCY_STOPPED_MODE = 5, + ROBOT_FATAL_ERROR_MODE = 6, + ROBOT_NO_POWER_MODE = 7, + ROBOT_NOT_CONNECTED_MODE = 8, + ROBOT_SHUTDOWN_MODE = 9, + ROBOT_SAFEGUARD_STOP_MODE = 10 }; } typedef robot_state_type_v18::robot_state_type robotStateTypeV18; namespace robot_state_type_v30 { enum robot_state_type { - ROBOT_MODE_DISCONNECTED = 0, - ROBOT_MODE_CONFIRM_SAFETY = 1, - ROBOT_MODE_BOOTING = 2, - ROBOT_MODE_POWER_OFF = 3, - ROBOT_MODE_POWER_ON = 4, - ROBOT_MODE_IDLE = 5, - ROBOT_MODE_BACKDRIVE = 6, - ROBOT_MODE_RUNNING = 7, - ROBOT_MODE_UPDATING_FIRMWARE = 8 + ROBOT_MODE_DISCONNECTED = 0, + ROBOT_MODE_CONFIRM_SAFETY = 1, + ROBOT_MODE_BOOTING = 2, + ROBOT_MODE_POWER_OFF = 3, + ROBOT_MODE_POWER_ON = 4, + ROBOT_MODE_IDLE = 5, + ROBOT_MODE_BACKDRIVE = 6, + ROBOT_MODE_RUNNING = 7, + ROBOT_MODE_UPDATING_FIRMWARE = 8 }; } typedef robot_state_type_v30::robot_state_type robotStateTypeV30; struct version_message { - uint64_t timestamp; - int8_t source; - int8_t robot_message_type; - int8_t project_name_size; - char project_name[15]; - uint8_t major_version; - uint8_t minor_version; - int svn_revision; - char build_date[25]; + uint64_t timestamp; + int8_t source; + int8_t robot_message_type; + int8_t project_name_size; + char project_name[15]; + uint8_t major_version; + uint8_t minor_version; + int svn_revision; + char build_date[25]; }; struct masterboard_data { - int digitalInputBits; - int digitalOutputBits; - char analogInputRange0; - char analogInputRange1; - double analogInput0; - double analogInput1; - char analogOutputDomain0; - char analogOutputDomain1; - double analogOutput0; - double analogOutput1; - float masterBoardTemperature; - float robotVoltage48V; - float robotCurrent; - float masterIOCurrent; - unsigned char safetyMode; - unsigned char masterOnOffState; - char euromap67InterfaceInstalled; - int euromapInputBits; - int euromapOutputBits; - float euromapVoltage; - float euromapCurrent; + int digitalInputBits; + int digitalOutputBits; + char analogInputRange0; + char analogInputRange1; + double analogInput0; + double analogInput1; + char analogOutputDomain0; + char analogOutputDomain1; + double analogOutput0; + double analogOutput1; + float masterBoardTemperature; + float robotVoltage48V; + float robotCurrent; + float masterIOCurrent; + unsigned char safetyMode; + unsigned char masterOnOffState; + char euromap67InterfaceInstalled; + int euromapInputBits; + int euromapOutputBits; + float euromapVoltage; + float euromapCurrent; }; struct robot_mode_data { - uint64_t timestamp; - bool isRobotConnected; - bool isRealRobotEnabled; - bool isPowerOnRobot; - bool isEmergencyStopped; - bool isProtectiveStopped; - bool isProgramRunning; - bool isProgramPaused; - unsigned char robotMode; - unsigned char controlMode; - double targetSpeedFraction; - double speedScaling; + uint64_t timestamp; + bool isRobotConnected; + bool isRealRobotEnabled; + bool isPowerOnRobot; + bool isEmergencyStopped; + bool isProtectiveStopped; + bool isProgramRunning; + bool isProgramPaused; + unsigned char robotMode; + unsigned char controlMode; + double targetSpeedFraction; + double speedScaling; }; class RobotState { private: - version_message version_msg_; - masterboard_data mb_data_; - robot_mode_data robot_mode_; + version_message version_msg_; + masterboard_data mb_data_; + robot_mode_data robot_mode_; - std::recursive_mutex val_lock_; // Locks the variables while unpack parses data; + std::recursive_mutex val_lock_; // Locks the variables while unpack parses data; - std::condition_variable* pMsg_cond_; //Signals that new vars are available - bool new_data_available_; //to avoid spurious wakes - unsigned char robot_mode_running_; + std::condition_variable* pMsg_cond_; //Signals that new vars are available + bool new_data_available_; //to avoid spurious wakes + unsigned char robot_mode_running_; - double ntohd(uint64_t nf); + double ntohd(uint64_t nf); public: - RobotState(std::condition_variable& msg_cond); - ~RobotState(); - double getVersion(); - double getTime(); - std::vector getQTarget(); - int getDigitalInputBits(); - int getDigitalOutputBits(); - char getAnalogInputRange0(); - char getAnalogInputRange1(); - double getAnalogInput0(); - double getAnalogInput1(); - char getAnalogOutputDomain0(); - char getAnalogOutputDomain1(); - double getAnalogOutput0(); - double getAnalogOutput1(); - std::vector getVActual(); - float getMasterBoardTemperature(); - float getRobotVoltage48V(); - float getRobotCurrent(); - float getMasterIOCurrent(); - unsigned char getSafetyMode(); - unsigned char getInReducedMode(); - char getEuromap67InterfaceInstalled(); - int getEuromapInputBits(); - int getEuromapOutputBits(); - float getEuromapVoltage(); - float getEuromapCurrent(); - - bool isRobotConnected(); - bool isRealRobotEnabled(); - bool isPowerOnRobot(); - bool isEmergencyStopped(); - bool isProtectiveStopped(); - bool isProgramRunning(); - bool isProgramPaused(); - unsigned char getRobotMode(); - bool isReady(); - - void setDisconnected(); - - bool getNewDataAvailable(); - void finishedReading(); - - void unpack(uint8_t * buf, unsigned int buf_length); - void unpackRobotMessage(uint8_t * buf, unsigned int offset, uint32_t len); - void unpackRobotMessageVersion(uint8_t * buf, unsigned int offset, - uint32_t len); - void unpackRobotState(uint8_t * buf, unsigned int offset, uint32_t len); - void unpackRobotStateMasterboard(uint8_t * buf, unsigned int offset); - void unpackRobotMode(uint8_t * buf, unsigned int offset); + RobotState(std::condition_variable& msg_cond); + ~RobotState(); + double getVersion(); + double getTime(); + std::vector getQTarget(); + int getDigitalInputBits(); + int getDigitalOutputBits(); + char getAnalogInputRange0(); + char getAnalogInputRange1(); + double getAnalogInput0(); + double getAnalogInput1(); + char getAnalogOutputDomain0(); + char getAnalogOutputDomain1(); + double getAnalogOutput0(); + double getAnalogOutput1(); + std::vector getVActual(); + float getMasterBoardTemperature(); + float getRobotVoltage48V(); + float getRobotCurrent(); + float getMasterIOCurrent(); + unsigned char getSafetyMode(); + unsigned char getInReducedMode(); + char getEuromap67InterfaceInstalled(); + int getEuromapInputBits(); + int getEuromapOutputBits(); + float getEuromapVoltage(); + float getEuromapCurrent(); + + bool isRobotConnected(); + bool isRealRobotEnabled(); + bool isPowerOnRobot(); + bool isEmergencyStopped(); + bool isProtectiveStopped(); + bool isProgramRunning(); + bool isProgramPaused(); + unsigned char getRobotMode(); + bool isReady(); + + void setDisconnected(); + + bool getNewDataAvailable(); + void finishedReading(); + + void unpack(uint8_t* buf, unsigned int buf_length); + void unpackRobotMessage(uint8_t* buf, unsigned int offset, uint32_t len); + void unpackRobotMessageVersion(uint8_t* buf, unsigned int offset, + uint32_t len); + void unpackRobotState(uint8_t* buf, unsigned int offset, uint32_t len); + void unpackRobotStateMasterboard(uint8_t* buf, unsigned int offset); + void unpackRobotMode(uint8_t* buf, unsigned int offset); }; #endif /* ROBOT_STATE_H_ */ diff --git a/include/ur_modern_driver/robot_state_RT.h b/include/ur_modern_driver/robot_state_RT.h index 1f5b2af6e..3d3b2e890 100644 --- a/include/ur_modern_driver/robot_state_RT.h +++ b/include/ur_modern_driver/robot_state_RT.h @@ -19,98 +19,98 @@ #ifndef ROBOT_STATE_RT_H_ #define ROBOT_STATE_RT_H_ +#include #include -#include -#include -#include #include #include -#include +#include +#include +#include class RobotStateRT { private: - double version_; //protocol version + double version_; //protocol version - double time_; //Time elapsed since the controller was started - std::vector q_target_; //Target joint positions - std::vector qd_target_; //Target joint velocities - std::vector qdd_target_; //Target joint accelerations - std::vector i_target_; //Target joint currents - std::vector m_target_; //Target joint moments (torques) - std::vector q_actual_; //Actual joint positions - std::vector qd_actual_; //Actual joint velocities - std::vector i_actual_; //Actual joint currents - std::vector i_control_; //Joint control currents - std::vector tool_vector_actual_; //Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation - std::vector tcp_speed_actual_; //Actual speed of the tool given in Cartesian coordinates - std::vector tcp_force_; //Generalised forces in the TC - std::vector tool_vector_target_; //Target Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation - std::vector tcp_speed_target_; //Target speed of the tool given in Cartesian coordinates - std::vector digital_input_bits_; //Current state of the digital inputs. NOTE: these are bits encoded as int64_t, e.g. a value of 5 corresponds to bit 0 and bit 2 set high - std::vector motor_temperatures_; //Temperature of each joint in degrees celsius - double controller_timer_; //Controller realtime thread execution time - double robot_mode_; //Robot mode - std::vector joint_modes_; //Joint control modes - double safety_mode_; //Safety mode - std::vector tool_accelerometer_values_; //Tool x,y and z accelerometer values (software version 1.7) - double speed_scaling_; //Speed scaling of the trajectory limiter - double linear_momentum_norm_; //Norm of Cartesian linear momentum - double v_main_; //Masterboard: Main voltage - double v_robot_; //Matorborad: Robot voltage (48V) - double i_robot_; //Masterboard: Robot current - std::vector v_actual_; //Actual joint voltages + double time_; //Time elapsed since the controller was started + std::vector q_target_; //Target joint positions + std::vector qd_target_; //Target joint velocities + std::vector qdd_target_; //Target joint accelerations + std::vector i_target_; //Target joint currents + std::vector m_target_; //Target joint moments (torques) + std::vector q_actual_; //Actual joint positions + std::vector qd_actual_; //Actual joint velocities + std::vector i_actual_; //Actual joint currents + std::vector i_control_; //Joint control currents + std::vector tool_vector_actual_; //Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation + std::vector tcp_speed_actual_; //Actual speed of the tool given in Cartesian coordinates + std::vector tcp_force_; //Generalised forces in the TC + std::vector tool_vector_target_; //Target Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation + std::vector tcp_speed_target_; //Target speed of the tool given in Cartesian coordinates + std::vector digital_input_bits_; //Current state of the digital inputs. NOTE: these are bits encoded as int64_t, e.g. a value of 5 corresponds to bit 0 and bit 2 set high + std::vector motor_temperatures_; //Temperature of each joint in degrees celsius + double controller_timer_; //Controller realtime thread execution time + double robot_mode_; //Robot mode + std::vector joint_modes_; //Joint control modes + double safety_mode_; //Safety mode + std::vector tool_accelerometer_values_; //Tool x,y and z accelerometer values (software version 1.7) + double speed_scaling_; //Speed scaling of the trajectory limiter + double linear_momentum_norm_; //Norm of Cartesian linear momentum + double v_main_; //Masterboard: Main voltage + double v_robot_; //Matorborad: Robot voltage (48V) + double i_robot_; //Masterboard: Robot current + std::vector v_actual_; //Actual joint voltages - std::mutex val_lock_; // Locks the variables while unpack parses data; + std::mutex val_lock_; // Locks the variables while unpack parses data; - std::condition_variable* pMsg_cond_; //Signals that new vars are available - bool data_published_; //to avoid spurious wakes - bool controller_updated_; //to avoid spurious wakes + std::condition_variable* pMsg_cond_; //Signals that new vars are available + bool data_published_; //to avoid spurious wakes + bool controller_updated_; //to avoid spurious wakes - std::vector unpackVector(uint8_t * buf, int start_index, - int nr_of_vals); - std::vector unpackDigitalInputBits(int64_t data); - double ntohd(uint64_t nf); + std::vector unpackVector(uint8_t* buf, int start_index, + int nr_of_vals); + std::vector unpackDigitalInputBits(int64_t data); + double ntohd(uint64_t nf); public: - RobotStateRT(std::condition_variable& msg_cond); - ~RobotStateRT(); - double getVersion(); - double getTime(); - std::vector getQTarget(); - std::vector getQdTarget(); - std::vector getQddTarget(); - std::vector getITarget(); - std::vector getMTarget(); - std::vector getQActual(); - std::vector getQdActual(); - std::vector getIActual(); - std::vector getIControl(); - std::vector getToolVectorActual(); - std::vector getTcpSpeedActual(); - std::vector getTcpForce(); - std::vector getToolVectorTarget(); - std::vector getTcpSpeedTarget(); - std::vector getDigitalInputBits(); - std::vector getMotorTemperatures(); - double getControllerTimer(); - double getRobotMode(); - std::vector getJointModes(); - double getSafety_mode(); - std::vector getToolAccelerometerValues(); - double getSpeedScaling(); - double getLinearMomentumNorm(); - double getVMain(); - double getVRobot(); - double getIRobot(); + RobotStateRT(std::condition_variable& msg_cond); + ~RobotStateRT(); + double getVersion(); + double getTime(); + std::vector getQTarget(); + std::vector getQdTarget(); + std::vector getQddTarget(); + std::vector getITarget(); + std::vector getMTarget(); + std::vector getQActual(); + std::vector getQdActual(); + std::vector getIActual(); + std::vector getIControl(); + std::vector getToolVectorActual(); + std::vector getTcpSpeedActual(); + std::vector getTcpForce(); + std::vector getToolVectorTarget(); + std::vector getTcpSpeedTarget(); + std::vector getDigitalInputBits(); + std::vector getMotorTemperatures(); + double getControllerTimer(); + double getRobotMode(); + std::vector getJointModes(); + double getSafety_mode(); + std::vector getToolAccelerometerValues(); + double getSpeedScaling(); + double getLinearMomentumNorm(); + double getVMain(); + double getVRobot(); + double getIRobot(); - void setVersion(double ver); + void setVersion(double ver); - void setDataPublished(); - bool getDataPublished(); - bool getControllerUpdated(); - void setControllerUpdated(); - std::vector getVActual(); - void unpack(uint8_t * buf); + void setDataPublished(); + bool getDataPublished(); + bool getControllerUpdated(); + void setControllerUpdated(); + std::vector getVActual(); + void unpack(uint8_t* buf); }; #endif /* ROBOT_STATE_RT_H_ */ diff --git a/include/ur_modern_driver/ros/converter.h b/include/ur_modern_driver/ros/converter.h index 3f59c932d..6f70f09be 100644 --- a/include/ur_modern_driver/ros/converter.h +++ b/include/ur_modern_driver/ros/converter.h @@ -1,2 +1 @@ #pragma once - diff --git a/include/ur_modern_driver/ros/rt_publisher.h b/include/ur_modern_driver/ros/rt_publisher.h index 3289174d5..0737e61d8 100644 --- a/include/ur_modern_driver/ros/rt_publisher.h +++ b/include/ur_modern_driver/ros/rt_publisher.h @@ -1,12 +1,12 @@ #pragma once #include -#include -#include -#include -#include #include #include +#include +#include +#include +#include #include "ur_modern_driver/ur/consumer.h" @@ -26,34 +26,34 @@ class RTPublisher : public URRTPacketConsumer { NodeHandle _nh; Publisher _joint_pub; Publisher _wrench_pub; - Publisher _tool_vel_pub; + Publisher _tool_vel_pub; std::vector _joint_names; std::string _base_frame; - bool publish_joints(RTShared &packet, ros::Time &t); - bool publish_wrench(RTShared &packet, ros::Time &t); - bool publish_tool(RTShared &packet, ros::Time &t); + bool publish_joints(RTShared& packet, ros::Time& t); + bool publish_wrench(RTShared& packet, ros::Time& t); + bool publish_tool(RTShared& packet, ros::Time& t); - bool publish(RTShared &packet); + bool publish(RTShared& packet); public: - RTPublisher(std::string &joint_prefix, std::string &base_frame) : - _joint_pub(_nh.advertise("joint_states", 1)), - _wrench_pub(_nh.advertise("wrench", 1)), - _tool_vel_pub(_nh.advertise("tool_velocity", 1)), - _base_frame(base_frame) + RTPublisher(std::string& joint_prefix, std::string& base_frame) + : _joint_pub(_nh.advertise("joint_states", 1)) + , _wrench_pub(_nh.advertise("wrench", 1)) + , _tool_vel_pub(_nh.advertise("tool_velocity", 1)) + , _base_frame(base_frame) { - for(auto const& j : JOINTS) { + for (auto const& j : JOINTS) { _joint_names.push_back(joint_prefix + j); } } - virtual bool consume(RTState_V1_6__7 &state); - virtual bool consume(RTState_V1_8 &state); - virtual bool consume(RTState_V3_0__1 &state); - virtual bool consume(RTState_V3_2__3 &state); + virtual bool consume(RTState_V1_6__7& state); + virtual bool consume(RTState_V1_8& state); + virtual bool consume(RTState_V3_0__1& state); + virtual bool consume(RTState_V3_2__3& state); - virtual void setup_consumer() { } - virtual void teardown_consumer() { } - virtual void stop_consumer() { } + virtual void setup_consumer() {} + virtual void teardown_consumer() {} + virtual void stop_consumer() {} }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/consumer.h b/include/ur_modern_driver/ur/consumer.h index 5b2a373e8..655e8bbe9 100644 --- a/include/ur_modern_driver/ur/consumer.h +++ b/include/ur_modern_driver/ur/consumer.h @@ -1,46 +1,47 @@ #pragma once #include "ur_modern_driver/pipeline.h" -#include "ur_modern_driver/ur/state.h" #include "ur_modern_driver/ur/master_board.h" +#include "ur_modern_driver/ur/messages.h" #include "ur_modern_driver/ur/robot_mode.h" #include "ur_modern_driver/ur/rt_state.h" -#include "ur_modern_driver/ur/messages.h" +#include "ur_modern_driver/ur/state.h" class URRTPacketConsumer : public IConsumer { public: - virtual bool consume(unique_ptr packet) { + virtual bool consume(unique_ptr packet) + { return packet->consume_with(*this); } - virtual bool consume(RTState_V1_6__7 &state) = 0; - virtual bool consume(RTState_V1_8 &state) = 0; - virtual bool consume(RTState_V3_0__1 &state) = 0; - virtual bool consume(RTState_V3_2__3 &state) = 0; + virtual bool consume(RTState_V1_6__7& state) = 0; + virtual bool consume(RTState_V1_8& state) = 0; + virtual bool consume(RTState_V3_0__1& state) = 0; + virtual bool consume(RTState_V3_2__3& state) = 0; }; - class URStatePacketConsumer : public IConsumer { public: - virtual bool consume(unique_ptr packet) { + virtual bool consume(unique_ptr packet) + { return packet->consume_with(*this); } - virtual bool consume(MasterBoardData_V1_X &data) = 0; - virtual bool consume(MasterBoardData_V3_0__1 &data) = 0; - virtual bool consume(MasterBoardData_V3_2 &data) = 0; - - virtual bool consume(RobotModeData_V1_X &data) = 0; - virtual bool consume(RobotModeData_V3_0__1 &data) = 0; - virtual bool consume(RobotModeData_V3_2 &data) = 0; -}; + virtual bool consume(MasterBoardData_V1_X& data) = 0; + virtual bool consume(MasterBoardData_V3_0__1& data) = 0; + virtual bool consume(MasterBoardData_V3_2& data) = 0; + virtual bool consume(RobotModeData_V1_X& data) = 0; + virtual bool consume(RobotModeData_V3_0__1& data) = 0; + virtual bool consume(RobotModeData_V3_2& data) = 0; +}; class URMessagePacketConsumer : public IConsumer { public: - virtual bool consume(unique_ptr packet) { + virtual bool consume(unique_ptr packet) + { return packet->consume_with(*this); } - virtual bool consume(VersionMessage &message) = 0; + virtual bool consume(VersionMessage& message) = 0; }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/factory.h b/include/ur_modern_driver/ur/factory.h index a09186f40..d3266ea29 100644 --- a/include/ur_modern_driver/ur/factory.h +++ b/include/ur_modern_driver/ur/factory.h @@ -1,14 +1,13 @@ #pragma once -#include -#include "ur_modern_driver/ur/stream.h" #include "ur_modern_driver/ur/consumer.h" -#include "ur_modern_driver/ur/producer.h" +#include "ur_modern_driver/ur/messages_parser.h" #include "ur_modern_driver/ur/parser.h" -#include "ur_modern_driver/ur/state_parser.h" +#include "ur_modern_driver/ur/producer.h" #include "ur_modern_driver/ur/rt_parser.h" -#include "ur_modern_driver/ur/messages_parser.h" - +#include "ur_modern_driver/ur/state_parser.h" +#include "ur_modern_driver/ur/stream.h" +#include class URFactory : private URMessagePacketConsumer { private: @@ -18,7 +17,8 @@ class URFactory : private URMessagePacketConsumer { uint8_t _major_version; uint8_t _minor_version; - bool consume(VersionMessage &vm) { + bool consume(VersionMessage& vm) + { LOG_INFO("Got VersionMessage:"); LOG_INFO("project name: %s", vm.project_name.c_str()); LOG_INFO("version: %u.%u.%d", vm.major_version, vm.minor_version, vm.svn_version); @@ -26,31 +26,33 @@ class URFactory : private URMessagePacketConsumer { _major_version = vm.major_version; _minor_version = vm.minor_version; - + return true; } - void setup_consumer() { } - void teardown_consumer() { } - void stop_consumer() { } + void setup_consumer() {} + void teardown_consumer() {} + void stop_consumer() {} public: - URFactory(std::string &host) : _stream(host, 30001) { + URFactory(std::string& host) + : _stream(host, 30001) + { URProducer p(_stream, _parser); - std::vector> results; + std::vector > results; p.setup_producer(); - if(!p.try_get(results) || results.size() == 0) { - LOG_FATAL("No version message received, init failed!"); + if (!p.try_get(results) || results.size() == 0) { + LOG_FATAL("No version message received, init failed!"); std::exit(EXIT_FAILURE); } - - for(auto const& p : results) { + + for (auto const& p : results) { p->consume_with(*this); } - if(_major_version == 0 && _minor_version == 0) { + if (_major_version == 0 && _minor_version == 0) { LOG_FATAL("No version message received, init failed!"); std::exit(EXIT_FAILURE); } @@ -58,28 +60,30 @@ class URFactory : private URMessagePacketConsumer { p.teardown_producer(); } - std::unique_ptr> get_state_parser() { - if(_major_version == 1) { - return std::unique_ptr>(new URStateParser_V1_X); + std::unique_ptr > get_state_parser() + { + if (_major_version == 1) { + return std::unique_ptr >(new URStateParser_V1_X); } else { - if(_minor_version < 3) - return std::unique_ptr>(new URStateParser_V3_0__1); + if (_minor_version < 3) + return std::unique_ptr >(new URStateParser_V3_0__1); else - return std::unique_ptr>(new URStateParser_V3_2); + return std::unique_ptr >(new URStateParser_V3_2); } } - std::unique_ptr> get_rt_parser() { - if(_major_version == 1) { - if(_minor_version < 8) - return std::unique_ptr>(new URRTStateParser_V1_6__7); + std::unique_ptr > get_rt_parser() + { + if (_major_version == 1) { + if (_minor_version < 8) + return std::unique_ptr >(new URRTStateParser_V1_6__7); else - return std::unique_ptr>(new URRTStateParser_V1_8); + return std::unique_ptr >(new URRTStateParser_V1_8); } else { - if(_minor_version < 3) - return std::unique_ptr>(new URRTStateParser_V3_0__1); - else - return std::unique_ptr>(new URRTStateParser_V3_2__3); + if (_minor_version < 3) + return std::unique_ptr >(new URRTStateParser_V3_0__1); + else + return std::unique_ptr >(new URRTStateParser_V3_2__3); } } }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/master_board.h b/include/ur_modern_driver/ur/master_board.h index 0a7daf135..5f2339707 100644 --- a/include/ur_modern_driver/ur/master_board.h +++ b/include/ur_modern_driver/ur/master_board.h @@ -1,23 +1,22 @@ #pragma once -#include -#include -#include "ur_modern_driver/types.h" #include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/types.h" #include "ur_modern_driver/ur/state.h" - +#include +#include class SharedMasterBoardData { public: - virtual bool parse_with(BinParser &bp); - - int8_t analog_input_range0; + virtual bool parse_with(BinParser& bp); + + int8_t analog_input_range0; int8_t analog_input_range1; - double analog_input0; + double analog_input0; double analog_input1; - int8_t analog_output_domain0; + int8_t analog_output_domain0; int8_t analog_output_domain1; - double analog_output0; + double analog_output0; double analog_output1; float master_board_temperature; float robot_voltage_48V; @@ -30,7 +29,7 @@ class SharedMasterBoardData { int32_t euromap_input_bits; int32_t euromap_output_bits; - static const size_t SIZE = sizeof(double) * 4 + static const size_t SIZE = sizeof(double) * 4 + sizeof(int8_t) * 4 + sizeof(float) * 4 + sizeof(uint8_t); @@ -40,8 +39,8 @@ class SharedMasterBoardData { 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 @@ class MasterBoardData_V1_X : public SharedMasterBoardData, public StatePacket { 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 @@ class MasterBoardData_V1_X : public SharedMasterBoardData, public StatePacket { class MasterBoardData_V3_0__1 : public SharedMasterBoardData, public StatePacket { public: - virtual bool parse_with(BinParser &bp); - virtual bool consume_with(URStatePacketConsumer &consumer); + virtual bool parse_with(BinParser& bp); + virtual bool consume_with(URStatePacketConsumer& consumer); int32_t digital_input_bits; int32_t digital_output_bits; @@ -77,7 +75,6 @@ class MasterBoardData_V3_0__1 : public SharedMasterBoardData, public StatePacket 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 @@ class MasterBoardData_V3_0__1 : public SharedMasterBoardData, public StatePacket class MasterBoardData_V3_2 : public MasterBoardData_V3_0__1 { public: - virtual bool parse_with(BinParser &bp); - virtual bool consume_with(URStatePacketConsumer &consumer); + virtual bool parse_with(BinParser& bp); + virtual bool consume_with(URStatePacketConsumer& consumer); uint8_t operational_mode_selector_input; uint8_t three_position_enabling_device_input; diff --git a/include/ur_modern_driver/ur/messages.h b/include/ur_modern_driver/ur/messages.h index 97315debd..dafad611f 100644 --- a/include/ur_modern_driver/ur/messages.h +++ b/include/ur_modern_driver/ur/messages.h @@ -1,30 +1,33 @@ #pragma once +#include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/pipeline.h" #include #include -#include "ur_modern_driver/pipeline.h" -#include "ur_modern_driver/bin_parser.h" - enum class robot_message_type : uint8_t { - ROBOT_MESSAGE_TEXT = 0, - ROBOT_MESSAGE_PROGRAM_LABEL = 1, - PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2, - ROBOT_MESSAGE_VERSION = 3, - ROBOT_MESSAGE_SAFETY_MODE = 5, - ROBOT_MESSAGE_ERROR_CODE = 6, - ROBOT_MESSAGE_KEY = 7, - ROBOT_MESSAGE_REQUEST_VALUE = 9, - ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10 + ROBOT_MESSAGE_TEXT = 0, + ROBOT_MESSAGE_PROGRAM_LABEL = 1, + PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2, + ROBOT_MESSAGE_VERSION = 3, + ROBOT_MESSAGE_SAFETY_MODE = 5, + ROBOT_MESSAGE_ERROR_CODE = 6, + ROBOT_MESSAGE_KEY = 7, + ROBOT_MESSAGE_REQUEST_VALUE = 9, + ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10 }; class URMessagePacketConsumer; class MessagePacket { public: - MessagePacket(uint64_t timestamp, uint8_t source) : timestamp(timestamp), source(source) { } - virtual bool parse_with(BinParser &bp) = 0; - virtual bool consume_with(URMessagePacketConsumer &consumer) = 0; + MessagePacket(uint64_t timestamp, uint8_t source) + : timestamp(timestamp) + , source(source) + { + } + virtual bool parse_with(BinParser& bp) = 0; + virtual bool consume_with(URMessagePacketConsumer& consumer) = 0; uint64_t timestamp; uint8_t source; @@ -32,10 +35,13 @@ class MessagePacket { class VersionMessage : public MessagePacket { public: - VersionMessage(uint64_t timestamp, uint8_t source) : MessagePacket(timestamp, source) { } - - virtual bool parse_with(BinParser &bp); - virtual bool consume_with(URMessagePacketConsumer &consumer); + VersionMessage(uint64_t timestamp, uint8_t source) + : MessagePacket(timestamp, source) + { + } + + virtual bool parse_with(BinParser& bp); + virtual bool consume_with(URMessagePacketConsumer& consumer); std::string project_name; uint8_t major_version; diff --git a/include/ur_modern_driver/ur/messages_parser.h b/include/ur_modern_driver/ur/messages_parser.h index 77dc9b705..b0a781f65 100644 --- a/include/ur_modern_driver/ur/messages_parser.h +++ b/include/ur_modern_driver/ur/messages_parser.h @@ -1,20 +1,21 @@ #pragma once -#include -#include "ur_modern_driver/log.h" #include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/log.h" #include "ur_modern_driver/pipeline.h" -#include "ur_modern_driver/ur/parser.h" #include "ur_modern_driver/ur/messages.h" +#include "ur_modern_driver/ur/parser.h" +#include class URMessageParser : public URParser { public: - bool parse(BinParser &bp, std::vector> &results) { + bool parse(BinParser& bp, std::vector >& results) + { int32_t packet_size; message_type type; bp.parse(packet_size); bp.parse(type); - if(type != message_type::ROBOT_MESSAGE) { + if (type != message_type::ROBOT_MESSAGE) { LOG_WARN("Invalid message type recieved: %u", static_cast(type)); return false; } @@ -30,16 +31,16 @@ class URMessageParser : public URParser { std::unique_ptr result; bool parsed = false; - switch(message_type) { - case robot_message_type::ROBOT_MESSAGE_VERSION: { - VersionMessage *vm = new VersionMessage(timestamp, source); - parsed = vm->parse_with(bp); - result.reset(vm); - break; - } + switch (message_type) { + case robot_message_type::ROBOT_MESSAGE_VERSION: { + VersionMessage* vm = new VersionMessage(timestamp, source); + parsed = vm->parse_with(bp); + result.reset(vm); + break; + } - default: - return false; + default: + return false; } results.push_back(std::move(result)); diff --git a/include/ur_modern_driver/ur/parser.h b/include/ur_modern_driver/ur/parser.h index 00a0bac6f..ea53b401b 100644 --- a/include/ur_modern_driver/ur/parser.h +++ b/include/ur_modern_driver/ur/parser.h @@ -1,10 +1,10 @@ #pragma once -#include -#include "ur_modern_driver/pipeline.h" #include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/pipeline.h" +#include template class URParser { public: - virtual bool parse(BinParser &bp, std::vector> &results) = 0; + virtual bool parse(BinParser& bp, std::vector >& results) = 0; }; diff --git a/include/ur_modern_driver/ur/producer.h b/include/ur_modern_driver/ur/producer.h index 35be085f3..cfbe95ce2 100644 --- a/include/ur_modern_driver/ur/producer.h +++ b/include/ur_modern_driver/ur/producer.h @@ -1,30 +1,36 @@ #pragma once #include "ur_modern_driver/pipeline.h" -#include "ur_modern_driver/ur/stream.h" #include "ur_modern_driver/ur/parser.h" +#include "ur_modern_driver/ur/stream.h" template class URProducer : public IProducer { private: - URStream &_stream; - URParser &_parser; + URStream& _stream; + URParser& _parser; public: - URProducer(URStream &stream, URParser &parser) - : _stream(stream), - _parser(parser) { } - - void setup_producer() { + URProducer(URStream& stream, URParser& parser) + : _stream(stream) + , _parser(parser) + { + } + + void setup_producer() + { _stream.connect(); } - void teardown_producer() { + void teardown_producer() + { _stream.disconnect(); } - void stop_producer() { + void stop_producer() + { _stream.disconnect(); } - - bool try_get(std::vector> &products) { + + bool try_get(std::vector >& products) + { //4KB should be enough to hold any packet received from UR uint8_t buf[4096]; @@ -33,7 +39,7 @@ class URProducer : public IProducer { //LOG_DEBUG("Read %d bytes from stream", len); - if(len < 1) { + if (len < 1) { LOG_WARN("Read nothing from stream"); return false; } diff --git a/include/ur_modern_driver/ur/robot_mode.h b/include/ur_modern_driver/ur/robot_mode.h index bce22af57..b5307b9d8 100644 --- a/include/ur_modern_driver/ur/robot_mode.h +++ b/include/ur_modern_driver/ur/robot_mode.h @@ -1,14 +1,14 @@ #pragma once -#include -#include -#include "ur_modern_driver/types.h" #include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/types.h" #include "ur_modern_driver/ur/state.h" +#include +#include class SharedRobotModeData { public: - virtual bool parse_with(BinParser &bp); + virtual bool parse_with(BinParser& bp); uint64_t timestamp; bool physical_robot_connected; @@ -23,31 +23,30 @@ class SharedRobotModeData { enum class robot_mode_V1_X : uint8_t { ROBOT_RUNNING_MODE = 0, - ROBOT_FREEDRIVE_MODE = 1, - ROBOT_READY_MODE = 2, - ROBOT_INITIALIZING_MODE = 3, - ROBOT_SECURITY_STOPPED_MODE = 4, - ROBOT_EMERGENCY_STOPPED_MODE = 5, - ROBOT_FATAL_ERROR_MODE = 6, - ROBOT_NO_POWER_MODE = 7, - ROBOT_NOT_CONNECTED_MODE = 8, - ROBOT_SHUTDOWN_MODE = 9, - ROBOT_SAFEGUARD_STOP_MODE = 10 + ROBOT_FREEDRIVE_MODE = 1, + ROBOT_READY_MODE = 2, + ROBOT_INITIALIZING_MODE = 3, + ROBOT_SECURITY_STOPPED_MODE = 4, + ROBOT_EMERGENCY_STOPPED_MODE = 5, + ROBOT_FATAL_ERROR_MODE = 6, + ROBOT_NO_POWER_MODE = 7, + ROBOT_NOT_CONNECTED_MODE = 8, + ROBOT_SHUTDOWN_MODE = 9, + ROBOT_SAFEGUARD_STOP_MODE = 10 }; class RobotModeData_V1_X : public SharedRobotModeData, public StatePacket { public: - virtual bool parse_with(BinParser &bp); - virtual bool consume_with(URStatePacketConsumer &consumer); - + virtual bool parse_with(BinParser& bp); + virtual bool consume_with(URStatePacketConsumer& consumer); bool security_stopped; robot_mode_V1_X robot_mode; double speed_fraction; - static const size_t SIZE = SharedRobotModeData::SIZE + static const size_t SIZE = SharedRobotModeData::SIZE + sizeof(uint8_t) - + sizeof(robot_mode_V1_X) + + sizeof(robot_mode_V1_X) + sizeof(double); static_assert(RobotModeData_V1_X::SIZE == 24, "RobotModeData_V1_X has missmatched size"); @@ -55,14 +54,14 @@ class RobotModeData_V1_X : public SharedRobotModeData, public StatePacket { enum class robot_mode_V3_X : uint8_t { DISCONNECTED = 0, - CONFIRM_SAFETY = 1, - BOOTING = 2, - POWER_OFF = 3, - POWER_ON = 4, - IDLE = 5, - BACKDRIVE = 6, - RUNNING = 7, - UPDATING_FIRMWARE = 8 + CONFIRM_SAFETY = 1, + BOOTING = 2, + POWER_OFF = 3, + POWER_ON = 4, + IDLE = 5, + BACKDRIVE = 6, + RUNNING = 7, + UPDATING_FIRMWARE = 8 }; enum class robot_control_mode_V3_X : uint8_t { @@ -74,9 +73,8 @@ enum class robot_control_mode_V3_X : uint8_t { class RobotModeData_V3_0__1 : public SharedRobotModeData, public StatePacket { public: - virtual bool parse_with(BinParser &bp); - virtual bool consume_with(URStatePacketConsumer &consumer); - + virtual bool parse_with(BinParser& bp); + virtual bool consume_with(URStatePacketConsumer& consumer); bool protective_stopped; @@ -85,10 +83,10 @@ class RobotModeData_V3_0__1 : public SharedRobotModeData, public StatePacket { double target_speed_fraction; double speed_scaling; - - static const size_t SIZE = SharedRobotModeData::SIZE - + sizeof(uint8_t) - + sizeof(robot_mode_V3_X) + + static const size_t SIZE = SharedRobotModeData::SIZE + + sizeof(uint8_t) + + sizeof(robot_mode_V3_X) + sizeof(robot_control_mode_V3_X) + sizeof(double) + sizeof(double); @@ -98,14 +96,13 @@ class RobotModeData_V3_0__1 : public SharedRobotModeData, public StatePacket { class RobotModeData_V3_2 : public RobotModeData_V3_0__1 { public: - virtual bool parse_with(BinParser &bp); - virtual bool consume_with(URStatePacketConsumer &consumer); - + virtual bool parse_with(BinParser& bp); + virtual bool consume_with(URStatePacketConsumer& consumer); double target_speed_fraction_limit; static const size_t SIZE = RobotModeData_V3_0__1::SIZE + sizeof(double); - static_assert(RobotModeData_V3_2::SIZE == 41, "RobotModeData_V3_2 has missmatched size"); + static_assert(RobotModeData_V3_2::SIZE == 41, "RobotModeData_V3_2 has missmatched size"); }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/rt_parser.h b/include/ur_modern_driver/ur/rt_parser.h index 3a3831a5c..e0cde0d56 100644 --- a/include/ur_modern_driver/ur/rt_parser.h +++ b/include/ur_modern_driver/ur/rt_parser.h @@ -1,31 +1,31 @@ #pragma once -#include -#include "ur_modern_driver/log.h" #include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/log.h" #include "ur_modern_driver/pipeline.h" #include "ur_modern_driver/ur/parser.h" #include "ur_modern_driver/ur/rt_state.h" - +#include template class URRTStateParser : public URParser { public: - bool parse(BinParser &bp, std::vector> &results) { + bool parse(BinParser& bp, std::vector >& results) + { int32_t packet_size = bp.peek(); - if(!bp.check_size(packet_size)) { + if (!bp.check_size(packet_size)) { LOG_ERROR("Buffer len shorter than expected packet length"); - return false; + return false; } bp.parse(packet_size); //consumes the peeked data std::unique_ptr packet(new T); - if(!packet->parse_with(bp)) + if (!packet->parse_with(bp)) return false; - + results.push_back(std::move(packet)); - + return true; } }; diff --git a/include/ur_modern_driver/ur/rt_state.h b/include/ur_modern_driver/ur/rt_state.h index c749ebbd8..92cd299cf 100644 --- a/include/ur_modern_driver/ur/rt_state.h +++ b/include/ur_modern_driver/ur/rt_state.h @@ -1,23 +1,23 @@ #pragma once -#include -#include -#include "ur_modern_driver/types.h" #include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/pipeline.h" +#include "ur_modern_driver/types.h" +#include +#include class URRTPacketConsumer; class RTPacket { public: - virtual bool parse_with(BinParser &bp) = 0; - virtual bool consume_with(URRTPacketConsumer &consumer) = 0; + virtual bool parse_with(BinParser& bp) = 0; + virtual bool consume_with(URRTPacketConsumer& consumer) = 0; }; class RTShared { protected: - bool parse_shared1(BinParser &bp); - bool parse_shared2(BinParser &bp); + bool parse_shared1(BinParser& bp); + bool parse_shared2(BinParser& bp); public: double time; @@ -43,18 +43,16 @@ class RTShared { double controller_time; double robot_mode; - - static const size_t SIZE = sizeof(double) * 3 + static const size_t SIZE = sizeof(double) * 3 + sizeof(double[6]) * 10 + sizeof(cartesian_coord_t) * 2 + sizeof(uint64_t); - }; class RTState_V1_6__7 : public RTShared, public RTPacket { public: - bool parse_with(BinParser &bp); - virtual bool consume_with(URRTPacketConsumer &consumer); + bool parse_with(BinParser& bp); + virtual bool consume_with(URRTPacketConsumer& consumer); double3_t tool_accelerometer_values; @@ -66,8 +64,8 @@ class RTState_V1_6__7 : public RTShared, public RTPacket { 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 @@ class RTState_V1_8 : public RTState_V1_6__7 { class RTState_V3_0__1 : public RTShared, public RTPacket { public: - bool parse_with(BinParser &bp); - virtual bool consume_with(URRTPacketConsumer &consumer); + bool parse_with(BinParser& bp); + virtual bool consume_with(URRTPacketConsumer& consumer); double i_control[6]; cartesian_coord_t tool_vector_target; cartesian_coord_t tcp_speed_target; - double joint_modes[6]; double safety_mode; double3_t tool_accelerometer_values; @@ -97,8 +94,7 @@ class RTState_V3_0__1 : public RTShared, public RTPacket { double i_robot; double v_actual[6]; - - static const size_t SIZE = RTShared::SIZE + static const size_t SIZE = RTShared::SIZE + sizeof(double[6]) * 3 + sizeof(double3_t) + sizeof(cartesian_coord_t) * 2 @@ -109,15 +105,15 @@ class RTState_V3_0__1 : public RTShared, public RTPacket { class RTState_V3_2__3 : public RTState_V3_0__1 { public: - bool parse_with(BinParser &bp); - virtual bool consume_with(URRTPacketConsumer &consumer); + bool parse_with(BinParser& bp); + virtual bool consume_with(URRTPacketConsumer& consumer); uint64_t digital_outputs; double program_state; - static const size_t SIZE = RTState_V3_0__1::SIZE + static const size_t SIZE = RTState_V3_0__1::SIZE + sizeof(uint64_t) + sizeof(double); - static_assert(RTState_V3_2__3::SIZE == 936, "RTState_V3_2__3 has mismatched size!"); + static_assert(RTState_V3_2__3::SIZE == 936, "RTState_V3_2__3 has mismatched size!"); }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/state.h b/include/ur_modern_driver/ur/state.h index d37cbf195..0ac8e691a 100644 --- a/include/ur_modern_driver/ur/state.h +++ b/include/ur_modern_driver/ur/state.h @@ -1,27 +1,27 @@ #pragma once -#include -#include -#include "ur_modern_driver/pipeline.h" #include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/log.h" +#include "ur_modern_driver/pipeline.h" +#include +#include enum class package_type : uint8_t { - ROBOT_MODE_DATA = 0, - JOINT_DATA = 1, - TOOL_DATA = 2, - MASTERBOARD_DATA = 3, - CARTESIAN_INFO = 4, - KINEMATICS_INFO = 5, - CONFIGURATION_DATA = 6, - FORCE_MODE_DATA = 7, - ADDITIONAL_INFO = 8, - CALIBRATION_DATA = 9 + ROBOT_MODE_DATA = 0, + JOINT_DATA = 1, + TOOL_DATA = 2, + MASTERBOARD_DATA = 3, + CARTESIAN_INFO = 4, + KINEMATICS_INFO = 5, + CONFIGURATION_DATA = 6, + FORCE_MODE_DATA = 7, + ADDITIONAL_INFO = 8, + CALIBRATION_DATA = 9 }; enum class message_type : uint8_t { - ROBOT_STATE = 16, - ROBOT_MESSAGE = 20, + ROBOT_STATE = 16, + ROBOT_MESSAGE = 20, PROGRAM_STATE_MESSAGE = 25 }; @@ -29,6 +29,6 @@ class URStatePacketConsumer; class StatePacket { public: - virtual bool parse_with(BinParser &bp) = 0; - virtual bool consume_with(URStatePacketConsumer &consumer) = 0; + virtual bool parse_with(BinParser& bp) = 0; + virtual bool consume_with(URStatePacketConsumer& consumer) = 0; }; diff --git a/include/ur_modern_driver/ur/state_parser.h b/include/ur_modern_driver/ur/state_parser.h index df5a7d60a..a68a6113c 100644 --- a/include/ur_modern_driver/ur/state_parser.h +++ b/include/ur_modern_driver/ur/state_parser.h @@ -1,73 +1,74 @@ #pragma once -#include -#include "ur_modern_driver/log.h" #include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/log.h" #include "ur_modern_driver/pipeline.h" -#include "ur_modern_driver/ur/parser.h" -#include "ur_modern_driver/ur/state.h" #include "ur_modern_driver/ur/master_board.h" +#include "ur_modern_driver/ur/parser.h" #include "ur_modern_driver/ur/robot_mode.h" - +#include "ur_modern_driver/ur/state.h" +#include template class URStateParser : public URParser { private: - StatePacket* from_type(package_type type) { - switch(type) { - case package_type::ROBOT_MODE_DATA: - return new RMD; - case package_type::MASTERBOARD_DATA: - return new MBD; - default: - return nullptr; + StatePacket* from_type(package_type type) + { + switch (type) { + case package_type::ROBOT_MODE_DATA: + return new RMD; + case package_type::MASTERBOARD_DATA: + return new MBD; + default: + return nullptr; } } public: - bool parse(BinParser &bp, std::vector> &results) { + bool parse(BinParser& bp, std::vector >& results) + { int32_t packet_size; message_type type; bp.parse(packet_size); bp.parse(type); - if(type != message_type::ROBOT_STATE) { + if (type != message_type::ROBOT_STATE) { LOG_WARN("Invalid message type recieved: %u", static_cast(type)); return false; } - while(!bp.empty()) { - if(!bp.check_size(sizeof(uint32_t))){ + while (!bp.empty()) { + if (!bp.check_size(sizeof(uint32_t))) { LOG_ERROR("Failed to read sub-package length, there's likely a parsing error"); - return false; + return false; } uint32_t sub_size = bp.peek(); - if(!bp.check_size(static_cast(sub_size))) { + if (!bp.check_size(static_cast(sub_size))) { LOG_WARN("Invalid sub-package size of %" PRIu32 " received!", sub_size); return false; } - - //deconstruction of a sub parser will increment the position of the parent parser + + //deconstruction of a sub parser will increment the position of the parent parser BinParser sbp(bp, sub_size); sbp.consume(sizeof(sub_size)); package_type type; sbp.parse(type); - + std::unique_ptr packet(from_type(type)); - if(packet == nullptr) { + if (packet == nullptr) { sbp.consume(); LOG_INFO("Skipping sub-packet of type %d", type); continue; } - if(!packet->parse_with(sbp)) { + if (!packet->parse_with(sbp)) { LOG_ERROR("Sub-package parsing of type %d failed!", type); return false; } results.push_back(std::move(packet)); - if(!sbp.empty()) { + if (!sbp.empty()) { LOG_ERROR("Sub-package was not parsed completely!"); return false; } @@ -77,6 +78,6 @@ class URStateParser : public URParser { } }; -typedef URStateParser URStateParser_V1_X; -typedef URStateParser URStateParser_V3_0__1; -typedef URStateParser URStateParser_V3_2; \ No newline at end of file +typedef URStateParser URStateParser_V1_X; +typedef URStateParser URStateParser_V3_0__1; +typedef URStateParser URStateParser_V3_2; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/stream.h b/include/ur_modern_driver/ur/stream.h index a6ac0d58c..7d7363b4f 100644 --- a/include/ur_modern_driver/ur/stream.h +++ b/include/ur_modern_driver/ur/stream.h @@ -1,11 +1,11 @@ #pragma once -#include #include -#include -#include #include +#include +#include +#include -/// Encapsulates a TCP socket +/// Encapsulates a TCP socket class URStream { private: int _socket_fd = -1; @@ -16,19 +16,22 @@ class URStream { std::atomic _stopping; public: - URStream(std::string &host, int port) - : _host(host), - _port(port), - _initialized(false), - _stopping(false) {} - - ~URStream() { + URStream(std::string& host, int port) + : _host(host) + , _port(port) + , _initialized(false) + , _stopping(false) + { + } + + ~URStream() + { disconnect(); } bool connect(); void disconnect(); - ssize_t send(uint8_t *buf, size_t buf_len); - ssize_t receive(uint8_t *buf, size_t buf_len); + ssize_t send(uint8_t* buf, size_t buf_len); + ssize_t receive(uint8_t* buf, size_t buf_len); }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur_communication.h b/include/ur_modern_driver/ur_communication.h index 5173c45b5..cad98e14e 100644 --- a/include/ur_modern_driver/ur_communication.h +++ b/include/ur_modern_driver/ur_communication.h @@ -19,46 +19,44 @@ #ifndef UR_COMMUNICATION_H_ #define UR_COMMUNICATION_H_ -#include "robot_state.h" #include "do_output.h" -#include -#include +#include "robot_state.h" +#include +#include +#include +#include +#include +#include +#include +#include #include +#include #include +#include #include -#include -#include -#include #include -#include -#include -#include -#include -#include -#include -#include -#include #include - +#include +#include +#include class UrCommunication { private: - int pri_sockfd_, sec_sockfd_; - struct sockaddr_in pri_serv_addr_, sec_serv_addr_; - struct hostent *server_; - bool keepalive_; - std::thread comThread_; - int flag_; - void run(); + int pri_sockfd_, sec_sockfd_; + struct sockaddr_in pri_serv_addr_, sec_serv_addr_; + struct hostent* server_; + bool keepalive_; + std::thread comThread_; + int flag_; + void run(); public: - bool connected_; - RobotState* robot_state_; - - UrCommunication(std::condition_variable& msg_cond, std::string host); - bool start(); - void halt(); + bool connected_; + RobotState* robot_state_; + UrCommunication(std::condition_variable& msg_cond, std::string host); + bool start(); + void halt(); }; #endif /* UR_COMMUNICATION_H_ */ diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h index 0f392fa97..1d414c109 100644 --- a/include/ur_modern_driver/ur_driver.h +++ b/include/ur_modern_driver/ur_driver.h @@ -19,83 +19,81 @@ #ifndef UR_DRIVER_H_ #define UR_DRIVER_H_ -#include -#include -#include "ur_realtime_communication.h" -#include "ur_communication.h" #include "do_output.h" -#include +#include "ur_communication.h" +#include "ur_realtime_communication.h" +#include #include +#include +#include #include -#include #include -#include +#include +#include #include - class UrDriver { private: - double maximum_time_step_; - double minimum_payload_; - double maximum_payload_; - std::vector joint_names_; - std::string ip_addr_; - const int MULT_JOINTSTATE_ = 1000000; - const int MULT_TIME_ = 1000000; - const unsigned int REVERSE_PORT_; - int incoming_sockfd_; - int new_sockfd_; - bool reverse_connected_; - double servoj_time_; - bool executing_traj_; - double firmware_version_; - double servoj_lookahead_time_; - double servoj_gain_; -public: - UrRealtimeCommunication* rt_interface_; - UrCommunication* sec_interface_; + double maximum_time_step_; + double minimum_payload_; + double maximum_payload_; + std::vector joint_names_; + std::string ip_addr_; + const int MULT_JOINTSTATE_ = 1000000; + const int MULT_TIME_ = 1000000; + const unsigned int REVERSE_PORT_; + int incoming_sockfd_; + int new_sockfd_; + bool reverse_connected_; + double servoj_time_; + bool executing_traj_; + double firmware_version_; + double servoj_lookahead_time_; + double servoj_gain_; - UrDriver(std::condition_variable& rt_msg_cond, - std::condition_variable& msg_cond, std::string host, - unsigned int reverse_port = 50007, double servoj_time = 0.016, unsigned int safety_count_max = - 12, double max_time_step = 0.08, double min_payload = 0., - double max_payload = 1., double servoj_lookahead_time=0.03, double servoj_gain=300.); - bool start(); - void halt(); +public: + UrRealtimeCommunication* rt_interface_; + UrCommunication* sec_interface_; - void setSpeed(double q0, double q1, double q2, double q3, double q4, - double q5, double acc = 100.); + UrDriver(std::condition_variable& rt_msg_cond, + std::condition_variable& msg_cond, std::string host, + unsigned int reverse_port = 50007, double servoj_time = 0.016, unsigned int safety_count_max = 12, double max_time_step = 0.08, double min_payload = 0., + double max_payload = 1., double servoj_lookahead_time = 0.03, double servoj_gain = 300.); + bool start(); + void halt(); - bool doTraj(std::vector inp_timestamps, - std::vector > inp_positions, - std::vector > inp_velocities); - void servoj(std::vector positions, int keepalive = 1); + void setSpeed(double q0, double q1, double q2, double q3, double q4, + double q5, double acc = 100.); - void stopTraj(); + bool doTraj(std::vector inp_timestamps, + std::vector > inp_positions, + std::vector > inp_velocities); + void servoj(std::vector positions, int keepalive = 1); - bool uploadProg(); - bool openServo(); - void closeServo(std::vector positions); + void stopTraj(); - std::vector interp_cubic(double t, double T, - std::vector p0_pos, std::vector p1_pos, - std::vector p0_vel, std::vector p1_vel); + bool uploadProg(); + bool openServo(); + void closeServo(std::vector positions); - std::vector getJointNames(); - void setJointNames(std::vector jn); - void setToolVoltage(unsigned int v); - void setFlag(unsigned int n, bool b); - void setDigitalOut(unsigned int n, bool b); - void setAnalogOut(unsigned int n, double f); - bool setPayload(double m); + std::vector interp_cubic(double t, double T, + std::vector p0_pos, std::vector p1_pos, + std::vector p0_vel, std::vector p1_vel); - void setMinPayload(double m); - void setMaxPayload(double m); - void setServojTime(double t); - void setServojLookahead(double t); - void setServojGain(double g); + std::vector getJointNames(); + void setJointNames(std::vector jn); + void setToolVoltage(unsigned int v); + void setFlag(unsigned int n, bool b); + void setDigitalOut(unsigned int n, bool b); + void setAnalogOut(unsigned int n, double f); + bool setPayload(double m); + void setMinPayload(double m); + void setMaxPayload(double m); + void setServojTime(double t); + void setServojLookahead(double t); + void setServojGain(double g); }; #endif /* UR_DRIVER_H_ */ diff --git a/include/ur_modern_driver/ur_hardware_interface.h b/include/ur_modern_driver/ur_hardware_interface.h index ac9f310a1..0724703cf 100644 --- a/include/ur_modern_driver/ur_hardware_interface.h +++ b/include/ur_modern_driver/ur_hardware_interface.h @@ -58,16 +58,16 @@ #ifndef UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H #define UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H -#include -#include +#include "do_output.h" +#include "ur_driver.h" +#include +#include #include +#include +#include #include -#include -#include -#include #include -#include "do_output.h" -#include "ur_driver.h" +#include namespace ros_control_ur { @@ -76,64 +76,61 @@ static const double POSITION_STEP_FACTOR = 1; static const double VELOCITY_STEP_FACTOR = 1; /// \brief Hardware interface for a robot -class UrHardwareInterface: public hardware_interface::RobotHW { +class UrHardwareInterface : public hardware_interface::RobotHW { public: - - /** + /** * \brief Constructor * \param nh - Node handle for topics. */ - UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot); + UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot); - /// \brief Initialize the hardware interface - virtual void init(); + /// \brief Initialize the hardware interface + virtual void init(); - /// \brief Read the state from the robot hardware. - virtual void read(); + /// \brief Read the state from the robot hardware. + virtual void read(); - /// \brief write the command to the robot hardware. - virtual void write(); + /// \brief write the command to the robot hardware. + virtual void write(); - void setMaxVelChange(double inp); + void setMaxVelChange(double inp); - bool canSwitch( - const std::list &start_list, - const std::list &stop_list) const; - void doSwitch(const std::list&start_list, - const std::list&stop_list); + bool canSwitch( + const std::list& start_list, + const std::list& stop_list) const; + void doSwitch(const std::list& start_list, + const std::list& stop_list); protected: - - // Startup and shutdown of the internal node inside a roscpp program - ros::NodeHandle nh_; - - // Interfaces - hardware_interface::JointStateInterface joint_state_interface_; - hardware_interface::ForceTorqueSensorInterface force_torque_interface_; - hardware_interface::PositionJointInterface position_joint_interface_; - hardware_interface::VelocityJointInterface velocity_joint_interface_; - bool velocity_interface_running_; - bool position_interface_running_; - // Shared memory - std::vector joint_names_; - std::vector joint_position_; - std::vector joint_velocity_; - std::vector joint_effort_; - std::vector joint_position_command_; - std::vector joint_velocity_command_; - std::vector prev_joint_velocity_command_; - std::size_t num_joints_; - double robot_force_[3] = { 0., 0., 0. }; - double robot_torque_[3] = { 0., 0., 0. }; - - double max_vel_change_; - - // Robot API - UrDriver* robot_; - + // Startup and shutdown of the internal node inside a roscpp program + ros::NodeHandle nh_; + + // Interfaces + hardware_interface::JointStateInterface joint_state_interface_; + hardware_interface::ForceTorqueSensorInterface force_torque_interface_; + hardware_interface::PositionJointInterface position_joint_interface_; + hardware_interface::VelocityJointInterface velocity_joint_interface_; + bool velocity_interface_running_; + bool position_interface_running_; + // Shared memory + std::vector joint_names_; + std::vector joint_position_; + std::vector joint_velocity_; + std::vector joint_effort_; + std::vector joint_position_command_; + std::vector joint_velocity_command_; + std::vector prev_joint_velocity_command_; + std::size_t num_joints_; + double robot_force_[3] = { 0., 0., 0. }; + double robot_torque_[3] = { 0., 0., 0. }; + + double max_vel_change_; + + // Robot API + UrDriver* robot_; }; // class -}// namespace +} // namespace #endif diff --git a/include/ur_modern_driver/ur_realtime_communication.h b/include/ur_modern_driver/ur_realtime_communication.h index 9d6b44547..61e547417 100644 --- a/include/ur_modern_driver/ur_realtime_communication.h +++ b/include/ur_modern_driver/ur_realtime_communication.h @@ -19,58 +19,56 @@ #ifndef UR_REALTIME_COMMUNICATION_H_ #define UR_REALTIME_COMMUNICATION_H_ -#include "robot_state_RT.h" #include "do_output.h" -#include -#include +#include "robot_state_RT.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include #include +#include #include +#include #include -#include -#include -#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include +#include +#include +#include class UrRealtimeCommunication { private: - unsigned int safety_count_max_; - int sockfd_; - struct sockaddr_in serv_addr_; - struct hostent *server_; - std::string local_ip_; - bool keepalive_; - std::thread comThread_; - int flag_; - std::recursive_mutex command_string_lock_; - std::string command_; - unsigned int safety_count_; - void run(); - + unsigned int safety_count_max_; + int sockfd_; + struct sockaddr_in serv_addr_; + struct hostent* server_; + std::string local_ip_; + bool keepalive_; + std::thread comThread_; + int flag_; + std::recursive_mutex command_string_lock_; + std::string command_; + unsigned int safety_count_; + void run(); public: - bool connected_; - RobotStateRT* robot_state_; - - UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host, - unsigned int safety_count_max = 12); - bool start(); - void halt(); - void setSpeed(double q0, double q1, double q2, double q3, double q4, - double q5, double acc = 100.); - void addCommandToQueue(std::string inp); - void setSafetyCountMax(uint inp); - std::string getLocalIp(); + bool connected_; + RobotStateRT* robot_state_; + UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host, + unsigned int safety_count_max = 12); + bool start(); + void halt(); + void setSpeed(double q0, double q1, double q2, double q3, double q4, + double q5, double acc = 100.); + void addCommandToQueue(std::string inp); + void setSafetyCountMax(uint inp); + std::string getLocalIp(); }; #endif /* UR_REALTIME_COMMUNICATION_H_ */ diff --git a/src/do_output.cpp b/src/do_output.cpp index 048cce16b..f4b0b4094 100644 --- a/src/do_output.cpp +++ b/src/do_output.cpp @@ -18,40 +18,45 @@ #include "ur_modern_driver/do_output.h" -void print_debug(std::string inp) { +void print_debug(std::string inp) +{ #ifdef ROS_BUILD - ROS_DEBUG("%s", inp.c_str()); + ROS_DEBUG("%s", inp.c_str()); #else - printf("DEBUG: %s\n", inp.c_str()); + printf("DEBUG: %s\n", inp.c_str()); #endif } -void print_info(std::string inp) { +void print_info(std::string inp) +{ #ifdef ROS_BUILD - ROS_INFO("%s", inp.c_str()); + ROS_INFO("%s", inp.c_str()); #else - printf("INFO: %s\n", inp.c_str()); + printf("INFO: %s\n", inp.c_str()); #endif } -void print_warning(std::string inp) { +void print_warning(std::string inp) +{ #ifdef ROS_BUILD - ROS_WARN("%s", inp.c_str()); + ROS_WARN("%s", inp.c_str()); #else - printf("WARNING: %s\n", inp.c_str()); + printf("WARNING: %s\n", inp.c_str()); #endif } -void print_error(std::string inp) { +void print_error(std::string inp) +{ #ifdef ROS_BUILD - ROS_ERROR("%s", inp.c_str()); + ROS_ERROR("%s", inp.c_str()); #else - printf("ERROR: %s\n", inp.c_str()); + printf("ERROR: %s\n", inp.c_str()); #endif } -void print_fatal(std::string inp) { +void print_fatal(std::string inp) +{ #ifdef ROS_BUILD - ROS_FATAL("%s", inp.c_str()); - ros::shutdown(); + ROS_FATAL("%s", inp.c_str()); + ros::shutdown(); #else - printf("FATAL: %s\n", inp.c_str()); - exit(1); + printf("FATAL: %s\n", inp.c_str()); + exit(1); #endif } diff --git a/src/robot_state.cpp b/src/robot_state.cpp index a7eb58930..743c2c223 100644 --- a/src/robot_state.cpp +++ b/src/robot_state.cpp @@ -18,370 +18,391 @@ #include "ur_modern_driver/robot_state.h" -RobotState::RobotState(std::condition_variable& msg_cond) { - version_msg_.major_version = 0; - version_msg_.minor_version = 0; - new_data_available_ = false; - pMsg_cond_ = &msg_cond; - RobotState::setDisconnected(); - robot_mode_running_ = robotStateTypeV30::ROBOT_MODE_RUNNING; +RobotState::RobotState(std::condition_variable& msg_cond) +{ + version_msg_.major_version = 0; + version_msg_.minor_version = 0; + new_data_available_ = false; + pMsg_cond_ = &msg_cond; + RobotState::setDisconnected(); + robot_mode_running_ = robotStateTypeV30::ROBOT_MODE_RUNNING; } -double RobotState::ntohd(uint64_t nf) { - double x; - nf = be64toh(nf); - memcpy(&x, &nf, sizeof(x)); - return x; +double RobotState::ntohd(uint64_t nf) +{ + double x; + nf = be64toh(nf); + memcpy(&x, &nf, sizeof(x)); + return x; } -void RobotState::unpack(uint8_t* buf, unsigned int buf_length) { - /* Returns missing bytes to unpack a message, or 0 if all data was parsed */ - unsigned int offset = 0; - while (buf_length > offset) { - int len; - unsigned char message_type; - memcpy(&len, &buf[offset], sizeof(len)); - len = ntohl(len); - if (len + offset > buf_length) { - return; - } - memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type)); - switch (message_type) { - case messageType::ROBOT_MESSAGE: - RobotState::unpackRobotMessage(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType - break; - case messageType::ROBOT_STATE: - RobotState::unpackRobotState(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType - break; - case messageType::PROGRAM_STATE_MESSAGE: - //Don't do anything atm... - default: - break; - } - offset += len; - - } - return; +void RobotState::unpack(uint8_t* buf, unsigned int buf_length) +{ + /* Returns missing bytes to unpack a message, or 0 if all data was parsed */ + unsigned int offset = 0; + while (buf_length > offset) { + int len; + unsigned char message_type; + memcpy(&len, &buf[offset], sizeof(len)); + len = ntohl(len); + if (len + offset > buf_length) { + return; + } + memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type)); + switch (message_type) { + case messageType::ROBOT_MESSAGE: + RobotState::unpackRobotMessage(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType + break; + case messageType::ROBOT_STATE: + RobotState::unpackRobotState(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType + break; + case messageType::PROGRAM_STATE_MESSAGE: + //Don't do anything atm... + default: + break; + } + offset += len; + } + return; } -void RobotState::unpackRobotMessage(uint8_t * buf, unsigned int offset, - uint32_t len) { - offset += 5; - uint64_t timestamp; - int8_t source, robot_message_type; - memcpy(×tamp, &buf[offset], sizeof(timestamp)); - offset += sizeof(timestamp); - memcpy(&source, &buf[offset], sizeof(source)); - offset += sizeof(source); - memcpy(&robot_message_type, &buf[offset], sizeof(robot_message_type)); - offset += sizeof(robot_message_type); - switch (robot_message_type) { - case robotMessageType::ROBOT_MESSAGE_VERSION: - val_lock_.lock(); - version_msg_.timestamp = timestamp; - version_msg_.source = source; - version_msg_.robot_message_type = robot_message_type; - RobotState::unpackRobotMessageVersion(buf, offset, len); - val_lock_.unlock(); - break; - default: - break; - } - +void RobotState::unpackRobotMessage(uint8_t* buf, unsigned int offset, + uint32_t len) +{ + offset += 5; + uint64_t timestamp; + int8_t source, robot_message_type; + memcpy(×tamp, &buf[offset], sizeof(timestamp)); + offset += sizeof(timestamp); + memcpy(&source, &buf[offset], sizeof(source)); + offset += sizeof(source); + memcpy(&robot_message_type, &buf[offset], sizeof(robot_message_type)); + offset += sizeof(robot_message_type); + switch (robot_message_type) { + case robotMessageType::ROBOT_MESSAGE_VERSION: + val_lock_.lock(); + version_msg_.timestamp = timestamp; + version_msg_.source = source; + version_msg_.robot_message_type = robot_message_type; + RobotState::unpackRobotMessageVersion(buf, offset, len); + val_lock_.unlock(); + break; + default: + break; + } } -void RobotState::unpackRobotState(uint8_t * buf, unsigned int offset, - uint32_t len) { - offset += 5; - while (offset < len) { - int32_t length; - uint8_t package_type; - memcpy(&length, &buf[offset], sizeof(length)); - length = ntohl(length); - memcpy(&package_type, &buf[offset + sizeof(length)], - sizeof(package_type)); - switch (package_type) { - case packageType::ROBOT_MODE_DATA: - val_lock_.lock(); - RobotState::unpackRobotMode(buf, offset + 5); - val_lock_.unlock(); - break; - - case packageType::MASTERBOARD_DATA: - val_lock_.lock(); - RobotState::unpackRobotStateMasterboard(buf, offset + 5); - val_lock_.unlock(); - break; - default: - break; - } - offset += length; - } - new_data_available_ = true; - pMsg_cond_->notify_all(); +void RobotState::unpackRobotState(uint8_t* buf, unsigned int offset, + uint32_t len) +{ + offset += 5; + while (offset < len) { + int32_t length; + uint8_t package_type; + memcpy(&length, &buf[offset], sizeof(length)); + length = ntohl(length); + memcpy(&package_type, &buf[offset + sizeof(length)], + sizeof(package_type)); + switch (package_type) { + case packageType::ROBOT_MODE_DATA: + val_lock_.lock(); + RobotState::unpackRobotMode(buf, offset + 5); + val_lock_.unlock(); + break; + case packageType::MASTERBOARD_DATA: + val_lock_.lock(); + RobotState::unpackRobotStateMasterboard(buf, offset + 5); + val_lock_.unlock(); + break; + default: + break; + } + offset += length; + } + new_data_available_ = true; + pMsg_cond_->notify_all(); } -void RobotState::unpackRobotMessageVersion(uint8_t * buf, unsigned int offset, - uint32_t len) { - memcpy(&version_msg_.project_name_size, &buf[offset], - sizeof(version_msg_.project_name_size)); - offset += sizeof(version_msg_.project_name_size); - memcpy(&version_msg_.project_name, &buf[offset], - sizeof(char) * version_msg_.project_name_size); - offset += version_msg_.project_name_size; - version_msg_.project_name[version_msg_.project_name_size] = '\0'; - memcpy(&version_msg_.major_version, &buf[offset], - sizeof(version_msg_.major_version)); - offset += sizeof(version_msg_.major_version); - memcpy(&version_msg_.minor_version, &buf[offset], - sizeof(version_msg_.minor_version)); - offset += sizeof(version_msg_.minor_version); - memcpy(&version_msg_.svn_revision, &buf[offset], - sizeof(version_msg_.svn_revision)); - offset += sizeof(version_msg_.svn_revision); - version_msg_.svn_revision = ntohl(version_msg_.svn_revision); - memcpy(&version_msg_.build_date, &buf[offset], sizeof(char) * len - offset); - version_msg_.build_date[len - offset] = '\0'; - if (version_msg_.major_version < 2) { - robot_mode_running_ = robotStateTypeV18::ROBOT_RUNNING_MODE; - } +void RobotState::unpackRobotMessageVersion(uint8_t* buf, unsigned int offset, + uint32_t len) +{ + memcpy(&version_msg_.project_name_size, &buf[offset], + sizeof(version_msg_.project_name_size)); + offset += sizeof(version_msg_.project_name_size); + memcpy(&version_msg_.project_name, &buf[offset], + sizeof(char) * version_msg_.project_name_size); + offset += version_msg_.project_name_size; + version_msg_.project_name[version_msg_.project_name_size] = '\0'; + memcpy(&version_msg_.major_version, &buf[offset], + sizeof(version_msg_.major_version)); + offset += sizeof(version_msg_.major_version); + memcpy(&version_msg_.minor_version, &buf[offset], + sizeof(version_msg_.minor_version)); + offset += sizeof(version_msg_.minor_version); + memcpy(&version_msg_.svn_revision, &buf[offset], + sizeof(version_msg_.svn_revision)); + offset += sizeof(version_msg_.svn_revision); + version_msg_.svn_revision = ntohl(version_msg_.svn_revision); + memcpy(&version_msg_.build_date, &buf[offset], sizeof(char) * len - offset); + version_msg_.build_date[len - offset] = '\0'; + if (version_msg_.major_version < 2) { + robot_mode_running_ = robotStateTypeV18::ROBOT_RUNNING_MODE; + } } -void RobotState::unpackRobotMode(uint8_t * buf, unsigned int offset) { - memcpy(&robot_mode_.timestamp, &buf[offset], sizeof(robot_mode_.timestamp)); - offset += sizeof(robot_mode_.timestamp); - uint8_t tmp; - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isRobotConnected = true; - else - robot_mode_.isRobotConnected = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isRealRobotEnabled = true; - else - robot_mode_.isRealRobotEnabled = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - //printf("PowerOnRobot: %d\n", tmp); - if (tmp > 0) - robot_mode_.isPowerOnRobot = true; - else - robot_mode_.isPowerOnRobot = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isEmergencyStopped = true; - else - robot_mode_.isEmergencyStopped = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isProtectiveStopped = true; - else - robot_mode_.isProtectiveStopped = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isProgramRunning = true; - else - robot_mode_.isProgramRunning = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isProgramPaused = true; - else - robot_mode_.isProgramPaused = false; - offset += sizeof(tmp); - memcpy(&robot_mode_.robotMode, &buf[offset], sizeof(robot_mode_.robotMode)); - offset += sizeof(robot_mode_.robotMode); - uint64_t temp; - if (RobotState::getVersion() > 2.) { - memcpy(&robot_mode_.controlMode, &buf[offset], - sizeof(robot_mode_.controlMode)); - offset += sizeof(robot_mode_.controlMode); - memcpy(&temp, &buf[offset], sizeof(temp)); - offset += sizeof(temp); - robot_mode_.targetSpeedFraction = RobotState::ntohd(temp); - } - memcpy(&temp, &buf[offset], sizeof(temp)); - offset += sizeof(temp); - robot_mode_.speedScaling = RobotState::ntohd(temp); +void RobotState::unpackRobotMode(uint8_t* buf, unsigned int offset) +{ + memcpy(&robot_mode_.timestamp, &buf[offset], sizeof(robot_mode_.timestamp)); + offset += sizeof(robot_mode_.timestamp); + uint8_t tmp; + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isRobotConnected = true; + else + robot_mode_.isRobotConnected = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isRealRobotEnabled = true; + else + robot_mode_.isRealRobotEnabled = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + //printf("PowerOnRobot: %d\n", tmp); + if (tmp > 0) + robot_mode_.isPowerOnRobot = true; + else + robot_mode_.isPowerOnRobot = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isEmergencyStopped = true; + else + robot_mode_.isEmergencyStopped = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isProtectiveStopped = true; + else + robot_mode_.isProtectiveStopped = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isProgramRunning = true; + else + robot_mode_.isProgramRunning = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isProgramPaused = true; + else + robot_mode_.isProgramPaused = false; + offset += sizeof(tmp); + memcpy(&robot_mode_.robotMode, &buf[offset], sizeof(robot_mode_.robotMode)); + offset += sizeof(robot_mode_.robotMode); + uint64_t temp; + if (RobotState::getVersion() > 2.) { + memcpy(&robot_mode_.controlMode, &buf[offset], + sizeof(robot_mode_.controlMode)); + offset += sizeof(robot_mode_.controlMode); + memcpy(&temp, &buf[offset], sizeof(temp)); + offset += sizeof(temp); + robot_mode_.targetSpeedFraction = RobotState::ntohd(temp); + } + memcpy(&temp, &buf[offset], sizeof(temp)); + offset += sizeof(temp); + robot_mode_.speedScaling = RobotState::ntohd(temp); } -void RobotState::unpackRobotStateMasterboard(uint8_t * buf, - unsigned int offset) { - if (RobotState::getVersion() < 3.0) { - int16_t digital_input_bits, digital_output_bits; - memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits)); - offset += sizeof(digital_input_bits); - memcpy(&digital_output_bits, &buf[offset], sizeof(digital_output_bits)); - offset += sizeof(digital_output_bits); - mb_data_.digitalInputBits = ntohs(digital_input_bits); - mb_data_.digitalOutputBits = ntohs(digital_output_bits); - } else { - memcpy(&mb_data_.digitalInputBits, &buf[offset], - sizeof(mb_data_.digitalInputBits)); - offset += sizeof(mb_data_.digitalInputBits); - mb_data_.digitalInputBits = ntohl(mb_data_.digitalInputBits); - memcpy(&mb_data_.digitalOutputBits, &buf[offset], - sizeof(mb_data_.digitalOutputBits)); - offset += sizeof(mb_data_.digitalOutputBits); - mb_data_.digitalOutputBits = ntohl(mb_data_.digitalOutputBits); - } +void RobotState::unpackRobotStateMasterboard(uint8_t* buf, + unsigned int offset) +{ + if (RobotState::getVersion() < 3.0) { + int16_t digital_input_bits, digital_output_bits; + memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits)); + offset += sizeof(digital_input_bits); + memcpy(&digital_output_bits, &buf[offset], sizeof(digital_output_bits)); + offset += sizeof(digital_output_bits); + mb_data_.digitalInputBits = ntohs(digital_input_bits); + mb_data_.digitalOutputBits = ntohs(digital_output_bits); + } else { + memcpy(&mb_data_.digitalInputBits, &buf[offset], + sizeof(mb_data_.digitalInputBits)); + offset += sizeof(mb_data_.digitalInputBits); + mb_data_.digitalInputBits = ntohl(mb_data_.digitalInputBits); + memcpy(&mb_data_.digitalOutputBits, &buf[offset], + sizeof(mb_data_.digitalOutputBits)); + offset += sizeof(mb_data_.digitalOutputBits); + mb_data_.digitalOutputBits = ntohl(mb_data_.digitalOutputBits); + } - memcpy(&mb_data_.analogInputRange0, &buf[offset], - sizeof(mb_data_.analogInputRange0)); - offset += sizeof(mb_data_.analogInputRange0); - memcpy(&mb_data_.analogInputRange1, &buf[offset], - sizeof(mb_data_.analogInputRange1)); - offset += sizeof(mb_data_.analogInputRange1); - uint64_t temp; - memcpy(&temp, &buf[offset], sizeof(temp)); - offset += sizeof(temp); - mb_data_.analogInput0 = RobotState::ntohd(temp); - memcpy(&temp, &buf[offset], sizeof(temp)); - offset += sizeof(temp); - mb_data_.analogInput1 = RobotState::ntohd(temp); - memcpy(&mb_data_.analogOutputDomain0, &buf[offset], - sizeof(mb_data_.analogOutputDomain0)); - offset += sizeof(mb_data_.analogOutputDomain0); - memcpy(&mb_data_.analogOutputDomain1, &buf[offset], - sizeof(mb_data_.analogOutputDomain1)); - offset += sizeof(mb_data_.analogOutputDomain1); - memcpy(&temp, &buf[offset], sizeof(temp)); - offset += sizeof(temp); - mb_data_.analogOutput0 = RobotState::ntohd(temp); - memcpy(&temp, &buf[offset], sizeof(temp)); - offset += sizeof(temp); - mb_data_.analogOutput1 = RobotState::ntohd(temp); + memcpy(&mb_data_.analogInputRange0, &buf[offset], + sizeof(mb_data_.analogInputRange0)); + offset += sizeof(mb_data_.analogInputRange0); + memcpy(&mb_data_.analogInputRange1, &buf[offset], + sizeof(mb_data_.analogInputRange1)); + offset += sizeof(mb_data_.analogInputRange1); + uint64_t temp; + memcpy(&temp, &buf[offset], sizeof(temp)); + offset += sizeof(temp); + mb_data_.analogInput0 = RobotState::ntohd(temp); + memcpy(&temp, &buf[offset], sizeof(temp)); + offset += sizeof(temp); + mb_data_.analogInput1 = RobotState::ntohd(temp); + memcpy(&mb_data_.analogOutputDomain0, &buf[offset], + sizeof(mb_data_.analogOutputDomain0)); + offset += sizeof(mb_data_.analogOutputDomain0); + memcpy(&mb_data_.analogOutputDomain1, &buf[offset], + sizeof(mb_data_.analogOutputDomain1)); + offset += sizeof(mb_data_.analogOutputDomain1); + memcpy(&temp, &buf[offset], sizeof(temp)); + offset += sizeof(temp); + mb_data_.analogOutput0 = RobotState::ntohd(temp); + memcpy(&temp, &buf[offset], sizeof(temp)); + offset += sizeof(temp); + mb_data_.analogOutput1 = RobotState::ntohd(temp); - memcpy(&mb_data_.masterBoardTemperature, &buf[offset], - sizeof(mb_data_.masterBoardTemperature)); - offset += sizeof(mb_data_.masterBoardTemperature); - mb_data_.masterBoardTemperature = ntohl(mb_data_.masterBoardTemperature); - memcpy(&mb_data_.robotVoltage48V, &buf[offset], - sizeof(mb_data_.robotVoltage48V)); - offset += sizeof(mb_data_.robotVoltage48V); - mb_data_.robotVoltage48V = ntohl(mb_data_.robotVoltage48V); - memcpy(&mb_data_.robotCurrent, &buf[offset], sizeof(mb_data_.robotCurrent)); - offset += sizeof(mb_data_.robotCurrent); - mb_data_.robotCurrent = ntohl(mb_data_.robotCurrent); - memcpy(&mb_data_.masterIOCurrent, &buf[offset], - sizeof(mb_data_.masterIOCurrent)); - offset += sizeof(mb_data_.masterIOCurrent); - mb_data_.masterIOCurrent = ntohl(mb_data_.masterIOCurrent); + memcpy(&mb_data_.masterBoardTemperature, &buf[offset], + sizeof(mb_data_.masterBoardTemperature)); + offset += sizeof(mb_data_.masterBoardTemperature); + mb_data_.masterBoardTemperature = ntohl(mb_data_.masterBoardTemperature); + memcpy(&mb_data_.robotVoltage48V, &buf[offset], + sizeof(mb_data_.robotVoltage48V)); + offset += sizeof(mb_data_.robotVoltage48V); + mb_data_.robotVoltage48V = ntohl(mb_data_.robotVoltage48V); + memcpy(&mb_data_.robotCurrent, &buf[offset], sizeof(mb_data_.robotCurrent)); + offset += sizeof(mb_data_.robotCurrent); + mb_data_.robotCurrent = ntohl(mb_data_.robotCurrent); + memcpy(&mb_data_.masterIOCurrent, &buf[offset], + sizeof(mb_data_.masterIOCurrent)); + offset += sizeof(mb_data_.masterIOCurrent); + mb_data_.masterIOCurrent = ntohl(mb_data_.masterIOCurrent); - memcpy(&mb_data_.safetyMode, &buf[offset], sizeof(mb_data_.safetyMode)); - offset += sizeof(mb_data_.safetyMode); - memcpy(&mb_data_.masterOnOffState, &buf[offset], - sizeof(mb_data_.masterOnOffState)); - offset += sizeof(mb_data_.masterOnOffState); + memcpy(&mb_data_.safetyMode, &buf[offset], sizeof(mb_data_.safetyMode)); + offset += sizeof(mb_data_.safetyMode); + memcpy(&mb_data_.masterOnOffState, &buf[offset], + sizeof(mb_data_.masterOnOffState)); + offset += sizeof(mb_data_.masterOnOffState); - memcpy(&mb_data_.euromap67InterfaceInstalled, &buf[offset], - sizeof(mb_data_.euromap67InterfaceInstalled)); - offset += sizeof(mb_data_.euromap67InterfaceInstalled); - if (mb_data_.euromap67InterfaceInstalled != 0) { - memcpy(&mb_data_.euromapInputBits, &buf[offset], - sizeof(mb_data_.euromapInputBits)); - offset += sizeof(mb_data_.euromapInputBits); - mb_data_.euromapInputBits = ntohl(mb_data_.euromapInputBits); - memcpy(&mb_data_.euromapOutputBits, &buf[offset], - sizeof(mb_data_.euromapOutputBits)); - offset += sizeof(mb_data_.euromapOutputBits); - mb_data_.euromapOutputBits = ntohl(mb_data_.euromapOutputBits); - if (RobotState::getVersion() < 3.0) { - int16_t euromap_voltage, euromap_current; - memcpy(&euromap_voltage, &buf[offset], sizeof(euromap_voltage)); - offset += sizeof(euromap_voltage); - memcpy(&euromap_current, &buf[offset], sizeof(euromap_current)); - offset += sizeof(euromap_current); - mb_data_.euromapVoltage = ntohs(euromap_voltage); - mb_data_.euromapCurrent = ntohs(euromap_current); - } else { - memcpy(&mb_data_.euromapVoltage, &buf[offset], - sizeof(mb_data_.euromapVoltage)); - offset += sizeof(mb_data_.euromapVoltage); - mb_data_.euromapVoltage = ntohl(mb_data_.euromapVoltage); - memcpy(&mb_data_.euromapCurrent, &buf[offset], - sizeof(mb_data_.euromapCurrent)); - offset += sizeof(mb_data_.euromapCurrent); - mb_data_.euromapCurrent = ntohl(mb_data_.euromapCurrent); - } - - } + memcpy(&mb_data_.euromap67InterfaceInstalled, &buf[offset], + sizeof(mb_data_.euromap67InterfaceInstalled)); + offset += sizeof(mb_data_.euromap67InterfaceInstalled); + if (mb_data_.euromap67InterfaceInstalled != 0) { + memcpy(&mb_data_.euromapInputBits, &buf[offset], + sizeof(mb_data_.euromapInputBits)); + offset += sizeof(mb_data_.euromapInputBits); + mb_data_.euromapInputBits = ntohl(mb_data_.euromapInputBits); + memcpy(&mb_data_.euromapOutputBits, &buf[offset], + sizeof(mb_data_.euromapOutputBits)); + offset += sizeof(mb_data_.euromapOutputBits); + mb_data_.euromapOutputBits = ntohl(mb_data_.euromapOutputBits); + if (RobotState::getVersion() < 3.0) { + int16_t euromap_voltage, euromap_current; + memcpy(&euromap_voltage, &buf[offset], sizeof(euromap_voltage)); + offset += sizeof(euromap_voltage); + memcpy(&euromap_current, &buf[offset], sizeof(euromap_current)); + offset += sizeof(euromap_current); + mb_data_.euromapVoltage = ntohs(euromap_voltage); + mb_data_.euromapCurrent = ntohs(euromap_current); + } else { + memcpy(&mb_data_.euromapVoltage, &buf[offset], + sizeof(mb_data_.euromapVoltage)); + offset += sizeof(mb_data_.euromapVoltage); + mb_data_.euromapVoltage = ntohl(mb_data_.euromapVoltage); + memcpy(&mb_data_.euromapCurrent, &buf[offset], + sizeof(mb_data_.euromapCurrent)); + offset += sizeof(mb_data_.euromapCurrent); + mb_data_.euromapCurrent = ntohl(mb_data_.euromapCurrent); + } + } } -double RobotState::getVersion() { - double ver; - val_lock_.lock(); - ver = version_msg_.major_version + 0.1 * version_msg_.minor_version - + .0000001 * version_msg_.svn_revision; - val_lock_.unlock(); - return ver; - +double RobotState::getVersion() +{ + double ver; + val_lock_.lock(); + ver = version_msg_.major_version + 0.1 * version_msg_.minor_version + + .0000001 * version_msg_.svn_revision; + val_lock_.unlock(); + return ver; } -void RobotState::finishedReading() { - new_data_available_ = false; +void RobotState::finishedReading() +{ + new_data_available_ = false; } -bool RobotState::getNewDataAvailable() { - return new_data_available_; +bool RobotState::getNewDataAvailable() +{ + return new_data_available_; } -int RobotState::getDigitalInputBits() { - return mb_data_.digitalInputBits; +int RobotState::getDigitalInputBits() +{ + return mb_data_.digitalInputBits; } -int RobotState::getDigitalOutputBits() { - return mb_data_.digitalOutputBits; +int RobotState::getDigitalOutputBits() +{ + return mb_data_.digitalOutputBits; } -double RobotState::getAnalogInput0() { - return mb_data_.analogInput0; +double RobotState::getAnalogInput0() +{ + return mb_data_.analogInput0; } -double RobotState::getAnalogInput1() { - return mb_data_.analogInput1; +double RobotState::getAnalogInput1() +{ + return mb_data_.analogInput1; } -double RobotState::getAnalogOutput0() { - return mb_data_.analogOutput0; - +double RobotState::getAnalogOutput0() +{ + return mb_data_.analogOutput0; } -double RobotState::getAnalogOutput1() { - return mb_data_.analogOutput1; +double RobotState::getAnalogOutput1() +{ + return mb_data_.analogOutput1; } -bool RobotState::isRobotConnected() { - return robot_mode_.isRobotConnected; +bool RobotState::isRobotConnected() +{ + return robot_mode_.isRobotConnected; } -bool RobotState::isRealRobotEnabled() { - return robot_mode_.isRealRobotEnabled; +bool RobotState::isRealRobotEnabled() +{ + return robot_mode_.isRealRobotEnabled; } -bool RobotState::isPowerOnRobot() { - return robot_mode_.isPowerOnRobot; +bool RobotState::isPowerOnRobot() +{ + return robot_mode_.isPowerOnRobot; } -bool RobotState::isEmergencyStopped() { - return robot_mode_.isEmergencyStopped; +bool RobotState::isEmergencyStopped() +{ + return robot_mode_.isEmergencyStopped; } -bool RobotState::isProtectiveStopped() { - return robot_mode_.isProtectiveStopped; +bool RobotState::isProtectiveStopped() +{ + return robot_mode_.isProtectiveStopped; } -bool RobotState::isProgramRunning() { - return robot_mode_.isProgramRunning; +bool RobotState::isProgramRunning() +{ + return robot_mode_.isProgramRunning; } -bool RobotState::isProgramPaused() { - return robot_mode_.isProgramPaused; +bool RobotState::isProgramPaused() +{ + return robot_mode_.isProgramPaused; } -unsigned char RobotState::getRobotMode() { - return robot_mode_.robotMode; +unsigned char RobotState::getRobotMode() +{ + return robot_mode_.robotMode; } -bool RobotState::isReady() { - if (robot_mode_.robotMode == robot_mode_running_) { - return true; - } - return false; +bool RobotState::isReady() +{ + if (robot_mode_.robotMode == robot_mode_running_) { + return true; + } + return false; } -void RobotState::setDisconnected() { - robot_mode_.isRobotConnected = false; - robot_mode_.isRealRobotEnabled = false; - robot_mode_.isPowerOnRobot = false; +void RobotState::setDisconnected() +{ + robot_mode_.isRobotConnected = false; + robot_mode_.isRealRobotEnabled = false; + robot_mode_.isPowerOnRobot = false; } diff --git a/src/robot_state_RT.cpp b/src/robot_state_RT.cpp index b170a04d7..46467fea2 100644 --- a/src/robot_state_RT.cpp +++ b/src/robot_state_RT.cpp @@ -18,420 +18,458 @@ #include "ur_modern_driver/robot_state_RT.h" -RobotStateRT::RobotStateRT(std::condition_variable& msg_cond) { - version_ = 0.0; - time_ = 0.0; - q_target_.assign(6, 0.0); - qd_target_.assign(6, 0.0); - qdd_target_.assign(6, 0.0); - i_target_.assign(6, 0.0); - m_target_.assign(6, 0.0); - q_actual_.assign(6, 0.0); - qd_actual_.assign(6, 0.0); - i_actual_.assign(6, 0.0); - i_control_.assign(6, 0.0); - tool_vector_actual_.assign(6, 0.0); - tcp_speed_actual_.assign(6, 0.0); - tcp_force_.assign(6, 0.0); - tool_vector_target_.assign(6, 0.0); - tcp_speed_target_.assign(6, 0.0); - digital_input_bits_.assign(64, false); - motor_temperatures_.assign(6, 0.0); - controller_timer_ = 0.0; - robot_mode_ = 0.0; - joint_modes_.assign(6, 0.0); - safety_mode_ = 0.0; - tool_accelerometer_values_.assign(3, 0.0); - speed_scaling_ = 0.0; - linear_momentum_norm_ = 0.0; - v_main_ = 0.0; - v_robot_ = 0.0; - i_robot_ = 0.0; - v_actual_.assign(6, 0.0); - data_published_ = false; - controller_updated_ = false; - pMsg_cond_ = &msg_cond; +RobotStateRT::RobotStateRT(std::condition_variable& msg_cond) +{ + version_ = 0.0; + time_ = 0.0; + q_target_.assign(6, 0.0); + qd_target_.assign(6, 0.0); + qdd_target_.assign(6, 0.0); + i_target_.assign(6, 0.0); + m_target_.assign(6, 0.0); + q_actual_.assign(6, 0.0); + qd_actual_.assign(6, 0.0); + i_actual_.assign(6, 0.0); + i_control_.assign(6, 0.0); + tool_vector_actual_.assign(6, 0.0); + tcp_speed_actual_.assign(6, 0.0); + tcp_force_.assign(6, 0.0); + tool_vector_target_.assign(6, 0.0); + tcp_speed_target_.assign(6, 0.0); + digital_input_bits_.assign(64, false); + motor_temperatures_.assign(6, 0.0); + controller_timer_ = 0.0; + robot_mode_ = 0.0; + joint_modes_.assign(6, 0.0); + safety_mode_ = 0.0; + tool_accelerometer_values_.assign(3, 0.0); + speed_scaling_ = 0.0; + linear_momentum_norm_ = 0.0; + v_main_ = 0.0; + v_robot_ = 0.0; + i_robot_ = 0.0; + v_actual_.assign(6, 0.0); + data_published_ = false; + controller_updated_ = false; + pMsg_cond_ = &msg_cond; } -RobotStateRT::~RobotStateRT() { - /* Make sure nobody is waiting after this thread is destroyed */ - data_published_ = true; - controller_updated_ = true; - pMsg_cond_->notify_all(); +RobotStateRT::~RobotStateRT() +{ + /* Make sure nobody is waiting after this thread is destroyed */ + data_published_ = true; + controller_updated_ = true; + pMsg_cond_->notify_all(); } -void RobotStateRT::setDataPublished() { - data_published_ = false; +void RobotStateRT::setDataPublished() +{ + data_published_ = false; } -bool RobotStateRT::getDataPublished() { - return data_published_; +bool RobotStateRT::getDataPublished() +{ + return data_published_; } -void RobotStateRT::setControllerUpdated() { - controller_updated_ = false; +void RobotStateRT::setControllerUpdated() +{ + controller_updated_ = false; } -bool RobotStateRT::getControllerUpdated() { - return controller_updated_; +bool RobotStateRT::getControllerUpdated() +{ + return controller_updated_; } -double RobotStateRT::ntohd(uint64_t nf) { - double x; - nf = be64toh(nf); - memcpy(&x, &nf, sizeof(x)); - return x; +double RobotStateRT::ntohd(uint64_t nf) +{ + double x; + nf = be64toh(nf); + memcpy(&x, &nf, sizeof(x)); + return x; } -std::vector RobotStateRT::unpackVector(uint8_t * buf, int start_index, - int nr_of_vals) { - uint64_t q; - std::vector ret; - for (int i = 0; i < nr_of_vals; i++) { - memcpy(&q, &buf[start_index + i * sizeof(q)], sizeof(q)); - ret.push_back(ntohd(q)); - } - return ret; +std::vector RobotStateRT::unpackVector(uint8_t* buf, int start_index, + int nr_of_vals) +{ + uint64_t q; + std::vector ret; + for (int i = 0; i < nr_of_vals; i++) { + memcpy(&q, &buf[start_index + i * sizeof(q)], sizeof(q)); + ret.push_back(ntohd(q)); + } + return ret; } -std::vector RobotStateRT::unpackDigitalInputBits(int64_t data) { - std::vector ret; - for (int i = 0; i < 64; i++) { - ret.push_back((data & (1 << i)) >> i); - } - return ret; +std::vector RobotStateRT::unpackDigitalInputBits(int64_t data) +{ + std::vector ret; + for (int i = 0; i < 64; i++) { + ret.push_back((data & (1 << i)) >> i); + } + return ret; } -void RobotStateRT::setVersion(double ver) { - val_lock_.lock(); - version_ = ver; - val_lock_.unlock(); +void RobotStateRT::setVersion(double ver) +{ + val_lock_.lock(); + version_ = ver; + val_lock_.unlock(); } -double RobotStateRT::getVersion() { - double ret; - val_lock_.lock(); - ret = version_; - val_lock_.unlock(); - return ret; -} -double RobotStateRT::getTime() { - double ret; - val_lock_.lock(); - ret = time_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getQTarget() { - std::vector ret; - val_lock_.lock(); - ret = q_target_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getQdTarget() { - std::vector ret; - val_lock_.lock(); - ret = qd_target_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getQddTarget() { - std::vector ret; - val_lock_.lock(); - ret = qdd_target_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getITarget() { - std::vector ret; - val_lock_.lock(); - ret = i_target_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getMTarget() { - std::vector ret; - val_lock_.lock(); - ret = m_target_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getQActual() { - std::vector ret; - val_lock_.lock(); - ret = q_actual_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getQdActual() { - std::vector ret; - val_lock_.lock(); - ret = qd_actual_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getIActual() { - std::vector ret; - val_lock_.lock(); - ret = i_actual_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getIControl() { - std::vector ret; - val_lock_.lock(); - ret = i_control_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getToolVectorActual() { - std::vector ret; - val_lock_.lock(); - ret = tool_vector_actual_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getTcpSpeedActual() { - std::vector ret; - val_lock_.lock(); - ret = tcp_speed_actual_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getTcpForce() { - std::vector ret; - val_lock_.lock(); - ret = tcp_force_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getToolVectorTarget() { - std::vector ret; - val_lock_.lock(); - ret = tool_vector_target_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getTcpSpeedTarget() { - std::vector ret; - val_lock_.lock(); - ret = tcp_speed_target_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getDigitalInputBits() { - std::vector ret; - val_lock_.lock(); - ret = digital_input_bits_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getMotorTemperatures() { - std::vector ret; - val_lock_.lock(); - ret = motor_temperatures_; - val_lock_.unlock(); - return ret; -} -double RobotStateRT::getControllerTimer() { - double ret; - val_lock_.lock(); - ret = controller_timer_; - val_lock_.unlock(); - return ret; -} -double RobotStateRT::getRobotMode() { - double ret; - val_lock_.lock(); - ret = robot_mode_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getJointModes() { - std::vector ret; - val_lock_.lock(); - ret = joint_modes_; - val_lock_.unlock(); - return ret; -} -double RobotStateRT::getSafety_mode() { - double ret; - val_lock_.lock(); - ret = safety_mode_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getToolAccelerometerValues() { - std::vector ret; - val_lock_.lock(); - ret = tool_accelerometer_values_; - val_lock_.unlock(); - return ret; -} -double RobotStateRT::getSpeedScaling() { - double ret; - val_lock_.lock(); - ret = speed_scaling_; - val_lock_.unlock(); - return ret; -} -double RobotStateRT::getLinearMomentumNorm() { - double ret; - val_lock_.lock(); - ret = linear_momentum_norm_; - val_lock_.unlock(); - return ret; -} -double RobotStateRT::getVMain() { - double ret; - val_lock_.lock(); - ret = v_main_; - val_lock_.unlock(); - return ret; -} -double RobotStateRT::getVRobot() { - double ret; - val_lock_.lock(); - ret = v_robot_; - val_lock_.unlock(); - return ret; -} -double RobotStateRT::getIRobot() { - double ret; - val_lock_.lock(); - ret = i_robot_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getVActual() { - std::vector ret; - val_lock_.lock(); - ret = v_actual_; - val_lock_.unlock(); - return ret; -} -void RobotStateRT::unpack(uint8_t * buf) { - int64_t digital_input_bits; - uint64_t unpack_to; - uint16_t offset = 0; - val_lock_.lock(); - int len; - memcpy(&len, &buf[offset], sizeof(len)); +double RobotStateRT::getVersion() +{ + double ret; + val_lock_.lock(); + ret = version_; + val_lock_.unlock(); + return ret; +} +double RobotStateRT::getTime() +{ + double ret; + val_lock_.lock(); + ret = time_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getQTarget() +{ + std::vector ret; + val_lock_.lock(); + ret = q_target_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getQdTarget() +{ + std::vector ret; + val_lock_.lock(); + ret = qd_target_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getQddTarget() +{ + std::vector ret; + val_lock_.lock(); + ret = qdd_target_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getITarget() +{ + std::vector ret; + val_lock_.lock(); + ret = i_target_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getMTarget() +{ + std::vector ret; + val_lock_.lock(); + ret = m_target_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getQActual() +{ + std::vector ret; + val_lock_.lock(); + ret = q_actual_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getQdActual() +{ + std::vector ret; + val_lock_.lock(); + ret = qd_actual_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getIActual() +{ + std::vector ret; + val_lock_.lock(); + ret = i_actual_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getIControl() +{ + std::vector ret; + val_lock_.lock(); + ret = i_control_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getToolVectorActual() +{ + std::vector ret; + val_lock_.lock(); + ret = tool_vector_actual_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getTcpSpeedActual() +{ + std::vector ret; + val_lock_.lock(); + ret = tcp_speed_actual_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getTcpForce() +{ + std::vector ret; + val_lock_.lock(); + ret = tcp_force_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getToolVectorTarget() +{ + std::vector ret; + val_lock_.lock(); + ret = tool_vector_target_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getTcpSpeedTarget() +{ + std::vector ret; + val_lock_.lock(); + ret = tcp_speed_target_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getDigitalInputBits() +{ + std::vector ret; + val_lock_.lock(); + ret = digital_input_bits_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getMotorTemperatures() +{ + std::vector ret; + val_lock_.lock(); + ret = motor_temperatures_; + val_lock_.unlock(); + return ret; +} +double RobotStateRT::getControllerTimer() +{ + double ret; + val_lock_.lock(); + ret = controller_timer_; + val_lock_.unlock(); + return ret; +} +double RobotStateRT::getRobotMode() +{ + double ret; + val_lock_.lock(); + ret = robot_mode_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getJointModes() +{ + std::vector ret; + val_lock_.lock(); + ret = joint_modes_; + val_lock_.unlock(); + return ret; +} +double RobotStateRT::getSafety_mode() +{ + double ret; + val_lock_.lock(); + ret = safety_mode_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getToolAccelerometerValues() +{ + std::vector ret; + val_lock_.lock(); + ret = tool_accelerometer_values_; + val_lock_.unlock(); + return ret; +} +double RobotStateRT::getSpeedScaling() +{ + double ret; + val_lock_.lock(); + ret = speed_scaling_; + val_lock_.unlock(); + return ret; +} +double RobotStateRT::getLinearMomentumNorm() +{ + double ret; + val_lock_.lock(); + ret = linear_momentum_norm_; + val_lock_.unlock(); + return ret; +} +double RobotStateRT::getVMain() +{ + double ret; + val_lock_.lock(); + ret = v_main_; + val_lock_.unlock(); + return ret; +} +double RobotStateRT::getVRobot() +{ + double ret; + val_lock_.lock(); + ret = v_robot_; + val_lock_.unlock(); + return ret; +} +double RobotStateRT::getIRobot() +{ + double ret; + val_lock_.lock(); + ret = i_robot_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getVActual() +{ + std::vector ret; + val_lock_.lock(); + ret = v_actual_; + val_lock_.unlock(); + return ret; +} +void RobotStateRT::unpack(uint8_t* buf) +{ + int64_t digital_input_bits; + uint64_t unpack_to; + uint16_t offset = 0; + val_lock_.lock(); + int len; + memcpy(&len, &buf[offset], sizeof(len)); - offset += sizeof(len); - len = ntohl(len); + offset += sizeof(len); + len = ntohl(len); - //Check the correct message length is received - bool len_good = true; - if (version_ >= 1.6 && version_ < 1.7) { //v1.6 - if (len != 756) - len_good = false; - } else if (version_ >= 1.7 && version_ < 1.8) { //v1.7 - if (len != 764) - len_good = false; - } else if (version_ >= 1.8 && version_ < 1.9) { //v1.8 - if (len != 812) - len_good = false; - } else if (version_ >= 3.0 && version_ < 3.2) { //v3.0 & v3.1 - if (len != 1044) - len_good = false; - } else if (version_ >= 3.2 && version_ < 3.3) { //v3.2 - if (len != 1060) - len_good = false; - } + //Check the correct message length is received + bool len_good = true; + if (version_ >= 1.6 && version_ < 1.7) { //v1.6 + if (len != 756) + len_good = false; + } else if (version_ >= 1.7 && version_ < 1.8) { //v1.7 + if (len != 764) + len_good = false; + } else if (version_ >= 1.8 && version_ < 1.9) { //v1.8 + if (len != 812) + len_good = false; + } else if (version_ >= 3.0 && version_ < 3.2) { //v3.0 & v3.1 + if (len != 1044) + len_good = false; + } else if (version_ >= 3.2 && version_ < 3.3) { //v3.2 + if (len != 1060) + len_good = false; + } - if (!len_good) { - printf("Wrong length of message on RT interface: %i\n", len); - val_lock_.unlock(); - return; - } + if (!len_good) { + printf("Wrong length of message on RT interface: %i\n", len); + val_lock_.unlock(); + return; + } - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - time_ = RobotStateRT::ntohd(unpack_to); - offset += sizeof(double); - q_target_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - qd_target_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - qdd_target_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - i_target_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - m_target_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - q_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - qd_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - i_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - if (version_ <= 1.9) { - if (version_ > 1.6) - tool_accelerometer_values_ = unpackVector(buf, offset, 3); - offset += sizeof(double) * (3 + 15); - tcp_force_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tool_vector_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tcp_speed_actual_ = unpackVector(buf, offset, 6); - } else { - i_control_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tool_vector_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tcp_speed_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tcp_force_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tool_vector_target_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tcp_speed_target_ = unpackVector(buf, offset, 6); - } - offset += sizeof(double) * 6; - - memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits)); - digital_input_bits_ = unpackDigitalInputBits(be64toh(digital_input_bits)); - offset += sizeof(double); - motor_temperatures_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - controller_timer_ = ntohd(unpack_to); - if (version_ > 1.6) { - offset += sizeof(double) * 2; - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - robot_mode_ = ntohd(unpack_to); - if (version_ > 1.7) { - offset += sizeof(double); - joint_modes_ = unpackVector(buf, offset, 6); - } - } - if (version_ > 1.8) { - offset += sizeof(double) * 6; - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - safety_mode_ = ntohd(unpack_to); - offset += sizeof(double); - tool_accelerometer_values_ = unpackVector(buf, offset, 3); - offset += sizeof(double) * 3; - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - speed_scaling_ = ntohd(unpack_to); - offset += sizeof(double); - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - linear_momentum_norm_ = ntohd(unpack_to); - offset += sizeof(double); - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - v_main_ = ntohd(unpack_to); - offset += sizeof(double); - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - v_robot_ = ntohd(unpack_to); - offset += sizeof(double); - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - i_robot_ = ntohd(unpack_to); - offset += sizeof(double); - v_actual_ = unpackVector(buf, offset, 6); - } - val_lock_.unlock(); - controller_updated_ = true; - data_published_ = true; - pMsg_cond_->notify_all(); + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); + time_ = RobotStateRT::ntohd(unpack_to); + offset += sizeof(double); + q_target_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + qd_target_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + qdd_target_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + i_target_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + m_target_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + q_actual_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + qd_actual_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + i_actual_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + if (version_ <= 1.9) { + if (version_ > 1.6) + tool_accelerometer_values_ = unpackVector(buf, offset, 3); + offset += sizeof(double) * (3 + 15); + tcp_force_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + tool_vector_actual_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + tcp_speed_actual_ = unpackVector(buf, offset, 6); + } else { + i_control_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + tool_vector_actual_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + tcp_speed_actual_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + tcp_force_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + tool_vector_target_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + tcp_speed_target_ = unpackVector(buf, offset, 6); + } + offset += sizeof(double) * 6; + memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits)); + digital_input_bits_ = unpackDigitalInputBits(be64toh(digital_input_bits)); + offset += sizeof(double); + motor_temperatures_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); + controller_timer_ = ntohd(unpack_to); + if (version_ > 1.6) { + offset += sizeof(double) * 2; + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); + robot_mode_ = ntohd(unpack_to); + if (version_ > 1.7) { + offset += sizeof(double); + joint_modes_ = unpackVector(buf, offset, 6); + } + } + if (version_ > 1.8) { + offset += sizeof(double) * 6; + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); + safety_mode_ = ntohd(unpack_to); + offset += sizeof(double); + tool_accelerometer_values_ = unpackVector(buf, offset, 3); + offset += sizeof(double) * 3; + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); + speed_scaling_ = ntohd(unpack_to); + offset += sizeof(double); + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); + linear_momentum_norm_ = ntohd(unpack_to); + offset += sizeof(double); + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); + v_main_ = ntohd(unpack_to); + offset += sizeof(double); + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); + v_robot_ = ntohd(unpack_to); + offset += sizeof(double); + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); + i_robot_ = ntohd(unpack_to); + offset += sizeof(double); + v_actual_ = unpackVector(buf, offset, 6); + } + val_lock_.unlock(); + controller_updated_ = true; + data_published_ = true; + pMsg_cond_->notify_all(); } - diff --git a/src/ros/rt_publisher.cpp b/src/ros/rt_publisher.cpp index c0db69060..af5a4a47a 100644 --- a/src/ros/rt_publisher.cpp +++ b/src/ros/rt_publisher.cpp @@ -1,17 +1,18 @@ #include "ur_modern_driver/ros/rt_publisher.h" -bool RTPublisher::publish_joints(RTShared &packet, ros::Time &t) { - sensor_msgs::JointState joint_msg; +bool RTPublisher::publish_joints(RTShared& packet, ros::Time& t) +{ + sensor_msgs::JointState joint_msg; joint_msg.header.stamp = t; joint_msg.name = _joint_names; - for(auto const& q : packet.q_actual) { + for (auto const& q : packet.q_actual) { joint_msg.position.push_back(q); } - for(auto const& qd : packet.qd_actual) { + for (auto const& qd : packet.qd_actual) { joint_msg.velocity.push_back(qd); } - for(auto const& i : packet.i_actual) { + for (auto const& i : packet.i_actual) { joint_msg.effort.push_back(i); } @@ -20,7 +21,8 @@ bool RTPublisher::publish_joints(RTShared &packet, ros::Time &t) { return true; } -bool RTPublisher::publish_wrench(RTShared &packet, ros::Time &t) { +bool RTPublisher::publish_wrench(RTShared& packet, ros::Time& t) +{ geometry_msgs::WrenchStamped wrench_msg; wrench_msg.header.stamp = t; @@ -36,7 +38,8 @@ bool RTPublisher::publish_wrench(RTShared &packet, ros::Time &t) { return true; } -bool RTPublisher::publish_tool(RTShared &packet, ros::Time &t) { +bool RTPublisher::publish_tool(RTShared& packet, ros::Time& t) +{ geometry_msgs::TwistStamped tool_twist; tool_twist.header.stamp = t; @@ -55,20 +58,25 @@ bool RTPublisher::publish_tool(RTShared &packet, ros::Time &t) { return true; } -bool RTPublisher::publish(RTShared &packet) { +bool RTPublisher::publish(RTShared& packet) +{ ros::Time time = ros::Time::now(); return publish_joints(packet, time) && publish_wrench(packet, time) && publish_tool(packet, time); } -bool RTPublisher::consume(RTState_V1_6__7 &state) { +bool RTPublisher::consume(RTState_V1_6__7& state) +{ return publish(state); } -bool RTPublisher::consume(RTState_V1_8 &state) { +bool RTPublisher::consume(RTState_V1_8& state) +{ return publish(state); } -bool RTPublisher::consume(RTState_V3_0__1 &state) { +bool RTPublisher::consume(RTState_V3_0__1& state) +{ return publish(state); } -bool RTPublisher::consume(RTState_V3_2__3 &state) { +bool RTPublisher::consume(RTState_V3_2__3& state) +{ return publish(state); } \ No newline at end of file diff --git a/src/ros_main.cpp b/src/ros_main.cpp index c2dc749a6..48cf6b640 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -1,18 +1,18 @@ +#include #include +#include #include -#include #include -#include #include "ur_modern_driver/log.h" #include "ur_modern_driver/pipeline.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/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/state.h" static const std::string IP_ADDR_ARG("~robot_ip_address"); static const std::string REVERSE_PORT_ARG("~reverse_port"); @@ -21,48 +21,48 @@ static const std::string PREFIX_ARG("~prefix"); static const std::string BASE_FRAME_ARG("~base_frame"); static const std::string TOOL_FRAME_ARG("~tool_frame"); - struct ProgArgs { public: - std::string host; - std::string prefix; - std::string base_frame; - std::string tool_frame; - int32_t reverse_port; - bool use_sim_time; + std::string host; + std::string prefix; + std::string base_frame; + std::string tool_frame; + int32_t reverse_port; + bool use_sim_time; }; -bool parse_args(ProgArgs &args) { - if(!ros::param::get(IP_ADDR_ARG, args.host)) { - LOG_ERROR("robot_ip_address parameter must be set!"); - return false; - } - ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001)); - ros::param::param(SIM_TIME_ARG, args.use_sim_time, false); - ros::param::param(PREFIX_ARG, args.prefix, std::string()); - ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link"); - ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller"); - return true; +bool parse_args(ProgArgs& args) +{ + if (!ros::param::get(IP_ADDR_ARG, args.host)) { + LOG_ERROR("robot_ip_address parameter must be set!"); + return false; + } + ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001)); + ros::param::param(SIM_TIME_ARG, args.use_sim_time, false); + ros::param::param(PREFIX_ARG, args.prefix, std::string()); + ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link"); + ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller"); + return true; } - -int main(int argc, char **argv) { +int main(int argc, char** argv) +{ ros::init(argc, argv, "ur_driver"); - ProgArgs args; - if(!parse_args(args)) { - return EXIT_FAILURE; - } + ProgArgs args; + if (!parse_args(args)) { + return EXIT_FAILURE; + } - URFactory factory(args.host); + URFactory factory(args.host); - auto parser = factory.get_rt_parser(); + auto parser = factory.get_rt_parser(); - URStream s2(args.host, 30003); - URProducer p2(s2, *parser); + URStream s2(args.host, 30003); + URProducer p2(s2, *parser); - /* + /* p2.setup_producer(); while(true) { @@ -77,18 +77,17 @@ int main(int argc, char **argv) { p2.teardown_producer(); */ - RTPublisher pub(args.prefix, args.base_frame); + RTPublisher pub(args.prefix, args.base_frame); - Pipeline pl(p2, pub); + Pipeline pl(p2, pub); - pl.run(); - - while(ros::ok()) { - std::this_thread::sleep_for(std::chrono::milliseconds(33)); - } + pl.run(); - pl.stop(); + while (ros::ok()) { + std::this_thread::sleep_for(std::chrono::milliseconds(33)); + } + pl.stop(); - return EXIT_SUCCESS; + return EXIT_SUCCESS; } \ No newline at end of file diff --git a/src/ur/master_board.cpp b/src/ur/master_board.cpp index c8d947b49..046d0eed1 100644 --- a/src/ur/master_board.cpp +++ b/src/ur/master_board.cpp @@ -1,11 +1,12 @@ #include "ur_modern_driver/ur/master_board.h" #include "ur_modern_driver/ur/consumer.h" -bool SharedMasterBoardData::parse_with(BinParser &bp) { +bool SharedMasterBoardData::parse_with(BinParser& bp) +{ bp.parse(analog_input_range0); bp.parse(analog_input_range1); bp.parse(analog_input0); - bp.parse(analog_input1); + bp.parse(analog_input1); bp.parse(analog_output_domain0); bp.parse(analog_output_domain1); bp.parse(analog_output0); @@ -17,8 +18,9 @@ bool SharedMasterBoardData::parse_with(BinParser &bp) { return true; } -bool MasterBoardData_V1_X::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool MasterBoardData_V1_X::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; bp.parse(digital_input_bits); @@ -30,8 +32,8 @@ bool MasterBoardData_V1_X::parse_with(BinParser &bp) { bp.parse(master_on_off_state); bp.parse(euromap67_interface_installed); - if(euromap67_interface_installed) { - if(!bp.check_size(MasterBoardData_V1_X::EURO_SIZE)) + if (euromap67_interface_installed) { + if (!bp.check_size(MasterBoardData_V1_X::EURO_SIZE)) return false; bp.parse(euromap_voltage); @@ -41,23 +43,24 @@ bool MasterBoardData_V1_X::parse_with(BinParser &bp) { return true; } -bool MasterBoardData_V3_0__1::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool MasterBoardData_V3_0__1::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; bp.parse(digital_input_bits); bp.parse(digital_output_bits); - + SharedMasterBoardData::parse_with(bp); bp.parse(safety_mode); bp.parse(in_reduced_mode); bp.parse(euromap67_interface_installed); - if(euromap67_interface_installed) { - if(!bp.check_size(MasterBoardData_V3_0__1::EURO_SIZE)) + if (euromap67_interface_installed) { + if (!bp.check_size(MasterBoardData_V3_0__1::EURO_SIZE)) return false; - + bp.parse(euromap_voltage); bp.parse(euromap_current); } @@ -67,10 +70,9 @@ bool MasterBoardData_V3_0__1::parse_with(BinParser &bp) { return true; } - - -bool MasterBoardData_V3_2::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool MasterBoardData_V3_2::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; MasterBoardData_V3_0__1::parse_with(bp); @@ -81,16 +83,15 @@ bool MasterBoardData_V3_2::parse_with(BinParser &bp) { return true; } - - - - -bool MasterBoardData_V1_X::consume_with(URStatePacketConsumer &consumer) { +bool MasterBoardData_V1_X::consume_with(URStatePacketConsumer& consumer) +{ return consumer.consume(*this); } -bool MasterBoardData_V3_0__1::consume_with(URStatePacketConsumer &consumer) { +bool MasterBoardData_V3_0__1::consume_with(URStatePacketConsumer& consumer) +{ return consumer.consume(*this); } -bool MasterBoardData_V3_2::consume_with(URStatePacketConsumer &consumer) { +bool MasterBoardData_V3_2::consume_with(URStatePacketConsumer& consumer) +{ return consumer.consume(*this); } \ No newline at end of file diff --git a/src/ur/messages.cpp b/src/ur/messages.cpp index e079f8d7c..b53279210 100644 --- a/src/ur/messages.cpp +++ b/src/ur/messages.cpp @@ -1,9 +1,9 @@ #include "ur_modern_driver/ur/messages.h" #include "ur_modern_driver/ur/consumer.h" +bool VersionMessage::parse_with(BinParser& bp) +{ -bool VersionMessage::parse_with(BinParser &bp) { - bp.parse(project_name); bp.parse(major_version); bp.parse(minor_version); @@ -11,10 +11,10 @@ bool VersionMessage::parse_with(BinParser &bp) { bp.consume(sizeof(uint32_t)); //undocumented field?? bp.parse_remainder(build_date); - return true; //not really possible to check dynamic size packets + return true; //not really possible to check dynamic size packets } - -bool VersionMessage::consume_with(URMessagePacketConsumer &consumer) { +bool VersionMessage::consume_with(URMessagePacketConsumer& consumer) +{ return consumer.consume(*this); } \ No newline at end of file diff --git a/src/ur/robot_mode.cpp b/src/ur/robot_mode.cpp index 1c6240633..be0770f62 100644 --- a/src/ur/robot_mode.cpp +++ b/src/ur/robot_mode.cpp @@ -1,8 +1,8 @@ #include "ur_modern_driver/ur/robot_mode.h" #include "ur_modern_driver/ur/consumer.h" - -bool SharedRobotModeData::parse_with(BinParser &bp) { +bool SharedRobotModeData::parse_with(BinParser& bp) +{ bp.parse(timestamp); bp.parse(physical_robot_connected); bp.parse(real_robot_enabled); @@ -11,9 +11,9 @@ bool SharedRobotModeData::parse_with(BinParser &bp) { return true; } - -bool RobotModeData_V1_X::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool RobotModeData_V1_X::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; SharedRobotModeData::parse_with(bp); @@ -27,13 +27,13 @@ bool RobotModeData_V1_X::parse_with(BinParser &bp) { return true; } - -bool RobotModeData_V3_0__1::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool RobotModeData_V3_0__1::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; SharedRobotModeData::parse_with(bp); - + bp.parse(protective_stopped); bp.parse(program_running); bp.parse(program_paused); @@ -45,8 +45,9 @@ bool RobotModeData_V3_0__1::parse_with(BinParser &bp) { return true; } -bool RobotModeData_V3_2::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool RobotModeData_V3_2::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; RobotModeData_V3_0__1::parse_with(bp); @@ -56,15 +57,15 @@ bool RobotModeData_V3_2::parse_with(BinParser &bp) { return true; } - - - -bool RobotModeData_V1_X::consume_with(URStatePacketConsumer &consumer) { +bool RobotModeData_V1_X::consume_with(URStatePacketConsumer& consumer) +{ return consumer.consume(*this); } -bool RobotModeData_V3_0__1::consume_with(URStatePacketConsumer &consumer) { +bool RobotModeData_V3_0__1::consume_with(URStatePacketConsumer& consumer) +{ return consumer.consume(*this); } -bool RobotModeData_V3_2::consume_with(URStatePacketConsumer &consumer) { +bool RobotModeData_V3_2::consume_with(URStatePacketConsumer& consumer) +{ return consumer.consume(*this); } \ No newline at end of file diff --git a/src/ur/rt_state.cpp b/src/ur/rt_state.cpp index 596134dec..f13aa7305 100644 --- a/src/ur/rt_state.cpp +++ b/src/ur/rt_state.cpp @@ -1,8 +1,8 @@ #include "ur_modern_driver/ur/rt_state.h" #include "ur_modern_driver/ur/consumer.h" - -bool RTShared::parse_shared1(BinParser &bp) { +bool RTShared::parse_shared1(BinParser& bp) +{ bp.parse(time); bp.parse(q_target); bp.parse(qd_target); @@ -15,7 +15,8 @@ bool RTShared::parse_shared1(BinParser &bp) { return true; } -bool RTShared::parse_shared2(BinParser &bp) { +bool RTShared::parse_shared2(BinParser& bp) +{ bp.parse(digital_input); bp.parse(motor_temperatures); bp.parse(controller_time); @@ -23,13 +24,13 @@ bool RTShared::parse_shared2(BinParser &bp) { return true; } - -bool RTState_V1_6__7::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool RTState_V1_6__7::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; parse_shared1(bp); - + bp.parse(tool_accelerometer_values); bp.parse(tcp_force); bp.parse(tool_vector_actual); @@ -40,8 +41,9 @@ bool RTState_V1_6__7::parse_with(BinParser &bp) { return true; } -bool RTState_V1_8::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool RTState_V1_8::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; RTState_V1_6__7::parse_with(bp); @@ -51,8 +53,9 @@ bool RTState_V1_8::parse_with(BinParser &bp) { return true; } -bool RTState_V3_0__1::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool RTState_V3_0__1::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; parse_shared1(bp); @@ -74,7 +77,7 @@ bool RTState_V3_0__1::parse_with(BinParser &bp) { bp.parse(speed_scaling); bp.parse(linear_momentum_norm); bp.consume(sizeof(double)); //skip undocumented - bp.consume(sizeof(double)); //skip undocumented + bp.consume(sizeof(double)); //skip undocumented bp.parse(v_main); bp.parse(v_robot); bp.parse(i_robot); @@ -83,8 +86,9 @@ bool RTState_V3_0__1::parse_with(BinParser &bp) { return true; } -bool RTState_V3_2__3::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool RTState_V3_2__3::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; RTState_V3_0__1::parse_with(bp); @@ -95,15 +99,19 @@ bool RTState_V3_2__3::parse_with(BinParser &bp) { return true; } -bool RTState_V1_6__7::consume_with(URRTPacketConsumer &consumer) { +bool RTState_V1_6__7::consume_with(URRTPacketConsumer& consumer) +{ return consumer.consume(*this); } -bool RTState_V1_8::consume_with(URRTPacketConsumer &consumer) { +bool RTState_V1_8::consume_with(URRTPacketConsumer& consumer) +{ return consumer.consume(*this); } -bool RTState_V3_0__1::consume_with(URRTPacketConsumer &consumer) { +bool RTState_V3_0__1::consume_with(URRTPacketConsumer& consumer) +{ return consumer.consume(*this); } -bool RTState_V3_2__3::consume_with(URRTPacketConsumer &consumer) { +bool RTState_V3_2__3::consume_with(URRTPacketConsumer& consumer) +{ return consumer.consume(*this); } \ No newline at end of file diff --git a/src/ur/state.cpp b/src/ur/state.cpp index fe5af1b83..0b4f4f2e8 100644 --- a/src/ur/state.cpp +++ b/src/ur/state.cpp @@ -1,7 +1,5 @@ -#include "ur_modern_driver/log.h" #include "ur_modern_driver/ur/state.h" - - +#include "ur_modern_driver/log.h" //StatePacket::~StatePacket() { } diff --git a/src/ur/stream.cpp b/src/ur/stream.cpp index f66482b96..d0fd10f7f 100644 --- a/src/ur/stream.cpp +++ b/src/ur/stream.cpp @@ -1,13 +1,14 @@ #include -#include -#include #include +#include +#include -#include "ur_modern_driver/ur/stream.h" #include "ur_modern_driver/log.h" +#include "ur_modern_driver/ur/stream.h" -bool URStream::connect() { - if(_initialized) +bool URStream::connect() +{ + if (_initialized) return false; LOG_INFO("Connecting to UR @ %s:%d", _host.c_str(), _port); @@ -23,20 +24,20 @@ bool URStream::connect() { hints.ai_socktype = SOCK_STREAM; hints.ai_flags = AI_PASSIVE; - if(getaddrinfo(_host.c_str(), service.c_str(), &hints, &result) != 0) { - LOG_ERROR("Failed to get host name"); + if (getaddrinfo(_host.c_str(), service.c_str(), &hints, &result) != 0) { + LOG_ERROR("Failed to get host name"); return false; } - + //loop through the list of addresses untill we find one that's connectable - for(struct addrinfo *p = result; p != nullptr; p = p->ai_next) { + for (struct addrinfo* p = result; p != nullptr; p = p->ai_next) { _socket_fd = socket(p->ai_family, p->ai_socktype, p->ai_protocol); - if(_socket_fd == -1) //socket error? + if (_socket_fd == -1) //socket error? continue; - if(::connect(_socket_fd, p->ai_addr, p->ai_addrlen) != 0) { - if(_stopping) + if (::connect(_socket_fd, p->ai_addr, p->ai_addrlen) != 0) { + if (_stopping) break; else continue; //try next addrinfo if connect fails @@ -51,14 +52,15 @@ bool URStream::connect() { } freeaddrinfo(result); - if(!_initialized) + if (!_initialized) LOG_ERROR("Connection failed"); return _initialized; } -void URStream::disconnect() { - if(!_initialized || _stopping) +void URStream::disconnect() +{ + if (!_initialized || _stopping) return; _stopping = true; @@ -66,10 +68,11 @@ void URStream::disconnect() { _initialized = false; } -ssize_t URStream::send(uint8_t *buf, size_t buf_len) { - if(!_initialized) +ssize_t URStream::send(uint8_t* buf, size_t buf_len) +{ + if (!_initialized) return -1; - if(_stopping) + if (_stopping) return 0; size_t total = 0; @@ -77,9 +80,9 @@ ssize_t URStream::send(uint8_t *buf, size_t buf_len) { //TODO: handle reconnect? //handle partial sends - while(total < buf_len) { - ssize_t sent = ::send(_socket_fd, buf+total, remaining, 0); - if(sent <= 0) + while (total < buf_len) { + ssize_t sent = ::send(_socket_fd, buf + total, remaining, 0); + if (sent <= 0) return _stopping ? 0 : sent; total += sent; remaining -= sent; @@ -88,24 +91,25 @@ ssize_t URStream::send(uint8_t *buf, size_t buf_len) { return total; } -ssize_t URStream::receive(uint8_t *buf, size_t buf_len) { - if(!_initialized) +ssize_t URStream::receive(uint8_t* buf, size_t buf_len) +{ + if (!_initialized) return -1; - if(_stopping) + if (_stopping) return 0; size_t remainder = sizeof(int32_t); - uint8_t *buf_pos = buf; + uint8_t* buf_pos = buf; bool initial = true; do { ssize_t read = recv(_socket_fd, buf_pos, remainder, 0); - if(read <= 0) //failed reading from socket - return _stopping ? 0 : read; - - if(initial) { + if (read <= 0) //failed reading from socket + return _stopping ? 0 : read; + + if (initial) { remainder = be32toh(*(reinterpret_cast(buf))); - if(remainder >= (buf_len - sizeof(int32_t))) { + if (remainder >= (buf_len - sizeof(int32_t))) { LOG_ERROR("Packet size %d is larger than buffer %d, discarding.", remainder, buf_len); return -1; } @@ -113,8 +117,8 @@ ssize_t URStream::receive(uint8_t *buf, size_t buf_len) { } buf_pos += read; - remainder -= read; - } while(remainder > 0); + remainder -= read; + } while (remainder > 0); return buf_pos - buf; } \ No newline at end of file diff --git a/src/ur_communication.cpp b/src/ur_communication.cpp index cf04cc3ae..b88300451 100644 --- a/src/ur_communication.cpp +++ b/src/ur_communication.cpp @@ -19,163 +19,167 @@ #include "ur_modern_driver/ur_communication.h" UrCommunication::UrCommunication(std::condition_variable& msg_cond, - std::string host) { - robot_state_ = new RobotState(msg_cond); - bzero((char *) &pri_serv_addr_, sizeof(pri_serv_addr_)); - bzero((char *) &sec_serv_addr_, sizeof(sec_serv_addr_)); - pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (pri_sockfd_ < 0) { - print_fatal("ERROR opening socket pri_sockfd"); - } - sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (sec_sockfd_ < 0) { - print_fatal("ERROR opening socket sec_sockfd"); - } - server_ = gethostbyname(host.c_str()); - if (server_ == NULL) { - print_fatal("ERROR, unknown host"); - } - pri_serv_addr_.sin_family = AF_INET; - sec_serv_addr_.sin_family = AF_INET; - bcopy((char *) server_->h_addr, (char *)&pri_serv_addr_.sin_addr.s_addr, server_->h_length); - bcopy((char *) server_->h_addr, (char *)&sec_serv_addr_.sin_addr.s_addr, server_->h_length); - pri_serv_addr_.sin_port = htons(30001); - sec_serv_addr_.sin_port = htons(30002); - flag_ = 1; - setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, - sizeof(int)); - setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, - sizeof(int)); - setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, - sizeof(int)); - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, - sizeof(int)); - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, - sizeof(int)); - setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, - sizeof(int)); - fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); - connected_ = false; - keepalive_ = false; + std::string host) +{ + robot_state_ = new RobotState(msg_cond); + bzero((char*)&pri_serv_addr_, sizeof(pri_serv_addr_)); + bzero((char*)&sec_serv_addr_, sizeof(sec_serv_addr_)); + pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (pri_sockfd_ < 0) { + print_fatal("ERROR opening socket pri_sockfd"); + } + sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (sec_sockfd_ < 0) { + print_fatal("ERROR opening socket sec_sockfd"); + } + server_ = gethostbyname(host.c_str()); + if (server_ == NULL) { + print_fatal("ERROR, unknown host"); + } + pri_serv_addr_.sin_family = AF_INET; + sec_serv_addr_.sin_family = AF_INET; + bcopy((char*)server_->h_addr, (char*)&pri_serv_addr_.sin_addr.s_addr, server_->h_length); + bcopy((char*)server_->h_addr, (char*)&sec_serv_addr_.sin_addr.s_addr, server_->h_length); + pri_serv_addr_.sin_port = htons(30001); + sec_serv_addr_.sin_port = htons(30002); + flag_ = 1; + setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, + sizeof(int)); + setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, + sizeof(int)); + setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, + sizeof(int)); + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, + sizeof(int)); + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, + sizeof(int)); + setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, + sizeof(int)); + fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); + connected_ = false; + keepalive_ = false; } -bool UrCommunication::start() { - keepalive_ = true; - uint8_t buf[512]; - unsigned int bytes_read; - std::string cmd; - bzero(buf, 512); - print_debug("Acquire firmware version: Connecting..."); - if (connect(pri_sockfd_, (struct sockaddr *) &pri_serv_addr_, - sizeof(pri_serv_addr_)) < 0) { - print_fatal("Error connecting to get firmware version"); - return false; - } - print_debug("Acquire firmware version: Got connection"); - bytes_read = read(pri_sockfd_, buf, 512); - setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, - sizeof(int)); - robot_state_->unpack(buf, bytes_read); - //wait for some traffic so the UR socket doesn't die in version 3.1. - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - char tmp[64]; - sprintf(tmp, "Firmware version detected: %.7f", robot_state_->getVersion()); - print_debug(tmp); - close(pri_sockfd_); +bool UrCommunication::start() +{ + keepalive_ = true; + uint8_t buf[512]; + unsigned int bytes_read; + std::string cmd; + bzero(buf, 512); + print_debug("Acquire firmware version: Connecting..."); + if (connect(pri_sockfd_, (struct sockaddr*)&pri_serv_addr_, + sizeof(pri_serv_addr_)) + < 0) { + print_fatal("Error connecting to get firmware version"); + return false; + } + print_debug("Acquire firmware version: Got connection"); + bytes_read = read(pri_sockfd_, buf, 512); + setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, + sizeof(int)); + robot_state_->unpack(buf, bytes_read); + //wait for some traffic so the UR socket doesn't die in version 3.1. + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + char tmp[64]; + sprintf(tmp, "Firmware version detected: %.7f", robot_state_->getVersion()); + print_debug(tmp); + close(pri_sockfd_); - print_debug( - "Switching to secondary interface for masterboard data: Connecting..."); + print_debug( + "Switching to secondary interface for masterboard data: Connecting..."); - fd_set writefds; - struct timeval timeout; + fd_set writefds; + struct timeval timeout; - connect(sec_sockfd_, (struct sockaddr *) &sec_serv_addr_, - sizeof(sec_serv_addr_)); - FD_ZERO(&writefds); - FD_SET(sec_sockfd_, &writefds); - timeout.tv_sec = 10; - timeout.tv_usec = 0; - select(sec_sockfd_ + 1, NULL, &writefds, NULL, &timeout); - unsigned int flag_len; - getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); - if (flag_ < 0) { - print_fatal("Error connecting to secondary interface"); - return false; - } - print_debug("Secondary interface: Got connection"); - comThread_ = std::thread(&UrCommunication::run, this); - return true; + connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_, + sizeof(sec_serv_addr_)); + FD_ZERO(&writefds); + FD_SET(sec_sockfd_, &writefds); + timeout.tv_sec = 10; + timeout.tv_usec = 0; + select(sec_sockfd_ + 1, NULL, &writefds, NULL, &timeout); + unsigned int flag_len; + getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); + if (flag_ < 0) { + print_fatal("Error connecting to secondary interface"); + return false; + } + print_debug("Secondary interface: Got connection"); + comThread_ = std::thread(&UrCommunication::run, this); + return true; } -void UrCommunication::halt() { - keepalive_ = false; - comThread_.join(); +void UrCommunication::halt() +{ + keepalive_ = false; + comThread_.join(); } -void UrCommunication::run() { - uint8_t buf[2048]; - int bytes_read; - bzero(buf, 2048); - struct timeval timeout; - fd_set readfds; - FD_ZERO(&readfds); - FD_SET(sec_sockfd_, &readfds); - connected_ = true; - while (keepalive_) { - while (connected_ && keepalive_) { - timeout.tv_sec = 0; //do this each loop as selects modifies timeout - timeout.tv_usec = 500000; // timeout of 0.5 sec - select(sec_sockfd_ + 1, &readfds, NULL, NULL, &timeout); - bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes - if (bytes_read > 0) { - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, - (char *) &flag_, sizeof(int)); - robot_state_->unpack(buf, bytes_read); - } else { - connected_ = false; - robot_state_->setDisconnected(); - close(sec_sockfd_); - } - } - if (keepalive_) { - //reconnect - print_warning("Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."); - sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (sec_sockfd_ < 0) { - print_fatal("ERROR opening secondary socket"); - } - flag_ = 1; - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, - sizeof(int)); - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, - sizeof(int)); - setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, - sizeof(int)); - fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); - while (keepalive_ && !connected_) { - std::this_thread::sleep_for(std::chrono::seconds(10)); - fd_set writefds; +void UrCommunication::run() +{ + uint8_t buf[2048]; + int bytes_read; + bzero(buf, 2048); + struct timeval timeout; + fd_set readfds; + FD_ZERO(&readfds); + FD_SET(sec_sockfd_, &readfds); + connected_ = true; + while (keepalive_) { + while (connected_ && keepalive_) { + timeout.tv_sec = 0; //do this each loop as selects modifies timeout + timeout.tv_usec = 500000; // timeout of 0.5 sec + select(sec_sockfd_ + 1, &readfds, NULL, NULL, &timeout); + bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes + if (bytes_read > 0) { + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, + (char*)&flag_, sizeof(int)); + robot_state_->unpack(buf, bytes_read); + } else { + connected_ = false; + robot_state_->setDisconnected(); + close(sec_sockfd_); + } + } + if (keepalive_) { + //reconnect + print_warning("Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."); + sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (sec_sockfd_ < 0) { + print_fatal("ERROR opening secondary socket"); + } + flag_ = 1; + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, + sizeof(int)); + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, + sizeof(int)); + setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, + sizeof(int)); + fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); + while (keepalive_ && !connected_) { + std::this_thread::sleep_for(std::chrono::seconds(10)); + fd_set writefds; - connect(sec_sockfd_, (struct sockaddr *) &sec_serv_addr_, - sizeof(sec_serv_addr_)); - FD_ZERO(&writefds); - FD_SET(sec_sockfd_, &writefds); - select(sec_sockfd_ + 1, NULL, &writefds, NULL, NULL); - unsigned int flag_len; - getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, - &flag_len); - if (flag_ < 0) { - print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds..."); - } else { - connected_ = true; - print_info("Secondary port: Reconnected"); - } - } - } - } + connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_, + sizeof(sec_serv_addr_)); + FD_ZERO(&writefds); + FD_SET(sec_sockfd_, &writefds); + select(sec_sockfd_ + 1, NULL, &writefds, NULL, NULL); + unsigned int flag_len; + getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, + &flag_len); + if (flag_ < 0) { + print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds..."); + } else { + connected_ = true; + print_info("Secondary port: Reconnected"); + } + } + } + } - //wait for some traffic so the UR socket doesn't die in version 3.1. - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - close(sec_sockfd_); + //wait for some traffic so the UR socket doesn't die in version 3.1. + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + close(sec_sockfd_); } - diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index 22081151e..08a535a44 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -19,364 +19,396 @@ #include "ur_modern_driver/ur_driver.h" UrDriver::UrDriver(std::condition_variable& rt_msg_cond, - std::condition_variable& msg_cond, std::string host, - unsigned int reverse_port, double servoj_time, - unsigned int safety_count_max, double max_time_step, double min_payload, - double max_payload, double servoj_lookahead_time, double servoj_gain) : - REVERSE_PORT_(reverse_port), maximum_time_step_(max_time_step), minimum_payload_( - min_payload), maximum_payload_(max_payload), servoj_time_( - servoj_time), servoj_lookahead_time_(servoj_lookahead_time), servoj_gain_(servoj_gain) { - char buffer[256]; - struct sockaddr_in serv_addr; - int n, flag; - - firmware_version_ = 0; - reverse_connected_ = false; - executing_traj_ = false; - rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host, - safety_count_max); - new_sockfd_ = -1; - sec_interface_ = new UrCommunication(msg_cond, host); - - incoming_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (incoming_sockfd_ < 0) { - print_fatal("ERROR opening socket for reverse communication"); - } - bzero((char *) &serv_addr, sizeof(serv_addr)); - - serv_addr.sin_family = AF_INET; - serv_addr.sin_addr.s_addr = INADDR_ANY; - serv_addr.sin_port = htons(REVERSE_PORT_); - flag = 1; - setsockopt(incoming_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag, - sizeof(int)); - setsockopt(incoming_sockfd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int)); - if (bind(incoming_sockfd_, (struct sockaddr *) &serv_addr, - sizeof(serv_addr)) < 0) { - print_fatal("ERROR on binding socket for reverse communication"); - } - listen(incoming_sockfd_, 5); + std::condition_variable& msg_cond, std::string host, + unsigned int reverse_port, double servoj_time, + unsigned int safety_count_max, double max_time_step, double min_payload, + double max_payload, double servoj_lookahead_time, double servoj_gain) + : REVERSE_PORT_(reverse_port) + , maximum_time_step_(max_time_step) + , minimum_payload_( + min_payload) + , maximum_payload_(max_payload) + , servoj_time_( + servoj_time) + , servoj_lookahead_time_(servoj_lookahead_time) + , servoj_gain_(servoj_gain) +{ + char buffer[256]; + struct sockaddr_in serv_addr; + int n, flag; + + firmware_version_ = 0; + reverse_connected_ = false; + executing_traj_ = false; + rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host, + safety_count_max); + new_sockfd_ = -1; + sec_interface_ = new UrCommunication(msg_cond, host); + + incoming_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (incoming_sockfd_ < 0) { + print_fatal("ERROR opening socket for reverse communication"); + } + bzero((char*)&serv_addr, sizeof(serv_addr)); + + serv_addr.sin_family = AF_INET; + serv_addr.sin_addr.s_addr = INADDR_ANY; + serv_addr.sin_port = htons(REVERSE_PORT_); + flag = 1; + setsockopt(incoming_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag, + sizeof(int)); + setsockopt(incoming_sockfd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int)); + if (bind(incoming_sockfd_, (struct sockaddr*)&serv_addr, + sizeof(serv_addr)) + < 0) { + print_fatal("ERROR on binding socket for reverse communication"); + } + listen(incoming_sockfd_, 5); } std::vector UrDriver::interp_cubic(double t, double T, - std::vector p0_pos, std::vector p1_pos, - std::vector p0_vel, std::vector p1_vel) { - /*Returns positions of the joints at time 't' */ - std::vector positions; - for (unsigned int i = 0; i < p0_pos.size(); i++) { - double a = p0_pos[i]; - double b = p0_vel[i]; - double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i] - - T * p1_vel[i]) / pow(T, 2); - double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i] - + T * p1_vel[i]) / pow(T, 3); - positions.push_back(a + b * t + c * pow(t, 2) + d * pow(t, 3)); - } - return positions; + std::vector p0_pos, std::vector p1_pos, + std::vector p0_vel, std::vector p1_vel) +{ + /*Returns positions of the joints at time 't' */ + std::vector positions; + for (unsigned int i = 0; i < p0_pos.size(); i++) { + double a = p0_pos[i]; + double b = p0_vel[i]; + double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i] + - T * p1_vel[i]) + / pow(T, 2); + double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i] + + T * p1_vel[i]) + / pow(T, 3); + positions.push_back(a + b * t + c * pow(t, 2) + d * pow(t, 3)); + } + return positions; } bool UrDriver::doTraj(std::vector inp_timestamps, - std::vector > inp_positions, - std::vector > inp_velocities) { - std::chrono::high_resolution_clock::time_point t0, t; - std::vector positions; - unsigned int j; - - if (!UrDriver::uploadProg()) { - return false; - } - executing_traj_ = true; - t0 = std::chrono::high_resolution_clock::now(); - t = t0; - j = 0; - while ((inp_timestamps[inp_timestamps.size() - 1] - >= std::chrono::duration_cast>(t - t0).count()) - and executing_traj_) { - while (inp_timestamps[j] - <= std::chrono::duration_cast>( - t - t0).count() && j < inp_timestamps.size() - 1) { - j += 1; - } - positions = UrDriver::interp_cubic( - std::chrono::duration_cast>( - t - t0).count() - inp_timestamps[j - 1], - inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1], - inp_positions[j], inp_velocities[j - 1], inp_velocities[j]); - UrDriver::servoj(positions); - - // oversample with 4 * sample_time - std::this_thread::sleep_for( - std::chrono::milliseconds((int) ((servoj_time_ * 1000) / 4.))); - t = std::chrono::high_resolution_clock::now(); - } - executing_traj_ = false; - //Signal robot to stop driverProg() - UrDriver::closeServo(positions); - return true; + std::vector > inp_positions, + std::vector > inp_velocities) +{ + std::chrono::high_resolution_clock::time_point t0, t; + std::vector positions; + unsigned int j; + + if (!UrDriver::uploadProg()) { + return false; + } + executing_traj_ = true; + t0 = std::chrono::high_resolution_clock::now(); + t = t0; + j = 0; + while ((inp_timestamps[inp_timestamps.size() - 1] + >= std::chrono::duration_cast >(t - t0).count()) + and executing_traj_) { + while (inp_timestamps[j] + <= std::chrono::duration_cast >( + t - t0) + .count() + && j < inp_timestamps.size() - 1) { + j += 1; + } + positions = UrDriver::interp_cubic( + std::chrono::duration_cast >( + t - t0) + .count() + - inp_timestamps[j - 1], + inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1], + inp_positions[j], inp_velocities[j - 1], inp_velocities[j]); + UrDriver::servoj(positions); + + // oversample with 4 * sample_time + std::this_thread::sleep_for( + std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.))); + t = std::chrono::high_resolution_clock::now(); + } + executing_traj_ = false; + //Signal robot to stop driverProg() + UrDriver::closeServo(positions); + return true; } -void UrDriver::servoj(std::vector positions, int keepalive) { - if (!reverse_connected_) { - print_error( - "UrDriver::servoj called without a reverse connection present. Keepalive: " - + std::to_string(keepalive)); - return; - } - unsigned int bytes_written; - int tmp; - unsigned char buf[28]; - for (int i = 0; i < 6; i++) { - tmp = htonl((int) (positions[i] * MULT_JOINTSTATE_)); - buf[i * 4] = tmp & 0xff; - buf[i * 4 + 1] = (tmp >> 8) & 0xff; - buf[i * 4 + 2] = (tmp >> 16) & 0xff; - buf[i * 4 + 3] = (tmp >> 24) & 0xff; - } - tmp = htonl((int) keepalive); - buf[6 * 4] = tmp & 0xff; - buf[6 * 4 + 1] = (tmp >> 8) & 0xff; - buf[6 * 4 + 2] = (tmp >> 16) & 0xff; - buf[6 * 4 + 3] = (tmp >> 24) & 0xff; - bytes_written = write(new_sockfd_, buf, 28); +void UrDriver::servoj(std::vector positions, int keepalive) +{ + if (!reverse_connected_) { + print_error( + "UrDriver::servoj called without a reverse connection present. Keepalive: " + + std::to_string(keepalive)); + return; + } + unsigned int bytes_written; + int tmp; + unsigned char buf[28]; + for (int i = 0; i < 6; i++) { + tmp = htonl((int)(positions[i] * MULT_JOINTSTATE_)); + buf[i * 4] = tmp & 0xff; + buf[i * 4 + 1] = (tmp >> 8) & 0xff; + buf[i * 4 + 2] = (tmp >> 16) & 0xff; + buf[i * 4 + 3] = (tmp >> 24) & 0xff; + } + tmp = htonl((int)keepalive); + buf[6 * 4] = tmp & 0xff; + buf[6 * 4 + 1] = (tmp >> 8) & 0xff; + buf[6 * 4 + 2] = (tmp >> 16) & 0xff; + buf[6 * 4 + 3] = (tmp >> 24) & 0xff; + bytes_written = write(new_sockfd_, buf, 28); } -void UrDriver::stopTraj() { - executing_traj_ = false; - rt_interface_->addCommandToQueue("stopj(10)\n"); +void UrDriver::stopTraj() +{ + executing_traj_ = false; + rt_interface_->addCommandToQueue("stopj(10)\n"); } -bool UrDriver::uploadProg() { - std::string cmd_str; - char buf[128]; - cmd_str = "def driverProg():\n"; - - sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_); - cmd_str += buf; - - cmd_str += "\tSERVO_IDLE = 0\n"; - cmd_str += "\tSERVO_RUNNING = 1\n"; - cmd_str += "\tcmd_servo_state = SERVO_IDLE\n"; - cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n"; - cmd_str += "\tdef set_servo_setpoint(q):\n"; - cmd_str += "\t\tenter_critical\n"; - cmd_str += "\t\tcmd_servo_state = SERVO_RUNNING\n"; - cmd_str += "\t\tcmd_servo_q = q\n"; - cmd_str += "\t\texit_critical\n"; - cmd_str += "\tend\n"; - cmd_str += "\tthread servoThread():\n"; - cmd_str += "\t\tstate = SERVO_IDLE\n"; - cmd_str += "\t\twhile True:\n"; - cmd_str += "\t\t\tenter_critical\n"; - cmd_str += "\t\t\tq = cmd_servo_q\n"; - cmd_str += "\t\t\tdo_brake = False\n"; - cmd_str += "\t\t\tif (state == SERVO_RUNNING) and "; - cmd_str += "(cmd_servo_state == SERVO_IDLE):\n"; - cmd_str += "\t\t\t\tdo_brake = True\n"; - cmd_str += "\t\t\tend\n"; - cmd_str += "\t\t\tstate = cmd_servo_state\n"; - cmd_str += "\t\t\tcmd_servo_state = SERVO_IDLE\n"; - cmd_str += "\t\t\texit_critical\n"; - cmd_str += "\t\t\tif do_brake:\n"; - cmd_str += "\t\t\t\tstopj(1.0)\n"; - cmd_str += "\t\t\t\tsync()\n"; - cmd_str += "\t\t\telif state == SERVO_RUNNING:\n"; - - if (sec_interface_->robot_state_->getVersion() >= 3.1) - sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=%.4f, gain=%.0f)\n", - servoj_time_, servoj_lookahead_time_, servoj_gain_); - else - sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_); - cmd_str += buf; - - cmd_str += "\t\t\telse:\n"; - cmd_str += "\t\t\t\tsync()\n"; - cmd_str += "\t\t\tend\n"; - cmd_str += "\t\tend\n"; - cmd_str += "\tend\n"; - - sprintf(buf, "\tsocket_open(\"%s\", %i)\n", ip_addr_.c_str(), - REVERSE_PORT_); - cmd_str += buf; - - cmd_str += "\tthread_servo = run servoThread()\n"; - cmd_str += "\tkeepalive = 1\n"; - cmd_str += "\twhile keepalive > 0:\n"; - cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1)\n"; - cmd_str += "\t\tif params_mult[0] > 0:\n"; - cmd_str += "\t\t\tq = [params_mult[1] / MULT_jointstate, "; - cmd_str += "params_mult[2] / MULT_jointstate, "; - cmd_str += "params_mult[3] / MULT_jointstate, "; - cmd_str += "params_mult[4] / MULT_jointstate, "; - cmd_str += "params_mult[5] / MULT_jointstate, "; - cmd_str += "params_mult[6] / MULT_jointstate]\n"; - cmd_str += "\t\t\tkeepalive = params_mult[7]\n"; - cmd_str += "\t\t\tset_servo_setpoint(q)\n"; - cmd_str += "\t\tend\n"; - cmd_str += "\tend\n"; - cmd_str += "\tsleep(.1)\n"; - cmd_str += "\tsocket_close()\n"; - cmd_str += "\tkill thread_servo\n"; - cmd_str += "end\n"; - - rt_interface_->addCommandToQueue(cmd_str); - return UrDriver::openServo(); +bool UrDriver::uploadProg() +{ + std::string cmd_str; + char buf[128]; + cmd_str = "def driverProg():\n"; + + sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_); + cmd_str += buf; + + cmd_str += "\tSERVO_IDLE = 0\n"; + cmd_str += "\tSERVO_RUNNING = 1\n"; + cmd_str += "\tcmd_servo_state = SERVO_IDLE\n"; + cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n"; + cmd_str += "\tdef set_servo_setpoint(q):\n"; + cmd_str += "\t\tenter_critical\n"; + cmd_str += "\t\tcmd_servo_state = SERVO_RUNNING\n"; + cmd_str += "\t\tcmd_servo_q = q\n"; + cmd_str += "\t\texit_critical\n"; + cmd_str += "\tend\n"; + cmd_str += "\tthread servoThread():\n"; + cmd_str += "\t\tstate = SERVO_IDLE\n"; + cmd_str += "\t\twhile True:\n"; + cmd_str += "\t\t\tenter_critical\n"; + cmd_str += "\t\t\tq = cmd_servo_q\n"; + cmd_str += "\t\t\tdo_brake = False\n"; + cmd_str += "\t\t\tif (state == SERVO_RUNNING) and "; + cmd_str += "(cmd_servo_state == SERVO_IDLE):\n"; + cmd_str += "\t\t\t\tdo_brake = True\n"; + cmd_str += "\t\t\tend\n"; + cmd_str += "\t\t\tstate = cmd_servo_state\n"; + cmd_str += "\t\t\tcmd_servo_state = SERVO_IDLE\n"; + cmd_str += "\t\t\texit_critical\n"; + cmd_str += "\t\t\tif do_brake:\n"; + cmd_str += "\t\t\t\tstopj(1.0)\n"; + cmd_str += "\t\t\t\tsync()\n"; + cmd_str += "\t\t\telif state == SERVO_RUNNING:\n"; + + if (sec_interface_->robot_state_->getVersion() >= 3.1) + sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=%.4f, gain=%.0f)\n", + servoj_time_, servoj_lookahead_time_, servoj_gain_); + else + sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_); + cmd_str += buf; + + cmd_str += "\t\t\telse:\n"; + cmd_str += "\t\t\t\tsync()\n"; + cmd_str += "\t\t\tend\n"; + cmd_str += "\t\tend\n"; + cmd_str += "\tend\n"; + + sprintf(buf, "\tsocket_open(\"%s\", %i)\n", ip_addr_.c_str(), + REVERSE_PORT_); + cmd_str += buf; + + cmd_str += "\tthread_servo = run servoThread()\n"; + cmd_str += "\tkeepalive = 1\n"; + cmd_str += "\twhile keepalive > 0:\n"; + cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1)\n"; + cmd_str += "\t\tif params_mult[0] > 0:\n"; + cmd_str += "\t\t\tq = [params_mult[1] / MULT_jointstate, "; + cmd_str += "params_mult[2] / MULT_jointstate, "; + cmd_str += "params_mult[3] / MULT_jointstate, "; + cmd_str += "params_mult[4] / MULT_jointstate, "; + cmd_str += "params_mult[5] / MULT_jointstate, "; + cmd_str += "params_mult[6] / MULT_jointstate]\n"; + cmd_str += "\t\t\tkeepalive = params_mult[7]\n"; + cmd_str += "\t\t\tset_servo_setpoint(q)\n"; + cmd_str += "\t\tend\n"; + cmd_str += "\tend\n"; + cmd_str += "\tsleep(.1)\n"; + cmd_str += "\tsocket_close()\n"; + cmd_str += "\tkill thread_servo\n"; + cmd_str += "end\n"; + + rt_interface_->addCommandToQueue(cmd_str); + return UrDriver::openServo(); } -bool UrDriver::openServo() { - struct sockaddr_in cli_addr; - socklen_t clilen; - clilen = sizeof(cli_addr); - new_sockfd_ = accept(incoming_sockfd_, (struct sockaddr *) &cli_addr, - &clilen); - if (new_sockfd_ < 0) { - print_fatal("ERROR on accepting reverse communication"); - return false; - } - reverse_connected_ = true; - return true; +bool UrDriver::openServo() +{ + struct sockaddr_in cli_addr; + socklen_t clilen; + clilen = sizeof(cli_addr); + new_sockfd_ = accept(incoming_sockfd_, (struct sockaddr*)&cli_addr, + &clilen); + if (new_sockfd_ < 0) { + print_fatal("ERROR on accepting reverse communication"); + return false; + } + reverse_connected_ = true; + return true; } -void UrDriver::closeServo(std::vector positions) { - if (positions.size() != 6) - UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0); - else - UrDriver::servoj(positions, 0); - - reverse_connected_ = false; - close(new_sockfd_); +void UrDriver::closeServo(std::vector positions) +{ + if (positions.size() != 6) + UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0); + else + UrDriver::servoj(positions, 0); + + reverse_connected_ = false; + close(new_sockfd_); } -bool UrDriver::start() { - if (!sec_interface_->start()) - return false; - firmware_version_ = sec_interface_->robot_state_->getVersion(); - rt_interface_->robot_state_->setVersion(firmware_version_); - if (!rt_interface_->start()) - return false; - ip_addr_ = rt_interface_->getLocalIp(); - print_debug( - "Listening on " + ip_addr_ + ":" + std::to_string(REVERSE_PORT_) - + "\n"); - return true; - +bool UrDriver::start() +{ + if (!sec_interface_->start()) + return false; + firmware_version_ = sec_interface_->robot_state_->getVersion(); + rt_interface_->robot_state_->setVersion(firmware_version_); + if (!rt_interface_->start()) + return false; + ip_addr_ = rt_interface_->getLocalIp(); + print_debug( + "Listening on " + ip_addr_ + ":" + std::to_string(REVERSE_PORT_) + + "\n"); + return true; } -void UrDriver::halt() { - if (executing_traj_) { - UrDriver::stopTraj(); - } - sec_interface_->halt(); - rt_interface_->halt(); - close(incoming_sockfd_); +void UrDriver::halt() +{ + if (executing_traj_) { + UrDriver::stopTraj(); + } + sec_interface_->halt(); + rt_interface_->halt(); + close(incoming_sockfd_); } void UrDriver::setSpeed(double q0, double q1, double q2, double q3, double q4, - double q5, double acc) { - rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc); + double q5, double acc) +{ + rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc); } -std::vector UrDriver::getJointNames() { - return joint_names_; +std::vector UrDriver::getJointNames() +{ + return joint_names_; } -void UrDriver::setJointNames(std::vector jn) { - joint_names_ = jn; +void UrDriver::setJointNames(std::vector jn) +{ + joint_names_ = jn; } -void UrDriver::setToolVoltage(unsigned int v) { - char buf[256]; - sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v); - rt_interface_->addCommandToQueue(buf); - print_debug(buf); +void UrDriver::setToolVoltage(unsigned int v) +{ + char buf[256]; + sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v); + rt_interface_->addCommandToQueue(buf); + print_debug(buf); } -void UrDriver::setFlag(unsigned int n, bool b) { - char buf[256]; - sprintf(buf, "sec setOut():\n\tset_flag(%d, %s)\nend\n", n, - b ? "True" : "False"); - rt_interface_->addCommandToQueue(buf); - print_debug(buf); +void UrDriver::setFlag(unsigned int n, bool b) +{ + char buf[256]; + sprintf(buf, "sec setOut():\n\tset_flag(%d, %s)\nend\n", n, + b ? "True" : "False"); + rt_interface_->addCommandToQueue(buf); + print_debug(buf); } -void UrDriver::setDigitalOut(unsigned int n, bool b) { - char buf[256]; - if (firmware_version_ < 2) { - sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n, - b ? "True" : "False"); +void UrDriver::setDigitalOut(unsigned int n, bool b) +{ + char buf[256]; + if (firmware_version_ < 2) { + sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n, + b ? "True" : "False"); } else if (n > 15) { sprintf(buf, - "sec setOut():\n\tset_tool_digital_out(%d, %s)\nend\n", - n - 16, b ? "True" : "False"); - } else if (n > 7) { + "sec setOut():\n\tset_tool_digital_out(%d, %s)\nend\n", + n - 16, b ? "True" : "False"); + } else if (n > 7) { sprintf(buf, "sec setOut():\n\tset_configurable_digital_out(%d, %s)\nend\n", - n - 8, b ? "True" : "False"); - - } else { - sprintf(buf, "sec setOut():\n\tset_standard_digital_out(%d, %s)\nend\n", - n, b ? "True" : "False"); - - } - rt_interface_->addCommandToQueue(buf); - print_debug(buf); - + n - 8, b ? "True" : "False"); + + } else { + sprintf(buf, "sec setOut():\n\tset_standard_digital_out(%d, %s)\nend\n", + n, b ? "True" : "False"); + } + rt_interface_->addCommandToQueue(buf); + print_debug(buf); } -void UrDriver::setAnalogOut(unsigned int n, double f) { - char buf[256]; - if (firmware_version_ < 2) { - sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f); - } else { - sprintf(buf, "sec setOut():\n\tset_standard_analog_out(%d, %1.4f)\nend\n", n, f); - } - - rt_interface_->addCommandToQueue(buf); - print_debug(buf); +void UrDriver::setAnalogOut(unsigned int n, double f) +{ + char buf[256]; + if (firmware_version_ < 2) { + sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f); + } else { + sprintf(buf, "sec setOut():\n\tset_standard_analog_out(%d, %1.4f)\nend\n", n, f); + } + + rt_interface_->addCommandToQueue(buf); + print_debug(buf); } -bool UrDriver::setPayload(double m) { - if ((m < maximum_payload_) && (m > minimum_payload_)) { - char buf[256]; - sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m); - rt_interface_->addCommandToQueue(buf); - print_debug(buf); - return true; - } else - return false; +bool UrDriver::setPayload(double m) +{ + if ((m < maximum_payload_) && (m > minimum_payload_)) { + char buf[256]; + sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m); + rt_interface_->addCommandToQueue(buf); + print_debug(buf); + return true; + } else + return false; } -void UrDriver::setMinPayload(double m) { - if (m > 0) { - minimum_payload_ = m; - } else { - minimum_payload_ = 0; - } - +void UrDriver::setMinPayload(double m) +{ + if (m > 0) { + minimum_payload_ = m; + } else { + minimum_payload_ = 0; + } } -void UrDriver::setMaxPayload(double m) { - maximum_payload_ = m; +void UrDriver::setMaxPayload(double m) +{ + maximum_payload_ = m; } -void UrDriver::setServojTime(double t) { - if (t > 0.008) { - servoj_time_ = t; - } else { - servoj_time_ = 0.008; - } +void UrDriver::setServojTime(double t) +{ + if (t > 0.008) { + servoj_time_ = t; + } else { + servoj_time_ = 0.008; + } } -void UrDriver::setServojLookahead(double t){ - if (t > 0.03) { - if (t < 0.2) { - servoj_lookahead_time_ = t; - } else { - servoj_lookahead_time_ = 0.2; - } - } else { - servoj_lookahead_time_ = 0.03; - } +void UrDriver::setServojLookahead(double t) +{ + if (t > 0.03) { + if (t < 0.2) { + servoj_lookahead_time_ = t; + } else { + servoj_lookahead_time_ = 0.2; + } + } else { + servoj_lookahead_time_ = 0.03; + } } -void UrDriver::setServojGain(double g){ - if (g > 100) { - if (g < 2000) { - servoj_gain_ = g; - } else { - servoj_gain_ = 2000; - } - } else { - servoj_gain_ = 100; - } +void UrDriver::setServojGain(double g) +{ + if (g > 100) { + if (g < 2000) { + servoj_gain_ = g; + } else { + servoj_gain_ = 2000; + } + } else { + servoj_gain_ = 100; + } } diff --git a/src/ur_hardware_interface.cpp b/src/ur_hardware_interface.cpp index ea6639556..152452c56 100644 --- a/src/ur_hardware_interface.cpp +++ b/src/ur_hardware_interface.cpp @@ -59,225 +59,226 @@ namespace ros_control_ur { -UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) : - nh_(nh), robot_(robot) { - // Initialize shared memory and interfaces here - init(); // this implementation loads from rosparam +UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) + : nh_(nh) + , robot_(robot) +{ + // Initialize shared memory and interfaces here + init(); // this implementation loads from rosparam - max_vel_change_ = 0.12; // equivalent of an acceleration of 15 rad/sec^2 + max_vel_change_ = 0.12; // equivalent of an acceleration of 15 rad/sec^2 - ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface."); + ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface."); } -void UrHardwareInterface::init() { - ROS_INFO_STREAM_NAMED("ur_hardware_interface", - "Reading rosparams from namespace: " << nh_.getNamespace()); +void UrHardwareInterface::init() +{ + ROS_INFO_STREAM_NAMED("ur_hardware_interface", + "Reading rosparams from namespace: " << nh_.getNamespace()); - // Get joint names - nh_.getParam("hardware_interface/joints", joint_names_); - if (joint_names_.size() == 0) { - ROS_FATAL_STREAM_NAMED("ur_hardware_interface", - "No joints found on parameter server for controller, did you load the proper yaml file?" << " Namespace: " << nh_.getNamespace()); - exit(-1); - } - num_joints_ = joint_names_.size(); + // Get joint names + nh_.getParam("hardware_interface/joints", joint_names_); + if (joint_names_.size() == 0) { + ROS_FATAL_STREAM_NAMED("ur_hardware_interface", + "No joints found on parameter server for controller, did you load the proper yaml file?" + << " Namespace: " << nh_.getNamespace()); + exit(-1); + } + num_joints_ = joint_names_.size(); - // Resize vectors - joint_position_.resize(num_joints_); - joint_velocity_.resize(num_joints_); - joint_effort_.resize(num_joints_); - joint_position_command_.resize(num_joints_); - joint_velocity_command_.resize(num_joints_); - prev_joint_velocity_command_.resize(num_joints_); + // Resize vectors + joint_position_.resize(num_joints_); + joint_velocity_.resize(num_joints_); + joint_effort_.resize(num_joints_); + joint_position_command_.resize(num_joints_); + joint_velocity_command_.resize(num_joints_); + prev_joint_velocity_command_.resize(num_joints_); - // Initialize controller - for (std::size_t i = 0; i < num_joints_; ++i) { - ROS_DEBUG_STREAM_NAMED("ur_hardware_interface", - "Loading joint name: " << joint_names_[i]); + // Initialize controller + for (std::size_t i = 0; i < num_joints_; ++i) { + ROS_DEBUG_STREAM_NAMED("ur_hardware_interface", + "Loading joint name: " << joint_names_[i]); - // Create joint state interface - joint_state_interface_.registerHandle( - hardware_interface::JointStateHandle(joint_names_[i], - &joint_position_[i], &joint_velocity_[i], - &joint_effort_[i])); + // Create joint state interface + joint_state_interface_.registerHandle( + hardware_interface::JointStateHandle(joint_names_[i], + &joint_position_[i], &joint_velocity_[i], + &joint_effort_[i])); - // Create position joint interface - position_joint_interface_.registerHandle( - hardware_interface::JointHandle( - joint_state_interface_.getHandle(joint_names_[i]), - &joint_position_command_[i])); + // Create position joint interface + position_joint_interface_.registerHandle( + hardware_interface::JointHandle( + joint_state_interface_.getHandle(joint_names_[i]), + &joint_position_command_[i])); - // Create velocity joint interface - velocity_joint_interface_.registerHandle( - hardware_interface::JointHandle( - joint_state_interface_.getHandle(joint_names_[i]), - &joint_velocity_command_[i])); - prev_joint_velocity_command_[i] = 0.; - } + // Create velocity joint interface + velocity_joint_interface_.registerHandle( + hardware_interface::JointHandle( + joint_state_interface_.getHandle(joint_names_[i]), + &joint_velocity_command_[i])); + prev_joint_velocity_command_[i] = 0.; + } - // Create force torque interface - force_torque_interface_.registerHandle( - hardware_interface::ForceTorqueSensorHandle("wrench", "", - robot_force_, robot_torque_)); + // Create force torque interface + force_torque_interface_.registerHandle( + hardware_interface::ForceTorqueSensorHandle("wrench", "", + robot_force_, robot_torque_)); - registerInterface(&joint_state_interface_); // From RobotHW base class. - registerInterface(&position_joint_interface_); // From RobotHW base class. - registerInterface(&velocity_joint_interface_); // From RobotHW base class. - registerInterface(&force_torque_interface_); // From RobotHW base class. - velocity_interface_running_ = false; - position_interface_running_ = false; + registerInterface(&joint_state_interface_); // From RobotHW base class. + registerInterface(&position_joint_interface_); // From RobotHW base class. + registerInterface(&velocity_joint_interface_); // From RobotHW base class. + registerInterface(&force_torque_interface_); // From RobotHW base class. + velocity_interface_running_ = false; + position_interface_running_ = false; } -void UrHardwareInterface::read() { - std::vector pos, vel, current, tcp; - pos = robot_->rt_interface_->robot_state_->getQActual(); - vel = robot_->rt_interface_->robot_state_->getQdActual(); - current = robot_->rt_interface_->robot_state_->getIActual(); - tcp = robot_->rt_interface_->robot_state_->getTcpForce(); - for (std::size_t i = 0; i < num_joints_; ++i) { - joint_position_[i] = pos[i]; - joint_velocity_[i] = vel[i]; - joint_effort_[i] = current[i]; - } - for (std::size_t i = 0; i < 3; ++i) { - robot_force_[i] = tcp[i]; - robot_torque_[i] = tcp[i + 3]; - } - +void UrHardwareInterface::read() +{ + std::vector pos, vel, current, tcp; + pos = robot_->rt_interface_->robot_state_->getQActual(); + vel = robot_->rt_interface_->robot_state_->getQdActual(); + current = robot_->rt_interface_->robot_state_->getIActual(); + tcp = robot_->rt_interface_->robot_state_->getTcpForce(); + for (std::size_t i = 0; i < num_joints_; ++i) { + joint_position_[i] = pos[i]; + joint_velocity_[i] = vel[i]; + joint_effort_[i] = current[i]; + } + for (std::size_t i = 0; i < 3; ++i) { + robot_force_[i] = tcp[i]; + robot_torque_[i] = tcp[i + 3]; + } } -void UrHardwareInterface::setMaxVelChange(double inp) { - max_vel_change_ = inp; +void UrHardwareInterface::setMaxVelChange(double inp) +{ + max_vel_change_ = inp; } -void UrHardwareInterface::write() { - if (velocity_interface_running_) { - std::vector cmd; - //do some rate limiting - cmd.resize(joint_velocity_command_.size()); - for (unsigned int i = 0; i < joint_velocity_command_.size(); i++) { - cmd[i] = joint_velocity_command_[i]; - if (cmd[i] > prev_joint_velocity_command_[i] + max_vel_change_) { - cmd[i] = prev_joint_velocity_command_[i] + max_vel_change_; - } else if (cmd[i] - < prev_joint_velocity_command_[i] - max_vel_change_) { - cmd[i] = prev_joint_velocity_command_[i] - max_vel_change_; - } - prev_joint_velocity_command_[i] = cmd[i]; - } - robot_->setSpeed(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], max_vel_change_*125); - } else if (position_interface_running_) { - robot_->servoj(joint_position_command_); - } +void UrHardwareInterface::write() +{ + if (velocity_interface_running_) { + std::vector cmd; + //do some rate limiting + cmd.resize(joint_velocity_command_.size()); + for (unsigned int i = 0; i < joint_velocity_command_.size(); i++) { + cmd[i] = joint_velocity_command_[i]; + if (cmd[i] > prev_joint_velocity_command_[i] + max_vel_change_) { + cmd[i] = prev_joint_velocity_command_[i] + max_vel_change_; + } else if (cmd[i] + < prev_joint_velocity_command_[i] - max_vel_change_) { + cmd[i] = prev_joint_velocity_command_[i] - max_vel_change_; + } + prev_joint_velocity_command_[i] = cmd[i]; + } + robot_->setSpeed(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], max_vel_change_ * 125); + } else if (position_interface_running_) { + robot_->servoj(joint_position_command_); + } } bool UrHardwareInterface::canSwitch( - const std::list &start_list, - const std::list &stop_list) const { - for (std::list::const_iterator controller_it = - start_list.begin(); controller_it != start_list.end(); - ++controller_it) { - if (controller_it->hardware_interface - == "hardware_interface::VelocityJointInterface") { - if (velocity_interface_running_) { - ROS_ERROR( - "%s: An interface of that type (%s) is already running", - controller_it->name.c_str(), - controller_it->hardware_interface.c_str()); - return false; - } - if (position_interface_running_) { - bool error = true; - for (std::list::const_iterator stop_controller_it = - stop_list.begin(); - stop_controller_it != stop_list.end(); - ++stop_controller_it) { - if (stop_controller_it->hardware_interface - == "hardware_interface::PositionJointInterface") { - error = false; - break; - } - } - if (error) { - ROS_ERROR( - "%s (type %s) can not be run simultaneously with a PositionJointInterface", - controller_it->name.c_str(), - controller_it->hardware_interface.c_str()); - return false; - } - } - } else if (controller_it->hardware_interface - == "hardware_interface::PositionJointInterface") { - if (position_interface_running_) { - ROS_ERROR( - "%s: An interface of that type (%s) is already running", - controller_it->name.c_str(), - controller_it->hardware_interface.c_str()); - return false; - } - if (velocity_interface_running_) { - bool error = true; - for (std::list::const_iterator stop_controller_it = - stop_list.begin(); - stop_controller_it != stop_list.end(); - ++stop_controller_it) { - if (stop_controller_it->hardware_interface - == "hardware_interface::VelocityJointInterface") { - error = false; - break; - } - } - if (error) { - ROS_ERROR( - "%s (type %s) can not be run simultaneously with a VelocityJointInterface", - controller_it->name.c_str(), - controller_it->hardware_interface.c_str()); - return false; - } - } - } - } + const std::list& start_list, + const std::list& stop_list) const +{ + for (std::list::const_iterator controller_it = start_list.begin(); controller_it != start_list.end(); + ++controller_it) { + if (controller_it->hardware_interface + == "hardware_interface::VelocityJointInterface") { + if (velocity_interface_running_) { + ROS_ERROR( + "%s: An interface of that type (%s) is already running", + controller_it->name.c_str(), + controller_it->hardware_interface.c_str()); + return false; + } + if (position_interface_running_) { + bool error = true; + for (std::list::const_iterator stop_controller_it = stop_list.begin(); + stop_controller_it != stop_list.end(); + ++stop_controller_it) { + if (stop_controller_it->hardware_interface + == "hardware_interface::PositionJointInterface") { + error = false; + break; + } + } + if (error) { + ROS_ERROR( + "%s (type %s) can not be run simultaneously with a PositionJointInterface", + controller_it->name.c_str(), + controller_it->hardware_interface.c_str()); + return false; + } + } + } else if (controller_it->hardware_interface + == "hardware_interface::PositionJointInterface") { + if (position_interface_running_) { + ROS_ERROR( + "%s: An interface of that type (%s) is already running", + controller_it->name.c_str(), + controller_it->hardware_interface.c_str()); + return false; + } + if (velocity_interface_running_) { + bool error = true; + for (std::list::const_iterator stop_controller_it = stop_list.begin(); + stop_controller_it != stop_list.end(); + ++stop_controller_it) { + if (stop_controller_it->hardware_interface + == "hardware_interface::VelocityJointInterface") { + error = false; + break; + } + } + if (error) { + ROS_ERROR( + "%s (type %s) can not be run simultaneously with a VelocityJointInterface", + controller_it->name.c_str(), + controller_it->hardware_interface.c_str()); + return false; + } + } + } + } -// we can always stop a controller - return true; + // we can always stop a controller + return true; } void UrHardwareInterface::doSwitch( - const std::list& start_list, - const std::list& stop_list) { - for (std::list::const_iterator controller_it = - stop_list.begin(); controller_it != stop_list.end(); - ++controller_it) { - if (controller_it->hardware_interface - == "hardware_interface::VelocityJointInterface") { - velocity_interface_running_ = false; - ROS_DEBUG("Stopping velocity interface"); - } - if (controller_it->hardware_interface - == "hardware_interface::PositionJointInterface") { - position_interface_running_ = false; - std::vector tmp; - robot_->closeServo(tmp); - ROS_DEBUG("Stopping position interface"); - } - } - for (std::list::const_iterator controller_it = - start_list.begin(); controller_it != start_list.end(); - ++controller_it) { - if (controller_it->hardware_interface - == "hardware_interface::VelocityJointInterface") { - velocity_interface_running_ = true; - ROS_DEBUG("Starting velocity interface"); - } - if (controller_it->hardware_interface - == "hardware_interface::PositionJointInterface") { - position_interface_running_ = true; - robot_->uploadProg(); - ROS_DEBUG("Starting position interface"); - } - } - + const std::list& start_list, + const std::list& stop_list) +{ + for (std::list::const_iterator controller_it = stop_list.begin(); controller_it != stop_list.end(); + ++controller_it) { + if (controller_it->hardware_interface + == "hardware_interface::VelocityJointInterface") { + velocity_interface_running_ = false; + ROS_DEBUG("Stopping velocity interface"); + } + if (controller_it->hardware_interface + == "hardware_interface::PositionJointInterface") { + position_interface_running_ = false; + std::vector tmp; + robot_->closeServo(tmp); + ROS_DEBUG("Stopping position interface"); + } + } + for (std::list::const_iterator controller_it = start_list.begin(); controller_it != start_list.end(); + ++controller_it) { + if (controller_it->hardware_interface + == "hardware_interface::VelocityJointInterface") { + velocity_interface_running_ = true; + ROS_DEBUG("Starting velocity interface"); + } + if (controller_it->hardware_interface + == "hardware_interface::PositionJointInterface") { + position_interface_running_ = true; + robot_->uploadProg(); + ROS_DEBUG("Starting position interface"); + } + } } } // namespace - diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp index 90f0cfe2e..b45224b57 100644 --- a/src/ur_realtime_communication.cpp +++ b/src/ur_realtime_communication.cpp @@ -19,175 +19,182 @@ #include "ur_modern_driver/ur_realtime_communication.h" UrRealtimeCommunication::UrRealtimeCommunication( - std::condition_variable& msg_cond, std::string host, - unsigned int safety_count_max) { - robot_state_ = new RobotStateRT(msg_cond); - bzero((char *) &serv_addr_, sizeof(serv_addr_)); - sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (sockfd_ < 0) { - print_fatal("ERROR opening socket"); - } - server_ = gethostbyname(host.c_str()); - if (server_ == NULL) { - print_fatal("ERROR, no such host"); - } - serv_addr_.sin_family = AF_INET; - bcopy((char *) server_->h_addr, (char *)&serv_addr_.sin_addr.s_addr, server_->h_length); - serv_addr_.sin_port = htons(30003); - flag_ = 1; - setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int)); - setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, sizeof(int)); - setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int)); - fcntl(sockfd_, F_SETFL, O_NONBLOCK); - connected_ = false; - keepalive_ = false; - safety_count_ = safety_count_max + 1; - safety_count_max_ = safety_count_max; + std::condition_variable& msg_cond, std::string host, + unsigned int safety_count_max) +{ + robot_state_ = new RobotStateRT(msg_cond); + bzero((char*)&serv_addr_, sizeof(serv_addr_)); + sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (sockfd_ < 0) { + print_fatal("ERROR opening socket"); + } + server_ = gethostbyname(host.c_str()); + if (server_ == NULL) { + print_fatal("ERROR, no such host"); + } + serv_addr_.sin_family = AF_INET; + bcopy((char*)server_->h_addr, (char*)&serv_addr_.sin_addr.s_addr, server_->h_length); + serv_addr_.sin_port = htons(30003); + flag_ = 1; + setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int)); + setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); + setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int)); + fcntl(sockfd_, F_SETFL, O_NONBLOCK); + connected_ = false; + keepalive_ = false; + safety_count_ = safety_count_max + 1; + safety_count_max_ = safety_count_max; } -bool UrRealtimeCommunication::start() { - fd_set writefds; - struct timeval timeout; +bool UrRealtimeCommunication::start() +{ + fd_set writefds; + struct timeval timeout; - keepalive_ = true; - print_debug("Realtime port: Connecting..."); + keepalive_ = true; + print_debug("Realtime port: Connecting..."); - connect(sockfd_, (struct sockaddr *) &serv_addr_, sizeof(serv_addr_)); - FD_ZERO(&writefds); - FD_SET(sockfd_, &writefds); - timeout.tv_sec = 10; - timeout.tv_usec = 0; - select(sockfd_ + 1, NULL, &writefds, NULL, &timeout); - unsigned int flag_len; - getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); - if (flag_ < 0) { - print_fatal("Error connecting to RT port 30003"); - return false; - } - sockaddr_in name; - socklen_t namelen = sizeof(name); - int err = getsockname(sockfd_, (sockaddr*) &name, &namelen); - if (err < 0) { - print_fatal("Could not get local IP"); - close(sockfd_); - return false; - } - char str[18]; - inet_ntop(AF_INET, &name.sin_addr, str, 18); - local_ip_ = str; - comThread_ = std::thread(&UrRealtimeCommunication::run, this); - return true; + connect(sockfd_, (struct sockaddr*)&serv_addr_, sizeof(serv_addr_)); + FD_ZERO(&writefds); + FD_SET(sockfd_, &writefds); + timeout.tv_sec = 10; + timeout.tv_usec = 0; + select(sockfd_ + 1, NULL, &writefds, NULL, &timeout); + unsigned int flag_len; + getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); + if (flag_ < 0) { + print_fatal("Error connecting to RT port 30003"); + return false; + } + sockaddr_in name; + socklen_t namelen = sizeof(name); + int err = getsockname(sockfd_, (sockaddr*)&name, &namelen); + if (err < 0) { + print_fatal("Could not get local IP"); + close(sockfd_); + return false; + } + char str[18]; + inet_ntop(AF_INET, &name.sin_addr, str, 18); + local_ip_ = str; + comThread_ = std::thread(&UrRealtimeCommunication::run, this); + return true; } -void UrRealtimeCommunication::halt() { - keepalive_ = false; - comThread_.join(); +void UrRealtimeCommunication::halt() +{ + keepalive_ = false; + comThread_.join(); } -void UrRealtimeCommunication::addCommandToQueue(std::string inp) { - int bytes_written; - if (inp.back() != '\n') { - inp.append("\n"); - } - if (connected_) - bytes_written = write(sockfd_, inp.c_str(), inp.length()); - else - print_error("Could not send command \"" +inp + "\". The robot is not connected! Command is discarded" ); +void UrRealtimeCommunication::addCommandToQueue(std::string inp) +{ + int bytes_written; + if (inp.back() != '\n') { + inp.append("\n"); + } + if (connected_) + bytes_written = write(sockfd_, inp.c_str(), inp.length()); + else + print_error("Could not send command \"" + inp + "\". The robot is not connected! Command is discarded"); } void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2, - double q3, double q4, double q5, double acc) { - char cmd[1024]; - if( robot_state_->getVersion() >= 3.1 ) { - sprintf(cmd, - "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n", - q0, q1, q2, q3, q4, q5, acc); - } - else { - sprintf(cmd, - "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n", - q0, q1, q2, q3, q4, q5, acc); - } - addCommandToQueue((std::string) (cmd)); - if (q0 != 0. or q1 != 0. or q2 != 0. or q3 != 0. or q4 != 0. or q5 != 0.) { - //If a joint speed is set, make sure we stop it again after some time if the user doesn't - safety_count_ = 0; - } + double q3, double q4, double q5, double acc) +{ + char cmd[1024]; + if (robot_state_->getVersion() >= 3.1) { + sprintf(cmd, + "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n", + q0, q1, q2, q3, q4, q5, acc); + } else { + sprintf(cmd, + "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n", + q0, q1, q2, q3, q4, q5, acc); + } + addCommandToQueue((std::string)(cmd)); + if (q0 != 0. or q1 != 0. or q2 != 0. or q3 != 0. or q4 != 0. or q5 != 0.) { + //If a joint speed is set, make sure we stop it again after some time if the user doesn't + safety_count_ = 0; + } } -void UrRealtimeCommunication::run() { - uint8_t buf[2048]; - int bytes_read; - bzero(buf, 2048); - struct timeval timeout; - fd_set readfds; - FD_ZERO(&readfds); - FD_SET(sockfd_, &readfds); - print_debug("Realtime port: Got connection"); - connected_ = true; - while (keepalive_) { - while (connected_ && keepalive_) { - timeout.tv_sec = 0; //do this each loop as selects modifies timeout - timeout.tv_usec = 500000; // timeout of 0.5 sec - select(sockfd_ + 1, &readfds, NULL, NULL, &timeout); - bytes_read = read(sockfd_, buf, 2048); - if (bytes_read > 0) { - setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, - sizeof(int)); - robot_state_->unpack(buf); - if (safety_count_ == safety_count_max_) { - setSpeed(0., 0., 0., 0., 0., 0.); - } - safety_count_ += 1; - } else { - connected_ = false; - close(sockfd_); - } - } - if (keepalive_) { - //reconnect - print_warning("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."); - sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (sockfd_ < 0) { - print_fatal("ERROR opening socket"); - } - flag_ = 1; - setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, - sizeof(int)); - setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, - sizeof(int)); - - setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, - sizeof(int)); - fcntl(sockfd_, F_SETFL, O_NONBLOCK); - while (keepalive_ && !connected_) { - std::this_thread::sleep_for(std::chrono::seconds(10)); - fd_set writefds; +void UrRealtimeCommunication::run() +{ + uint8_t buf[2048]; + int bytes_read; + bzero(buf, 2048); + struct timeval timeout; + fd_set readfds; + FD_ZERO(&readfds); + FD_SET(sockfd_, &readfds); + print_debug("Realtime port: Got connection"); + connected_ = true; + while (keepalive_) { + while (connected_ && keepalive_) { + timeout.tv_sec = 0; //do this each loop as selects modifies timeout + timeout.tv_usec = 500000; // timeout of 0.5 sec + select(sockfd_ + 1, &readfds, NULL, NULL, &timeout); + bytes_read = read(sockfd_, buf, 2048); + if (bytes_read > 0) { + setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, + sizeof(int)); + robot_state_->unpack(buf); + if (safety_count_ == safety_count_max_) { + setSpeed(0., 0., 0., 0., 0., 0.); + } + safety_count_ += 1; + } else { + connected_ = false; + close(sockfd_); + } + } + if (keepalive_) { + //reconnect + print_warning("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."); + sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (sockfd_ < 0) { + print_fatal("ERROR opening socket"); + } + flag_ = 1; + setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, + sizeof(int)); + setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, + sizeof(int)); - connect(sockfd_, (struct sockaddr *) &serv_addr_, - sizeof(serv_addr_)); - FD_ZERO(&writefds); - FD_SET(sockfd_, &writefds); - select(sockfd_ + 1, NULL, &writefds, NULL, NULL); - unsigned int flag_len; - getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); - if (flag_ < 0) { - print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds..."); - } else { - connected_ = true; - print_info("Realtime port: Reconnected"); - } - } - } - } - setSpeed(0., 0., 0., 0., 0., 0.); - close(sockfd_); + setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, + sizeof(int)); + fcntl(sockfd_, F_SETFL, O_NONBLOCK); + while (keepalive_ && !connected_) { + std::this_thread::sleep_for(std::chrono::seconds(10)); + fd_set writefds; + + connect(sockfd_, (struct sockaddr*)&serv_addr_, + sizeof(serv_addr_)); + FD_ZERO(&writefds); + FD_SET(sockfd_, &writefds); + select(sockfd_ + 1, NULL, &writefds, NULL, NULL); + unsigned int flag_len; + getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); + if (flag_ < 0) { + print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds..."); + } else { + connected_ = true; + print_info("Realtime port: Reconnected"); + } + } + } + } + setSpeed(0., 0., 0., 0., 0., 0.); + close(sockfd_); } -void UrRealtimeCommunication::setSafetyCountMax(uint inp) { - safety_count_max_ = inp; +void UrRealtimeCommunication::setSafetyCountMax(uint inp) +{ + safety_count_max_ = inp; } -std::string UrRealtimeCommunication::getLocalIp() { - return local_ip_; +std::string UrRealtimeCommunication::getLocalIp() +{ + return local_ip_; } diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index ab74534b2..92fe341d1 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -16,40 +16,40 @@ * limitations under the License. */ +#include "ur_modern_driver/do_output.h" #include "ur_modern_driver/ur_driver.h" #include "ur_modern_driver/ur_hardware_interface.h" -#include "ur_modern_driver/do_output.h" -#include -#include -#include -#include -#include #include -#include #include +#include +#include +#include +#include +#include #include +#include -#include "ros/ros.h" -#include -#include "sensor_msgs/JointState.h" -#include "geometry_msgs/WrenchStamped.h" -#include "geometry_msgs/PoseStamped.h" -#include "control_msgs/FollowJointTrajectoryAction.h" #include "actionlib/server/action_server.h" #include "actionlib/server/server_goal_handle.h" +#include "control_msgs/FollowJointTrajectoryAction.h" +#include "geometry_msgs/PoseStamped.h" +#include "geometry_msgs/WrenchStamped.h" +#include "ros/ros.h" +#include "sensor_msgs/JointState.h" +#include "std_msgs/String.h" #include "trajectory_msgs/JointTrajectoryPoint.h" +#include "ur_msgs/Analog.h" +#include "ur_msgs/Digital.h" +#include "ur_msgs/IOStates.h" #include "ur_msgs/SetIO.h" +#include "ur_msgs/SetIORequest.h" +#include "ur_msgs/SetIOResponse.h" #include "ur_msgs/SetPayload.h" #include "ur_msgs/SetPayloadRequest.h" #include "ur_msgs/SetPayloadResponse.h" -#include "ur_msgs/SetIORequest.h" -#include "ur_msgs/SetIOResponse.h" -#include "ur_msgs/IOStates.h" -#include "ur_msgs/Digital.h" -#include "ur_msgs/Analog.h" -#include "std_msgs/String.h" #include #include +#include /// TF #include @@ -57,125 +57,129 @@ class RosWrapper { protected: - UrDriver robot_; - std::condition_variable rt_msg_cond_; - std::condition_variable msg_cond_; - ros::NodeHandle nh_; - actionlib::ActionServer as_; - actionlib::ServerGoalHandle goal_handle_; - bool has_goal_; - control_msgs::FollowJointTrajectoryFeedback feedback_; - control_msgs::FollowJointTrajectoryResult result_; - ros::Subscriber speed_sub_; - ros::Subscriber urscript_sub_; - ros::ServiceServer io_srv_; - ros::ServiceServer payload_srv_; - std::thread* rt_publish_thread_; - std::thread* mb_publish_thread_; - double io_flag_delay_; - double max_velocity_; - std::vector joint_offsets_; + UrDriver robot_; + std::condition_variable rt_msg_cond_; + std::condition_variable msg_cond_; + ros::NodeHandle nh_; + actionlib::ActionServer as_; + actionlib::ServerGoalHandle goal_handle_; + bool has_goal_; + control_msgs::FollowJointTrajectoryFeedback feedback_; + control_msgs::FollowJointTrajectoryResult result_; + ros::Subscriber speed_sub_; + ros::Subscriber urscript_sub_; + ros::ServiceServer io_srv_; + ros::ServiceServer payload_srv_; + std::thread* rt_publish_thread_; + std::thread* mb_publish_thread_; + double io_flag_delay_; + double max_velocity_; + std::vector joint_offsets_; std::string base_frame_; std::string tool_frame_; - bool use_ros_control_; - std::thread* ros_control_thread_; - boost::shared_ptr hardware_interface_; - boost::shared_ptr controller_manager_; + bool use_ros_control_; + std::thread* ros_control_thread_; + boost::shared_ptr hardware_interface_; + boost::shared_ptr controller_manager_; public: - RosWrapper(std::string host, int reverse_port) : - as_(nh_, "follow_joint_trajectory", - boost::bind(&RosWrapper::goalCB, this, _1), - boost::bind(&RosWrapper::cancelCB, this, _1), false), robot_( - rt_msg_cond_, msg_cond_, host, reverse_port, 0.03, 300), io_flag_delay_(0.05), joint_offsets_( - 6, 0.0) { - - std::string joint_prefix = ""; - std::vector joint_names; - char buf[256]; - - if (ros::param::get("~prefix", joint_prefix)) { - if (joint_prefix.length() > 0) { - sprintf(buf, "Setting prefix to %s", joint_prefix.c_str()); - print_info(buf); - } - } - joint_names.push_back(joint_prefix + "shoulder_pan_joint"); - joint_names.push_back(joint_prefix + "shoulder_lift_joint"); - joint_names.push_back(joint_prefix + "elbow_joint"); - joint_names.push_back(joint_prefix + "wrist_1_joint"); - joint_names.push_back(joint_prefix + "wrist_2_joint"); - joint_names.push_back(joint_prefix + "wrist_3_joint"); - robot_.setJointNames(joint_names); - - use_ros_control_ = false; - ros::param::get("~use_ros_control", use_ros_control_); - - if (use_ros_control_) { - hardware_interface_.reset( - new ros_control_ur::UrHardwareInterface(nh_, &robot_)); - controller_manager_.reset( - new controller_manager::ControllerManager( - hardware_interface_.get(), nh_)); - double max_vel_change = 0.12; // equivalent of an acceleration of 15 rad/sec^2 - if (ros::param::get("~max_acceleration", max_vel_change)) { - max_vel_change = max_vel_change / 125; - } - sprintf(buf, "Max acceleration set to: %f [rad/sec²]", - max_vel_change * 125); - print_debug(buf); - hardware_interface_->setMaxVelChange(max_vel_change); - } - //Using a very high value in order to not limit execution of trajectories being sent from MoveIt! - max_velocity_ = 10.; - if (ros::param::get("~max_velocity", max_velocity_)) { - sprintf(buf, "Max velocity accepted by ur_driver: %f [rad/s]", - max_velocity_); - print_debug(buf); - } - - //Bounds for SetPayload service - //Using a very conservative value as it should be set through the parameter server - double min_payload = 0.; - double max_payload = 1.; - if (ros::param::get("~min_payload", min_payload)) { - sprintf(buf, "Min payload set to: %f [kg]", min_payload); - print_debug(buf); - } - if (ros::param::get("~max_payload", max_payload)) { - sprintf(buf, "Max payload set to: %f [kg]", max_payload); - print_debug(buf); - } - robot_.setMinPayload(min_payload); - robot_.setMaxPayload(max_payload); - sprintf(buf, "Bounds for set_payload service calls: [%f, %f]", - min_payload, max_payload); - print_debug(buf); - - double servoj_time = 0.008; - if (ros::param::get("~servoj_time", servoj_time)) { - sprintf(buf, "Servoj_time set to: %f [sec]", servoj_time); - print_debug(buf); - } - robot_.setServojTime(servoj_time); - - double servoj_lookahead_time = 0.03; - if (ros::param::get("~servoj_lookahead_time", servoj_lookahead_time)) { - sprintf(buf, "Servoj_lookahead_time set to: %f [sec]", servoj_lookahead_time); - print_debug(buf); - } - robot_.setServojLookahead(servoj_lookahead_time); - - double servoj_gain = 300.; - if (ros::param::get("~servoj_gain", servoj_gain)) { - sprintf(buf, "Servoj_gain set to: %f [sec]", servoj_gain); - print_debug(buf); - } - robot_.setServojGain(servoj_gain); + RosWrapper(std::string host, int reverse_port) + : as_(nh_, "follow_joint_trajectory", + boost::bind(&RosWrapper::goalCB, this, _1), + boost::bind(&RosWrapper::cancelCB, this, _1), false) + , robot_( + rt_msg_cond_, msg_cond_, host, reverse_port, 0.03, 300) + , io_flag_delay_(0.05) + , joint_offsets_( + 6, 0.0) + { + + std::string joint_prefix = ""; + std::vector joint_names; + char buf[256]; + + if (ros::param::get("~prefix", joint_prefix)) { + if (joint_prefix.length() > 0) { + sprintf(buf, "Setting prefix to %s", joint_prefix.c_str()); + print_info(buf); + } + } + joint_names.push_back(joint_prefix + "shoulder_pan_joint"); + joint_names.push_back(joint_prefix + "shoulder_lift_joint"); + joint_names.push_back(joint_prefix + "elbow_joint"); + joint_names.push_back(joint_prefix + "wrist_1_joint"); + joint_names.push_back(joint_prefix + "wrist_2_joint"); + joint_names.push_back(joint_prefix + "wrist_3_joint"); + robot_.setJointNames(joint_names); + + use_ros_control_ = false; + ros::param::get("~use_ros_control", use_ros_control_); + + if (use_ros_control_) { + hardware_interface_.reset( + new ros_control_ur::UrHardwareInterface(nh_, &robot_)); + controller_manager_.reset( + new controller_manager::ControllerManager( + hardware_interface_.get(), nh_)); + double max_vel_change = 0.12; // equivalent of an acceleration of 15 rad/sec^2 + if (ros::param::get("~max_acceleration", max_vel_change)) { + max_vel_change = max_vel_change / 125; + } + sprintf(buf, "Max acceleration set to: %f [rad/sec²]", + max_vel_change * 125); + print_debug(buf); + hardware_interface_->setMaxVelChange(max_vel_change); + } + //Using a very high value in order to not limit execution of trajectories being sent from MoveIt! + max_velocity_ = 10.; + if (ros::param::get("~max_velocity", max_velocity_)) { + sprintf(buf, "Max velocity accepted by ur_driver: %f [rad/s]", + max_velocity_); + print_debug(buf); + } + + //Bounds for SetPayload service + //Using a very conservative value as it should be set through the parameter server + double min_payload = 0.; + double max_payload = 1.; + if (ros::param::get("~min_payload", min_payload)) { + sprintf(buf, "Min payload set to: %f [kg]", min_payload); + print_debug(buf); + } + if (ros::param::get("~max_payload", max_payload)) { + sprintf(buf, "Max payload set to: %f [kg]", max_payload); + print_debug(buf); + } + robot_.setMinPayload(min_payload); + robot_.setMaxPayload(max_payload); + sprintf(buf, "Bounds for set_payload service calls: [%f, %f]", + min_payload, max_payload); + print_debug(buf); + + double servoj_time = 0.008; + if (ros::param::get("~servoj_time", servoj_time)) { + sprintf(buf, "Servoj_time set to: %f [sec]", servoj_time); + print_debug(buf); + } + robot_.setServojTime(servoj_time); + + double servoj_lookahead_time = 0.03; + if (ros::param::get("~servoj_lookahead_time", servoj_lookahead_time)) { + sprintf(buf, "Servoj_lookahead_time set to: %f [sec]", servoj_lookahead_time); + print_debug(buf); + } + robot_.setServojLookahead(servoj_lookahead_time); + + double servoj_gain = 300.; + if (ros::param::get("~servoj_gain", servoj_gain)) { + sprintf(buf, "Servoj_gain set to: %f [sec]", servoj_gain); + print_debug(buf); + } + robot_.setServojGain(servoj_gain); //Base and tool frames base_frame_ = joint_prefix + "base_link"; - tool_frame_ = joint_prefix + "tool0_controller"; + tool_frame_ = joint_prefix + "tool0_controller"; if (ros::param::get("~base_frame", base_frame_)) { sprintf(buf, "Base frame set to: %s", base_frame_.c_str()); print_debug(buf); @@ -185,506 +189,496 @@ class RosWrapper { print_debug(buf); } - if (robot_.start()) { - if (use_ros_control_) { - ros_control_thread_ = new std::thread( - boost::bind(&RosWrapper::rosControlLoop, this)); - print_debug( - "The control thread for this driver has been started"); - } else { - //start actionserver - has_goal_ = false; - as_.start(); - - //subscribe to the data topic of interest - rt_publish_thread_ = new std::thread( - boost::bind(&RosWrapper::publishRTMsg, this)); - print_debug( - "The action server for this driver has been started"); - } - mb_publish_thread_ = new std::thread( - boost::bind(&RosWrapper::publishMbMsg, this)); - speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1, - &RosWrapper::speedInterface, this); - urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, - &RosWrapper::urscriptInterface, this); - - io_srv_ = nh_.advertiseService("ur_driver/set_io", - &RosWrapper::setIO, this); - payload_srv_ = nh_.advertiseService("ur_driver/set_payload", - &RosWrapper::setPayload, this); - } - } - - void halt() { - robot_.halt(); - rt_publish_thread_->join(); - - } + if (robot_.start()) { + if (use_ros_control_) { + ros_control_thread_ = new std::thread( + boost::bind(&RosWrapper::rosControlLoop, this)); + print_debug( + "The control thread for this driver has been started"); + } else { + //start actionserver + has_goal_ = false; + as_.start(); + + //subscribe to the data topic of interest + rt_publish_thread_ = new std::thread( + boost::bind(&RosWrapper::publishRTMsg, this)); + print_debug( + "The action server for this driver has been started"); + } + mb_publish_thread_ = new std::thread( + boost::bind(&RosWrapper::publishMbMsg, this)); + speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1, + &RosWrapper::speedInterface, this); + urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, + &RosWrapper::urscriptInterface, this); + + io_srv_ = nh_.advertiseService("ur_driver/set_io", + &RosWrapper::setIO, this); + payload_srv_ = nh_.advertiseService("ur_driver/set_payload", + &RosWrapper::setPayload, this); + } + } + + void halt() + { + robot_.halt(); + rt_publish_thread_->join(); + } + private: - void trajThread(std::vector timestamps, - std::vector > positions, - std::vector > velocities) { - - robot_.doTraj(timestamps, positions, velocities); - if (has_goal_) { - result_.error_code = result_.SUCCESSFUL; - goal_handle_.setSucceeded(result_); - has_goal_ = false; - } - } - void goalCB( - actionlib::ServerGoalHandle< - control_msgs::FollowJointTrajectoryAction> gh) { - std::string buf; - print_info("on_goal"); - if (!robot_.sec_interface_->robot_state_->isReady()) { - result_.error_code = -100; //nothing is defined for this...? - - if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) { - result_.error_string = - "Cannot accept new trajectories: Robot arm is not powered on"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - if (!robot_.sec_interface_->robot_state_->isRealRobotEnabled()) { - result_.error_string = - "Cannot accept new trajectories: Robot is not enabled"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - result_.error_string = - "Cannot accept new trajectories. (Debug: Robot mode is " - + std::to_string( - robot_.sec_interface_->robot_state_->getRobotMode()) - + ")"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - if (robot_.sec_interface_->robot_state_->isEmergencyStopped()) { - result_.error_code = -100; //nothing is defined for this...? - result_.error_string = - "Cannot accept new trajectories: Robot is emergency stopped"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - if (robot_.sec_interface_->robot_state_->isProtectiveStopped()) { - result_.error_code = -100; //nothing is defined for this...? - result_.error_string = - "Cannot accept new trajectories: Robot is protective stopped"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - - actionlib::ActionServer::Goal goal = - *gh.getGoal(); //make a copy that we can modify - if (has_goal_) { - print_warning( - "Received new goal while still executing previous trajectory. Canceling previous trajectory"); - has_goal_ = false; - robot_.stopTraj(); - result_.error_code = -100; //nothing is defined for this...? - result_.error_string = "Received another trajectory"; - goal_handle_.setAborted(result_, result_.error_string); - std::this_thread::sleep_for(std::chrono::milliseconds(250)); - } - goal_handle_ = gh; - if (!validateJointNames()) { - std::string outp_joint_names = ""; - for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); - i++) { - outp_joint_names += goal.trajectory.joint_names[i] + " "; - } - result_.error_code = result_.INVALID_JOINTS; - result_.error_string = - "Received a goal with incorrect joint names: " - + outp_joint_names; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - if (!has_positions()) { - result_.error_code = result_.INVALID_GOAL; - result_.error_string = "Received a goal without positions"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - - if (!has_velocities()) { - result_.error_code = result_.INVALID_GOAL; - result_.error_string = "Received a goal without velocities"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - - if (!traj_is_finite()) { - result_.error_string = "Received a goal with infinities or NaNs"; - result_.error_code = result_.INVALID_GOAL; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - - if (!has_limited_velocities()) { - result_.error_code = result_.INVALID_GOAL; - result_.error_string = - "Received a goal with velocities that are higher than " - + std::to_string(max_velocity_); - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - - reorder_traj_joints(goal.trajectory); - - if (!start_positions_match(goal.trajectory, 0.01)) { - result_.error_code = result_.INVALID_GOAL; - result_.error_string = "Goal start doesn't match current pose"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - - std::vector timestamps; - std::vector > positions, velocities; - if (goal.trajectory.points[0].time_from_start.toSec() != 0.) { - print_warning( - "Trajectory's first point should be the current position, with time_from_start set to 0.0 - Inserting point in malformed trajectory"); - timestamps.push_back(0.0); - positions.push_back( - robot_.rt_interface_->robot_state_->getQActual()); - velocities.push_back( - robot_.rt_interface_->robot_state_->getQdActual()); - } - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { - timestamps.push_back( - goal.trajectory.points[i].time_from_start.toSec()); - positions.push_back(goal.trajectory.points[i].positions); - velocities.push_back(goal.trajectory.points[i].velocities); - - } - - goal_handle_.setAccepted(); - has_goal_ = true; - std::thread(&RosWrapper::trajThread, this, timestamps, positions, - velocities).detach(); - } - - void cancelCB( - actionlib::ServerGoalHandle< - control_msgs::FollowJointTrajectoryAction> gh) { - // set the action state to preempted - print_info("on_cancel"); - if (has_goal_) { - if (gh == goal_handle_) { - robot_.stopTraj(); - has_goal_ = false; - } - } - result_.error_code = -100; //nothing is defined for this...? - result_.error_string = "Goal cancelled by client"; - gh.setCanceled(result_); - } - - bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp) { - resp.success = true; - //if (req.fun == ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT) { - if (req.fun == 1) { - robot_.setDigitalOut(req.pin, req.state > 0.0 ? true : false); - } else if (req.fun == 2) { - //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_FLAG) { - robot_.setFlag(req.pin, req.state > 0.0 ? true : false); - //According to urdriver.py, set_flag will fail if called to rapidly in succession - ros::Duration(io_flag_delay_).sleep(); - } else if (req.fun == 3) { - //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_ANALOG_OUT) { - robot_.setAnalogOut(req.pin, req.state); - } else if (req.fun == 4) { - //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_TOOL_VOLTAGE) { - robot_.setToolVoltage((int) req.state); - } else { - resp.success = false; - } - return resp.success; - } - - bool setPayload(ur_msgs::SetPayloadRequest& req, - ur_msgs::SetPayloadResponse& resp) { - if (robot_.setPayload(req.payload)) - resp.success = true; - else - resp.success = true; - return resp.success; - } - - bool validateJointNames() { - std::vector actual_joint_names = robot_.getJointNames(); - actionlib::ActionServer::Goal goal = - *goal_handle_.getGoal(); - if (goal.trajectory.joint_names.size() != actual_joint_names.size()) - return false; - - for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) { - unsigned int j; - for (j = 0; j < actual_joint_names.size(); j++) { - if (goal.trajectory.joint_names[i] == actual_joint_names[j]) - break; - } - if (goal.trajectory.joint_names[i] == actual_joint_names[j]) { - actual_joint_names.erase(actual_joint_names.begin() + j); - } else { - return false; - } - } - - return true; - } - - void reorder_traj_joints(trajectory_msgs::JointTrajectory& traj) { - /* Reorders trajectory - destructive */ - std::vector actual_joint_names = robot_.getJointNames(); - std::vector mapping; - mapping.resize(actual_joint_names.size(), actual_joint_names.size()); - for (unsigned int i = 0; i < traj.joint_names.size(); i++) { - for (unsigned int j = 0; j < actual_joint_names.size(); j++) { - if (traj.joint_names[i] == actual_joint_names[j]) - mapping[j] = i; - } - } - traj.joint_names = actual_joint_names; - std::vector new_traj; - for (unsigned int i = 0; i < traj.points.size(); i++) { - trajectory_msgs::JointTrajectoryPoint new_point; - for (unsigned int j = 0; j < traj.points[i].positions.size(); j++) { - new_point.positions.push_back( - traj.points[i].positions[mapping[j]]); - new_point.velocities.push_back( - traj.points[i].velocities[mapping[j]]); - if (traj.points[i].accelerations.size() != 0) - new_point.accelerations.push_back( - traj.points[i].accelerations[mapping[j]]); - } - new_point.time_from_start = traj.points[i].time_from_start; - new_traj.push_back(new_point); - } - traj.points = new_traj; - } - - bool has_velocities() { - actionlib::ActionServer::Goal goal = - *goal_handle_.getGoal(); - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { - if (goal.trajectory.points[i].positions.size() - != goal.trajectory.points[i].velocities.size()) - return false; - } - return true; - } - - bool has_positions() { - actionlib::ActionServer::Goal goal = - *goal_handle_.getGoal(); - if (goal.trajectory.points.size() == 0) - return false; - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { - if (goal.trajectory.points[i].positions.size() - != goal.trajectory.joint_names.size()) - return false; - } - return true; - } - - bool start_positions_match(const trajectory_msgs::JointTrajectory &traj, double eps) - { - for (unsigned int i = 0; i < traj.points[0].positions.size(); i++) - { - std::vector qActual = robot_.rt_interface_->robot_state_->getQActual(); - if( fabs(traj.points[0].positions[i] - qActual[i]) > eps ) - { - return false; - } - } - return true; - } - - bool has_limited_velocities() { - actionlib::ActionServer::Goal goal = - *goal_handle_.getGoal(); - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { - for (unsigned int j = 0; - j < goal.trajectory.points[i].velocities.size(); j++) { - if (fabs(goal.trajectory.points[i].velocities[j]) - > max_velocity_) - return false; - } - } - return true; - } - - bool traj_is_finite() { - actionlib::ActionServer::Goal goal = - *goal_handle_.getGoal(); - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { - for (unsigned int j = 0; - j < goal.trajectory.points[i].velocities.size(); j++) { - if (!std::isfinite(goal.trajectory.points[i].positions[j])) - return false; - if (!std::isfinite(goal.trajectory.points[i].velocities[j])) - return false; - } - } - return true; - } - - void speedInterface(const trajectory_msgs::JointTrajectory::Ptr& msg) { - if (msg->points[0].velocities.size() == 6) { - double acc = 100; - if (msg->points[0].accelerations.size() > 0) - acc = *std::max_element(msg->points[0].accelerations.begin(), - msg->points[0].accelerations.end()); - robot_.setSpeed(msg->points[0].velocities[0], - msg->points[0].velocities[1], msg->points[0].velocities[2], - msg->points[0].velocities[3], msg->points[0].velocities[4], - msg->points[0].velocities[5], acc); - } - - } - void urscriptInterface(const std_msgs::String::ConstPtr& msg) { - - robot_.rt_interface_->addCommandToQueue(msg->data); - - } - - void rosControlLoop() { - ros::Duration elapsed_time; - struct timespec last_time, current_time; - static const double BILLION = 1000000000.0; - - realtime_tools::RealtimePublisher tf_pub( nh_, "/tf", 1 ); - geometry_msgs::TransformStamped tool_transform; - tool_transform.header.frame_id = base_frame_; - tool_transform.child_frame_id = tool_frame_; - tf_pub.msg_.transforms.push_back( tool_transform ); - - realtime_tools::RealtimePublisher tool_vel_pub( nh_, "tool_velocity", 1 ); - tool_vel_pub.msg_.header.frame_id = base_frame_; - - - clock_gettime(CLOCK_MONOTONIC, &last_time); - while (ros::ok()) { - std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex - std::unique_lock locker(msg_lock); - while (!robot_.rt_interface_->robot_state_->getControllerUpdated()) { - rt_msg_cond_.wait(locker); - } - // Input - hardware_interface_->read(); - robot_.rt_interface_->robot_state_->setControllerUpdated(); - - // Control - clock_gettime(CLOCK_MONOTONIC, ¤t_time); - elapsed_time = ros::Duration(current_time.tv_sec - last_time.tv_sec + (current_time.tv_nsec - last_time.tv_nsec)/ BILLION); - ros::Time ros_time = ros::Time::now(); - controller_manager_->update(ros_time, elapsed_time); - last_time = current_time; - - // Output - hardware_interface_->write(); - - // Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation - std::vector tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual(); - - // Compute rotation angle - double rx = tool_vector_actual[3]; - double ry = tool_vector_actual[4]; - double rz = tool_vector_actual[5]; - double angle = std::sqrt(std::pow(rx,2) + std::pow(ry,2) + std::pow(rz,2)); - - // Broadcast transform - if( tf_pub.trylock() ) - { - tf_pub.msg_.transforms[0].header.stamp = ros_time; - if (angle < 1e-16) { - tf_pub.msg_.transforms[0].transform.rotation.x = 0; - tf_pub.msg_.transforms[0].transform.rotation.y = 0; - tf_pub.msg_.transforms[0].transform.rotation.z = 0; - tf_pub.msg_.transforms[0].transform.rotation.w = 1; - } else { - tf_pub.msg_.transforms[0].transform.rotation.x = (rx/angle) * std::sin(angle*0.5); - tf_pub.msg_.transforms[0].transform.rotation.y = (ry/angle) * std::sin(angle*0.5); - tf_pub.msg_.transforms[0].transform.rotation.z = (rz/angle) * std::sin(angle*0.5); - tf_pub.msg_.transforms[0].transform.rotation.w = std::cos(angle*0.5); - } - tf_pub.msg_.transforms[0].transform.translation.x = tool_vector_actual[0]; - tf_pub.msg_.transforms[0].transform.translation.y = tool_vector_actual[1]; - tf_pub.msg_.transforms[0].transform.translation.z = tool_vector_actual[2]; - - tf_pub.unlockAndPublish(); - } - - //Publish tool velocity - std::vector tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual(); - - if( tool_vel_pub.trylock() ) - { - tool_vel_pub.msg_.header.stamp = ros_time; - tool_vel_pub.msg_.twist.linear.x = tcp_speed[0]; - tool_vel_pub.msg_.twist.linear.y = tcp_speed[1]; - tool_vel_pub.msg_.twist.linear.z = tcp_speed[2]; - tool_vel_pub.msg_.twist.angular.x = tcp_speed[3]; - tool_vel_pub.msg_.twist.angular.y = tcp_speed[4]; - tool_vel_pub.msg_.twist.angular.z = tcp_speed[5]; - - tool_vel_pub.unlockAndPublish(); - } - - } - } - - void publishRTMsg() { - ros::Publisher joint_pub = nh_.advertise( - "joint_states", 1); - ros::Publisher wrench_pub = nh_.advertise( - "wrench", 1); + void trajThread(std::vector timestamps, + std::vector > positions, + std::vector > velocities) + { + + robot_.doTraj(timestamps, positions, velocities); + if (has_goal_) { + result_.error_code = result_.SUCCESSFUL; + goal_handle_.setSucceeded(result_); + has_goal_ = false; + } + } + void goalCB( + actionlib::ServerGoalHandle gh) + { + std::string buf; + print_info("on_goal"); + if (!robot_.sec_interface_->robot_state_->isReady()) { + result_.error_code = -100; //nothing is defined for this...? + + if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) { + result_.error_string = "Cannot accept new trajectories: Robot arm is not powered on"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + if (!robot_.sec_interface_->robot_state_->isRealRobotEnabled()) { + result_.error_string = "Cannot accept new trajectories: Robot is not enabled"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + result_.error_string = "Cannot accept new trajectories. (Debug: Robot mode is " + + std::to_string( + robot_.sec_interface_->robot_state_->getRobotMode()) + + ")"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + if (robot_.sec_interface_->robot_state_->isEmergencyStopped()) { + result_.error_code = -100; //nothing is defined for this...? + result_.error_string = "Cannot accept new trajectories: Robot is emergency stopped"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + if (robot_.sec_interface_->robot_state_->isProtectiveStopped()) { + result_.error_code = -100; //nothing is defined for this...? + result_.error_string = "Cannot accept new trajectories: Robot is protective stopped"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + + actionlib::ActionServer::Goal goal = *gh.getGoal(); //make a copy that we can modify + if (has_goal_) { + print_warning( + "Received new goal while still executing previous trajectory. Canceling previous trajectory"); + has_goal_ = false; + robot_.stopTraj(); + result_.error_code = -100; //nothing is defined for this...? + result_.error_string = "Received another trajectory"; + goal_handle_.setAborted(result_, result_.error_string); + std::this_thread::sleep_for(std::chrono::milliseconds(250)); + } + goal_handle_ = gh; + if (!validateJointNames()) { + std::string outp_joint_names = ""; + for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); + i++) { + outp_joint_names += goal.trajectory.joint_names[i] + " "; + } + result_.error_code = result_.INVALID_JOINTS; + result_.error_string = "Received a goal with incorrect joint names: " + + outp_joint_names; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + if (!has_positions()) { + result_.error_code = result_.INVALID_GOAL; + result_.error_string = "Received a goal without positions"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + + if (!has_velocities()) { + result_.error_code = result_.INVALID_GOAL; + result_.error_string = "Received a goal without velocities"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + + if (!traj_is_finite()) { + result_.error_string = "Received a goal with infinities or NaNs"; + result_.error_code = result_.INVALID_GOAL; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + + if (!has_limited_velocities()) { + result_.error_code = result_.INVALID_GOAL; + result_.error_string = "Received a goal with velocities that are higher than " + + std::to_string(max_velocity_); + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + + reorder_traj_joints(goal.trajectory); + + if (!start_positions_match(goal.trajectory, 0.01)) { + result_.error_code = result_.INVALID_GOAL; + result_.error_string = "Goal start doesn't match current pose"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + + std::vector timestamps; + std::vector > positions, velocities; + if (goal.trajectory.points[0].time_from_start.toSec() != 0.) { + print_warning( + "Trajectory's first point should be the current position, with time_from_start set to 0.0 - Inserting point in malformed trajectory"); + timestamps.push_back(0.0); + positions.push_back( + robot_.rt_interface_->robot_state_->getQActual()); + velocities.push_back( + robot_.rt_interface_->robot_state_->getQdActual()); + } + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { + timestamps.push_back( + goal.trajectory.points[i].time_from_start.toSec()); + positions.push_back(goal.trajectory.points[i].positions); + velocities.push_back(goal.trajectory.points[i].velocities); + } + + goal_handle_.setAccepted(); + has_goal_ = true; + std::thread(&RosWrapper::trajThread, this, timestamps, positions, + velocities) + .detach(); + } + + void cancelCB( + actionlib::ServerGoalHandle gh) + { + // set the action state to preempted + print_info("on_cancel"); + if (has_goal_) { + if (gh == goal_handle_) { + robot_.stopTraj(); + has_goal_ = false; + } + } + result_.error_code = -100; //nothing is defined for this...? + result_.error_string = "Goal cancelled by client"; + gh.setCanceled(result_); + } + + bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp) + { + resp.success = true; + //if (req.fun == ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT) { + if (req.fun == 1) { + robot_.setDigitalOut(req.pin, req.state > 0.0 ? true : false); + } else if (req.fun == 2) { + //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_FLAG) { + robot_.setFlag(req.pin, req.state > 0.0 ? true : false); + //According to urdriver.py, set_flag will fail if called to rapidly in succession + ros::Duration(io_flag_delay_).sleep(); + } else if (req.fun == 3) { + //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_ANALOG_OUT) { + robot_.setAnalogOut(req.pin, req.state); + } else if (req.fun == 4) { + //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_TOOL_VOLTAGE) { + robot_.setToolVoltage((int)req.state); + } else { + resp.success = false; + } + return resp.success; + } + + bool setPayload(ur_msgs::SetPayloadRequest& req, + ur_msgs::SetPayloadResponse& resp) + { + if (robot_.setPayload(req.payload)) + resp.success = true; + else + resp.success = true; + return resp.success; + } + + bool validateJointNames() + { + std::vector actual_joint_names = robot_.getJointNames(); + actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); + if (goal.trajectory.joint_names.size() != actual_joint_names.size()) + return false; + + for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) { + unsigned int j; + for (j = 0; j < actual_joint_names.size(); j++) { + if (goal.trajectory.joint_names[i] == actual_joint_names[j]) + break; + } + if (goal.trajectory.joint_names[i] == actual_joint_names[j]) { + actual_joint_names.erase(actual_joint_names.begin() + j); + } else { + return false; + } + } + + return true; + } + + void reorder_traj_joints(trajectory_msgs::JointTrajectory& traj) + { + /* Reorders trajectory - destructive */ + std::vector actual_joint_names = robot_.getJointNames(); + std::vector mapping; + mapping.resize(actual_joint_names.size(), actual_joint_names.size()); + for (unsigned int i = 0; i < traj.joint_names.size(); i++) { + for (unsigned int j = 0; j < actual_joint_names.size(); j++) { + if (traj.joint_names[i] == actual_joint_names[j]) + mapping[j] = i; + } + } + traj.joint_names = actual_joint_names; + std::vector new_traj; + for (unsigned int i = 0; i < traj.points.size(); i++) { + trajectory_msgs::JointTrajectoryPoint new_point; + for (unsigned int j = 0; j < traj.points[i].positions.size(); j++) { + new_point.positions.push_back( + traj.points[i].positions[mapping[j]]); + new_point.velocities.push_back( + traj.points[i].velocities[mapping[j]]); + if (traj.points[i].accelerations.size() != 0) + new_point.accelerations.push_back( + traj.points[i].accelerations[mapping[j]]); + } + new_point.time_from_start = traj.points[i].time_from_start; + new_traj.push_back(new_point); + } + traj.points = new_traj; + } + + bool has_velocities() + { + actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { + if (goal.trajectory.points[i].positions.size() + != goal.trajectory.points[i].velocities.size()) + return false; + } + return true; + } + + bool has_positions() + { + actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); + if (goal.trajectory.points.size() == 0) + return false; + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { + if (goal.trajectory.points[i].positions.size() + != goal.trajectory.joint_names.size()) + return false; + } + return true; + } + + bool start_positions_match(const trajectory_msgs::JointTrajectory& traj, double eps) + { + for (unsigned int i = 0; i < traj.points[0].positions.size(); i++) { + std::vector qActual = robot_.rt_interface_->robot_state_->getQActual(); + if (fabs(traj.points[0].positions[i] - qActual[i]) > eps) { + return false; + } + } + return true; + } + + bool has_limited_velocities() + { + actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { + for (unsigned int j = 0; + j < goal.trajectory.points[i].velocities.size(); j++) { + if (fabs(goal.trajectory.points[i].velocities[j]) + > max_velocity_) + return false; + } + } + return true; + } + + bool traj_is_finite() + { + actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { + for (unsigned int j = 0; + j < goal.trajectory.points[i].velocities.size(); j++) { + if (!std::isfinite(goal.trajectory.points[i].positions[j])) + return false; + if (!std::isfinite(goal.trajectory.points[i].velocities[j])) + return false; + } + } + return true; + } + + void speedInterface(const trajectory_msgs::JointTrajectory::Ptr& msg) + { + if (msg->points[0].velocities.size() == 6) { + double acc = 100; + if (msg->points[0].accelerations.size() > 0) + acc = *std::max_element(msg->points[0].accelerations.begin(), + msg->points[0].accelerations.end()); + robot_.setSpeed(msg->points[0].velocities[0], + msg->points[0].velocities[1], msg->points[0].velocities[2], + msg->points[0].velocities[3], msg->points[0].velocities[4], + msg->points[0].velocities[5], acc); + } + } + void urscriptInterface(const std_msgs::String::ConstPtr& msg) + { + + robot_.rt_interface_->addCommandToQueue(msg->data); + } + + void rosControlLoop() + { + ros::Duration elapsed_time; + struct timespec last_time, current_time; + static const double BILLION = 1000000000.0; + + realtime_tools::RealtimePublisher tf_pub(nh_, "/tf", 1); + geometry_msgs::TransformStamped tool_transform; + tool_transform.header.frame_id = base_frame_; + tool_transform.child_frame_id = tool_frame_; + tf_pub.msg_.transforms.push_back(tool_transform); + + realtime_tools::RealtimePublisher tool_vel_pub(nh_, "tool_velocity", 1); + tool_vel_pub.msg_.header.frame_id = base_frame_; + + clock_gettime(CLOCK_MONOTONIC, &last_time); + while (ros::ok()) { + std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex + std::unique_lock locker(msg_lock); + while (!robot_.rt_interface_->robot_state_->getControllerUpdated()) { + rt_msg_cond_.wait(locker); + } + // Input + hardware_interface_->read(); + robot_.rt_interface_->robot_state_->setControllerUpdated(); + + // Control + clock_gettime(CLOCK_MONOTONIC, ¤t_time); + elapsed_time = ros::Duration(current_time.tv_sec - last_time.tv_sec + (current_time.tv_nsec - last_time.tv_nsec) / BILLION); + ros::Time ros_time = ros::Time::now(); + controller_manager_->update(ros_time, elapsed_time); + last_time = current_time; + + // Output + hardware_interface_->write(); + + // Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation + std::vector tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual(); + + // Compute rotation angle + double rx = tool_vector_actual[3]; + double ry = tool_vector_actual[4]; + double rz = tool_vector_actual[5]; + double angle = std::sqrt(std::pow(rx, 2) + std::pow(ry, 2) + std::pow(rz, 2)); + + // Broadcast transform + if (tf_pub.trylock()) { + tf_pub.msg_.transforms[0].header.stamp = ros_time; + if (angle < 1e-16) { + tf_pub.msg_.transforms[0].transform.rotation.x = 0; + tf_pub.msg_.transforms[0].transform.rotation.y = 0; + tf_pub.msg_.transforms[0].transform.rotation.z = 0; + tf_pub.msg_.transforms[0].transform.rotation.w = 1; + } else { + tf_pub.msg_.transforms[0].transform.rotation.x = (rx / angle) * std::sin(angle * 0.5); + tf_pub.msg_.transforms[0].transform.rotation.y = (ry / angle) * std::sin(angle * 0.5); + tf_pub.msg_.transforms[0].transform.rotation.z = (rz / angle) * std::sin(angle * 0.5); + tf_pub.msg_.transforms[0].transform.rotation.w = std::cos(angle * 0.5); + } + tf_pub.msg_.transforms[0].transform.translation.x = tool_vector_actual[0]; + tf_pub.msg_.transforms[0].transform.translation.y = tool_vector_actual[1]; + tf_pub.msg_.transforms[0].transform.translation.z = tool_vector_actual[2]; + + tf_pub.unlockAndPublish(); + } + + //Publish tool velocity + std::vector tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual(); + + if (tool_vel_pub.trylock()) { + tool_vel_pub.msg_.header.stamp = ros_time; + tool_vel_pub.msg_.twist.linear.x = tcp_speed[0]; + tool_vel_pub.msg_.twist.linear.y = tcp_speed[1]; + tool_vel_pub.msg_.twist.linear.z = tcp_speed[2]; + tool_vel_pub.msg_.twist.angular.x = tcp_speed[3]; + tool_vel_pub.msg_.twist.angular.y = tcp_speed[4]; + tool_vel_pub.msg_.twist.angular.z = tcp_speed[5]; + + tool_vel_pub.unlockAndPublish(); + } + } + } + + void publishRTMsg() + { + ros::Publisher joint_pub = nh_.advertise( + "joint_states", 1); + ros::Publisher wrench_pub = nh_.advertise( + "wrench", 1); ros::Publisher tool_vel_pub = nh_.advertise("tool_velocity", 1); static tf::TransformBroadcaster br; - while (ros::ok()) { - sensor_msgs::JointState joint_msg; - joint_msg.name = robot_.getJointNames(); - geometry_msgs::WrenchStamped wrench_msg; + while (ros::ok()) { + sensor_msgs::JointState joint_msg; + joint_msg.name = robot_.getJointNames(); + geometry_msgs::WrenchStamped wrench_msg; geometry_msgs::PoseStamped tool_pose_msg; - std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex - std::unique_lock locker(msg_lock); - while (!robot_.rt_interface_->robot_state_->getDataPublished()) { - rt_msg_cond_.wait(locker); - } - joint_msg.header.stamp = ros::Time::now(); - joint_msg.position = - robot_.rt_interface_->robot_state_->getQActual(); - for (unsigned int i = 0; i < joint_msg.position.size(); i++) { - joint_msg.position[i] += joint_offsets_[i]; - } - joint_msg.velocity = - robot_.rt_interface_->robot_state_->getQdActual(); - joint_msg.effort = robot_.rt_interface_->robot_state_->getIActual(); - joint_pub.publish(joint_msg); - std::vector tcp_force = - robot_.rt_interface_->robot_state_->getTcpForce(); - wrench_msg.header.stamp = joint_msg.header.stamp; - wrench_msg.wrench.force.x = tcp_force[0]; - wrench_msg.wrench.force.y = tcp_force[1]; - wrench_msg.wrench.force.z = tcp_force[2]; - wrench_msg.wrench.torque.x = tcp_force[3]; - wrench_msg.wrench.torque.y = tcp_force[4]; - wrench_msg.wrench.torque.z = tcp_force[5]; - wrench_pub.publish(wrench_msg); + std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex + std::unique_lock locker(msg_lock); + while (!robot_.rt_interface_->robot_state_->getDataPublished()) { + rt_msg_cond_.wait(locker); + } + joint_msg.header.stamp = ros::Time::now(); + joint_msg.position = robot_.rt_interface_->robot_state_->getQActual(); + for (unsigned int i = 0; i < joint_msg.position.size(); i++) { + joint_msg.position[i] += joint_offsets_[i]; + } + joint_msg.velocity = robot_.rt_interface_->robot_state_->getQdActual(); + joint_msg.effort = robot_.rt_interface_->robot_state_->getIActual(); + joint_pub.publish(joint_msg); + std::vector tcp_force = robot_.rt_interface_->robot_state_->getTcpForce(); + wrench_msg.header.stamp = joint_msg.header.stamp; + wrench_msg.wrench.force.x = tcp_force[0]; + wrench_msg.wrench.force.y = tcp_force[1]; + wrench_msg.wrench.force.z = tcp_force[2]; + wrench_msg.wrench.torque.x = tcp_force[3]; + wrench_msg.wrench.torque.y = tcp_force[4]; + wrench_msg.wrench.torque.z = tcp_force[5]; + wrench_pub.publish(wrench_msg); // Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation std::vector tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual(); @@ -694,11 +688,11 @@ class RosWrapper { 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 @@ class RosWrapper { br.sendTransform(tf::StampedTransform(transform, joint_msg.header.stamp, base_frame_, tool_frame_)); //Publish tool velocity - std::vector tcp_speed = - robot_.rt_interface_->robot_state_->getTcpSpeedActual(); + std::vector tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual(); geometry_msgs::TwistStamped tool_twist; tool_twist.header.frame_id = base_frame_; tool_twist.header.stamp = joint_msg.header.stamp; @@ -721,119 +714,118 @@ class RosWrapper { tool_twist.twist.angular.z = tcp_speed[5]; tool_vel_pub.publish(tool_twist); - robot_.rt_interface_->robot_state_->setDataPublished(); - } - } - - void publishMbMsg() { - bool warned = false; - ros::Publisher io_pub = nh_.advertise( - "ur_driver/io_states", 1); - - while (ros::ok()) { - ur_msgs::IOStates io_msg; - std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex - std::unique_lock locker(msg_lock); - while (!robot_.sec_interface_->robot_state_->getNewDataAvailable()) { - msg_cond_.wait(locker); - } - int i_max = 10; - if (robot_.sec_interface_->robot_state_->getVersion() > 3.0) - i_max = 18; // From version 3.0, there are up to 18 inputs and outputs - for (unsigned int i = 0; i < i_max; i++) { - ur_msgs::Digital digi; - digi.pin = i; - digi.state = - ((robot_.sec_interface_->robot_state_->getDigitalInputBits() - & (1 << i)) >> i); - io_msg.digital_in_states.push_back(digi); - digi.state = - ((robot_.sec_interface_->robot_state_->getDigitalOutputBits() - & (1 << i)) >> i); - io_msg.digital_out_states.push_back(digi); - } - ur_msgs::Analog ana; - ana.pin = 0; - ana.state = robot_.sec_interface_->robot_state_->getAnalogInput0(); - io_msg.analog_in_states.push_back(ana); - ana.pin = 1; - ana.state = robot_.sec_interface_->robot_state_->getAnalogInput1(); - io_msg.analog_in_states.push_back(ana); - - ana.pin = 0; - ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput0(); - io_msg.analog_out_states.push_back(ana); - ana.pin = 1; - ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput1(); - io_msg.analog_out_states.push_back(ana); - io_pub.publish(io_msg); - - if (robot_.sec_interface_->robot_state_->isEmergencyStopped() - or robot_.sec_interface_->robot_state_->isProtectiveStopped()) { - if (robot_.sec_interface_->robot_state_->isEmergencyStopped() - and !warned) { - print_error("Emergency stop pressed!"); - } else if (robot_.sec_interface_->robot_state_->isProtectiveStopped() - and !warned) { - print_error("Robot is protective stopped!"); - } - if (has_goal_) { - print_error("Aborting trajectory"); - robot_.stopTraj(); - result_.error_code = result_.SUCCESSFUL; - result_.error_string = "Robot was halted"; - goal_handle_.setAborted(result_, result_.error_string); - has_goal_ = false; - } - warned = true; - } else - warned = false; - - robot_.sec_interface_->robot_state_->finishedReading(); - - } - } - + robot_.rt_interface_->robot_state_->setDataPublished(); + } + } + + void publishMbMsg() + { + bool warned = false; + ros::Publisher io_pub = nh_.advertise( + "ur_driver/io_states", 1); + + while (ros::ok()) { + ur_msgs::IOStates io_msg; + std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex + std::unique_lock locker(msg_lock); + while (!robot_.sec_interface_->robot_state_->getNewDataAvailable()) { + msg_cond_.wait(locker); + } + int i_max = 10; + if (robot_.sec_interface_->robot_state_->getVersion() > 3.0) + i_max = 18; // From version 3.0, there are up to 18 inputs and outputs + for (unsigned int i = 0; i < i_max; i++) { + ur_msgs::Digital digi; + digi.pin = i; + digi.state = ((robot_.sec_interface_->robot_state_->getDigitalInputBits() + & (1 << i)) + >> i); + io_msg.digital_in_states.push_back(digi); + digi.state = ((robot_.sec_interface_->robot_state_->getDigitalOutputBits() + & (1 << i)) + >> i); + io_msg.digital_out_states.push_back(digi); + } + ur_msgs::Analog ana; + ana.pin = 0; + ana.state = robot_.sec_interface_->robot_state_->getAnalogInput0(); + io_msg.analog_in_states.push_back(ana); + ana.pin = 1; + ana.state = robot_.sec_interface_->robot_state_->getAnalogInput1(); + io_msg.analog_in_states.push_back(ana); + + ana.pin = 0; + ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput0(); + io_msg.analog_out_states.push_back(ana); + ana.pin = 1; + ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput1(); + io_msg.analog_out_states.push_back(ana); + io_pub.publish(io_msg); + + if (robot_.sec_interface_->robot_state_->isEmergencyStopped() + or robot_.sec_interface_->robot_state_->isProtectiveStopped()) { + if (robot_.sec_interface_->robot_state_->isEmergencyStopped() + and !warned) { + print_error("Emergency stop pressed!"); + } else if (robot_.sec_interface_->robot_state_->isProtectiveStopped() + and !warned) { + print_error("Robot is protective stopped!"); + } + if (has_goal_) { + print_error("Aborting trajectory"); + robot_.stopTraj(); + result_.error_code = result_.SUCCESSFUL; + result_.error_string = "Robot was halted"; + goal_handle_.setAborted(result_, result_.error_string); + has_goal_ = false; + } + warned = true; + } else + warned = false; + + robot_.sec_interface_->robot_state_->finishedReading(); + } + } }; -int main(int argc, char **argv) { - bool use_sim_time = false; - std::string host; - int reverse_port = 50001; - - ros::init(argc, argv, "ur_driver"); - ros::NodeHandle nh; - if (ros::param::get("use_sim_time", use_sim_time)) { - print_warning("use_sim_time is set!!"); - } - if (!(ros::param::get("~robot_ip_address", host))) { - if (argc > 1) { - print_warning( - "Please set the parameter robot_ip_address instead of giving it as a command line argument. This method is DEPRECATED"); - host = argv[1]; - } else { - print_fatal( - "Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip"); - exit(1); - } - - } - if ((ros::param::get("~reverse_port", reverse_port))) { - if((reverse_port <= 0) or (reverse_port >= 65535)) { - print_warning("Reverse port value is not valid (Use number between 1 and 65534. Using default value of 50001"); - reverse_port = 50001; - } - } else - reverse_port = 50001; - - RosWrapper interface(host, reverse_port); - - ros::AsyncSpinner spinner(3); - spinner.start(); - - ros::waitForShutdown(); - - interface.halt(); - - exit(0); +int main(int argc, char** argv) +{ + bool use_sim_time = false; + std::string host; + int reverse_port = 50001; + + ros::init(argc, argv, "ur_driver"); + ros::NodeHandle nh; + if (ros::param::get("use_sim_time", use_sim_time)) { + print_warning("use_sim_time is set!!"); + } + if (!(ros::param::get("~robot_ip_address", host))) { + if (argc > 1) { + print_warning( + "Please set the parameter robot_ip_address instead of giving it as a command line argument. This method is DEPRECATED"); + host = argv[1]; + } else { + print_fatal( + "Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip"); + exit(1); + } + } + if ((ros::param::get("~reverse_port", reverse_port))) { + if ((reverse_port <= 0) or (reverse_port >= 65535)) { + print_warning("Reverse port value is not valid (Use number between 1 and 65534. Using default value of 50001"); + reverse_port = 50001; + } + } else + reverse_port = 50001; + + RosWrapper interface(host, reverse_port); + + ros::AsyncSpinner spinner(3); + spinner.start(); + + ros::waitForShutdown(); + + interface.halt(); + + exit(0); } From b422107c082972af468a0e77e4c498794877d0f4 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Thu, 16 Feb 2017 22:49:10 +0100 Subject: [PATCH 019/114] Improved RTPublisher --- include/ur_modern_driver/ros/rt_publisher.h | 19 +++++-- src/ros/rt_publisher.cpp | 57 +++++++++++++++++---- 2 files changed, 62 insertions(+), 14 deletions(-) diff --git a/include/ur_modern_driver/ros/rt_publisher.h b/include/ur_modern_driver/ros/rt_publisher.h index 0737e61d8..087052657 100644 --- a/include/ur_modern_driver/ros/rt_publisher.h +++ b/include/ur_modern_driver/ros/rt_publisher.h @@ -6,11 +6,15 @@ #include #include #include +#include +#include +#include #include #include "ur_modern_driver/ur/consumer.h" using namespace ros; +using namespace tf; const std::string JOINTS[] = { "shoulder_pan_joint", @@ -27,21 +31,28 @@ class RTPublisher : public URRTPacketConsumer { Publisher _joint_pub; Publisher _wrench_pub; Publisher _tool_vel_pub; + Publisher _joint_temperature_pub; + TransformBroadcaster _transform_broadcaster; std::vector _joint_names; std::string _base_frame; + std::string _tool_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, Time& t); + bool publish_wrench(RTShared& packet, Time& t); + bool publish_tool(RTShared& packet, Time& t); + bool publish_transform(RTShared& packet, Time& t); + bool publish_temperature(RTShared& packet, Time& t); bool publish(RTShared& packet); public: - RTPublisher(std::string& joint_prefix, std::string& base_frame) + RTPublisher(std::string& joint_prefix, std::string& base_frame, std::string& tool_frame) : _joint_pub(_nh.advertise("joint_states", 1)) , _wrench_pub(_nh.advertise("wrench", 1)) , _tool_vel_pub(_nh.advertise("tool_velocity", 1)) + , _joint_temperature_pub(_nh.advertise("joint_temperature", 1)) , _base_frame(base_frame) + , _tool_frame(tool_frame) { for (auto const& j : JOINTS) { _joint_names.push_back(joint_prefix + j); diff --git a/src/ros/rt_publisher.cpp b/src/ros/rt_publisher.cpp index af5a4a47a..d58b023da 100644 --- a/src/ros/rt_publisher.cpp +++ b/src/ros/rt_publisher.cpp @@ -1,6 +1,6 @@ #include "ur_modern_driver/ros/rt_publisher.h" -bool RTPublisher::publish_joints(RTShared& packet, ros::Time& t) +bool RTPublisher::publish_joints(RTShared& packet, Time& t) { sensor_msgs::JointState joint_msg; joint_msg.header.stamp = t; @@ -21,11 +21,10 @@ 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, Time& t) { geometry_msgs::WrenchStamped wrench_msg; wrench_msg.header.stamp = t; - wrench_msg.wrench.force.x = packet.tcp_force[0]; wrench_msg.wrench.force.y = packet.tcp_force[1]; wrench_msg.wrench.force.z = packet.tcp_force[2]; @@ -34,18 +33,14 @@ bool RTPublisher::publish_wrench(RTShared& packet, ros::Time& t) wrench_msg.wrench.torque.z = packet.tcp_force[5]; _wrench_pub.publish(wrench_msg); - return true; } -bool RTPublisher::publish_tool(RTShared& packet, ros::Time& t) +bool RTPublisher::publish_tool(RTShared& packet, Time& t) { geometry_msgs::TwistStamped tool_twist; - tool_twist.header.stamp = t; - tool_twist.header.frame_id = _base_frame; - tool_twist.twist.linear.x = packet.tcp_speed_actual.position.x; tool_twist.twist.linear.y = packet.tcp_speed_actual.position.y; tool_twist.twist.linear.z = packet.tcp_speed_actual.position.z; @@ -54,14 +49,56 @@ bool RTPublisher::publish_tool(RTShared& packet, ros::Time& t) tool_twist.twist.angular.z = packet.tcp_speed_actual.rotation.z; _tool_vel_pub.publish(tool_twist); + return true; +} + +bool RTPublisher::publish_transform(RTShared& packet, Time& t) +{ + + auto tv = packet.tool_vector_actual; + + Transform transform; + transform.setOrigin(Vector3(tv.position.x, tv.position.y, tv.position.z)); + //Create quaternion + Quaternion quat; + + double angle = std::sqrt(std::pow(tv.rotation.x, 2) + std::pow(tv.rotation.y, 2) + std::pow(tv.rotation.z, 2)); + if (angle < 1e-16) { + quat.setValue(0, 0, 0, 1); + } else { + quat.setRotation(Vector3(tv.rotation.x / angle, tv.rotation.y / angle, tv.rotation.z / angle), angle); + } + + transform.setRotation(quat); + + _transform_broadcaster.sendTransform(StampedTransform(transform, t, _base_frame, _tool_frame)); + + return true; +} + +bool RTPublisher::publish_temperature(RTShared& packet, Time& t) +{ + size_t len = _joint_names.size(); + for (size_t i = 0; i < len; i++) { + sensor_msgs::Temperature msg; + msg.header.stamp = t; + msg.header.frame_id = _joint_names[i]; + msg.temperature = packet.motor_temperatures[i]; + + _joint_temperature_pub.publish(msg); + } return true; } bool RTPublisher::publish(RTShared& packet) { - ros::Time time = ros::Time::now(); - return publish_joints(packet, time) && publish_wrench(packet, time) && publish_tool(packet, time); + Time time = Time::now(); + return publish_joints(packet, time) + && publish_wrench(packet, time) + && publish_tool(packet, time) + && publish_transform(packet, time) + && publish_temperature(packet, time); } bool RTPublisher::consume(RTState_V1_6__7& state) From fc53fbdd689526a078812f3fe9d49e93ada7120f Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Thu, 16 Feb 2017 22:49:58 +0100 Subject: [PATCH 020/114] Fixed consumer thread getting stuck in dequeue on shutdown --- include/ur_modern_driver/pipeline.h | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/include/ur_modern_driver/pipeline.h b/include/ur_modern_driver/pipeline.h index 13a9b1bcd..df56421d0 100644 --- a/include/ur_modern_driver/pipeline.h +++ b/include/ur_modern_driver/pipeline.h @@ -49,7 +49,7 @@ class Pipeline { for (auto& p : products) { if (!_queue.try_enqueue(std::move(p))) { - LOG_WARN("Pipeline owerflowed!"); + LOG_ERROR("Pipeline owerflowed!"); } } @@ -57,6 +57,7 @@ class Pipeline { } _producer.teardown_producer(); //todo cleanup + LOG_DEBUG("Pipline producer ended"); } void run_consumer() @@ -64,12 +65,18 @@ class Pipeline { _consumer.setup_consumer(); unique_ptr product; while (_running) { - _queue.wait_dequeue(product); + // 16000us timeout was chosen because we should + // roughly recieve messages at 125hz which is every + // 8ms == 8000us and double it for some error margin + if (!_queue.wait_dequeue_timed(product, 16000)) { + continue; + } if (!_consumer.consume(std::move(product))) break; } _consumer.teardown_consumer(); //todo cleanup + LOG_DEBUG("Pipline consumer ended"); } public: @@ -96,6 +103,8 @@ class Pipeline { if (!_running) return; + LOG_DEBUG("Stopping pipeline"); + _consumer.stop_consumer(); _producer.stop_producer(); From 9c1077a7797f50a7ad8eb0fb0fa0a5bc960c43cb Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Thu, 16 Feb 2017 22:50:35 +0100 Subject: [PATCH 021/114] Improved URStream logging --- src/ur/stream.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/ur/stream.cpp b/src/ur/stream.cpp index d0fd10f7f..f05adf85a 100644 --- a/src/ur/stream.cpp +++ b/src/ur/stream.cpp @@ -63,6 +63,8 @@ void URStream::disconnect() if (!_initialized || _stopping) return; + LOG_INFO("Disconnecting from %s:%d", _host.c_str(), _port); + _stopping = true; close(_socket_fd); _initialized = false; From 68e72e393dc90e3932a9b4e73e581dceb95e9e63 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Thu, 16 Feb 2017 22:51:07 +0100 Subject: [PATCH 022/114] RTShared documentation improvement --- include/ur_modern_driver/ur/rt_state.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/include/ur_modern_driver/ur/rt_state.h b/include/ur_modern_driver/ur/rt_state.h index 92cd299cf..55742935d 100644 --- a/include/ur_modern_driver/ur/rt_state.h +++ b/include/ur_modern_driver/ur/rt_state.h @@ -33,6 +33,9 @@ class RTShared { //gap here depending on version double tcp_force[6]; + + //does not contain "_actual" postfix in V11_X but + //they're the same fields so share anyway cartesian_coord_t tool_vector_actual; cartesian_coord_t tcp_speed_actual; From 70cfe6cad35de4ac067c7fb368e8112efd21b291 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Thu, 16 Feb 2017 22:51:44 +0100 Subject: [PATCH 023/114] Improved logging and formatting --- include/ur_modern_driver/ur/producer.h | 20 +++++++------------- 1 file changed, 7 insertions(+), 13 deletions(-) diff --git a/include/ur_modern_driver/ur/producer.h b/include/ur_modern_driver/ur/producer.h index cfbe95ce2..7db11c9f3 100644 --- a/include/ur_modern_driver/ur/producer.h +++ b/include/ur_modern_driver/ur/producer.h @@ -16,18 +16,9 @@ class URProducer : public IProducer { { } - void setup_producer() - { - _stream.connect(); - } - void teardown_producer() - { - _stream.disconnect(); - } - void stop_producer() - { - _stream.disconnect(); - } + void setup_producer() { _stream.connect(); } + void teardown_producer() { _stream.disconnect(); } + void stop_producer() { _stream.disconnect(); } bool try_get(std::vector >& products) { @@ -39,9 +30,12 @@ class URProducer : public IProducer { //LOG_DEBUG("Read %d bytes from stream", len); - if (len < 1) { + if (len == 0) { LOG_WARN("Read nothing from stream"); return false; + } else if (len < 0) { + LOG_WARN("Stream closed"); + return false; } BinParser bp(buf, static_cast(len)); From f8be2834b0f93f52fc51b8455771bb2373ab84d2 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Thu, 16 Feb 2017 22:52:19 +0100 Subject: [PATCH 024/114] RTPublisher main --- src/ros_main.cpp | 20 +------------------- 1 file changed, 1 insertion(+), 19 deletions(-) diff --git a/src/ros_main.cpp b/src/ros_main.cpp index 48cf6b640..adf7a9918 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -47,7 +47,6 @@ bool parse_args(ProgArgs& args) int main(int argc, char** argv) { - ros::init(argc, argv, "ur_driver"); ProgArgs args; @@ -56,28 +55,11 @@ int main(int argc, char** argv) } URFactory factory(args.host); - auto parser = factory.get_rt_parser(); URStream s2(args.host, 30003); URProducer p2(s2, *parser); - - /* - p2.setup_producer(); - - while(true) { - std::vector>> products; - p2.try_get(products); - for(auto const& p : products) { - LOG_INFO("Got packet! %x", p.get()); - } - products.clear(); - } - - p2.teardown_producer(); - */ - - RTPublisher pub(args.prefix, args.base_frame); + RTPublisher pub(args.prefix, args.base_frame, args.tool_frame); Pipeline pl(p2, pub); From ff36a13aea1205596d5ef7be842de2bd53de1437 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Wed, 1 Mar 2017 00:00:03 +0100 Subject: [PATCH 025/114] Fixed RTState parsing bug --- src/ur/rt_state.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/ur/rt_state.cpp b/src/ur/rt_state.cpp index f13aa7305..fe405afaf 100644 --- a/src/ur/rt_state.cpp +++ b/src/ur/rt_state.cpp @@ -20,6 +20,7 @@ bool RTShared::parse_shared2(BinParser& bp) bp.parse(digital_input); bp.parse(motor_temperatures); bp.parse(controller_time); + bp.consume(sizeof(double)); //Unused "Test value" field bp.parse(robot_mode); return true; } From 7d5e14910222370bc8c01c3f3f5ef704aa2aafbd Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Wed, 1 Mar 2017 00:00:55 +0100 Subject: [PATCH 026/114] Fixed strict alisasing warnings and double/float big endian issues --- include/ur_modern_driver/bin_parser.h | 25 ++++++++++++++++--------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/include/ur_modern_driver/bin_parser.h b/include/ur_modern_driver/bin_parser.h index 12d49cbbc..2bc102e17 100644 --- a/include/ur_modern_driver/bin_parser.h +++ b/include/ur_modern_driver/bin_parser.h @@ -66,20 +66,14 @@ class BinParser { { return be64toh(val); } - float decode(float val) - { - return be32toh(val); - } - double decode(double val) - { - return be64toh(val); - } template T peek() { assert(_buf_pos <= _buf_end); - return decode(*(reinterpret_cast(_buf_pos))); + T val; + std::memcpy(&val, _buf_pos, sizeof(T)); + return decode(val); } template @@ -89,6 +83,19 @@ class BinParser { _buf_pos += sizeof(T); } + void parse(double& val) + { + uint64_t inner; + parse(inner); + std::memcpy(&val, &inner, sizeof(double)); + } + void parse(float& val) + { + uint32_t inner; + parse(inner); + std::memcpy(&val, &inner, sizeof(float)); + } + // 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) From 88c582cd58bd3f671660f6ea56bb82144b5a1f87 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Wed, 1 Mar 2017 00:03:44 +0100 Subject: [PATCH 027/114] Fixed log format error --- src/ur/stream.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ur/stream.cpp b/src/ur/stream.cpp index f05adf85a..955d1f3b6 100644 --- a/src/ur/stream.cpp +++ b/src/ur/stream.cpp @@ -112,7 +112,7 @@ ssize_t URStream::receive(uint8_t* buf, size_t buf_len) if (initial) { remainder = be32toh(*(reinterpret_cast(buf))); if (remainder >= (buf_len - sizeof(int32_t))) { - LOG_ERROR("Packet size %d is larger than buffer %d, discarding.", remainder, buf_len); + LOG_ERROR("Packet size %zd is larger than buffer %zu, discarding.", remainder, buf_len); return -1; } initial = false; From d778559894d5a390b51300bc8149a5bc8188494b Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Wed, 1 Mar 2017 12:39:04 +0100 Subject: [PATCH 028/114] Minor pipeline improvements --- include/ur_modern_driver/pipeline.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/include/ur_modern_driver/pipeline.h b/include/ur_modern_driver/pipeline.h index df56421d0..199345af7 100644 --- a/include/ur_modern_driver/pipeline.h +++ b/include/ur_modern_driver/pipeline.h @@ -49,15 +49,15 @@ class Pipeline { for (auto& p : products) { if (!_queue.try_enqueue(std::move(p))) { - LOG_ERROR("Pipeline owerflowed!"); + LOG_ERROR("Pipeline producer owerflowed!"); } } products.clear(); } _producer.teardown_producer(); - //todo cleanup LOG_DEBUG("Pipline producer ended"); + _consumer.stop_consumer(); } void run_consumer() @@ -75,8 +75,8 @@ class Pipeline { break; } _consumer.teardown_consumer(); - //todo cleanup LOG_DEBUG("Pipline consumer ended"); + _producer.stop_producer(); } public: From 71ebb4afbf014582a4ecba2f4c4af8deaf56d061 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Wed, 1 Mar 2017 12:39:25 +0100 Subject: [PATCH 029/114] Added equality operators for vector types --- include/ur_modern_driver/types.h | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/include/ur_modern_driver/types.h b/include/ur_modern_driver/types.h index 8c5144334..86d3a244e 100644 --- a/include/ur_modern_driver/types.h +++ b/include/ur_modern_driver/types.h @@ -2,11 +2,21 @@ #include -typedef struct { +struct double3_t { double x, y, z; -} double3_t; +}; -typedef struct { +struct cartesian_coord_t { double3_t position; double3_t rotation; -} cartesian_coord_t; \ No newline at end of file +}; + +inline bool operator==(const double3_t& lhs, const double3_t& rhs) +{ + return lhs.x == rhs.x && lhs.y == rhs.y && lhs.z == rhs.z; +} + +inline bool operator==(const cartesian_coord_t& lhs, const cartesian_coord_t& rhs) +{ + return lhs.position == rhs.position && lhs.rotation == rhs.rotation; +} \ No newline at end of file From e4bc40fc09988cb49151fc9469719d23fd20dbd4 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Wed, 1 Mar 2017 12:40:55 +0100 Subject: [PATCH 030/114] Fixed minor parsing issues with RTState --- include/ur_modern_driver/ur/rt_state.h | 8 ++++---- src/ur/rt_state.cpp | 9 ++++----- 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/include/ur_modern_driver/ur/rt_state.h b/include/ur_modern_driver/ur/rt_state.h index 55742935d..ea2dd5e01 100644 --- a/include/ur_modern_driver/ur/rt_state.h +++ b/include/ur_modern_driver/ur/rt_state.h @@ -16,8 +16,8 @@ class RTPacket { class RTShared { protected: - bool parse_shared1(BinParser& bp); - bool parse_shared2(BinParser& bp); + void parse_shared1(BinParser& bp); + void parse_shared2(BinParser& bp); public: double time; @@ -34,14 +34,14 @@ class RTShared { double tcp_force[6]; - //does not contain "_actual" postfix in V11_X but + //does not contain "_actual" postfix in V1_X but //they're the same fields so share anyway cartesian_coord_t tool_vector_actual; cartesian_coord_t tcp_speed_actual; //gap here depending on version - uint64_t digital_input; + uint64_t digital_inputs; double motor_temperatures[6]; double controller_time; double robot_mode; diff --git a/src/ur/rt_state.cpp b/src/ur/rt_state.cpp index fe405afaf..226483557 100644 --- a/src/ur/rt_state.cpp +++ b/src/ur/rt_state.cpp @@ -1,7 +1,7 @@ #include "ur_modern_driver/ur/rt_state.h" #include "ur_modern_driver/ur/consumer.h" -bool RTShared::parse_shared1(BinParser& bp) +void RTShared::parse_shared1(BinParser& bp) { bp.parse(time); bp.parse(q_target); @@ -12,17 +12,15 @@ bool RTShared::parse_shared1(BinParser& bp) bp.parse(q_actual); bp.parse(qd_actual); bp.parse(i_actual); - return true; } -bool RTShared::parse_shared2(BinParser& bp) +void RTShared::parse_shared2(BinParser& bp) { - bp.parse(digital_input); + bp.parse(digital_inputs); bp.parse(motor_temperatures); bp.parse(controller_time); bp.consume(sizeof(double)); //Unused "Test value" field bp.parse(robot_mode); - return true; } bool RTState_V1_6__7::parse_with(BinParser& bp) @@ -33,6 +31,7 @@ bool RTState_V1_6__7::parse_with(BinParser& bp) parse_shared1(bp); bp.parse(tool_accelerometer_values); + bp.consume(sizeof(double)*15); bp.parse(tcp_force); bp.parse(tool_vector_actual); bp.parse(tcp_speed_actual); From 474f469e97349a2a71c49f20c271bc4321bf2687 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Wed, 1 Mar 2017 13:59:48 +0100 Subject: [PATCH 031/114] Adopted roscpp code style and naming convention --- .clang-format | 117 +- include/ur_modern_driver/bin_parser.h | 331 ++-- include/ur_modern_driver/packet.h | 5 +- include/ur_modern_driver/parser.h | 5 +- include/ur_modern_driver/pipeline.h | 182 +- include/ur_modern_driver/queue/atomicops.h | 932 ++++++----- .../queue/readerwriterqueue.h | 1331 ++++++++------- include/ur_modern_driver/robot_state.h | 329 ++-- include/ur_modern_driver/robot_state_RT.h | 157 +- include/ur_modern_driver/ros/rt_publisher.h | 86 +- include/ur_modern_driver/types.h | 16 +- include/ur_modern_driver/ur/consumer.h | 61 +- include/ur_modern_driver/ur/factory.h | 133 +- include/ur_modern_driver/ur/master_board.h | 130 +- include/ur_modern_driver/ur/messages.h | 74 +- include/ur_modern_driver/ur/messages_parser.h | 80 +- include/ur_modern_driver/ur/parser.h | 7 +- include/ur_modern_driver/ur/producer.h | 71 +- include/ur_modern_driver/ur/robot_mode.h | 143 +- include/ur_modern_driver/ur/rt_parser.h | 34 +- include/ur_modern_driver/ur/rt_state.h | 169 +- include/ur_modern_driver/ur/state.h | 43 +- include/ur_modern_driver/ur/state_parser.h | 117 +- include/ur_modern_driver/ur/stream.h | 43 +- include/ur_modern_driver/ur_communication.h | 41 +- include/ur_modern_driver/ur_driver.h | 110 +- .../ur_modern_driver/ur_hardware_interface.h | 100 +- .../ur_realtime_communication.h | 59 +- src/do_output.cpp | 24 +- src/robot_state.cpp | 569 ++++--- src/robot_state_RT.cpp | 645 ++++---- src/ros/rt_publisher.cpp | 163 +- src/ros_main.cpp | 74 +- src/ur/master_board.cpp | 122 +- src/ur/messages.cpp | 21 +- src/ur/robot_mode.cpp | 82 +- src/ur/rt_state.cpp | 152 +- src/ur/state.cpp | 28 +- src/ur/stream.cpp | 189 ++- src/ur_communication.cpp | 287 ++-- src/ur_driver.cpp | 602 +++---- src/ur_hardware_interface.cpp | 367 ++-- src/ur_realtime_communication.cpp | 299 ++-- src/ur_ros_wrapper.cpp | 1474 +++++++++-------- 44 files changed, 5105 insertions(+), 4899 deletions(-) diff --git a/.clang-format b/.clang-format index 95f7eb56b..e03f80b84 100644 --- a/.clang-format +++ b/.clang-format @@ -1,95 +1,48 @@ --- -Language: Cpp -# BasedOnStyle: WebKit -AccessModifierOffset: -4 -AlignAfterOpenBracket: DontAlign -AlignConsecutiveAssignments: false -AlignConsecutiveDeclarations: false +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 AlignEscapedNewlinesLeft: false -AlignOperands: false -AlignTrailingComments: false -AllowAllParametersOfDeclarationOnNextLine: true -AllowShortBlocksOnASingleLine: false -AllowShortCaseLabelsOnASingleLine: false -AllowShortFunctionsOnASingleLine: All +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false AllowShortIfStatementsOnASingleLine: false AllowShortLoopsOnASingleLine: false -AlwaysBreakAfterDefinitionReturnType: None -AlwaysBreakAfterReturnType: None +AllowShortFunctionsOnASingleLine: None +AllowShortLoopsOnASingleLine: false +AlwaysBreakTemplateDeclarations: true AlwaysBreakBeforeMultilineStrings: false -AlwaysBreakTemplateDeclarations: false -BinPackArguments: true -BinPackParameters: true -BraceWrapping: - AfterClass: false - AfterControlStatement: false - AfterEnum: false - AfterFunction: true - AfterNamespace: false - AfterObjCDeclaration: false - AfterStruct: false - AfterUnion: false - BeforeCatch: false - BeforeElse: false - IndentBraces: false -BreakBeforeBinaryOperators: All -BreakBeforeBraces: WebKit -BreakBeforeTernaryOperators: true +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false BreakConstructorInitializersBeforeComma: true -BreakAfterJavaFieldAnnotations: false -BreakStringLiterals: true -ColumnLimit: 0 -CommentPragmas: '^ IWYU pragma:' -ConstructorInitializerAllOnOneLineOrOnePerLine: false -ConstructorInitializerIndentWidth: 4 -ContinuationIndentWidth: 4 -Cpp11BracedListStyle: false -DerivePointerAlignment: false -DisableFormat: false +BinPackParameters: true +ColumnLimit: 120 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: true ExperimentalAutoDetectBinPacking: false -ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] -IncludeCategories: - - Regex: '^"(llvm|llvm-c|clang|clang-c)/' - Priority: 2 - - Regex: '^(<|"(gtest|isl|json)/)' - Priority: 3 - - Regex: '.*' - Priority: 1 -IncludeIsMainRegex: '$' -IndentCaseLabels: false -IndentWidth: 4 -IndentWrappedFunctionNames: false -JavaScriptQuotes: Leave -JavaScriptWrapImports: true -KeepEmptyLinesAtTheStartOfBlocks: true -MacroBlockBegin: '' -MacroBlockEnd: '' +IndentCaseLabels: true MaxEmptyLinesToKeep: 1 -NamespaceIndentation: Inner -ObjCBlockIndentWidth: 4 -ObjCSpaceAfterProperty: true +NamespaceIndentation: None ObjCSpaceBeforeProtocolList: true PenaltyBreakBeforeFirstCallParameter: 19 -PenaltyBreakComment: 300 -PenaltyBreakFirstLessLess: 120 -PenaltyBreakString: 1000 -PenaltyExcessCharacter: 1000000 -PenaltyReturnTypeOnItsOwnLine: 60 -PointerAlignment: Left -ReflowComments: true -SortIncludes: true -SpaceAfterCStyleCast: false -SpaceBeforeAssignmentOperators: true -SpaceBeforeParens: ControlStatements -SpaceInEmptyParentheses: false -SpacesBeforeTrailingComments: 1 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +PointerBindsToType: false +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +BreakBeforeBraces: Allman +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false SpacesInAngles: false -SpacesInContainerLiterals: true +SpaceInEmptyParentheses: false SpacesInCStyleCastParentheses: false -SpacesInParentheses: false -SpacesInSquareBrackets: false -Standard: Cpp03 -TabWidth: 8 -UseTab: Never +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +ContinuationIndentWidth: 4 ... - diff --git a/include/ur_modern_driver/bin_parser.h b/include/ur_modern_driver/bin_parser.h index 2bc102e17..a4f9372c1 100644 --- a/include/ur_modern_driver/bin_parser.h +++ b/include/ur_modern_driver/bin_parser.h @@ -1,178 +1,175 @@ #pragma once -#include "ur_modern_driver/log.h" -#include "ur_modern_driver/types.h" #include -#include -#include #include #include +#include +#include #include +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/types.h" -class BinParser { +class BinParser +{ private: - uint8_t *_buf_pos, *_buf_end; - BinParser& _parent; + uint8_t *buf_pos_, *buf_end_; + BinParser& parent_; public: - BinParser(uint8_t* buffer, size_t buf_len) - : _buf_pos(buffer) - , _buf_end(buffer + buf_len) - , _parent(*this) - { - assert(_buf_pos <= _buf_end); - } - - BinParser(BinParser& parent, size_t sub_len) - : _buf_pos(parent._buf_pos) - , _buf_end(parent._buf_pos + sub_len) - , _parent(parent) - { - assert(_buf_pos <= _buf_end); - } - - ~BinParser() - { - _parent._buf_pos = _buf_pos; - } - - //Decode from network encoding (big endian) to host encoding - template - T decode(T val) - { - return val; - } - uint16_t decode(uint16_t val) - { - return be16toh(val); - } - uint32_t decode(uint32_t val) - { - return be32toh(val); - } - uint64_t decode(uint64_t val) - { - return be64toh(val); - } - int16_t decode(int16_t val) - { - return be16toh(val); - } - int32_t decode(int32_t val) - { - return be32toh(val); - } - int64_t decode(int64_t val) - { - return be64toh(val); - } - - template - T peek() - { - assert(_buf_pos <= _buf_end); - T val; - std::memcpy(&val, _buf_pos, sizeof(T)); - return decode(val); - } - - template - void parse(T& val) - { - val = peek(); - _buf_pos += sizeof(T); - } - - void parse(double& val) - { - uint64_t inner; - parse(inner); - std::memcpy(&val, &inner, sizeof(double)); - } - void parse(float& val) - { - uint32_t inner; - parse(inner); - std::memcpy(&val, &inner, sizeof(float)); - } - - // 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) - { - uint8_t inner; - parse(inner); - val = inner != 0; - } - - // Explicit parsing order of fields to avoid issues with struct layout - 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) - { - parse(val.position); - parse(val.rotation); - } - - void parse_remainder(std::string& val) - { - parse(val, size_t(_buf_end - _buf_pos)); - } - - void parse(std::string& val, size_t len) - { - val.assign(reinterpret_cast(_buf_pos), len); - _buf_pos += len; - } - - // Special string parse function that assumes uint8_t len followed by chars - void parse(std::string& val) - { - uint8_t len; - parse(len); - parse(val, size_t(len)); - } - - template - void parse(T (&array)[N]) - { - for (size_t i = 0; i < N; i++) { - parse(array[i]); - } - } - - void consume() - { - _buf_pos = _buf_end; - } - void consume(size_t bytes) - { - _buf_pos += bytes; - } - - bool check_size(size_t bytes) - { - return bytes <= size_t(_buf_end - _buf_pos); - } - template - bool check_size(void) - { - return check_size(T::SIZE); - } - - bool empty() - { - return _buf_pos == _buf_end; - } - - void debug() - { - LOG_DEBUG("BinParser: %p - %p (%zu bytes)", _buf_pos, _buf_end, _buf_end - _buf_pos); - } + BinParser(uint8_t* buffer, size_t buf_len) : buf_pos_(buffer), buf_end_(buffer + buf_len), parent_(*this) + { + assert(buf_pos_ <= buf_end_); + } + + BinParser(BinParser& parent, size_t sub_len) + : buf_pos_(parent.buf_pos_), buf_end_(parent.buf_pos_ + sub_len), parent_(parent) + { + assert(buf_pos_ <= buf_end_); + } + + ~BinParser() + { + parent_.buf_pos_ = buf_pos_; + } + + // Decode from network encoding (big endian) to host encoding + template + T decode(T val) + { + return val; + } + uint16_t decode(uint16_t val) + { + return be16toh(val); + } + uint32_t decode(uint32_t val) + { + return be32toh(val); + } + uint64_t decode(uint64_t val) + { + return be64toh(val); + } + int16_t decode(int16_t val) + { + return be16toh(val); + } + int32_t decode(int32_t val) + { + return be32toh(val); + } + int64_t decode(int64_t val) + { + return be64toh(val); + } + + template + T peek() + { + assert(buf_pos_ <= buf_end_); + T val; + std::memcpy(&val, buf_pos_, sizeof(T)); + return decode(val); + } + + template + void parse(T& val) + { + val = peek(); + buf_pos_ += sizeof(T); + } + + void parse(double& val) + { + uint64_t inner; + parse(inner); + std::memcpy(&val, &inner, sizeof(double)); + } + void parse(float& val) + { + uint32_t inner; + parse(inner); + std::memcpy(&val, &inner, sizeof(float)); + } + + // 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) + { + uint8_t inner; + parse(inner); + val = inner != 0; + } + + // Explicit parsing order of fields to avoid issues with struct layout + 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) + { + parse(val.position); + parse(val.rotation); + } + + void parse_remainder(std::string& val) + { + parse(val, size_t(buf_end_ - buf_pos_)); + } + + void parse(std::string& val, size_t len) + { + val.assign(reinterpret_cast(buf_pos_), len); + buf_pos_ += len; + } + + // Special string parse function that assumes uint8_t len followed by chars + void parse(std::string& val) + { + uint8_t len; + parse(len); + parse(val, size_t(len)); + } + + template + void parse(T (&array)[N]) + { + for (size_t i = 0; i < N; i++) + { + parse(array[i]); + } + } + + void consume() + { + buf_pos_ = buf_end_; + } + void consume(size_t bytes) + { + buf_pos_ += bytes; + } + + bool checkSize(size_t bytes) + { + return bytes <= size_t(buf_end_ - buf_pos_); + } + template + bool checkSize(void) + { + return checkSize(T::SIZE); + } + + bool empty() + { + return buf_pos_ == buf_end_; + } + + void debug() + { + LOG_DEBUG("BinParser: %p - %p (%zu bytes)", buf_pos_, buf_end_, buf_end_ - buf_pos_); + } }; diff --git a/include/ur_modern_driver/packet.h b/include/ur_modern_driver/packet.h index 541e3e5ae..521d75bf1 100644 --- a/include/ur_modern_driver/packet.h +++ b/include/ur_modern_driver/packet.h @@ -1,7 +1,8 @@ #pragma once #include "ur_modern_driver/bin_parser.h" -class Packet { +class Packet +{ public: - virtual bool parse_with(BinParser& bp) = 0; + virtual bool parseWith(BinParser& bp) = 0; }; \ No newline at end of file diff --git a/include/ur_modern_driver/parser.h b/include/ur_modern_driver/parser.h index 99975156b..e824c57f9 100644 --- a/include/ur_modern_driver/parser.h +++ b/include/ur_modern_driver/parser.h @@ -2,7 +2,8 @@ #include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/packet.h" -class Parser { +class Parser +{ public: - virtual std::unique_ptr parse(BinParser& bp) = 0; + virtual std::unique_ptr parse(BinParser& bp) = 0; }; diff --git a/include/ur_modern_driver/pipeline.h b/include/ur_modern_driver/pipeline.h index 199345af7..49b1c9eca 100644 --- a/include/ur_modern_driver/pipeline.h +++ b/include/ur_modern_driver/pipeline.h @@ -1,116 +1,134 @@ #pragma once -#include "ur_modern_driver/log.h" -#include "ur_modern_driver/queue/readerwriterqueue.h" #include #include #include +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/queue/readerwriterqueue.h" using namespace moodycamel; using namespace std; template -class IConsumer { +class IConsumer +{ public: - virtual void setup_consumer() {} - virtual void teardown_consumer() {} - virtual void stop_consumer() {} - - virtual bool consume(unique_ptr product) = 0; + virtual void setupConsumer() + { + } + virtual void teardownConsumer() + { + } + virtual void stopConsumer() + { + } + + virtual bool consume(unique_ptr product) = 0; }; template -class IProducer { +class IProducer +{ public: - virtual void setup_producer() {} - virtual void teardown_producer() {} - virtual void stop_producer() {} - - virtual bool try_get(std::vector >& products) = 0; + virtual void setupProducer() + { + } + virtual void teardownProducer() + { + } + virtual void stopProducer() + { + } + + virtual bool tryGet(std::vector>& products) = 0; }; template -class Pipeline { +class Pipeline +{ private: - IProducer& _producer; - IConsumer& _consumer; - BlockingReaderWriterQueue > _queue; - atomic _running; - thread _pThread, _cThread; - - void run_producer() + IProducer& producer_; + IConsumer& consumer_; + BlockingReaderWriterQueue> queue_; + atomic running_; + thread pThread_, cThread_; + + void run_producer() + { + producer_.setupProducer(); + std::vector> products; + while (running_) { - _producer.setup_producer(); - std::vector > products; - while (_running) { - if (!_producer.try_get(products)) { - break; - } - - for (auto& p : products) { - if (!_queue.try_enqueue(std::move(p))) { - LOG_ERROR("Pipeline producer owerflowed!"); - } - } - - products.clear(); + if (!producer_.tryGet(products)) + { + break; + } + + for (auto& p : products) + { + if (!queue_.try_enqueue(std::move(p))) + { + LOG_ERROR("Pipeline producer owerflowed!"); } - _producer.teardown_producer(); - LOG_DEBUG("Pipline producer ended"); - _consumer.stop_consumer(); - } + } - void run_consumer() + products.clear(); + } + producer_.teardownProducer(); + LOG_DEBUG("Pipline producer ended"); + consumer_.stopConsumer(); + } + + void run_consumer() + { + consumer_.setupConsumer(); + unique_ptr product; + while (running_) { - _consumer.setup_consumer(); - unique_ptr product; - while (_running) { - // 16000us timeout was chosen because we should - // roughly recieve messages at 125hz which is every - // 8ms == 8000us and double it for some error margin - if (!_queue.wait_dequeue_timed(product, 16000)) { - continue; - } - if (!_consumer.consume(std::move(product))) - break; - } - _consumer.teardown_consumer(); - LOG_DEBUG("Pipline consumer ended"); - _producer.stop_producer(); + // 16000us timeout was chosen because we should + // roughly recieve messages at 125hz which is every + // 8ms == 8000us and double it for some error margin + if (!queue_.wait_dequeue_timed(product, 16000)) + { + continue; + } + if (!consumer_.consume(std::move(product))) + break; } + consumer_.teardownConsumer(); + LOG_DEBUG("Pipline consumer ended"); + producer_.stopProducer(); + } public: - Pipeline(IProducer& producer, IConsumer& consumer) - : _producer(producer) - , _consumer(consumer) - , _queue{ 32 } - , _running{ false } - { - } + Pipeline(IProducer& producer, IConsumer& consumer) + : producer_(producer), consumer_(consumer), queue_{ 32 }, running_{ false } + { + } - void run() - { - if (_running) - return; + void run() + { + if (running_) + return; - _running = true; - _pThread = thread(&Pipeline::run_producer, this); - _cThread = thread(&Pipeline::run_consumer, this); - } + running_ = true; + pThread_ = thread(&Pipeline::run_producer, this); + cThread_ = thread(&Pipeline::run_consumer, this); + } - void stop() - { - if (!_running) - return; + void stop() + { + if (!running_) + return; - LOG_DEBUG("Stopping pipeline"); + LOG_DEBUG("Stopping pipeline"); - _consumer.stop_consumer(); - _producer.stop_producer(); + consumer_.stopConsumer(); + producer_.stopProducer(); - _running = false; + running_ = false; - _pThread.join(); - _cThread.join(); - } + pThread_.join(); + cThread_.join(); + } }; \ No newline at end of file diff --git a/include/ur_modern_driver/queue/atomicops.h b/include/ur_modern_driver/queue/atomicops.h index af0089e43..8cbf463bd 100644 --- a/include/ur_modern_driver/queue/atomicops.h +++ b/include/ur_modern_driver/queue/atomicops.h @@ -63,21 +63,22 @@ // Portable atomic fences implemented below: -namespace moodycamel { - -enum memory_order { - memory_order_relaxed, - memory_order_acquire, - memory_order_release, - memory_order_acq_rel, - memory_order_seq_cst, - - // memory_order_sync: Forces a full sync: - // #LoadLoad, #LoadStore, #StoreStore, and most significantly, #StoreLoad - memory_order_sync = memory_order_seq_cst +namespace moodycamel +{ +enum memory_order +{ + memory_order_relaxed, + memory_order_acquire, + memory_order_release, + memory_order_acq_rel, + memory_order_seq_cst, + + // memory_order_sync: Forces a full sync: + // #LoadLoad, #LoadStore, #StoreStore, and most significantly, #StoreLoad + memory_order_sync = memory_order_seq_cst }; -} // end namespace moodycamel +} // end namespace moodycamel #if (defined(AE_VCPP) && (_MSC_VER < 1700 || defined(__cplusplus_cli))) || defined(AE_ICC) // VS2010 and ICC13 don't support std::atomic_*_fence, implement our own fences @@ -98,34 +99,36 @@ enum memory_order { #ifdef AE_VCPP #pragma warning(push) -#pragma warning(disable : 4365) // Disable erroneous 'conversion from long to unsigned int, signed/unsigned mismatch' error when using `assert` +#pragma warning(disable : 4365) // Disable erroneous 'conversion from long to unsigned int, signed/unsigned mismatch' + // error when using `assert` #ifdef __cplusplus_cli #pragma managed(push, off) #endif #endif -namespace moodycamel { - +namespace moodycamel +{ AE_FORCEINLINE void compiler_fence(memory_order order) { - switch (order) { + switch (order) + { case memory_order_relaxed: - break; + break; case memory_order_acquire: - _ReadBarrier(); - break; + _ReadBarrier(); + break; case memory_order_release: - _WriteBarrier(); - break; + _WriteBarrier(); + break; case memory_order_acq_rel: - _ReadWriteBarrier(); - break; + _ReadWriteBarrier(); + break; case memory_order_seq_cst: - _ReadWriteBarrier(); - break; + _ReadWriteBarrier(); + break; default: - assert(false); - } + assert(false); + } } // x86/x64 have a strong memory model -- all loads and stores have @@ -134,111 +137,115 @@ AE_FORCEINLINE void compiler_fence(memory_order order) #if defined(AE_ARCH_X86) || defined(AE_ARCH_X64) AE_FORCEINLINE void fence(memory_order order) { - switch (order) { + switch (order) + { case memory_order_relaxed: - break; + break; case memory_order_acquire: - _ReadBarrier(); - break; + _ReadBarrier(); + break; case memory_order_release: - _WriteBarrier(); - break; + _WriteBarrier(); + break; case memory_order_acq_rel: - _ReadWriteBarrier(); - break; + _ReadWriteBarrier(); + break; case memory_order_seq_cst: - _ReadWriteBarrier(); - AeFullSync(); - _ReadWriteBarrier(); - break; + _ReadWriteBarrier(); + AeFullSync(); + _ReadWriteBarrier(); + break; default: - assert(false); - } + assert(false); + } } #else AE_FORCEINLINE void fence(memory_order order) { - // Non-specialized arch, use heavier memory barriers everywhere just in case :-( - switch (order) { + // Non-specialized arch, use heavier memory barriers everywhere just in case :-( + switch (order) + { case memory_order_relaxed: - break; + break; case memory_order_acquire: - _ReadBarrier(); - AeLiteSync(); - _ReadBarrier(); - break; + _ReadBarrier(); + AeLiteSync(); + _ReadBarrier(); + break; case memory_order_release: - _WriteBarrier(); - AeLiteSync(); - _WriteBarrier(); - break; + _WriteBarrier(); + AeLiteSync(); + _WriteBarrier(); + break; case memory_order_acq_rel: - _ReadWriteBarrier(); - AeLiteSync(); - _ReadWriteBarrier(); - break; + _ReadWriteBarrier(); + AeLiteSync(); + _ReadWriteBarrier(); + break; case memory_order_seq_cst: - _ReadWriteBarrier(); - AeFullSync(); - _ReadWriteBarrier(); - break; + _ReadWriteBarrier(); + AeFullSync(); + _ReadWriteBarrier(); + break; default: - assert(false); - } + assert(false); + } } #endif -} // end namespace moodycamel +} // end namespace moodycamel #else // Use standard library of atomics #include -namespace moodycamel { - +namespace moodycamel +{ AE_FORCEINLINE void compiler_fence(memory_order order) { - switch (order) { + switch (order) + { case memory_order_relaxed: - break; + break; case memory_order_acquire: - std::atomic_signal_fence(std::memory_order_acquire); - break; + std::atomic_signal_fence(std::memory_order_acquire); + break; case memory_order_release: - std::atomic_signal_fence(std::memory_order_release); - break; + std::atomic_signal_fence(std::memory_order_release); + break; case memory_order_acq_rel: - std::atomic_signal_fence(std::memory_order_acq_rel); - break; + 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; + std::atomic_signal_fence(std::memory_order_seq_cst); + break; default: - assert(false); - } + assert(false); + } } AE_FORCEINLINE void fence(memory_order order) { - switch (order) { + switch (order) + { case memory_order_relaxed: - break; + break; case memory_order_acquire: - std::atomic_thread_fence(std::memory_order_acquire); - break; + std::atomic_thread_fence(std::memory_order_acquire); + break; case memory_order_release: - std::atomic_thread_fence(std::memory_order_release); - break; + std::atomic_thread_fence(std::memory_order_release); + break; case memory_order_acq_rel: - std::atomic_thread_fence(std::memory_order_acq_rel); - break; + 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; + std::atomic_thread_fence(std::memory_order_seq_cst); + break; default: - assert(false); - } + assert(false); + } } -} // end namespace moodycamel +} // end namespace moodycamel #endif @@ -255,127 +262,133 @@ AE_FORCEINLINE void fence(memory_order order) // Provides basic support for atomic variables -- no memory ordering guarantees are provided. // The guarantee of atomicity is only made for types that already have atomic load and store guarantees // at the hardware level -- on most platforms this generally means aligned pointers and integers (only). -namespace moodycamel { +namespace moodycamel +{ template -class weak_atomic { +class weak_atomic +{ public: - weak_atomic() {} + weak_atomic() + { + } #ifdef AE_VCPP -#pragma warning(disable : 4100) // Get rid of (erroneous) 'unreferenced formal parameter' warning +#pragma warning(disable : 4100) // Get rid of (erroneous) 'unreferenced formal parameter' warning #endif - template - weak_atomic(U&& x) - : value(std::forward(x)) - { - } + template + weak_atomic(U&& x) : value(std::forward(x)) + { + } #ifdef __cplusplus_cli - // Work around bug with universal reference/nullptr combination that only appears when /clr is on - weak_atomic(nullptr_t) - : value(nullptr) - { - } + // Work around bug with universal reference/nullptr combination that only appears when /clr is on + weak_atomic(nullptr_t) : value(nullptr) + { + } #endif - weak_atomic(weak_atomic const& other) - : value(other.value) - { - } - weak_atomic(weak_atomic&& other) - : value(std::move(other.value)) - { - } + weak_atomic(weak_atomic const& other) : value(other.value) + { + } + weak_atomic(weak_atomic&& other) : value(std::move(other.value)) + { + } #ifdef AE_VCPP #pragma warning(default : 4100) #endif - AE_FORCEINLINE operator T() const - { - return load(); - } + AE_FORCEINLINE operator T() const + { + return load(); + } #ifndef AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC - template - AE_FORCEINLINE weak_atomic const& operator=(U&& x) - { - value = std::forward(x); - return *this; - } - AE_FORCEINLINE weak_atomic const& operator=(weak_atomic const& other) - { - value = other.value; - return *this; - } - - AE_FORCEINLINE T load() const { return value; } - - AE_FORCEINLINE T fetch_add_acquire(T increment) - { + template + AE_FORCEINLINE weak_atomic const& operator=(U&& x) + { + value = std::forward(x); + return *this; + } + AE_FORCEINLINE weak_atomic const& operator=(weak_atomic const& other) + { + value = other.value; + return *this; + } + + AE_FORCEINLINE T load() const + { + return value; + } + + AE_FORCEINLINE T fetch_add_acquire(T increment) + { #if defined(AE_ARCH_X64) || defined(AE_ARCH_X86) - if (sizeof(T) == 4) - return _InterlockedExchangeAdd((long volatile*)&value, (long)increment); + if (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 #endif - assert(false && "T must be either a 32 or 64 bit type"); - return value; - } + assert(false && "T must be either a 32 or 64 bit type"); + return value; + } - AE_FORCEINLINE T fetch_add_release(T increment) - { + AE_FORCEINLINE T fetch_add_release(T increment) + { #if defined(AE_ARCH_X64) || defined(AE_ARCH_X86) - if (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 #endif - assert(false && "T must be either a 32 or 64 bit type"); - return value; - } + assert(false && "T must be either a 32 or 64 bit type"); + return value; + } #else - template - AE_FORCEINLINE weak_atomic const& operator=(U&& x) - { - value.store(std::forward(x), std::memory_order_relaxed); - return *this; - } - - AE_FORCEINLINE weak_atomic const& operator=(weak_atomic const& other) - { - value.store(other.value.load(std::memory_order_relaxed), std::memory_order_relaxed); - return *this; - } - - AE_FORCEINLINE T load() const { return value.load(std::memory_order_relaxed); } - - AE_FORCEINLINE T fetch_add_acquire(T increment) - { - return value.fetch_add(increment, std::memory_order_acquire); - } - - AE_FORCEINLINE T fetch_add_release(T increment) - { - return value.fetch_add(increment, std::memory_order_release); - } + template + AE_FORCEINLINE weak_atomic const& operator=(U&& x) + { + value.store(std::forward(x), std::memory_order_relaxed); + return *this; + } + + AE_FORCEINLINE weak_atomic const& operator=(weak_atomic const& other) + { + value.store(other.value.load(std::memory_order_relaxed), std::memory_order_relaxed); + return *this; + } + + AE_FORCEINLINE T load() const + { + return value.load(std::memory_order_relaxed); + } + + AE_FORCEINLINE T fetch_add_acquire(T increment) + { + return value.fetch_add(increment, std::memory_order_acquire); + } + + AE_FORCEINLINE T fetch_add_release(T increment) + { + return value.fetch_add(increment, std::memory_order_release); + } #endif private: #ifndef AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC - // No std::atomic support, but still need to circumvent compiler optimizations. - // `volatile` will make memory access slow, but is guaranteed to be reliable. - volatile T value; + // No std::atomic support, but still need to circumvent compiler optimizations. + // `volatile` will make memory access slow, but is guaranteed to be reliable. + volatile T value; #else - std::atomic value; + std::atomic value; #endif }; -} // end namespace moodycamel +} // end namespace moodycamel // Portable single-producer, single-consumer semaphore below: @@ -387,7 +400,8 @@ class weak_atomic { // namespace with thousands of generic names or adding a .cpp for nothing. extern "C" { struct _SECURITY_ATTRIBUTES; -__declspec(dllimport) void* __stdcall CreateSemaphoreW(_SECURITY_ATTRIBUTES* lpSemaphoreAttributes, long lInitialCount, long lMaximumCount, const wchar_t* lpName); +__declspec(dllimport) void* __stdcall CreateSemaphoreW(_SECURITY_ATTRIBUTES* lpSemaphoreAttributes, long lInitialCount, + long lMaximumCount, const wchar_t* lpName); __declspec(dllimport) int __stdcall CloseHandle(void* hObject); __declspec(dllimport) unsigned long __stdcall WaitForSingleObject(void* hHandle, unsigned long dwMilliseconds); __declspec(dllimport) int __stdcall ReleaseSemaphore(void* hSemaphore, long lReleaseCount, long* lpPreviousCount); @@ -398,7 +412,8 @@ __declspec(dllimport) int __stdcall ReleaseSemaphore(void* hSemaphore, long lRel #include #endif -namespace moodycamel { +namespace moodycamel +{ // Code in the spsc_sema namespace below is an adaptation of Jeff Preshing's // portable + lightweight semaphore implementations, originally from // https://github.com/preshing/cpp11-on-multicore/blob/master/common/sema.h @@ -420,283 +435,300 @@ namespace moodycamel { // 2. Altered source versions must be plainly marked as such, and must not be // misrepresented as being the original software. // 3. This notice may not be removed or altered from any source distribution. -namespace spsc_sema { +namespace spsc_sema +{ #if defined(_WIN32) - class Semaphore { - private: - void* m_hSema; - - Semaphore(const Semaphore& other); - Semaphore& operator=(const Semaphore& other); - - public: - Semaphore(int initialCount = 0) - { - assert(initialCount >= 0); - const long maxLong = 0x7fffffff; - m_hSema = CreateSemaphoreW(nullptr, initialCount, maxLong, nullptr); - } - - ~Semaphore() - { - CloseHandle(m_hSema); - } - - void wait() - { - const unsigned long infinite = 0xffffffff; - WaitForSingleObject(m_hSema, infinite); - } - - bool try_wait() - { - const unsigned long RC_WAIT_TIMEOUT = 0x00000102; - return WaitForSingleObject(m_hSema, 0) != RC_WAIT_TIMEOUT; - } - - bool timed_wait(std::uint64_t usecs) - { - const unsigned long RC_WAIT_TIMEOUT = 0x00000102; - return WaitForSingleObject(m_hSema, (unsigned long)(usecs / 1000)) != RC_WAIT_TIMEOUT; - } - - void signal(int count = 1) - { - ReleaseSemaphore(m_hSema, count, nullptr); - } - }; +class Semaphore +{ +private: + void* m_hSema; + + Semaphore(const Semaphore& other); + Semaphore& operator=(const Semaphore& other); + +public: + Semaphore(int initialCount = 0) + { + assert(initialCount >= 0); + const long maxLong = 0x7fffffff; + m_hSema = CreateSemaphoreW(nullptr, initialCount, maxLong, nullptr); + } + + ~Semaphore() + { + CloseHandle(m_hSema); + } + + void wait() + { + const unsigned long infinite = 0xffffffff; + WaitForSingleObject(m_hSema, infinite); + } + + bool try_wait() + { + const unsigned long RC_WAIT_TIMEOUT = 0x00000102; + return WaitForSingleObject(m_hSema, 0) != RC_WAIT_TIMEOUT; + } + + bool timed_wait(std::uint64_t usecs) + { + const unsigned long RC_WAIT_TIMEOUT = 0x00000102; + return WaitForSingleObject(m_hSema, (unsigned long)(usecs / 1000)) != RC_WAIT_TIMEOUT; + } + + void signal(int count = 1) + { + ReleaseSemaphore(m_hSema, count, nullptr); + } +}; #elif defined(__MACH__) - //--------------------------------------------------------- - // Semaphore (Apple iOS and OSX) - // Can't use POSIX semaphores due to http://lists.apple.com/archives/darwin-kernel/2009/Apr/msg00010.html - //--------------------------------------------------------- - class Semaphore { - private: - semaphore_t m_sema; - - Semaphore(const Semaphore& other); - Semaphore& operator=(const Semaphore& other); - - public: - Semaphore(int initialCount = 0) - { - assert(initialCount >= 0); - semaphore_create(mach_task_self(), &m_sema, SYNC_POLICY_FIFO, initialCount); - } - - ~Semaphore() - { - semaphore_destroy(mach_task_self(), m_sema); - } - - void wait() - { - semaphore_wait(m_sema); - } - - bool try_wait() - { - return timed_wait(0); - } - - bool timed_wait(std::int64_t timeout_usecs) - { - mach_timespec_t ts; - ts.tv_sec = timeout_usecs / 1000000; - ts.tv_nsec = (timeout_usecs % 1000000) * 1000; - - // added in OSX 10.10: https://developer.apple.com/library/prerelease/mac/documentation/General/Reference/APIDiffsMacOSX10_10SeedDiff/modules/Darwin.html - kern_return_t rc = semaphore_timedwait(m_sema, ts); - - return rc != KERN_OPERATION_TIMED_OUT; - } - - void signal() - { - semaphore_signal(m_sema); - } - - void signal(int count) - { - while (count-- > 0) { - semaphore_signal(m_sema); - } - } - }; +//--------------------------------------------------------- +// Semaphore (Apple iOS and OSX) +// Can't use POSIX semaphores due to http://lists.apple.com/archives/darwin-kernel/2009/Apr/msg00010.html +//--------------------------------------------------------- +class Semaphore +{ +private: + semaphore_t m_sema; + + Semaphore(const Semaphore& other); + Semaphore& operator=(const Semaphore& other); + +public: + Semaphore(int initialCount = 0) + { + assert(initialCount >= 0); + semaphore_create(mach_task_self(), &m_sema, SYNC_POLICY_FIFO, initialCount); + } + + ~Semaphore() + { + semaphore_destroy(mach_task_self(), m_sema); + } + + void wait() + { + semaphore_wait(m_sema); + } + + bool try_wait() + { + return timed_wait(0); + } + + bool timed_wait(std::int64_t timeout_usecs) + { + mach_timespec_t ts; + ts.tv_sec = timeout_usecs / 1000000; + ts.tv_nsec = (timeout_usecs % 1000000) * 1000; + + // added in OSX 10.10: + // https://developer.apple.com/library/prerelease/mac/documentation/General/Reference/APIDiffsMacOSX10_10SeedDiff/modules/Darwin.html + kern_return_t rc = semaphore_timedwait(m_sema, ts); + + return rc != KERN_OPERATION_TIMED_OUT; + } + + void signal() + { + semaphore_signal(m_sema); + } + + void signal(int count) + { + while (count-- > 0) + { + semaphore_signal(m_sema); + } + } +}; #elif defined(__unix__) - //--------------------------------------------------------- - // Semaphore (POSIX, Linux) - //--------------------------------------------------------- - class Semaphore { - private: - sem_t m_sema; - - Semaphore(const Semaphore& other); - Semaphore& operator=(const Semaphore& other); - - public: - Semaphore(int initialCount = 0) - { - assert(initialCount >= 0); - sem_init(&m_sema, 0, initialCount); - } - - ~Semaphore() - { - sem_destroy(&m_sema); - } - - void wait() - { - // http://stackoverflow.com/questions/2013181/gdb-causes-sem-wait-to-fail-with-eintr-error - int rc; - do { - rc = sem_wait(&m_sema); - } while (rc == -1 && errno == EINTR); - } - - bool try_wait() - { - int rc; - do { - rc = sem_trywait(&m_sema); - } while (rc == -1 && errno == EINTR); - return !(rc == -1 && errno == EAGAIN); - } - - bool timed_wait(std::uint64_t usecs) - { - struct timespec ts; - const int usecs_in_1_sec = 1000000; - const int nsecs_in_1_sec = 1000000000; - clock_gettime(CLOCK_REALTIME, &ts); - ts.tv_sec += usecs / usecs_in_1_sec; - ts.tv_nsec += (usecs % usecs_in_1_sec) * 1000; - // sem_timedwait bombs if you have more than 1e9 in tv_nsec - // so we have to clean things up before passing it in - if (ts.tv_nsec > nsecs_in_1_sec) { - ts.tv_nsec -= nsecs_in_1_sec; - ++ts.tv_sec; - } - - int rc; - do { - rc = sem_timedwait(&m_sema, &ts); - } while (rc == -1 && errno == EINTR); - return !(rc == -1 && errno == ETIMEDOUT); - } - - void signal() - { - sem_post(&m_sema); - } - - void signal(int count) - { - while (count-- > 0) { - sem_post(&m_sema); - } - } - }; +//--------------------------------------------------------- +// Semaphore (POSIX, Linux) +//--------------------------------------------------------- +class Semaphore +{ +private: + sem_t m_sema; + + Semaphore(const Semaphore& other); + Semaphore& operator=(const Semaphore& other); + +public: + Semaphore(int initialCount = 0) + { + assert(initialCount >= 0); + sem_init(&m_sema, 0, initialCount); + } + + ~Semaphore() + { + sem_destroy(&m_sema); + } + + void wait() + { + // http://stackoverflow.com/questions/2013181/gdb-causes-sem-wait-to-fail-with-eintr-error + int rc; + do + { + rc = sem_wait(&m_sema); + } while (rc == -1 && errno == EINTR); + } + + bool try_wait() + { + int rc; + do + { + rc = sem_trywait(&m_sema); + } while (rc == -1 && errno == EINTR); + return !(rc == -1 && errno == EAGAIN); + } + + bool timed_wait(std::uint64_t usecs) + { + struct timespec ts; + const int usecs_in_1_sec = 1000000; + const int nsecs_in_1_sec = 1000000000; + clock_gettime(CLOCK_REALTIME, &ts); + ts.tv_sec += usecs / usecs_in_1_sec; + ts.tv_nsec += (usecs % usecs_in_1_sec) * 1000; + // sem_timedwait bombs if you have more than 1e9 in tv_nsec + // so we have to clean things up before passing it in + if (ts.tv_nsec > nsecs_in_1_sec) + { + ts.tv_nsec -= nsecs_in_1_sec; + ++ts.tv_sec; + } + + int rc; + do + { + rc = sem_timedwait(&m_sema, &ts); + } while (rc == -1 && errno == EINTR); + return !(rc == -1 && errno == ETIMEDOUT); + } + + void signal() + { + sem_post(&m_sema); + } + + void signal(int count) + { + while (count-- > 0) + { + sem_post(&m_sema); + } + } +}; #else #error Unsupported platform! (No semaphore wrapper available) #endif - //--------------------------------------------------------- - // LightweightSemaphore - //--------------------------------------------------------- - class LightweightSemaphore { - public: - typedef std::make_signed::type ssize_t; - - private: - weak_atomic m_count; - Semaphore m_sema; - - bool waitWithPartialSpinning(std::int64_t timeout_usecs = -1) - { - ssize_t oldCount; - // Is there a better way to set the initial spin count? - // If we lower it to 1000, testBenaphore becomes 15x slower on my Core i7-5930K Windows PC, - // as threads start hitting the kernel semaphore. - int spin = 10000; - while (--spin >= 0) { - if (m_count.load() > 0) { - m_count.fetch_add_acquire(-1); - return true; - } - compiler_fence(memory_order_acquire); // Prevent the compiler from collapsing the loop. - } - oldCount = m_count.fetch_add_acquire(-1); - if (oldCount > 0) - return true; - if (timeout_usecs < 0) { - m_sema.wait(); - return true; - } - if (m_sema.timed_wait(timeout_usecs)) - return true; - // At this point, we've timed out waiting for the semaphore, but the - // count is still decremented indicating we may still be waiting on - // it. So we have to re-adjust the count, but only if the semaphore - // wasn't signaled enough times for us too since then. If it was, we - // need to release the semaphore too. - while (true) { - oldCount = m_count.fetch_add_release(1); - if (oldCount < 0) - return false; // successfully restored things to the way they were - // Oh, the producer thread just signaled the semaphore after all. Try again: - oldCount = m_count.fetch_add_acquire(-1); - if (oldCount > 0 && m_sema.try_wait()) - return true; - } - } - - public: - LightweightSemaphore(ssize_t initialCount = 0) - : m_count(initialCount) - { - assert(initialCount >= 0); - } - - bool tryWait() - { - if (m_count.load() > 0) { - m_count.fetch_add_acquire(-1); - return true; - } - return false; - } - - void wait() - { - if (!tryWait()) - waitWithPartialSpinning(); - } - - bool wait(std::int64_t timeout_usecs) - { - return tryWait() || waitWithPartialSpinning(timeout_usecs); - } - - void signal(ssize_t count = 1) - { - assert(count >= 0); - ssize_t oldCount = m_count.fetch_add_release(count); - assert(oldCount >= -1); - if (oldCount < 0) { - m_sema.signal(1); - } - } - - ssize_t availableApprox() const - { - ssize_t count = m_count.load(); - return count > 0 ? count : 0; - } - }; -} // end namespace spsc_sema -} // end namespace moodycamel +//--------------------------------------------------------- +// LightweightSemaphore +//--------------------------------------------------------- +class LightweightSemaphore +{ +public: + typedef std::make_signed::type ssize_t; + +private: + weak_atomic m_count; + Semaphore m_sema; + + bool waitWithPartialSpinning(std::int64_t timeout_usecs = -1) + { + ssize_t oldCount; + // Is there a better way to set the initial spin count? + // If we lower it to 1000, testBenaphore becomes 15x slower on my Core i7-5930K Windows PC, + // as threads start hitting the kernel semaphore. + int spin = 10000; + while (--spin >= 0) + { + if (m_count.load() > 0) + { + m_count.fetch_add_acquire(-1); + return true; + } + compiler_fence(memory_order_acquire); // Prevent the compiler from collapsing the loop. + } + oldCount = m_count.fetch_add_acquire(-1); + if (oldCount > 0) + return true; + if (timeout_usecs < 0) + { + m_sema.wait(); + return true; + } + if (m_sema.timed_wait(timeout_usecs)) + return true; + // At this point, we've timed out waiting for the semaphore, but the + // count is still decremented indicating we may still be waiting on + // it. So we have to re-adjust the count, but only if the semaphore + // wasn't signaled enough times for us too since then. If it was, we + // need to release the semaphore too. + while (true) + { + oldCount = m_count.fetch_add_release(1); + if (oldCount < 0) + return false; // successfully restored things to the way they were + // Oh, the producer thread just signaled the semaphore after all. Try again: + oldCount = m_count.fetch_add_acquire(-1); + if (oldCount > 0 && m_sema.try_wait()) + return true; + } + } + +public: + LightweightSemaphore(ssize_t initialCount = 0) : m_count(initialCount) + { + assert(initialCount >= 0); + } + + bool tryWait() + { + if (m_count.load() > 0) + { + m_count.fetch_add_acquire(-1); + return true; + } + return false; + } + + void wait() + { + if (!tryWait()) + waitWithPartialSpinning(); + } + + bool wait(std::int64_t timeout_usecs) + { + return tryWait() || waitWithPartialSpinning(timeout_usecs); + } + + void signal(ssize_t count = 1) + { + assert(count >= 0); + ssize_t oldCount = m_count.fetch_add_release(count); + assert(oldCount >= -1); + if (oldCount < 0) + { + m_sema.signal(1); + } + } + + ssize_t availableApprox() const + { + ssize_t count = m_count.load(); + return count > 0 ? count : 0; + } +}; +} // end namespace spsc_sema +} // end namespace moodycamel #if defined(AE_VCPP) && (_MSC_VER < 1700 || defined(__cplusplus_cli)) #pragma warning(pop) diff --git a/include/ur_modern_driver/queue/readerwriterqueue.h b/include/ur_modern_driver/queue/readerwriterqueue.h index 1503dcbe7..fdfcdf880 100644 --- a/include/ur_modern_driver/queue/readerwriterqueue.h +++ b/include/ur_modern_driver/queue/readerwriterqueue.h @@ -4,15 +4,15 @@ #pragma once -#include "atomicops.h" #include #include -#include // For malloc/free/abort & size_t +#include // For malloc/free/abort & size_t #include #include #include #include -#if __cplusplus > 199711L || _MSC_VER >= 1700 // C++11 or VS2012 +#include "atomicops.h" +#if __cplusplus > 199711L || _MSC_VER >= 1700 // C++11 or VS2012 #include #endif @@ -34,757 +34,830 @@ #endif #ifndef MOODYCAMEL_EXCEPTIONS_ENABLED -#if (defined(_MSC_VER) && defined(_CPPUNWIND)) || (defined(__GNUC__) && defined(__EXCEPTIONS)) || (!defined(_MSC_VER) && !defined(__GNUC__)) +#if (defined(_MSC_VER) && defined(_CPPUNWIND)) || (defined(__GNUC__) && defined(__EXCEPTIONS)) || \ + (!defined(_MSC_VER) && !defined(__GNUC__)) #define MOODYCAMEL_EXCEPTIONS_ENABLED #endif #endif #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 { - +namespace moodycamel +{ template -class ReaderWriterQueue { - // Design: Based on a queue-of-queues. The low-level queues are just - // circular buffers with front and tail indices indicating where the - // next element to dequeue is and where the next element can be enqueued, - // respectively. Each low-level queue is called a "block". Each block - // wastes exactly one element's worth of space to keep the design simple - // (if front == tail then the queue is empty, and can't be full). - // The high-level queue is a circular linked list of blocks; again there - // is a front and tail, but this time they are pointers to the blocks. - // The front block is where the next element to be dequeued is, provided - // the block is not empty. The back block is where elements are to be - // enqueued, provided the block is not full. - // The producer thread owns all the tail indices/pointers. The consumer - // thread owns all the front indices/pointers. Both threads read each - // other's variables, but only the owning thread updates them. E.g. After - // the consumer reads the producer's tail, the tail may change before the - // consumer is done dequeuing an object, but the consumer knows the tail - // will never go backwards, only forwards. - // If there is no room to enqueue an object, an additional block (of - // equal size to the last block) is added. Blocks are never removed. +class ReaderWriterQueue +{ + // Design: Based on a queue-of-queues. The low-level queues are just + // circular buffers with front and tail indices indicating where the + // next element to dequeue is and where the next element can be enqueued, + // respectively. Each low-level queue is called a "block". Each block + // wastes exactly one element's worth of space to keep the design simple + // (if front == tail then the queue is empty, and can't be full). + // The high-level queue is a circular linked list of blocks; again there + // is a front and tail, but this time they are pointers to the blocks. + // The front block is where the next element to be dequeued is, provided + // the block is not empty. The back block is where elements are to be + // enqueued, provided the block is not full. + // The producer thread owns all the tail indices/pointers. The consumer + // thread owns all the front indices/pointers. Both threads read each + // other's variables, but only the owning thread updates them. E.g. After + // the consumer reads the producer's tail, the tail may change before the + // consumer is done dequeuing an object, but the consumer knows the tail + // will never go backwards, only forwards. + // If there is no room to enqueue an object, an additional block (of + // equal size to the last block) is added. Blocks are never removed. public: - // Constructs a queue that can hold maxSize elements without further - // allocations. If more than MAX_BLOCK_SIZE elements are requested, - // then several blocks of MAX_BLOCK_SIZE each are reserved (including - // at least one extra buffer block). - explicit ReaderWriterQueue(size_t maxSize = 15) + // Constructs a queue that can hold maxSize elements without further + // allocations. If more than MAX_BLOCK_SIZE elements are requested, + // then several blocks of MAX_BLOCK_SIZE each are reserved (including + // at least one extra buffer block). + explicit ReaderWriterQueue(size_t maxSize = 15) #ifndef NDEBUG - : enqueuing(false) - , dequeuing(false) + : enqueuing(false), dequeuing(false) #endif + { + assert(maxSize > 0); + assert(MAX_BLOCK_SIZE == ceilToPow2(MAX_BLOCK_SIZE) && "MAX_BLOCK_SIZE must be a power of 2"); + assert(MAX_BLOCK_SIZE >= 2 && "MAX_BLOCK_SIZE must be at least 2"); + + Block* firstBlock = nullptr; + + largestBlockSize = ceilToPow2(maxSize + 1); // We need a spare slot to fit maxSize elements in the block + if (largestBlockSize > MAX_BLOCK_SIZE * 2) { - assert(maxSize > 0); - assert(MAX_BLOCK_SIZE == ceilToPow2(MAX_BLOCK_SIZE) && "MAX_BLOCK_SIZE must be a power of 2"); - assert(MAX_BLOCK_SIZE >= 2 && "MAX_BLOCK_SIZE must be at least 2"); - - Block* firstBlock = nullptr; - - largestBlockSize = ceilToPow2(maxSize + 1); // We need a spare slot to fit maxSize elements in the block - if (largestBlockSize > MAX_BLOCK_SIZE * 2) { - // We need a spare block in case the producer is writing to a different block the consumer is reading from, and - // wants to enqueue the maximum number of elements. We also need a spare element in each block to avoid the ambiguity - // between front == tail meaning "empty" and "full". - // So the effective number of slots that are guaranteed to be usable at any time is the block size - 1 times the - // number of blocks - 1. Solving for maxSize and applying a ceiling to the division gives us (after simplifying): - size_t initialBlockCount = (maxSize + MAX_BLOCK_SIZE * 2 - 3) / (MAX_BLOCK_SIZE - 1); - largestBlockSize = MAX_BLOCK_SIZE; - Block* lastBlock = nullptr; - for (size_t i = 0; i != initialBlockCount; ++i) { - auto block = make_block(largestBlockSize); - if (block == nullptr) { + // We need a spare block in case the producer is writing to a different block the consumer is reading from, and + // wants to enqueue the maximum number of elements. We also need a spare element in each block to avoid the + // ambiguity + // between front == tail meaning "empty" and "full". + // So the effective number of slots that are guaranteed to be usable at any time is the block size - 1 times the + // number of blocks - 1. Solving for maxSize and applying a ceiling to the division gives us (after simplifying): + size_t initialBlockCount = (maxSize + MAX_BLOCK_SIZE * 2 - 3) / (MAX_BLOCK_SIZE - 1); + largestBlockSize = MAX_BLOCK_SIZE; + Block* lastBlock = nullptr; + for (size_t i = 0; i != initialBlockCount; ++i) + { + auto block = make_block(largestBlockSize); + if (block == nullptr) + { #ifdef MOODYCAMEL_EXCEPTIONS_ENABLED - throw std::bad_alloc(); + throw std::bad_alloc(); #else - abort(); + abort(); #endif - } - if (firstBlock == nullptr) { - firstBlock = block; - } else { - lastBlock->next = block; - } - lastBlock = block; - block->next = firstBlock; - } - } else { - firstBlock = make_block(largestBlockSize); - if (firstBlock == nullptr) { + } + if (firstBlock == nullptr) + { + firstBlock = block; + } + else + { + lastBlock->next = block; + } + lastBlock = block; + block->next = firstBlock; + } + } + else + { + firstBlock = make_block(largestBlockSize); + if (firstBlock == nullptr) + { #ifdef MOODYCAMEL_EXCEPTIONS_ENABLED - throw std::bad_alloc(); + throw std::bad_alloc(); #else - abort(); + abort(); #endif - } - firstBlock->next = firstBlock; - } - frontBlock = firstBlock; - tailBlock = firstBlock; - - // Make sure the reader/writer threads will have the initialized memory setup above: - fence(memory_order_sync); + } + firstBlock->next = firstBlock; } - - // Note: The queue should not be accessed concurrently while it's - // being deleted. It's up to the user to synchronize this. - ~ReaderWriterQueue() + frontBlock = firstBlock; + tailBlock = firstBlock; + + // Make sure the reader/writer threads will have the initialized memory setup above: + fence(memory_order_sync); + } + + // Note: The queue should not be accessed concurrently while it's + // being deleted. It's up to the user to synchronize this. + ~ReaderWriterQueue() + { + // Make sure we get the latest version of all variables from other CPUs: + fence(memory_order_sync); + + // Destroy any remaining objects in queue and free memory + Block* frontBlock_ = frontBlock; + Block* block = frontBlock_; + do { - // Make sure we get the latest version of all variables from other CPUs: - fence(memory_order_sync); - - // Destroy any remaining objects in queue and free memory - Block* frontBlock_ = frontBlock; - Block* block = frontBlock_; - do { - Block* nextBlock = block->next; - size_t blockFront = block->front; - size_t blockTail = block->tail; - - for (size_t i = blockFront; i != blockTail; i = (i + 1) & block->sizeMask) { - auto element = reinterpret_cast(block->data + i * sizeof(T)); - element->~T(); - (void)element; - } - - auto rawBlock = block->rawThis; - block->~Block(); - std::free(rawBlock); - block = nextBlock; - } while (block != frontBlock_); - } + Block* nextBlock = block->next; + size_t blockFront = block->front; + size_t blockTail = block->tail; + + for (size_t i = blockFront; i != blockTail; i = (i + 1) & block->sizeMask) + { + auto element = reinterpret_cast(block->data + i * sizeof(T)); + element->~T(); + (void)element; + } + + auto rawBlock = block->rawThis; + block->~Block(); + std::free(rawBlock); + block = nextBlock; + } 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. + AE_FORCEINLINE bool try_enqueue(T const& element) + { + return inner_enqueue(element); + } + + // Enqueues a moved copy of element if there is room in the queue. + // Returns true if the element was enqueued, false otherwise. + // Does not allocate memory. + AE_FORCEINLINE bool try_enqueue(T&& element) + { + return inner_enqueue(std::forward(element)); + } + + // Enqueues a copy of element on the queue. + // Allocates an additional block of memory if needed. + // Only fails (returns false) if memory allocation fails. + AE_FORCEINLINE bool enqueue(T const& element) + { + return inner_enqueue(element); + } + + // Enqueues a moved copy of element on the queue. + // Allocates an additional block of memory if needed. + // Only fails (returns false) if memory allocation fails. + AE_FORCEINLINE bool enqueue(T&& element) + { + return inner_enqueue(std::forward(element)); + } + + // Attempts to dequeue an element; if the queue is empty, + // returns false instead. If the queue has at least one element, + // moves front to result using operator=, then returns true. + template + bool try_dequeue(U& result) + { +#ifndef NDEBUG + ReentrantGuard guard(this->dequeuing); +#endif - // Enqueues a copy of element if there is room in the queue. - // Returns true if the element was enqueued, false otherwise. - // Does not allocate memory. - AE_FORCEINLINE bool try_enqueue(T const& element) + // High-level pseudocode: + // Remember where the tail block is + // If the front block has an element in it, dequeue it + // Else + // If front block was the tail block when we entered the function, return false + // Else advance to next block and dequeue the item there + + // Note that we have to use the value of the tail block from before we check if the front + // block is full or not, in case the front block is empty and then, before we check if the + // tail block is at the front block or not, the producer fills up the front block *and + // moves on*, which would make us skip a filled block. Seems unlikely, but was consistently + // reproducible in practice. + // In order to avoid overhead in the common case, though, we do a double-checked pattern + // where we have the fast path if the front block is not empty, then read the tail block, + // then re-read the front block and check if it's not empty again, then check if the tail + // block has advanced. + + Block* frontBlock_ = frontBlock.load(); + size_t blockTail = frontBlock_->localTail; + size_t blockFront = frontBlock_->front.load(); + + if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) { - return inner_enqueue(element); - } + fence(memory_order_acquire); - // Enqueues a moved copy of element if there is room in the queue. - // Returns true if the element was enqueued, false otherwise. - // Does not allocate memory. - AE_FORCEINLINE bool try_enqueue(T&& element) - { - return inner_enqueue(std::forward(element)); - } + non_empty_front_block: + // Front block not empty, dequeue from here + auto element = reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); + result = std::move(*element); + element->~T(); - // Enqueues a copy of element on the queue. - // Allocates an additional block of memory if needed. - // Only fails (returns false) if memory allocation fails. - AE_FORCEINLINE bool enqueue(T const& element) - { - return inner_enqueue(element); + blockFront = (blockFront + 1) & frontBlock_->sizeMask; + + fence(memory_order_release); + frontBlock_->front = blockFront; } + else if (frontBlock_ != tailBlock.load()) + { + fence(memory_order_acquire); + + frontBlock_ = frontBlock.load(); + blockTail = frontBlock_->localTail = frontBlock_->tail.load(); + blockFront = frontBlock_->front.load(); + fence(memory_order_acquire); + + if (blockFront != blockTail) + { + // Oh look, the front block isn't empty after all + goto non_empty_front_block; + } + + // Front block is empty but there's another block ahead, advance to it + Block* nextBlock = frontBlock_->next; + // Don't need an acquire fence here since next can only ever be set on the tailBlock, + // and we're not the tailBlock, and we did an acquire earlier after reading tailBlock which + // ensures next is up-to-date on this CPU in case we recently were at tailBlock. + + size_t nextBlockFront = nextBlock->front.load(); + size_t nextBlockTail = nextBlock->localTail = nextBlock->tail.load(); + fence(memory_order_acquire); + + // Since the tailBlock is only ever advanced after being written to, + // we know there's for sure an element to dequeue on it + assert(nextBlockFront != nextBlockTail); + AE_UNUSED(nextBlockTail); - // Enqueues a moved copy of element on the queue. - // Allocates an additional block of memory if needed. - // Only fails (returns false) if memory allocation fails. - AE_FORCEINLINE bool enqueue(T&& element) + // We're done with this block, let the producer use it if it needs + fence(memory_order_release); // Expose possibly pending changes to frontBlock->front from last dequeue + frontBlock = frontBlock_ = nextBlock; + + compiler_fence(memory_order_release); // Not strictly needed + + auto element = reinterpret_cast(frontBlock_->data + nextBlockFront * sizeof(T)); + + result = std::move(*element); + element->~T(); + + nextBlockFront = (nextBlockFront + 1) & frontBlock_->sizeMask; + + fence(memory_order_release); + frontBlock_->front = nextBlockFront; + } + else { - return inner_enqueue(std::forward(element)); + // No elements in current block and no other block to advance to + return false; } - // Attempts to dequeue an element; if the queue is empty, - // returns false instead. If the queue has at least one element, - // moves front to result using operator=, then returns true. - template - bool try_dequeue(U& result) - { + return true; + } + + // Returns a pointer to the front element in the queue (the one that + // would be removed next by a call to `try_dequeue` or `pop`). If the + // queue appears empty at the time the method is called, nullptr is + // returned instead. + // Must be called only from the consumer thread. + T* peek() + { #ifndef NDEBUG - ReentrantGuard guard(this->dequeuing); + ReentrantGuard guard(this->dequeuing); #endif + // See try_dequeue() for reasoning - // High-level pseudocode: - // Remember where the tail block is - // If the front block has an element in it, dequeue it - // Else - // If front block was the tail block when we entered the function, return false - // Else advance to next block and dequeue the item there - - // Note that we have to use the value of the tail block from before we check if the front - // block is full or not, in case the front block is empty and then, before we check if the - // tail block is at the front block or not, the producer fills up the front block *and - // moves on*, which would make us skip a filled block. Seems unlikely, but was consistently - // reproducible in practice. - // In order to avoid overhead in the common case, though, we do a double-checked pattern - // where we have the fast path if the front block is not empty, then read the tail block, - // then re-read the front block and check if it's not empty again, then check if the tail - // block has advanced. - - Block* frontBlock_ = frontBlock.load(); - size_t blockTail = frontBlock_->localTail; - size_t blockFront = frontBlock_->front.load(); - - if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) { - fence(memory_order_acquire); - - non_empty_front_block: - // Front block not empty, dequeue from here - auto element = reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); - result = std::move(*element); - element->~T(); - - blockFront = (blockFront + 1) & frontBlock_->sizeMask; - - fence(memory_order_release); - frontBlock_->front = blockFront; - } else if (frontBlock_ != tailBlock.load()) { - fence(memory_order_acquire); - - frontBlock_ = frontBlock.load(); - blockTail = frontBlock_->localTail = frontBlock_->tail.load(); - blockFront = frontBlock_->front.load(); - fence(memory_order_acquire); - - if (blockFront != blockTail) { - // Oh look, the front block isn't empty after all - goto non_empty_front_block; - } - - // Front block is empty but there's another block ahead, advance to it - Block* nextBlock = frontBlock_->next; - // Don't need an acquire fence here since next can only ever be set on the tailBlock, - // and we're not the tailBlock, and we did an acquire earlier after reading tailBlock which - // ensures next is up-to-date on this CPU in case we recently were at tailBlock. - - size_t nextBlockFront = nextBlock->front.load(); - size_t nextBlockTail = nextBlock->localTail = nextBlock->tail.load(); - fence(memory_order_acquire); - - // Since the tailBlock is only ever advanced after being written to, - // we know there's for sure an element to dequeue on it - assert(nextBlockFront != nextBlockTail); - AE_UNUSED(nextBlockTail); - - // We're done with this block, let the producer use it if it needs - fence(memory_order_release); // Expose possibly pending changes to frontBlock->front from last dequeue - frontBlock = frontBlock_ = nextBlock; - - compiler_fence(memory_order_release); // Not strictly needed - - auto element = reinterpret_cast(frontBlock_->data + nextBlockFront * sizeof(T)); - - result = std::move(*element); - element->~T(); - - nextBlockFront = (nextBlockFront + 1) & frontBlock_->sizeMask; - - fence(memory_order_release); - frontBlock_->front = nextBlockFront; - } else { - // No elements in current block and no other block to advance to - return false; - } + Block* frontBlock_ = frontBlock.load(); + size_t blockTail = frontBlock_->localTail; + size_t blockFront = frontBlock_->front.load(); - return true; + if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) + { + fence(memory_order_acquire); + non_empty_front_block: + return reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); } - - // Returns a pointer to the front element in the queue (the one that - // would be removed next by a call to `try_dequeue` or `pop`). If the - // queue appears empty at the time the method is called, nullptr is - // returned instead. - // Must be called only from the consumer thread. - T* peek() + else if (frontBlock_ != tailBlock.load()) { -#ifndef NDEBUG - ReentrantGuard guard(this->dequeuing); -#endif - // See try_dequeue() for reasoning - - Block* frontBlock_ = frontBlock.load(); - size_t blockTail = frontBlock_->localTail; - size_t blockFront = frontBlock_->front.load(); - - if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) { - fence(memory_order_acquire); - non_empty_front_block: - return reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); - } else if (frontBlock_ != tailBlock.load()) { - fence(memory_order_acquire); - frontBlock_ = frontBlock.load(); - blockTail = frontBlock_->localTail = frontBlock_->tail.load(); - blockFront = frontBlock_->front.load(); - fence(memory_order_acquire); - - if (blockFront != blockTail) { - goto non_empty_front_block; - } - - Block* nextBlock = frontBlock_->next; - - size_t nextBlockFront = nextBlock->front.load(); - fence(memory_order_acquire); - - assert(nextBlockFront != nextBlock->tail.load()); - return reinterpret_cast(nextBlock->data + nextBlockFront * sizeof(T)); - } + fence(memory_order_acquire); + frontBlock_ = frontBlock.load(); + blockTail = frontBlock_->localTail = frontBlock_->tail.load(); + blockFront = frontBlock_->front.load(); + fence(memory_order_acquire); + + if (blockFront != blockTail) + { + goto non_empty_front_block; + } - return nullptr; + Block* nextBlock = frontBlock_->next; + + size_t nextBlockFront = nextBlock->front.load(); + fence(memory_order_acquire); + + assert(nextBlockFront != nextBlock->tail.load()); + return reinterpret_cast(nextBlock->data + nextBlockFront * sizeof(T)); } - // Removes the front element from the queue, if any, without returning it. - // Returns true on success, or false if the queue appeared empty at the time - // `pop` was called. - bool pop() - { + return nullptr; + } + + // Removes the front element from the queue, if any, without returning it. + // Returns true on success, or false if the queue appeared empty at the time + // `pop` was called. + bool pop() + { #ifndef NDEBUG - ReentrantGuard guard(this->dequeuing); + ReentrantGuard guard(this->dequeuing); #endif - // See try_dequeue() for reasoning + // See try_dequeue() for reasoning - Block* frontBlock_ = frontBlock.load(); - size_t blockTail = frontBlock_->localTail; - size_t blockFront = frontBlock_->front.load(); + Block* frontBlock_ = frontBlock.load(); + size_t blockTail = frontBlock_->localTail; + size_t blockFront = frontBlock_->front.load(); - if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) { - fence(memory_order_acquire); + if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) + { + fence(memory_order_acquire); - non_empty_front_block: - auto element = reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); - element->~T(); + non_empty_front_block: + auto element = reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); + element->~T(); - blockFront = (blockFront + 1) & frontBlock_->sizeMask; + blockFront = (blockFront + 1) & frontBlock_->sizeMask; - fence(memory_order_release); - frontBlock_->front = blockFront; - } else if (frontBlock_ != tailBlock.load()) { - fence(memory_order_acquire); - frontBlock_ = frontBlock.load(); - blockTail = frontBlock_->localTail = frontBlock_->tail.load(); - blockFront = frontBlock_->front.load(); - fence(memory_order_acquire); + fence(memory_order_release); + frontBlock_->front = blockFront; + } + else if (frontBlock_ != tailBlock.load()) + { + fence(memory_order_acquire); + frontBlock_ = frontBlock.load(); + blockTail = frontBlock_->localTail = frontBlock_->tail.load(); + blockFront = frontBlock_->front.load(); + fence(memory_order_acquire); - if (blockFront != blockTail) { - goto non_empty_front_block; - } + if (blockFront != blockTail) + { + goto non_empty_front_block; + } - // Front block is empty but there's another block ahead, advance to it - Block* nextBlock = frontBlock_->next; + // Front block is empty but there's another block ahead, advance to it + Block* nextBlock = frontBlock_->next; - size_t nextBlockFront = nextBlock->front.load(); - size_t nextBlockTail = nextBlock->localTail = nextBlock->tail.load(); - fence(memory_order_acquire); + size_t nextBlockFront = nextBlock->front.load(); + size_t nextBlockTail = nextBlock->localTail = nextBlock->tail.load(); + fence(memory_order_acquire); - assert(nextBlockFront != nextBlockTail); - AE_UNUSED(nextBlockTail); + assert(nextBlockFront != nextBlockTail); + AE_UNUSED(nextBlockTail); - fence(memory_order_release); - frontBlock = frontBlock_ = nextBlock; + fence(memory_order_release); + frontBlock = frontBlock_ = nextBlock; - compiler_fence(memory_order_release); + compiler_fence(memory_order_release); - auto element = reinterpret_cast(frontBlock_->data + nextBlockFront * sizeof(T)); - element->~T(); + auto element = reinterpret_cast(frontBlock_->data + nextBlockFront * sizeof(T)); + element->~T(); - nextBlockFront = (nextBlockFront + 1) & frontBlock_->sizeMask; + nextBlockFront = (nextBlockFront + 1) & frontBlock_->sizeMask; - fence(memory_order_release); - frontBlock_->front = nextBlockFront; - } else { - // No elements in current block and no other block to advance to - return false; - } - - return true; + fence(memory_order_release); + frontBlock_->front = nextBlockFront; } - - // Returns the approximate number of items currently in the queue. - // Safe to call from both the producer and consumer threads. - inline size_t size_approx() const + else { - size_t result = 0; - Block* frontBlock_ = frontBlock.load(); - Block* block = frontBlock_; - do { - fence(memory_order_acquire); - size_t blockFront = block->front.load(); - size_t blockTail = block->tail.load(); - result += (blockTail - blockFront) & block->sizeMask; - block = block->next.load(); - } while (block != frontBlock_); - return result; + // No elements in current block and no other block to advance to + return false; } -private: - enum AllocationMode { CanAlloc, - CannotAlloc }; - - template - bool inner_enqueue(U&& element) + return true; + } + + // Returns the approximate number of items currently in the queue. + // Safe to call from both the producer and consumer threads. + inline size_t size_approx() const + { + size_t result = 0; + Block* frontBlock_ = frontBlock.load(); + Block* block = frontBlock_; + do { + fence(memory_order_acquire); + size_t blockFront = block->front.load(); + size_t blockTail = block->tail.load(); + result += (blockTail - blockFront) & block->sizeMask; + block = block->next.load(); + } while (block != frontBlock_); + return result; + } + +private: + enum AllocationMode + { + CanAlloc, + CannotAlloc + }; + + template + bool inner_enqueue(U&& element) + { #ifndef NDEBUG - ReentrantGuard guard(this->enqueuing); + ReentrantGuard guard(this->enqueuing); #endif - // High-level pseudocode (assuming we're allowed to alloc a new block): - // If room in tail block, add to tail - // Else check next block - // If next block is not the head block, enqueue on next block - // Else create a new block and enqueue there - // Advance tail to the block we just enqueued to - - Block* tailBlock_ = tailBlock.load(); - size_t blockFront = tailBlock_->localFront; - size_t blockTail = tailBlock_->tail.load(); - - size_t nextBlockTail = (blockTail + 1) & tailBlock_->sizeMask; - if (nextBlockTail != blockFront || nextBlockTail != (tailBlock_->localFront = tailBlock_->front.load())) { - fence(memory_order_acquire); - // This block has room for at least one more element - char* location = tailBlock_->data + blockTail * sizeof(T); - new (location) T(std::forward(element)); - - fence(memory_order_release); - tailBlock_->tail = nextBlockTail; - } else { - fence(memory_order_acquire); - if (tailBlock_->next.load() != frontBlock) { - // Note that the reason we can't advance to the frontBlock and start adding new entries there - // is because if we did, then dequeue would stay in that block, eventually reading the new values, - // instead of advancing to the next full block (whose values were enqueued first and so should be - // consumed first). - - fence(memory_order_acquire); // Ensure we get latest writes if we got the latest frontBlock - - // tailBlock is full, but there's a free block ahead, use it - Block* tailBlockNext = tailBlock_->next.load(); - size_t nextBlockFront = tailBlockNext->localFront = tailBlockNext->front.load(); - nextBlockTail = tailBlockNext->tail.load(); - fence(memory_order_acquire); - - // This block must be empty since it's not the head block and we - // go through the blocks in a circle - assert(nextBlockFront == nextBlockTail); - tailBlockNext->localFront = nextBlockFront; - - char* location = tailBlockNext->data + nextBlockTail * sizeof(T); - new (location) T(std::forward(element)); - - tailBlockNext->tail = (nextBlockTail + 1) & tailBlockNext->sizeMask; - - fence(memory_order_release); - tailBlock = tailBlockNext; - } else if (canAlloc == CanAlloc) { - // tailBlock is full and there's no free block ahead; create a new block - auto newBlockSize = largestBlockSize >= MAX_BLOCK_SIZE ? largestBlockSize : largestBlockSize * 2; - auto newBlock = make_block(newBlockSize); - if (newBlock == nullptr) { - // Could not allocate a block! - return false; - } - largestBlockSize = newBlockSize; - - new (newBlock->data) T(std::forward(element)); - - assert(newBlock->front == 0); - newBlock->tail = newBlock->localTail = 1; - - newBlock->next = tailBlock_->next.load(); - tailBlock_->next = newBlock; - - // Might be possible for the dequeue thread to see the new tailBlock->next - // *without* seeing the new tailBlock value, but this is OK since it can't - // advance to the next block until tailBlock is set anyway (because the only - // case where it could try to read the next is if it's already at the tailBlock, - // and it won't advance past tailBlock in any circumstance). - - fence(memory_order_release); - tailBlock = newBlock; - } else if (canAlloc == CannotAlloc) { - // Would have had to allocate a new block to enqueue, but not allowed - return false; - } else { - assert(false && "Should be unreachable code"); - return false; - } - } - - return true; - } - - // Disable copying - ReaderWriterQueue(ReaderWriterQueue const&) {} + // High-level pseudocode (assuming we're allowed to alloc a new block): + // If room in tail block, add to tail + // Else check next block + // If next block is not the head block, enqueue on next block + // Else create a new block and enqueue there + // Advance tail to the block we just enqueued to - // Disable assignment - ReaderWriterQueue& operator=(ReaderWriterQueue const&) {} + Block* tailBlock_ = tailBlock.load(); + size_t blockFront = tailBlock_->localFront; + size_t blockTail = tailBlock_->tail.load(); - AE_FORCEINLINE static size_t ceilToPow2(size_t x) + size_t nextBlockTail = (blockTail + 1) & tailBlock_->sizeMask; + if (nextBlockTail != blockFront || nextBlockTail != (tailBlock_->localFront = tailBlock_->front.load())) { - // From http://graphics.stanford.edu/~seander/bithacks.html#RoundUpPowerOf2 - --x; - x |= x >> 1; - x |= x >> 2; - x |= x >> 4; - for (size_t i = 1; i < sizeof(size_t); i <<= 1) { - x |= x >> (i << 3); - } - ++x; - return x; - } + fence(memory_order_acquire); + // This block has room for at least one more element + char* location = tailBlock_->data + blockTail * sizeof(T); + new (location) T(std::forward(element)); - template - static AE_FORCEINLINE char* align_for(char* ptr) - { - const std::size_t alignment = std::alignment_of::value; - return ptr + (alignment - (reinterpret_cast(ptr) % alignment)) % alignment; + fence(memory_order_release); + tailBlock_->tail = nextBlockTail; } - -private: -#ifndef NDEBUG - struct ReentrantGuard { - ReentrantGuard(bool& _inSection) - : inSection(_inSection) + else + { + fence(memory_order_acquire); + if (tailBlock_->next.load() != frontBlock) + { + // Note that the reason we can't advance to the frontBlock and start adding new entries there + // is because if we did, then dequeue would stay in that block, eventually reading the new values, + // instead of advancing to the next full block (whose values were enqueued first and so should be + // consumed first). + + fence(memory_order_acquire); // Ensure we get latest writes if we got the latest frontBlock + + // tailBlock is full, but there's a free block ahead, use it + Block* tailBlockNext = tailBlock_->next.load(); + size_t nextBlockFront = tailBlockNext->localFront = tailBlockNext->front.load(); + nextBlockTail = tailBlockNext->tail.load(); + fence(memory_order_acquire); + + // This block must be empty since it's not the head block and we + // go through the blocks in a circle + assert(nextBlockFront == nextBlockTail); + tailBlockNext->localFront = nextBlockFront; + + char* location = tailBlockNext->data + nextBlockTail * sizeof(T); + new (location) T(std::forward(element)); + + tailBlockNext->tail = (nextBlockTail + 1) & tailBlockNext->sizeMask; + + fence(memory_order_release); + tailBlock = tailBlockNext; + } + else if (canAlloc == CanAlloc) + { + // tailBlock is full and there's no free block ahead; create a new block + auto newBlockSize = largestBlockSize >= MAX_BLOCK_SIZE ? largestBlockSize : largestBlockSize * 2; + auto newBlock = make_block(newBlockSize); + if (newBlock == nullptr) { - assert(!inSection && "ReaderWriterQueue does not support enqueuing or dequeuing elements from other elements' ctors and dtors"); - inSection = true; + // Could not allocate a block! + return false; } + largestBlockSize = newBlockSize; - ~ReentrantGuard() { inSection = false; } + new (newBlock->data) T(std::forward(element)); - private: - ReentrantGuard& operator=(ReentrantGuard const&); + assert(newBlock->front == 0); + newBlock->tail = newBlock->localTail = 1; - private: - bool& inSection; - }; -#endif + newBlock->next = tailBlock_->next.load(); + tailBlock_->next = newBlock; - struct Block { - // Avoid false-sharing by putting highly contended variables on their own cache lines - weak_atomic front; // (Atomic) Elements are read from here - size_t localTail; // An uncontended shadow copy of tail, owned by the consumer + // Might be possible for the dequeue thread to see the new tailBlock->next + // *without* seeing the new tailBlock value, but this is OK since it can't + // advance to the next block until tailBlock is set anyway (because the only + // case where it could try to read the next is if it's already at the tailBlock, + // and it won't advance past tailBlock in any circumstance). - char cachelineFiller0[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic) - sizeof(size_t)]; - weak_atomic tail; // (Atomic) Elements are enqueued here - size_t localFront; + fence(memory_order_release); + tailBlock = newBlock; + } + else if (canAlloc == CannotAlloc) + { + // Would have had to allocate a new block to enqueue, but not allowed + return false; + } + else + { + assert(false && "Should be unreachable code"); + return false; + } + } - char cachelineFiller1[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic) - sizeof(size_t)]; // next isn't very contended, but we don't want it on the same cache line as tail (which is) - weak_atomic next; // (Atomic) + return true; + } + + // Disable copying + ReaderWriterQueue(ReaderWriterQueue const&) + { + } + + // Disable assignment + ReaderWriterQueue& operator=(ReaderWriterQueue const&) + { + } + + AE_FORCEINLINE static size_t ceilToPow2(size_t x) + { + // From http://graphics.stanford.edu/~seander/bithacks.html#RoundUpPowerOf2 + --x; + x |= x >> 1; + x |= x >> 2; + x |= x >> 4; + for (size_t i = 1; i < sizeof(size_t); i <<= 1) + { + x |= x >> (i << 3); + } + ++x; + return x; + } - char* data; // Contents (on heap) are aligned to T's alignment + template + static AE_FORCEINLINE char* align_for(char* ptr) + { + const std::size_t alignment = std::alignment_of::value; + return ptr + (alignment - (reinterpret_cast(ptr) % alignment)) % alignment; + } - const size_t sizeMask; +private: +#ifndef NDEBUG + struct ReentrantGuard + { + ReentrantGuard(bool& _inSection) : inSection(_inSection) + { + assert(!inSection && "ReaderWriterQueue does not support enqueuing or dequeuing elements from other elements' " + "ctors and dtors"); + inSection = true; + } - // 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) - { - } + ~ReentrantGuard() + { + inSection = false; + } - private: - // C4512 - Assignment operator could not be generated - Block& operator=(Block const&); + private: + ReentrantGuard& operator=(ReentrantGuard const&); - public: - char* rawThis; - }; + private: + bool& inSection; + }; +#endif - static Block* make_block(size_t capacity) + struct Block + { + // Avoid false-sharing by putting highly contended variables on their own cache lines + weak_atomic front; // (Atomic) Elements are read from here + size_t localTail; // An uncontended shadow copy of tail, owned by the consumer + + char cachelineFiller0[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic) - sizeof(size_t)]; + weak_atomic tail; // (Atomic) Elements are enqueued here + size_t localFront; + + char cachelineFiller1[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic) - sizeof(size_t)]; // next isn't + // very + // contended, but + // we don't want + // it on the same + // cache line as + // tail (which + // is) + weak_atomic next; // (Atomic) + + char* data; // Contents (on heap) are aligned to T's alignment + + 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) { - // Allocate enough memory for the block itself, as well as all the elements it will contain - auto size = sizeof(Block) + std::alignment_of::value - 1; - size += sizeof(T) * capacity + std::alignment_of::value - 1; - auto newBlockRaw = static_cast(std::malloc(size)); - if (newBlockRaw == nullptr) { - return nullptr; - } + } - auto newBlockAligned = align_for(newBlockRaw); - auto newBlockData = align_for(newBlockAligned + sizeof(Block)); - return new (newBlockAligned) Block(capacity, newBlockRaw, newBlockData); + private: + // C4512 - Assignment operator could not be generated + Block& operator=(Block const&); + + public: + char* rawThis; + }; + + static Block* make_block(size_t capacity) + { + // Allocate enough memory for the block itself, as well as all the elements it will contain + auto size = sizeof(Block) + std::alignment_of::value - 1; + size += sizeof(T) * capacity + std::alignment_of::value - 1; + auto newBlockRaw = static_cast(std::malloc(size)); + if (newBlockRaw == nullptr) + { + return nullptr; } + auto newBlockAligned = align_for(newBlockRaw); + auto newBlockData = align_for(newBlockAligned + sizeof(Block)); + return new (newBlockAligned) Block(capacity, newBlockRaw, newBlockData); + } + private: - weak_atomic frontBlock; // (Atomic) Elements are enqueued to this block + weak_atomic frontBlock; // (Atomic) Elements are enqueued to this block - char cachelineFiller[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic)]; - weak_atomic tailBlock; // (Atomic) Elements are dequeued from this block + char cachelineFiller[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic)]; + weak_atomic tailBlock; // (Atomic) Elements are dequeued from this block - size_t largestBlockSize; + size_t largestBlockSize; #ifndef NDEBUG - bool enqueuing; - bool dequeuing; + bool enqueuing; + bool dequeuing; #endif }; // Like ReaderWriterQueue, but also providees blocking operations template -class BlockingReaderWriterQueue { +class BlockingReaderWriterQueue +{ private: - typedef ::moodycamel::ReaderWriterQueue ReaderWriterQueue; + typedef ::moodycamel::ReaderWriterQueue 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. - // Does not allocate memory. - AE_FORCEINLINE bool try_enqueue(T const& element) + explicit BlockingReaderWriterQueue(size_t maxSize = 15) : inner(maxSize) + { + } + + // Enqueues a copy of element if there is room in the queue. + // Returns true if the element was enqueued, false otherwise. + // Does not allocate memory. + AE_FORCEINLINE bool try_enqueue(T const& element) + { + if (inner.try_enqueue(element)) { - if (inner.try_enqueue(element)) { - sema.signal(); - return true; - } - return false; + sema.signal(); + return true; } - - // Enqueues a moved copy of element if there is room in the queue. - // Returns true if the element was enqueued, false otherwise. - // Does not allocate memory. - AE_FORCEINLINE bool try_enqueue(T&& element) + return false; + } + + // Enqueues a moved copy of element if there is room in the queue. + // Returns true if the element was enqueued, false otherwise. + // Does not allocate memory. + AE_FORCEINLINE bool try_enqueue(T&& element) + { + if (inner.try_enqueue(std::forward(element))) { - if (inner.try_enqueue(std::forward(element))) { - sema.signal(); - return true; - } - return false; + sema.signal(); + return true; } - - // Enqueues a copy of element on the queue. - // Allocates an additional block of memory if needed. - // Only fails (returns false) if memory allocation fails. - AE_FORCEINLINE bool enqueue(T const& element) + return false; + } + + // Enqueues a copy of element on the queue. + // Allocates an additional block of memory if needed. + // Only fails (returns false) if memory allocation fails. + AE_FORCEINLINE bool enqueue(T const& element) + { + if (inner.enqueue(element)) { - if (inner.enqueue(element)) { - sema.signal(); - return true; - } - return false; - } - - // Enqueues a moved copy of element on the queue. - // Allocates an additional block of memory if needed. - // Only fails (returns false) if memory allocation fails. - AE_FORCEINLINE bool enqueue(T&& element) - { - if (inner.enqueue(std::forward(element))) { - sema.signal(); - return true; - } - return false; + sema.signal(); + return true; } - - // Attempts to dequeue an element; if the queue is empty, - // returns false instead. If the queue has at least one element, - // moves front to result using operator=, then returns true. - template - bool try_dequeue(U& result) + return false; + } + + // Enqueues a moved copy of element on the queue. + // Allocates an additional block of memory if needed. + // Only fails (returns false) if memory allocation fails. + AE_FORCEINLINE bool enqueue(T&& element) + { + if (inner.enqueue(std::forward(element))) { - if (sema.tryWait()) { - bool success = inner.try_dequeue(result); - assert(success); - AE_UNUSED(success); - return true; - } - return false; + sema.signal(); + return true; } - - // Attempts to dequeue an element; if the queue is empty, - // waits until an element is available, then dequeues it. - template - void wait_dequeue(U& result) + return false; + } + + // Attempts to dequeue an element; if the queue is empty, + // returns false instead. If the queue has at least one element, + // moves front to result using operator=, then returns true. + template + bool try_dequeue(U& result) + { + if (sema.tryWait()) { - sema.wait(); - bool success = inner.try_dequeue(result); - AE_UNUSED(result); - assert(success); - AE_UNUSED(success); + bool success = inner.try_dequeue(result); + assert(success); + AE_UNUSED(success); + return true; } - - // Attempts to dequeue an element; if the queue is empty, - // waits until an element is available up to the specified timeout, - // then dequeues it and returns true, or returns false if the timeout - // expires before an element can be dequeued. - // Using a negative timeout indicates an indefinite timeout, - // and is thus functionally equivalent to calling wait_dequeue. - template - bool wait_dequeue_timed(U& result, std::int64_t timeout_usecs) + return false; + } + + // Attempts to dequeue an element; if the queue is empty, + // waits until an element is available, then dequeues it. + template + void wait_dequeue(U& result) + { + sema.wait(); + bool success = inner.try_dequeue(result); + AE_UNUSED(result); + assert(success); + AE_UNUSED(success); + } + + // Attempts to dequeue an element; if the queue is empty, + // waits until an element is available up to the specified timeout, + // then dequeues it and returns true, or returns false if the timeout + // expires before an element can be dequeued. + // Using a negative timeout indicates an indefinite timeout, + // and is thus functionally equivalent to calling wait_dequeue. + template + bool wait_dequeue_timed(U& result, std::int64_t timeout_usecs) + { + if (!sema.wait(timeout_usecs)) { - if (!sema.wait(timeout_usecs)) { - return false; - } - bool success = inner.try_dequeue(result); - AE_UNUSED(result); - assert(success); - AE_UNUSED(success); - return true; + return false; } + bool success = inner.try_dequeue(result); + AE_UNUSED(result); + assert(success); + AE_UNUSED(success); + return true; + } #if __cplusplus > 199711L || _MSC_VER >= 1700 - // Attempts to dequeue an element; if the queue is empty, - // waits until an element is available up to the specified timeout, - // then dequeues it and returns true, or returns false if the timeout - // expires before an element can be dequeued. - // Using a negative timeout indicates an indefinite timeout, - // and is thus functionally equivalent to calling wait_dequeue. - template - inline bool wait_dequeue_timed(U& result, std::chrono::duration const& timeout) - { - return wait_dequeue_timed(result, std::chrono::duration_cast(timeout).count()); - } + // Attempts to dequeue an element; if the queue is empty, + // waits until an element is available up to the specified timeout, + // then dequeues it and returns true, or returns false if the timeout + // expires before an element can be dequeued. + // Using a negative timeout indicates an indefinite timeout, + // and is thus functionally equivalent to calling wait_dequeue. + template + inline bool wait_dequeue_timed(U& result, std::chrono::duration const& timeout) + { + return wait_dequeue_timed(result, std::chrono::duration_cast(timeout).count()); + } #endif - // Returns a pointer to the front element in the queue (the one that - // would be removed next by a call to `try_dequeue` or `pop`). If the - // queue appears empty at the time the method is called, nullptr is - // returned instead. - // Must be called only from the consumer thread. - AE_FORCEINLINE T* peek() + // Returns a pointer to the front element in the queue (the one that + // would be removed next by a call to `try_dequeue` or `pop`). If the + // queue appears empty at the time the method is called, nullptr is + // returned instead. + // Must be called only from the consumer thread. + AE_FORCEINLINE T* peek() + { + return inner.peek(); + } + + // Removes the front element from the queue, if any, without returning it. + // Returns true on success, or false if the queue appeared empty at the time + // `pop` was called. + AE_FORCEINLINE bool pop() + { + if (sema.tryWait()) { - return inner.peek(); + bool result = inner.pop(); + assert(result); + AE_UNUSED(result); + return true; } + return false; + } - // Removes the front element from the queue, if any, without returning it. - // Returns true on success, or false if the queue appeared empty at the time - // `pop` was called. - AE_FORCEINLINE bool pop() - { - if (sema.tryWait()) { - bool result = inner.pop(); - assert(result); - AE_UNUSED(result); - return true; - } - return false; - } - - // Returns the approximate number of items currently in the queue. - // Safe to call from both the producer and consumer threads. - AE_FORCEINLINE size_t size_approx() const - { - return sema.availableApprox(); - } + // Returns the approximate number of items currently in the queue. + // Safe to call from both the producer and consumer threads. + AE_FORCEINLINE size_t size_approx() const + { + return sema.availableApprox(); + } private: - // Disable copying & assignment - BlockingReaderWriterQueue(ReaderWriterQueue const&) {} - BlockingReaderWriterQueue& operator=(ReaderWriterQueue const&) {} + // Disable copying & assignment + BlockingReaderWriterQueue(ReaderWriterQueue const&) + { + } + BlockingReaderWriterQueue& operator=(ReaderWriterQueue const&) + { + } private: - ReaderWriterQueue inner; - spsc_sema::LightweightSemaphore sema; + ReaderWriterQueue inner; + spsc_sema::LightweightSemaphore sema; }; -} // end namespace moodycamel +} // end namespace moodycamel #ifdef AE_VCPP #pragma warning(pop) diff --git a/include/ur_modern_driver/robot_state.h b/include/ur_modern_driver/robot_state.h index a35da4486..621926a1f 100644 --- a/include/ur_modern_driver/robot_state.h +++ b/include/ur_modern_driver/robot_state.h @@ -19,202 +19,215 @@ #ifndef ROBOT_STATE_H_ #define ROBOT_STATE_H_ -#include #include -#include #include #include #include +#include +#include #include -namespace message_types { -enum message_type { - ROBOT_STATE = 16, - ROBOT_MESSAGE = 20, - PROGRAM_STATE_MESSAGE = 25 +namespace message_types +{ +enum message_type +{ + ROBOT_STATE = 16, + ROBOT_MESSAGE = 20, + PROGRAM_STATE_MESSAGE = 25 }; } typedef message_types::message_type messageType; -namespace package_types { -enum package_type { - ROBOT_MODE_DATA = 0, - JOINT_DATA = 1, - TOOL_DATA = 2, - MASTERBOARD_DATA = 3, - CARTESIAN_INFO = 4, - KINEMATICS_INFO = 5, - CONFIGURATION_DATA = 6, - FORCE_MODE_DATA = 7, - ADDITIONAL_INFO = 8, - CALIBRATION_DATA = 9 +namespace package_types +{ +enum package_type +{ + ROBOT_MODE_DATA = 0, + JOINT_DATA = 1, + TOOL_DATA = 2, + MASTERBOARD_DATA = 3, + CARTESIAN_INFO = 4, + KINEMATICS_INFO = 5, + CONFIGURATION_DATA = 6, + FORCE_MODE_DATA = 7, + ADDITIONAL_INFO = 8, + CALIBRATION_DATA = 9 }; } typedef package_types::package_type packageType; -namespace robot_message_types { -enum robot_message_type { - ROBOT_MESSAGE_TEXT = 0, - ROBOT_MESSAGE_PROGRAM_LABEL = 1, - PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2, - ROBOT_MESSAGE_VERSION = 3, - ROBOT_MESSAGE_SAFETY_MODE = 5, - ROBOT_MESSAGE_ERROR_CODE = 6, - ROBOT_MESSAGE_KEY = 7, - ROBOT_MESSAGE_REQUEST_VALUE = 9, - ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10 +namespace robot_message_types +{ +enum robot_message_type +{ + ROBOT_MESSAGE_TEXT = 0, + ROBOT_MESSAGE_PROGRAM_LABEL = 1, + PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2, + ROBOT_MESSAGE_VERSION = 3, + ROBOT_MESSAGE_SAFETY_MODE = 5, + ROBOT_MESSAGE_ERROR_CODE = 6, + ROBOT_MESSAGE_KEY = 7, + ROBOT_MESSAGE_REQUEST_VALUE = 9, + ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10 }; } typedef robot_message_types::robot_message_type robotMessageType; -namespace robot_state_type_v18 { -enum robot_state_type { - ROBOT_RUNNING_MODE = 0, - ROBOT_FREEDRIVE_MODE = 1, - ROBOT_READY_MODE = 2, - ROBOT_INITIALIZING_MODE = 3, - ROBOT_SECURITY_STOPPED_MODE = 4, - ROBOT_EMERGENCY_STOPPED_MODE = 5, - ROBOT_FATAL_ERROR_MODE = 6, - ROBOT_NO_POWER_MODE = 7, - ROBOT_NOT_CONNECTED_MODE = 8, - ROBOT_SHUTDOWN_MODE = 9, - ROBOT_SAFEGUARD_STOP_MODE = 10 +namespace robot_state_type_v18 +{ +enum robot_state_type +{ + ROBOT_RUNNING_MODE = 0, + ROBOT_FREEDRIVE_MODE = 1, + ROBOT_READY_MODE = 2, + ROBOT_INITIALIZING_MODE = 3, + ROBOT_SECURITY_STOPPED_MODE = 4, + ROBOT_EMERGENCY_STOPPED_MODE = 5, + ROBOT_FATAL_ERROR_MODE = 6, + ROBOT_NO_POWER_MODE = 7, + ROBOT_NOT_CONNECTED_MODE = 8, + ROBOT_SHUTDOWN_MODE = 9, + ROBOT_SAFEGUARD_STOP_MODE = 10 }; } typedef robot_state_type_v18::robot_state_type robotStateTypeV18; -namespace robot_state_type_v30 { -enum robot_state_type { - ROBOT_MODE_DISCONNECTED = 0, - ROBOT_MODE_CONFIRM_SAFETY = 1, - ROBOT_MODE_BOOTING = 2, - ROBOT_MODE_POWER_OFF = 3, - ROBOT_MODE_POWER_ON = 4, - ROBOT_MODE_IDLE = 5, - ROBOT_MODE_BACKDRIVE = 6, - ROBOT_MODE_RUNNING = 7, - ROBOT_MODE_UPDATING_FIRMWARE = 8 +namespace robot_state_type_v30 +{ +enum robot_state_type +{ + ROBOT_MODE_DISCONNECTED = 0, + ROBOT_MODE_CONFIRM_SAFETY = 1, + ROBOT_MODE_BOOTING = 2, + ROBOT_MODE_POWER_OFF = 3, + ROBOT_MODE_POWER_ON = 4, + ROBOT_MODE_IDLE = 5, + ROBOT_MODE_BACKDRIVE = 6, + ROBOT_MODE_RUNNING = 7, + ROBOT_MODE_UPDATING_FIRMWARE = 8 }; } typedef robot_state_type_v30::robot_state_type robotStateTypeV30; -struct version_message { - uint64_t timestamp; - int8_t source; - int8_t robot_message_type; - int8_t project_name_size; - char project_name[15]; - uint8_t major_version; - uint8_t minor_version; - int svn_revision; - char build_date[25]; +struct version_message +{ + uint64_t timestamp; + int8_t source; + int8_t robot_message_type; + int8_t project_name_size; + char project_name[15]; + uint8_t major_version; + uint8_t minor_version; + int svn_revision; + char build_date[25]; }; -struct masterboard_data { - int digitalInputBits; - int digitalOutputBits; - char analogInputRange0; - char analogInputRange1; - double analogInput0; - double analogInput1; - char analogOutputDomain0; - char analogOutputDomain1; - double analogOutput0; - double analogOutput1; - float masterBoardTemperature; - float robotVoltage48V; - float robotCurrent; - float masterIOCurrent; - unsigned char safetyMode; - unsigned char masterOnOffState; - char euromap67InterfaceInstalled; - int euromapInputBits; - int euromapOutputBits; - float euromapVoltage; - float euromapCurrent; +struct masterboard_data +{ + int digitalInputBits; + int digitalOutputBits; + char analogInputRange0; + char analogInputRange1; + double analogInput0; + double analogInput1; + char analogOutputDomain0; + char analogOutputDomain1; + double analogOutput0; + double analogOutput1; + float masterBoardTemperature; + float robotVoltage48V; + float robotCurrent; + float masterIOCurrent; + unsigned char safetyMode; + unsigned char masterOnOffState; + char euromap67InterfaceInstalled; + int euromapInputBits; + int euromapOutputBits; + float euromapVoltage; + float euromapCurrent; }; -struct robot_mode_data { - uint64_t timestamp; - bool isRobotConnected; - bool isRealRobotEnabled; - bool isPowerOnRobot; - bool isEmergencyStopped; - bool isProtectiveStopped; - bool isProgramRunning; - bool isProgramPaused; - unsigned char robotMode; - unsigned char controlMode; - double targetSpeedFraction; - double speedScaling; +struct robot_mode_data +{ + uint64_t timestamp; + bool isRobotConnected; + bool isRealRobotEnabled; + bool isPowerOnRobot; + bool isEmergencyStopped; + bool isProtectiveStopped; + bool isProgramRunning; + bool isProgramPaused; + unsigned char robotMode; + unsigned char controlMode; + double targetSpeedFraction; + double speedScaling; }; -class RobotState { +class RobotState +{ private: - version_message version_msg_; - masterboard_data mb_data_; - robot_mode_data robot_mode_; + version_message version_msg_; + masterboard_data mb_data_; + robot_mode_data robot_mode_; - std::recursive_mutex val_lock_; // Locks the variables while unpack parses data; + std::recursive_mutex val_lock_; // Locks the variables while unpack parses data; - std::condition_variable* pMsg_cond_; //Signals that new vars are available - bool new_data_available_; //to avoid spurious wakes - unsigned char robot_mode_running_; + std::condition_variable* pMsg_cond_; // Signals that new vars are available + bool new_data_available_; // to avoid spurious wakes + unsigned char robot_mode_running_; - double ntohd(uint64_t nf); + double ntohd(uint64_t nf); public: - RobotState(std::condition_variable& msg_cond); - ~RobotState(); - double getVersion(); - double getTime(); - std::vector getQTarget(); - int getDigitalInputBits(); - int getDigitalOutputBits(); - char getAnalogInputRange0(); - char getAnalogInputRange1(); - double getAnalogInput0(); - double getAnalogInput1(); - char getAnalogOutputDomain0(); - char getAnalogOutputDomain1(); - double getAnalogOutput0(); - double getAnalogOutput1(); - std::vector getVActual(); - float getMasterBoardTemperature(); - float getRobotVoltage48V(); - float getRobotCurrent(); - float getMasterIOCurrent(); - unsigned char getSafetyMode(); - unsigned char getInReducedMode(); - char getEuromap67InterfaceInstalled(); - int getEuromapInputBits(); - int getEuromapOutputBits(); - float getEuromapVoltage(); - float getEuromapCurrent(); - - bool isRobotConnected(); - bool isRealRobotEnabled(); - bool isPowerOnRobot(); - bool isEmergencyStopped(); - bool isProtectiveStopped(); - bool isProgramRunning(); - bool isProgramPaused(); - unsigned char getRobotMode(); - bool isReady(); - - void setDisconnected(); - - bool getNewDataAvailable(); - void finishedReading(); - - void unpack(uint8_t* buf, unsigned int buf_length); - void unpackRobotMessage(uint8_t* buf, unsigned int offset, uint32_t len); - void unpackRobotMessageVersion(uint8_t* buf, unsigned int offset, - uint32_t len); - void unpackRobotState(uint8_t* buf, unsigned int offset, uint32_t len); - void unpackRobotStateMasterboard(uint8_t* buf, unsigned int offset); - void unpackRobotMode(uint8_t* buf, unsigned int offset); + RobotState(std::condition_variable& msg_cond); + ~RobotState(); + double getVersion(); + double getTime(); + std::vector getQTarget(); + int getDigitalInputBits(); + int getDigitalOutputBits(); + char getAnalogInputRange0(); + char getAnalogInputRange1(); + double getAnalogInput0(); + double getAnalogInput1(); + char getAnalogOutputDomain0(); + char getAnalogOutputDomain1(); + double getAnalogOutput0(); + double getAnalogOutput1(); + std::vector getVActual(); + float getMasterBoardTemperature(); + float getRobotVoltage48V(); + float getRobotCurrent(); + float getMasterIOCurrent(); + unsigned char getSafetyMode(); + unsigned char getInReducedMode(); + char getEuromap67InterfaceInstalled(); + int getEuromapInputBits(); + int getEuromapOutputBits(); + float getEuromapVoltage(); + float getEuromapCurrent(); + + bool isRobotConnected(); + bool isRealRobotEnabled(); + bool isPowerOnRobot(); + bool isEmergencyStopped(); + bool isProtectiveStopped(); + bool isProgramRunning(); + bool isProgramPaused(); + unsigned char getRobotMode(); + bool isReady(); + + void setDisconnected(); + + bool getNewDataAvailable(); + void finishedReading(); + + void unpack(uint8_t* buf, unsigned int buf_length); + void unpackRobotMessage(uint8_t* buf, unsigned int offset, uint32_t len); + void unpackRobotMessageVersion(uint8_t* buf, unsigned int offset, uint32_t len); + void unpackRobotState(uint8_t* buf, unsigned int offset, uint32_t len); + void unpackRobotStateMasterboard(uint8_t* buf, unsigned int offset); + void unpackRobotMode(uint8_t* buf, unsigned int offset); }; #endif /* ROBOT_STATE_H_ */ diff --git a/include/ur_modern_driver/robot_state_RT.h b/include/ur_modern_driver/robot_state_RT.h index 3d3b2e890..bca6faca2 100644 --- a/include/ur_modern_driver/robot_state_RT.h +++ b/include/ur_modern_driver/robot_state_RT.h @@ -19,98 +19,101 @@ #ifndef ROBOT_STATE_RT_H_ #define ROBOT_STATE_RT_H_ -#include #include -#include #include #include #include +#include +#include #include -class RobotStateRT { +class RobotStateRT +{ private: - double version_; //protocol version + double version_; // protocol version - double time_; //Time elapsed since the controller was started - std::vector q_target_; //Target joint positions - std::vector qd_target_; //Target joint velocities - std::vector qdd_target_; //Target joint accelerations - std::vector i_target_; //Target joint currents - std::vector m_target_; //Target joint moments (torques) - std::vector q_actual_; //Actual joint positions - std::vector qd_actual_; //Actual joint velocities - std::vector i_actual_; //Actual joint currents - std::vector i_control_; //Joint control currents - std::vector tool_vector_actual_; //Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation - std::vector tcp_speed_actual_; //Actual speed of the tool given in Cartesian coordinates - std::vector tcp_force_; //Generalised forces in the TC - std::vector tool_vector_target_; //Target Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation - std::vector tcp_speed_target_; //Target speed of the tool given in Cartesian coordinates - std::vector digital_input_bits_; //Current state of the digital inputs. NOTE: these are bits encoded as int64_t, e.g. a value of 5 corresponds to bit 0 and bit 2 set high - std::vector motor_temperatures_; //Temperature of each joint in degrees celsius - double controller_timer_; //Controller realtime thread execution time - double robot_mode_; //Robot mode - std::vector joint_modes_; //Joint control modes - double safety_mode_; //Safety mode - std::vector tool_accelerometer_values_; //Tool x,y and z accelerometer values (software version 1.7) - double speed_scaling_; //Speed scaling of the trajectory limiter - double linear_momentum_norm_; //Norm of Cartesian linear momentum - double v_main_; //Masterboard: Main voltage - double v_robot_; //Matorborad: Robot voltage (48V) - double i_robot_; //Masterboard: Robot current - std::vector v_actual_; //Actual joint voltages + double time_; // Time elapsed since the controller was started + std::vector q_target_; // Target joint positions + std::vector qd_target_; // Target joint velocities + std::vector qdd_target_; // Target joint accelerations + std::vector i_target_; // Target joint currents + std::vector m_target_; // Target joint moments (torques) + std::vector q_actual_; // Actual joint positions + std::vector qd_actual_; // Actual joint velocities + std::vector i_actual_; // Actual joint currents + std::vector i_control_; // Joint control currents + std::vector tool_vector_actual_; // Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry + // and rz is a rotation vector representation of the tool orientation + std::vector tcp_speed_actual_; // Actual speed of the tool given in Cartesian coordinates + std::vector tcp_force_; // Generalised forces in the TC + std::vector tool_vector_target_; // Target Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry + // and rz is a rotation vector representation of the tool orientation + std::vector tcp_speed_target_; // Target speed of the tool given in Cartesian coordinates + std::vector digital_input_bits_; // Current state of the digital inputs. NOTE: these are bits encoded as + // int64_t, e.g. a value of 5 corresponds to bit 0 and bit 2 set high + std::vector motor_temperatures_; // Temperature of each joint in degrees celsius + double controller_timer_; // Controller realtime thread execution time + double robot_mode_; // Robot mode + std::vector joint_modes_; // Joint control modes + double safety_mode_; // Safety mode + std::vector tool_accelerometer_values_; // Tool x,y and z accelerometer values (software version 1.7) + double speed_scaling_; // Speed scaling of the trajectory limiter + double linear_momentum_norm_; // Norm of Cartesian linear momentum + double v_main_; // Masterboard: Main voltage + double v_robot_; // Matorborad: Robot voltage (48V) + double i_robot_; // Masterboard: Robot current + std::vector v_actual_; // Actual joint voltages - std::mutex val_lock_; // Locks the variables while unpack parses data; + std::mutex val_lock_; // Locks the variables while unpack parses data; - std::condition_variable* pMsg_cond_; //Signals that new vars are available - bool data_published_; //to avoid spurious wakes - bool controller_updated_; //to avoid spurious wakes + std::condition_variable* pMsg_cond_; // Signals that new vars are available + bool data_published_; // to avoid spurious wakes + bool controller_updated_; // to avoid spurious wakes - std::vector unpackVector(uint8_t* buf, int start_index, - int nr_of_vals); - std::vector unpackDigitalInputBits(int64_t data); - double ntohd(uint64_t nf); + std::vector unpackVector(uint8_t* buf, int start_index, int nr_of_vals); + std::vector unpackDigitalInputBits(int64_t data); + double ntohd(uint64_t nf); public: - RobotStateRT(std::condition_variable& msg_cond); - ~RobotStateRT(); - double getVersion(); - double getTime(); - std::vector getQTarget(); - std::vector getQdTarget(); - std::vector getQddTarget(); - std::vector getITarget(); - std::vector getMTarget(); - std::vector getQActual(); - std::vector getQdActual(); - std::vector getIActual(); - std::vector getIControl(); - std::vector getToolVectorActual(); - std::vector getTcpSpeedActual(); - std::vector getTcpForce(); - std::vector getToolVectorTarget(); - std::vector getTcpSpeedTarget(); - std::vector getDigitalInputBits(); - std::vector getMotorTemperatures(); - double getControllerTimer(); - double getRobotMode(); - std::vector getJointModes(); - double getSafety_mode(); - std::vector getToolAccelerometerValues(); - double getSpeedScaling(); - double getLinearMomentumNorm(); - double getVMain(); - double getVRobot(); - double getIRobot(); + RobotStateRT(std::condition_variable& msg_cond); + ~RobotStateRT(); + double getVersion(); + double getTime(); + std::vector getQTarget(); + std::vector getQdTarget(); + std::vector getQddTarget(); + std::vector getITarget(); + std::vector getMTarget(); + std::vector getQActual(); + std::vector getQdActual(); + std::vector getIActual(); + std::vector getIControl(); + std::vector getToolVectorActual(); + std::vector getTcpSpeedActual(); + std::vector getTcpForce(); + std::vector getToolVectorTarget(); + std::vector getTcpSpeedTarget(); + std::vector getDigitalInputBits(); + std::vector getMotorTemperatures(); + double getControllerTimer(); + double getRobotMode(); + std::vector getJointModes(); + double getSafety_mode(); + std::vector getToolAccelerometerValues(); + double getSpeedScaling(); + double getLinearMomentumNorm(); + double getVMain(); + double getVRobot(); + double getIRobot(); - void setVersion(double ver); + void setVersion(double ver); - void setDataPublished(); - bool getDataPublished(); - bool getControllerUpdated(); - void setControllerUpdated(); - std::vector getVActual(); - void unpack(uint8_t* buf); + void setDataPublished(); + bool getDataPublished(); + bool getControllerUpdated(); + void setControllerUpdated(); + std::vector getVActual(); + void unpack(uint8_t* buf); }; #endif /* ROBOT_STATE_RT_H_ */ diff --git a/include/ur_modern_driver/ros/rt_publisher.h b/include/ur_modern_driver/ros/rt_publisher.h index 087052657..a3fbb786d 100644 --- a/include/ur_modern_driver/ros/rt_publisher.h +++ b/include/ur_modern_driver/ros/rt_publisher.h @@ -1,6 +1,5 @@ #pragma once -#include #include #include #include @@ -9,6 +8,7 @@ #include #include #include +#include #include #include "ur_modern_driver/ur/consumer.h" @@ -16,55 +16,57 @@ using namespace ros; using namespace tf; -const std::string JOINTS[] = { - "shoulder_pan_joint", - "shoulder_lift_joint", - "elbow_joint", - "wrist_1_joint", - "wrist_2_joint", - "wrist_3_joint" -}; +const std::string JOINTS[] = { "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", + "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" }; -class RTPublisher : public URRTPacketConsumer { +class RTPublisher : public URRTPacketConsumer +{ private: - NodeHandle _nh; - Publisher _joint_pub; - Publisher _wrench_pub; - Publisher _tool_vel_pub; - Publisher _joint_temperature_pub; - TransformBroadcaster _transform_broadcaster; - std::vector _joint_names; - std::string _base_frame; - std::string _tool_frame; + NodeHandle nh_; + Publisher joint_pub_; + Publisher wrench_pub_; + Publisher tool_vel_pub_; + Publisher joint_temperature_pub_; + TransformBroadcaster transform_broadcaster_; + std::vector joint_names_; + std::string base_frame_; + std::string tool_frame_; - bool publish_joints(RTShared& packet, Time& t); - bool publish_wrench(RTShared& packet, Time& t); - bool publish_tool(RTShared& packet, Time& t); - bool publish_transform(RTShared& packet, Time& t); - bool publish_temperature(RTShared& packet, Time& t); + bool publishJoints(RTShared& packet, Time& t); + bool publishWrench(RTShared& packet, Time& t); + bool publishTool(RTShared& packet, Time& t); + bool publishTransform(RTShared& packet, Time& t); + bool publishTemperature(RTShared& packet, Time& t); - bool publish(RTShared& packet); + bool publish(RTShared& packet); public: - RTPublisher(std::string& joint_prefix, std::string& base_frame, std::string& tool_frame) - : _joint_pub(_nh.advertise("joint_states", 1)) - , _wrench_pub(_nh.advertise("wrench", 1)) - , _tool_vel_pub(_nh.advertise("tool_velocity", 1)) - , _joint_temperature_pub(_nh.advertise("joint_temperature", 1)) - , _base_frame(base_frame) - , _tool_frame(tool_frame) + RTPublisher(std::string& joint_prefix, std::string& base_frame, std::string& tool_frame) + : joint_pub_(nh_.advertise("joint_states", 1)) + , wrench_pub_(nh_.advertise("wrench", 1)) + , tool_vel_pub_(nh_.advertise("tool_velocity", 1)) + , joint_temperature_pub_(nh_.advertise("joint_temperature", 1)) + , base_frame_(base_frame) + , tool_frame_(tool_frame) + { + for (auto const& j : JOINTS) { - for (auto const& j : JOINTS) { - _joint_names.push_back(joint_prefix + j); - } + joint_names_.push_back(joint_prefix + j); } + } - virtual bool consume(RTState_V1_6__7& state); - virtual bool consume(RTState_V1_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 setupConsumer() + { + } + virtual void teardownConsumer() + { + } + virtual void stopConsumer() + { + } }; \ No newline at end of file diff --git a/include/ur_modern_driver/types.h b/include/ur_modern_driver/types.h index 86d3a244e..34cb905df 100644 --- a/include/ur_modern_driver/types.h +++ b/include/ur_modern_driver/types.h @@ -2,21 +2,23 @@ #include -struct double3_t { - double x, y, z; +struct double3_t +{ + double x, y, z; }; -struct cartesian_coord_t { - double3_t position; - double3_t rotation; +struct cartesian_coord_t +{ + double3_t position; + double3_t rotation; }; inline bool operator==(const double3_t& lhs, const double3_t& rhs) { - return lhs.x == rhs.x && lhs.y == rhs.y && lhs.z == rhs.z; + return lhs.x == rhs.x && lhs.y == rhs.y && lhs.z == rhs.z; } inline bool operator==(const cartesian_coord_t& lhs, const cartesian_coord_t& rhs) { - return lhs.position == rhs.position && lhs.rotation == rhs.rotation; + return lhs.position == rhs.position && lhs.rotation == rhs.rotation; } \ No newline at end of file diff --git a/include/ur_modern_driver/ur/consumer.h b/include/ur_modern_driver/ur/consumer.h index 655e8bbe9..991401ad4 100644 --- a/include/ur_modern_driver/ur/consumer.h +++ b/include/ur_modern_driver/ur/consumer.h @@ -7,41 +7,44 @@ #include "ur_modern_driver/ur/rt_state.h" #include "ur_modern_driver/ur/state.h" -class URRTPacketConsumer : public IConsumer { +class URRTPacketConsumer : public IConsumer +{ public: - virtual bool consume(unique_ptr packet) - { - return packet->consume_with(*this); - } - - virtual bool consume(RTState_V1_6__7& state) = 0; - virtual bool consume(RTState_V1_8& state) = 0; - virtual bool consume(RTState_V3_0__1& state) = 0; - virtual bool consume(RTState_V3_2__3& state) = 0; + virtual bool consume(unique_ptr packet) + { + return packet->consumeWith(*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; }; -class URStatePacketConsumer : public IConsumer { +class URStatePacketConsumer : public IConsumer +{ public: - virtual bool consume(unique_ptr packet) - { - return packet->consume_with(*this); - } - - virtual bool consume(MasterBoardData_V1_X& data) = 0; - virtual bool consume(MasterBoardData_V3_0__1& data) = 0; - virtual bool consume(MasterBoardData_V3_2& data) = 0; - - virtual bool consume(RobotModeData_V1_X& data) = 0; - virtual bool consume(RobotModeData_V3_0__1& data) = 0; - virtual bool consume(RobotModeData_V3_2& data) = 0; + virtual bool consume(unique_ptr packet) + { + return packet->consumeWith(*this); + } + + virtual bool consume(MasterBoardData_V1_X& data) = 0; + virtual bool consume(MasterBoardData_V3_0__1& data) = 0; + virtual bool consume(MasterBoardData_V3_2& data) = 0; + + virtual bool consume(RobotModeData_V1_X& data) = 0; + virtual bool consume(RobotModeData_V3_0__1& data) = 0; + virtual bool consume(RobotModeData_V3_2& data) = 0; }; -class URMessagePacketConsumer : public IConsumer { +class URMessagePacketConsumer : public IConsumer +{ public: - virtual bool consume(unique_ptr packet) - { - return packet->consume_with(*this); - } + virtual bool consume(unique_ptr packet) + { + return packet->consumeWith(*this); + } - virtual bool consume(VersionMessage& message) = 0; + virtual bool consume(VersionMessage& message) = 0; }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/factory.h b/include/ur_modern_driver/ur/factory.h index d3266ea29..83d89abb5 100644 --- a/include/ur_modern_driver/ur/factory.h +++ b/include/ur_modern_driver/ur/factory.h @@ -1,5 +1,6 @@ #pragma once +#include #include "ur_modern_driver/ur/consumer.h" #include "ur_modern_driver/ur/messages_parser.h" #include "ur_modern_driver/ur/parser.h" @@ -7,83 +8,97 @@ #include "ur_modern_driver/ur/rt_parser.h" #include "ur_modern_driver/ur/state_parser.h" #include "ur_modern_driver/ur/stream.h" -#include -class URFactory : private URMessagePacketConsumer { +class URFactory : private URMessagePacketConsumer +{ private: - URStream _stream; - URMessageParser _parser; + URStream stream_; + URMessageParser parser_; - uint8_t _major_version; - uint8_t _minor_version; + uint8_t major_version_; + uint8_t minor_version_; - bool consume(VersionMessage& vm) - { - 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); - LOG_INFO("build date: %s", vm.build_date.c_str()); + 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); + LOG_INFO("build date: %s", vm.build_date.c_str()); - _major_version = vm.major_version; - _minor_version = vm.minor_version; + major_version_ = vm.major_version; + minor_version_ = vm.minor_version; - return true; - } + return true; + } - void setup_consumer() {} - void teardown_consumer() {} - void stop_consumer() {} + void setupConsumer() + { + } + void teardownConsumer() + { + } + void stopConsumer() + { + } public: - URFactory(std::string& host) - : _stream(host, 30001) - { - URProducer p(_stream, _parser); - std::vector > results; - - p.setup_producer(); + URFactory(std::string& host) : stream_(host, 30001) + { + URProducer prod(stream_, parser_); + std::vector> results; - if (!p.try_get(results) || results.size() == 0) { - LOG_FATAL("No version message received, init failed!"); - std::exit(EXIT_FAILURE); - } + prod.setupProducer(); - for (auto const& p : results) { - p->consume_with(*this); - } + if (!prod.tryGet(results) || results.size() == 0) + { + LOG_FATAL("No version message received, init failed!"); + std::exit(EXIT_FAILURE); + } - if (_major_version == 0 && _minor_version == 0) { - LOG_FATAL("No version message received, init failed!"); - std::exit(EXIT_FAILURE); - } + for (auto const& p : results) + { + p->consumeWith(*this); + } - p.teardown_producer(); + if (major_version_ == 0 && minor_version_ == 0) + { + LOG_FATAL("No version message received, init failed!"); + std::exit(EXIT_FAILURE); } - std::unique_ptr > get_state_parser() + prod.teardownProducer(); + } + + std::unique_ptr> getStateParser() + { + if (major_version_ == 1) { - if (_major_version == 1) { - return std::unique_ptr >(new URStateParser_V1_X); - } else { - if (_minor_version < 3) - return std::unique_ptr >(new URStateParser_V3_0__1); - else - return std::unique_ptr >(new URStateParser_V3_2); - } + return std::unique_ptr>(new URStateParser_V1_X); } + else + { + if (minor_version_ < 3) + return std::unique_ptr>(new URStateParser_V3_0__1); + else + return std::unique_ptr>(new URStateParser_V3_2); + } + } - std::unique_ptr > get_rt_parser() + std::unique_ptr> getRTParser() + { + if (major_version_ == 1) + { + if (minor_version_ < 8) + return std::unique_ptr>(new URRTStateParser_V1_6__7); + else + return std::unique_ptr>(new URRTStateParser_V1_8); + } + else { - if (_major_version == 1) { - if (_minor_version < 8) - return std::unique_ptr >(new URRTStateParser_V1_6__7); - else - return std::unique_ptr >(new URRTStateParser_V1_8); - } else { - if (_minor_version < 3) - return std::unique_ptr >(new URRTStateParser_V3_0__1); - else - return std::unique_ptr >(new URRTStateParser_V3_2__3); - } + if (minor_version_ < 3) + return std::unique_ptr>(new URRTStateParser_V3_0__1); + else + return std::unique_ptr>(new URRTStateParser_V3_2__3); } + } }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/master_board.h b/include/ur_modern_driver/ur/master_board.h index 5f2339707..a0d9e7a6c 100644 --- a/include/ur_modern_driver/ur/master_board.h +++ b/include/ur_modern_driver/ur/master_board.h @@ -1,97 +1,91 @@ #pragma once +#include +#include #include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/types.h" #include "ur_modern_driver/ur/state.h" -#include -#include -class SharedMasterBoardData { +class SharedMasterBoardData +{ public: - virtual bool parse_with(BinParser& bp); - - int8_t analog_input_range0; - int8_t analog_input_range1; - double analog_input0; - double analog_input1; - int8_t analog_output_domain0; - int8_t analog_output_domain1; - double analog_output0; - double analog_output1; - float master_board_temperature; - float robot_voltage_48V; - float robot_current; - float master_IO_current; - - bool euromap67_interface_installed; - - //euromap fields are dynamic so don't include in SIZE - int32_t euromap_input_bits; - int32_t euromap_output_bits; - - static const size_t SIZE = sizeof(double) * 4 - + sizeof(int8_t) * 4 - + sizeof(float) * 4 - + sizeof(uint8_t); - - static const size_t EURO_SIZE = sizeof(int32_t) * 2; + virtual bool parseWith(BinParser& bp); + + int8_t analog_input_range0; + int8_t analog_input_range1; + double analog_input0; + double analog_input1; + int8_t analog_output_domain0; + int8_t analog_output_domain1; + double analog_output0; + double analog_output1; + float master_board_temperature; + float robot_voltage_48V; + float robot_current; + float master_IO_current; + + bool euromap67_interface_installed; + + // euromap fields are dynamic so don't include in SIZE + int32_t euromap_input_bits; + int32_t euromap_output_bits; + + static const size_t SIZE = sizeof(double) * 4 + sizeof(int8_t) * 4 + sizeof(float) * 4 + sizeof(uint8_t); + + static const size_t EURO_SIZE = sizeof(int32_t) * 2; }; -class MasterBoardData_V1_X : public SharedMasterBoardData, public StatePacket { +class MasterBoardData_V1_X : public SharedMasterBoardData, public StatePacket +{ public: - virtual bool parse_with(BinParser& bp); - virtual bool consume_with(URStatePacketConsumer& consumer); + virtual bool parseWith(BinParser& bp); + virtual bool consumeWith(URStatePacketConsumer& consumer); - int16_t digital_input_bits; - int16_t digital_output_bits; + int16_t digital_input_bits; + int16_t digital_output_bits; - uint8_t master_safety_state; - bool master_on_off_state; + uint8_t master_safety_state; + bool master_on_off_state; - //euromap fields are dynamic so don't include in SIZE - int16_t euromap_voltage; - int16_t euromap_current; + // euromap fields are dynamic so don't include in SIZE + int16_t euromap_voltage; + int16_t euromap_current; - static const size_t SIZE = SharedMasterBoardData::SIZE - + sizeof(int16_t) * 2 - + sizeof(uint8_t) * 2; + static const size_t SIZE = SharedMasterBoardData::SIZE + sizeof(int16_t) * 2 + sizeof(uint8_t) * 2; - static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE - + sizeof(int16_t) * 2; + static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE + sizeof(int16_t) * 2; }; -class MasterBoardData_V3_0__1 : public SharedMasterBoardData, public StatePacket { +class MasterBoardData_V3_0__1 : public SharedMasterBoardData, public StatePacket +{ public: - virtual bool parse_with(BinParser& bp); - virtual bool consume_with(URStatePacketConsumer& consumer); + virtual bool parseWith(BinParser& bp); + virtual bool consumeWith(URStatePacketConsumer& consumer); - int32_t digital_input_bits; - int32_t digital_output_bits; + int32_t digital_input_bits; + int32_t digital_output_bits; - uint8_t safety_mode; - bool in_reduced_mode; + uint8_t safety_mode; + bool in_reduced_mode; - //euromap fields are dynamic so don't include in SIZE - float euromap_voltage; - float euromap_current; + // euromap fields are dynamic so don't include in SIZE + float euromap_voltage; + float euromap_current; - static const size_t SIZE = SharedMasterBoardData::SIZE - + sizeof(int32_t) * 2 - + sizeof(uint8_t) * 2 - + sizeof(uint32_t); //UR internal field we skip + static const size_t SIZE = SharedMasterBoardData::SIZE + sizeof(int32_t) * 2 + sizeof(uint8_t) * 2 + + sizeof(uint32_t); // UR internal field we skip - static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE - + sizeof(float) * 2; + static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE + sizeof(float) * 2; }; -class MasterBoardData_V3_2 : public MasterBoardData_V3_0__1 { +class MasterBoardData_V3_2 : public MasterBoardData_V3_0__1 +{ public: - virtual bool parse_with(BinParser& bp); - virtual bool consume_with(URStatePacketConsumer& consumer); + virtual bool parseWith(BinParser& bp); + virtual bool consumeWith(URStatePacketConsumer& consumer); - uint8_t operational_mode_selector_input; - uint8_t three_position_enabling_device_input; + uint8_t operational_mode_selector_input; + uint8_t three_position_enabling_device_input; - static const size_t SIZE = MasterBoardData_V3_0__1::SIZE - + sizeof(uint8_t) * 2; + static const size_t SIZE = MasterBoardData_V3_0__1::SIZE + sizeof(uint8_t) * 2; }; diff --git a/include/ur_modern_driver/ur/messages.h b/include/ur_modern_driver/ur/messages.h index dafad611f..b693c1e63 100644 --- a/include/ur_modern_driver/ur/messages.h +++ b/include/ur_modern_driver/ur/messages.h @@ -1,51 +1,51 @@ #pragma once +#include +#include #include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/pipeline.h" -#include -#include -enum class robot_message_type : uint8_t { - ROBOT_MESSAGE_TEXT = 0, - ROBOT_MESSAGE_PROGRAM_LABEL = 1, - PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2, - ROBOT_MESSAGE_VERSION = 3, - ROBOT_MESSAGE_SAFETY_MODE = 5, - ROBOT_MESSAGE_ERROR_CODE = 6, - ROBOT_MESSAGE_KEY = 7, - ROBOT_MESSAGE_REQUEST_VALUE = 9, - ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10 +enum class robot_message_type : uint8_t +{ + ROBOT_MESSAGE_TEXT = 0, + ROBOT_MESSAGE_PROGRAM_LABEL = 1, + PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2, + ROBOT_MESSAGE_VERSION = 3, + ROBOT_MESSAGE_SAFETY_MODE = 5, + ROBOT_MESSAGE_ERROR_CODE = 6, + ROBOT_MESSAGE_KEY = 7, + ROBOT_MESSAGE_REQUEST_VALUE = 9, + ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10 }; class URMessagePacketConsumer; -class MessagePacket { +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; - - uint64_t timestamp; - uint8_t source; + MessagePacket(uint64_t timestamp, uint8_t source) : timestamp(timestamp), source(source) + { + } + virtual bool parseWith(BinParser& bp) = 0; + virtual bool consumeWith(URMessagePacketConsumer& consumer) = 0; + + uint64_t timestamp; + uint8_t source; }; -class VersionMessage : public MessagePacket { +class VersionMessage : public MessagePacket +{ public: - VersionMessage(uint64_t timestamp, uint8_t source) - : MessagePacket(timestamp, source) - { - } - - virtual bool parse_with(BinParser& bp); - virtual bool consume_with(URMessagePacketConsumer& consumer); - - std::string project_name; - uint8_t major_version; - uint8_t minor_version; - int32_t svn_version; - std::string build_date; + VersionMessage(uint64_t timestamp, uint8_t source) : MessagePacket(timestamp, source) + { + } + + virtual bool parseWith(BinParser& bp); + virtual bool consumeWith(URMessagePacketConsumer& consumer); + + std::string project_name; + uint8_t major_version; + uint8_t minor_version; + int32_t svn_version; + std::string build_date; }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/messages_parser.h b/include/ur_modern_driver/ur/messages_parser.h index b0a781f65..80d58060e 100644 --- a/include/ur_modern_driver/ur/messages_parser.h +++ b/include/ur_modern_driver/ur/messages_parser.h @@ -1,49 +1,53 @@ #pragma once +#include #include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/log.h" #include "ur_modern_driver/pipeline.h" #include "ur_modern_driver/ur/messages.h" #include "ur_modern_driver/ur/parser.h" -#include -class URMessageParser : public URParser { +class URMessageParser : public URParser +{ public: - bool parse(BinParser& bp, std::vector >& results) + bool parse(BinParser& bp, std::vector>& results) + { + int32_t packet_size; + message_type type; + bp.parse(packet_size); + bp.parse(type); + + if (type != message_type::ROBOT_MESSAGE) { - int32_t packet_size; - message_type type; - bp.parse(packet_size); - bp.parse(type); - - if (type != message_type::ROBOT_MESSAGE) { - LOG_WARN("Invalid message type recieved: %u", static_cast(type)); - return false; - } - - uint64_t timestamp; - uint8_t source; - robot_message_type message_type; - - bp.parse(timestamp); - bp.parse(source); - bp.parse(message_type); - - std::unique_ptr result; - bool parsed = false; - - switch (message_type) { - case robot_message_type::ROBOT_MESSAGE_VERSION: { - VersionMessage* vm = new VersionMessage(timestamp, source); - parsed = vm->parse_with(bp); - result.reset(vm); - break; - } - - default: - return false; - } - - results.push_back(std::move(result)); - return parsed; + LOG_WARN("Invalid message type recieved: %u", static_cast(type)); + return false; } + + uint64_t timestamp; + uint8_t source; + robot_message_type message_type; + + bp.parse(timestamp); + bp.parse(source); + bp.parse(message_type); + + std::unique_ptr result; + bool parsed = false; + + switch (message_type) + { + case robot_message_type::ROBOT_MESSAGE_VERSION: + { + VersionMessage* vm = new VersionMessage(timestamp, source); + parsed = vm->parseWith(bp); + result.reset(vm); + break; + } + + default: + return false; + } + + results.push_back(std::move(result)); + return parsed; + } }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/parser.h b/include/ur_modern_driver/ur/parser.h index ea53b401b..4eb90261f 100644 --- a/include/ur_modern_driver/ur/parser.h +++ b/include/ur_modern_driver/ur/parser.h @@ -1,10 +1,11 @@ #pragma once +#include #include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/pipeline.h" -#include template -class URParser { +class URParser +{ public: - virtual bool parse(BinParser& bp, std::vector >& results) = 0; + virtual bool parse(BinParser& bp, std::vector>& results) = 0; }; diff --git a/include/ur_modern_driver/ur/producer.h b/include/ur_modern_driver/ur/producer.h index 7db11c9f3..5907f976b 100644 --- a/include/ur_modern_driver/ur/producer.h +++ b/include/ur_modern_driver/ur/producer.h @@ -4,41 +4,52 @@ #include "ur_modern_driver/ur/stream.h" template -class URProducer : public IProducer { +class URProducer : public IProducer +{ private: - URStream& _stream; - URParser& _parser; + URStream& stream_; + URParser& parser_; public: - URProducer(URStream& stream, URParser& parser) - : _stream(stream) - , _parser(parser) + URProducer(URStream& stream, URParser& parser) : stream_(stream), parser_(parser) + { + } + + void setupProducer() + { + stream_.connect(); + } + void teardownProducer() + { + stream_.disconnect(); + } + void stopProducer() + { + stream_.disconnect(); + } + + bool tryGet(std::vector>& products) + { + // 4KB should be enough to hold any packet received from UR + uint8_t buf[4096]; + + // blocking call + ssize_t len = stream_.receive(buf, sizeof(buf)); + + // LOG_DEBUG("Read %d bytes from stream", len); + + if (len == 0) { + LOG_WARN("Read nothing from stream"); + return false; } - - void setup_producer() { _stream.connect(); } - void teardown_producer() { _stream.disconnect(); } - void stop_producer() { _stream.disconnect(); } - - bool try_get(std::vector >& products) + else if (len < 0) { - //4KB should be enough to hold any packet received from UR - uint8_t buf[4096]; - - //blocking call - ssize_t len = _stream.receive(buf, sizeof(buf)); - - //LOG_DEBUG("Read %d bytes from stream", len); - - if (len == 0) { - LOG_WARN("Read nothing from stream"); - return false; - } else if (len < 0) { - LOG_WARN("Stream closed"); - return false; - } - - BinParser bp(buf, static_cast(len)); - return _parser.parse(bp, products); + LOG_WARN("Stream closed"); + return false; } + + BinParser bp(buf, static_cast(len)); + return parser_.parse(bp, products); + } }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/robot_mode.h b/include/ur_modern_driver/ur/robot_mode.h index b5307b9d8..90c9eb8a0 100644 --- a/include/ur_modern_driver/ur/robot_mode.h +++ b/include/ur_modern_driver/ur/robot_mode.h @@ -1,108 +1,107 @@ #pragma once +#include +#include #include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/types.h" #include "ur_modern_driver/ur/state.h" -#include -#include -class SharedRobotModeData { +class SharedRobotModeData +{ public: - virtual bool parse_with(BinParser& bp); + virtual bool parseWith(BinParser& bp); - uint64_t timestamp; - bool physical_robot_connected; - bool real_robot_enabled; - bool robot_power_on; - bool emergency_stopped; - bool program_running; - bool program_paused; + uint64_t timestamp; + bool physical_robot_connected; + bool real_robot_enabled; + bool robot_power_on; + bool emergency_stopped; + bool program_running; + bool program_paused; - static const size_t SIZE = sizeof(uint64_t) + sizeof(uint8_t) * 6; + static const size_t SIZE = sizeof(uint64_t) + sizeof(uint8_t) * 6; }; -enum class robot_mode_V1_X : uint8_t { - ROBOT_RUNNING_MODE = 0, - ROBOT_FREEDRIVE_MODE = 1, - ROBOT_READY_MODE = 2, - ROBOT_INITIALIZING_MODE = 3, - ROBOT_SECURITY_STOPPED_MODE = 4, - ROBOT_EMERGENCY_STOPPED_MODE = 5, - ROBOT_FATAL_ERROR_MODE = 6, - ROBOT_NO_POWER_MODE = 7, - ROBOT_NOT_CONNECTED_MODE = 8, - ROBOT_SHUTDOWN_MODE = 9, - ROBOT_SAFEGUARD_STOP_MODE = 10 +enum class robot_mode_V1_X : uint8_t +{ + ROBOT_RUNNING_MODE = 0, + ROBOT_FREEDRIVE_MODE = 1, + ROBOT_READY_MODE = 2, + ROBOT_INITIALIZING_MODE = 3, + ROBOT_SECURITY_STOPPED_MODE = 4, + ROBOT_EMERGENCY_STOPPED_MODE = 5, + ROBOT_FATAL_ERROR_MODE = 6, + ROBOT_NO_POWER_MODE = 7, + ROBOT_NOT_CONNECTED_MODE = 8, + ROBOT_SHUTDOWN_MODE = 9, + ROBOT_SAFEGUARD_STOP_MODE = 10 }; -class RobotModeData_V1_X : public SharedRobotModeData, public StatePacket { +class RobotModeData_V1_X : public SharedRobotModeData, public StatePacket +{ public: - virtual bool parse_with(BinParser& bp); - virtual bool consume_with(URStatePacketConsumer& consumer); + virtual bool parseWith(BinParser& bp); + virtual bool consumeWith(URStatePacketConsumer& consumer); - bool security_stopped; - robot_mode_V1_X robot_mode; - double speed_fraction; + bool security_stopped; + robot_mode_V1_X robot_mode; + double speed_fraction; - static const size_t SIZE = SharedRobotModeData::SIZE - + sizeof(uint8_t) - + sizeof(robot_mode_V1_X) - + sizeof(double); + static const size_t SIZE = SharedRobotModeData::SIZE + sizeof(uint8_t) + sizeof(robot_mode_V1_X) + sizeof(double); - static_assert(RobotModeData_V1_X::SIZE == 24, "RobotModeData_V1_X has missmatched size"); + static_assert(RobotModeData_V1_X::SIZE == 24, "RobotModeData_V1_X has missmatched size"); }; -enum class robot_mode_V3_X : uint8_t { - DISCONNECTED = 0, - CONFIRM_SAFETY = 1, - BOOTING = 2, - POWER_OFF = 3, - POWER_ON = 4, - IDLE = 5, - BACKDRIVE = 6, - RUNNING = 7, - UPDATING_FIRMWARE = 8 +enum class robot_mode_V3_X : uint8_t +{ + DISCONNECTED = 0, + CONFIRM_SAFETY = 1, + BOOTING = 2, + POWER_OFF = 3, + POWER_ON = 4, + IDLE = 5, + BACKDRIVE = 6, + RUNNING = 7, + UPDATING_FIRMWARE = 8 }; -enum class robot_control_mode_V3_X : uint8_t { - POSITION = 0, - TEACH = 1, - FORCE = 2, - TORQUE = 3 +enum class robot_control_mode_V3_X : uint8_t +{ + POSITION = 0, + TEACH = 1, + FORCE = 2, + TORQUE = 3 }; -class RobotModeData_V3_0__1 : public SharedRobotModeData, public StatePacket { +class RobotModeData_V3_0__1 : public SharedRobotModeData, public StatePacket +{ public: - virtual bool parse_with(BinParser& bp); - virtual bool consume_with(URStatePacketConsumer& consumer); + virtual bool parseWith(BinParser& bp); + virtual bool consumeWith(URStatePacketConsumer& consumer); - bool protective_stopped; + bool protective_stopped; - robot_mode_V3_X robot_mode; - robot_control_mode_V3_X control_mode; + robot_mode_V3_X robot_mode; + robot_control_mode_V3_X control_mode; - double target_speed_fraction; - double speed_scaling; + double target_speed_fraction; + double speed_scaling; - static const size_t SIZE = SharedRobotModeData::SIZE - + sizeof(uint8_t) - + sizeof(robot_mode_V3_X) - + sizeof(robot_control_mode_V3_X) - + sizeof(double) - + sizeof(double); + static const size_t SIZE = SharedRobotModeData::SIZE + sizeof(uint8_t) + sizeof(robot_mode_V3_X) + + sizeof(robot_control_mode_V3_X) + sizeof(double) + sizeof(double); - static_assert(RobotModeData_V3_0__1::SIZE == 33, "RobotModeData_V3_0__1 has missmatched size"); + static_assert(RobotModeData_V3_0__1::SIZE == 33, "RobotModeData_V3_0__1 has missmatched size"); }; -class RobotModeData_V3_2 : public RobotModeData_V3_0__1 { +class RobotModeData_V3_2 : public RobotModeData_V3_0__1 +{ public: - virtual bool parse_with(BinParser& bp); - virtual bool consume_with(URStatePacketConsumer& consumer); + virtual bool parseWith(BinParser& bp); + virtual bool consumeWith(URStatePacketConsumer& consumer); - double target_speed_fraction_limit; + double target_speed_fraction_limit; - static const size_t SIZE = RobotModeData_V3_0__1::SIZE - + sizeof(double); + static const size_t SIZE = RobotModeData_V3_0__1::SIZE + sizeof(double); - static_assert(RobotModeData_V3_2::SIZE == 41, "RobotModeData_V3_2 has missmatched size"); + static_assert(RobotModeData_V3_2::SIZE == 41, "RobotModeData_V3_2 has missmatched size"); }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/rt_parser.h b/include/ur_modern_driver/ur/rt_parser.h index e0cde0d56..952cd56ed 100644 --- a/include/ur_modern_driver/ur/rt_parser.h +++ b/include/ur_modern_driver/ur/rt_parser.h @@ -1,33 +1,35 @@ #pragma once +#include #include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/log.h" #include "ur_modern_driver/pipeline.h" #include "ur_modern_driver/ur/parser.h" #include "ur_modern_driver/ur/rt_state.h" -#include template -class URRTStateParser : public URParser { +class URRTStateParser : public URParser +{ public: - bool parse(BinParser& bp, std::vector >& results) - { - int32_t packet_size = bp.peek(); + bool parse(BinParser& bp, std::vector>& results) + { + int32_t packet_size = bp.peek(); - if (!bp.check_size(packet_size)) { - LOG_ERROR("Buffer len shorter than expected packet length"); - return false; - } + if (!bp.checkSize(packet_size)) + { + LOG_ERROR("Buffer len shorter than expected packet length"); + return false; + } - bp.parse(packet_size); //consumes the peeked data + bp.parse(packet_size); // consumes the peeked data - std::unique_ptr packet(new T); - if (!packet->parse_with(bp)) - return false; + std::unique_ptr packet(new T); + if (!packet->parseWith(bp)) + return false; - results.push_back(std::move(packet)); + results.push_back(std::move(packet)); - return true; - } + return true; + } }; typedef URRTStateParser URRTStateParser_V1_6__7; diff --git a/include/ur_modern_driver/ur/rt_state.h b/include/ur_modern_driver/ur/rt_state.h index ea2dd5e01..bb4e9e991 100644 --- a/include/ur_modern_driver/ur/rt_state.h +++ b/include/ur_modern_driver/ur/rt_state.h @@ -1,122 +1,119 @@ #pragma once +#include +#include #include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/pipeline.h" #include "ur_modern_driver/types.h" -#include -#include class URRTPacketConsumer; -class RTPacket { +class RTPacket +{ public: - virtual bool parse_with(BinParser& bp) = 0; - virtual bool consume_with(URRTPacketConsumer& consumer) = 0; + virtual bool parseWith(BinParser& bp) = 0; + virtual bool consumeWith(URRTPacketConsumer& consumer) = 0; }; -class RTShared { +class RTShared +{ protected: - void parse_shared1(BinParser& bp); - void parse_shared2(BinParser& bp); + void parse_shared1(BinParser& bp); + void parse_shared2(BinParser& bp); public: - double time; - double q_target[6]; - double qd_target[6]; - double qdd_target[6]; - double i_target[6]; - double m_target[6]; - double q_actual[6]; - double qd_actual[6]; - double i_actual[6]; - - //gap here depending on version - - double tcp_force[6]; - - //does not contain "_actual" postfix in V1_X but - //they're the same fields so share anyway - cartesian_coord_t tool_vector_actual; - cartesian_coord_t tcp_speed_actual; - - //gap here depending on version - - uint64_t digital_inputs; - double motor_temperatures[6]; - 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); + double time; + double q_target[6]; + double qd_target[6]; + double qdd_target[6]; + double i_target[6]; + double m_target[6]; + double q_actual[6]; + double qd_actual[6]; + double i_actual[6]; + + // gap here depending on version + + double tcp_force[6]; + + // does not contain "_actual" postfix in V1_X but + // they're the same fields so share anyway + cartesian_coord_t tool_vector_actual; + cartesian_coord_t tcp_speed_actual; + + // gap here depending on version + + uint64_t digital_inputs; + double motor_temperatures[6]; + 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 { +class RTState_V1_6__7 : public RTShared, public RTPacket +{ public: - bool parse_with(BinParser& bp); - virtual bool consume_with(URRTPacketConsumer& consumer); + bool parseWith(BinParser& bp); + virtual bool consumeWith(URRTPacketConsumer& consumer); - double3_t tool_accelerometer_values; + double3_t tool_accelerometer_values; - static const size_t SIZE = RTShared::SIZE - + sizeof(double3_t); + static const size_t SIZE = RTShared::SIZE + sizeof(double3_t); - static_assert(RTState_V1_6__7::SIZE == 632, "RTState_V1_6__7 has mismatched size!"); + static_assert(RTState_V1_6__7::SIZE == 632, "RTState_V1_6__7 has mismatched size!"); }; -class RTState_V1_8 : public RTState_V1_6__7 { +class RTState_V1_8 : public RTState_V1_6__7 +{ public: - bool parse_with(BinParser& bp); - virtual bool consume_with(URRTPacketConsumer& consumer); + bool parseWith(BinParser& bp); + virtual bool consumeWith(URRTPacketConsumer& consumer); - double joint_modes[6]; + double joint_modes[6]; - static const size_t SIZE = RTState_V1_6__7::SIZE - + sizeof(double[6]); + static const size_t SIZE = RTState_V1_6__7::SIZE + sizeof(double[6]); - static_assert(RTState_V1_8::SIZE == 680, "RTState_V1_8 has mismatched size!"); + static_assert(RTState_V1_8::SIZE == 680, "RTState_V1_8 has mismatched size!"); }; -class RTState_V3_0__1 : public RTShared, public RTPacket { +class RTState_V3_0__1 : public RTShared, public RTPacket +{ public: - bool parse_with(BinParser& bp); - virtual bool consume_with(URRTPacketConsumer& consumer); - - 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; - double speed_scaling; - double linear_momentum_norm; - double v_main; - double v_robot; - double i_robot; - double v_actual[6]; - - static const size_t SIZE = RTShared::SIZE - + sizeof(double[6]) * 3 - + sizeof(double3_t) - + sizeof(cartesian_coord_t) * 2 - + sizeof(double) * 6; - - static_assert(RTState_V3_0__1::SIZE == 920, "RTState_V3_0__1 has mismatched size!"); + bool parseWith(BinParser& bp); + virtual bool consumeWith(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; + double speed_scaling; + double linear_momentum_norm; + double v_main; + double v_robot; + double i_robot; + double v_actual[6]; + + static const size_t SIZE = + RTShared::SIZE + sizeof(double[6]) * 3 + sizeof(double3_t) + sizeof(cartesian_coord_t) * 2 + sizeof(double) * 6; + + static_assert(RTState_V3_0__1::SIZE == 920, "RTState_V3_0__1 has mismatched size!"); }; -class RTState_V3_2__3 : public RTState_V3_0__1 { +class RTState_V3_2__3 : public RTState_V3_0__1 +{ public: - bool parse_with(BinParser& bp); - virtual bool consume_with(URRTPacketConsumer& consumer); + bool parseWith(BinParser& bp); + virtual bool consumeWith(URRTPacketConsumer& consumer); - uint64_t digital_outputs; - double program_state; + uint64_t digital_outputs; + double program_state; - static const size_t SIZE = RTState_V3_0__1::SIZE - + sizeof(uint64_t) - + sizeof(double); + static const size_t SIZE = RTState_V3_0__1::SIZE + sizeof(uint64_t) + sizeof(double); - static_assert(RTState_V3_2__3::SIZE == 936, "RTState_V3_2__3 has mismatched size!"); + static_assert(RTState_V3_2__3::SIZE == 936, "RTState_V3_2__3 has mismatched size!"); }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/state.h b/include/ur_modern_driver/ur/state.h index 0ac8e691a..b76d34ef9 100644 --- a/include/ur_modern_driver/ur/state.h +++ b/include/ur_modern_driver/ur/state.h @@ -1,34 +1,37 @@ #pragma once +#include +#include #include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/log.h" #include "ur_modern_driver/pipeline.h" -#include -#include -enum class package_type : uint8_t { - ROBOT_MODE_DATA = 0, - JOINT_DATA = 1, - TOOL_DATA = 2, - MASTERBOARD_DATA = 3, - CARTESIAN_INFO = 4, - KINEMATICS_INFO = 5, - CONFIGURATION_DATA = 6, - FORCE_MODE_DATA = 7, - ADDITIONAL_INFO = 8, - CALIBRATION_DATA = 9 +enum class package_type : uint8_t +{ + ROBOT_MODE_DATA = 0, + JOINT_DATA = 1, + TOOL_DATA = 2, + MASTERBOARD_DATA = 3, + CARTESIAN_INFO = 4, + KINEMATICS_INFO = 5, + CONFIGURATION_DATA = 6, + FORCE_MODE_DATA = 7, + ADDITIONAL_INFO = 8, + CALIBRATION_DATA = 9 }; -enum class message_type : uint8_t { - ROBOT_STATE = 16, - ROBOT_MESSAGE = 20, - PROGRAM_STATE_MESSAGE = 25 +enum class message_type : uint8_t +{ + ROBOT_STATE = 16, + ROBOT_MESSAGE = 20, + PROGRAM_STATE_MESSAGE = 25 }; class URStatePacketConsumer; -class StatePacket { +class StatePacket +{ public: - virtual bool parse_with(BinParser& bp) = 0; - virtual bool consume_with(URStatePacketConsumer& consumer) = 0; + virtual bool parseWith(BinParser& bp) = 0; + virtual bool consumeWith(URStatePacketConsumer& consumer) = 0; }; diff --git a/include/ur_modern_driver/ur/state_parser.h b/include/ur_modern_driver/ur/state_parser.h index a68a6113c..031efb2e2 100644 --- a/include/ur_modern_driver/ur/state_parser.h +++ b/include/ur_modern_driver/ur/state_parser.h @@ -1,4 +1,5 @@ #pragma once +#include #include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/log.h" #include "ur_modern_driver/pipeline.h" @@ -6,76 +7,84 @@ #include "ur_modern_driver/ur/parser.h" #include "ur_modern_driver/ur/robot_mode.h" #include "ur_modern_driver/ur/state.h" -#include template -class URStateParser : public URParser { +class URStateParser : public URParser +{ private: - StatePacket* from_type(package_type type) + StatePacket* from_type(package_type type) + { + switch (type) { - switch (type) { - case package_type::ROBOT_MODE_DATA: - return new RMD; - case package_type::MASTERBOARD_DATA: - return new MBD; - default: - return nullptr; - } + case package_type::ROBOT_MODE_DATA: + return new RMD; + case package_type::MASTERBOARD_DATA: + return new MBD; + default: + return nullptr; } + } public: - bool parse(BinParser& bp, std::vector >& results) - { - int32_t packet_size; - message_type type; - bp.parse(packet_size); - bp.parse(type); - - if (type != message_type::ROBOT_STATE) { - LOG_WARN("Invalid message type recieved: %u", static_cast(type)); - return false; - } + bool parse(BinParser& bp, std::vector>& results) + { + int32_t packet_size; + message_type type; + bp.parse(packet_size); + bp.parse(type); - 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(); - if (!bp.check_size(static_cast(sub_size))) { - LOG_WARN("Invalid sub-package size of %" PRIu32 " received!", sub_size); - return false; - } + if (type != message_type::ROBOT_STATE) + { + LOG_WARN("Invalid message type recieved: %u", static_cast(type)); + return false; + } - //deconstruction of a sub parser will increment the position of the parent parser - BinParser sbp(bp, sub_size); - sbp.consume(sizeof(sub_size)); - package_type type; - sbp.parse(type); + while (!bp.empty()) + { + if (!bp.checkSize(sizeof(uint32_t))) + { + LOG_ERROR("Failed to read sub-package length, there's likely a parsing error"); + return false; + } + uint32_t sub_size = bp.peek(); + if (!bp.checkSize(static_cast(sub_size))) + { + LOG_WARN("Invalid sub-package size of %" PRIu32 " received!", sub_size); + return false; + } - std::unique_ptr packet(from_type(type)); + // deconstruction of a sub parser will increment the position of the parent parser + BinParser sbp(bp, sub_size); + sbp.consume(sizeof(sub_size)); + package_type type; + sbp.parse(type); - if (packet == nullptr) { - sbp.consume(); - LOG_INFO("Skipping sub-packet of type %d", type); - continue; - } + std::unique_ptr packet(from_type(type)); - if (!packet->parse_with(sbp)) { - LOG_ERROR("Sub-package parsing of type %d failed!", type); - return false; - } + if (packet == nullptr) + { + sbp.consume(); + LOG_INFO("Skipping sub-packet of type %d", type); + continue; + } - results.push_back(std::move(packet)); + if (!packet->parseWith(sbp)) + { + LOG_ERROR("Sub-package parsing of type %d failed!", type); + return false; + } - if (!sbp.empty()) { - LOG_ERROR("Sub-package was not parsed completely!"); - return false; - } - } + results.push_back(std::move(packet)); - return true; + if (!sbp.empty()) + { + LOG_ERROR("Sub-package was not parsed completely!"); + return false; + } } + + return true; + } }; typedef URStateParser URStateParser_V1_X; diff --git a/include/ur_modern_driver/ur/stream.h b/include/ur_modern_driver/ur/stream.h index 7d7363b4f..6990c4a6d 100644 --- a/include/ur_modern_driver/ur/stream.h +++ b/include/ur_modern_driver/ur/stream.h @@ -1,37 +1,34 @@ #pragma once -#include #include -#include #include #include +#include +#include /// Encapsulates a TCP socket -class URStream { +class URStream +{ private: - int _socket_fd = -1; - std::string _host; - int _port; + int socket_fd_ = -1; + std::string host_; + int port_; - std::atomic _initialized; - std::atomic _stopping; + std::atomic initialized_; + std::atomic 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() - { - disconnect(); - } + ~URStream() + { + disconnect(); + } - bool connect(); - void disconnect(); + bool connect(); + void disconnect(); - ssize_t send(uint8_t* buf, size_t buf_len); - ssize_t receive(uint8_t* buf, size_t buf_len); + ssize_t send(uint8_t* buf, size_t buf_len); + ssize_t receive(uint8_t* buf, size_t buf_len); }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur_communication.h b/include/ur_modern_driver/ur_communication.h index cad98e14e..29460c5a6 100644 --- a/include/ur_modern_driver/ur_communication.h +++ b/include/ur_modern_driver/ur_communication.h @@ -19,13 +19,7 @@ #ifndef UR_COMMUNICATION_H_ #define UR_COMMUNICATION_H_ -#include "do_output.h" -#include "robot_state.h" -#include -#include #include -#include -#include #include #include #include @@ -36,27 +30,34 @@ #include #include #include -#include #include +#include +#include +#include +#include +#include #include +#include "do_output.h" +#include "robot_state.h" -class UrCommunication { +class UrCommunication +{ private: - int pri_sockfd_, sec_sockfd_; - struct sockaddr_in pri_serv_addr_, sec_serv_addr_; - struct hostent* server_; - bool keepalive_; - std::thread comThread_; - int flag_; - void run(); + int pri_sockfd_, sec_sockfd_; + struct sockaddr_in pri_serv_addr_, sec_serv_addr_; + struct hostent* server_; + bool keepalive_; + std::thread comThread_; + int flag_; + void run(); public: - bool connected_; - RobotState* robot_state_; + bool connected_; + RobotState* robot_state_; - UrCommunication(std::condition_variable& msg_cond, std::string host); - bool start(); - void halt(); + UrCommunication(std::condition_variable& msg_cond, std::string host); + bool start(); + void halt(); }; #endif /* UR_COMMUNICATION_H_ */ diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h index 1d414c109..dbfb3bd32 100644 --- a/include/ur_modern_driver/ur_driver.h +++ b/include/ur_modern_driver/ur_driver.h @@ -19,81 +19,79 @@ #ifndef UR_DRIVER_H_ #define UR_DRIVER_H_ -#include "do_output.h" -#include "ur_communication.h" -#include "ur_realtime_communication.h" -#include #include -#include #include -#include #include #include +#include +#include +#include #include +#include "do_output.h" +#include "ur_communication.h" +#include "ur_realtime_communication.h" #include -class UrDriver { +class UrDriver +{ private: - double maximum_time_step_; - double minimum_payload_; - double maximum_payload_; - std::vector joint_names_; - std::string ip_addr_; - const int MULT_JOINTSTATE_ = 1000000; - const int MULT_TIME_ = 1000000; - const unsigned int REVERSE_PORT_; - int incoming_sockfd_; - int new_sockfd_; - bool reverse_connected_; - double servoj_time_; - bool executing_traj_; - double firmware_version_; - double servoj_lookahead_time_; - double servoj_gain_; + double maximum_time_step_; + double minimum_payload_; + double maximum_payload_; + std::vector joint_names_; + std::string ip_addr_; + const int MULT_JOINTSTATE_ = 1000000; + const int MULT_TIME_ = 1000000; + const unsigned int REVERSE_PORT_; + int incoming_sockfd_; + int new_sockfd_; + bool reverse_connected_; + double servoj_time_; + bool executing_traj_; + double firmware_version_; + double servoj_lookahead_time_; + double servoj_gain_; public: - UrRealtimeCommunication* rt_interface_; - UrCommunication* sec_interface_; + UrRealtimeCommunication* rt_interface_; + UrCommunication* sec_interface_; - UrDriver(std::condition_variable& rt_msg_cond, - std::condition_variable& msg_cond, std::string host, - unsigned int reverse_port = 50007, double servoj_time = 0.016, unsigned int safety_count_max = 12, double max_time_step = 0.08, double min_payload = 0., - double max_payload = 1., double servoj_lookahead_time = 0.03, double servoj_gain = 300.); - bool start(); - void halt(); + UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host, + unsigned int reverse_port = 50007, double servoj_time = 0.016, unsigned int safety_count_max = 12, + double max_time_step = 0.08, double min_payload = 0., double max_payload = 1., + double servoj_lookahead_time = 0.03, double servoj_gain = 300.); + bool start(); + void halt(); - void setSpeed(double q0, double q1, double q2, double q3, double q4, - double q5, double acc = 100.); + void setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc = 100.); - bool doTraj(std::vector inp_timestamps, - std::vector > inp_positions, - std::vector > inp_velocities); - void servoj(std::vector positions, int keepalive = 1); + bool doTraj(std::vector inp_timestamps, std::vector> inp_positions, + std::vector> inp_velocities); + void servoj(std::vector positions, int keepalive = 1); - void stopTraj(); + void stopTraj(); - bool uploadProg(); - bool openServo(); - void closeServo(std::vector positions); + bool uploadProg(); + bool openServo(); + void closeServo(std::vector positions); - std::vector interp_cubic(double t, double T, - std::vector p0_pos, std::vector p1_pos, - std::vector p0_vel, std::vector p1_vel); + std::vector interp_cubic(double t, double T, std::vector p0_pos, std::vector p1_pos, + std::vector p0_vel, std::vector p1_vel); - std::vector getJointNames(); - void setJointNames(std::vector jn); - void setToolVoltage(unsigned int v); - void setFlag(unsigned int n, bool b); - void setDigitalOut(unsigned int n, bool b); - void setAnalogOut(unsigned int n, double f); - bool setPayload(double m); + std::vector getJointNames(); + void setJointNames(std::vector jn); + void setToolVoltage(unsigned int v); + void setFlag(unsigned int n, bool b); + void setDigitalOut(unsigned int n, bool b); + void setAnalogOut(unsigned int n, double f); + bool setPayload(double m); - void setMinPayload(double m); - void setMaxPayload(double m); - void setServojTime(double t); - void setServojLookahead(double t); - void setServojGain(double g); + void setMinPayload(double m); + void setMaxPayload(double m); + void setServojTime(double t); + void setServojLookahead(double t); + void setServojGain(double g); }; #endif /* UR_DRIVER_H_ */ diff --git a/include/ur_modern_driver/ur_hardware_interface.h b/include/ur_modern_driver/ur_hardware_interface.h index 0724703cf..b2a7db107 100644 --- a/include/ur_modern_driver/ur_hardware_interface.h +++ b/include/ur_modern_driver/ur_hardware_interface.h @@ -58,9 +58,6 @@ #ifndef UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H #define UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H -#include "do_output.h" -#include "ur_driver.h" -#include #include #include #include @@ -68,69 +65,72 @@ #include #include #include +#include +#include "do_output.h" +#include "ur_driver.h" -namespace ros_control_ur { - +namespace ros_control_ur +{ // For simulation only - determines how fast a trajectory is followed static const double POSITION_STEP_FACTOR = 1; static const double VELOCITY_STEP_FACTOR = 1; /// \brief Hardware interface for a robot -class UrHardwareInterface : public hardware_interface::RobotHW { +class UrHardwareInterface : public hardware_interface::RobotHW +{ public: - /** - * \brief Constructor - * \param nh - Node handle for topics. - */ - UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot); + /** + * \brief Constructor + * \param nh - Node handle for topics. + */ + UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot); - /// \brief Initialize the hardware interface - virtual void init(); + /// \brief Initialize the hardware interface + virtual void init(); - /// \brief Read the state from the robot hardware. - virtual void read(); + /// \brief Read the state from the robot hardware. + virtual void read(); - /// \brief write the command to the robot hardware. - virtual void write(); + /// \brief write the command to the robot hardware. + virtual void write(); - void setMaxVelChange(double inp); + void setMaxVelChange(double inp); - bool canSwitch( - const std::list& start_list, - const std::list& stop_list) const; - void doSwitch(const std::list& start_list, - const std::list& stop_list); + bool canSwitch(const std::list& start_list, + const std::list& stop_list) const; + void doSwitch(const std::list& start_list, + const std::list& stop_list); protected: - // Startup and shutdown of the internal node inside a roscpp program - ros::NodeHandle nh_; - - // Interfaces - hardware_interface::JointStateInterface joint_state_interface_; - hardware_interface::ForceTorqueSensorInterface force_torque_interface_; - hardware_interface::PositionJointInterface position_joint_interface_; - hardware_interface::VelocityJointInterface velocity_joint_interface_; - bool velocity_interface_running_; - bool position_interface_running_; - // Shared memory - std::vector joint_names_; - std::vector joint_position_; - std::vector joint_velocity_; - std::vector joint_effort_; - std::vector joint_position_command_; - std::vector joint_velocity_command_; - std::vector prev_joint_velocity_command_; - std::size_t num_joints_; - double robot_force_[3] = { 0., 0., 0. }; - double robot_torque_[3] = { 0., 0., 0. }; - - double max_vel_change_; - - // Robot API - UrDriver* robot_; + // Startup and shutdown of the internal node inside a roscpp program + ros::NodeHandle nh_; + + // Interfaces + hardware_interface::JointStateInterface joint_state_interface_; + hardware_interface::ForceTorqueSensorInterface force_torque_interface_; + hardware_interface::PositionJointInterface position_joint_interface_; + hardware_interface::VelocityJointInterface velocity_joint_interface_; + bool velocity_interface_running_; + bool position_interface_running_; + // Shared memory + std::vector joint_names_; + std::vector joint_position_; + std::vector joint_velocity_; + std::vector joint_effort_; + std::vector joint_position_command_; + std::vector joint_velocity_command_; + std::vector prev_joint_velocity_command_; + std::size_t num_joints_; + double robot_force_[3] = { 0., 0., 0. }; + double robot_torque_[3] = { 0., 0., 0. }; + + double max_vel_change_; + + // Robot API + UrDriver* robot_; }; // class -} // namespace +} // namespace #endif diff --git a/include/ur_modern_driver/ur_realtime_communication.h b/include/ur_modern_driver/ur_realtime_communication.h index 61e547417..1de9fdb62 100644 --- a/include/ur_modern_driver/ur_realtime_communication.h +++ b/include/ur_modern_driver/ur_realtime_communication.h @@ -19,14 +19,9 @@ #ifndef UR_REALTIME_COMMUNICATION_H_ #define UR_REALTIME_COMMUNICATION_H_ -#include "do_output.h" -#include "robot_state_RT.h" #include -#include #include #include -#include -#include #include #include #include @@ -37,38 +32,42 @@ #include #include #include -#include #include +#include +#include +#include +#include #include +#include "do_output.h" +#include "robot_state_RT.h" -class UrRealtimeCommunication { +class UrRealtimeCommunication +{ private: - unsigned int safety_count_max_; - int sockfd_; - struct sockaddr_in serv_addr_; - struct hostent* server_; - std::string local_ip_; - bool keepalive_; - std::thread comThread_; - int flag_; - std::recursive_mutex command_string_lock_; - std::string command_; - unsigned int safety_count_; - void run(); + unsigned int safety_count_max_; + int sockfd_; + struct sockaddr_in serv_addr_; + struct hostent* server_; + std::string local_ip_; + bool keepalive_; + std::thread comThread_; + int flag_; + std::recursive_mutex command_string_lock_; + std::string command_; + unsigned int safety_count_; + void run(); public: - bool connected_; - RobotStateRT* robot_state_; + bool connected_; + RobotStateRT* robot_state_; - UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host, - unsigned int safety_count_max = 12); - bool start(); - void halt(); - void setSpeed(double q0, double q1, double q2, double q3, double q4, - double q5, double acc = 100.); - void addCommandToQueue(std::string inp); - void setSafetyCountMax(uint inp); - std::string getLocalIp(); + UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host, unsigned int safety_count_max = 12); + bool start(); + void halt(); + void setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc = 100.); + void addCommandToQueue(std::string inp); + void setSafetyCountMax(uint inp); + std::string getLocalIp(); }; #endif /* UR_REALTIME_COMMUNICATION_H_ */ diff --git a/src/do_output.cpp b/src/do_output.cpp index f4b0b4094..765e9b429 100644 --- a/src/do_output.cpp +++ b/src/do_output.cpp @@ -21,42 +21,42 @@ void print_debug(std::string inp) { #ifdef ROS_BUILD - ROS_DEBUG("%s", inp.c_str()); + ROS_DEBUG("%s", inp.c_str()); #else - printf("DEBUG: %s\n", inp.c_str()); + printf("DEBUG: %s\n", inp.c_str()); #endif } void print_info(std::string inp) { #ifdef ROS_BUILD - ROS_INFO("%s", inp.c_str()); + ROS_INFO("%s", inp.c_str()); #else - printf("INFO: %s\n", inp.c_str()); + printf("INFO: %s\n", inp.c_str()); #endif } void print_warning(std::string inp) { #ifdef ROS_BUILD - ROS_WARN("%s", inp.c_str()); + ROS_WARN("%s", inp.c_str()); #else - printf("WARNING: %s\n", inp.c_str()); + printf("WARNING: %s\n", inp.c_str()); #endif } void print_error(std::string inp) { #ifdef ROS_BUILD - ROS_ERROR("%s", inp.c_str()); + ROS_ERROR("%s", inp.c_str()); #else - printf("ERROR: %s\n", inp.c_str()); + printf("ERROR: %s\n", inp.c_str()); #endif } void print_fatal(std::string inp) { #ifdef ROS_BUILD - ROS_FATAL("%s", inp.c_str()); - ros::shutdown(); + ROS_FATAL("%s", inp.c_str()); + ros::shutdown(); #else - printf("FATAL: %s\n", inp.c_str()); - exit(1); + printf("FATAL: %s\n", inp.c_str()); + exit(1); #endif } diff --git a/src/robot_state.cpp b/src/robot_state.cpp index 743c2c223..786517c82 100644 --- a/src/robot_state.cpp +++ b/src/robot_state.cpp @@ -20,389 +20,380 @@ RobotState::RobotState(std::condition_variable& msg_cond) { - version_msg_.major_version = 0; - version_msg_.minor_version = 0; - new_data_available_ = false; - pMsg_cond_ = &msg_cond; - RobotState::setDisconnected(); - robot_mode_running_ = robotStateTypeV30::ROBOT_MODE_RUNNING; + version_msg_.major_version = 0; + version_msg_.minor_version = 0; + new_data_available_ = false; + pMsg_cond_ = &msg_cond; + RobotState::setDisconnected(); + robot_mode_running_ = robotStateTypeV30::ROBOT_MODE_RUNNING; } double RobotState::ntohd(uint64_t nf) { - double x; - nf = be64toh(nf); - memcpy(&x, &nf, sizeof(x)); - return x; + double x; + nf = be64toh(nf); + memcpy(&x, &nf, sizeof(x)); + return x; } void RobotState::unpack(uint8_t* buf, unsigned int buf_length) { - /* Returns missing bytes to unpack a message, or 0 if all data was parsed */ - unsigned int offset = 0; - while (buf_length > offset) { - int len; - unsigned char message_type; - memcpy(&len, &buf[offset], sizeof(len)); - len = ntohl(len); - if (len + offset > buf_length) { - return; - } - memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type)); - switch (message_type) { - case messageType::ROBOT_MESSAGE: - RobotState::unpackRobotMessage(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType - break; - case messageType::ROBOT_STATE: - RobotState::unpackRobotState(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType - break; - case messageType::PROGRAM_STATE_MESSAGE: - //Don't do anything atm... - default: - break; - } - offset += len; + /* Returns missing bytes to unpack a message, or 0 if all data was parsed */ + unsigned int offset = 0; + while (buf_length > offset) + { + int len; + unsigned char message_type; + memcpy(&len, &buf[offset], sizeof(len)); + len = ntohl(len); + if (len + offset > buf_length) + { + return; } - return; + memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type)); + switch (message_type) + { + case messageType::ROBOT_MESSAGE: + RobotState::unpackRobotMessage(buf, offset, + len); //'len' is inclusive the 5 bytes from messageSize and messageType + break; + case messageType::ROBOT_STATE: + RobotState::unpackRobotState(buf, offset, + len); //'len' is inclusive the 5 bytes from messageSize and messageType + break; + case messageType::PROGRAM_STATE_MESSAGE: + // Don't do anything atm... + default: + break; + } + offset += len; + } + return; } -void RobotState::unpackRobotMessage(uint8_t* buf, unsigned int offset, - uint32_t len) +void RobotState::unpackRobotMessage(uint8_t* buf, unsigned int offset, uint32_t len) { - offset += 5; - uint64_t timestamp; - int8_t source, robot_message_type; - memcpy(×tamp, &buf[offset], sizeof(timestamp)); - offset += sizeof(timestamp); - memcpy(&source, &buf[offset], sizeof(source)); - offset += sizeof(source); - memcpy(&robot_message_type, &buf[offset], sizeof(robot_message_type)); - offset += sizeof(robot_message_type); - switch (robot_message_type) { + offset += 5; + uint64_t timestamp; + int8_t source, robot_message_type; + memcpy(×tamp, &buf[offset], sizeof(timestamp)); + offset += sizeof(timestamp); + memcpy(&source, &buf[offset], sizeof(source)); + offset += sizeof(source); + memcpy(&robot_message_type, &buf[offset], sizeof(robot_message_type)); + offset += sizeof(robot_message_type); + switch (robot_message_type) + { case robotMessageType::ROBOT_MESSAGE_VERSION: - val_lock_.lock(); - version_msg_.timestamp = timestamp; - version_msg_.source = source; - version_msg_.robot_message_type = robot_message_type; - RobotState::unpackRobotMessageVersion(buf, offset, len); - val_lock_.unlock(); - break; + val_lock_.lock(); + version_msg_.timestamp = timestamp; + version_msg_.source = source; + version_msg_.robot_message_type = robot_message_type; + RobotState::unpackRobotMessageVersion(buf, offset, len); + val_lock_.unlock(); + break; default: - break; - } + 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; - uint8_t package_type; - memcpy(&length, &buf[offset], sizeof(length)); - length = ntohl(length); - memcpy(&package_type, &buf[offset + sizeof(length)], - sizeof(package_type)); - switch (package_type) { - case packageType::ROBOT_MODE_DATA: - val_lock_.lock(); - RobotState::unpackRobotMode(buf, offset + 5); - val_lock_.unlock(); - break; + offset += 5; + while (offset < len) + { + int32_t length; + uint8_t package_type; + memcpy(&length, &buf[offset], sizeof(length)); + length = ntohl(length); + memcpy(&package_type, &buf[offset + sizeof(length)], sizeof(package_type)); + switch (package_type) + { + case packageType::ROBOT_MODE_DATA: + val_lock_.lock(); + RobotState::unpackRobotMode(buf, offset + 5); + val_lock_.unlock(); + break; - case packageType::MASTERBOARD_DATA: - val_lock_.lock(); - RobotState::unpackRobotStateMasterboard(buf, offset + 5); - val_lock_.unlock(); - break; - default: - break; - } - offset += length; + case packageType::MASTERBOARD_DATA: + val_lock_.lock(); + RobotState::unpackRobotStateMasterboard(buf, offset + 5); + val_lock_.unlock(); + break; + default: + break; } - new_data_available_ = true; - pMsg_cond_->notify_all(); + offset += length; + } + 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); - memcpy(&version_msg_.project_name, &buf[offset], - sizeof(char) * version_msg_.project_name_size); - offset += version_msg_.project_name_size; - version_msg_.project_name[version_msg_.project_name_size] = '\0'; - memcpy(&version_msg_.major_version, &buf[offset], - sizeof(version_msg_.major_version)); - offset += sizeof(version_msg_.major_version); - memcpy(&version_msg_.minor_version, &buf[offset], - sizeof(version_msg_.minor_version)); - offset += sizeof(version_msg_.minor_version); - memcpy(&version_msg_.svn_revision, &buf[offset], - sizeof(version_msg_.svn_revision)); - offset += sizeof(version_msg_.svn_revision); - version_msg_.svn_revision = ntohl(version_msg_.svn_revision); - memcpy(&version_msg_.build_date, &buf[offset], sizeof(char) * len - offset); - version_msg_.build_date[len - offset] = '\0'; - if (version_msg_.major_version < 2) { - robot_mode_running_ = robotStateTypeV18::ROBOT_RUNNING_MODE; - } + memcpy(&version_msg_.project_name_size, &buf[offset], sizeof(version_msg_.project_name_size)); + offset += sizeof(version_msg_.project_name_size); + memcpy(&version_msg_.project_name, &buf[offset], sizeof(char) * version_msg_.project_name_size); + offset += version_msg_.project_name_size; + version_msg_.project_name[version_msg_.project_name_size] = '\0'; + memcpy(&version_msg_.major_version, &buf[offset], sizeof(version_msg_.major_version)); + offset += sizeof(version_msg_.major_version); + memcpy(&version_msg_.minor_version, &buf[offset], sizeof(version_msg_.minor_version)); + offset += sizeof(version_msg_.minor_version); + memcpy(&version_msg_.svn_revision, &buf[offset], sizeof(version_msg_.svn_revision)); + offset += sizeof(version_msg_.svn_revision); + version_msg_.svn_revision = ntohl(version_msg_.svn_revision); + memcpy(&version_msg_.build_date, &buf[offset], sizeof(char) * len - offset); + version_msg_.build_date[len - offset] = '\0'; + if (version_msg_.major_version < 2) + { + robot_mode_running_ = robotStateTypeV18::ROBOT_RUNNING_MODE; + } } void RobotState::unpackRobotMode(uint8_t* buf, unsigned int offset) { - memcpy(&robot_mode_.timestamp, &buf[offset], sizeof(robot_mode_.timestamp)); - offset += sizeof(robot_mode_.timestamp); - uint8_t tmp; - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isRobotConnected = true; - else - robot_mode_.isRobotConnected = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isRealRobotEnabled = true; - else - robot_mode_.isRealRobotEnabled = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - //printf("PowerOnRobot: %d\n", tmp); - if (tmp > 0) - robot_mode_.isPowerOnRobot = true; - else - robot_mode_.isPowerOnRobot = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isEmergencyStopped = true; - else - robot_mode_.isEmergencyStopped = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isProtectiveStopped = true; - else - robot_mode_.isProtectiveStopped = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isProgramRunning = true; - else - robot_mode_.isProgramRunning = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isProgramPaused = true; - else - robot_mode_.isProgramPaused = false; - offset += sizeof(tmp); - memcpy(&robot_mode_.robotMode, &buf[offset], sizeof(robot_mode_.robotMode)); - offset += sizeof(robot_mode_.robotMode); - uint64_t temp; - if (RobotState::getVersion() > 2.) { - memcpy(&robot_mode_.controlMode, &buf[offset], - sizeof(robot_mode_.controlMode)); - offset += sizeof(robot_mode_.controlMode); - memcpy(&temp, &buf[offset], sizeof(temp)); - offset += sizeof(temp); - robot_mode_.targetSpeedFraction = RobotState::ntohd(temp); - } + memcpy(&robot_mode_.timestamp, &buf[offset], sizeof(robot_mode_.timestamp)); + offset += sizeof(robot_mode_.timestamp); + uint8_t tmp; + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isRobotConnected = true; + else + robot_mode_.isRobotConnected = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isRealRobotEnabled = true; + else + robot_mode_.isRealRobotEnabled = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + // printf("PowerOnRobot: %d\n", tmp); + if (tmp > 0) + robot_mode_.isPowerOnRobot = true; + else + robot_mode_.isPowerOnRobot = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isEmergencyStopped = true; + else + robot_mode_.isEmergencyStopped = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isProtectiveStopped = true; + else + robot_mode_.isProtectiveStopped = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isProgramRunning = true; + else + robot_mode_.isProgramRunning = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isProgramPaused = true; + else + robot_mode_.isProgramPaused = false; + offset += sizeof(tmp); + memcpy(&robot_mode_.robotMode, &buf[offset], sizeof(robot_mode_.robotMode)); + offset += sizeof(robot_mode_.robotMode); + uint64_t temp; + if (RobotState::getVersion() > 2.) + { + memcpy(&robot_mode_.controlMode, &buf[offset], sizeof(robot_mode_.controlMode)); + offset += sizeof(robot_mode_.controlMode); memcpy(&temp, &buf[offset], sizeof(temp)); offset += sizeof(temp); - robot_mode_.speedScaling = RobotState::ntohd(temp); + robot_mode_.targetSpeedFraction = RobotState::ntohd(temp); + } + memcpy(&temp, &buf[offset], sizeof(temp)); + offset += sizeof(temp); + robot_mode_.speedScaling = RobotState::ntohd(temp); } -void RobotState::unpackRobotStateMasterboard(uint8_t* buf, - 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)); - offset += sizeof(digital_input_bits); - memcpy(&digital_output_bits, &buf[offset], sizeof(digital_output_bits)); - offset += sizeof(digital_output_bits); - mb_data_.digitalInputBits = ntohs(digital_input_bits); - mb_data_.digitalOutputBits = ntohs(digital_output_bits); - } else { - memcpy(&mb_data_.digitalInputBits, &buf[offset], - sizeof(mb_data_.digitalInputBits)); - offset += sizeof(mb_data_.digitalInputBits); - mb_data_.digitalInputBits = ntohl(mb_data_.digitalInputBits); - memcpy(&mb_data_.digitalOutputBits, &buf[offset], - sizeof(mb_data_.digitalOutputBits)); - offset += sizeof(mb_data_.digitalOutputBits); - mb_data_.digitalOutputBits = ntohl(mb_data_.digitalOutputBits); - } + if (RobotState::getVersion() < 3.0) + { + int16_t digital_input_bits, digital_output_bits; + memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits)); + offset += sizeof(digital_input_bits); + memcpy(&digital_output_bits, &buf[offset], sizeof(digital_output_bits)); + offset += sizeof(digital_output_bits); + mb_data_.digitalInputBits = ntohs(digital_input_bits); + mb_data_.digitalOutputBits = ntohs(digital_output_bits); + } + else + { + memcpy(&mb_data_.digitalInputBits, &buf[offset], sizeof(mb_data_.digitalInputBits)); + offset += sizeof(mb_data_.digitalInputBits); + mb_data_.digitalInputBits = ntohl(mb_data_.digitalInputBits); + memcpy(&mb_data_.digitalOutputBits, &buf[offset], sizeof(mb_data_.digitalOutputBits)); + offset += sizeof(mb_data_.digitalOutputBits); + mb_data_.digitalOutputBits = ntohl(mb_data_.digitalOutputBits); + } - memcpy(&mb_data_.analogInputRange0, &buf[offset], - sizeof(mb_data_.analogInputRange0)); - offset += sizeof(mb_data_.analogInputRange0); - memcpy(&mb_data_.analogInputRange1, &buf[offset], - sizeof(mb_data_.analogInputRange1)); - offset += sizeof(mb_data_.analogInputRange1); - uint64_t temp; - memcpy(&temp, &buf[offset], sizeof(temp)); - offset += sizeof(temp); - mb_data_.analogInput0 = RobotState::ntohd(temp); - memcpy(&temp, &buf[offset], sizeof(temp)); - offset += sizeof(temp); - mb_data_.analogInput1 = RobotState::ntohd(temp); - memcpy(&mb_data_.analogOutputDomain0, &buf[offset], - sizeof(mb_data_.analogOutputDomain0)); - offset += sizeof(mb_data_.analogOutputDomain0); - memcpy(&mb_data_.analogOutputDomain1, &buf[offset], - sizeof(mb_data_.analogOutputDomain1)); - offset += sizeof(mb_data_.analogOutputDomain1); - memcpy(&temp, &buf[offset], sizeof(temp)); - offset += sizeof(temp); - mb_data_.analogOutput0 = RobotState::ntohd(temp); - memcpy(&temp, &buf[offset], sizeof(temp)); - offset += sizeof(temp); - mb_data_.analogOutput1 = RobotState::ntohd(temp); + memcpy(&mb_data_.analogInputRange0, &buf[offset], sizeof(mb_data_.analogInputRange0)); + offset += sizeof(mb_data_.analogInputRange0); + memcpy(&mb_data_.analogInputRange1, &buf[offset], sizeof(mb_data_.analogInputRange1)); + offset += sizeof(mb_data_.analogInputRange1); + uint64_t temp; + memcpy(&temp, &buf[offset], sizeof(temp)); + offset += sizeof(temp); + mb_data_.analogInput0 = RobotState::ntohd(temp); + memcpy(&temp, &buf[offset], sizeof(temp)); + offset += sizeof(temp); + mb_data_.analogInput1 = RobotState::ntohd(temp); + memcpy(&mb_data_.analogOutputDomain0, &buf[offset], sizeof(mb_data_.analogOutputDomain0)); + offset += sizeof(mb_data_.analogOutputDomain0); + memcpy(&mb_data_.analogOutputDomain1, &buf[offset], sizeof(mb_data_.analogOutputDomain1)); + offset += sizeof(mb_data_.analogOutputDomain1); + memcpy(&temp, &buf[offset], sizeof(temp)); + offset += sizeof(temp); + mb_data_.analogOutput0 = RobotState::ntohd(temp); + memcpy(&temp, &buf[offset], sizeof(temp)); + offset += sizeof(temp); + mb_data_.analogOutput1 = RobotState::ntohd(temp); - memcpy(&mb_data_.masterBoardTemperature, &buf[offset], - sizeof(mb_data_.masterBoardTemperature)); - offset += sizeof(mb_data_.masterBoardTemperature); - mb_data_.masterBoardTemperature = ntohl(mb_data_.masterBoardTemperature); - memcpy(&mb_data_.robotVoltage48V, &buf[offset], - sizeof(mb_data_.robotVoltage48V)); - offset += sizeof(mb_data_.robotVoltage48V); - mb_data_.robotVoltage48V = ntohl(mb_data_.robotVoltage48V); - memcpy(&mb_data_.robotCurrent, &buf[offset], sizeof(mb_data_.robotCurrent)); - offset += sizeof(mb_data_.robotCurrent); - mb_data_.robotCurrent = ntohl(mb_data_.robotCurrent); - memcpy(&mb_data_.masterIOCurrent, &buf[offset], - sizeof(mb_data_.masterIOCurrent)); - offset += sizeof(mb_data_.masterIOCurrent); - mb_data_.masterIOCurrent = ntohl(mb_data_.masterIOCurrent); + memcpy(&mb_data_.masterBoardTemperature, &buf[offset], sizeof(mb_data_.masterBoardTemperature)); + offset += sizeof(mb_data_.masterBoardTemperature); + mb_data_.masterBoardTemperature = ntohl(mb_data_.masterBoardTemperature); + memcpy(&mb_data_.robotVoltage48V, &buf[offset], sizeof(mb_data_.robotVoltage48V)); + offset += sizeof(mb_data_.robotVoltage48V); + mb_data_.robotVoltage48V = ntohl(mb_data_.robotVoltage48V); + memcpy(&mb_data_.robotCurrent, &buf[offset], sizeof(mb_data_.robotCurrent)); + offset += sizeof(mb_data_.robotCurrent); + mb_data_.robotCurrent = ntohl(mb_data_.robotCurrent); + memcpy(&mb_data_.masterIOCurrent, &buf[offset], sizeof(mb_data_.masterIOCurrent)); + offset += sizeof(mb_data_.masterIOCurrent); + mb_data_.masterIOCurrent = ntohl(mb_data_.masterIOCurrent); - memcpy(&mb_data_.safetyMode, &buf[offset], sizeof(mb_data_.safetyMode)); - offset += sizeof(mb_data_.safetyMode); - memcpy(&mb_data_.masterOnOffState, &buf[offset], - sizeof(mb_data_.masterOnOffState)); - offset += sizeof(mb_data_.masterOnOffState); + memcpy(&mb_data_.safetyMode, &buf[offset], sizeof(mb_data_.safetyMode)); + offset += sizeof(mb_data_.safetyMode); + memcpy(&mb_data_.masterOnOffState, &buf[offset], sizeof(mb_data_.masterOnOffState)); + offset += sizeof(mb_data_.masterOnOffState); - memcpy(&mb_data_.euromap67InterfaceInstalled, &buf[offset], - sizeof(mb_data_.euromap67InterfaceInstalled)); - offset += sizeof(mb_data_.euromap67InterfaceInstalled); - if (mb_data_.euromap67InterfaceInstalled != 0) { - memcpy(&mb_data_.euromapInputBits, &buf[offset], - sizeof(mb_data_.euromapInputBits)); - offset += sizeof(mb_data_.euromapInputBits); - mb_data_.euromapInputBits = ntohl(mb_data_.euromapInputBits); - memcpy(&mb_data_.euromapOutputBits, &buf[offset], - sizeof(mb_data_.euromapOutputBits)); - offset += sizeof(mb_data_.euromapOutputBits); - mb_data_.euromapOutputBits = ntohl(mb_data_.euromapOutputBits); - if (RobotState::getVersion() < 3.0) { - int16_t euromap_voltage, euromap_current; - memcpy(&euromap_voltage, &buf[offset], sizeof(euromap_voltage)); - offset += sizeof(euromap_voltage); - memcpy(&euromap_current, &buf[offset], sizeof(euromap_current)); - offset += sizeof(euromap_current); - mb_data_.euromapVoltage = ntohs(euromap_voltage); - mb_data_.euromapCurrent = ntohs(euromap_current); - } else { - memcpy(&mb_data_.euromapVoltage, &buf[offset], - sizeof(mb_data_.euromapVoltage)); - offset += sizeof(mb_data_.euromapVoltage); - mb_data_.euromapVoltage = ntohl(mb_data_.euromapVoltage); - memcpy(&mb_data_.euromapCurrent, &buf[offset], - sizeof(mb_data_.euromapCurrent)); - offset += sizeof(mb_data_.euromapCurrent); - mb_data_.euromapCurrent = ntohl(mb_data_.euromapCurrent); - } + memcpy(&mb_data_.euromap67InterfaceInstalled, &buf[offset], sizeof(mb_data_.euromap67InterfaceInstalled)); + offset += sizeof(mb_data_.euromap67InterfaceInstalled); + if (mb_data_.euromap67InterfaceInstalled != 0) + { + memcpy(&mb_data_.euromapInputBits, &buf[offset], sizeof(mb_data_.euromapInputBits)); + offset += sizeof(mb_data_.euromapInputBits); + mb_data_.euromapInputBits = ntohl(mb_data_.euromapInputBits); + memcpy(&mb_data_.euromapOutputBits, &buf[offset], sizeof(mb_data_.euromapOutputBits)); + offset += sizeof(mb_data_.euromapOutputBits); + mb_data_.euromapOutputBits = ntohl(mb_data_.euromapOutputBits); + if (RobotState::getVersion() < 3.0) + { + int16_t euromap_voltage, euromap_current; + memcpy(&euromap_voltage, &buf[offset], sizeof(euromap_voltage)); + offset += sizeof(euromap_voltage); + memcpy(&euromap_current, &buf[offset], sizeof(euromap_current)); + offset += sizeof(euromap_current); + mb_data_.euromapVoltage = ntohs(euromap_voltage); + mb_data_.euromapCurrent = ntohs(euromap_current); + } + else + { + memcpy(&mb_data_.euromapVoltage, &buf[offset], sizeof(mb_data_.euromapVoltage)); + offset += sizeof(mb_data_.euromapVoltage); + mb_data_.euromapVoltage = ntohl(mb_data_.euromapVoltage); + memcpy(&mb_data_.euromapCurrent, &buf[offset], sizeof(mb_data_.euromapCurrent)); + offset += sizeof(mb_data_.euromapCurrent); + mb_data_.euromapCurrent = ntohl(mb_data_.euromapCurrent); } + } } double RobotState::getVersion() { - double ver; - val_lock_.lock(); - ver = version_msg_.major_version + 0.1 * version_msg_.minor_version - + .0000001 * version_msg_.svn_revision; - val_lock_.unlock(); - return ver; + double ver; + val_lock_.lock(); + ver = version_msg_.major_version + 0.1 * version_msg_.minor_version + .0000001 * version_msg_.svn_revision; + val_lock_.unlock(); + return ver; } void RobotState::finishedReading() { - new_data_available_ = false; + new_data_available_ = false; } bool RobotState::getNewDataAvailable() { - return new_data_available_; + return new_data_available_; } int RobotState::getDigitalInputBits() { - return mb_data_.digitalInputBits; + return mb_data_.digitalInputBits; } int RobotState::getDigitalOutputBits() { - return mb_data_.digitalOutputBits; + return mb_data_.digitalOutputBits; } double RobotState::getAnalogInput0() { - return mb_data_.analogInput0; + return mb_data_.analogInput0; } double RobotState::getAnalogInput1() { - return mb_data_.analogInput1; + return mb_data_.analogInput1; } double RobotState::getAnalogOutput0() { - return mb_data_.analogOutput0; + return mb_data_.analogOutput0; } double RobotState::getAnalogOutput1() { - return mb_data_.analogOutput1; + return mb_data_.analogOutput1; } bool RobotState::isRobotConnected() { - return robot_mode_.isRobotConnected; + return robot_mode_.isRobotConnected; } bool RobotState::isRealRobotEnabled() { - return robot_mode_.isRealRobotEnabled; + return robot_mode_.isRealRobotEnabled; } bool RobotState::isPowerOnRobot() { - return robot_mode_.isPowerOnRobot; + return robot_mode_.isPowerOnRobot; } bool RobotState::isEmergencyStopped() { - return robot_mode_.isEmergencyStopped; + return robot_mode_.isEmergencyStopped; } bool RobotState::isProtectiveStopped() { - return robot_mode_.isProtectiveStopped; + return robot_mode_.isProtectiveStopped; } bool RobotState::isProgramRunning() { - return robot_mode_.isProgramRunning; + return robot_mode_.isProgramRunning; } bool RobotState::isProgramPaused() { - return robot_mode_.isProgramPaused; + return robot_mode_.isProgramPaused; } unsigned char RobotState::getRobotMode() { - return robot_mode_.robotMode; + return robot_mode_.robotMode; } bool RobotState::isReady() { - if (robot_mode_.robotMode == robot_mode_running_) { - return true; - } - return false; + if (robot_mode_.robotMode == robot_mode_running_) + { + return true; + } + return false; } void RobotState::setDisconnected() { - robot_mode_.isRobotConnected = false; - robot_mode_.isRealRobotEnabled = false; - robot_mode_.isPowerOnRobot = false; + robot_mode_.isRobotConnected = false; + robot_mode_.isRealRobotEnabled = false; + robot_mode_.isPowerOnRobot = false; } diff --git a/src/robot_state_RT.cpp b/src/robot_state_RT.cpp index 46467fea2..71bb7ba57 100644 --- a/src/robot_state_RT.cpp +++ b/src/robot_state_RT.cpp @@ -20,456 +20,473 @@ RobotStateRT::RobotStateRT(std::condition_variable& msg_cond) { - version_ = 0.0; - time_ = 0.0; - q_target_.assign(6, 0.0); - qd_target_.assign(6, 0.0); - qdd_target_.assign(6, 0.0); - i_target_.assign(6, 0.0); - m_target_.assign(6, 0.0); - q_actual_.assign(6, 0.0); - qd_actual_.assign(6, 0.0); - i_actual_.assign(6, 0.0); - i_control_.assign(6, 0.0); - tool_vector_actual_.assign(6, 0.0); - tcp_speed_actual_.assign(6, 0.0); - tcp_force_.assign(6, 0.0); - tool_vector_target_.assign(6, 0.0); - tcp_speed_target_.assign(6, 0.0); - digital_input_bits_.assign(64, false); - motor_temperatures_.assign(6, 0.0); - controller_timer_ = 0.0; - robot_mode_ = 0.0; - joint_modes_.assign(6, 0.0); - safety_mode_ = 0.0; - tool_accelerometer_values_.assign(3, 0.0); - speed_scaling_ = 0.0; - linear_momentum_norm_ = 0.0; - v_main_ = 0.0; - v_robot_ = 0.0; - i_robot_ = 0.0; - v_actual_.assign(6, 0.0); - data_published_ = false; - controller_updated_ = false; - pMsg_cond_ = &msg_cond; + version_ = 0.0; + time_ = 0.0; + q_target_.assign(6, 0.0); + qd_target_.assign(6, 0.0); + qdd_target_.assign(6, 0.0); + i_target_.assign(6, 0.0); + m_target_.assign(6, 0.0); + q_actual_.assign(6, 0.0); + qd_actual_.assign(6, 0.0); + i_actual_.assign(6, 0.0); + i_control_.assign(6, 0.0); + tool_vector_actual_.assign(6, 0.0); + tcp_speed_actual_.assign(6, 0.0); + tcp_force_.assign(6, 0.0); + tool_vector_target_.assign(6, 0.0); + tcp_speed_target_.assign(6, 0.0); + digital_input_bits_.assign(64, false); + motor_temperatures_.assign(6, 0.0); + controller_timer_ = 0.0; + robot_mode_ = 0.0; + joint_modes_.assign(6, 0.0); + safety_mode_ = 0.0; + tool_accelerometer_values_.assign(3, 0.0); + speed_scaling_ = 0.0; + linear_momentum_norm_ = 0.0; + v_main_ = 0.0; + v_robot_ = 0.0; + i_robot_ = 0.0; + v_actual_.assign(6, 0.0); + data_published_ = false; + controller_updated_ = false; + pMsg_cond_ = &msg_cond; } RobotStateRT::~RobotStateRT() { - /* Make sure nobody is waiting after this thread is destroyed */ - data_published_ = true; - controller_updated_ = true; - pMsg_cond_->notify_all(); + /* Make sure nobody is waiting after this thread is destroyed */ + data_published_ = true; + controller_updated_ = true; + pMsg_cond_->notify_all(); } void RobotStateRT::setDataPublished() { - data_published_ = false; + data_published_ = false; } bool RobotStateRT::getDataPublished() { - return data_published_; + return data_published_; } void RobotStateRT::setControllerUpdated() { - controller_updated_ = false; + controller_updated_ = false; } bool RobotStateRT::getControllerUpdated() { - return controller_updated_; + return controller_updated_; } double RobotStateRT::ntohd(uint64_t nf) { - double x; - nf = be64toh(nf); - memcpy(&x, &nf, sizeof(x)); - return x; + double x; + nf = be64toh(nf); + memcpy(&x, &nf, sizeof(x)); + return x; } -std::vector RobotStateRT::unpackVector(uint8_t* buf, int start_index, - int nr_of_vals) -{ - uint64_t q; - std::vector ret; - for (int i = 0; i < nr_of_vals; i++) { - memcpy(&q, &buf[start_index + i * sizeof(q)], sizeof(q)); - ret.push_back(ntohd(q)); - } - return ret; +std::vector RobotStateRT::unpackVector(uint8_t* buf, int start_index, int nr_of_vals) +{ + uint64_t q; + std::vector ret; + for (int i = 0; i < nr_of_vals; i++) + { + memcpy(&q, &buf[start_index + i * sizeof(q)], sizeof(q)); + ret.push_back(ntohd(q)); + } + return ret; } std::vector RobotStateRT::unpackDigitalInputBits(int64_t data) { - std::vector ret; - for (int i = 0; i < 64; i++) { - ret.push_back((data & (1 << i)) >> i); - } - return ret; + std::vector ret; + for (int i = 0; i < 64; i++) + { + ret.push_back((data & (1 << i)) >> i); + } + return ret; } void RobotStateRT::setVersion(double ver) { - val_lock_.lock(); - version_ = ver; - val_lock_.unlock(); + val_lock_.lock(); + version_ = ver; + val_lock_.unlock(); } double RobotStateRT::getVersion() { - double ret; - val_lock_.lock(); - ret = version_; - val_lock_.unlock(); - return ret; + double ret; + val_lock_.lock(); + ret = version_; + val_lock_.unlock(); + return ret; } double RobotStateRT::getTime() { - double ret; - val_lock_.lock(); - ret = time_; - val_lock_.unlock(); - return ret; + double ret; + val_lock_.lock(); + ret = time_; + val_lock_.unlock(); + return ret; } std::vector RobotStateRT::getQTarget() { - std::vector ret; - val_lock_.lock(); - ret = q_target_; - val_lock_.unlock(); - return ret; + std::vector ret; + val_lock_.lock(); + ret = q_target_; + val_lock_.unlock(); + return ret; } std::vector RobotStateRT::getQdTarget() { - std::vector ret; - val_lock_.lock(); - ret = qd_target_; - val_lock_.unlock(); - return ret; + std::vector ret; + val_lock_.lock(); + ret = qd_target_; + val_lock_.unlock(); + return ret; } std::vector RobotStateRT::getQddTarget() { - std::vector ret; - val_lock_.lock(); - ret = qdd_target_; - val_lock_.unlock(); - return ret; + std::vector ret; + val_lock_.lock(); + ret = qdd_target_; + val_lock_.unlock(); + return ret; } std::vector RobotStateRT::getITarget() { - std::vector ret; - val_lock_.lock(); - ret = i_target_; - val_lock_.unlock(); - return ret; + std::vector ret; + val_lock_.lock(); + ret = i_target_; + val_lock_.unlock(); + return ret; } std::vector RobotStateRT::getMTarget() { - std::vector ret; - val_lock_.lock(); - ret = m_target_; - val_lock_.unlock(); - return ret; + std::vector ret; + val_lock_.lock(); + ret = m_target_; + val_lock_.unlock(); + return ret; } std::vector RobotStateRT::getQActual() { - std::vector ret; - val_lock_.lock(); - ret = q_actual_; - val_lock_.unlock(); - return ret; + std::vector ret; + val_lock_.lock(); + ret = q_actual_; + val_lock_.unlock(); + return ret; } std::vector RobotStateRT::getQdActual() { - std::vector ret; - val_lock_.lock(); - ret = qd_actual_; - val_lock_.unlock(); - return ret; + std::vector ret; + val_lock_.lock(); + ret = qd_actual_; + val_lock_.unlock(); + return ret; } std::vector RobotStateRT::getIActual() { - std::vector ret; - val_lock_.lock(); - ret = i_actual_; - val_lock_.unlock(); - return ret; + std::vector ret; + val_lock_.lock(); + ret = i_actual_; + val_lock_.unlock(); + return ret; } std::vector RobotStateRT::getIControl() { - std::vector ret; - val_lock_.lock(); - ret = i_control_; - val_lock_.unlock(); - return ret; + std::vector ret; + val_lock_.lock(); + ret = i_control_; + val_lock_.unlock(); + return ret; } std::vector RobotStateRT::getToolVectorActual() { - std::vector ret; - val_lock_.lock(); - ret = tool_vector_actual_; - val_lock_.unlock(); - return ret; + std::vector ret; + val_lock_.lock(); + ret = tool_vector_actual_; + val_lock_.unlock(); + return ret; } std::vector RobotStateRT::getTcpSpeedActual() { - std::vector ret; - val_lock_.lock(); - ret = tcp_speed_actual_; - val_lock_.unlock(); - return ret; + std::vector ret; + val_lock_.lock(); + ret = tcp_speed_actual_; + val_lock_.unlock(); + return ret; } std::vector RobotStateRT::getTcpForce() { - std::vector ret; - val_lock_.lock(); - ret = tcp_force_; - val_lock_.unlock(); - return ret; + std::vector ret; + val_lock_.lock(); + ret = tcp_force_; + val_lock_.unlock(); + return ret; } std::vector RobotStateRT::getToolVectorTarget() { - std::vector ret; - val_lock_.lock(); - ret = tool_vector_target_; - val_lock_.unlock(); - return ret; + std::vector ret; + val_lock_.lock(); + ret = tool_vector_target_; + val_lock_.unlock(); + return ret; } std::vector RobotStateRT::getTcpSpeedTarget() { - std::vector ret; - val_lock_.lock(); - ret = tcp_speed_target_; - val_lock_.unlock(); - return ret; + std::vector ret; + val_lock_.lock(); + ret = tcp_speed_target_; + val_lock_.unlock(); + return ret; } std::vector RobotStateRT::getDigitalInputBits() { - std::vector ret; - val_lock_.lock(); - ret = digital_input_bits_; - val_lock_.unlock(); - return ret; + std::vector ret; + val_lock_.lock(); + ret = digital_input_bits_; + val_lock_.unlock(); + return ret; } std::vector RobotStateRT::getMotorTemperatures() { - std::vector ret; - val_lock_.lock(); - ret = motor_temperatures_; - val_lock_.unlock(); - return ret; + std::vector ret; + val_lock_.lock(); + ret = motor_temperatures_; + val_lock_.unlock(); + return ret; } double RobotStateRT::getControllerTimer() { - double ret; - val_lock_.lock(); - ret = controller_timer_; - val_lock_.unlock(); - return ret; + double ret; + val_lock_.lock(); + ret = controller_timer_; + val_lock_.unlock(); + return ret; } double RobotStateRT::getRobotMode() { - double ret; - val_lock_.lock(); - ret = robot_mode_; - val_lock_.unlock(); - return ret; + double ret; + val_lock_.lock(); + ret = robot_mode_; + val_lock_.unlock(); + return ret; } std::vector RobotStateRT::getJointModes() { - std::vector ret; - val_lock_.lock(); - ret = joint_modes_; - val_lock_.unlock(); - return ret; + std::vector ret; + val_lock_.lock(); + ret = joint_modes_; + val_lock_.unlock(); + return ret; } double RobotStateRT::getSafety_mode() { - double ret; - val_lock_.lock(); - ret = safety_mode_; - val_lock_.unlock(); - return ret; + double ret; + val_lock_.lock(); + ret = safety_mode_; + val_lock_.unlock(); + return ret; } std::vector RobotStateRT::getToolAccelerometerValues() { - std::vector ret; - val_lock_.lock(); - ret = tool_accelerometer_values_; - val_lock_.unlock(); - return ret; + std::vector ret; + val_lock_.lock(); + ret = tool_accelerometer_values_; + val_lock_.unlock(); + return ret; } double RobotStateRT::getSpeedScaling() { - double ret; - val_lock_.lock(); - ret = speed_scaling_; - val_lock_.unlock(); - return ret; + double ret; + val_lock_.lock(); + ret = speed_scaling_; + val_lock_.unlock(); + return ret; } double RobotStateRT::getLinearMomentumNorm() { - double ret; - val_lock_.lock(); - ret = linear_momentum_norm_; - val_lock_.unlock(); - return ret; + double ret; + val_lock_.lock(); + ret = linear_momentum_norm_; + val_lock_.unlock(); + return ret; } double RobotStateRT::getVMain() { - double ret; - val_lock_.lock(); - ret = v_main_; - val_lock_.unlock(); - return ret; + double ret; + val_lock_.lock(); + ret = v_main_; + val_lock_.unlock(); + return ret; } double RobotStateRT::getVRobot() { - double ret; - val_lock_.lock(); - ret = v_robot_; - val_lock_.unlock(); - return ret; + double ret; + val_lock_.lock(); + ret = v_robot_; + val_lock_.unlock(); + return ret; } double RobotStateRT::getIRobot() { - double ret; - val_lock_.lock(); - ret = i_robot_; - val_lock_.unlock(); - return ret; + double ret; + val_lock_.lock(); + ret = i_robot_; + val_lock_.unlock(); + return ret; } std::vector RobotStateRT::getVActual() { - std::vector ret; - val_lock_.lock(); - ret = v_actual_; - val_lock_.unlock(); - return ret; + std::vector ret; + val_lock_.lock(); + ret = v_actual_; + val_lock_.unlock(); + return ret; } void RobotStateRT::unpack(uint8_t* buf) { - int64_t digital_input_bits; - uint64_t unpack_to; - uint16_t offset = 0; - val_lock_.lock(); - int len; - memcpy(&len, &buf[offset], sizeof(len)); + int64_t digital_input_bits; + uint64_t unpack_to; + uint16_t offset = 0; + val_lock_.lock(); + int len; + memcpy(&len, &buf[offset], sizeof(len)); - offset += sizeof(len); - len = ntohl(len); + offset += sizeof(len); + len = ntohl(len); - //Check the correct message length is received - bool len_good = true; - if (version_ >= 1.6 && version_ < 1.7) { //v1.6 - if (len != 756) - len_good = false; - } else if (version_ >= 1.7 && version_ < 1.8) { //v1.7 - if (len != 764) - len_good = false; - } else if (version_ >= 1.8 && version_ < 1.9) { //v1.8 - if (len != 812) - len_good = false; - } else if (version_ >= 3.0 && version_ < 3.2) { //v3.0 & v3.1 - if (len != 1044) - len_good = false; - } else if (version_ >= 3.2 && version_ < 3.3) { //v3.2 - if (len != 1060) - len_good = false; - } + // Check the correct message length is received + bool len_good = true; + if (version_ >= 1.6 && version_ < 1.7) + { // v1.6 + if (len != 756) + len_good = false; + } + else if (version_ >= 1.7 && version_ < 1.8) + { // v1.7 + if (len != 764) + len_good = false; + } + else if (version_ >= 1.8 && version_ < 1.9) + { // v1.8 + if (len != 812) + len_good = false; + } + else if (version_ >= 3.0 && version_ < 3.2) + { // v3.0 & v3.1 + if (len != 1044) + len_good = false; + } + else if (version_ >= 3.2 && version_ < 3.3) + { // v3.2 + if (len != 1060) + len_good = false; + } - if (!len_good) { - printf("Wrong length of message on RT interface: %i\n", len); - val_lock_.unlock(); - return; - } + if (!len_good) + { + printf("Wrong length of message on RT interface: %i\n", len); + val_lock_.unlock(); + return; + } - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - time_ = RobotStateRT::ntohd(unpack_to); - offset += sizeof(double); - q_target_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - qd_target_ = unpackVector(buf, offset, 6); + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); + time_ = RobotStateRT::ntohd(unpack_to); + offset += sizeof(double); + q_target_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + qd_target_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + qdd_target_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + i_target_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + m_target_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + q_actual_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + qd_actual_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + i_actual_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + if (version_ <= 1.9) + { + if (version_ > 1.6) + tool_accelerometer_values_ = unpackVector(buf, offset, 3); + offset += sizeof(double) * (3 + 15); + tcp_force_ = unpackVector(buf, offset, 6); offset += sizeof(double) * 6; - qdd_target_ = unpackVector(buf, offset, 6); + tool_vector_actual_ = unpackVector(buf, offset, 6); offset += sizeof(double) * 6; - i_target_ = unpackVector(buf, offset, 6); + tcp_speed_actual_ = unpackVector(buf, offset, 6); + } + else + { + i_control_ = unpackVector(buf, offset, 6); offset += sizeof(double) * 6; - m_target_ = unpackVector(buf, offset, 6); + tool_vector_actual_ = unpackVector(buf, offset, 6); offset += sizeof(double) * 6; - q_actual_ = unpackVector(buf, offset, 6); + tcp_speed_actual_ = unpackVector(buf, offset, 6); offset += sizeof(double) * 6; - qd_actual_ = unpackVector(buf, offset, 6); + tcp_force_ = unpackVector(buf, offset, 6); offset += sizeof(double) * 6; - i_actual_ = unpackVector(buf, offset, 6); + tool_vector_target_ = unpackVector(buf, offset, 6); offset += sizeof(double) * 6; - if (version_ <= 1.9) { - if (version_ > 1.6) - tool_accelerometer_values_ = unpackVector(buf, offset, 3); - offset += sizeof(double) * (3 + 15); - tcp_force_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tool_vector_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tcp_speed_actual_ = unpackVector(buf, offset, 6); - } else { - i_control_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tool_vector_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tcp_speed_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tcp_force_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tool_vector_target_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tcp_speed_target_ = unpackVector(buf, offset, 6); + tcp_speed_target_ = unpackVector(buf, offset, 6); + } + offset += sizeof(double) * 6; + + memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits)); + digital_input_bits_ = unpackDigitalInputBits(be64toh(digital_input_bits)); + offset += sizeof(double); + motor_temperatures_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); + controller_timer_ = ntohd(unpack_to); + if (version_ > 1.6) + { + offset += sizeof(double) * 2; + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); + robot_mode_ = ntohd(unpack_to); + if (version_ > 1.7) + { + offset += sizeof(double); + joint_modes_ = unpackVector(buf, offset, 6); } + } + if (version_ > 1.8) + { offset += sizeof(double) * 6; - - memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits)); - digital_input_bits_ = unpackDigitalInputBits(be64toh(digital_input_bits)); + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); + safety_mode_ = ntohd(unpack_to); offset += sizeof(double); - motor_temperatures_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; + tool_accelerometer_values_ = unpackVector(buf, offset, 3); + offset += sizeof(double) * 3; memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - controller_timer_ = ntohd(unpack_to); - if (version_ > 1.6) { - offset += sizeof(double) * 2; - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - robot_mode_ = ntohd(unpack_to); - if (version_ > 1.7) { - offset += sizeof(double); - joint_modes_ = unpackVector(buf, offset, 6); - } - } - if (version_ > 1.8) { - offset += sizeof(double) * 6; - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - safety_mode_ = ntohd(unpack_to); - offset += sizeof(double); - tool_accelerometer_values_ = unpackVector(buf, offset, 3); - offset += sizeof(double) * 3; - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - speed_scaling_ = ntohd(unpack_to); - offset += sizeof(double); - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - linear_momentum_norm_ = ntohd(unpack_to); - offset += sizeof(double); - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - v_main_ = ntohd(unpack_to); - offset += sizeof(double); - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - v_robot_ = ntohd(unpack_to); - offset += sizeof(double); - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - i_robot_ = ntohd(unpack_to); - offset += sizeof(double); - v_actual_ = unpackVector(buf, offset, 6); - } - val_lock_.unlock(); - controller_updated_ = true; - data_published_ = true; - pMsg_cond_->notify_all(); + speed_scaling_ = ntohd(unpack_to); + offset += sizeof(double); + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); + linear_momentum_norm_ = ntohd(unpack_to); + offset += sizeof(double); + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); + v_main_ = ntohd(unpack_to); + offset += sizeof(double); + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); + v_robot_ = ntohd(unpack_to); + offset += sizeof(double); + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); + i_robot_ = ntohd(unpack_to); + offset += sizeof(double); + v_actual_ = unpackVector(buf, offset, 6); + } + val_lock_.unlock(); + controller_updated_ = true; + data_published_ = true; + pMsg_cond_->notify_all(); } diff --git a/src/ros/rt_publisher.cpp b/src/ros/rt_publisher.cpp index d58b023da..26a283bfa 100644 --- a/src/ros/rt_publisher.cpp +++ b/src/ros/rt_publisher.cpp @@ -1,119 +1,122 @@ #include "ur_modern_driver/ros/rt_publisher.h" -bool RTPublisher::publish_joints(RTShared& packet, Time& t) +bool RTPublisher::publishJoints(RTShared& packet, Time& t) { - sensor_msgs::JointState joint_msg; - joint_msg.header.stamp = t; - joint_msg.name = _joint_names; - - for (auto const& q : packet.q_actual) { - joint_msg.position.push_back(q); - } - for (auto const& qd : packet.qd_actual) { - joint_msg.velocity.push_back(qd); - } - for (auto const& i : packet.i_actual) { - joint_msg.effort.push_back(i); - } - - _joint_pub.publish(joint_msg); - - return true; + sensor_msgs::JointState joint_msg; + joint_msg.header.stamp = t; + joint_msg.name = joint_names_; + + for (auto const& q : packet.q_actual) + { + joint_msg.position.push_back(q); + } + for (auto const& qd : packet.qd_actual) + { + joint_msg.velocity.push_back(qd); + } + for (auto const& i : packet.i_actual) + { + joint_msg.effort.push_back(i); + } + + joint_pub_.publish(joint_msg); + + return true; } -bool RTPublisher::publish_wrench(RTShared& packet, Time& t) +bool RTPublisher::publishWrench(RTShared& packet, Time& t) { - geometry_msgs::WrenchStamped wrench_msg; - wrench_msg.header.stamp = t; - wrench_msg.wrench.force.x = packet.tcp_force[0]; - wrench_msg.wrench.force.y = packet.tcp_force[1]; - wrench_msg.wrench.force.z = packet.tcp_force[2]; - wrench_msg.wrench.torque.x = packet.tcp_force[3]; - wrench_msg.wrench.torque.y = packet.tcp_force[4]; - wrench_msg.wrench.torque.z = packet.tcp_force[5]; - - _wrench_pub.publish(wrench_msg); - return true; + geometry_msgs::WrenchStamped wrench_msg; + wrench_msg.header.stamp = t; + wrench_msg.wrench.force.x = packet.tcp_force[0]; + wrench_msg.wrench.force.y = packet.tcp_force[1]; + wrench_msg.wrench.force.z = packet.tcp_force[2]; + wrench_msg.wrench.torque.x = packet.tcp_force[3]; + wrench_msg.wrench.torque.y = packet.tcp_force[4]; + wrench_msg.wrench.torque.z = packet.tcp_force[5]; + + wrench_pub_.publish(wrench_msg); + return true; } -bool RTPublisher::publish_tool(RTShared& packet, Time& t) +bool RTPublisher::publishTool(RTShared& packet, Time& t) { - geometry_msgs::TwistStamped tool_twist; - tool_twist.header.stamp = t; - tool_twist.header.frame_id = _base_frame; - tool_twist.twist.linear.x = packet.tcp_speed_actual.position.x; - tool_twist.twist.linear.y = packet.tcp_speed_actual.position.y; - tool_twist.twist.linear.z = packet.tcp_speed_actual.position.z; - tool_twist.twist.angular.x = packet.tcp_speed_actual.rotation.x; - tool_twist.twist.angular.y = packet.tcp_speed_actual.rotation.y; - tool_twist.twist.angular.z = packet.tcp_speed_actual.rotation.z; - - _tool_vel_pub.publish(tool_twist); - return true; + geometry_msgs::TwistStamped tool_twist; + tool_twist.header.stamp = t; + tool_twist.header.frame_id = base_frame_; + tool_twist.twist.linear.x = packet.tcp_speed_actual.position.x; + tool_twist.twist.linear.y = packet.tcp_speed_actual.position.y; + tool_twist.twist.linear.z = packet.tcp_speed_actual.position.z; + tool_twist.twist.angular.x = packet.tcp_speed_actual.rotation.x; + tool_twist.twist.angular.y = packet.tcp_speed_actual.rotation.y; + tool_twist.twist.angular.z = packet.tcp_speed_actual.rotation.z; + + tool_vel_pub_.publish(tool_twist); + return true; } -bool RTPublisher::publish_transform(RTShared& packet, Time& t) +bool RTPublisher::publishTransform(RTShared& packet, Time& t) { + auto tv = packet.tool_vector_actual; - auto tv = packet.tool_vector_actual; + Transform transform; + transform.setOrigin(Vector3(tv.position.x, tv.position.y, tv.position.z)); - Transform transform; - transform.setOrigin(Vector3(tv.position.x, tv.position.y, tv.position.z)); + // Create quaternion + Quaternion quat; - //Create quaternion - Quaternion quat; + double angle = std::sqrt(std::pow(tv.rotation.x, 2) + std::pow(tv.rotation.y, 2) + std::pow(tv.rotation.z, 2)); + if (angle < 1e-16) + { + quat.setValue(0, 0, 0, 1); + } + else + { + quat.setRotation(Vector3(tv.rotation.x / angle, tv.rotation.y / angle, tv.rotation.z / angle), angle); + } - double angle = std::sqrt(std::pow(tv.rotation.x, 2) + std::pow(tv.rotation.y, 2) + std::pow(tv.rotation.z, 2)); - if (angle < 1e-16) { - quat.setValue(0, 0, 0, 1); - } else { - quat.setRotation(Vector3(tv.rotation.x / angle, tv.rotation.y / angle, tv.rotation.z / angle), angle); - } + transform.setRotation(quat); - transform.setRotation(quat); + transform_broadcaster_.sendTransform(StampedTransform(transform, t, base_frame_, tool_frame_)); - _transform_broadcaster.sendTransform(StampedTransform(transform, t, _base_frame, _tool_frame)); - - return true; + return true; } -bool RTPublisher::publish_temperature(RTShared& packet, Time& t) +bool RTPublisher::publishTemperature(RTShared& packet, Time& t) { - size_t len = _joint_names.size(); - for (size_t i = 0; i < len; i++) { - sensor_msgs::Temperature msg; - msg.header.stamp = t; - msg.header.frame_id = _joint_names[i]; - msg.temperature = packet.motor_temperatures[i]; - - _joint_temperature_pub.publish(msg); - } - return true; + size_t len = joint_names_.size(); + for (size_t i = 0; i < len; i++) + { + sensor_msgs::Temperature msg; + msg.header.stamp = t; + msg.header.frame_id = joint_names_[i]; + msg.temperature = packet.motor_temperatures[i]; + + joint_temperature_pub_.publish(msg); + } + return true; } bool RTPublisher::publish(RTShared& packet) { - Time time = Time::now(); - return publish_joints(packet, time) - && publish_wrench(packet, time) - && publish_tool(packet, time) - && publish_transform(packet, time) - && publish_temperature(packet, time); + Time time = Time::now(); + return publishJoints(packet, time) && publishWrench(packet, time) && publishTool(packet, time) && + publishTransform(packet, time) && publishTemperature(packet, time); } bool RTPublisher::consume(RTState_V1_6__7& state) { - return publish(state); + return publish(state); } bool RTPublisher::consume(RTState_V1_8& state) { - return publish(state); + return publish(state); } bool RTPublisher::consume(RTState_V3_0__1& state) { - return publish(state); + return publish(state); } bool RTPublisher::consume(RTState_V3_2__3& state) { - return publish(state); + return publish(state); } \ No newline at end of file diff --git a/src/ros_main.cpp b/src/ros_main.cpp index adf7a9918..09b142a7a 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -1,6 +1,6 @@ +#include #include #include -#include #include #include @@ -21,55 +21,59 @@ static const std::string PREFIX_ARG("~prefix"); static const std::string BASE_FRAME_ARG("~base_frame"); static const std::string TOOL_FRAME_ARG("~tool_frame"); -struct ProgArgs { +struct ProgArgs +{ public: - std::string host; - std::string prefix; - std::string base_frame; - std::string tool_frame; - int32_t reverse_port; - bool use_sim_time; + std::string host; + std::string prefix; + std::string base_frame; + std::string tool_frame; + int32_t reverse_port; + bool use_sim_time; }; bool parse_args(ProgArgs& args) { - if (!ros::param::get(IP_ADDR_ARG, args.host)) { - LOG_ERROR("robot_ip_address parameter must be set!"); - return false; - } - ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001)); - ros::param::param(SIM_TIME_ARG, args.use_sim_time, false); - ros::param::param(PREFIX_ARG, args.prefix, std::string()); - ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link"); - ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller"); - return true; + if (!ros::param::get(IP_ADDR_ARG, args.host)) + { + LOG_ERROR("robot_ip_address parameter must be set!"); + return false; + } + ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001)); + ros::param::param(SIM_TIME_ARG, args.use_sim_time, false); + ros::param::param(PREFIX_ARG, args.prefix, std::string()); + ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link"); + ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller"); + return true; } int main(int argc, char** argv) { - ros::init(argc, argv, "ur_driver"); + ros::init(argc, argv, "ur_driver"); - ProgArgs args; - if (!parse_args(args)) { - return EXIT_FAILURE; - } + ProgArgs args; + if (!parse_args(args)) + { + return EXIT_FAILURE; + } - URFactory factory(args.host); - auto parser = factory.get_rt_parser(); + URFactory factory(args.host); + auto parser = factory.getRTParser(); - URStream s2(args.host, 30003); - URProducer p2(s2, *parser); - RTPublisher pub(args.prefix, args.base_frame, args.tool_frame); + URStream s2(args.host, 30003); + URProducer p2(s2, *parser); + RTPublisher pub(args.prefix, args.base_frame, args.tool_frame); - Pipeline pl(p2, pub); + Pipeline pl(p2, pub); - pl.run(); + pl.run(); - while (ros::ok()) { - std::this_thread::sleep_for(std::chrono::milliseconds(33)); - } + while (ros::ok()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(33)); + } - pl.stop(); + pl.stop(); - return EXIT_SUCCESS; + return EXIT_SUCCESS; } \ No newline at end of file diff --git a/src/ur/master_board.cpp b/src/ur/master_board.cpp index 046d0eed1..ecada6929 100644 --- a/src/ur/master_board.cpp +++ b/src/ur/master_board.cpp @@ -1,97 +1,99 @@ #include "ur_modern_driver/ur/master_board.h" #include "ur_modern_driver/ur/consumer.h" -bool SharedMasterBoardData::parse_with(BinParser& bp) +bool SharedMasterBoardData::parseWith(BinParser& bp) { - bp.parse(analog_input_range0); - bp.parse(analog_input_range1); - bp.parse(analog_input0); - bp.parse(analog_input1); - bp.parse(analog_output_domain0); - bp.parse(analog_output_domain1); - bp.parse(analog_output0); - bp.parse(analog_output1); - bp.parse(master_board_temperature); - bp.parse(robot_voltage_48V); - bp.parse(robot_current); - bp.parse(master_IO_current); - return true; + bp.parse(analog_input_range0); + bp.parse(analog_input_range1); + bp.parse(analog_input0); + bp.parse(analog_input1); + bp.parse(analog_output_domain0); + bp.parse(analog_output_domain1); + bp.parse(analog_output0); + bp.parse(analog_output1); + bp.parse(master_board_temperature); + bp.parse(robot_voltage_48V); + bp.parse(robot_current); + bp.parse(master_IO_current); + return true; } -bool MasterBoardData_V1_X::parse_with(BinParser& bp) +bool MasterBoardData_V1_X::parseWith(BinParser& bp) { - if (!bp.check_size()) - return false; + if (!bp.checkSize()) + return false; - bp.parse(digital_input_bits); - bp.parse(digital_output_bits); + bp.parse(digital_input_bits); + bp.parse(digital_output_bits); - SharedMasterBoardData::parse_with(bp); + SharedMasterBoardData::parseWith(bp); - bp.parse(master_safety_state); - bp.parse(master_on_off_state); - bp.parse(euromap67_interface_installed); + bp.parse(master_safety_state); + bp.parse(master_on_off_state); + bp.parse(euromap67_interface_installed); - if (euromap67_interface_installed) { - if (!bp.check_size(MasterBoardData_V1_X::EURO_SIZE)) - return false; + if (euromap67_interface_installed) + { + if (!bp.checkSize(MasterBoardData_V1_X::EURO_SIZE)) + return false; - bp.parse(euromap_voltage); - bp.parse(euromap_current); - } + bp.parse(euromap_voltage); + bp.parse(euromap_current); + } - return true; + return true; } -bool MasterBoardData_V3_0__1::parse_with(BinParser& bp) +bool MasterBoardData_V3_0__1::parseWith(BinParser& bp) { - if (!bp.check_size()) - return false; + if (!bp.checkSize()) + return false; - bp.parse(digital_input_bits); - bp.parse(digital_output_bits); + bp.parse(digital_input_bits); + bp.parse(digital_output_bits); - SharedMasterBoardData::parse_with(bp); + SharedMasterBoardData::parseWith(bp); - bp.parse(safety_mode); - bp.parse(in_reduced_mode); - bp.parse(euromap67_interface_installed); + bp.parse(safety_mode); + bp.parse(in_reduced_mode); + bp.parse(euromap67_interface_installed); - if (euromap67_interface_installed) { - if (!bp.check_size(MasterBoardData_V3_0__1::EURO_SIZE)) - return false; + if (euromap67_interface_installed) + { + if (!bp.checkSize(MasterBoardData_V3_0__1::EURO_SIZE)) + return false; - bp.parse(euromap_voltage); - bp.parse(euromap_current); - } + bp.parse(euromap_voltage); + bp.parse(euromap_current); + } - bp.consume(sizeof(uint32_t)); + bp.consume(sizeof(uint32_t)); - return true; + return true; } -bool MasterBoardData_V3_2::parse_with(BinParser& bp) +bool MasterBoardData_V3_2::parseWith(BinParser& bp) { - if (!bp.check_size()) - return false; + if (!bp.checkSize()) + return false; - MasterBoardData_V3_0__1::parse_with(bp); + MasterBoardData_V3_0__1::parseWith(bp); - bp.parse(operational_mode_selector_input); - bp.parse(three_position_enabling_device_input); + bp.parse(operational_mode_selector_input); + bp.parse(three_position_enabling_device_input); - return true; + return true; } -bool MasterBoardData_V1_X::consume_with(URStatePacketConsumer& consumer) +bool MasterBoardData_V1_X::consumeWith(URStatePacketConsumer& consumer) { - return consumer.consume(*this); + return consumer.consume(*this); } -bool MasterBoardData_V3_0__1::consume_with(URStatePacketConsumer& consumer) +bool MasterBoardData_V3_0__1::consumeWith(URStatePacketConsumer& consumer) { - return consumer.consume(*this); + return consumer.consume(*this); } -bool MasterBoardData_V3_2::consume_with(URStatePacketConsumer& consumer) +bool MasterBoardData_V3_2::consumeWith(URStatePacketConsumer& consumer) { - return consumer.consume(*this); + return consumer.consume(*this); } \ No newline at end of file diff --git a/src/ur/messages.cpp b/src/ur/messages.cpp index b53279210..5cfc4403e 100644 --- a/src/ur/messages.cpp +++ b/src/ur/messages.cpp @@ -1,20 +1,19 @@ #include "ur_modern_driver/ur/messages.h" #include "ur_modern_driver/ur/consumer.h" -bool VersionMessage::parse_with(BinParser& bp) +bool VersionMessage::parseWith(BinParser& bp) { + bp.parse(project_name); + bp.parse(major_version); + bp.parse(minor_version); + bp.parse(svn_version); + bp.consume(sizeof(uint32_t)); // undocumented field?? + bp.parse_remainder(build_date); - bp.parse(project_name); - bp.parse(major_version); - bp.parse(minor_version); - bp.parse(svn_version); - bp.consume(sizeof(uint32_t)); //undocumented field?? - bp.parse_remainder(build_date); - - return true; //not really possible to check dynamic size packets + return true; // not really possible to check dynamic size packets } -bool VersionMessage::consume_with(URMessagePacketConsumer& consumer) +bool VersionMessage::consumeWith(URMessagePacketConsumer& consumer) { - return consumer.consume(*this); + return consumer.consume(*this); } \ No newline at end of file diff --git a/src/ur/robot_mode.cpp b/src/ur/robot_mode.cpp index be0770f62..5bee4cf49 100644 --- a/src/ur/robot_mode.cpp +++ b/src/ur/robot_mode.cpp @@ -1,71 +1,71 @@ #include "ur_modern_driver/ur/robot_mode.h" #include "ur_modern_driver/ur/consumer.h" -bool SharedRobotModeData::parse_with(BinParser& bp) +bool SharedRobotModeData::parseWith(BinParser& bp) { - bp.parse(timestamp); - bp.parse(physical_robot_connected); - bp.parse(real_robot_enabled); - bp.parse(robot_power_on); - bp.parse(emergency_stopped); - return true; + bp.parse(timestamp); + bp.parse(physical_robot_connected); + bp.parse(real_robot_enabled); + bp.parse(robot_power_on); + bp.parse(emergency_stopped); + return true; } -bool RobotModeData_V1_X::parse_with(BinParser& bp) +bool RobotModeData_V1_X::parseWith(BinParser& bp) { - if (!bp.check_size()) - return false; + if (!bp.checkSize()) + return false; - SharedRobotModeData::parse_with(bp); + SharedRobotModeData::parseWith(bp); - bp.parse(security_stopped); - bp.parse(program_running); - bp.parse(program_paused); - bp.parse(robot_mode); - bp.parse(speed_fraction); + bp.parse(security_stopped); + bp.parse(program_running); + bp.parse(program_paused); + bp.parse(robot_mode); + bp.parse(speed_fraction); - return true; + return true; } -bool RobotModeData_V3_0__1::parse_with(BinParser& bp) +bool RobotModeData_V3_0__1::parseWith(BinParser& bp) { - if (!bp.check_size()) - return false; + if (!bp.checkSize()) + return false; - SharedRobotModeData::parse_with(bp); + SharedRobotModeData::parseWith(bp); - bp.parse(protective_stopped); - bp.parse(program_running); - bp.parse(program_paused); - bp.parse(robot_mode); - bp.parse(control_mode); - bp.parse(target_speed_fraction); - bp.parse(speed_scaling); + bp.parse(protective_stopped); + bp.parse(program_running); + bp.parse(program_paused); + bp.parse(robot_mode); + bp.parse(control_mode); + bp.parse(target_speed_fraction); + bp.parse(speed_scaling); - return true; + return true; } -bool RobotModeData_V3_2::parse_with(BinParser& bp) +bool RobotModeData_V3_2::parseWith(BinParser& bp) { - if (!bp.check_size()) - return false; + if (!bp.checkSize()) + return false; - RobotModeData_V3_0__1::parse_with(bp); + RobotModeData_V3_0__1::parseWith(bp); - bp.parse(target_speed_fraction_limit); + bp.parse(target_speed_fraction_limit); - return true; + return true; } -bool RobotModeData_V1_X::consume_with(URStatePacketConsumer& consumer) +bool RobotModeData_V1_X::consumeWith(URStatePacketConsumer& consumer) { - return consumer.consume(*this); + return consumer.consume(*this); } -bool RobotModeData_V3_0__1::consume_with(URStatePacketConsumer& consumer) +bool RobotModeData_V3_0__1::consumeWith(URStatePacketConsumer& consumer) { - return consumer.consume(*this); + return consumer.consume(*this); } -bool RobotModeData_V3_2::consume_with(URStatePacketConsumer& consumer) +bool RobotModeData_V3_2::consumeWith(URStatePacketConsumer& consumer) { - return consumer.consume(*this); + return consumer.consume(*this); } \ No newline at end of file diff --git a/src/ur/rt_state.cpp b/src/ur/rt_state.cpp index 226483557..18d682294 100644 --- a/src/ur/rt_state.cpp +++ b/src/ur/rt_state.cpp @@ -3,115 +3,115 @@ void RTShared::parse_shared1(BinParser& bp) { - bp.parse(time); - bp.parse(q_target); - bp.parse(qd_target); - bp.parse(qdd_target); - bp.parse(i_target); - bp.parse(m_target); - bp.parse(q_actual); - bp.parse(qd_actual); - bp.parse(i_actual); + bp.parse(time); + bp.parse(q_target); + bp.parse(qd_target); + bp.parse(qdd_target); + bp.parse(i_target); + bp.parse(m_target); + bp.parse(q_actual); + bp.parse(qd_actual); + bp.parse(i_actual); } void RTShared::parse_shared2(BinParser& bp) { - bp.parse(digital_inputs); - bp.parse(motor_temperatures); - bp.parse(controller_time); - bp.consume(sizeof(double)); //Unused "Test value" field - bp.parse(robot_mode); + bp.parse(digital_inputs); + bp.parse(motor_temperatures); + bp.parse(controller_time); + bp.consume(sizeof(double)); // Unused "Test value" field + bp.parse(robot_mode); } -bool RTState_V1_6__7::parse_with(BinParser& bp) +bool RTState_V1_6__7::parseWith(BinParser& bp) { - if (!bp.check_size()) - return false; + if (!bp.checkSize()) + return false; - parse_shared1(bp); + parse_shared1(bp); - bp.parse(tool_accelerometer_values); - bp.consume(sizeof(double)*15); - bp.parse(tcp_force); - bp.parse(tool_vector_actual); - bp.parse(tcp_speed_actual); + bp.parse(tool_accelerometer_values); + bp.consume(sizeof(double) * 15); + bp.parse(tcp_force); + bp.parse(tool_vector_actual); + bp.parse(tcp_speed_actual); - parse_shared2(bp); + parse_shared2(bp); - return true; + return true; } -bool RTState_V1_8::parse_with(BinParser& bp) +bool RTState_V1_8::parseWith(BinParser& bp) { - if (!bp.check_size()) - return false; + if (!bp.checkSize()) + return false; - RTState_V1_6__7::parse_with(bp); + RTState_V1_6__7::parseWith(bp); - bp.parse(joint_modes); + bp.parse(joint_modes); - return true; + return true; } -bool RTState_V3_0__1::parse_with(BinParser& bp) +bool RTState_V3_0__1::parseWith(BinParser& bp) { - if (!bp.check_size()) - return false; - - parse_shared1(bp); - - bp.parse(i_control); - bp.parse(tool_vector_actual); - bp.parse(tcp_speed_actual); - bp.parse(tcp_force); - bp.parse(tool_vector_target); - bp.parse(tcp_speed_target); - - parse_shared2(bp); - - bp.parse(joint_modes); - bp.parse(safety_mode); - bp.consume(sizeof(double[6])); //skip undocumented - bp.parse(tool_accelerometer_values); - bp.consume(sizeof(double[6])); //skip undocumented - bp.parse(speed_scaling); - bp.parse(linear_momentum_norm); - bp.consume(sizeof(double)); //skip undocumented - bp.consume(sizeof(double)); //skip undocumented - bp.parse(v_main); - bp.parse(v_robot); - bp.parse(i_robot); - bp.parse(v_actual); - - return true; + if (!bp.checkSize()) + return false; + + parse_shared1(bp); + + bp.parse(i_control); + bp.parse(tool_vector_actual); + bp.parse(tcp_speed_actual); + bp.parse(tcp_force); + bp.parse(tool_vector_target); + bp.parse(tcp_speed_target); + + parse_shared2(bp); + + bp.parse(joint_modes); + bp.parse(safety_mode); + bp.consume(sizeof(double[6])); // skip undocumented + bp.parse(tool_accelerometer_values); + bp.consume(sizeof(double[6])); // skip undocumented + bp.parse(speed_scaling); + bp.parse(linear_momentum_norm); + bp.consume(sizeof(double)); // skip undocumented + bp.consume(sizeof(double)); // skip undocumented + bp.parse(v_main); + bp.parse(v_robot); + bp.parse(i_robot); + bp.parse(v_actual); + + return true; } -bool RTState_V3_2__3::parse_with(BinParser& bp) +bool RTState_V3_2__3::parseWith(BinParser& bp) { - if (!bp.check_size()) - return false; + if (!bp.checkSize()) + return false; - RTState_V3_0__1::parse_with(bp); + RTState_V3_0__1::parseWith(bp); - bp.parse(digital_outputs); - bp.parse(program_state); + bp.parse(digital_outputs); + bp.parse(program_state); - return true; + return true; } -bool RTState_V1_6__7::consume_with(URRTPacketConsumer& consumer) +bool RTState_V1_6__7::consumeWith(URRTPacketConsumer& consumer) { - return consumer.consume(*this); + return consumer.consume(*this); } -bool RTState_V1_8::consume_with(URRTPacketConsumer& consumer) +bool RTState_V1_8::consumeWith(URRTPacketConsumer& consumer) { - return consumer.consume(*this); + return consumer.consume(*this); } -bool RTState_V3_0__1::consume_with(URRTPacketConsumer& consumer) +bool RTState_V3_0__1::consumeWith(URRTPacketConsumer& consumer) { - return consumer.consume(*this); + return consumer.consume(*this); } -bool RTState_V3_2__3::consume_with(URRTPacketConsumer& consumer) +bool RTState_V3_2__3::consumeWith(URRTPacketConsumer& consumer) { - return consumer.consume(*this); + return consumer.consume(*this); } \ No newline at end of file diff --git a/src/ur/state.cpp b/src/ur/state.cpp index 0b4f4f2e8..66af615b6 100644 --- a/src/ur/state.cpp +++ b/src/ur/state.cpp @@ -1,25 +1,25 @@ #include "ur_modern_driver/ur/state.h" #include "ur_modern_driver/log.h" -//StatePacket::~StatePacket() { } +// StatePacket::~StatePacket() { } /* -bool RobotState::parse_with(BinParser &bp) { +bool RobotState::parseWith(BinParser &bp) { //continue as long as there are bytes to read while(!bp.empty()) { - - if(!bp.check_size(sizeof(uint32_t))){ + + if(!bp.checkSize(sizeof(uint32_t))){ LOG_ERROR("Failed to read sub-package length, there's likely a parsing error"); - return false; + return false; } uint32_t sub_size = bp.peek(); - if(!bp.check_size(static_cast(sub_size))) { + if(!bp.checkSize(static_cast(sub_size))) { LOG_WARN("Invalid sub-package size of %" PRIu32 " received!", sub_size); return false; } - - //deconstruction of a sub parser will increment the position of the parent parser + + //deconstruction of a sub parser will increment the position of the parent parser BinParser sub_parser(bp, sub_size); sub_parser.consume(sizeof(sub_size)); @@ -43,19 +43,19 @@ bool parse_base(BinParser &bp, T &pkg) { case package_type::ROBOT_MODE_DATA: LOG_DEBUG("Parsing robot_mode"); bp.consume(sizeof(package_type)); - pkg.robot_mode.parse_with(bp); + pkg.robot_mode.parseWith(bp); break; case package_type::MASTERBOARD_DATA: LOG_DEBUG("Parsing master_board"); bp.consume(sizeof(package_type)); - pkg.master_board.parse_with(bp); + pkg.master_board.parseWith(bp); break; case package_type::TOOL_DATA: case package_type::CARTESIAN_INFO: case package_type::JOINT_DATA: - LOG_DEBUG("Skipping tool, cartesian or joint data"); - //for unhandled packages we consume the rest of the + LOG_DEBUG("Skipping tool, cartesian or joint data"); + //for unhandled packages we consume the rest of the //package buffer bp.consume(); break; @@ -79,8 +79,8 @@ bool parse_advanced(BinParser &bp, T &pkg) { case package_type::ADDITIONAL_INFO: case package_type::CALIBRATION_DATA: case package_type::FORCE_MODE_DATA: - LOG_DEBUG("Skipping kinematics, config, additional, calibration or force mode data"); - //for unhandled packages we consume the rest of the + LOG_DEBUG("Skipping kinematics, config, additional, calibration or force mode data"); + //for unhandled packages we consume the rest of the //package buffer bp.consume(); break; diff --git a/src/ur/stream.cpp b/src/ur/stream.cpp index 955d1f3b6..819651e34 100644 --- a/src/ur/stream.cpp +++ b/src/ur/stream.cpp @@ -1,126 +1,133 @@ -#include #include #include #include +#include #include "ur_modern_driver/log.h" #include "ur_modern_driver/ur/stream.h" bool URStream::connect() { - if (_initialized) - return false; + if (initialized_) + return false; - LOG_INFO("Connecting to UR @ %s:%d", _host.c_str(), _port); + LOG_INFO("Connecting to UR @ %s:%d", host_.c_str(), port_); - //gethostbyname() is deprecated so use getadderinfo() as described in: - //http://www.beej.us/guide/bgnet/output/html/multipage/syscalls.html#getaddrinfo + // gethostbyname() is deprecated so use getadderinfo() as described in: + // http://www.beej.us/guide/bgnet/output/html/multipage/syscalls.html#getaddrinfo - std::string service = std::to_string(_port); - struct addrinfo hints, *result; - std::memset(&hints, 0, sizeof(hints)); + std::string service = std::to_string(port_); + struct addrinfo hints, *result; + std::memset(&hints, 0, sizeof(hints)); - hints.ai_family = AF_UNSPEC; - hints.ai_socktype = SOCK_STREAM; - hints.ai_flags = AI_PASSIVE; + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_STREAM; + hints.ai_flags = AI_PASSIVE; - if (getaddrinfo(_host.c_str(), service.c_str(), &hints, &result) != 0) { - LOG_ERROR("Failed to get host name"); - return false; - } + if (getaddrinfo(host_.c_str(), service.c_str(), &hints, &result) != 0) + { + LOG_ERROR("Failed to get host name"); + return false; + } - //loop through the list of addresses untill we find one that's connectable - for (struct addrinfo* p = result; p != nullptr; p = p->ai_next) { - _socket_fd = socket(p->ai_family, p->ai_socktype, p->ai_protocol); - - if (_socket_fd == -1) //socket error? - continue; - - if (::connect(_socket_fd, p->ai_addr, p->ai_addrlen) != 0) { - if (_stopping) - break; - else - continue; //try next addrinfo if connect fails - } - - //disable Nagle's algorithm to ensure we sent packets as fast as possible - int flag = 1; - setsockopt(_socket_fd, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(flag)); - _initialized = true; - LOG_INFO("Connection successfully established"); + // loop through the list of addresses untill we find one that's connectable + for (struct addrinfo* p = result; p != nullptr; p = p->ai_next) + { + socket_fd_ = socket(p->ai_family, p->ai_socktype, p->ai_protocol); + + if (socket_fd_ == -1) // socket error? + continue; + + if (::connect(socket_fd_, p->ai_addr, p->ai_addrlen) != 0) + { + if (stopping_) break; + else + continue; // try next addrinfo if connect fails } - freeaddrinfo(result); - if (!_initialized) - LOG_ERROR("Connection failed"); + // disable Nagle's algorithm to ensure we sent packets as fast as possible + int flag = 1; + setsockopt(socket_fd_, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(flag)); + initialized_ = true; + LOG_INFO("Connection successfully established"); + break; + } + + freeaddrinfo(result); + if (!initialized_) + LOG_ERROR("Connection failed"); - return _initialized; + return initialized_; } void URStream::disconnect() { - if (!_initialized || _stopping) - return; + if (!initialized_ || stopping_) + return; - LOG_INFO("Disconnecting from %s:%d", _host.c_str(), _port); + LOG_INFO("Disconnecting from %s:%d", host_.c_str(), port_); - _stopping = true; - close(_socket_fd); - _initialized = false; + stopping_ = true; + close(socket_fd_); + initialized_ = false; } ssize_t URStream::send(uint8_t* buf, size_t buf_len) { - if (!_initialized) - return -1; - if (_stopping) - return 0; - - size_t total = 0; - size_t remaining = 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) - return _stopping ? 0 : sent; - total += sent; - remaining -= sent; - } - - return total; + if (!initialized_) + return -1; + if (stopping_) + return 0; + + size_t total = 0; + size_t remaining = 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) + return stopping_ ? 0 : sent; + total += sent; + remaining -= sent; + } + + return total; } ssize_t URStream::receive(uint8_t* buf, size_t buf_len) { - if (!_initialized) + if (!initialized_) + return -1; + if (stopping_) + return 0; + + size_t remainder = sizeof(int32_t); + uint8_t* buf_pos = buf; + bool initial = true; + + do + { + ssize_t read = recv(socket_fd_, buf_pos, remainder, 0); + if (read <= 0) // failed reading from socket + return stopping_ ? 0 : read; + + if (initial) + { + remainder = be32toh(*(reinterpret_cast(buf))); + if (remainder >= (buf_len - sizeof(int32_t))) + { + LOG_ERROR("Packet size %zd is larger than buffer %zu, discarding.", remainder, buf_len); return -1; - if (_stopping) - return 0; - - size_t remainder = sizeof(int32_t); - uint8_t* buf_pos = buf; - bool initial = true; - - do { - ssize_t read = recv(_socket_fd, buf_pos, remainder, 0); - if (read <= 0) //failed reading from socket - return _stopping ? 0 : read; - - if (initial) { - remainder = be32toh(*(reinterpret_cast(buf))); - if (remainder >= (buf_len - sizeof(int32_t))) { - LOG_ERROR("Packet size %zd is larger than buffer %zu, discarding.", remainder, buf_len); - return -1; - } - initial = false; - } - - buf_pos += read; - remainder -= read; - } while (remainder > 0); - - return buf_pos - buf; + } + initial = false; + } + + buf_pos += read; + remainder -= read; + } while (remainder > 0); + + return buf_pos - buf; } \ No newline at end of file diff --git a/src/ur_communication.cpp b/src/ur_communication.cpp index b88300451..d7da04f62 100644 --- a/src/ur_communication.cpp +++ b/src/ur_communication.cpp @@ -18,168 +18,167 @@ #include "ur_modern_driver/ur_communication.h" -UrCommunication::UrCommunication(std::condition_variable& msg_cond, - std::string host) +UrCommunication::UrCommunication(std::condition_variable& msg_cond, std::string host) { - robot_state_ = new RobotState(msg_cond); - bzero((char*)&pri_serv_addr_, sizeof(pri_serv_addr_)); - bzero((char*)&sec_serv_addr_, sizeof(sec_serv_addr_)); - pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (pri_sockfd_ < 0) { - print_fatal("ERROR opening socket pri_sockfd"); - } - sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (sec_sockfd_ < 0) { - print_fatal("ERROR opening socket sec_sockfd"); - } - server_ = gethostbyname(host.c_str()); - if (server_ == NULL) { - print_fatal("ERROR, unknown host"); - } - pri_serv_addr_.sin_family = AF_INET; - sec_serv_addr_.sin_family = AF_INET; - bcopy((char*)server_->h_addr, (char*)&pri_serv_addr_.sin_addr.s_addr, server_->h_length); - bcopy((char*)server_->h_addr, (char*)&sec_serv_addr_.sin_addr.s_addr, server_->h_length); - pri_serv_addr_.sin_port = htons(30001); - sec_serv_addr_.sin_port = htons(30002); - flag_ = 1; - setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, - sizeof(int)); - setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, - sizeof(int)); - setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, - sizeof(int)); - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, - sizeof(int)); - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, - sizeof(int)); - setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, - sizeof(int)); - fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); - connected_ = false; - keepalive_ = false; + robot_state_ = new RobotState(msg_cond); + bzero((char*)&pri_serv_addr_, sizeof(pri_serv_addr_)); + bzero((char*)&sec_serv_addr_, sizeof(sec_serv_addr_)); + pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (pri_sockfd_ < 0) + { + print_fatal("ERROR opening socket pri_sockfd"); + } + sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (sec_sockfd_ < 0) + { + print_fatal("ERROR opening socket sec_sockfd"); + } + server_ = gethostbyname(host.c_str()); + if (server_ == NULL) + { + print_fatal("ERROR, unknown host"); + } + pri_serv_addr_.sin_family = AF_INET; + sec_serv_addr_.sin_family = AF_INET; + bcopy((char*)server_->h_addr, (char*)&pri_serv_addr_.sin_addr.s_addr, server_->h_length); + bcopy((char*)server_->h_addr, (char*)&sec_serv_addr_.sin_addr.s_addr, server_->h_length); + pri_serv_addr_.sin_port = htons(30001); + sec_serv_addr_.sin_port = htons(30002); + flag_ = 1; + setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int)); + setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); + setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int)); + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int)); + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); + setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int)); + fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); + connected_ = false; + keepalive_ = false; } bool UrCommunication::start() { - keepalive_ = true; - uint8_t buf[512]; - unsigned int bytes_read; - std::string cmd; - bzero(buf, 512); - print_debug("Acquire firmware version: Connecting..."); - if (connect(pri_sockfd_, (struct sockaddr*)&pri_serv_addr_, - sizeof(pri_serv_addr_)) - < 0) { - print_fatal("Error connecting to get firmware version"); - return false; - } - print_debug("Acquire firmware version: Got connection"); - bytes_read = read(pri_sockfd_, buf, 512); - setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, - sizeof(int)); - robot_state_->unpack(buf, bytes_read); - //wait for some traffic so the UR socket doesn't die in version 3.1. - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - char tmp[64]; - sprintf(tmp, "Firmware version detected: %.7f", robot_state_->getVersion()); - print_debug(tmp); - close(pri_sockfd_); + keepalive_ = true; + uint8_t buf[512]; + unsigned int bytes_read; + std::string cmd; + bzero(buf, 512); + print_debug("Acquire firmware version: Connecting..."); + if (connect(pri_sockfd_, (struct sockaddr*)&pri_serv_addr_, sizeof(pri_serv_addr_)) < 0) + { + print_fatal("Error connecting to get firmware version"); + return false; + } + print_debug("Acquire firmware version: Got connection"); + bytes_read = read(pri_sockfd_, buf, 512); + setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); + robot_state_->unpack(buf, bytes_read); + // wait for some traffic so the UR socket doesn't die in version 3.1. + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + char tmp[64]; + sprintf(tmp, "Firmware version detected: %.7f", robot_state_->getVersion()); + print_debug(tmp); + close(pri_sockfd_); - print_debug( - "Switching to secondary interface for masterboard data: Connecting..."); + print_debug("Switching to secondary interface for masterboard data: Connecting..."); - fd_set writefds; - struct timeval timeout; + fd_set writefds; + struct timeval timeout; - connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_, - sizeof(sec_serv_addr_)); - FD_ZERO(&writefds); - FD_SET(sec_sockfd_, &writefds); - timeout.tv_sec = 10; - timeout.tv_usec = 0; - select(sec_sockfd_ + 1, NULL, &writefds, NULL, &timeout); - unsigned int flag_len; - getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); - if (flag_ < 0) { - print_fatal("Error connecting to secondary interface"); - return false; - } - print_debug("Secondary interface: Got connection"); - comThread_ = std::thread(&UrCommunication::run, this); - return true; + connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_, sizeof(sec_serv_addr_)); + FD_ZERO(&writefds); + FD_SET(sec_sockfd_, &writefds); + timeout.tv_sec = 10; + timeout.tv_usec = 0; + select(sec_sockfd_ + 1, NULL, &writefds, NULL, &timeout); + unsigned int flag_len; + getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); + if (flag_ < 0) + { + print_fatal("Error connecting to secondary interface"); + return false; + } + print_debug("Secondary interface: Got connection"); + comThread_ = std::thread(&UrCommunication::run, this); + return true; } void UrCommunication::halt() { - keepalive_ = false; - comThread_.join(); + keepalive_ = false; + comThread_.join(); } void UrCommunication::run() { - uint8_t buf[2048]; - int bytes_read; - bzero(buf, 2048); - struct timeval timeout; - fd_set readfds; - FD_ZERO(&readfds); - FD_SET(sec_sockfd_, &readfds); - connected_ = true; - while (keepalive_) { - while (connected_ && keepalive_) { - timeout.tv_sec = 0; //do this each loop as selects modifies timeout - timeout.tv_usec = 500000; // timeout of 0.5 sec - select(sec_sockfd_ + 1, &readfds, NULL, NULL, &timeout); - bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes - if (bytes_read > 0) { - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, - (char*)&flag_, sizeof(int)); - robot_state_->unpack(buf, bytes_read); - } else { - connected_ = false; - robot_state_->setDisconnected(); - close(sec_sockfd_); - } - } - if (keepalive_) { - //reconnect - print_warning("Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."); - sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (sec_sockfd_ < 0) { - print_fatal("ERROR opening secondary socket"); - } - flag_ = 1; - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, - sizeof(int)); - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, - sizeof(int)); - setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, - sizeof(int)); - fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); - while (keepalive_ && !connected_) { - std::this_thread::sleep_for(std::chrono::seconds(10)); - fd_set writefds; + uint8_t buf[2048]; + int bytes_read; + bzero(buf, 2048); + struct timeval timeout; + fd_set readfds; + FD_ZERO(&readfds); + FD_SET(sec_sockfd_, &readfds); + connected_ = true; + while (keepalive_) + { + while (connected_ && keepalive_) + { + timeout.tv_sec = 0; // do this each loop as selects modifies timeout + timeout.tv_usec = 500000; // timeout of 0.5 sec + select(sec_sockfd_ + 1, &readfds, NULL, NULL, &timeout); + bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes + if (bytes_read > 0) + { + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); + robot_state_->unpack(buf, bytes_read); + } + else + { + connected_ = false; + robot_state_->setDisconnected(); + close(sec_sockfd_); + } + } + if (keepalive_) + { + // reconnect + print_warning("Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."); + sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (sec_sockfd_ < 0) + { + print_fatal("ERROR opening secondary socket"); + } + flag_ = 1; + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int)); + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); + setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int)); + fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); + while (keepalive_ && !connected_) + { + std::this_thread::sleep_for(std::chrono::seconds(10)); + fd_set writefds; - connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_, - sizeof(sec_serv_addr_)); - FD_ZERO(&writefds); - FD_SET(sec_sockfd_, &writefds); - select(sec_sockfd_ + 1, NULL, &writefds, NULL, NULL); - unsigned int flag_len; - getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, - &flag_len); - if (flag_ < 0) { - print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds..."); - } else { - connected_ = true; - print_info("Secondary port: Reconnected"); - } - } + connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_, sizeof(sec_serv_addr_)); + FD_ZERO(&writefds); + FD_SET(sec_sockfd_, &writefds); + select(sec_sockfd_ + 1, NULL, &writefds, NULL, NULL); + unsigned int flag_len; + getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); + if (flag_ < 0) + { + print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 " + "seconds..."); + } + else + { + connected_ = true; + print_info("Secondary port: Reconnected"); } + } } + } - //wait for some traffic so the UR socket doesn't die in version 3.1. - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - close(sec_sockfd_); + // wait for some traffic so the UR socket doesn't die in version 3.1. + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + close(sec_sockfd_); } diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index 08a535a44..c5cf3bdd9 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -18,397 +18,403 @@ #include "ur_modern_driver/ur_driver.h" -UrDriver::UrDriver(std::condition_variable& rt_msg_cond, - std::condition_variable& msg_cond, std::string host, - unsigned int reverse_port, double servoj_time, - unsigned int safety_count_max, double max_time_step, double min_payload, - double max_payload, double servoj_lookahead_time, double servoj_gain) - : REVERSE_PORT_(reverse_port) - , maximum_time_step_(max_time_step) - , minimum_payload_( - min_payload) - , maximum_payload_(max_payload) - , servoj_time_( - servoj_time) - , servoj_lookahead_time_(servoj_lookahead_time) - , servoj_gain_(servoj_gain) +UrDriver::UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host, + unsigned int reverse_port, double servoj_time, unsigned int safety_count_max, double max_time_step, + double min_payload, double max_payload, double servoj_lookahead_time, double servoj_gain) + : REVERSE_PORT_(reverse_port) + , maximum_time_step_(max_time_step) + , minimum_payload_(min_payload) + , maximum_payload_(max_payload) + , servoj_time_(servoj_time) + , servoj_lookahead_time_(servoj_lookahead_time) + , servoj_gain_(servoj_gain) { - char buffer[256]; - struct sockaddr_in serv_addr; - int n, flag; + char buffer[256]; + struct sockaddr_in serv_addr; + int n, flag; - firmware_version_ = 0; - reverse_connected_ = false; - executing_traj_ = false; - rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host, - safety_count_max); - new_sockfd_ = -1; - sec_interface_ = new UrCommunication(msg_cond, host); + firmware_version_ = 0; + reverse_connected_ = false; + executing_traj_ = false; + rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host, safety_count_max); + new_sockfd_ = -1; + sec_interface_ = new UrCommunication(msg_cond, host); - incoming_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (incoming_sockfd_ < 0) { - print_fatal("ERROR opening socket for reverse communication"); - } - bzero((char*)&serv_addr, sizeof(serv_addr)); + incoming_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (incoming_sockfd_ < 0) + { + print_fatal("ERROR opening socket for reverse communication"); + } + bzero((char*)&serv_addr, sizeof(serv_addr)); - serv_addr.sin_family = AF_INET; - serv_addr.sin_addr.s_addr = INADDR_ANY; - serv_addr.sin_port = htons(REVERSE_PORT_); - flag = 1; - setsockopt(incoming_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag, - sizeof(int)); - setsockopt(incoming_sockfd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int)); - if (bind(incoming_sockfd_, (struct sockaddr*)&serv_addr, - sizeof(serv_addr)) - < 0) { - print_fatal("ERROR on binding socket for reverse communication"); - } - listen(incoming_sockfd_, 5); + serv_addr.sin_family = AF_INET; + serv_addr.sin_addr.s_addr = INADDR_ANY; + serv_addr.sin_port = htons(REVERSE_PORT_); + flag = 1; + setsockopt(incoming_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag, sizeof(int)); + setsockopt(incoming_sockfd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int)); + if (bind(incoming_sockfd_, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0) + { + print_fatal("ERROR on binding socket for reverse communication"); + } + listen(incoming_sockfd_, 5); } -std::vector UrDriver::interp_cubic(double t, double T, - std::vector p0_pos, std::vector p1_pos, - std::vector p0_vel, std::vector p1_vel) +std::vector UrDriver::interp_cubic(double t, double T, std::vector p0_pos, std::vector p1_pos, + std::vector p0_vel, std::vector p1_vel) { - /*Returns positions of the joints at time 't' */ - std::vector positions; - for (unsigned int i = 0; i < p0_pos.size(); i++) { - double a = p0_pos[i]; - double b = p0_vel[i]; - double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i] - - T * p1_vel[i]) - / pow(T, 2); - double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i] - + T * p1_vel[i]) - / pow(T, 3); - positions.push_back(a + b * t + c * pow(t, 2) + d * pow(t, 3)); - } - return positions; + /*Returns positions of the joints at time 't' */ + std::vector positions; + for (unsigned int i = 0; i < p0_pos.size(); i++) + { + double a = p0_pos[i]; + double b = p0_vel[i]; + double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i] - T * p1_vel[i]) / pow(T, 2); + double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i] + T * p1_vel[i]) / pow(T, 3); + positions.push_back(a + b * t + c * pow(t, 2) + d * pow(t, 3)); + } + return positions; } -bool UrDriver::doTraj(std::vector inp_timestamps, - std::vector > inp_positions, - std::vector > inp_velocities) +bool UrDriver::doTraj(std::vector inp_timestamps, std::vector> inp_positions, + std::vector> inp_velocities) { - std::chrono::high_resolution_clock::time_point t0, t; - std::vector positions; - unsigned int j; + std::chrono::high_resolution_clock::time_point t0, t; + std::vector positions; + unsigned int j; - if (!UrDriver::uploadProg()) { - return false; + if (!UrDriver::uploadProg()) + { + return false; + } + executing_traj_ = true; + t0 = std::chrono::high_resolution_clock::now(); + t = t0; + j = 0; + while ((inp_timestamps[inp_timestamps.size() - 1] >= + std::chrono::duration_cast>(t - t0).count()) and + executing_traj_) + { + while (inp_timestamps[j] <= std::chrono::duration_cast>(t - t0).count() && + j < inp_timestamps.size() - 1) + { + j += 1; } - executing_traj_ = true; - t0 = std::chrono::high_resolution_clock::now(); - t = t0; - j = 0; - while ((inp_timestamps[inp_timestamps.size() - 1] - >= std::chrono::duration_cast >(t - t0).count()) - and executing_traj_) { - while (inp_timestamps[j] - <= std::chrono::duration_cast >( - t - t0) - .count() - && j < inp_timestamps.size() - 1) { - j += 1; - } - positions = UrDriver::interp_cubic( - std::chrono::duration_cast >( - t - t0) - .count() - - inp_timestamps[j - 1], - inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1], - inp_positions[j], inp_velocities[j - 1], inp_velocities[j]); - UrDriver::servoj(positions); + positions = UrDriver::interp_cubic(std::chrono::duration_cast>(t - t0).count() - + inp_timestamps[j - 1], + inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1], + inp_positions[j], inp_velocities[j - 1], inp_velocities[j]); + UrDriver::servoj(positions); - // oversample with 4 * sample_time - std::this_thread::sleep_for( - std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.))); - t = std::chrono::high_resolution_clock::now(); - } - executing_traj_ = false; - //Signal robot to stop driverProg() - UrDriver::closeServo(positions); - return true; + // oversample with 4 * sample_time + std::this_thread::sleep_for(std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.))); + t = std::chrono::high_resolution_clock::now(); + } + executing_traj_ = false; + // Signal robot to stop driverProg() + UrDriver::closeServo(positions); + return true; } void UrDriver::servoj(std::vector positions, int keepalive) { - if (!reverse_connected_) { - print_error( - "UrDriver::servoj called without a reverse connection present. Keepalive: " - + std::to_string(keepalive)); - return; - } - unsigned int bytes_written; - int tmp; - unsigned char buf[28]; - for (int i = 0; i < 6; i++) { - tmp = htonl((int)(positions[i] * MULT_JOINTSTATE_)); - buf[i * 4] = tmp & 0xff; - buf[i * 4 + 1] = (tmp >> 8) & 0xff; - buf[i * 4 + 2] = (tmp >> 16) & 0xff; - buf[i * 4 + 3] = (tmp >> 24) & 0xff; - } - tmp = htonl((int)keepalive); - buf[6 * 4] = tmp & 0xff; - buf[6 * 4 + 1] = (tmp >> 8) & 0xff; - buf[6 * 4 + 2] = (tmp >> 16) & 0xff; - buf[6 * 4 + 3] = (tmp >> 24) & 0xff; - bytes_written = write(new_sockfd_, buf, 28); + if (!reverse_connected_) + { + print_error("UrDriver::servoj called without a reverse connection present. Keepalive: " + + std::to_string(keepalive)); + return; + } + unsigned int bytes_written; + int tmp; + unsigned char buf[28]; + for (int i = 0; i < 6; i++) + { + tmp = htonl((int)(positions[i] * MULT_JOINTSTATE_)); + buf[i * 4] = tmp & 0xff; + buf[i * 4 + 1] = (tmp >> 8) & 0xff; + buf[i * 4 + 2] = (tmp >> 16) & 0xff; + buf[i * 4 + 3] = (tmp >> 24) & 0xff; + } + tmp = htonl((int)keepalive); + buf[6 * 4] = tmp & 0xff; + buf[6 * 4 + 1] = (tmp >> 8) & 0xff; + buf[6 * 4 + 2] = (tmp >> 16) & 0xff; + buf[6 * 4 + 3] = (tmp >> 24) & 0xff; + bytes_written = write(new_sockfd_, buf, 28); } void UrDriver::stopTraj() { - executing_traj_ = false; - rt_interface_->addCommandToQueue("stopj(10)\n"); + executing_traj_ = false; + rt_interface_->addCommandToQueue("stopj(10)\n"); } bool UrDriver::uploadProg() { - std::string cmd_str; - char buf[128]; - cmd_str = "def driverProg():\n"; + std::string cmd_str; + char buf[128]; + cmd_str = "def driverProg():\n"; - sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_); - cmd_str += buf; + sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_); + cmd_str += buf; - cmd_str += "\tSERVO_IDLE = 0\n"; - cmd_str += "\tSERVO_RUNNING = 1\n"; - cmd_str += "\tcmd_servo_state = SERVO_IDLE\n"; - cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n"; - cmd_str += "\tdef set_servo_setpoint(q):\n"; - cmd_str += "\t\tenter_critical\n"; - cmd_str += "\t\tcmd_servo_state = SERVO_RUNNING\n"; - cmd_str += "\t\tcmd_servo_q = q\n"; - cmd_str += "\t\texit_critical\n"; - cmd_str += "\tend\n"; - cmd_str += "\tthread servoThread():\n"; - cmd_str += "\t\tstate = SERVO_IDLE\n"; - cmd_str += "\t\twhile True:\n"; - cmd_str += "\t\t\tenter_critical\n"; - cmd_str += "\t\t\tq = cmd_servo_q\n"; - cmd_str += "\t\t\tdo_brake = False\n"; - cmd_str += "\t\t\tif (state == SERVO_RUNNING) and "; - cmd_str += "(cmd_servo_state == SERVO_IDLE):\n"; - cmd_str += "\t\t\t\tdo_brake = True\n"; - cmd_str += "\t\t\tend\n"; - cmd_str += "\t\t\tstate = cmd_servo_state\n"; - cmd_str += "\t\t\tcmd_servo_state = SERVO_IDLE\n"; - cmd_str += "\t\t\texit_critical\n"; - cmd_str += "\t\t\tif do_brake:\n"; - cmd_str += "\t\t\t\tstopj(1.0)\n"; - cmd_str += "\t\t\t\tsync()\n"; - cmd_str += "\t\t\telif state == SERVO_RUNNING:\n"; + cmd_str += "\tSERVO_IDLE = 0\n"; + cmd_str += "\tSERVO_RUNNING = 1\n"; + cmd_str += "\tcmd_servo_state = SERVO_IDLE\n"; + cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n"; + cmd_str += "\tdef set_servo_setpoint(q):\n"; + cmd_str += "\t\tenter_critical\n"; + cmd_str += "\t\tcmd_servo_state = SERVO_RUNNING\n"; + cmd_str += "\t\tcmd_servo_q = q\n"; + cmd_str += "\t\texit_critical\n"; + cmd_str += "\tend\n"; + cmd_str += "\tthread servoThread():\n"; + cmd_str += "\t\tstate = SERVO_IDLE\n"; + cmd_str += "\t\twhile True:\n"; + cmd_str += "\t\t\tenter_critical\n"; + cmd_str += "\t\t\tq = cmd_servo_q\n"; + cmd_str += "\t\t\tdo_brake = False\n"; + cmd_str += "\t\t\tif (state == SERVO_RUNNING) and "; + cmd_str += "(cmd_servo_state == SERVO_IDLE):\n"; + cmd_str += "\t\t\t\tdo_brake = True\n"; + cmd_str += "\t\t\tend\n"; + cmd_str += "\t\t\tstate = cmd_servo_state\n"; + cmd_str += "\t\t\tcmd_servo_state = SERVO_IDLE\n"; + cmd_str += "\t\t\texit_critical\n"; + cmd_str += "\t\t\tif do_brake:\n"; + cmd_str += "\t\t\t\tstopj(1.0)\n"; + cmd_str += "\t\t\t\tsync()\n"; + cmd_str += "\t\t\telif state == SERVO_RUNNING:\n"; - if (sec_interface_->robot_state_->getVersion() >= 3.1) - sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=%.4f, gain=%.0f)\n", - servoj_time_, servoj_lookahead_time_, servoj_gain_); - else - sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_); - cmd_str += buf; + if (sec_interface_->robot_state_->getVersion() >= 3.1) + sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=%.4f, gain=%.0f)\n", servoj_time_, servoj_lookahead_time_, + servoj_gain_); + else + sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_); + cmd_str += buf; - cmd_str += "\t\t\telse:\n"; - cmd_str += "\t\t\t\tsync()\n"; - cmd_str += "\t\t\tend\n"; - cmd_str += "\t\tend\n"; - cmd_str += "\tend\n"; + cmd_str += "\t\t\telse:\n"; + cmd_str += "\t\t\t\tsync()\n"; + cmd_str += "\t\t\tend\n"; + cmd_str += "\t\tend\n"; + cmd_str += "\tend\n"; - sprintf(buf, "\tsocket_open(\"%s\", %i)\n", ip_addr_.c_str(), - REVERSE_PORT_); - cmd_str += buf; + sprintf(buf, "\tsocket_open(\"%s\", %i)\n", ip_addr_.c_str(), REVERSE_PORT_); + cmd_str += buf; - cmd_str += "\tthread_servo = run servoThread()\n"; - cmd_str += "\tkeepalive = 1\n"; - cmd_str += "\twhile keepalive > 0:\n"; - cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1)\n"; - cmd_str += "\t\tif params_mult[0] > 0:\n"; - cmd_str += "\t\t\tq = [params_mult[1] / MULT_jointstate, "; - cmd_str += "params_mult[2] / MULT_jointstate, "; - cmd_str += "params_mult[3] / MULT_jointstate, "; - cmd_str += "params_mult[4] / MULT_jointstate, "; - cmd_str += "params_mult[5] / MULT_jointstate, "; - cmd_str += "params_mult[6] / MULT_jointstate]\n"; - cmd_str += "\t\t\tkeepalive = params_mult[7]\n"; - cmd_str += "\t\t\tset_servo_setpoint(q)\n"; - cmd_str += "\t\tend\n"; - cmd_str += "\tend\n"; - cmd_str += "\tsleep(.1)\n"; - cmd_str += "\tsocket_close()\n"; - cmd_str += "\tkill thread_servo\n"; - cmd_str += "end\n"; + cmd_str += "\tthread_servo = run servoThread()\n"; + cmd_str += "\tkeepalive = 1\n"; + cmd_str += "\twhile keepalive > 0:\n"; + cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1)\n"; + cmd_str += "\t\tif params_mult[0] > 0:\n"; + cmd_str += "\t\t\tq = [params_mult[1] / MULT_jointstate, "; + cmd_str += "params_mult[2] / MULT_jointstate, "; + cmd_str += "params_mult[3] / MULT_jointstate, "; + cmd_str += "params_mult[4] / MULT_jointstate, "; + cmd_str += "params_mult[5] / MULT_jointstate, "; + cmd_str += "params_mult[6] / MULT_jointstate]\n"; + cmd_str += "\t\t\tkeepalive = params_mult[7]\n"; + cmd_str += "\t\t\tset_servo_setpoint(q)\n"; + cmd_str += "\t\tend\n"; + cmd_str += "\tend\n"; + cmd_str += "\tsleep(.1)\n"; + cmd_str += "\tsocket_close()\n"; + cmd_str += "\tkill thread_servo\n"; + cmd_str += "end\n"; - rt_interface_->addCommandToQueue(cmd_str); - return UrDriver::openServo(); + rt_interface_->addCommandToQueue(cmd_str); + return UrDriver::openServo(); } bool UrDriver::openServo() { - struct sockaddr_in cli_addr; - socklen_t clilen; - clilen = sizeof(cli_addr); - new_sockfd_ = accept(incoming_sockfd_, (struct sockaddr*)&cli_addr, - &clilen); - if (new_sockfd_ < 0) { - print_fatal("ERROR on accepting reverse communication"); - return false; - } - reverse_connected_ = true; - return true; + struct sockaddr_in cli_addr; + socklen_t clilen; + clilen = sizeof(cli_addr); + new_sockfd_ = accept(incoming_sockfd_, (struct sockaddr*)&cli_addr, &clilen); + if (new_sockfd_ < 0) + { + print_fatal("ERROR on accepting reverse communication"); + return false; + } + reverse_connected_ = true; + return true; } void UrDriver::closeServo(std::vector positions) { - if (positions.size() != 6) - UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0); - else - UrDriver::servoj(positions, 0); + if (positions.size() != 6) + UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0); + else + UrDriver::servoj(positions, 0); - reverse_connected_ = false; - close(new_sockfd_); + reverse_connected_ = false; + close(new_sockfd_); } bool UrDriver::start() { - if (!sec_interface_->start()) - return false; - firmware_version_ = sec_interface_->robot_state_->getVersion(); - rt_interface_->robot_state_->setVersion(firmware_version_); - if (!rt_interface_->start()) - return false; - ip_addr_ = rt_interface_->getLocalIp(); - print_debug( - "Listening on " + ip_addr_ + ":" + std::to_string(REVERSE_PORT_) - + "\n"); - return true; + if (!sec_interface_->start()) + return false; + firmware_version_ = sec_interface_->robot_state_->getVersion(); + rt_interface_->robot_state_->setVersion(firmware_version_); + if (!rt_interface_->start()) + return false; + ip_addr_ = rt_interface_->getLocalIp(); + print_debug("Listening on " + ip_addr_ + ":" + std::to_string(REVERSE_PORT_) + "\n"); + return true; } void UrDriver::halt() { - if (executing_traj_) { - UrDriver::stopTraj(); - } - sec_interface_->halt(); - rt_interface_->halt(); - close(incoming_sockfd_); + if (executing_traj_) + { + UrDriver::stopTraj(); + } + sec_interface_->halt(); + rt_interface_->halt(); + close(incoming_sockfd_); } -void UrDriver::setSpeed(double q0, double q1, double q2, double q3, double q4, - double q5, double acc) +void UrDriver::setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc) { - rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc); + rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc); } std::vector UrDriver::getJointNames() { - return joint_names_; + return joint_names_; } void UrDriver::setJointNames(std::vector jn) { - joint_names_ = jn; + joint_names_ = jn; } void UrDriver::setToolVoltage(unsigned int v) { - char buf[256]; - sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v); - rt_interface_->addCommandToQueue(buf); - print_debug(buf); + char buf[256]; + sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v); + rt_interface_->addCommandToQueue(buf); + print_debug(buf); } void UrDriver::setFlag(unsigned int n, bool b) { - char buf[256]; - sprintf(buf, "sec setOut():\n\tset_flag(%d, %s)\nend\n", n, - b ? "True" : "False"); - rt_interface_->addCommandToQueue(buf); - print_debug(buf); + char buf[256]; + sprintf(buf, "sec setOut():\n\tset_flag(%d, %s)\nend\n", n, b ? "True" : "False"); + rt_interface_->addCommandToQueue(buf); + print_debug(buf); } void UrDriver::setDigitalOut(unsigned int n, bool b) { - char buf[256]; - if (firmware_version_ < 2) { - sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n, - b ? "True" : "False"); - } else if (n > 15) { - sprintf(buf, - "sec setOut():\n\tset_tool_digital_out(%d, %s)\nend\n", - n - 16, b ? "True" : "False"); - } else if (n > 7) { - sprintf(buf, "sec setOut():\n\tset_configurable_digital_out(%d, %s)\nend\n", - n - 8, b ? "True" : "False"); - - } else { - sprintf(buf, "sec setOut():\n\tset_standard_digital_out(%d, %s)\nend\n", - n, b ? "True" : "False"); - } - rt_interface_->addCommandToQueue(buf); - print_debug(buf); + char buf[256]; + if (firmware_version_ < 2) + { + sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n, b ? "True" : "False"); + } + else if (n > 15) + { + sprintf(buf, "sec setOut():\n\tset_tool_digital_out(%d, %s)\nend\n", n - 16, b ? "True" : "False"); + } + else if (n > 7) + { + sprintf(buf, "sec setOut():\n\tset_configurable_digital_out(%d, %s)\nend\n", n - 8, b ? "True" : "False"); + } + else + { + sprintf(buf, "sec setOut():\n\tset_standard_digital_out(%d, %s)\nend\n", n, b ? "True" : "False"); + } + rt_interface_->addCommandToQueue(buf); + print_debug(buf); } void UrDriver::setAnalogOut(unsigned int n, double f) { - char buf[256]; - if (firmware_version_ < 2) { - sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f); - } else { - sprintf(buf, "sec setOut():\n\tset_standard_analog_out(%d, %1.4f)\nend\n", n, f); - } + char buf[256]; + if (firmware_version_ < 2) + { + sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f); + } + else + { + sprintf(buf, "sec setOut():\n\tset_standard_analog_out(%d, %1.4f)\nend\n", n, f); + } - rt_interface_->addCommandToQueue(buf); - print_debug(buf); + rt_interface_->addCommandToQueue(buf); + print_debug(buf); } bool UrDriver::setPayload(double m) { - if ((m < maximum_payload_) && (m > minimum_payload_)) { - char buf[256]; - sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m); - rt_interface_->addCommandToQueue(buf); - print_debug(buf); - return true; - } else - return false; + if ((m < maximum_payload_) && (m > minimum_payload_)) + { + char buf[256]; + sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m); + rt_interface_->addCommandToQueue(buf); + print_debug(buf); + return true; + } + else + return false; } void UrDriver::setMinPayload(double m) { - if (m > 0) { - minimum_payload_ = m; - } else { - minimum_payload_ = 0; - } + if (m > 0) + { + minimum_payload_ = m; + } + else + { + minimum_payload_ = 0; + } } void UrDriver::setMaxPayload(double m) { - maximum_payload_ = m; + maximum_payload_ = m; } void UrDriver::setServojTime(double t) { - if (t > 0.008) { - servoj_time_ = t; - } else { - servoj_time_ = 0.008; - } + if (t > 0.008) + { + servoj_time_ = t; + } + else + { + servoj_time_ = 0.008; + } } void UrDriver::setServojLookahead(double t) { - if (t > 0.03) { - if (t < 0.2) { - servoj_lookahead_time_ = t; - } else { - servoj_lookahead_time_ = 0.2; - } - } else { - servoj_lookahead_time_ = 0.03; + if (t > 0.03) + { + if (t < 0.2) + { + servoj_lookahead_time_ = t; } + else + { + servoj_lookahead_time_ = 0.2; + } + } + else + { + servoj_lookahead_time_ = 0.03; + } } void UrDriver::setServojGain(double g) { - if (g > 100) { - if (g < 2000) { - servoj_gain_ = g; - } else { - servoj_gain_ = 2000; - } - } else { - servoj_gain_ = 100; + if (g > 100) + { + if (g < 2000) + { + servoj_gain_ = g; + } + else + { + servoj_gain_ = 2000; } + } + else + { + servoj_gain_ = 100; + } } diff --git a/src/ur_hardware_interface.cpp b/src/ur_hardware_interface.cpp index 152452c56..06b59f979 100644 --- a/src/ur_hardware_interface.cpp +++ b/src/ur_hardware_interface.cpp @@ -57,228 +57,227 @@ #include -namespace ros_control_ur { - -UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) - : nh_(nh) - , robot_(robot) +namespace ros_control_ur +{ +UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) : nh_(nh), robot_(robot) { - // Initialize shared memory and interfaces here - init(); // this implementation loads from rosparam + // Initialize shared memory and interfaces here + init(); // this implementation loads from rosparam - max_vel_change_ = 0.12; // equivalent of an acceleration of 15 rad/sec^2 + max_vel_change_ = 0.12; // equivalent of an acceleration of 15 rad/sec^2 - ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface."); + ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface."); } void UrHardwareInterface::init() { - ROS_INFO_STREAM_NAMED("ur_hardware_interface", - "Reading rosparams from namespace: " << nh_.getNamespace()); + ROS_INFO_STREAM_NAMED("ur_hardware_interface", "Reading rosparams from namespace: " << nh_.getNamespace()); - // Get joint names - nh_.getParam("hardware_interface/joints", joint_names_); - if (joint_names_.size() == 0) { - ROS_FATAL_STREAM_NAMED("ur_hardware_interface", - "No joints found on parameter server for controller, did you load the proper yaml file?" - << " Namespace: " << nh_.getNamespace()); - exit(-1); - } - num_joints_ = joint_names_.size(); + // Get joint names + nh_.getParam("hardware_interface/joints", joint_names_); + if (joint_names_.size() == 0) + { + ROS_FATAL_STREAM_NAMED("ur_hardware_interface", + "No joints found on parameter server for controller, did you load the proper yaml file?" + << " Namespace: " << nh_.getNamespace()); + exit(-1); + } + num_joints_ = joint_names_.size(); - // Resize vectors - joint_position_.resize(num_joints_); - joint_velocity_.resize(num_joints_); - joint_effort_.resize(num_joints_); - joint_position_command_.resize(num_joints_); - joint_velocity_command_.resize(num_joints_); - prev_joint_velocity_command_.resize(num_joints_); + // Resize vectors + joint_position_.resize(num_joints_); + joint_velocity_.resize(num_joints_); + joint_effort_.resize(num_joints_); + joint_position_command_.resize(num_joints_); + joint_velocity_command_.resize(num_joints_); + prev_joint_velocity_command_.resize(num_joints_); - // Initialize controller - for (std::size_t i = 0; i < num_joints_; ++i) { - ROS_DEBUG_STREAM_NAMED("ur_hardware_interface", - "Loading joint name: " << joint_names_[i]); + // Initialize controller + for (std::size_t i = 0; i < num_joints_; ++i) + { + ROS_DEBUG_STREAM_NAMED("ur_hardware_interface", "Loading joint name: " << joint_names_[i]); - // Create joint state interface - joint_state_interface_.registerHandle( - hardware_interface::JointStateHandle(joint_names_[i], - &joint_position_[i], &joint_velocity_[i], - &joint_effort_[i])); + // Create joint state interface + joint_state_interface_.registerHandle(hardware_interface::JointStateHandle(joint_names_[i], &joint_position_[i], + &joint_velocity_[i], &joint_effort_[i])); - // Create position joint interface - position_joint_interface_.registerHandle( - hardware_interface::JointHandle( - joint_state_interface_.getHandle(joint_names_[i]), - &joint_position_command_[i])); + // Create position joint interface + position_joint_interface_.registerHandle(hardware_interface::JointHandle( + joint_state_interface_.getHandle(joint_names_[i]), &joint_position_command_[i])); - // Create velocity joint interface - velocity_joint_interface_.registerHandle( - hardware_interface::JointHandle( - joint_state_interface_.getHandle(joint_names_[i]), - &joint_velocity_command_[i])); - prev_joint_velocity_command_[i] = 0.; - } + // Create velocity joint interface + velocity_joint_interface_.registerHandle(hardware_interface::JointHandle( + joint_state_interface_.getHandle(joint_names_[i]), &joint_velocity_command_[i])); + prev_joint_velocity_command_[i] = 0.; + } - // Create force torque interface - force_torque_interface_.registerHandle( - hardware_interface::ForceTorqueSensorHandle("wrench", "", - robot_force_, robot_torque_)); + // Create force torque interface + force_torque_interface_.registerHandle( + hardware_interface::ForceTorqueSensorHandle("wrench", "", robot_force_, robot_torque_)); - registerInterface(&joint_state_interface_); // From RobotHW base class. - registerInterface(&position_joint_interface_); // From RobotHW base class. - registerInterface(&velocity_joint_interface_); // From RobotHW base class. - registerInterface(&force_torque_interface_); // From RobotHW base class. - velocity_interface_running_ = false; - position_interface_running_ = false; + registerInterface(&joint_state_interface_); // From RobotHW base class. + registerInterface(&position_joint_interface_); // From RobotHW base class. + registerInterface(&velocity_joint_interface_); // From RobotHW base class. + registerInterface(&force_torque_interface_); // From RobotHW base class. + velocity_interface_running_ = false; + position_interface_running_ = false; } void UrHardwareInterface::read() { - std::vector pos, vel, current, tcp; - pos = robot_->rt_interface_->robot_state_->getQActual(); - vel = robot_->rt_interface_->robot_state_->getQdActual(); - current = robot_->rt_interface_->robot_state_->getIActual(); - tcp = robot_->rt_interface_->robot_state_->getTcpForce(); - for (std::size_t i = 0; i < num_joints_; ++i) { - joint_position_[i] = pos[i]; - joint_velocity_[i] = vel[i]; - joint_effort_[i] = current[i]; - } - for (std::size_t i = 0; i < 3; ++i) { - robot_force_[i] = tcp[i]; - robot_torque_[i] = tcp[i + 3]; - } + std::vector pos, vel, current, tcp; + pos = robot_->rt_interface_->robot_state_->getQActual(); + vel = robot_->rt_interface_->robot_state_->getQdActual(); + current = robot_->rt_interface_->robot_state_->getIActual(); + tcp = robot_->rt_interface_->robot_state_->getTcpForce(); + for (std::size_t i = 0; i < num_joints_; ++i) + { + joint_position_[i] = pos[i]; + joint_velocity_[i] = vel[i]; + joint_effort_[i] = current[i]; + } + for (std::size_t i = 0; i < 3; ++i) + { + robot_force_[i] = tcp[i]; + robot_torque_[i] = tcp[i + 3]; + } } void UrHardwareInterface::setMaxVelChange(double inp) { - max_vel_change_ = inp; + max_vel_change_ = inp; } void UrHardwareInterface::write() { - if (velocity_interface_running_) { - std::vector cmd; - //do some rate limiting - cmd.resize(joint_velocity_command_.size()); - for (unsigned int i = 0; i < joint_velocity_command_.size(); i++) { - cmd[i] = joint_velocity_command_[i]; - if (cmd[i] > prev_joint_velocity_command_[i] + max_vel_change_) { - cmd[i] = prev_joint_velocity_command_[i] + max_vel_change_; - } else if (cmd[i] - < prev_joint_velocity_command_[i] - max_vel_change_) { - cmd[i] = prev_joint_velocity_command_[i] - max_vel_change_; - } - prev_joint_velocity_command_[i] = cmd[i]; - } - robot_->setSpeed(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], max_vel_change_ * 125); - } else if (position_interface_running_) { - robot_->servoj(joint_position_command_); + if (velocity_interface_running_) + { + std::vector cmd; + // do some rate limiting + cmd.resize(joint_velocity_command_.size()); + for (unsigned int i = 0; i < joint_velocity_command_.size(); i++) + { + cmd[i] = joint_velocity_command_[i]; + if (cmd[i] > prev_joint_velocity_command_[i] + max_vel_change_) + { + cmd[i] = prev_joint_velocity_command_[i] + max_vel_change_; + } + else if (cmd[i] < prev_joint_velocity_command_[i] - max_vel_change_) + { + cmd[i] = prev_joint_velocity_command_[i] - max_vel_change_; + } + prev_joint_velocity_command_[i] = cmd[i]; } + robot_->setSpeed(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], max_vel_change_ * 125); + } + else if (position_interface_running_) + { + robot_->servoj(joint_position_command_); + } } -bool UrHardwareInterface::canSwitch( - const std::list& start_list, - const std::list& stop_list) const +bool UrHardwareInterface::canSwitch(const std::list& start_list, + const std::list& stop_list) const { - for (std::list::const_iterator controller_it = start_list.begin(); controller_it != start_list.end(); - ++controller_it) { - if (controller_it->hardware_interface - == "hardware_interface::VelocityJointInterface") { - if (velocity_interface_running_) { - ROS_ERROR( - "%s: An interface of that type (%s) is already running", - controller_it->name.c_str(), - controller_it->hardware_interface.c_str()); - return false; - } - if (position_interface_running_) { - bool error = true; - for (std::list::const_iterator stop_controller_it = stop_list.begin(); - stop_controller_it != stop_list.end(); - ++stop_controller_it) { - if (stop_controller_it->hardware_interface - == "hardware_interface::PositionJointInterface") { - error = false; - break; - } - } - if (error) { - ROS_ERROR( - "%s (type %s) can not be run simultaneously with a PositionJointInterface", - controller_it->name.c_str(), - controller_it->hardware_interface.c_str()); - return false; - } - } - } else if (controller_it->hardware_interface - == "hardware_interface::PositionJointInterface") { - if (position_interface_running_) { - ROS_ERROR( - "%s: An interface of that type (%s) is already running", - controller_it->name.c_str(), - controller_it->hardware_interface.c_str()); - return false; - } - if (velocity_interface_running_) { - bool error = true; - for (std::list::const_iterator stop_controller_it = stop_list.begin(); - stop_controller_it != stop_list.end(); - ++stop_controller_it) { - if (stop_controller_it->hardware_interface - == "hardware_interface::VelocityJointInterface") { - error = false; - break; - } - } - if (error) { - ROS_ERROR( - "%s (type %s) can not be run simultaneously with a VelocityJointInterface", - controller_it->name.c_str(), - controller_it->hardware_interface.c_str()); - return false; - } - } + for (std::list::const_iterator controller_it = start_list.begin(); + controller_it != start_list.end(); ++controller_it) + { + if (controller_it->hardware_interface == "hardware_interface::VelocityJointInterface") + { + if (velocity_interface_running_) + { + ROS_ERROR("%s: An interface of that type (%s) is already running", controller_it->name.c_str(), + controller_it->hardware_interface.c_str()); + return false; + } + if (position_interface_running_) + { + bool error = true; + for (std::list::const_iterator stop_controller_it = stop_list.begin(); + stop_controller_it != stop_list.end(); ++stop_controller_it) + { + if (stop_controller_it->hardware_interface == "hardware_interface::PositionJointInterface") + { + error = false; + break; + } } + if (error) + { + ROS_ERROR("%s (type %s) can not be run simultaneously with a PositionJointInterface", + controller_it->name.c_str(), controller_it->hardware_interface.c_str()); + return false; + } + } } + else if (controller_it->hardware_interface == "hardware_interface::PositionJointInterface") + { + if (position_interface_running_) + { + ROS_ERROR("%s: An interface of that type (%s) is already running", controller_it->name.c_str(), + controller_it->hardware_interface.c_str()); + return false; + } + if (velocity_interface_running_) + { + bool error = true; + for (std::list::const_iterator stop_controller_it = stop_list.begin(); + stop_controller_it != stop_list.end(); ++stop_controller_it) + { + if (stop_controller_it->hardware_interface == "hardware_interface::VelocityJointInterface") + { + error = false; + break; + } + } + if (error) + { + ROS_ERROR("%s (type %s) can not be run simultaneously with a VelocityJointInterface", + controller_it->name.c_str(), controller_it->hardware_interface.c_str()); + return false; + } + } + } + } - // we can always stop a controller - return true; + // we can always stop a controller + return true; } -void UrHardwareInterface::doSwitch( - const std::list& start_list, - const std::list& stop_list) +void UrHardwareInterface::doSwitch(const std::list& start_list, + const std::list& stop_list) { - for (std::list::const_iterator controller_it = stop_list.begin(); controller_it != stop_list.end(); - ++controller_it) { - if (controller_it->hardware_interface - == "hardware_interface::VelocityJointInterface") { - velocity_interface_running_ = false; - ROS_DEBUG("Stopping velocity interface"); - } - if (controller_it->hardware_interface - == "hardware_interface::PositionJointInterface") { - position_interface_running_ = false; - std::vector tmp; - robot_->closeServo(tmp); - ROS_DEBUG("Stopping position interface"); - } + for (std::list::const_iterator controller_it = stop_list.begin(); + controller_it != stop_list.end(); ++controller_it) + { + if (controller_it->hardware_interface == "hardware_interface::VelocityJointInterface") + { + velocity_interface_running_ = false; + ROS_DEBUG("Stopping velocity interface"); } - for (std::list::const_iterator controller_it = start_list.begin(); controller_it != start_list.end(); - ++controller_it) { - if (controller_it->hardware_interface - == "hardware_interface::VelocityJointInterface") { - velocity_interface_running_ = true; - ROS_DEBUG("Starting velocity interface"); - } - if (controller_it->hardware_interface - == "hardware_interface::PositionJointInterface") { - position_interface_running_ = true; - robot_->uploadProg(); - ROS_DEBUG("Starting position interface"); - } + if (controller_it->hardware_interface == "hardware_interface::PositionJointInterface") + { + position_interface_running_ = false; + std::vector tmp; + robot_->closeServo(tmp); + ROS_DEBUG("Stopping position interface"); + } + } + for (std::list::const_iterator controller_it = start_list.begin(); + controller_it != start_list.end(); ++controller_it) + { + if (controller_it->hardware_interface == "hardware_interface::VelocityJointInterface") + { + velocity_interface_running_ = true; + ROS_DEBUG("Starting velocity interface"); + } + if (controller_it->hardware_interface == "hardware_interface::PositionJointInterface") + { + position_interface_running_ = true; + robot_->uploadProg(); + ROS_DEBUG("Starting position interface"); } + } } -} // namespace +} // namespace diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp index b45224b57..e947aa9bb 100644 --- a/src/ur_realtime_communication.cpp +++ b/src/ur_realtime_communication.cpp @@ -18,183 +18,194 @@ #include "ur_modern_driver/ur_realtime_communication.h" -UrRealtimeCommunication::UrRealtimeCommunication( - std::condition_variable& msg_cond, std::string host, - unsigned int safety_count_max) +UrRealtimeCommunication::UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host, + unsigned int safety_count_max) { - robot_state_ = new RobotStateRT(msg_cond); - bzero((char*)&serv_addr_, sizeof(serv_addr_)); - sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (sockfd_ < 0) { - print_fatal("ERROR opening socket"); - } - server_ = gethostbyname(host.c_str()); - if (server_ == NULL) { - print_fatal("ERROR, no such host"); - } - serv_addr_.sin_family = AF_INET; - bcopy((char*)server_->h_addr, (char*)&serv_addr_.sin_addr.s_addr, server_->h_length); - serv_addr_.sin_port = htons(30003); - flag_ = 1; - setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int)); - setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); - setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int)); - fcntl(sockfd_, F_SETFL, O_NONBLOCK); - connected_ = false; - keepalive_ = false; - safety_count_ = safety_count_max + 1; - safety_count_max_ = safety_count_max; + robot_state_ = new RobotStateRT(msg_cond); + bzero((char*)&serv_addr_, sizeof(serv_addr_)); + sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (sockfd_ < 0) + { + print_fatal("ERROR opening socket"); + } + server_ = gethostbyname(host.c_str()); + if (server_ == NULL) + { + print_fatal("ERROR, no such host"); + } + serv_addr_.sin_family = AF_INET; + bcopy((char*)server_->h_addr, (char*)&serv_addr_.sin_addr.s_addr, server_->h_length); + serv_addr_.sin_port = htons(30003); + flag_ = 1; + setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int)); + setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); + setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int)); + fcntl(sockfd_, F_SETFL, O_NONBLOCK); + connected_ = false; + keepalive_ = false; + safety_count_ = safety_count_max + 1; + safety_count_max_ = safety_count_max; } bool UrRealtimeCommunication::start() { - fd_set writefds; - struct timeval timeout; + fd_set writefds; + struct timeval timeout; - keepalive_ = true; - print_debug("Realtime port: Connecting..."); + keepalive_ = true; + print_debug("Realtime port: Connecting..."); - connect(sockfd_, (struct sockaddr*)&serv_addr_, sizeof(serv_addr_)); - FD_ZERO(&writefds); - FD_SET(sockfd_, &writefds); - timeout.tv_sec = 10; - timeout.tv_usec = 0; - select(sockfd_ + 1, NULL, &writefds, NULL, &timeout); - unsigned int flag_len; - getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); - if (flag_ < 0) { - print_fatal("Error connecting to RT port 30003"); - return false; - } - sockaddr_in name; - socklen_t namelen = sizeof(name); - int err = getsockname(sockfd_, (sockaddr*)&name, &namelen); - if (err < 0) { - print_fatal("Could not get local IP"); - close(sockfd_); - return false; - } - char str[18]; - inet_ntop(AF_INET, &name.sin_addr, str, 18); - local_ip_ = str; - comThread_ = std::thread(&UrRealtimeCommunication::run, this); - return true; + connect(sockfd_, (struct sockaddr*)&serv_addr_, sizeof(serv_addr_)); + FD_ZERO(&writefds); + FD_SET(sockfd_, &writefds); + timeout.tv_sec = 10; + timeout.tv_usec = 0; + select(sockfd_ + 1, NULL, &writefds, NULL, &timeout); + unsigned int flag_len; + getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); + if (flag_ < 0) + { + print_fatal("Error connecting to RT port 30003"); + return false; + } + sockaddr_in name; + socklen_t namelen = sizeof(name); + int err = getsockname(sockfd_, (sockaddr*)&name, &namelen); + if (err < 0) + { + print_fatal("Could not get local IP"); + close(sockfd_); + return false; + } + char str[18]; + inet_ntop(AF_INET, &name.sin_addr, str, 18); + local_ip_ = str; + comThread_ = std::thread(&UrRealtimeCommunication::run, this); + return true; } void UrRealtimeCommunication::halt() { - keepalive_ = false; - comThread_.join(); + keepalive_ = false; + comThread_.join(); } void UrRealtimeCommunication::addCommandToQueue(std::string inp) { - int bytes_written; - if (inp.back() != '\n') { - inp.append("\n"); - } - if (connected_) - bytes_written = write(sockfd_, inp.c_str(), inp.length()); - else - print_error("Could not send command \"" + inp + "\". The robot is not connected! Command is discarded"); + int bytes_written; + if (inp.back() != '\n') + { + inp.append("\n"); + } + if (connected_) + bytes_written = write(sockfd_, inp.c_str(), inp.length()); + else + print_error("Could not send command \"" + inp + "\". The robot is not connected! Command is discarded"); } -void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2, - double q3, double q4, double q5, double acc) +void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc) { - char cmd[1024]; - if (robot_state_->getVersion() >= 3.1) { - sprintf(cmd, - "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n", - q0, q1, q2, q3, q4, q5, acc); - } else { - sprintf(cmd, - "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n", - q0, q1, q2, q3, q4, q5, acc); - } - addCommandToQueue((std::string)(cmd)); - if (q0 != 0. or q1 != 0. or q2 != 0. or q3 != 0. or q4 != 0. or q5 != 0.) { - //If a joint speed is set, make sure we stop it again after some time if the user doesn't - safety_count_ = 0; - } + char cmd[1024]; + if (robot_state_->getVersion() >= 3.1) + { + sprintf(cmd, "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n", q0, q1, q2, q3, q4, q5, acc); + } + else + { + sprintf(cmd, "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n", q0, q1, q2, q3, q4, q5, acc); + } + addCommandToQueue((std::string)(cmd)); + if (q0 != 0. or q1 != 0. or q2 != 0. or q3 != 0. or q4 != 0. or q5 != 0.) + { + // If a joint speed is set, make sure we stop it again after some time if the user doesn't + safety_count_ = 0; + } } void UrRealtimeCommunication::run() { - uint8_t buf[2048]; - int bytes_read; - bzero(buf, 2048); - struct timeval timeout; - fd_set readfds; - FD_ZERO(&readfds); - FD_SET(sockfd_, &readfds); - print_debug("Realtime port: Got connection"); - connected_ = true; - while (keepalive_) { - while (connected_ && keepalive_) { - timeout.tv_sec = 0; //do this each loop as selects modifies timeout - timeout.tv_usec = 500000; // timeout of 0.5 sec - select(sockfd_ + 1, &readfds, NULL, NULL, &timeout); - bytes_read = read(sockfd_, buf, 2048); - if (bytes_read > 0) { - setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, - sizeof(int)); - robot_state_->unpack(buf); - if (safety_count_ == safety_count_max_) { - setSpeed(0., 0., 0., 0., 0., 0.); - } - safety_count_ += 1; - } else { - connected_ = false; - close(sockfd_); - } + uint8_t buf[2048]; + int bytes_read; + bzero(buf, 2048); + struct timeval timeout; + fd_set readfds; + FD_ZERO(&readfds); + FD_SET(sockfd_, &readfds); + print_debug("Realtime port: Got connection"); + connected_ = true; + while (keepalive_) + { + while (connected_ && keepalive_) + { + timeout.tv_sec = 0; // do this each loop as selects modifies timeout + timeout.tv_usec = 500000; // timeout of 0.5 sec + select(sockfd_ + 1, &readfds, NULL, NULL, &timeout); + bytes_read = read(sockfd_, buf, 2048); + if (bytes_read > 0) + { + setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); + robot_state_->unpack(buf); + if (safety_count_ == safety_count_max_) + { + setSpeed(0., 0., 0., 0., 0., 0.); } - if (keepalive_) { - //reconnect - print_warning("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."); - sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (sockfd_ < 0) { - print_fatal("ERROR opening socket"); - } - flag_ = 1; - setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, - sizeof(int)); - setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, - sizeof(int)); + safety_count_ += 1; + } + else + { + connected_ = false; + close(sockfd_); + } + } + if (keepalive_) + { + // reconnect + print_warning("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."); + sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (sockfd_ < 0) + { + print_fatal("ERROR opening socket"); + } + flag_ = 1; + setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int)); + setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); - setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, - sizeof(int)); - fcntl(sockfd_, F_SETFL, O_NONBLOCK); - while (keepalive_ && !connected_) { - std::this_thread::sleep_for(std::chrono::seconds(10)); - fd_set writefds; + setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int)); + fcntl(sockfd_, F_SETFL, O_NONBLOCK); + while (keepalive_ && !connected_) + { + std::this_thread::sleep_for(std::chrono::seconds(10)); + fd_set writefds; - connect(sockfd_, (struct sockaddr*)&serv_addr_, - sizeof(serv_addr_)); - FD_ZERO(&writefds); - FD_SET(sockfd_, &writefds); - select(sockfd_ + 1, NULL, &writefds, NULL, NULL); - unsigned int flag_len; - getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); - if (flag_ < 0) { - print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds..."); - } else { - connected_ = true; - print_info("Realtime port: Reconnected"); - } - } + connect(sockfd_, (struct sockaddr*)&serv_addr_, sizeof(serv_addr_)); + FD_ZERO(&writefds); + FD_SET(sockfd_, &writefds); + select(sockfd_ + 1, NULL, &writefds, NULL, NULL); + unsigned int flag_len; + getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); + if (flag_ < 0) + { + print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 " + "seconds..."); + } + else + { + connected_ = true; + print_info("Realtime port: Reconnected"); } + } } - setSpeed(0., 0., 0., 0., 0., 0.); - close(sockfd_); + } + setSpeed(0., 0., 0., 0., 0., 0.); + close(sockfd_); } void UrRealtimeCommunication::setSafetyCountMax(uint inp) { - safety_count_max_ = inp; + safety_count_max_ = inp; } std::string UrRealtimeCommunication::getLocalIp() { - return local_ip_; + return local_ip_; } diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 92fe341d1..6f3f85404 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -16,19 +16,22 @@ * limitations under the License. */ -#include "ur_modern_driver/do_output.h" -#include "ur_modern_driver/ur_driver.h" -#include "ur_modern_driver/ur_hardware_interface.h" +#include +#include #include #include #include #include #include -#include #include -#include #include +#include "ur_modern_driver/do_output.h" +#include "ur_modern_driver/ur_driver.h" +#include "ur_modern_driver/ur_hardware_interface.h" +#include +#include +#include #include "actionlib/server/action_server.h" #include "actionlib/server/server_goal_handle.h" #include "control_msgs/FollowJointTrajectoryAction.h" @@ -47,785 +50,822 @@ #include "ur_msgs/SetPayload.h" #include "ur_msgs/SetPayloadRequest.h" #include "ur_msgs/SetPayloadResponse.h" -#include -#include -#include /// TF #include #include -class RosWrapper { +class RosWrapper +{ protected: - UrDriver robot_; - std::condition_variable rt_msg_cond_; - std::condition_variable msg_cond_; - ros::NodeHandle nh_; - actionlib::ActionServer as_; - actionlib::ServerGoalHandle goal_handle_; - bool has_goal_; - control_msgs::FollowJointTrajectoryFeedback feedback_; - control_msgs::FollowJointTrajectoryResult result_; - ros::Subscriber speed_sub_; - ros::Subscriber urscript_sub_; - ros::ServiceServer io_srv_; - ros::ServiceServer payload_srv_; - std::thread* rt_publish_thread_; - std::thread* mb_publish_thread_; - double io_flag_delay_; - double max_velocity_; - std::vector joint_offsets_; - std::string base_frame_; - std::string tool_frame_; - bool use_ros_control_; - std::thread* ros_control_thread_; - boost::shared_ptr hardware_interface_; - boost::shared_ptr controller_manager_; + UrDriver robot_; + std::condition_variable rt_msg_cond_; + std::condition_variable msg_cond_; + ros::NodeHandle nh_; + actionlib::ActionServer as_; + actionlib::ServerGoalHandle goal_handle_; + bool has_goal_; + control_msgs::FollowJointTrajectoryFeedback feedback_; + control_msgs::FollowJointTrajectoryResult result_; + ros::Subscriber speed_sub_; + ros::Subscriber urscript_sub_; + ros::ServiceServer io_srv_; + ros::ServiceServer payload_srv_; + std::thread* rt_publish_thread_; + std::thread* mb_publish_thread_; + double io_flag_delay_; + double max_velocity_; + std::vector joint_offsets_; + std::string base_frame_; + std::string tool_frame_; + bool use_ros_control_; + std::thread* ros_control_thread_; + boost::shared_ptr hardware_interface_; + boost::shared_ptr controller_manager_; public: - RosWrapper(std::string host, int reverse_port) - : as_(nh_, "follow_joint_trajectory", - boost::bind(&RosWrapper::goalCB, this, _1), - boost::bind(&RosWrapper::cancelCB, this, _1), false) - , robot_( - rt_msg_cond_, msg_cond_, host, reverse_port, 0.03, 300) - , io_flag_delay_(0.05) - , joint_offsets_( - 6, 0.0) - { - - std::string joint_prefix = ""; - std::vector joint_names; - char buf[256]; - - if (ros::param::get("~prefix", joint_prefix)) { - if (joint_prefix.length() > 0) { - sprintf(buf, "Setting prefix to %s", joint_prefix.c_str()); - print_info(buf); - } - } - joint_names.push_back(joint_prefix + "shoulder_pan_joint"); - joint_names.push_back(joint_prefix + "shoulder_lift_joint"); - joint_names.push_back(joint_prefix + "elbow_joint"); - joint_names.push_back(joint_prefix + "wrist_1_joint"); - joint_names.push_back(joint_prefix + "wrist_2_joint"); - joint_names.push_back(joint_prefix + "wrist_3_joint"); - robot_.setJointNames(joint_names); - - use_ros_control_ = false; - ros::param::get("~use_ros_control", use_ros_control_); - - if (use_ros_control_) { - hardware_interface_.reset( - new ros_control_ur::UrHardwareInterface(nh_, &robot_)); - controller_manager_.reset( - new controller_manager::ControllerManager( - hardware_interface_.get(), nh_)); - double max_vel_change = 0.12; // equivalent of an acceleration of 15 rad/sec^2 - if (ros::param::get("~max_acceleration", max_vel_change)) { - max_vel_change = max_vel_change / 125; - } - sprintf(buf, "Max acceleration set to: %f [rad/sec²]", - max_vel_change * 125); - print_debug(buf); - hardware_interface_->setMaxVelChange(max_vel_change); - } - //Using a very high value in order to not limit execution of trajectories being sent from MoveIt! - max_velocity_ = 10.; - if (ros::param::get("~max_velocity", max_velocity_)) { - sprintf(buf, "Max velocity accepted by ur_driver: %f [rad/s]", - max_velocity_); - print_debug(buf); - } - - //Bounds for SetPayload service - //Using a very conservative value as it should be set through the parameter server - double min_payload = 0.; - double max_payload = 1.; - if (ros::param::get("~min_payload", min_payload)) { - sprintf(buf, "Min payload set to: %f [kg]", min_payload); - print_debug(buf); - } - if (ros::param::get("~max_payload", max_payload)) { - sprintf(buf, "Max payload set to: %f [kg]", max_payload); - print_debug(buf); - } - robot_.setMinPayload(min_payload); - robot_.setMaxPayload(max_payload); - sprintf(buf, "Bounds for set_payload service calls: [%f, %f]", - min_payload, max_payload); - print_debug(buf); - - double servoj_time = 0.008; - if (ros::param::get("~servoj_time", servoj_time)) { - sprintf(buf, "Servoj_time set to: %f [sec]", servoj_time); - print_debug(buf); - } - robot_.setServojTime(servoj_time); + RosWrapper(std::string host, int reverse_port) + : as_(nh_, "follow_joint_trajectory", boost::bind(&RosWrapper::goalCB, this, _1), + boost::bind(&RosWrapper::cancelCB, this, _1), false) + , robot_(rt_msg_cond_, msg_cond_, host, reverse_port, 0.03, 300) + , io_flag_delay_(0.05) + , joint_offsets_(6, 0.0) + { + std::string joint_prefix = ""; + std::vector joint_names; + char buf[256]; + + if (ros::param::get("~prefix", joint_prefix)) + { + if (joint_prefix.length() > 0) + { + sprintf(buf, "Setting prefix to %s", joint_prefix.c_str()); + print_info(buf); + } + } + joint_names.push_back(joint_prefix + "shoulder_pan_joint"); + joint_names.push_back(joint_prefix + "shoulder_lift_joint"); + joint_names.push_back(joint_prefix + "elbow_joint"); + joint_names.push_back(joint_prefix + "wrist_1_joint"); + joint_names.push_back(joint_prefix + "wrist_2_joint"); + joint_names.push_back(joint_prefix + "wrist_3_joint"); + robot_.setJointNames(joint_names); + + use_ros_control_ = false; + ros::param::get("~use_ros_control", use_ros_control_); + + if (use_ros_control_) + { + hardware_interface_.reset(new ros_control_ur::UrHardwareInterface(nh_, &robot_)); + controller_manager_.reset(new controller_manager::ControllerManager(hardware_interface_.get(), nh_)); + double max_vel_change = 0.12; // equivalent of an acceleration of 15 rad/sec^2 + if (ros::param::get("~max_acceleration", max_vel_change)) + { + max_vel_change = max_vel_change / 125; + } + sprintf(buf, "Max acceleration set to: %f [rad/sec²]", max_vel_change * 125); + print_debug(buf); + hardware_interface_->setMaxVelChange(max_vel_change); + } + // Using a very high value in order to not limit execution of trajectories being sent from MoveIt! + max_velocity_ = 10.; + if (ros::param::get("~max_velocity", max_velocity_)) + { + sprintf(buf, "Max velocity accepted by ur_driver: %f [rad/s]", max_velocity_); + print_debug(buf); + } - double servoj_lookahead_time = 0.03; - if (ros::param::get("~servoj_lookahead_time", servoj_lookahead_time)) { - sprintf(buf, "Servoj_lookahead_time set to: %f [sec]", servoj_lookahead_time); - print_debug(buf); - } - robot_.setServojLookahead(servoj_lookahead_time); + // Bounds for SetPayload service + // Using a very conservative value as it should be set through the parameter server + double min_payload = 0.; + double max_payload = 1.; + if (ros::param::get("~min_payload", min_payload)) + { + sprintf(buf, "Min payload set to: %f [kg]", min_payload); + print_debug(buf); + } + if (ros::param::get("~max_payload", max_payload)) + { + sprintf(buf, "Max payload set to: %f [kg]", max_payload); + print_debug(buf); + } + robot_.setMinPayload(min_payload); + robot_.setMaxPayload(max_payload); + sprintf(buf, "Bounds for set_payload service calls: [%f, %f]", min_payload, max_payload); + print_debug(buf); - double servoj_gain = 300.; - if (ros::param::get("~servoj_gain", servoj_gain)) { - sprintf(buf, "Servoj_gain set to: %f [sec]", servoj_gain); - print_debug(buf); - } - robot_.setServojGain(servoj_gain); - - //Base and tool frames - base_frame_ = joint_prefix + "base_link"; - tool_frame_ = joint_prefix + "tool0_controller"; - if (ros::param::get("~base_frame", base_frame_)) { - sprintf(buf, "Base frame set to: %s", base_frame_.c_str()); - print_debug(buf); - } - if (ros::param::get("~tool_frame", tool_frame_)) { - sprintf(buf, "Tool frame set to: %s", tool_frame_.c_str()); - print_debug(buf); - } + double servoj_time = 0.008; + if (ros::param::get("~servoj_time", servoj_time)) + { + sprintf(buf, "Servoj_time set to: %f [sec]", servoj_time); + print_debug(buf); + } + robot_.setServojTime(servoj_time); - if (robot_.start()) { - if (use_ros_control_) { - ros_control_thread_ = new std::thread( - boost::bind(&RosWrapper::rosControlLoop, this)); - print_debug( - "The control thread for this driver has been started"); - } else { - //start actionserver - has_goal_ = false; - as_.start(); - - //subscribe to the data topic of interest - rt_publish_thread_ = new std::thread( - boost::bind(&RosWrapper::publishRTMsg, this)); - print_debug( - "The action server for this driver has been started"); - } - mb_publish_thread_ = new std::thread( - boost::bind(&RosWrapper::publishMbMsg, this)); - speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1, - &RosWrapper::speedInterface, this); - urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, - &RosWrapper::urscriptInterface, this); - - io_srv_ = nh_.advertiseService("ur_driver/set_io", - &RosWrapper::setIO, this); - payload_srv_ = nh_.advertiseService("ur_driver/set_payload", - &RosWrapper::setPayload, this); - } + double servoj_lookahead_time = 0.03; + if (ros::param::get("~servoj_lookahead_time", servoj_lookahead_time)) + { + sprintf(buf, "Servoj_lookahead_time set to: %f [sec]", servoj_lookahead_time); + print_debug(buf); } + robot_.setServojLookahead(servoj_lookahead_time); - void halt() + double servoj_gain = 300.; + if (ros::param::get("~servoj_gain", servoj_gain)) { - robot_.halt(); - rt_publish_thread_->join(); + sprintf(buf, "Servoj_gain set to: %f [sec]", servoj_gain); + print_debug(buf); } + robot_.setServojGain(servoj_gain); -private: - void trajThread(std::vector timestamps, - std::vector > positions, - std::vector > velocities) + // Base and tool frames + base_frame_ = joint_prefix + "base_link"; + tool_frame_ = joint_prefix + "tool0_controller"; + if (ros::param::get("~base_frame", base_frame_)) + { + sprintf(buf, "Base frame set to: %s", base_frame_.c_str()); + print_debug(buf); + } + if (ros::param::get("~tool_frame", tool_frame_)) { + sprintf(buf, "Tool frame set to: %s", tool_frame_.c_str()); + print_debug(buf); + } - robot_.doTraj(timestamps, positions, velocities); - if (has_goal_) { - result_.error_code = result_.SUCCESSFUL; - goal_handle_.setSucceeded(result_); - has_goal_ = false; - } + if (robot_.start()) + { + if (use_ros_control_) + { + ros_control_thread_ = new std::thread(boost::bind(&RosWrapper::rosControlLoop, this)); + print_debug("The control thread for this driver has been started"); + } + else + { + // start actionserver + has_goal_ = false; + as_.start(); + + // subscribe to the data topic of interest + rt_publish_thread_ = new std::thread(boost::bind(&RosWrapper::publishRTMsg, this)); + print_debug("The action server for this driver has been started"); + } + mb_publish_thread_ = new std::thread(boost::bind(&RosWrapper::publishMbMsg, this)); + speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1, &RosWrapper::speedInterface, this); + urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, &RosWrapper::urscriptInterface, this); + + io_srv_ = nh_.advertiseService("ur_driver/set_io", &RosWrapper::setIO, this); + payload_srv_ = nh_.advertiseService("ur_driver/set_payload", &RosWrapper::setPayload, this); } - void goalCB( - actionlib::ServerGoalHandle gh) - { - std::string buf; - print_info("on_goal"); - if (!robot_.sec_interface_->robot_state_->isReady()) { - result_.error_code = -100; //nothing is defined for this...? - - if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) { - result_.error_string = "Cannot accept new trajectories: Robot arm is not powered on"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - if (!robot_.sec_interface_->robot_state_->isRealRobotEnabled()) { - result_.error_string = "Cannot accept new trajectories: Robot is not enabled"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - result_.error_string = "Cannot accept new trajectories. (Debug: Robot mode is " - + std::to_string( - robot_.sec_interface_->robot_state_->getRobotMode()) - + ")"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - if (robot_.sec_interface_->robot_state_->isEmergencyStopped()) { - result_.error_code = -100; //nothing is defined for this...? - result_.error_string = "Cannot accept new trajectories: Robot is emergency stopped"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - if (robot_.sec_interface_->robot_state_->isProtectiveStopped()) { - result_.error_code = -100; //nothing is defined for this...? - result_.error_string = "Cannot accept new trajectories: Robot is protective stopped"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } + } - actionlib::ActionServer::Goal goal = *gh.getGoal(); //make a copy that we can modify - if (has_goal_) { - print_warning( - "Received new goal while still executing previous trajectory. Canceling previous trajectory"); - has_goal_ = false; - robot_.stopTraj(); - result_.error_code = -100; //nothing is defined for this...? - result_.error_string = "Received another trajectory"; - goal_handle_.setAborted(result_, result_.error_string); - std::this_thread::sleep_for(std::chrono::milliseconds(250)); - } - goal_handle_ = gh; - if (!validateJointNames()) { - std::string outp_joint_names = ""; - for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); - i++) { - outp_joint_names += goal.trajectory.joint_names[i] + " "; - } - result_.error_code = result_.INVALID_JOINTS; - result_.error_string = "Received a goal with incorrect joint names: " - + outp_joint_names; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - if (!has_positions()) { - result_.error_code = result_.INVALID_GOAL; - result_.error_string = "Received a goal without positions"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } + void halt() + { + robot_.halt(); + rt_publish_thread_->join(); + } - if (!has_velocities()) { - result_.error_code = result_.INVALID_GOAL; - result_.error_string = "Received a goal without velocities"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } +private: + void trajThread(std::vector timestamps, std::vector> positions, + std::vector> velocities) + { + robot_.doTraj(timestamps, positions, velocities); + if (has_goal_) + { + result_.error_code = result_.SUCCESSFUL; + goal_handle_.setSucceeded(result_); + has_goal_ = false; + } + } + void goalCB(actionlib::ServerGoalHandle gh) + { + std::string buf; + print_info("on_goal"); + if (!robot_.sec_interface_->robot_state_->isReady()) + { + result_.error_code = -100; // nothing is defined for this...? + + if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) + { + result_.error_string = "Cannot accept new trajectories: Robot arm is not powered on"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + if (!robot_.sec_interface_->robot_state_->isRealRobotEnabled()) + { + result_.error_string = "Cannot accept new trajectories: Robot is not enabled"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + result_.error_string = "Cannot accept new trajectories. (Debug: Robot mode is " + + std::to_string(robot_.sec_interface_->robot_state_->getRobotMode()) + ")"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + if (robot_.sec_interface_->robot_state_->isEmergencyStopped()) + { + result_.error_code = -100; // nothing is defined for this...? + result_.error_string = "Cannot accept new trajectories: Robot is emergency stopped"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + if (robot_.sec_interface_->robot_state_->isProtectiveStopped()) + { + result_.error_code = -100; // nothing is defined for this...? + result_.error_string = "Cannot accept new trajectories: Robot is protective stopped"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } - if (!traj_is_finite()) { - result_.error_string = "Received a goal with infinities or NaNs"; - result_.error_code = result_.INVALID_GOAL; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } + actionlib::ActionServer::Goal goal = + *gh.getGoal(); // make a copy that we can modify + if (has_goal_) + { + print_warning("Received new goal while still executing previous trajectory. Canceling previous trajectory"); + has_goal_ = false; + robot_.stopTraj(); + result_.error_code = -100; // nothing is defined for this...? + result_.error_string = "Received another trajectory"; + goal_handle_.setAborted(result_, result_.error_string); + std::this_thread::sleep_for(std::chrono::milliseconds(250)); + } + goal_handle_ = gh; + if (!validateJointNames()) + { + std::string outp_joint_names = ""; + for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) + { + outp_joint_names += goal.trajectory.joint_names[i] + " "; + } + result_.error_code = result_.INVALID_JOINTS; + result_.error_string = "Received a goal with incorrect joint names: " + outp_joint_names; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + if (!has_positions()) + { + result_.error_code = result_.INVALID_GOAL; + result_.error_string = "Received a goal without positions"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } - if (!has_limited_velocities()) { - result_.error_code = result_.INVALID_GOAL; - result_.error_string = "Received a goal with velocities that are higher than " - + std::to_string(max_velocity_); - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } + if (!has_velocities()) + { + result_.error_code = result_.INVALID_GOAL; + result_.error_string = "Received a goal without velocities"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } - reorder_traj_joints(goal.trajectory); + if (!traj_is_finite()) + { + result_.error_string = "Received a goal with infinities or NaNs"; + result_.error_code = result_.INVALID_GOAL; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } - if (!start_positions_match(goal.trajectory, 0.01)) { - result_.error_code = result_.INVALID_GOAL; - result_.error_string = "Goal start doesn't match current pose"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } + if (!has_limited_velocities()) + { + result_.error_code = result_.INVALID_GOAL; + result_.error_string = "Received a goal with velocities that are higher than " + std::to_string(max_velocity_); + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } - std::vector timestamps; - std::vector > positions, velocities; - if (goal.trajectory.points[0].time_from_start.toSec() != 0.) { - print_warning( - "Trajectory's first point should be the current position, with time_from_start set to 0.0 - Inserting point in malformed trajectory"); - timestamps.push_back(0.0); - positions.push_back( - robot_.rt_interface_->robot_state_->getQActual()); - velocities.push_back( - robot_.rt_interface_->robot_state_->getQdActual()); - } - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { - timestamps.push_back( - goal.trajectory.points[i].time_from_start.toSec()); - positions.push_back(goal.trajectory.points[i].positions); - velocities.push_back(goal.trajectory.points[i].velocities); - } + reorder_traj_joints(goal.trajectory); - goal_handle_.setAccepted(); - has_goal_ = true; - std::thread(&RosWrapper::trajThread, this, timestamps, positions, - velocities) - .detach(); + if (!start_positions_match(goal.trajectory, 0.01)) + { + result_.error_code = result_.INVALID_GOAL; + result_.error_string = "Goal start doesn't match current pose"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; } - void cancelCB( - actionlib::ServerGoalHandle gh) + std::vector timestamps; + std::vector> positions, velocities; + if (goal.trajectory.points[0].time_from_start.toSec() != 0.) { - // set the action state to preempted - print_info("on_cancel"); - if (has_goal_) { - if (gh == goal_handle_) { - robot_.stopTraj(); - has_goal_ = false; - } - } - result_.error_code = -100; //nothing is defined for this...? - result_.error_string = "Goal cancelled by client"; - gh.setCanceled(result_); - } - - bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp) - { - resp.success = true; - //if (req.fun == ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT) { - if (req.fun == 1) { - robot_.setDigitalOut(req.pin, req.state > 0.0 ? true : false); - } else if (req.fun == 2) { - //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_FLAG) { - robot_.setFlag(req.pin, req.state > 0.0 ? true : false); - //According to urdriver.py, set_flag will fail if called to rapidly in succession - ros::Duration(io_flag_delay_).sleep(); - } else if (req.fun == 3) { - //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_ANALOG_OUT) { - robot_.setAnalogOut(req.pin, req.state); - } else if (req.fun == 4) { - //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_TOOL_VOLTAGE) { - robot_.setToolVoltage((int)req.state); - } else { - resp.success = false; - } - return resp.success; + print_warning("Trajectory's first point should be the current position, with time_from_start set to 0.0 - " + "Inserting point in malformed trajectory"); + timestamps.push_back(0.0); + positions.push_back(robot_.rt_interface_->robot_state_->getQActual()); + velocities.push_back(robot_.rt_interface_->robot_state_->getQdActual()); } - - bool setPayload(ur_msgs::SetPayloadRequest& req, - ur_msgs::SetPayloadResponse& resp) + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { - if (robot_.setPayload(req.payload)) - resp.success = true; - else - resp.success = true; - return resp.success; - } - - bool validateJointNames() - { - std::vector actual_joint_names = robot_.getJointNames(); - actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); - if (goal.trajectory.joint_names.size() != actual_joint_names.size()) - return false; - - for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) { - unsigned int j; - for (j = 0; j < actual_joint_names.size(); j++) { - if (goal.trajectory.joint_names[i] == actual_joint_names[j]) - break; - } - if (goal.trajectory.joint_names[i] == actual_joint_names[j]) { - actual_joint_names.erase(actual_joint_names.begin() + j); - } else { - return false; - } - } - - return true; + timestamps.push_back(goal.trajectory.points[i].time_from_start.toSec()); + positions.push_back(goal.trajectory.points[i].positions); + velocities.push_back(goal.trajectory.points[i].velocities); } - void reorder_traj_joints(trajectory_msgs::JointTrajectory& traj) + goal_handle_.setAccepted(); + has_goal_ = true; + std::thread(&RosWrapper::trajThread, this, timestamps, positions, velocities).detach(); + } + + void cancelCB(actionlib::ServerGoalHandle gh) + { + // set the action state to preempted + print_info("on_cancel"); + if (has_goal_) { - /* Reorders trajectory - destructive */ - std::vector actual_joint_names = robot_.getJointNames(); - std::vector mapping; - mapping.resize(actual_joint_names.size(), actual_joint_names.size()); - for (unsigned int i = 0; i < traj.joint_names.size(); i++) { - for (unsigned int j = 0; j < actual_joint_names.size(); j++) { - if (traj.joint_names[i] == actual_joint_names[j]) - mapping[j] = i; - } - } - traj.joint_names = actual_joint_names; - std::vector new_traj; - for (unsigned int i = 0; i < traj.points.size(); i++) { - trajectory_msgs::JointTrajectoryPoint new_point; - for (unsigned int j = 0; j < traj.points[i].positions.size(); j++) { - new_point.positions.push_back( - traj.points[i].positions[mapping[j]]); - new_point.velocities.push_back( - traj.points[i].velocities[mapping[j]]); - if (traj.points[i].accelerations.size() != 0) - new_point.accelerations.push_back( - traj.points[i].accelerations[mapping[j]]); - } - new_point.time_from_start = traj.points[i].time_from_start; - new_traj.push_back(new_point); - } - traj.points = new_traj; + if (gh == goal_handle_) + { + robot_.stopTraj(); + has_goal_ = false; + } + } + result_.error_code = -100; // nothing is defined for this...? + result_.error_string = "Goal cancelled by client"; + gh.setCanceled(result_); + } + + bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp) + { + resp.success = true; + // if (req.fun == ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT) { + if (req.fun == 1) + { + robot_.setDigitalOut(req.pin, req.state > 0.0 ? true : false); + } + else if (req.fun == 2) + { + //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_FLAG) { + robot_.setFlag(req.pin, req.state > 0.0 ? true : false); + // According to urdriver.py, set_flag will fail if called to rapidly in succession + ros::Duration(io_flag_delay_).sleep(); + } + else if (req.fun == 3) + { + //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_ANALOG_OUT) { + robot_.setAnalogOut(req.pin, req.state); + } + else if (req.fun == 4) + { + //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_TOOL_VOLTAGE) { + robot_.setToolVoltage((int)req.state); + } + else + { + resp.success = false; + } + return resp.success; + } + + bool setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::SetPayloadResponse& resp) + { + if (robot_.setPayload(req.payload)) + resp.success = true; + else + resp.success = true; + return resp.success; + } + + bool validateJointNames() + { + std::vector actual_joint_names = robot_.getJointNames(); + actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); + if (goal.trajectory.joint_names.size() != actual_joint_names.size()) + return false; + + for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) + { + unsigned int j; + for (j = 0; j < actual_joint_names.size(); j++) + { + if (goal.trajectory.joint_names[i] == actual_joint_names[j]) + break; + } + if (goal.trajectory.joint_names[i] == actual_joint_names[j]) + { + actual_joint_names.erase(actual_joint_names.begin() + j); + } + else + { + return false; + } } - bool has_velocities() + return true; + } + + void reorder_traj_joints(trajectory_msgs::JointTrajectory& traj) + { + /* Reorders trajectory - destructive */ + std::vector actual_joint_names = robot_.getJointNames(); + std::vector mapping; + mapping.resize(actual_joint_names.size(), actual_joint_names.size()); + for (unsigned int i = 0; i < traj.joint_names.size(); i++) { - actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { - if (goal.trajectory.points[i].positions.size() - != goal.trajectory.points[i].velocities.size()) - return false; - } - return true; + for (unsigned int j = 0; j < actual_joint_names.size(); j++) + { + if (traj.joint_names[i] == actual_joint_names[j]) + mapping[j] = i; + } } + traj.joint_names = actual_joint_names; + std::vector new_traj; + for (unsigned int i = 0; i < traj.points.size(); i++) + { + trajectory_msgs::JointTrajectoryPoint new_point; + for (unsigned int j = 0; j < traj.points[i].positions.size(); j++) + { + new_point.positions.push_back(traj.points[i].positions[mapping[j]]); + new_point.velocities.push_back(traj.points[i].velocities[mapping[j]]); + if (traj.points[i].accelerations.size() != 0) + new_point.accelerations.push_back(traj.points[i].accelerations[mapping[j]]); + } + new_point.time_from_start = traj.points[i].time_from_start; + new_traj.push_back(new_point); + } + traj.points = new_traj; + } - bool has_positions() + bool has_velocities() + { + actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { - actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); - if (goal.trajectory.points.size() == 0) - return false; - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { - if (goal.trajectory.points[i].positions.size() - != goal.trajectory.joint_names.size()) - return false; - } - return true; + if (goal.trajectory.points[i].positions.size() != goal.trajectory.points[i].velocities.size()) + return false; + } + return true; + } + + bool has_positions() + { + actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); + if (goal.trajectory.points.size() == 0) + return false; + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) + { + if (goal.trajectory.points[i].positions.size() != goal.trajectory.joint_names.size()) + return false; } + return true; + } - bool start_positions_match(const trajectory_msgs::JointTrajectory& traj, double eps) + bool start_positions_match(const trajectory_msgs::JointTrajectory& traj, double eps) + { + for (unsigned int i = 0; i < traj.points[0].positions.size(); i++) { - for (unsigned int i = 0; i < traj.points[0].positions.size(); i++) { - std::vector qActual = robot_.rt_interface_->robot_state_->getQActual(); - if (fabs(traj.points[0].positions[i] - qActual[i]) > eps) { - return false; - } - } - return true; + std::vector qActual = robot_.rt_interface_->robot_state_->getQActual(); + if (fabs(traj.points[0].positions[i] - qActual[i]) > eps) + { + return false; + } } + return true; + } - bool has_limited_velocities() + bool has_limited_velocities() + { + actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { - actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { - for (unsigned int j = 0; - j < goal.trajectory.points[i].velocities.size(); j++) { - if (fabs(goal.trajectory.points[i].velocities[j]) - > max_velocity_) - return false; - } - } - return true; + for (unsigned int j = 0; j < goal.trajectory.points[i].velocities.size(); j++) + { + if (fabs(goal.trajectory.points[i].velocities[j]) > max_velocity_) + return false; + } } + return true; + } - bool traj_is_finite() + bool traj_is_finite() + { + actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { - actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { - for (unsigned int j = 0; - j < goal.trajectory.points[i].velocities.size(); j++) { - if (!std::isfinite(goal.trajectory.points[i].positions[j])) - return false; - if (!std::isfinite(goal.trajectory.points[i].velocities[j])) - return false; - } - } - return true; + for (unsigned int j = 0; j < goal.trajectory.points[i].velocities.size(); j++) + { + if (!std::isfinite(goal.trajectory.points[i].positions[j])) + return false; + if (!std::isfinite(goal.trajectory.points[i].velocities[j])) + return false; + } } + return true; + } - void speedInterface(const trajectory_msgs::JointTrajectory::Ptr& msg) + void speedInterface(const trajectory_msgs::JointTrajectory::Ptr& msg) + { + if (msg->points[0].velocities.size() == 6) { - if (msg->points[0].velocities.size() == 6) { - double acc = 100; - if (msg->points[0].accelerations.size() > 0) - acc = *std::max_element(msg->points[0].accelerations.begin(), - msg->points[0].accelerations.end()); - robot_.setSpeed(msg->points[0].velocities[0], - msg->points[0].velocities[1], msg->points[0].velocities[2], - msg->points[0].velocities[3], msg->points[0].velocities[4], - msg->points[0].velocities[5], acc); - } + double acc = 100; + if (msg->points[0].accelerations.size() > 0) + acc = *std::max_element(msg->points[0].accelerations.begin(), msg->points[0].accelerations.end()); + robot_.setSpeed(msg->points[0].velocities[0], msg->points[0].velocities[1], msg->points[0].velocities[2], + msg->points[0].velocities[3], msg->points[0].velocities[4], msg->points[0].velocities[5], acc); } - void urscriptInterface(const std_msgs::String::ConstPtr& msg) - { - - robot_.rt_interface_->addCommandToQueue(msg->data); - } - - void rosControlLoop() - { - ros::Duration elapsed_time; - struct timespec last_time, current_time; - static const double BILLION = 1000000000.0; - - realtime_tools::RealtimePublisher tf_pub(nh_, "/tf", 1); - geometry_msgs::TransformStamped tool_transform; - tool_transform.header.frame_id = base_frame_; - tool_transform.child_frame_id = tool_frame_; - tf_pub.msg_.transforms.push_back(tool_transform); - - realtime_tools::RealtimePublisher tool_vel_pub(nh_, "tool_velocity", 1); - tool_vel_pub.msg_.header.frame_id = base_frame_; - - clock_gettime(CLOCK_MONOTONIC, &last_time); - while (ros::ok()) { - std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex - std::unique_lock locker(msg_lock); - while (!robot_.rt_interface_->robot_state_->getControllerUpdated()) { - rt_msg_cond_.wait(locker); - } - // Input - hardware_interface_->read(); - robot_.rt_interface_->robot_state_->setControllerUpdated(); - - // Control - clock_gettime(CLOCK_MONOTONIC, ¤t_time); - elapsed_time = ros::Duration(current_time.tv_sec - last_time.tv_sec + (current_time.tv_nsec - last_time.tv_nsec) / BILLION); - ros::Time ros_time = ros::Time::now(); - controller_manager_->update(ros_time, elapsed_time); - last_time = current_time; - - // Output - hardware_interface_->write(); - - // Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation - std::vector tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual(); - - // Compute rotation angle - double rx = tool_vector_actual[3]; - double ry = tool_vector_actual[4]; - double rz = tool_vector_actual[5]; - double angle = std::sqrt(std::pow(rx, 2) + std::pow(ry, 2) + std::pow(rz, 2)); - - // Broadcast transform - if (tf_pub.trylock()) { - tf_pub.msg_.transforms[0].header.stamp = ros_time; - if (angle < 1e-16) { - tf_pub.msg_.transforms[0].transform.rotation.x = 0; - tf_pub.msg_.transforms[0].transform.rotation.y = 0; - tf_pub.msg_.transforms[0].transform.rotation.z = 0; - tf_pub.msg_.transforms[0].transform.rotation.w = 1; - } else { - tf_pub.msg_.transforms[0].transform.rotation.x = (rx / angle) * std::sin(angle * 0.5); - tf_pub.msg_.transforms[0].transform.rotation.y = (ry / angle) * std::sin(angle * 0.5); - tf_pub.msg_.transforms[0].transform.rotation.z = (rz / angle) * std::sin(angle * 0.5); - tf_pub.msg_.transforms[0].transform.rotation.w = std::cos(angle * 0.5); - } - tf_pub.msg_.transforms[0].transform.translation.x = tool_vector_actual[0]; - tf_pub.msg_.transforms[0].transform.translation.y = tool_vector_actual[1]; - tf_pub.msg_.transforms[0].transform.translation.z = tool_vector_actual[2]; - - tf_pub.unlockAndPublish(); - } - - //Publish tool velocity - std::vector tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual(); - - if (tool_vel_pub.trylock()) { - tool_vel_pub.msg_.header.stamp = ros_time; - tool_vel_pub.msg_.twist.linear.x = tcp_speed[0]; - tool_vel_pub.msg_.twist.linear.y = tcp_speed[1]; - tool_vel_pub.msg_.twist.linear.z = tcp_speed[2]; - tool_vel_pub.msg_.twist.angular.x = tcp_speed[3]; - tool_vel_pub.msg_.twist.angular.y = tcp_speed[4]; - tool_vel_pub.msg_.twist.angular.z = tcp_speed[5]; - - tool_vel_pub.unlockAndPublish(); - } + } + void urscriptInterface(const std_msgs::String::ConstPtr& msg) + { + robot_.rt_interface_->addCommandToQueue(msg->data); + } + + void rosControlLoop() + { + ros::Duration elapsed_time; + struct timespec last_time, current_time; + static const double BILLION = 1000000000.0; + + realtime_tools::RealtimePublisher tf_pub(nh_, "/tf", 1); + geometry_msgs::TransformStamped tool_transform; + tool_transform.header.frame_id = base_frame_; + tool_transform.child_frame_id = tool_frame_; + tf_pub.msg_.transforms.push_back(tool_transform); + + realtime_tools::RealtimePublisher tool_vel_pub(nh_, "tool_velocity", 1); + tool_vel_pub.msg_.header.frame_id = base_frame_; + + clock_gettime(CLOCK_MONOTONIC, &last_time); + while (ros::ok()) + { + std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex + std::unique_lock locker(msg_lock); + while (!robot_.rt_interface_->robot_state_->getControllerUpdated()) + { + rt_msg_cond_.wait(locker); + } + // Input + hardware_interface_->read(); + robot_.rt_interface_->robot_state_->setControllerUpdated(); + + // Control + clock_gettime(CLOCK_MONOTONIC, ¤t_time); + elapsed_time = + ros::Duration(current_time.tv_sec - last_time.tv_sec + (current_time.tv_nsec - last_time.tv_nsec) / BILLION); + ros::Time ros_time = ros::Time::now(); + controller_manager_->update(ros_time, elapsed_time); + last_time = current_time; + + // Output + hardware_interface_->write(); + + // Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation + // vector representation of the tool orientation + std::vector tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual(); + + // Compute rotation angle + double rx = tool_vector_actual[3]; + double ry = tool_vector_actual[4]; + double rz = tool_vector_actual[5]; + double angle = std::sqrt(std::pow(rx, 2) + std::pow(ry, 2) + std::pow(rz, 2)); + + // Broadcast transform + if (tf_pub.trylock()) + { + tf_pub.msg_.transforms[0].header.stamp = ros_time; + if (angle < 1e-16) + { + tf_pub.msg_.transforms[0].transform.rotation.x = 0; + tf_pub.msg_.transforms[0].transform.rotation.y = 0; + tf_pub.msg_.transforms[0].transform.rotation.z = 0; + tf_pub.msg_.transforms[0].transform.rotation.w = 1; } + else + { + tf_pub.msg_.transforms[0].transform.rotation.x = (rx / angle) * std::sin(angle * 0.5); + tf_pub.msg_.transforms[0].transform.rotation.y = (ry / angle) * std::sin(angle * 0.5); + tf_pub.msg_.transforms[0].transform.rotation.z = (rz / angle) * std::sin(angle * 0.5); + tf_pub.msg_.transforms[0].transform.rotation.w = std::cos(angle * 0.5); + } + tf_pub.msg_.transforms[0].transform.translation.x = tool_vector_actual[0]; + tf_pub.msg_.transforms[0].transform.translation.y = tool_vector_actual[1]; + tf_pub.msg_.transforms[0].transform.translation.z = tool_vector_actual[2]; + + tf_pub.unlockAndPublish(); + } + + // Publish tool velocity + std::vector tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual(); + + if (tool_vel_pub.trylock()) + { + tool_vel_pub.msg_.header.stamp = ros_time; + tool_vel_pub.msg_.twist.linear.x = tcp_speed[0]; + tool_vel_pub.msg_.twist.linear.y = tcp_speed[1]; + tool_vel_pub.msg_.twist.linear.z = tcp_speed[2]; + tool_vel_pub.msg_.twist.angular.x = tcp_speed[3]; + tool_vel_pub.msg_.twist.angular.y = tcp_speed[4]; + tool_vel_pub.msg_.twist.angular.z = tcp_speed[5]; + + tool_vel_pub.unlockAndPublish(); + } } - - void publishRTMsg() - { - ros::Publisher joint_pub = nh_.advertise( - "joint_states", 1); - ros::Publisher wrench_pub = nh_.advertise( - "wrench", 1); - ros::Publisher tool_vel_pub = nh_.advertise("tool_velocity", 1); - static tf::TransformBroadcaster br; - while (ros::ok()) { - sensor_msgs::JointState joint_msg; - joint_msg.name = robot_.getJointNames(); - geometry_msgs::WrenchStamped wrench_msg; - geometry_msgs::PoseStamped tool_pose_msg; - std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex - std::unique_lock locker(msg_lock); - while (!robot_.rt_interface_->robot_state_->getDataPublished()) { - rt_msg_cond_.wait(locker); - } - joint_msg.header.stamp = ros::Time::now(); - joint_msg.position = robot_.rt_interface_->robot_state_->getQActual(); - for (unsigned int i = 0; i < joint_msg.position.size(); i++) { - joint_msg.position[i] += joint_offsets_[i]; - } - joint_msg.velocity = robot_.rt_interface_->robot_state_->getQdActual(); - joint_msg.effort = robot_.rt_interface_->robot_state_->getIActual(); - joint_pub.publish(joint_msg); - std::vector tcp_force = robot_.rt_interface_->robot_state_->getTcpForce(); - wrench_msg.header.stamp = joint_msg.header.stamp; - wrench_msg.wrench.force.x = tcp_force[0]; - wrench_msg.wrench.force.y = tcp_force[1]; - wrench_msg.wrench.force.z = tcp_force[2]; - wrench_msg.wrench.torque.x = tcp_force[3]; - wrench_msg.wrench.torque.y = tcp_force[4]; - wrench_msg.wrench.torque.z = tcp_force[5]; - wrench_pub.publish(wrench_msg); - - // Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation - std::vector tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual(); - - //Create quaternion - tf::Quaternion quat; - 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)); - if (angle < 1e-16) { - quat.setValue(0, 0, 0, 1); - } else { - quat.setRotation(tf::Vector3(rx / angle, ry / angle, rz / angle), angle); - } - - //Create and broadcast transform - tf::Transform transform; - transform.setOrigin(tf::Vector3(tool_vector_actual[0], tool_vector_actual[1], tool_vector_actual[2])); - transform.setRotation(quat); - br.sendTransform(tf::StampedTransform(transform, joint_msg.header.stamp, base_frame_, tool_frame_)); - - //Publish tool velocity - std::vector tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual(); - geometry_msgs::TwistStamped tool_twist; - tool_twist.header.frame_id = base_frame_; - tool_twist.header.stamp = joint_msg.header.stamp; - tool_twist.twist.linear.x = tcp_speed[0]; - tool_twist.twist.linear.y = tcp_speed[1]; - tool_twist.twist.linear.z = tcp_speed[2]; - tool_twist.twist.angular.x = tcp_speed[3]; - tool_twist.twist.angular.y = tcp_speed[4]; - tool_twist.twist.angular.z = tcp_speed[5]; - tool_vel_pub.publish(tool_twist); - - robot_.rt_interface_->robot_state_->setDataPublished(); - } + } + + void publishRTMsg() + { + ros::Publisher joint_pub = nh_.advertise("joint_states", 1); + ros::Publisher wrench_pub = nh_.advertise("wrench", 1); + ros::Publisher tool_vel_pub = nh_.advertise("tool_velocity", 1); + static tf::TransformBroadcaster br; + while (ros::ok()) + { + sensor_msgs::JointState joint_msg; + joint_msg.name = robot_.getJointNames(); + geometry_msgs::WrenchStamped wrench_msg; + geometry_msgs::PoseStamped tool_pose_msg; + std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex + std::unique_lock locker(msg_lock); + while (!robot_.rt_interface_->robot_state_->getDataPublished()) + { + rt_msg_cond_.wait(locker); + } + joint_msg.header.stamp = ros::Time::now(); + joint_msg.position = robot_.rt_interface_->robot_state_->getQActual(); + for (unsigned int i = 0; i < joint_msg.position.size(); i++) + { + joint_msg.position[i] += joint_offsets_[i]; + } + joint_msg.velocity = robot_.rt_interface_->robot_state_->getQdActual(); + joint_msg.effort = robot_.rt_interface_->robot_state_->getIActual(); + joint_pub.publish(joint_msg); + std::vector tcp_force = robot_.rt_interface_->robot_state_->getTcpForce(); + wrench_msg.header.stamp = joint_msg.header.stamp; + wrench_msg.wrench.force.x = tcp_force[0]; + wrench_msg.wrench.force.y = tcp_force[1]; + wrench_msg.wrench.force.z = tcp_force[2]; + wrench_msg.wrench.torque.x = tcp_force[3]; + wrench_msg.wrench.torque.y = tcp_force[4]; + wrench_msg.wrench.torque.z = tcp_force[5]; + wrench_pub.publish(wrench_msg); + + // Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation + // vector representation of the tool orientation + std::vector tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual(); + + // Create quaternion + tf::Quaternion quat; + 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)); + if (angle < 1e-16) + { + quat.setValue(0, 0, 0, 1); + } + else + { + quat.setRotation(tf::Vector3(rx / angle, ry / angle, rz / angle), angle); + } + + // Create and broadcast transform + tf::Transform transform; + transform.setOrigin(tf::Vector3(tool_vector_actual[0], tool_vector_actual[1], tool_vector_actual[2])); + transform.setRotation(quat); + br.sendTransform(tf::StampedTransform(transform, joint_msg.header.stamp, base_frame_, tool_frame_)); + + // Publish tool velocity + std::vector tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual(); + geometry_msgs::TwistStamped tool_twist; + tool_twist.header.frame_id = base_frame_; + tool_twist.header.stamp = joint_msg.header.stamp; + tool_twist.twist.linear.x = tcp_speed[0]; + tool_twist.twist.linear.y = tcp_speed[1]; + tool_twist.twist.linear.z = tcp_speed[2]; + tool_twist.twist.angular.x = tcp_speed[3]; + tool_twist.twist.angular.y = tcp_speed[4]; + tool_twist.twist.angular.z = tcp_speed[5]; + tool_vel_pub.publish(tool_twist); + + robot_.rt_interface_->robot_state_->setDataPublished(); } + } - void publishMbMsg() - { - bool warned = false; - ros::Publisher io_pub = nh_.advertise( - "ur_driver/io_states", 1); - - while (ros::ok()) { - ur_msgs::IOStates io_msg; - std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex - std::unique_lock locker(msg_lock); - while (!robot_.sec_interface_->robot_state_->getNewDataAvailable()) { - msg_cond_.wait(locker); - } - int i_max = 10; - if (robot_.sec_interface_->robot_state_->getVersion() > 3.0) - i_max = 18; // From version 3.0, there are up to 18 inputs and outputs - for (unsigned int i = 0; i < i_max; i++) { - ur_msgs::Digital digi; - digi.pin = i; - digi.state = ((robot_.sec_interface_->robot_state_->getDigitalInputBits() - & (1 << i)) - >> i); - io_msg.digital_in_states.push_back(digi); - digi.state = ((robot_.sec_interface_->robot_state_->getDigitalOutputBits() - & (1 << i)) - >> i); - io_msg.digital_out_states.push_back(digi); - } - ur_msgs::Analog ana; - ana.pin = 0; - ana.state = robot_.sec_interface_->robot_state_->getAnalogInput0(); - io_msg.analog_in_states.push_back(ana); - ana.pin = 1; - ana.state = robot_.sec_interface_->robot_state_->getAnalogInput1(); - io_msg.analog_in_states.push_back(ana); - - ana.pin = 0; - ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput0(); - io_msg.analog_out_states.push_back(ana); - ana.pin = 1; - ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput1(); - io_msg.analog_out_states.push_back(ana); - io_pub.publish(io_msg); - - if (robot_.sec_interface_->robot_state_->isEmergencyStopped() - or robot_.sec_interface_->robot_state_->isProtectiveStopped()) { - if (robot_.sec_interface_->robot_state_->isEmergencyStopped() - and !warned) { - print_error("Emergency stop pressed!"); - } else if (robot_.sec_interface_->robot_state_->isProtectiveStopped() - and !warned) { - print_error("Robot is protective stopped!"); - } - if (has_goal_) { - print_error("Aborting trajectory"); - robot_.stopTraj(); - result_.error_code = result_.SUCCESSFUL; - result_.error_string = "Robot was halted"; - goal_handle_.setAborted(result_, result_.error_string); - has_goal_ = false; - } - warned = true; - } else - warned = false; - - robot_.sec_interface_->robot_state_->finishedReading(); - } + void publishMbMsg() + { + bool warned = false; + ros::Publisher io_pub = nh_.advertise("ur_driver/io_states", 1); + + while (ros::ok()) + { + ur_msgs::IOStates io_msg; + std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex + std::unique_lock locker(msg_lock); + while (!robot_.sec_interface_->robot_state_->getNewDataAvailable()) + { + msg_cond_.wait(locker); + } + int i_max = 10; + if (robot_.sec_interface_->robot_state_->getVersion() > 3.0) + i_max = 18; // From version 3.0, there are up to 18 inputs and outputs + for (unsigned int i = 0; i < i_max; i++) + { + ur_msgs::Digital digi; + digi.pin = i; + digi.state = ((robot_.sec_interface_->robot_state_->getDigitalInputBits() & (1 << i)) >> i); + io_msg.digital_in_states.push_back(digi); + digi.state = ((robot_.sec_interface_->robot_state_->getDigitalOutputBits() & (1 << i)) >> i); + io_msg.digital_out_states.push_back(digi); + } + ur_msgs::Analog ana; + ana.pin = 0; + ana.state = robot_.sec_interface_->robot_state_->getAnalogInput0(); + io_msg.analog_in_states.push_back(ana); + ana.pin = 1; + ana.state = robot_.sec_interface_->robot_state_->getAnalogInput1(); + io_msg.analog_in_states.push_back(ana); + + ana.pin = 0; + ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput0(); + io_msg.analog_out_states.push_back(ana); + ana.pin = 1; + ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput1(); + io_msg.analog_out_states.push_back(ana); + io_pub.publish(io_msg); + + if (robot_.sec_interface_->robot_state_->isEmergencyStopped() or + robot_.sec_interface_->robot_state_->isProtectiveStopped()) + { + if (robot_.sec_interface_->robot_state_->isEmergencyStopped() and !warned) + { + print_error("Emergency stop pressed!"); + } + else if (robot_.sec_interface_->robot_state_->isProtectiveStopped() and !warned) + { + print_error("Robot is protective stopped!"); + } + if (has_goal_) + { + print_error("Aborting trajectory"); + robot_.stopTraj(); + result_.error_code = result_.SUCCESSFUL; + result_.error_string = "Robot was halted"; + goal_handle_.setAborted(result_, result_.error_string); + has_goal_ = false; + } + warned = true; + } + else + warned = false; + + robot_.sec_interface_->robot_state_->finishedReading(); } + } }; int main(int argc, char** argv) { - bool use_sim_time = false; - std::string host; - int reverse_port = 50001; - - ros::init(argc, argv, "ur_driver"); - ros::NodeHandle nh; - if (ros::param::get("use_sim_time", use_sim_time)) { - print_warning("use_sim_time is set!!"); - } - if (!(ros::param::get("~robot_ip_address", host))) { - if (argc > 1) { - print_warning( - "Please set the parameter robot_ip_address instead of giving it as a command line argument. This method is DEPRECATED"); - host = argv[1]; - } else { - print_fatal( - "Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip"); - exit(1); - } + bool use_sim_time = false; + std::string host; + int reverse_port = 50001; + + ros::init(argc, argv, "ur_driver"); + ros::NodeHandle nh; + if (ros::param::get("use_sim_time", use_sim_time)) + { + print_warning("use_sim_time is set!!"); + } + if (!(ros::param::get("~robot_ip_address", host))) + { + if (argc > 1) + { + print_warning("Please set the parameter robot_ip_address instead of giving it as a command line argument. This " + "method is DEPRECATED"); + host = argv[1]; } - if ((ros::param::get("~reverse_port", reverse_port))) { - if ((reverse_port <= 0) or (reverse_port >= 65535)) { - print_warning("Reverse port value is not valid (Use number between 1 and 65534. Using default value of 50001"); - reverse_port = 50001; - } - } else - reverse_port = 50001; + else + { + print_fatal("Could not get robot ip. Please supply it as command line parameter or on the parameter server as " + "robot_ip"); + exit(1); + } + } + if ((ros::param::get("~reverse_port", reverse_port))) + { + if ((reverse_port <= 0) or (reverse_port >= 65535)) + { + print_warning("Reverse port value is not valid (Use number between 1 and 65534. Using default value of 50001"); + reverse_port = 50001; + } + } + else + reverse_port = 50001; - RosWrapper interface(host, reverse_port); + RosWrapper interface(host, reverse_port); - ros::AsyncSpinner spinner(3); - spinner.start(); + ros::AsyncSpinner spinner(3); + spinner.start(); - ros::waitForShutdown(); + ros::waitForShutdown(); - interface.halt(); + interface.halt(); - exit(0); + exit(0); } From 7828304e37f490adfb10c288c338d16594ecd3e5 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Fri, 3 Mar 2017 03:55:29 +0100 Subject: [PATCH 032/114] Removed old state file --- src/ur/state.cpp | 115 ----------------------------------------------- 1 file changed, 115 deletions(-) delete mode 100644 src/ur/state.cpp diff --git a/src/ur/state.cpp b/src/ur/state.cpp deleted file mode 100644 index 66af615b6..000000000 --- a/src/ur/state.cpp +++ /dev/null @@ -1,115 +0,0 @@ -#include "ur_modern_driver/ur/state.h" -#include "ur_modern_driver/log.h" - -// StatePacket::~StatePacket() { } - -/* -bool RobotState::parseWith(BinParser &bp) { - //continue as long as there are bytes to read - while(!bp.empty()) { - - if(!bp.checkSize(sizeof(uint32_t))){ - LOG_ERROR("Failed to read sub-package length, there's likely a parsing error"); - return false; - } - - uint32_t sub_size = bp.peek(); - if(!bp.checkSize(static_cast(sub_size))) { - LOG_WARN("Invalid sub-package size of %" PRIu32 " received!", sub_size); - return false; - } - - //deconstruction of a sub parser will increment the position of the parent parser - BinParser sub_parser(bp, sub_size); - sub_parser.consume(sizeof(sub_size)); - - if(!parse_package(sub_parser)) { - LOG_ERROR("Failed to parse sub-package"); - return false; - } - - if(!sub_parser.empty()) { - LOG_ERROR("Sub-package was not parsed completely!"); - return false; - } - } - return true; -} - -template -bool parse_base(BinParser &bp, T &pkg) { - package_type type = bp.peek(); - switch(type) { - case package_type::ROBOT_MODE_DATA: - LOG_DEBUG("Parsing robot_mode"); - bp.consume(sizeof(package_type)); - pkg.robot_mode.parseWith(bp); - break; - case package_type::MASTERBOARD_DATA: - LOG_DEBUG("Parsing master_board"); - bp.consume(sizeof(package_type)); - pkg.master_board.parseWith(bp); - break; - - case package_type::TOOL_DATA: - case package_type::CARTESIAN_INFO: - case package_type::JOINT_DATA: - LOG_DEBUG("Skipping tool, cartesian or joint data"); - //for unhandled packages we consume the rest of the - //package buffer - bp.consume(); - break; - default: - return false; - } - return true; -} - - -template -bool parse_advanced(BinParser &bp, T &pkg) { - if(parse_base(bp, pkg)) - return true; - - package_type type = bp.peek(); - - switch(type) { - case package_type::KINEMATICS_INFO: - case package_type::CONFIGURATION_DATA: - case package_type::ADDITIONAL_INFO: - case package_type::CALIBRATION_DATA: - case package_type::FORCE_MODE_DATA: - LOG_DEBUG("Skipping kinematics, config, additional, calibration or force mode data"); - //for unhandled packages we consume the rest of the - //package buffer - bp.consume(); - break; - default: - LOG_ERROR("Invalid sub-package type parsed: %" PRIu8 "", type); - return false; - } - - return true; -} - - -bool RobotState_V1_6__7::parse_package(BinParser &bp) { - if(!parse_base(bp, *this)) { - LOG_ERROR("Invalid sub-package type parsed: %" PRIu8 "", bp.peek()); - return false; - } - return true; -} - -bool RobotState_V1_8::parse_package(BinParser &bp) { - return parse_advanced(bp, *this); -} - -bool RobotState_V3_0__1::parse_package(BinParser &bp) { - return parse_advanced(bp, *this); -} - -bool RobotState_V3_2::parse_package(BinParser &bp) { - return parse_advanced(bp, *this); -} -*/ \ No newline at end of file From aa26bb599690b20c495bf1ccf168e855ad3b767b Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Tue, 7 Mar 2017 02:50:46 +0100 Subject: [PATCH 033/114] Added tests --- CMakeLists.txt | 35 +++- include/ur_modern_driver/test/random_data.h | 46 +++++ include/ur_modern_driver/test/utils.h | 8 + tests/main.cpp | 7 + tests/ur/master_board.cpp | 0 tests/ur/robot_mode.cpp | 105 +++++++++++ tests/ur/rt_state.cpp | 192 ++++++++++++++++++++ 7 files changed, 383 insertions(+), 10 deletions(-) create mode 100644 include/ur_modern_driver/test/random_data.h create mode 100644 include/ur_modern_driver/test/utils.h create mode 100644 tests/main.cpp create mode 100644 tests/ur/master_board.cpp create mode 100644 tests/ur/robot_mode.cpp create mode 100644 tests/ur/rt_state.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index e9f3b834b..7f1badfae 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,10 @@ cmake_minimum_required(VERSION 2.8.3) project(ur_modern_driver) + + add_definitions( -DROS_BUILD ) + + ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages @@ -130,6 +134,8 @@ else() message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler. Suggested solution: update the pkg build-essential ") endif() +set(CMAKE_CXX_FLAGS "-g -Wall -Wextra ${CMAKE_CXX_FLAGS}") + ## Specify additional locations of header files ## Your package locations should be listed before other locations # include_directories(include) @@ -152,18 +158,26 @@ target_link_libraries(ur_hardware_interface ## Declare a C++ executable set(${PROJECT_NAME}_SOURCES - src/ur_ros_wrapper.cpp + src/ros/rt_publisher.cpp + src/ur/stream.cpp + src/ur/robot_mode.cpp + src/ur/master_board.cpp + src/ur/rt_state.cpp + src/ur/messages.cpp + #src/ros_main.cpp + #src/ur_ros_wrapper.cpp src/ur_driver.cpp src/ur_realtime_communication.cpp src/ur_communication.cpp src/robot_state.cpp src/robot_state_RT.cpp src/do_output.cpp) -add_executable(ur_driver ${${PROJECT_NAME}_SOURCES}) + +add_executable(ur_driver ${${PROJECT_NAME}_SOURCES} src/ros_main.cpp) ## Add cmake target dependencies of the executable ## same as for the library above - add_dependencies(ur_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(ur_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against target_link_libraries(ur_driver @@ -200,11 +214,12 @@ install(DIRECTORY include/${PROJECT_NAME}/ ## Testing ## ############# -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_ur_modern_driver.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +set(${PROJECT_NAME}_TEST_SOURCES + tests/ur/rt_state.cpp + tests/ur/robot_mode.cpp) + +if (CATKIN_ENABLE_TESTING) + catkin_add_gtest(ur_modern_driver_test ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_TEST_SOURCES} tests/main.cpp) + target_link_libraries(ur_modern_driver_test ur_hardware_interface ${catkin_LIBRARIES}) +endif() diff --git a/include/ur_modern_driver/test/random_data.h b/include/ur_modern_driver/test/random_data.h new file mode 100644 index 000000000..14ae65b0a --- /dev/null +++ b/include/ur_modern_driver/test/random_data.h @@ -0,0 +1,46 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include "ur_modern_driver/bin_parser.h" + +class RandomDataTest +{ +private: + using random_bytes_engine = std::independent_bits_engine; + uint8_t* _buf; + BinParser bp_; + size_t n_; + +public: + RandomDataTest(size_t n) : _buf(new uint8_t[n]), bp_(_buf, n), n_(n) + { + random_bytes_engine rbe; + std::generate(_buf, _buf + n, std::ref(rbe)); + } + + ~RandomDataTest() + { + delete _buf; + } + + BinParser getParser(bool skip = false) + { + return BinParser(_buf, n_ - (skip ? sizeof(int32_t) : 0)); + } + + template + T getNext() + { + T actual; + bp_.parse(actual); + return actual; + } + void skip(size_t n) + { + bp_.consume(n); + } +}; \ No newline at end of file diff --git a/include/ur_modern_driver/test/utils.h b/include/ur_modern_driver/test/utils.h new file mode 100644 index 000000000..edbb53116 --- /dev/null +++ b/include/ur_modern_driver/test/utils.h @@ -0,0 +1,8 @@ +#pragma once + +#define ASSERT_DOUBLE_ARRAY_EQ(fn, name) \ + for (auto const& v : name) \ + { \ + ASSERT_EQ(fn, v) << #name " failed parsing"; \ + } + diff --git a/tests/main.cpp b/tests/main.cpp new file mode 100644 index 000000000..92cbc41f1 --- /dev/null +++ b/tests/main.cpp @@ -0,0 +1,7 @@ +#include "gtest/gtest.h" + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/tests/ur/master_board.cpp b/tests/ur/master_board.cpp new file mode 100644 index 000000000..e69de29bb diff --git a/tests/ur/robot_mode.cpp b/tests/ur/robot_mode.cpp new file mode 100644 index 000000000..170e0cddb --- /dev/null +++ b/tests/ur/robot_mode.cpp @@ -0,0 +1,105 @@ +#include +#include "ur_modern_driver/ur/robot_mode.h" +#include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/types.h" +#include "ur_modern_driver/test/utils.h" +#include "ur_modern_driver/test/random_data.h" + +TEST(RobotModeData_V1_X, testRandomDataParsing) +{ + RandomDataTest rdt(24); + BinParser bp = rdt.getParser(); + RobotModeData_V1_X state; + ASSERT_TRUE(state.parseWith(bp)) << "parse() returned false"; + + ASSERT_EQ(rdt.getNext(), state.timestamp); + ASSERT_EQ(rdt.getNext(), state.physical_robot_connected); + ASSERT_EQ(rdt.getNext(), state.real_robot_enabled); + ASSERT_EQ(rdt.getNext(), state.robot_power_on); + ASSERT_EQ(rdt.getNext(), state.emergency_stopped); + ASSERT_EQ(rdt.getNext(), state.security_stopped); + ASSERT_EQ(rdt.getNext(), state.program_running); + ASSERT_EQ(rdt.getNext(), state.program_paused); + ASSERT_EQ(rdt.getNext(), state.robot_mode); + ASSERT_EQ(rdt.getNext(), state.speed_fraction); + + ASSERT_TRUE(bp.empty()) << "Did not consume all data"; +} + + +TEST(RobotModeData_V3_0__1, testRandomDataParsing) +{ + RandomDataTest rdt(33); + BinParser bp = rdt.getParser(); + RobotModeData_V3_0__1 state; + ASSERT_TRUE(state.parseWith(bp)) << "parse() returned false"; + + ASSERT_EQ(rdt.getNext(), state.timestamp); + ASSERT_EQ(rdt.getNext(), state.physical_robot_connected); + ASSERT_EQ(rdt.getNext(), state.real_robot_enabled); + ASSERT_EQ(rdt.getNext(), state.robot_power_on); + ASSERT_EQ(rdt.getNext(), state.emergency_stopped); + ASSERT_EQ(rdt.getNext(), state.protective_stopped); + ASSERT_EQ(rdt.getNext(), state.program_running); + ASSERT_EQ(rdt.getNext(), state.program_paused); + ASSERT_EQ(rdt.getNext(), state.robot_mode); + ASSERT_EQ(rdt.getNext(), state.control_mode); + ASSERT_EQ(rdt.getNext(), state.target_speed_fraction); + ASSERT_EQ(rdt.getNext(), state.speed_scaling); + + ASSERT_TRUE(bp.empty()) << "Did not consume all data"; +} + + +TEST(RobotModeData_V3_2, testRandomDataParsing) +{ + RandomDataTest rdt(41); + BinParser bp = rdt.getParser(); + RobotModeData_V3_2 state; + ASSERT_TRUE(state.parseWith(bp)) << "parse() returned false"; + + ASSERT_EQ(rdt.getNext(), state.timestamp); + ASSERT_EQ(rdt.getNext(), state.physical_robot_connected); + ASSERT_EQ(rdt.getNext(), state.real_robot_enabled); + ASSERT_EQ(rdt.getNext(), state.robot_power_on); + ASSERT_EQ(rdt.getNext(), state.emergency_stopped); + ASSERT_EQ(rdt.getNext(), state.protective_stopped); + ASSERT_EQ(rdt.getNext(), state.program_running); + ASSERT_EQ(rdt.getNext(), state.program_paused); + ASSERT_EQ(rdt.getNext(), state.robot_mode); + ASSERT_EQ(rdt.getNext(), state.control_mode); + ASSERT_EQ(rdt.getNext(), state.target_speed_fraction); + ASSERT_EQ(rdt.getNext(), state.speed_scaling); + ASSERT_EQ(rdt.getNext(), state.target_speed_fraction_limit); + + ASSERT_TRUE(bp.empty()) << "Did not consume all data"; +} + + + +TEST(RobotModeData_V1_X, testTooSmallBuffer) +{ + RandomDataTest rdt(10); + BinParser bp = rdt.getParser(); + RobotModeData_V1_X state; + EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough"; +} + + +TEST(RobotModeData_V3_0__1, testTooSmallBuffer) +{ + RandomDataTest rdt(10); + BinParser bp = rdt.getParser(); + RobotModeData_V3_0__1 state; + EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough"; +} + + +TEST(RobotModeData_V3_2, testTooSmallBuffer) +{ + RandomDataTest rdt(10); + BinParser bp = rdt.getParser(); + RobotModeData_V3_2 state; + EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough"; +} diff --git a/tests/ur/rt_state.cpp b/tests/ur/rt_state.cpp new file mode 100644 index 000000000..c24c72a85 --- /dev/null +++ b/tests/ur/rt_state.cpp @@ -0,0 +1,192 @@ +#include +#include "ur_modern_driver/ur/rt_state.h" +#include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/types.h" +#include "ur_modern_driver/test/utils.h" +#include "ur_modern_driver/test/random_data.h" + +TEST(RTState_V1_6__7, testRandomDataParsing) +{ + RandomDataTest rdt(764); + BinParser bp = rdt.getParser(true); + RTState_V1_6__7 state; + EXPECT_TRUE(state.parseWith(bp)) << "parse() returned false"; + + ASSERT_EQ(rdt.getNext(), state.time); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.q_target); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.qd_target); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.qdd_target); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.i_target); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.m_target); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.q_actual); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.qd_actual); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.i_actual); + ASSERT_EQ(rdt.getNext(), state.tool_accelerometer_values); + rdt.skip(sizeof(double) * 15); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.tcp_force); + ASSERT_EQ(rdt.getNext(), state.tool_vector_actual); + ASSERT_EQ(rdt.getNext(), state.tcp_speed_actual); + ASSERT_EQ(rdt.getNext(), state.digital_inputs); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.motor_temperatures); + ASSERT_EQ(rdt.getNext(), state.controller_time); + rdt.skip(sizeof(double)); // skip unused value + ASSERT_EQ(rdt.getNext(), state.robot_mode); + + EXPECT_TRUE(bp.empty()) << "Did not consume all data"; +} + +TEST(RTState_V1_8, testRandomDataParsing) +{ + RandomDataTest rdt(812); + BinParser bp = rdt.getParser(true); + RTState_V1_8 state; + EXPECT_TRUE(state.parseWith(bp)) << "parse() returned false"; + + ASSERT_EQ(rdt.getNext(), state.time); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.q_target); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.qd_target); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.qdd_target); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.i_target); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.m_target); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.q_actual); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.qd_actual); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.i_actual); + ASSERT_EQ(rdt.getNext(), state.tool_accelerometer_values); + rdt.skip(sizeof(double) * 15); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.tcp_force); + ASSERT_EQ(rdt.getNext(), state.tool_vector_actual); + ASSERT_EQ(rdt.getNext(), state.tcp_speed_actual); + ASSERT_EQ(rdt.getNext(), state.digital_inputs); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.motor_temperatures); + ASSERT_EQ(rdt.getNext(), state.controller_time); + rdt.skip(sizeof(double)); // skip unused value + ASSERT_EQ(rdt.getNext(), state.robot_mode); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.joint_modes); + + EXPECT_TRUE(bp.empty()) << "Did not consume all data"; +} + +TEST(RTState_V3_0__1, testRandomDataParsing) +{ + RandomDataTest rdt(1044); + BinParser bp = rdt.getParser(true); + RTState_V3_0__1 state; + EXPECT_TRUE(state.parseWith(bp)) << "parse() returned false"; + + ASSERT_EQ(rdt.getNext(), state.time); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.q_target); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.qd_target); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.qdd_target); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.i_target); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.m_target); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.q_actual); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.qd_actual); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.i_actual); + + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.i_control); + ASSERT_EQ(rdt.getNext(), state.tool_vector_actual); + ASSERT_EQ(rdt.getNext(), state.tcp_speed_actual); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.tcp_force); + ASSERT_EQ(rdt.getNext(), state.tool_vector_target); + ASSERT_EQ(rdt.getNext(), state.tcp_speed_target); + + ASSERT_EQ(rdt.getNext(), state.digital_inputs); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.motor_temperatures); + ASSERT_EQ(rdt.getNext(), state.controller_time); + rdt.skip(sizeof(double)); // skip unused value + ASSERT_EQ(rdt.getNext(), state.robot_mode); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.joint_modes); + ASSERT_EQ(rdt.getNext(), state.safety_mode); + rdt.skip(sizeof(double) * 6); + ASSERT_EQ(rdt.getNext(), state.tool_accelerometer_values); + rdt.skip(sizeof(double) * 6); + ASSERT_EQ(rdt.getNext(), state.speed_scaling); + ASSERT_EQ(rdt.getNext(), state.linear_momentum_norm); + rdt.skip(sizeof(double) * 2); + ASSERT_EQ(rdt.getNext(), state.v_main); + ASSERT_EQ(rdt.getNext(), state.v_robot); + ASSERT_EQ(rdt.getNext(), state.i_robot); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.v_actual); + + EXPECT_TRUE(bp.empty()) << "Did not consume all data"; +} + +TEST(RTState_V3_2__3, testRandomDataParsing) +{ + RandomDataTest rdt(1060); + BinParser bp = rdt.getParser(true); + RTState_V3_2__3 state; + EXPECT_TRUE(state.parseWith(bp)) << "parse() returned false"; + + ASSERT_EQ(rdt.getNext(), state.time); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.q_target); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.qd_target); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.qdd_target); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.i_target); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.m_target); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.q_actual); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.qd_actual); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.i_actual); + + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.i_control); + ASSERT_EQ(rdt.getNext(), state.tool_vector_actual); + ASSERT_EQ(rdt.getNext(), state.tcp_speed_actual); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.tcp_force); + ASSERT_EQ(rdt.getNext(), state.tool_vector_target); + ASSERT_EQ(rdt.getNext(), state.tcp_speed_target); + + ASSERT_EQ(rdt.getNext(), state.digital_inputs); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.motor_temperatures); + ASSERT_EQ(rdt.getNext(), state.controller_time); + rdt.skip(sizeof(double)); // skip unused value + ASSERT_EQ(rdt.getNext(), state.robot_mode); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.joint_modes); + ASSERT_EQ(rdt.getNext(), state.safety_mode); + rdt.skip(sizeof(double) * 6); + ASSERT_EQ(rdt.getNext(), state.tool_accelerometer_values); + rdt.skip(sizeof(double) * 6); + ASSERT_EQ(rdt.getNext(), state.speed_scaling); + ASSERT_EQ(rdt.getNext(), state.linear_momentum_norm); + rdt.skip(sizeof(double) * 2); + ASSERT_EQ(rdt.getNext(), state.v_main); + ASSERT_EQ(rdt.getNext(), state.v_robot); + ASSERT_EQ(rdt.getNext(), state.i_robot); + ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.v_actual); + ASSERT_EQ(rdt.getNext(), state.digital_outputs); + ASSERT_EQ(rdt.getNext(), state.program_state); + + EXPECT_TRUE(bp.empty()) << "did not consume all data"; +} + +TEST(RTState_V1_6__7, testTooSmallBuffer) +{ + RandomDataTest rdt(10); + BinParser bp = rdt.getParser(true); + RTState_V1_6__7 state; + EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough"; +} + +TEST(RTState_V1_8, testTooSmallBuffer) +{ + RandomDataTest rdt(10); + BinParser bp = rdt.getParser(true); + RTState_V1_8 state; + EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough"; +} + +TEST(RTState_V3_0__1, testTooSmallBuffer) +{ + RandomDataTest rdt(10); + BinParser bp = rdt.getParser(true); + RTState_V3_0__1 state; + EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough"; +} + +TEST(RTState_V3_2__3, testTooSmallBuffer) +{ + RandomDataTest rdt(10); + BinParser bp = rdt.getParser(true); + RTState_V3_2__3 state; + EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough"; +} \ No newline at end of file From e478987dc4f9eddbe86d4b38cb3bca59aef01f87 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Wed, 8 Mar 2017 11:00:16 +0100 Subject: [PATCH 034/114] Refactored const size arrays to std::array --- include/ur_modern_driver/bin_parser.h | 2 +- include/ur_modern_driver/ur/rt_state.h | 28 +++++++++++++------------- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/include/ur_modern_driver/bin_parser.h b/include/ur_modern_driver/bin_parser.h index a4f9372c1..16378a0ba 100644 --- a/include/ur_modern_driver/bin_parser.h +++ b/include/ur_modern_driver/bin_parser.h @@ -136,7 +136,7 @@ class BinParser } template - void parse(T (&array)[N]) + void parse(std::array &array) { for (size_t i = 0; i < N; i++) { diff --git a/include/ur_modern_driver/ur/rt_state.h b/include/ur_modern_driver/ur/rt_state.h index bb4e9e991..fdc93bc72 100644 --- a/include/ur_modern_driver/ur/rt_state.h +++ b/include/ur_modern_driver/ur/rt_state.h @@ -23,18 +23,18 @@ class RTShared public: double time; - double q_target[6]; - double qd_target[6]; - double qdd_target[6]; - double i_target[6]; - double m_target[6]; - double q_actual[6]; - double qd_actual[6]; - double i_actual[6]; + std::array q_target; + std::array qd_target; + std::array qdd_target; + std::array i_target; + std::array m_target; + std::array q_actual; + std::array qd_actual; + std::array i_actual; // gap here depending on version - double tcp_force[6]; + std::array tcp_force; // does not contain "_actual" postfix in V1_X but // they're the same fields so share anyway @@ -44,7 +44,7 @@ class RTShared // gap here depending on version uint64_t digital_inputs; - double motor_temperatures[6]; + std::array motor_temperatures; double controller_time; double robot_mode; @@ -71,7 +71,7 @@ class RTState_V1_8 : public RTState_V1_6__7 bool parseWith(BinParser& bp); virtual bool consumeWith(URRTPacketConsumer& consumer); - double joint_modes[6]; + std::array joint_modes; static const size_t SIZE = RTState_V1_6__7::SIZE + sizeof(double[6]); @@ -84,11 +84,11 @@ class RTState_V3_0__1 : public RTShared, public RTPacket bool parseWith(BinParser& bp); virtual bool consumeWith(URRTPacketConsumer& consumer); - double i_control[6]; + std::array i_control; cartesian_coord_t tool_vector_target; cartesian_coord_t tcp_speed_target; - double joint_modes[6]; + std::array joint_modes; double safety_mode; double3_t tool_accelerometer_values; double speed_scaling; @@ -96,7 +96,7 @@ class RTState_V3_0__1 : public RTShared, public RTPacket double v_main; double v_robot; double i_robot; - double v_actual[6]; + std::array v_actual; static const size_t SIZE = RTShared::SIZE + sizeof(double[6]) * 3 + sizeof(double3_t) + sizeof(cartesian_coord_t) * 2 + sizeof(double) * 6; From 618dd35c435f8d0af00fe4a6c05198a1c6873c88 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Thu, 16 Mar 2017 04:56:07 +0100 Subject: [PATCH 035/114] formatting --- include/ur_modern_driver/bin_parser.h | 2 +- include/ur_modern_driver/test/utils.h | 5 ++--- tests/ur/robot_mode.cpp | 12 +++--------- tests/ur/rt_state.cpp | 6 +++--- 4 files changed, 9 insertions(+), 16 deletions(-) diff --git a/include/ur_modern_driver/bin_parser.h b/include/ur_modern_driver/bin_parser.h index 16378a0ba..e5692f5ee 100644 --- a/include/ur_modern_driver/bin_parser.h +++ b/include/ur_modern_driver/bin_parser.h @@ -136,7 +136,7 @@ class BinParser } template - void parse(std::array &array) + void parse(std::array& array) { for (size_t i = 0; i < N; i++) { diff --git a/include/ur_modern_driver/test/utils.h b/include/ur_modern_driver/test/utils.h index edbb53116..83b668459 100644 --- a/include/ur_modern_driver/test/utils.h +++ b/include/ur_modern_driver/test/utils.h @@ -1,8 +1,7 @@ #pragma once -#define ASSERT_DOUBLE_ARRAY_EQ(fn, name) \ +#define ASSERT_DOUBLE_ARRAY_EQ(fn, name) \ for (auto const& v : name) \ { \ - ASSERT_EQ(fn, v) << #name " failed parsing"; \ + ASSERT_EQ(fn, v) << #name " failed parsing"; \ } - diff --git a/tests/ur/robot_mode.cpp b/tests/ur/robot_mode.cpp index 170e0cddb..a13fcb805 100644 --- a/tests/ur/robot_mode.cpp +++ b/tests/ur/robot_mode.cpp @@ -1,10 +1,10 @@ -#include #include "ur_modern_driver/ur/robot_mode.h" +#include #include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/log.h" -#include "ur_modern_driver/types.h" -#include "ur_modern_driver/test/utils.h" #include "ur_modern_driver/test/random_data.h" +#include "ur_modern_driver/test/utils.h" +#include "ur_modern_driver/types.h" TEST(RobotModeData_V1_X, testRandomDataParsing) { @@ -27,7 +27,6 @@ TEST(RobotModeData_V1_X, testRandomDataParsing) ASSERT_TRUE(bp.empty()) << "Did not consume all data"; } - TEST(RobotModeData_V3_0__1, testRandomDataParsing) { RandomDataTest rdt(33); @@ -51,7 +50,6 @@ TEST(RobotModeData_V3_0__1, testRandomDataParsing) ASSERT_TRUE(bp.empty()) << "Did not consume all data"; } - TEST(RobotModeData_V3_2, testRandomDataParsing) { RandomDataTest rdt(41); @@ -76,8 +74,6 @@ TEST(RobotModeData_V3_2, testRandomDataParsing) ASSERT_TRUE(bp.empty()) << "Did not consume all data"; } - - TEST(RobotModeData_V1_X, testTooSmallBuffer) { RandomDataTest rdt(10); @@ -86,7 +82,6 @@ TEST(RobotModeData_V1_X, testTooSmallBuffer) EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough"; } - TEST(RobotModeData_V3_0__1, testTooSmallBuffer) { RandomDataTest rdt(10); @@ -95,7 +90,6 @@ TEST(RobotModeData_V3_0__1, testTooSmallBuffer) EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough"; } - TEST(RobotModeData_V3_2, testTooSmallBuffer) { RandomDataTest rdt(10); diff --git a/tests/ur/rt_state.cpp b/tests/ur/rt_state.cpp index c24c72a85..5c7920e51 100644 --- a/tests/ur/rt_state.cpp +++ b/tests/ur/rt_state.cpp @@ -1,10 +1,10 @@ -#include #include "ur_modern_driver/ur/rt_state.h" +#include #include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/log.h" -#include "ur_modern_driver/types.h" -#include "ur_modern_driver/test/utils.h" #include "ur_modern_driver/test/random_data.h" +#include "ur_modern_driver/test/utils.h" +#include "ur_modern_driver/types.h" TEST(RTState_V1_6__7, testRandomDataParsing) { From a4ca9f1703079d3117f400cb67ad367abb85a054 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Thu, 13 Apr 2017 10:34:51 +0200 Subject: [PATCH 036/114] Changed masterboard IO data to bitset --- include/ur_modern_driver/bin_parser.h | 10 ++++++++++ include/ur_modern_driver/ur/master_board.h | 10 ++++++---- src/ur/master_board.cpp | 10 +++++----- 3 files changed, 21 insertions(+), 9 deletions(-) diff --git a/include/ur_modern_driver/bin_parser.h b/include/ur_modern_driver/bin_parser.h index e5692f5ee..dbf6df55f 100644 --- a/include/ur_modern_driver/bin_parser.h +++ b/include/ur_modern_driver/bin_parser.h @@ -6,6 +6,8 @@ #include #include #include +#include +#include #include "ur_modern_driver/log.h" #include "ur_modern_driver/types.h" @@ -144,6 +146,14 @@ class BinParser } } + template + void parse(std::bitset& set) + { + T val; + parse(val); + set = std::bitset(val); + } + void consume() { buf_pos_ = buf_end_; diff --git a/include/ur_modern_driver/ur/master_board.h b/include/ur_modern_driver/ur/master_board.h index a0d9e7a6c..c625cbf8d 100644 --- a/include/ur_modern_driver/ur/master_board.h +++ b/include/ur_modern_driver/ur/master_board.h @@ -2,6 +2,7 @@ #include #include +#include #include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/types.h" #include "ur_modern_driver/ur/state.h" @@ -41,8 +42,9 @@ class MasterBoardData_V1_X : public SharedMasterBoardData, public StatePacket virtual bool parseWith(BinParser& bp); virtual bool consumeWith(URStatePacketConsumer& consumer); - int16_t digital_input_bits; - int16_t digital_output_bits; + + std::bitset<10> digital_input_bits; + std::bitset<10> digital_output_bits; uint8_t master_safety_state; bool master_on_off_state; @@ -62,8 +64,8 @@ class MasterBoardData_V3_0__1 : public SharedMasterBoardData, public StatePacket virtual bool parseWith(BinParser& bp); virtual bool consumeWith(URStatePacketConsumer& consumer); - int32_t digital_input_bits; - int32_t digital_output_bits; + std::bitset<18> digital_input_bits; + std::bitset<18> digital_output_bits; uint8_t safety_mode; bool in_reduced_mode; diff --git a/src/ur/master_board.cpp b/src/ur/master_board.cpp index ecada6929..991b6ac85 100644 --- a/src/ur/master_board.cpp +++ b/src/ur/master_board.cpp @@ -23,8 +23,8 @@ bool MasterBoardData_V1_X::parseWith(BinParser& bp) if (!bp.checkSize()) return false; - bp.parse(digital_input_bits); - bp.parse(digital_output_bits); + bp.parse(digital_input_bits); + bp.parse(digital_output_bits); SharedMasterBoardData::parseWith(bp); @@ -49,8 +49,8 @@ bool MasterBoardData_V3_0__1::parseWith(BinParser& bp) if (!bp.checkSize()) return false; - bp.parse(digital_input_bits); - bp.parse(digital_output_bits); + bp.parse(digital_input_bits); + bp.parse(digital_output_bits); SharedMasterBoardData::parseWith(bp); @@ -59,7 +59,7 @@ bool MasterBoardData_V3_0__1::parseWith(BinParser& bp) bp.parse(euromap67_interface_installed); if (euromap67_interface_installed) - { + { if (!bp.checkSize(MasterBoardData_V3_0__1::EURO_SIZE)) return false; From 06b2db45bf0a6c1194d4f0e185fe939286ab27b5 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Thu, 13 Apr 2017 10:39:22 +0200 Subject: [PATCH 037/114] Improved pipeline design --- include/ur_modern_driver/pipeline.h | 52 ++++++++++++++++++++++++-- include/ur_modern_driver/ur/consumer.h | 6 +-- include/ur_modern_driver/ur/state.h | 2 + 3 files changed, 54 insertions(+), 6 deletions(-) diff --git a/include/ur_modern_driver/pipeline.h b/include/ur_modern_driver/pipeline.h index 49b1c9eca..9f487b154 100644 --- a/include/ur_modern_driver/pipeline.h +++ b/include/ur_modern_driver/pipeline.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include "ur_modern_driver/log.h" @@ -23,7 +24,52 @@ class IConsumer { } - virtual bool consume(unique_ptr product) = 0; + virtual bool consume(shared_ptr product) = 0; +}; + +template +class MultiConsumer : public IConsumer +{ +private: + std::vector*> consumers_; + +public: + MultiConsumer(std::vector*> consumers) : consumers_(consumers) + { + } + + virtual void setupConsumer() + { + for(auto &con : consumers_) + { + con->setupConsumer(); + } + } + virtual void teardownConsumer() + { + for(auto &con : consumers_) + { + con->teardownConsumer(); + } + } + virtual void stopConsumer() + { + for(auto &con : consumers_) + { + con->stopConsumer(); + } + } + + bool consume(shared_ptr product) + { + bool res = true; + for(auto &con : consumers_) + { + if(!con->consume(product)) + res = false; + } + return res; + } }; template @@ -87,8 +133,8 @@ class Pipeline { // 16000us timeout was chosen because we should // roughly recieve messages at 125hz which is every - // 8ms == 8000us and double it for some error margin - if (!queue_.wait_dequeue_timed(product, 16000)) + // 8ms so double it for some error margin + if (!queue_.wait_dequeue_timed(product, std::chrono::milliseconds(16))) { continue; } diff --git a/include/ur_modern_driver/ur/consumer.h b/include/ur_modern_driver/ur/consumer.h index 991401ad4..6617a80ba 100644 --- a/include/ur_modern_driver/ur/consumer.h +++ b/include/ur_modern_driver/ur/consumer.h @@ -10,7 +10,7 @@ class URRTPacketConsumer : public IConsumer { public: - virtual bool consume(unique_ptr packet) + virtual bool consume(shared_ptr packet) { return packet->consumeWith(*this); } @@ -24,7 +24,7 @@ class URRTPacketConsumer : public IConsumer class URStatePacketConsumer : public IConsumer { public: - virtual bool consume(unique_ptr packet) + virtual bool consume(shared_ptr packet) { return packet->consumeWith(*this); } @@ -41,7 +41,7 @@ class URStatePacketConsumer : public IConsumer class URMessagePacketConsumer : public IConsumer { public: - virtual bool consume(unique_ptr packet) + virtual bool consume(shared_ptr packet) { return packet->consumeWith(*this); } diff --git a/include/ur_modern_driver/ur/state.h b/include/ur_modern_driver/ur/state.h index b76d34ef9..4aeddbaed 100644 --- a/include/ur_modern_driver/ur/state.h +++ b/include/ur_modern_driver/ur/state.h @@ -32,6 +32,8 @@ class URStatePacketConsumer; class StatePacket { public: + StatePacket() {} + virtual ~StatePacket() {} virtual bool parseWith(BinParser& bp) = 0; virtual bool consumeWith(URStatePacketConsumer& consumer) = 0; }; From 1e34cdaa373890339e76069ef5f69bbff9bbce49 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Thu, 13 Apr 2017 10:40:27 +0200 Subject: [PATCH 038/114] Improved stream implementation --- include/ur_modern_driver/ur/stream.h | 5 ++++- src/ur/stream.cpp | 13 ++++++++++++- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/include/ur_modern_driver/ur/stream.h b/include/ur_modern_driver/ur/stream.h index 6990c4a6d..eac3c098f 100644 --- a/include/ur_modern_driver/ur/stream.h +++ b/include/ur_modern_driver/ur/stream.h @@ -2,6 +2,7 @@ #include #include #include +#include #include #include @@ -15,6 +16,7 @@ class URStream std::atomic initialized_; std::atomic stopping_; + std::mutex send_mutex_, receive_mutex_; public: URStream(std::string& host, int port) : host_(host), port_(port), initialized_(false), stopping_(false) @@ -28,7 +30,8 @@ class URStream bool connect(); void disconnect(); + void reconnect(); - ssize_t send(uint8_t* buf, size_t buf_len); + ssize_t send(const uint8_t* buf, size_t buf_len); ssize_t receive(uint8_t* buf, size_t buf_len); }; \ No newline at end of file diff --git a/src/ur/stream.cpp b/src/ur/stream.cpp index 819651e34..240b261c6 100644 --- a/src/ur/stream.cpp +++ b/src/ur/stream.cpp @@ -73,13 +73,22 @@ void URStream::disconnect() initialized_ = false; } -ssize_t URStream::send(uint8_t* buf, size_t buf_len) +void URStream::reconnect() +{ + disconnect(); + stopping_ = false; + connect(); +} + +ssize_t URStream::send(const uint8_t* buf, size_t buf_len) { if (!initialized_) return -1; if (stopping_) return 0; + std::lock_guard lock(send_mutex_); + size_t total = 0; size_t remaining = buf_len; @@ -104,6 +113,8 @@ ssize_t URStream::receive(uint8_t* buf, size_t buf_len) if (stopping_) return 0; + std::lock_guard lock(receive_mutex_); + size_t remainder = sizeof(int32_t); uint8_t* buf_pos = buf; bool initial = true; From 0302b0569133f7eb4e6a38e942acab9a80ead41c Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Thu, 13 Apr 2017 10:42:39 +0200 Subject: [PATCH 039/114] Ignore initial version message --- include/ur_modern_driver/ur/state_parser.h | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/include/ur_modern_driver/ur/state_parser.h b/include/ur_modern_driver/ur/state_parser.h index 031efb2e2..1e9331434 100644 --- a/include/ur_modern_driver/ur/state_parser.h +++ b/include/ur_modern_driver/ur/state_parser.h @@ -33,6 +33,13 @@ class URStateParser : public URParser bp.parse(packet_size); bp.parse(type); + //quietly ignore the intial version message + if (type == message_type::ROBOT_MESSAGE) + { + bp.consume(); + return true; + } + if (type != message_type::ROBOT_STATE) { LOG_WARN("Invalid message type recieved: %u", static_cast(type)); @@ -53,6 +60,8 @@ class URStateParser : public URParser return false; } + LOG_DEBUG("sub-packet size: %" PRIu32, sub_size); + // deconstruction of a sub parser will increment the position of the parent parser BinParser sbp(bp, sub_size); sbp.consume(sizeof(sub_size)); @@ -64,7 +73,7 @@ class URStateParser : public URParser if (packet == nullptr) { sbp.consume(); - LOG_INFO("Skipping sub-packet of type %d", type); + LOG_DEBUG("Skipping sub-packet of type %d", type); continue; } @@ -78,7 +87,8 @@ class URStateParser : public URParser if (!sbp.empty()) { - LOG_ERROR("Sub-package was not parsed completely!"); + LOG_ERROR("Sub-package of type %d was not parsed completely!", type); + sbp.debug(); return false; } } From bbe2102ac88871d2c5d85d235ca1c52caef1b32e Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Thu, 13 Apr 2017 10:43:23 +0200 Subject: [PATCH 040/114] Initial implementation of commander --- include/ur_modern_driver/ur/commander.h | 27 +++++++++++ src/ur/commander.cpp | 62 +++++++++++++++++++++++++ 2 files changed, 89 insertions(+) create mode 100644 include/ur_modern_driver/ur/commander.h create mode 100644 src/ur/commander.cpp diff --git a/include/ur_modern_driver/ur/commander.h b/include/ur_modern_driver/ur/commander.h new file mode 100644 index 000000000..6325f8c62 --- /dev/null +++ b/include/ur_modern_driver/ur/commander.h @@ -0,0 +1,27 @@ +#pragma once +#include +#include +#include +#include "ur_modern_driver/ur/stream.h" + +class URCommander +{ +private: + URStream& stream_; + +protected: + bool write(std::string& s); + +public: + URCommander(URStream& stream) : stream_(stream) + { + } + + virtual bool speedj(std::array &speeds, double acceleration); + virtual bool stopj(double a = 10.0); + virtual bool setDigitalOut(uint8_t pin, bool value); + virtual bool setAnalogOut(uint8_t pin, double value); + virtual bool setToolVoltage(uint8_t voltage); + virtual bool setFlag(bool value); + virtual bool setPayload(double value); +}; \ No newline at end of file diff --git a/src/ur/commander.cpp b/src/ur/commander.cpp new file mode 100644 index 000000000..d9e16d2f1 --- /dev/null +++ b/src/ur/commander.cpp @@ -0,0 +1,62 @@ +#include "ur_modern_driver/ur/commander.h" +#include "ur_modern_driver/log.h" + +bool URCommander::write(std::string& s) +{ + size_t len = s.size(); + const uint8_t* data = reinterpret_cast(s.c_str()); + ssize_t res = stream_.send(data, len); + return res > 0 && static_cast(res) == len; +} + + +bool URCommander::speedj(std::array &speeds, double acceleration) +{ + std::ostringstream out; + out << std::fixed << std::setprecision(4); + out << "speedj(["; + std::string mod; + for(auto const& val : speeds) + { + out << mod << val; + mod = ","; + } + out << "]," << acceleration << ")\n"; + std::string s(out.str()); + return write(s); +} +bool URCommander::stopj(double a) +{ + +} + +bool URCommander::setAnalogOut(uint8_t pin, double value) +{ + std::ostringstream out; + out << "set_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(4) << value << ")\n"; + std::string s(out.str()); + return write(s); +} + +bool URCommander::setDigitalOut(uint8_t pin, bool value) +{ + std::ostringstream out; + out << "set_digital_out(" << (int)pin << "," << (value ? "True" : "False") << ")\n"; + std::string s(out.str()); + return write(s); +} + +bool URCommander::setToolVoltage(uint8_t voltage) +{ + +} + + +bool URCommander::setFlag(bool value) +{ + +} +bool URCommander::setPayload(double value) +{ + +} \ No newline at end of file From 2a80601e843a68f8364e61e0a2e95dbb508df29a Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Thu, 13 Apr 2017 10:45:58 +0200 Subject: [PATCH 041/114] Rt publisher improvement --- src/ros/rt_publisher.cpp | 17 ++++------------- 1 file changed, 4 insertions(+), 13 deletions(-) diff --git a/src/ros/rt_publisher.cpp b/src/ros/rt_publisher.cpp index 26a283bfa..f9ab2ab2f 100644 --- a/src/ros/rt_publisher.cpp +++ b/src/ros/rt_publisher.cpp @@ -4,20 +4,11 @@ bool RTPublisher::publishJoints(RTShared& packet, Time& t) { sensor_msgs::JointState joint_msg; joint_msg.header.stamp = t; - joint_msg.name = joint_names_; - for (auto const& q : packet.q_actual) - { - joint_msg.position.push_back(q); - } - for (auto const& qd : packet.qd_actual) - { - joint_msg.velocity.push_back(qd); - } - for (auto const& i : packet.i_actual) - { - joint_msg.effort.push_back(i); - } + joint_msg.name.assign(joint_names_.begin(), joint_names_.end()); + joint_msg.position.assign(packet.q_actual.begin(), packet.q_actual.end()); + joint_msg.velocity.assign(packet.qd_actual.begin(), packet.qd_actual.end()); + joint_msg.effort.assign(packet.i_actual.begin(), packet.i_actual.end()); joint_pub_.publish(joint_msg); From 97add752a168209d96ee5b42da60618bf25aee22 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Thu, 13 Apr 2017 10:47:41 +0200 Subject: [PATCH 042/114] Implemented ros control --- .../ur_modern_driver/ros/hardware_interface.h | 168 ++++++++++++++++++ include/ur_modern_driver/ros/io_service.h | 61 +++++++ include/ur_modern_driver/ros/mb_publisher.h | 55 ++++++ include/ur_modern_driver/ros/robot_hardware.h | 46 +++++ include/ur_modern_driver/ros/ros_controller.h | 60 +++++++ src/ros/mb_publisher.cpp | 54 ++++++ src/ros/robot_hardware.cpp | 76 ++++++++ 7 files changed, 520 insertions(+) create mode 100644 include/ur_modern_driver/ros/hardware_interface.h create mode 100644 include/ur_modern_driver/ros/io_service.h create mode 100644 include/ur_modern_driver/ros/mb_publisher.h create mode 100644 include/ur_modern_driver/ros/robot_hardware.h create mode 100644 include/ur_modern_driver/ros/ros_controller.h create mode 100644 src/ros/mb_publisher.cpp create mode 100644 src/ros/robot_hardware.cpp diff --git a/include/ur_modern_driver/ros/hardware_interface.h b/include/ur_modern_driver/ros/hardware_interface.h new file mode 100644 index 000000000..e970e301e --- /dev/null +++ b/include/ur_modern_driver/ros/hardware_interface.h @@ -0,0 +1,168 @@ +#pragma once +#include +#include +#include +#include +#include +#include "ur_modern_driver/ur/commander.h" +#include "ur_modern_driver/ur/rt_state.h" + +class HardwareInterface +{ +public: + virtual void write() = 0; + virtual void start() + { + } + virtual void stop() + { + } +}; + +using hardware_interface::JointHandle; +using hardware_interface::JointStateHandle; +using hardware_interface::JointStateInterface; + +class JointInterface : public JointStateInterface +{ +private: + std::array velocities_, positions_, efforts_; + +public: + JointInterface(std::vector& joint_names) + { + for (size_t i = 0; i < 6; i++) + { + registerHandle(JointStateHandle(joint_names[i], &positions_[i], &velocities_[i], &efforts_[i])); + } + } + + void update(RTShared& packet) + { + positions_ = packet.q_actual; + velocities_ = packet.qd_actual; + efforts_ = packet.i_actual; + } +}; + +class WrenchInterface : public hardware_interface::ForceTorqueSensorInterface +{ + std::array tcp_; +public: + WrenchInterface() + { + registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", "", tcp_.begin(), tcp_.begin()+3)); + } + + void update(RTShared& packet) + { + tcp_ = packet.tcp_force; + } +}; + +class VelocityInterface : public HardwareInterface, public hardware_interface::VelocityJointInterface +{ +private: + URCommander& commander_; + std::array velocity_cmd_, prev_velocity_cmd_; + double max_vel_change_; + +public: + VelocityInterface(URCommander& commander, JointStateInterface& js_interface, std::vector& joint_names) + : commander_(commander) + { + for (size_t i = 0; i < 6; i++) + { + registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &velocity_cmd_[i])); + } + } + + virtual void write() + { + for (size_t i = 0; i < 6; i++) + { + double prev = prev_velocity_cmd_[i]; + double lo = prev - max_vel_change_; + double hi = prev + max_vel_change_; + // clamp value to ±max_vel_change + prev_velocity_cmd_[i] = std::max(lo, std::min(velocity_cmd_[i], hi)); + } + + //times 125??? + commander_.speedj(prev_velocity_cmd_, max_vel_change_ * 125); + } +}; + +static const std::string POSITION_PROGRAM = R"( +def driverProg(): + MULT_jointstate = XXXXX + + SERVO_IDLE = 0 + SERVO_RUNNING = 1 + cmd_servo_state = SERVO_IDLE + cmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + def set_servo_setpoint(q): + enter_critical + cmd_servo_state = SERVO_RUNNING + cmd_servo_q = q + exit_critical + end + thread servoThread(): + state = SERVO_IDLE + while True: + enter_critical + q = cmd_servo_q + do_brake = False + if (state == SERVO_RUNNING) and (cmd_servo_state == SERVO_IDLE): + do_brake = True + end + state = cmd_servo_state + cmd_servo_state = SERVO_IDLE + exit_critical + if do_brake: + stopj(1.0) + sync() + elif state == SERVO_RUNNING: + servoj(q, YYYYYYYY) + else: + sync() + end + end + end + + thread_servo = run servoThread() + keepalive = 1 + while keepalive > 0: + params_mult = socket_read_binary_integer(6+1) + if params_mult[0] > 0: + q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate] + keepalive = params_mult[7] + set_servo_setpoint(q) + end + end + sleep(.1) + socket_close() + kill thread_servo +end +)"; + +class PositionInterface : public HardwareInterface, public hardware_interface::PositionJointInterface +{ +private: + URCommander& commander_; + std::array position_cmd_; + +public: + PositionInterface(URCommander& commander, JointStateInterface& js_interface, std::vector& joint_names) + : commander_(commander) + { + for (size_t i = 0; i < 6; i++) + { + registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &position_cmd_[i])); + } + } + + virtual void write() + { + } +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ros/io_service.h b/include/ur_modern_driver/ros/io_service.h new file mode 100644 index 000000000..be59376fb --- /dev/null +++ b/include/ur_modern_driver/ros/io_service.h @@ -0,0 +1,61 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/ur/commander.h" + +class IOService +{ +private: + ros::NodeHandle nh_; + URCommander &commander_; + ros::ServiceServer io_service_; + ros::ServiceServer payload_service_; + + bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp) + { + LOG_INFO("setIO called with [%d, %d]", req.fun, req.pin); + bool res = false; + bool flag = req.state > 0.0 ? true : false; + switch(req.fun) + { + case ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT: + res = commander_.setDigitalOut(req.pin, flag); + break; + case ur_msgs::SetIO::Request::FUN_SET_ANALOG_OUT: + res = commander_.setAnalogOut(req.pin, req.state); + break; + case ur_msgs::SetIO::Request::FUN_SET_TOOL_VOLTAGE: + res = commander_.setToolVoltage(static_cast(req.state)); + break; + case ur_msgs::SetIO::Request::FUN_SET_FLAG: + res = commander_.setFlag(flag); + break; + } + + return resp.success = res; + } + + bool setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::SetPayloadResponse& resp) + { + LOG_INFO("setPayload called"); + //TODO check min and max payload? + return resp.success = commander_.setPayload(req.payload); + } + + +public: + IOService(URCommander &commander) + : commander_(commander) + , io_service_(nh_.advertiseService("ur_driver/set_io", &IOService::setIO, this)) + , payload_service_(nh_.advertiseService("ur_driver/set_payload", &IOService::setPayload, this)) + { + + } +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ros/mb_publisher.h b/include/ur_modern_driver/ros/mb_publisher.h new file mode 100644 index 000000000..11c4d8b53 --- /dev/null +++ b/include/ur_modern_driver/ros/mb_publisher.h @@ -0,0 +1,55 @@ +#pragma once + +#include +#include +#include +#include + +#include "ur_modern_driver/ur/consumer.h" + +using namespace ros; + +class MBPublisher : public URStatePacketConsumer +{ +private: + NodeHandle nh_; + Publisher io_pub_; + + template + inline void appendDigital(std::vector &vec, std::bitset bits) + { + for(size_t i = 0; i < N; i++) + { + ur_msgs::Digital digi; + digi.pin = static_cast(i); + digi.state = bits.test(i); + vec.push_back(digi); + } + } + + void publish(ur_msgs::IOStates &io_msg, SharedMasterBoardData& data); + +public: + MBPublisher() + : io_pub_(nh_.advertise("ur_driver/io_states", 1)) + { + } + + virtual bool consume(MasterBoardData_V1_X& data); + virtual bool consume(MasterBoardData_V3_0__1& data); + virtual bool consume(MasterBoardData_V3_2& data); + + virtual bool consume(RobotModeData_V1_X& data); + virtual bool consume(RobotModeData_V3_0__1& data); + virtual bool consume(RobotModeData_V3_2& data); + + virtual void setupConsumer() + { + } + virtual void teardownConsumer() + { + } + virtual void stopConsumer() + { + } +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ros/robot_hardware.h b/include/ur_modern_driver/ros/robot_hardware.h new file mode 100644 index 000000000..966443207 --- /dev/null +++ b/include/ur_modern_driver/ros/robot_hardware.h @@ -0,0 +1,46 @@ +#pragma once +#include +#include +#include +#include +#include +#include "ur_modern_driver/ros/hardware_interface.h" + +using hardware_interface::RobotHW; +using hardware_interface::ControllerInfo; + +class RobotHardware : public RobotHW +{ +private: + JointInterface joint_interface_; + WrenchInterface wrench_interface_; + PositionInterface position_interface_; + VelocityInterface velocity_interface_; + hardware_interface::ForceTorqueSensorInterface force_torque_interface_; + HardwareInterface* active_interface_; + std::vector available_interfaces_; + +public: + RobotHardware(URCommander& commander, std::vector& joint_names) + : joint_interface_(joint_names) + , wrench_interface_() + , position_interface_(commander, joint_interface_, joint_names) + , velocity_interface_(commander, joint_interface_, joint_names) + , available_interfaces_{&position_interface_, &velocity_interface_} + { + registerInterface(&joint_interface_); + registerInterface(&wrench_interface_); + registerInterface(&position_interface_); + registerInterface(&velocity_interface_); + } + + //bool canSwitch(const std::list& start_list, const std::list& stop_list) const; + void doSwitch(const std::list& start_list, const std::list& stop_list); + + + /// \brief Read the state from the robot hardware. + virtual void read(RTShared& packet); + + /// \brief write the command to the robot hardware. + virtual void write(); +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ros/ros_controller.h b/include/ur_modern_driver/ros/ros_controller.h new file mode 100644 index 000000000..ad9f2ae48 --- /dev/null +++ b/include/ur_modern_driver/ros/ros_controller.h @@ -0,0 +1,60 @@ +#pragma once +#include +#include +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/pipeline.h" +#include "ur_modern_driver/ur/rt_state.h" +#include "ur_modern_driver/ur/commander.h" +#include "ur_modern_driver/ros/robot_hardware.h" + +class ROSController : public URRTPacketConsumer +{ +private: + ros::NodeHandle nh_; + ros::Time lastUpdate_; + RobotHardware robot_; + controller_manager::ControllerManager controller_; + +public: + ROSController(URCommander& commander, std::vector& joint_names) + : robot_(commander, joint_names) + , controller_(&robot_, nh_) + { + } + + virtual void setupConsumer() + { + lastUpdate_ = ros::Time::now(); + } + + bool handle(RTShared& state) + { + auto time = ros::Time::now(); + auto diff = time - lastUpdate_; + lastUpdate_ = time; + + robot_.read(state); + controller_.update(time, diff); + robot_.write(); + //todo: return result of write + return true; + } + + + virtual bool consume(RTState_V1_6__7& state) + { + return handle(state); + } + virtual bool consume(RTState_V1_8& state) + { + return handle(state); + } + virtual bool consume(RTState_V3_0__1& state) + { + return handle(state); + } + virtual bool consume(RTState_V3_2__3& state) + { + return handle(state); + } +}; \ No newline at end of file diff --git a/src/ros/mb_publisher.cpp b/src/ros/mb_publisher.cpp new file mode 100644 index 000000000..1177be568 --- /dev/null +++ b/src/ros/mb_publisher.cpp @@ -0,0 +1,54 @@ +#include "ur_modern_driver/ros/mb_publisher.h" + +inline void appendAnalog(std::vector &vec, double val, uint8_t pin) +{ + ur_msgs::Analog ana; + ana.pin = pin; + ana.state = val; + vec.push_back(ana); +} + +void MBPublisher::publish(ur_msgs::IOStates &io_msg, SharedMasterBoardData& data) +{ + appendAnalog(io_msg.analog_in_states, data.analog_input0, 0); + appendAnalog(io_msg.analog_in_states, data.analog_input1, 1); + appendAnalog(io_msg.analog_out_states, data.analog_output0, 0); + appendAnalog(io_msg.analog_out_states, data.analog_output1, 1); + + io_pub_.publish(io_msg); +} + +bool MBPublisher::consume(MasterBoardData_V1_X& data) +{ + ur_msgs::IOStates io_msg; + appendDigital(io_msg.digital_in_states, data.digital_input_bits); + appendDigital(io_msg.digital_out_states, data.digital_output_bits); + publish(io_msg, data); + return true; +} +bool MBPublisher::consume(MasterBoardData_V3_0__1& data) +{ + ur_msgs::IOStates io_msg; + appendDigital(io_msg.digital_in_states, data.digital_input_bits); + appendDigital(io_msg.digital_out_states, data.digital_output_bits); + publish(io_msg, data); + return true; +} +bool MBPublisher::consume(MasterBoardData_V3_2& data) +{ + consume(static_cast(data)); + return true; +} + +bool MBPublisher::consume(RobotModeData_V1_X& data) +{ + return true; +} +bool MBPublisher::consume(RobotModeData_V3_0__1& data) +{ + return true; +} +bool MBPublisher::consume(RobotModeData_V3_2& data) +{ + return true; +} \ No newline at end of file diff --git a/src/ros/robot_hardware.cpp b/src/ros/robot_hardware.cpp new file mode 100644 index 000000000..cddcaff61 --- /dev/null +++ b/src/ros/robot_hardware.cpp @@ -0,0 +1,76 @@ +#include "ur_modern_driver/ros/robot_hardware.h" + +/* +bool RobotHardware::canSwitch(const std::list& start_list, + const std::list& stop_list) const +{ + + bool running = active_interface_ != nullptr; + size_t start_size = start_list.size(); + size_t stop_size = stop_list.size(); + + + for (auto const& ci : stop_list) + { + auto it = interfaces_.find(ci.hardware_interface); + if(it == interfaces_.end() || it->second != active_interface_) + return false; + } + + for (auto const& ci : start_list) + { + auto it = interfaces_.find(ci.hardware_interface); + //we can only start a controller that's already running if we stop it first + if(it == interfaces_.end() || (it->second == active_interface_ && stop_size == 0)) + return false; + } + + return true; +} +*/ + +void RobotHardware::doSwitch(const std::list& start_list, + const std::list& stop_list) +{ + if(active_interface_ != nullptr && stop_list.size() > 0) + { + active_interface_->stop(); + active_interface_ = nullptr; + } + + for(auto const& ci : start_list) + { + auto it = interfaces_.find(ci.hardware_interface); + if(it == interfaces_.end()) + continue; + + //we can only switch to one of the available interfaces + if(std::find(available_interfaces_.begin(), available_interfaces_.end(), it->second) == available_interfaces_.end()) + continue; + + auto new_interface = static_cast(it->second); + + if(new_interface == nullptr) + continue; + + LOG_INFO("Starting %s", ci.hardware_interface.c_str()); + active_interface_ = new_interface; + new_interface->start(); + + return; + } +} + +void RobotHardware::write() +{ + if(active_interface_ == nullptr) + return; + + active_interface_->write(); +} + +void RobotHardware::read(RTShared& packet) +{ + joint_interface_.update(packet); + wrench_interface_.update(packet); +} \ No newline at end of file From dd8169d37186f9dfd818749296a8850a7ae426d3 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Thu, 13 Apr 2017 10:48:59 +0200 Subject: [PATCH 043/114] Factory and setup improvements --- CMakeLists.txt | 5 +- include/ur_modern_driver/event_counter.h | 86 ++++++++++++++++++++++++ include/ur_modern_driver/ur/factory.h | 4 +- src/ros_main.cpp | 64 +++++++++++++++--- 4 files changed, 145 insertions(+), 14 deletions(-) create mode 100644 include/ur_modern_driver/event_counter.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 7f1badfae..944df13e9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -159,13 +159,14 @@ target_link_libraries(ur_hardware_interface ## Declare a C++ executable set(${PROJECT_NAME}_SOURCES src/ros/rt_publisher.cpp + src/ros/mb_publisher.cpp + src/ros/robot_hardware.cpp src/ur/stream.cpp + src/ur/commander.cpp src/ur/robot_mode.cpp src/ur/master_board.cpp src/ur/rt_state.cpp src/ur/messages.cpp - #src/ros_main.cpp - #src/ur_ros_wrapper.cpp src/ur_driver.cpp src/ur_realtime_communication.cpp src/ur_communication.cpp diff --git a/include/ur_modern_driver/event_counter.h b/include/ur_modern_driver/event_counter.h new file mode 100644 index 000000000..2c8ff314b --- /dev/null +++ b/include/ur_modern_driver/event_counter.h @@ -0,0 +1,86 @@ +#pragma once + +#include +#include +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/ur/consumer.h" + + +class EventCounter : public URRTPacketConsumer +{ +private: + typedef std::chrono::high_resolution_clock Clock; + Clock::time_point events_[250]; + size_t idx_ = 0; + + + Clock::time_point last_; + +public: + void trigger() + { + //auto now = Clock::now(); + //LOG_INFO("Time diff: %d ms", std::chrono::duration_cast(now - last_)); + //last_ = now; + //return; + + events_[idx_] = Clock::now(); + idx_ += 1; + + if(idx_ > 250) + { + std::chrono::time_point t_min = + std::chrono::time_point::max(); + std::chrono::time_point t_max = + std::chrono::time_point::min(); + + for(auto const& e : events_) + { + if(e < t_min) + t_min = e; + if(e > t_max) + t_max = e; + } + + auto diff = t_max - t_min; + auto secs = std::chrono::duration_cast(diff).count(); + auto ms = std::chrono::duration_cast(diff).count(); + std::chrono::duration test(t_max - t_min); + LOG_INFO("Recieved 250 messages at %f Hz", (250.0/test.count())); + idx_ = 0; + } + } + +public: + bool consume(RTState_V1_6__7& state) + { + trigger(); + return true; + } + bool consume(RTState_V1_8& state) + { + trigger(); + return true; + } + bool consume(RTState_V3_0__1& state) + { + trigger(); + return true; + } + bool consume(RTState_V3_2__3& state) + { + trigger(); + return true; + } + + void setupConsumer() + { + last_ = Clock::now(); + } + void teardownConsumer() + { + } + void stopConsumer() + { + } +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/factory.h b/include/ur_modern_driver/ur/factory.h index 83d89abb5..83f0cd6ff 100644 --- a/include/ur_modern_driver/ur/factory.h +++ b/include/ur_modern_driver/ur/factory.h @@ -9,6 +9,8 @@ #include "ur_modern_driver/ur/state_parser.h" #include "ur_modern_driver/ur/stream.h" +static const int UR_PRIMARY_PORT = 30001; + class URFactory : private URMessagePacketConsumer { private: @@ -42,7 +44,7 @@ class URFactory : private URMessagePacketConsumer } public: - URFactory(std::string& host) : stream_(host, 30001) + URFactory(std::string& host) : stream_(host, UR_PRIMARY_PORT) { URProducer prod(stream_, parser_); std::vector> results; diff --git a/src/ros_main.cpp b/src/ros_main.cpp index 09b142a7a..1c660dd4e 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -7,6 +7,10 @@ #include "ur_modern_driver/log.h" #include "ur_modern_driver/pipeline.h" #include "ur_modern_driver/ros/rt_publisher.h" +#include "ur_modern_driver/ros/mb_publisher.h" +#include "ur_modern_driver/ros/io_service.h" +#include "ur_modern_driver/ros/ros_controller.h" +#include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/factory.h" #include "ur_modern_driver/ur/messages.h" #include "ur_modern_driver/ur/parser.h" @@ -17,9 +21,14 @@ static const std::string IP_ADDR_ARG("~robot_ip_address"); static const std::string REVERSE_PORT_ARG("~reverse_port"); static const std::string SIM_TIME_ARG("~use_sim_time"); +static const std::string ROS_CONTROL_ARG("~use_ros_control"); 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"); +static const std::string JOINT_NAMES_PARAM("hardware_interface/joints"); + +static const int UR_SECONDARY_PORT = 30002; +static const int UR_RT_PORT = 30003; struct ProgArgs { @@ -28,8 +37,12 @@ struct ProgArgs std::string prefix; std::string base_frame; std::string tool_frame; + std::vector joint_names; + double max_acceleration; + double max_velocity; int32_t reverse_port; bool use_sim_time; + bool use_ros_control; }; bool parse_args(ProgArgs& args) @@ -41,12 +54,16 @@ bool parse_args(ProgArgs& args) } ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001)); ros::param::param(SIM_TIME_ARG, args.use_sim_time, false); + ros::param::param(ROS_CONTROL_ARG, args.use_ros_control, false); ros::param::param(PREFIX_ARG, args.prefix, std::string()); ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link"); ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller"); + ros::param::get(JOINT_NAMES_PARAM, args.joint_names); return true; } +#include "ur_modern_driver/event_counter.h" + int main(int argc, char** argv) { ros::init(argc, argv, "ur_driver"); @@ -58,22 +75,47 @@ int main(int argc, char** argv) } URFactory factory(args.host); - auto parser = factory.getRTParser(); - - URStream s2(args.host, 30003); - URProducer p2(s2, *parser); - RTPublisher pub(args.prefix, args.base_frame, args.tool_frame); + //RT packets + auto rt_parser = factory.getRTParser(); + URStream rt_stream(args.host, UR_RT_PORT); + URProducer rt_prod(rt_stream, *rt_parser); + RTPublisher rt_pub(args.prefix, args.base_frame, args.tool_frame); + EventCounter rt_ec; - Pipeline pl(p2, pub); + URCommander rt_commander(rt_stream); + vector*> rt_vec; - pl.run(); - - while (ros::ok()) + if(args.use_ros_control) { - std::this_thread::sleep_for(std::chrono::milliseconds(33)); + rt_vec.push_back(new ROSController(rt_commander, args.joint_names)); } - pl.stop(); + //rt_vec.push_back(&rt_pub); + + MultiConsumer rt_cons(rt_vec); + Pipeline rt_pl(rt_prod, rt_cons); + + //Message packets + auto state_parser = factory.getStateParser(); + URStream state_stream(args.host, UR_SECONDARY_PORT); + URProducer state_prod(state_stream, *state_parser); + MBPublisher state_pub; + vector*> state_vec{&state_pub}; + MultiConsumer state_cons(state_vec); + Pipeline state_pl(state_prod, state_cons); + + LOG_INFO("Starting main loop"); + + rt_pl.run(); + state_pl.run(); + + URCommander state_commander(state_stream); + IOService service(state_commander); + + ros::spin(); + + rt_pl.stop(); + state_pl.stop(); return EXIT_SUCCESS; } \ No newline at end of file From b4bb424058afd5f55ccd39c2cf773ab65b45b8d3 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Sat, 15 Apr 2017 01:45:29 +0200 Subject: [PATCH 044/114] Fixed minor issues --- include/ur_modern_driver/ros/converter.h | 1 - .../ur_modern_driver/ros/hardware_interface.h | 13 +++++--- include/ur_modern_driver/ros/robot_hardware.h | 21 ++++++++---- include/ur_modern_driver/ros/rt_publisher.h | 4 ++- src/ros/robot_hardware.cpp | 32 ++++++++----------- src/ros/rt_publisher.cpp | 10 ++++-- src/ros_main.cpp | 23 ++++++------- 7 files changed, 58 insertions(+), 46 deletions(-) delete mode 100644 include/ur_modern_driver/ros/converter.h diff --git a/include/ur_modern_driver/ros/converter.h b/include/ur_modern_driver/ros/converter.h deleted file mode 100644 index 6f70f09be..000000000 --- a/include/ur_modern_driver/ros/converter.h +++ /dev/null @@ -1 +0,0 @@ -#pragma once diff --git a/include/ur_modern_driver/ros/hardware_interface.h b/include/ur_modern_driver/ros/hardware_interface.h index e970e301e..dfb36c71e 100644 --- a/include/ur_modern_driver/ros/hardware_interface.h +++ b/include/ur_modern_driver/ros/hardware_interface.h @@ -48,10 +48,11 @@ class JointInterface : public JointStateInterface class WrenchInterface : public hardware_interface::ForceTorqueSensorInterface { std::array tcp_; + public: - WrenchInterface() + WrenchInterface() { - registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", "", tcp_.begin(), tcp_.begin()+3)); + registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", "", tcp_.begin(), tcp_.begin() + 3)); } void update(RTShared& packet) @@ -65,7 +66,7 @@ class VelocityInterface : public HardwareInterface, public hardware_interface::V private: URCommander& commander_; std::array velocity_cmd_, prev_velocity_cmd_; - double max_vel_change_; + double max_vel_change_ = 0.12 / 125; public: VelocityInterface(URCommander& commander, JointStateInterface& js_interface, std::vector& joint_names) @@ -88,9 +89,11 @@ class VelocityInterface : public HardwareInterface, public hardware_interface::V prev_velocity_cmd_[i] = std::max(lo, std::min(velocity_cmd_[i], hi)); } - //times 125??? + // times 125??? commander_.speedj(prev_velocity_cmd_, max_vel_change_ * 125); } + + typedef hardware_interface::VelocityJointInterface parent_type; }; static const std::string POSITION_PROGRAM = R"( @@ -165,4 +168,6 @@ class PositionInterface : public HardwareInterface, public hardware_interface::P virtual void write() { } + + typedef hardware_interface::PositionJointInterface parent_type; }; \ No newline at end of file diff --git a/include/ur_modern_driver/ros/robot_hardware.h b/include/ur_modern_driver/ros/robot_hardware.h index 966443207..5e7c566d9 100644 --- a/include/ur_modern_driver/ros/robot_hardware.h +++ b/include/ur_modern_driver/ros/robot_hardware.h @@ -1,6 +1,7 @@ #pragma once #include #include +#include #include #include #include @@ -16,9 +17,16 @@ class RobotHardware : public RobotHW WrenchInterface wrench_interface_; PositionInterface position_interface_; VelocityInterface velocity_interface_; - hardware_interface::ForceTorqueSensorInterface force_torque_interface_; + HardwareInterface* active_interface_; - std::vector available_interfaces_; + std::map available_interfaces_; + + template + void registerHardwareInterface(T* interface) + { + registerInterface(interface); + available_interfaces_[hardware_interface::internal::demangledTypeName()] = interface; + } public: RobotHardware(URCommander& commander, std::vector& joint_names) @@ -26,18 +34,17 @@ class RobotHardware : public RobotHW , wrench_interface_() , position_interface_(commander, joint_interface_, joint_names) , velocity_interface_(commander, joint_interface_, joint_names) - , available_interfaces_{&position_interface_, &velocity_interface_} { registerInterface(&joint_interface_); registerInterface(&wrench_interface_); - registerInterface(&position_interface_); - registerInterface(&velocity_interface_); + + registerHardwareInterface(&position_interface_); + registerHardwareInterface(&velocity_interface_); } - //bool canSwitch(const std::list& start_list, const std::list& stop_list) const; + // bool canSwitch(const std::list& start_list, const std::list& stop_list) const; void doSwitch(const std::list& start_list, const std::list& stop_list); - /// \brief Read the state from the robot hardware. virtual void read(RTShared& packet); diff --git a/include/ur_modern_driver/ros/rt_publisher.h b/include/ur_modern_driver/ros/rt_publisher.h index a3fbb786d..f4945d9ab 100644 --- a/include/ur_modern_driver/ros/rt_publisher.h +++ b/include/ur_modern_driver/ros/rt_publisher.h @@ -31,6 +31,7 @@ class RTPublisher : public URRTPacketConsumer std::vector joint_names_; std::string base_frame_; std::string tool_frame_; + bool temp_only_; bool publishJoints(RTShared& packet, Time& t); bool publishWrench(RTShared& packet, Time& t); @@ -41,13 +42,14 @@ class RTPublisher : public URRTPacketConsumer bool publish(RTShared& packet); public: - RTPublisher(std::string& joint_prefix, std::string& base_frame, std::string& tool_frame) + RTPublisher(std::string& joint_prefix, std::string& base_frame, std::string& tool_frame, bool temp_only = false) : joint_pub_(nh_.advertise("joint_states", 1)) , wrench_pub_(nh_.advertise("wrench", 1)) , tool_vel_pub_(nh_.advertise("tool_velocity", 1)) , joint_temperature_pub_(nh_.advertise("joint_temperature", 1)) , base_frame_(base_frame) , tool_frame_(tool_frame) + , temp_only_(temp_only) { for (auto const& j : JOINTS) { diff --git a/src/ros/robot_hardware.cpp b/src/ros/robot_hardware.cpp index cddcaff61..fce866db1 100644 --- a/src/ros/robot_hardware.cpp +++ b/src/ros/robot_hardware.cpp @@ -16,7 +16,7 @@ bool RobotHardware::canSwitch(const std::list& start_list, if(it == interfaces_.end() || it->second != active_interface_) return false; } - + for (auto const& ci : start_list) { auto it = interfaces_.find(ci.hardware_interface); @@ -29,43 +29,39 @@ bool RobotHardware::canSwitch(const std::list& start_list, } */ -void RobotHardware::doSwitch(const std::list& start_list, - const std::list& stop_list) +void RobotHardware::doSwitch(const std::list& start_list, const std::list& stop_list) { - if(active_interface_ != nullptr && stop_list.size() > 0) + if (active_interface_ != nullptr && stop_list.size() > 0) { + LOG_INFO("Stopping active interface"); active_interface_->stop(); active_interface_ = nullptr; } - for(auto const& ci : start_list) + for (auto const& ci : start_list) { - auto it = interfaces_.find(ci.hardware_interface); - if(it == interfaces_.end()) - continue; - - //we can only switch to one of the available interfaces - if(std::find(available_interfaces_.begin(), available_interfaces_.end(), it->second) == available_interfaces_.end()) + auto ait = available_interfaces_.find(ci.hardware_interface); + + if (ait == available_interfaces_.end()) continue; - auto new_interface = static_cast(it->second); + auto new_interface = ait->second; - if(new_interface == nullptr) - continue; - LOG_INFO("Starting %s", ci.hardware_interface.c_str()); active_interface_ = new_interface; new_interface->start(); - + return; } + + LOG_WARN("Failed to start interface!"); } void RobotHardware::write() { - if(active_interface_ == nullptr) + if (active_interface_ == nullptr) return; - + active_interface_->write(); } diff --git a/src/ros/rt_publisher.cpp b/src/ros/rt_publisher.cpp index f9ab2ab2f..65d07266e 100644 --- a/src/ros/rt_publisher.cpp +++ b/src/ros/rt_publisher.cpp @@ -91,8 +91,14 @@ bool RTPublisher::publishTemperature(RTShared& packet, Time& t) bool RTPublisher::publish(RTShared& packet) { Time time = Time::now(); - return publishJoints(packet, time) && publishWrench(packet, time) && publishTool(packet, time) && - publishTransform(packet, time) && publishTemperature(packet, time); + bool res = true; + if (!temp_only_) + { + res = publishJoints(packet, time) && publishWrench(packet, time) && publishTool(packet, time) && + publishTransform(packet, time); + } + + return res && publishTemperature(packet, time); } bool RTPublisher::consume(RTState_V1_6__7& state) diff --git a/src/ros_main.cpp b/src/ros_main.cpp index 1c660dd4e..b08e90b78 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -6,10 +6,10 @@ #include "ur_modern_driver/log.h" #include "ur_modern_driver/pipeline.h" -#include "ur_modern_driver/ros/rt_publisher.h" -#include "ur_modern_driver/ros/mb_publisher.h" #include "ur_modern_driver/ros/io_service.h" +#include "ur_modern_driver/ros/mb_publisher.h" #include "ur_modern_driver/ros/ros_controller.h" +#include "ur_modern_driver/ros/rt_publisher.h" #include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/factory.h" #include "ur_modern_driver/ur/messages.h" @@ -58,7 +58,7 @@ bool parse_args(ProgArgs& args) ros::param::param(PREFIX_ARG, args.prefix, std::string()); ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link"); ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller"); - ros::param::get(JOINT_NAMES_PARAM, args.joint_names); + ros::param::get(JOINT_NAMES_PARAM, args.joint_names); return true; } @@ -75,32 +75,29 @@ int main(int argc, char** argv) } URFactory factory(args.host); - //RT packets + // RT packets auto rt_parser = factory.getRTParser(); URStream rt_stream(args.host, UR_RT_PORT); URProducer rt_prod(rt_stream, *rt_parser); - RTPublisher rt_pub(args.prefix, args.base_frame, args.tool_frame); - EventCounter rt_ec; - + RTPublisher rt_pub(args.prefix, args.base_frame, args.tool_frame, args.use_ros_control); URCommander rt_commander(rt_stream); - vector*> rt_vec; + vector*> rt_vec{ &rt_pub }; - if(args.use_ros_control) + if (args.use_ros_control) { + LOG_INFO("ROS control enabled"); rt_vec.push_back(new ROSController(rt_commander, args.joint_names)); } - //rt_vec.push_back(&rt_pub); - MultiConsumer rt_cons(rt_vec); Pipeline rt_pl(rt_prod, rt_cons); - //Message packets + // Message packets auto state_parser = factory.getStateParser(); URStream state_stream(args.host, UR_SECONDARY_PORT); URProducer state_prod(state_stream, *state_parser); MBPublisher state_pub; - vector*> state_vec{&state_pub}; + vector*> state_vec{ &state_pub }; MultiConsumer state_cons(state_vec); Pipeline state_pl(state_prod, state_cons); From 46f4e493cfe99d9ea9ca87541c59fb543e1eb1a5 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Mon, 17 Apr 2017 13:03:54 +0200 Subject: [PATCH 045/114] Big code dump --- CMakeLists.txt | 9 +- include/ur_modern_driver/ros/action_server.h | 69 ++++++ include/ur_modern_driver/ros/controller.h | 80 ++++++ .../ur_modern_driver/ros/hardware_interface.h | 144 ++--------- include/ur_modern_driver/ros/io_service.h | 6 +- include/ur_modern_driver/ros/robot_hardware.h | 4 +- include/ur_modern_driver/ros/ros_controller.h | 60 ----- .../ur_modern_driver/ros/service_stopper.h | 52 ++++ .../ros/trajectory_follower.h | 44 ++++ include/ur_modern_driver/ur/commander.h | 3 +- include/ur_modern_driver/ur/robot_mode.h | 3 +- include/ur_modern_driver/ur/server.h | 19 ++ include/ur_modern_driver/ur/stream.h | 23 ++ src/ros/action_server.cpp | 231 ++++++++++++++++++ src/ros/controller.cpp | 84 +++++++ src/ros/hardware_interface.cpp | 74 ++++++ src/ros/robot_hardware.cpp | 42 ---- src/ros/service_stopper.cpp | 47 ++++ src/ros/trajectory_follower.cpp | 137 +++++++++++ src/ros_main.cpp | 37 ++- src/ur/commander.cpp | 16 +- src/ur/robot_mode.cpp | 9 +- src/ur/server.cpp | 51 ++++ tests/ur/robot_mode.cpp | 2 +- 24 files changed, 989 insertions(+), 257 deletions(-) create mode 100644 include/ur_modern_driver/ros/action_server.h create mode 100644 include/ur_modern_driver/ros/controller.h delete mode 100644 include/ur_modern_driver/ros/ros_controller.h create mode 100644 include/ur_modern_driver/ros/service_stopper.h create mode 100644 include/ur_modern_driver/ros/trajectory_follower.h create mode 100644 include/ur_modern_driver/ur/server.h create mode 100644 src/ros/action_server.cpp create mode 100644 src/ros/controller.cpp create mode 100644 src/ros/hardware_interface.cpp create mode 100644 src/ros/service_stopper.cpp create mode 100644 src/ros/trajectory_follower.cpp create mode 100644 src/ur/server.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 944df13e9..1de272572 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -158,10 +158,15 @@ target_link_libraries(ur_hardware_interface ## Declare a C++ executable set(${PROJECT_NAME}_SOURCES - src/ros/rt_publisher.cpp + src/ros/action_server.cpp + src/ros/controller.cpp + src/ros/hardware_interface.cpp src/ros/mb_publisher.cpp - src/ros/robot_hardware.cpp + src/ros/rt_publisher.cpp + src/ros/service_stopper.cpp + src/ros/trajectory_follower.cpp src/ur/stream.cpp + src/ur/server.cpp src/ur/commander.cpp src/ur/robot_mode.cpp src/ur/master_board.cpp diff --git a/include/ur_modern_driver/ros/action_server.h b/include/ur_modern_driver/ros/action_server.h new file mode 100644 index 000000000..dd11de40f --- /dev/null +++ b/include/ur_modern_driver/ros/action_server.h @@ -0,0 +1,69 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/ur/consumer.h" +#include "ur_modern_driver/ur/master_board.h" +#include "ur_modern_driver/ur/state.h" +#include "ur_modern_driver/ros/service_stopper.h" +#include "ur_modern_driver/ros/trajectory_follower.h" + + +class ActionServer : public URRTPacketConsumer, public Service +{ +private: + typedef control_msgs::FollowJointTrajectoryAction Action; + typedef control_msgs::FollowJointTrajectoryResult Result; + typedef actionlib::ServerGoalHandle GoalHandle; + typedef actionlib::ActionServer Server; + + ros::NodeHandle nh_; + Server as_; + + std::vector joint_names_; + std::set joint_set_; + double max_velocity_; + RobotState state_; + + + GoalHandle curr_gh_; + std::atomic has_goal_, running_; + std::mutex tj_mutex_; + std::condition_variable tj_cv_; + + TrajectoryFollower& follower_; + + void onGoal(GoalHandle gh); + void onCancel(GoalHandle gh); + + bool validate(GoalHandle& gh, Result& res); + bool validateState(GoalHandle& gh, Result& res); + bool validateJoints(GoalHandle& gh, Result& res); + bool validateTrajectory(GoalHandle& gh, Result& res); + + bool try_execute(GoalHandle& gh, Result& res); + + std::vector reorderMap(std::vector goal_joints); + double interp_cubic(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel); + + void trajectoryThread(); + + template + double toSec(U const& u) + { + return std::chrono::duration_cast>(u).count(); + } + +public: + ActionServer(TrajectoryFollower& follower, std::vector& joint_names, double max_velocity); + + virtual void onRobotStateChange(RobotState state); +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ros/controller.h b/include/ur_modern_driver/ros/controller.h new file mode 100644 index 000000000..eb940d079 --- /dev/null +++ b/include/ur_modern_driver/ros/controller.h @@ -0,0 +1,80 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/ur/commander.h" +#include "ur_modern_driver/ur/consumer.h" +#include "ur_modern_driver/ur/rt_state.h" +#include "ur_modern_driver/ros/hardware_interface.h" +#include "ur_modern_driver/ros/service_stopper.h" + +class ROSController : private hardware_interface::RobotHW, public URRTPacketConsumer, public Service +{ +private: + ros::NodeHandle nh_; + ros::Time lastUpdate_; + controller_manager::ControllerManager controller_; + + // state interfaces + JointInterface joint_interface_; + WrenchInterface wrench_interface_; + // controller interfaces + PositionInterface position_interface_; + VelocityInterface velocity_interface_; + + // currently activated controller + HardwareInterface* active_interface_; + // map of switchable controllers + std::map available_interfaces_; + + std::atomic service_enabled_; + + // helper functions to map interfaces + template + void registerInterface(T* interface) + { + RobotHW::registerInterface(interface); + } + template + void registerControllereInterface(T* interface) + { + registerInterface(interface); + available_interfaces_[hardware_interface::internal::demangledTypeName()] = interface; + } + + void read(RTShared& state); + bool update(RTShared& state); + bool write(); + +public: + ROSController(URCommander& commander, std::vector& joint_names, double max_vel_change); + virtual ~ROSController() { } + // from RobotHW + void doSwitch(const std::list& start_list, const std::list& stop_list); + // from URRTPacketConsumer + virtual void setupConsumer(); + virtual bool consume(RTState_V1_6__7& state) + { + return update(state); + } + virtual bool consume(RTState_V1_8& state) + { + return update(state); + } + virtual bool consume(RTState_V3_0__1& state) + { + return update(state); + } + virtual bool consume(RTState_V3_2__3& state) + { + return update(state); + } + + virtual void onRobotStateChange(RobotState state); +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ros/hardware_interface.h b/include/ur_modern_driver/ros/hardware_interface.h index dfb36c71e..0c6618d53 100644 --- a/include/ur_modern_driver/ros/hardware_interface.h +++ b/include/ur_modern_driver/ros/hardware_interface.h @@ -10,39 +10,22 @@ class HardwareInterface { public: - virtual void write() = 0; - virtual void start() - { - } - virtual void stop() - { - } + virtual bool write() = 0; + virtual void start() {} + virtual void stop() {} }; using hardware_interface::JointHandle; -using hardware_interface::JointStateHandle; -using hardware_interface::JointStateInterface; -class JointInterface : public JointStateInterface +class JointInterface : public hardware_interface::JointStateInterface { private: std::array velocities_, positions_, efforts_; public: - JointInterface(std::vector& joint_names) - { - for (size_t i = 0; i < 6; i++) - { - registerHandle(JointStateHandle(joint_names[i], &positions_[i], &velocities_[i], &efforts_[i])); - } - } - - void update(RTShared& packet) - { - positions_ = packet.q_actual; - velocities_ = packet.qd_actual; - efforts_ = packet.i_actual; - } + JointInterface(std::vector &joint_names); + void update(RTShared &packet); + typedef hardware_interface::JointStateInterface parent_type; }; class WrenchInterface : public hardware_interface::ForceTorqueSensorInterface @@ -50,124 +33,35 @@ class WrenchInterface : public hardware_interface::ForceTorqueSensorInterface std::array tcp_; public: - WrenchInterface() - { - registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", "", tcp_.begin(), tcp_.begin() + 3)); - } - - void update(RTShared& packet) - { - tcp_ = packet.tcp_force; - } + WrenchInterface(); + void update(RTShared &packet); + typedef hardware_interface::ForceTorqueSensorInterface parent_type; }; class VelocityInterface : public HardwareInterface, public hardware_interface::VelocityJointInterface { private: - URCommander& commander_; + URCommander &commander_; std::array velocity_cmd_, prev_velocity_cmd_; - double max_vel_change_ = 0.12 / 125; + double max_vel_change_; public: - VelocityInterface(URCommander& commander, JointStateInterface& js_interface, std::vector& joint_names) - : commander_(commander) - { - for (size_t i = 0; i < 6; i++) - { - registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &velocity_cmd_[i])); - } - } - - virtual void write() - { - for (size_t i = 0; i < 6; i++) - { - double prev = prev_velocity_cmd_[i]; - double lo = prev - max_vel_change_; - double hi = prev + max_vel_change_; - // clamp value to ±max_vel_change - prev_velocity_cmd_[i] = std::max(lo, std::min(velocity_cmd_[i], hi)); - } - - // times 125??? - commander_.speedj(prev_velocity_cmd_, max_vel_change_ * 125); - } - + VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names, double max_vel_change); + virtual bool write(); typedef hardware_interface::VelocityJointInterface parent_type; }; -static const std::string POSITION_PROGRAM = R"( -def driverProg(): - MULT_jointstate = XXXXX - - SERVO_IDLE = 0 - SERVO_RUNNING = 1 - cmd_servo_state = SERVO_IDLE - cmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - def set_servo_setpoint(q): - enter_critical - cmd_servo_state = SERVO_RUNNING - cmd_servo_q = q - exit_critical - end - thread servoThread(): - state = SERVO_IDLE - while True: - enter_critical - q = cmd_servo_q - do_brake = False - if (state == SERVO_RUNNING) and (cmd_servo_state == SERVO_IDLE): - do_brake = True - end - state = cmd_servo_state - cmd_servo_state = SERVO_IDLE - exit_critical - if do_brake: - stopj(1.0) - sync() - elif state == SERVO_RUNNING: - servoj(q, YYYYYYYY) - else: - sync() - end - end - end - - thread_servo = run servoThread() - keepalive = 1 - while keepalive > 0: - params_mult = socket_read_binary_integer(6+1) - if params_mult[0] > 0: - q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate] - keepalive = params_mult[7] - set_servo_setpoint(q) - end - end - sleep(.1) - socket_close() - kill thread_servo -end -)"; - class PositionInterface : public HardwareInterface, public hardware_interface::PositionJointInterface { private: - URCommander& commander_; + URCommander &commander_; std::array position_cmd_; public: - PositionInterface(URCommander& commander, JointStateInterface& js_interface, std::vector& joint_names) - : commander_(commander) - { - for (size_t i = 0; i < 6; i++) - { - registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &position_cmd_[i])); - } - } - - virtual void write() - { - } + PositionInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names); + virtual bool write(); + virtual void start(); + virtual void stop(); typedef hardware_interface::PositionJointInterface parent_type; }; \ No newline at end of file diff --git a/include/ur_modern_driver/ros/io_service.h b/include/ur_modern_driver/ros/io_service.h index be59376fb..36ce662de 100644 --- a/include/ur_modern_driver/ros/io_service.h +++ b/include/ur_modern_driver/ros/io_service.h @@ -35,18 +35,18 @@ class IOService res = commander_.setToolVoltage(static_cast(req.state)); break; case ur_msgs::SetIO::Request::FUN_SET_FLAG: - res = commander_.setFlag(flag); + res = commander_.setFlag(req.pin, flag); break; } - return resp.success = res; + return (resp.success = res); } bool setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::SetPayloadResponse& resp) { LOG_INFO("setPayload called"); //TODO check min and max payload? - return resp.success = commander_.setPayload(req.payload); + return (resp.success = commander_.setPayload(req.payload)); } diff --git a/include/ur_modern_driver/ros/robot_hardware.h b/include/ur_modern_driver/ros/robot_hardware.h index 5e7c566d9..47ef455bb 100644 --- a/include/ur_modern_driver/ros/robot_hardware.h +++ b/include/ur_modern_driver/ros/robot_hardware.h @@ -29,11 +29,11 @@ class RobotHardware : public RobotHW } public: - RobotHardware(URCommander& commander, std::vector& joint_names) + RobotHardware(URCommander& commander, std::vector& joint_names, double max_vel_change) : joint_interface_(joint_names) , wrench_interface_() , position_interface_(commander, joint_interface_, joint_names) - , velocity_interface_(commander, joint_interface_, joint_names) + , velocity_interface_(commander, joint_interface_, joint_names, max_vel_change) { registerInterface(&joint_interface_); registerInterface(&wrench_interface_); diff --git a/include/ur_modern_driver/ros/ros_controller.h b/include/ur_modern_driver/ros/ros_controller.h deleted file mode 100644 index ad9f2ae48..000000000 --- a/include/ur_modern_driver/ros/ros_controller.h +++ /dev/null @@ -1,60 +0,0 @@ -#pragma once -#include -#include -#include "ur_modern_driver/log.h" -#include "ur_modern_driver/pipeline.h" -#include "ur_modern_driver/ur/rt_state.h" -#include "ur_modern_driver/ur/commander.h" -#include "ur_modern_driver/ros/robot_hardware.h" - -class ROSController : public URRTPacketConsumer -{ -private: - ros::NodeHandle nh_; - ros::Time lastUpdate_; - RobotHardware robot_; - controller_manager::ControllerManager controller_; - -public: - ROSController(URCommander& commander, std::vector& joint_names) - : robot_(commander, joint_names) - , controller_(&robot_, nh_) - { - } - - virtual void setupConsumer() - { - lastUpdate_ = ros::Time::now(); - } - - bool handle(RTShared& state) - { - auto time = ros::Time::now(); - auto diff = time - lastUpdate_; - lastUpdate_ = time; - - robot_.read(state); - controller_.update(time, diff); - robot_.write(); - //todo: return result of write - return true; - } - - - virtual bool consume(RTState_V1_6__7& state) - { - return handle(state); - } - virtual bool consume(RTState_V1_8& state) - { - return handle(state); - } - virtual bool consume(RTState_V3_0__1& state) - { - return handle(state); - } - virtual bool consume(RTState_V3_2__3& state) - { - return handle(state); - } -}; \ No newline at end of file diff --git a/include/ur_modern_driver/ros/service_stopper.h b/include/ur_modern_driver/ros/service_stopper.h new file mode 100644 index 000000000..af09dfa6e --- /dev/null +++ b/include/ur_modern_driver/ros/service_stopper.h @@ -0,0 +1,52 @@ +#pragma once +#include +#include +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/ur/consumer.h" + +enum class RobotState +{ + Running, + Error, + EmergencyStopped, + ProtectiveStopped +}; + +class Service +{ +public: + virtual void onRobotStateChange(RobotState state) = 0; +}; + +class ServiceStopper : public URStatePacketConsumer { +private: + ros::NodeHandle nh_; + ros::ServiceServer enable_service_; + std::vector services_; + RobotState last_state_; + + void notify_all(RobotState state); + bool handle(SharedRobotModeData &data, bool error); + bool enableCallback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp); + +public: + ServiceStopper(std::vector services); + + virtual bool consume(RobotModeData_V1_X& data) + { + return handle(data, data.robot_mode != robot_mode_V1_X::ROBOT_RUNNING_MODE); + } + virtual bool consume(RobotModeData_V3_0__1& data) + { + return handle(data, data.robot_mode != robot_mode_V3_X::RUNNING); + } + virtual bool consume(RobotModeData_V3_2& data) + { + return handle(data, data.robot_mode != robot_mode_V3_X::RUNNING); + } + + //unused + virtual bool consume(MasterBoardData_V1_X& data) { return true; } + virtual bool consume(MasterBoardData_V3_0__1& data) { return true; } + virtual bool consume(MasterBoardData_V3_2& data) { return true; } +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ros/trajectory_follower.h b/include/ur_modern_driver/ros/trajectory_follower.h new file mode 100644 index 000000000..a1df97f06 --- /dev/null +++ b/include/ur_modern_driver/ros/trajectory_follower.h @@ -0,0 +1,44 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include "ur_modern_driver/ur/commander.h" +#include "ur_modern_driver/ur/server.h" +#include "ur_modern_driver/ur/stream.h" + +class TrajectoryFollower +{ +private: + const int32_t MULT_JOINTSTATE_ = 1000000; + double servoj_time_, servoj_lookahead_time_, servoj_gain_; + std::atomic running_; + std::array last_positions_; + URCommander &commander_; + URServer server_; + URStream stream_; + std::string program_; + + template + size_t append(uint8_t *buffer, T &val) + { + size_t s = sizeof(T); + std::memcpy(buffer, &val, s); + return s; + } + + bool execute(std::array &positions, bool keep_alive); + +public: + TrajectoryFollower(URCommander &commander, int reverse_port, bool version_3); + + std::string buildProgram(bool version_3); + + bool start(); + bool execute(std::array &positions); + void stop(); + void halt(); //maybe +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/commander.h b/include/ur_modern_driver/ur/commander.h index 6325f8c62..da92ae86c 100644 --- a/include/ur_modern_driver/ur/commander.h +++ b/include/ur_modern_driver/ur/commander.h @@ -17,11 +17,12 @@ class URCommander { } + virtual bool uploadProg(std::string &s); virtual bool speedj(std::array &speeds, double acceleration); virtual bool stopj(double a = 10.0); virtual bool setDigitalOut(uint8_t pin, bool value); virtual bool setAnalogOut(uint8_t pin, double value); virtual bool setToolVoltage(uint8_t voltage); - virtual bool setFlag(bool value); + virtual bool setFlag(uint8_t pin, bool value); virtual bool setPayload(double value); }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/robot_mode.h b/include/ur_modern_driver/ur/robot_mode.h index 90c9eb8a0..6c7c6416a 100644 --- a/include/ur_modern_driver/ur/robot_mode.h +++ b/include/ur_modern_driver/ur/robot_mode.h @@ -16,6 +16,7 @@ class SharedRobotModeData bool real_robot_enabled; bool robot_power_on; bool emergency_stopped; + bool protective_stopped; //AKA security_stopped bool program_running; bool program_paused; @@ -43,7 +44,6 @@ class RobotModeData_V1_X : public SharedRobotModeData, public StatePacket virtual bool parseWith(BinParser& bp); virtual bool consumeWith(URStatePacketConsumer& consumer); - bool security_stopped; robot_mode_V1_X robot_mode; double speed_fraction; @@ -79,7 +79,6 @@ class RobotModeData_V3_0__1 : public SharedRobotModeData, public StatePacket virtual bool parseWith(BinParser& bp); virtual bool consumeWith(URStatePacketConsumer& consumer); - bool protective_stopped; robot_mode_V3_X robot_mode; robot_control_mode_V3_X control_mode; diff --git a/include/ur_modern_driver/ur/server.h b/include/ur_modern_driver/ur/server.h new file mode 100644 index 000000000..87408060c --- /dev/null +++ b/include/ur_modern_driver/ur/server.h @@ -0,0 +1,19 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include "ur_modern_driver/ur/stream.h" + +class URServer +{ +private: + int socket_fd_ = -1; + +public: + URServer(int port); + URStream accept(); +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/stream.h b/include/ur_modern_driver/ur/stream.h index eac3c098f..16719d719 100644 --- a/include/ur_modern_driver/ur/stream.h +++ b/include/ur_modern_driver/ur/stream.h @@ -19,15 +19,38 @@ class URStream std::mutex send_mutex_, receive_mutex_; public: + URStream() + { + } + URStream(std::string& host, int port) : host_(host), port_(port), initialized_(false), stopping_(false) { } + URStream(int socket_fd) : socket_fd_(socket_fd), initialized_(true), stopping_(false) + { + + } + + URStream(URStream&& other) noexcept : socket_fd_(other.socket_fd_), host_(other.host_), initialized_(other.initialized_.load()), stopping_(other.stopping_.load()) + { + + } + ~URStream() { disconnect(); } + URStream& operator=(URStream&& other) + { + socket_fd_ = std::move(other.socket_fd_); + host_ = std::move(other.host_); + initialized_ = std::move(other.initialized_.load()); + stopping_ = std::move(other.stopping_.load()); + return *this; + } + bool connect(); void disconnect(); void reconnect(); diff --git a/src/ros/action_server.cpp b/src/ros/action_server.cpp new file mode 100644 index 000000000..3b6637dea --- /dev/null +++ b/src/ros/action_server.cpp @@ -0,0 +1,231 @@ +#include +#include "ur_modern_driver/ros/action_server.h" + +ActionServer::ActionServer(TrajectoryFollower& follower, std::vector& joint_names, double max_velocity) + : as_( + nh_, + "follow_joint_trajectory", + boost::bind(&ActionServer::onGoal, this, _1), + boost::bind(&ActionServer::onCancel, this, _1), + false + ) + , joint_names_(joint_names) + , joint_set_(joint_names.begin(), joint_names.end()) + , max_velocity_(max_velocity) + , state_(RobotState::Error) + , follower_(follower) +{ + +} + +void ActionServer::onRobotStateChange(RobotState state) +{ + state_ = state; +} + +void ActionServer::onGoal(GoalHandle gh) +{ + Result res; + res.error_code = -100; + + if(!validate(gh, res) || !try_execute(gh, res)) + gh.setRejected(res, res.error_string); +} + +void ActionServer::onCancel(GoalHandle gh) +{ + +} + +bool ActionServer::validate(GoalHandle& gh, Result& res) +{ + return !validateState(gh, res) || !validateJoints(gh, res) || !validateTrajectory(gh, res); +} + +bool ActionServer::validateState(GoalHandle& gh, Result& res) +{ + switch(state_) + { + case RobotState::EmergencyStopped: + res.error_string = "Robot is emergency stopped"; + return false; + + case RobotState::ProtectiveStopped: + res.error_string = "Robot is protective stopped"; + return false; + + case RobotState::Error: + res.error_string = "Robot is not ready, check robot_mode"; + return false; + + case RobotState::Running: + return true; + + default: + res.error_string = "Undefined state"; + return false; + } +} + +bool ActionServer::validateJoints(GoalHandle& gh, Result& res) +{ + auto goal = gh.getGoal(); + auto const& joints = goal->trajectory.joint_names; + std::set goal_joints(joints.begin(), joints.end()); + + if(goal_joints == joint_set_) + return true; + + res.error_code = Result::INVALID_JOINTS; + res.error_string = "Invalid joint names for goal"; + return false; +} + +bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res) +{ + auto goal = gh.getGoal(); + res.error_code = Result::INVALID_GOAL; + + for(auto const& point : goal->trajectory.points) + { + if(point.velocities.size() != joint_names_.size()) + { + res.error_code = Result::INVALID_GOAL; + res.error_string = "Received a goal with an invalid number of velocities"; + return false; + } + + if(point.positions.size() != joint_names_.size()) + { + res.error_code = Result::INVALID_GOAL; + res.error_string = "Received a goal with an invalid number of positions"; + return false; + } + + for(auto const& velocity : point.velocities) + { + if(!std::isfinite(velocity)) + { + res.error_string = "Received a goal with infinities or NaNs in velocity"; + return false; + } + if(std::fabs(velocity) > max_velocity_) + { + res.error_string = "Received a goal with velocities that are higher than " + std::to_string(max_velocity_); + return false; + } + } + for(auto const& position : point.positions) + { + if(!std::isfinite(position)) + { + res.error_string = "Received a goal with infinities or NaNs in positions"; + return false; + } + } + } + + return true; +} + +bool ActionServer::try_execute(GoalHandle& gh, Result& res) +{ + if(!running_) + { + res.error_string = "Internal error"; + return false; + } + if(!tj_mutex_.try_lock()) + { + has_goal_ = false; + //stop_trajectory(); + res.error_string = "Received another trajectory"; + curr_gh_.setAborted(res, res.error_string); + tj_mutex_.lock(); + } + //locked here + curr_gh_ = gh; + has_goal_ = true; + tj_mutex_.unlock(); + tj_cv_.notify_one(); +} + +inline double ActionServer::interp_cubic(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel) +{ + using std::pow; + double a = p0_pos; + double b = p0_vel; + double c = (-3 * a + 3 * p1_pos - 2 * T * b - T * p1_vel) / pow(T, 2); + double d = (2 * a - 2 * p1_pos + T * b + T * p1_vel) / pow(T, 3); + return a + b * t + c * pow(t, 2) + d * pow(t, 3); +} + +std::vector ActionServer::reorderMap(std::vector goal_joints) +{ + std::vector indecies; + for(auto const& aj : joint_names_) + { + size_t j = 0; + for(auto const& gj : goal_joints) + { + if(aj == gj) + break; + j++; + } + indecies.push_back(j); + } + return indecies; +} + +void ActionServer::trajectoryThread() +{ + while(running_) + { + std::unique_lock lk(tj_mutex_); + if(!tj_cv_.wait_for(lk, std::chrono::milliseconds(100), [&]{return running_ && has_goal_;})) + continue; + + auto g = curr_gh_.getGoal(); + auto const& traj = g->trajectory; + auto const& points = traj.points; + size_t len = points.size(); + auto const& last_point = points[points.size() - 1]; + double end_time = last_point.time_from_start.toSec(); + + auto mapping = reorderMap(traj.joint_names); + std::chrono::high_resolution_clock::time_point t0, t; + t = t0 = std::chrono::high_resolution_clock::now(); + + size_t i = 0; + while(end_time >= toSec(t - t0) && has_goal_) + { + while(points[i].time_from_start.toSec() <= toSec(t - t0) && i < len) + i++; + + auto const& pp = points[i-1]; + auto const& p = points[i]; + + auto pp_t = pp.time_from_start.toSec(); + auto p_t =p.time_from_start.toSec(); + + std::array pos; + for(size_t j = 0; j < pos.size(); j++) + { + pos[i] = interp_cubic( + toSec(t - t0) - pp_t, + p_t - pp_t, + pp.positions[j], + p.positions[j], + pp.velocities[j], + p.velocities[j] + ); + } + + follower_.execute(pos); + //std::this_thread::sleep_for(std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.))); + t = std::chrono::high_resolution_clock::now(); + } + + has_goal_ = false; + } +} \ No newline at end of file diff --git a/src/ros/controller.cpp b/src/ros/controller.cpp new file mode 100644 index 000000000..753f28d30 --- /dev/null +++ b/src/ros/controller.cpp @@ -0,0 +1,84 @@ +#include "ur_modern_driver/ros/controller.h" + +ROSController::ROSController(URCommander &commander, std::vector &joint_names, double max_vel_change) + : controller_(this, nh_) + , joint_interface_(joint_names) + , wrench_interface_() + , position_interface_(commander, joint_interface_, joint_names) + , velocity_interface_(commander, joint_interface_, joint_names, max_vel_change) +{ + registerInterface(&joint_interface_); + registerInterface(&wrench_interface_); + registerControllereInterface(&position_interface_); + registerControllereInterface(&velocity_interface_); +} + +void ROSController::setupConsumer() +{ + lastUpdate_ = ros::Time::now(); +} + +void ROSController::doSwitch(const std::list& start_list, const std::list& stop_list) +{ + if (active_interface_ != nullptr && stop_list.size() > 0) + { + LOG_INFO("Stopping active interface"); + active_interface_->stop(); + active_interface_ = nullptr; + } + + for (auto const& ci : start_list) + { + auto ait = available_interfaces_.find(ci.hardware_interface); + + if (ait == available_interfaces_.end()) + continue; + + auto new_interface = ait->second; + + LOG_INFO("Starting %s", ci.hardware_interface.c_str()); + active_interface_ = new_interface; + new_interface->start(); + + return; + } + + LOG_WARN("Failed to start interface!"); +} + +bool ROSController::write() +{ + if (active_interface_ == nullptr) + return true; + + return active_interface_->write(); +} + +void ROSController::read(RTShared& packet) +{ + joint_interface_.update(packet); + wrench_interface_.update(packet); +} + + +bool ROSController::update(RTShared& state) +{ + auto time = ros::Time::now(); + auto diff = time - lastUpdate_; + lastUpdate_ = time; + + read(state); + controller_.update(time, diff); + + //emergency stop and such should not kill the pipeline + //but still prevent writes + if(!service_enabled_) + return true; + + return write(); +} + +void ROSController::onRobotStateChange(RobotState state) +{ + service_enabled_ = (state == RobotState::Running); +} \ No newline at end of file diff --git a/src/ros/hardware_interface.cpp b/src/ros/hardware_interface.cpp new file mode 100644 index 000000000..1bf0cc4aa --- /dev/null +++ b/src/ros/hardware_interface.cpp @@ -0,0 +1,74 @@ +#include "ur_modern_driver/ros/hardware_interface.h" + +JointInterface::JointInterface(std::vector &joint_names) +{ + for (size_t i = 0; i < 6; i++) + { + registerHandle(hardware_interface::JointStateHandle(joint_names[i], &positions_[i], &velocities_[i], &efforts_[i])); + } +} + +void JointInterface::update(RTShared &packet) +{ + positions_ = packet.q_actual; + velocities_ = packet.qd_actual; + efforts_ = packet.i_actual; +} + +WrenchInterface::WrenchInterface() +{ + registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", "", tcp_.begin(), tcp_.begin() + 3)); +} + +void WrenchInterface::update(RTShared &packet) +{ + tcp_ = packet.tcp_force; +} + +VelocityInterface::VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names, double max_vel_change) + : commander_(commander), max_vel_change_(max_vel_change) +{ + for (size_t i = 0; i < 6; i++) + { + registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &velocity_cmd_[i])); + } +} + +bool VelocityInterface::write() +{ + for (size_t i = 0; i < 6; i++) + { + double prev = prev_velocity_cmd_[i]; + double lo = prev - max_vel_change_; + double hi = prev + max_vel_change_; + // clamp value to ±max_vel_change + prev_velocity_cmd_[i] = std::max(lo, std::min(velocity_cmd_[i], hi)); + } + + return commander_.speedj(prev_velocity_cmd_, max_vel_change_); +} + + +PositionInterface:: PositionInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names) + : commander_(commander) +{ + for (size_t i = 0; i < 6; i++) + { + registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &position_cmd_[i])); + } +} + +bool PositionInterface::write() +{ + +} + +void PositionInterface::start() +{ + +} + +void PositionInterface::stop() +{ + +} \ No newline at end of file diff --git a/src/ros/robot_hardware.cpp b/src/ros/robot_hardware.cpp index fce866db1..008daec27 100644 --- a/src/ros/robot_hardware.cpp +++ b/src/ros/robot_hardware.cpp @@ -28,45 +28,3 @@ bool RobotHardware::canSwitch(const std::list& start_list, return true; } */ - -void RobotHardware::doSwitch(const std::list& start_list, const std::list& stop_list) -{ - if (active_interface_ != nullptr && stop_list.size() > 0) - { - LOG_INFO("Stopping active interface"); - active_interface_->stop(); - active_interface_ = nullptr; - } - - for (auto const& ci : start_list) - { - auto ait = available_interfaces_.find(ci.hardware_interface); - - if (ait == available_interfaces_.end()) - continue; - - auto new_interface = ait->second; - - LOG_INFO("Starting %s", ci.hardware_interface.c_str()); - active_interface_ = new_interface; - new_interface->start(); - - return; - } - - LOG_WARN("Failed to start interface!"); -} - -void RobotHardware::write() -{ - if (active_interface_ == nullptr) - return; - - active_interface_->write(); -} - -void RobotHardware::read(RTShared& packet) -{ - joint_interface_.update(packet); - wrench_interface_.update(packet); -} \ No newline at end of file diff --git a/src/ros/service_stopper.cpp b/src/ros/service_stopper.cpp new file mode 100644 index 000000000..53cb43cf3 --- /dev/null +++ b/src/ros/service_stopper.cpp @@ -0,0 +1,47 @@ +#include "ur_modern_driver/ros/service_stopper.h" + +ServiceStopper::ServiceStopper(std::vector services) + : enable_service_(nh_.advertiseService("ur_driver/robot_enable", &ServiceStopper::enableCallback, this)) + , services_(services) + , last_state_(RobotState::Error) +{ + //enable_all(); +} + + +bool ServiceStopper::enableCallback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp) +{ + notify_all(RobotState::Running); + return true; +} + +void ServiceStopper::notify_all(RobotState state) +{ + if(last_state_ == state) + return; + + for(auto const service : services_) + { + service->onRobotStateChange(state); + } + + last_state_ = state; +} + +bool ServiceStopper::handle(SharedRobotModeData& data, bool error) +{ + if(data.emergency_stopped) + { + notify_all(RobotState::EmergencyStopped); + } + else if(data.protective_stopped) + { + notify_all(RobotState::ProtectiveStopped); + } + else if(error) + { + notify_all(RobotState::Error); + } + + return true; +} \ No newline at end of file diff --git a/src/ros/trajectory_follower.cpp b/src/ros/trajectory_follower.cpp new file mode 100644 index 000000000..ca36e5914 --- /dev/null +++ b/src/ros/trajectory_follower.cpp @@ -0,0 +1,137 @@ +#include +#include "ur_modern_driver/ros/trajectory_follower.h" + + +TrajectoryFollower::TrajectoryFollower(URCommander &commander, int reverse_port, bool version_3) + : running_(false) + , commander_(commander) + , server_(reverse_port) + , program_(buildProgram(version_3)) +{ +} +static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}"); +static const std::string SERVO_J_REPLACE("{{SERVO_J_REPLACE}}"); +static const std::string POSITION_PROGRAM = R"( +def driverProg(): + MULT_jointstate = {{JOINT_STATE_REPLACE}} + + SERVO_IDLE = 0 + SERVO_RUNNING = 1 + cmd_servo_state = SERVO_IDLE + cmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + def set_servo_setpoint(q): + enter_critical + cmd_servo_state = SERVO_RUNNING + cmd_servo_q = q + exit_critical + end + thread servoThread(): + state = SERVO_IDLE + while True: + enter_critical + q = cmd_servo_q + do_brake = False + if (state == SERVO_RUNNING) and (cmd_servo_state == SERVO_IDLE): + do_brake = True + end + state = cmd_servo_state + cmd_servo_state = SERVO_IDLE + exit_critical + if do_brake: + stopj(1.0) + sync() + elif state == SERVO_RUNNING: + servoj(q, {{SERVO_J_REPLACE}}) + else: + sync() + end + end + end + + thread_servo = run servoThread() + keepalive = 1 + while keepalive > 0: + params_mult = socket_read_binary_integer(6+1) + if params_mult[0] > 0: + q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate] + keepalive = params_mult[7] + set_servo_setpoint(q) + end + end + sleep(.1) + socket_close() + kill thread_servo +end +)"; +std::string TrajectoryFollower::buildProgram(bool version_3) +{ + std::string res(POSITION_PROGRAM); + size_t js_idx = POSITION_PROGRAM.find(JOINT_STATE_REPLACE); + size_t sj_idx = POSITION_PROGRAM.find(SERVO_J_REPLACE); + + + std::ostringstream out; + out << "t=" << std::fixed << std::setprecision(4) << servoj_time_; + + if(version_3) + out << ", lookahead_time=" << servoj_lookahead_time_ << ", gain=" << servoj_gain_; + + res.replace(js_idx, JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_)); + res.replace(sj_idx, SERVO_J_REPLACE.length(), out.str()); + return res; +} + +bool TrajectoryFollower::start() +{ + if(running_) + return true; //not sure + + //TODO + std::string prog(""); // buildProg(); + if(!commander_.uploadProg(prog)) + return false; + + stream_ = std::move(server_.accept()); //todo: pointer instead? + return (running_ = true); +} + +bool TrajectoryFollower::execute(std::array &positions, bool keep_alive) +{ + if(!running_) + return false; + + last_positions_ = positions; + + uint8_t buf[sizeof(uint32_t)*7]; + uint8_t *idx = buf; + + for(auto const& pos : positions) + { + int32_t val = static_cast(pos * MULT_JOINTSTATE_); + val = htobe32(val); + idx += append(idx, val); + } + + int32_t val = htobe32(static_cast(keep_alive)); + append(idx, val); + + ssize_t res = stream_.send(buf, sizeof(buf)); + return res > 0 && res == sizeof(buf); +} + +bool TrajectoryFollower::execute(std::array &positions) +{ + return execute(positions, true); +} + +void TrajectoryFollower::stop() +{ + if(!running_) + return; + + std::array empty; + execute(empty, false); + + stream_.disconnect(); + running_ = false; +} \ No newline at end of file diff --git a/src/ros_main.cpp b/src/ros_main.cpp index b08e90b78..7f7679291 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -8,8 +8,9 @@ #include "ur_modern_driver/pipeline.h" #include "ur_modern_driver/ros/io_service.h" #include "ur_modern_driver/ros/mb_publisher.h" -#include "ur_modern_driver/ros/ros_controller.h" +#include "ur_modern_driver/ros/controller.h" #include "ur_modern_driver/ros/rt_publisher.h" +#include "ur_modern_driver/ros/service_stopper.h" #include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/factory.h" #include "ur_modern_driver/ur/messages.h" @@ -20,8 +21,8 @@ static const std::string IP_ADDR_ARG("~robot_ip_address"); static const std::string REVERSE_PORT_ARG("~reverse_port"); -static const std::string SIM_TIME_ARG("~use_sim_time"); static const std::string ROS_CONTROL_ARG("~use_ros_control"); +static const std::string MAX_VEL_CHANGE_ARG("~max_vel_change"); 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"); @@ -40,12 +41,12 @@ struct ProgArgs std::vector joint_names; double max_acceleration; double max_velocity; + double max_vel_change; int32_t reverse_port; - bool use_sim_time; bool use_ros_control; }; -bool parse_args(ProgArgs& args) +bool parse_args(ProgArgs &args) { if (!ros::param::get(IP_ADDR_ARG, args.host)) { @@ -53,7 +54,7 @@ bool parse_args(ProgArgs& args) return false; } ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001)); - ros::param::param(SIM_TIME_ARG, args.use_sim_time, false); + ros::param::param(MAX_VEL_CHANGE_ARG, args.max_vel_change, 15.0); // rad/s ros::param::param(ROS_CONTROL_ARG, args.use_ros_control, false); ros::param::param(PREFIX_ARG, args.prefix, std::string()); ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link"); @@ -64,7 +65,7 @@ bool parse_args(ProgArgs& args) #include "ur_modern_driver/event_counter.h" -int main(int argc, char** argv) +int main(int argc, char **argv) { ros::init(argc, argv, "ur_driver"); @@ -75,18 +76,24 @@ int main(int argc, char** argv) } URFactory factory(args.host); + + vector services; + // RT packets auto rt_parser = factory.getRTParser(); URStream rt_stream(args.host, UR_RT_PORT); URProducer rt_prod(rt_stream, *rt_parser); RTPublisher rt_pub(args.prefix, args.base_frame, args.tool_frame, args.use_ros_control); URCommander rt_commander(rt_stream); - vector*> rt_vec{ &rt_pub }; + vector *> rt_vec{&rt_pub}; + ROSController *controller(nullptr); if (args.use_ros_control) { LOG_INFO("ROS control enabled"); - rt_vec.push_back(new ROSController(rt_commander, args.joint_names)); + controller = new ROSController(rt_commander, args.joint_names, args.max_vel_change); + rt_vec.push_back(controller); + services.push_back(controller); } MultiConsumer rt_cons(rt_vec); @@ -97,7 +104,10 @@ int main(int argc, char** argv) URStream state_stream(args.host, UR_SECONDARY_PORT); URProducer state_prod(state_stream, *state_parser); MBPublisher state_pub; - vector*> state_vec{ &state_pub }; + + ServiceStopper service_stopper(services); + + vector *> state_vec{&state_pub, &service_stopper}; MultiConsumer state_cons(state_vec); Pipeline state_pl(state_prod, state_cons); @@ -107,12 +117,19 @@ int main(int argc, char** argv) state_pl.run(); URCommander state_commander(state_stream); - IOService service(state_commander); + IOService io_service(state_commander); ros::spin(); + LOG_INFO("ROS stopping, shutting down pipelines"); + rt_pl.stop(); state_pl.stop(); + + if(controller) + delete controller; + + LOG_INFO("Pipelines shutdown complete"); return EXIT_SUCCESS; } \ No newline at end of file diff --git a/src/ur/commander.cpp b/src/ur/commander.cpp index d9e16d2f1..333bc5265 100644 --- a/src/ur/commander.cpp +++ b/src/ur/commander.cpp @@ -9,6 +9,10 @@ bool URCommander::write(std::string& s) return res > 0 && static_cast(res) == len; } +bool URCommander::uploadProg(std::string &s) +{ + return write(s); +} bool URCommander::speedj(std::array &speeds, double acceleration) { @@ -52,11 +56,17 @@ bool URCommander::setToolVoltage(uint8_t voltage) } -bool URCommander::setFlag(bool value) +bool URCommander::setFlag(uint8_t pin, bool value) { - + std::ostringstream out; + out << "set_flag(" << (int)pin << "," << (value ? "True" : "False") << ")\n"; + std::string s(out.str()); + return write(s); } bool URCommander::setPayload(double value) { - + std::ostringstream out; + out << "set_payload(" << std::fixed << std::setprecision(4) << value << ")\n"; + std::string s(out.str()); + return write(s); } \ No newline at end of file diff --git a/src/ur/robot_mode.cpp b/src/ur/robot_mode.cpp index 5bee4cf49..da3a65ba8 100644 --- a/src/ur/robot_mode.cpp +++ b/src/ur/robot_mode.cpp @@ -8,6 +8,9 @@ bool SharedRobotModeData::parseWith(BinParser& bp) bp.parse(real_robot_enabled); bp.parse(robot_power_on); bp.parse(emergency_stopped); + bp.parse(protective_stopped); + bp.parse(program_running); + bp.parse(program_paused); return true; } @@ -18,9 +21,6 @@ bool RobotModeData_V1_X::parseWith(BinParser& bp) SharedRobotModeData::parseWith(bp); - bp.parse(security_stopped); - bp.parse(program_running); - bp.parse(program_paused); bp.parse(robot_mode); bp.parse(speed_fraction); @@ -34,9 +34,6 @@ bool RobotModeData_V3_0__1::parseWith(BinParser& bp) SharedRobotModeData::parseWith(bp); - bp.parse(protective_stopped); - bp.parse(program_running); - bp.parse(program_paused); bp.parse(robot_mode); bp.parse(control_mode); bp.parse(target_speed_fraction); diff --git a/src/ur/server.cpp b/src/ur/server.cpp new file mode 100644 index 000000000..d99f1a91d --- /dev/null +++ b/src/ur/server.cpp @@ -0,0 +1,51 @@ +#include +#include +#include +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/ur/server.h" + +URServer::URServer(int port) +{ + std::string service = std::to_string(port); + struct addrinfo hints, *result; + std::memset(&hints, 0, sizeof(hints)); + + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_STREAM; + hints.ai_flags = AI_PASSIVE; + + if (getaddrinfo(nullptr, service.c_str(), &hints, &result) != 0) + { + LOG_ERROR("Failed to setup recieving server"); + return; + } + + // loop through the list of addresses untill we find one that's connectable + for (struct addrinfo* p = result; p != nullptr; p = p->ai_next) + { + socket_fd_ = socket(p->ai_family, p->ai_socktype, p->ai_protocol); + + if (socket_fd_ == -1) // socket error? + continue; + + if (bind(socket_fd_, p->ai_addr, p->ai_addrlen) != 0) + continue; + + // disable Nagle's algorithm to ensure we sent packets as fast as possible + int flag = 1; + setsockopt(socket_fd_, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(flag)); + LOG_INFO("Server awaiting connection"); + return; + } + + LOG_ERROR("Failed to setup recieving server"); + std::exit(EXIT_FAILURE); +} + +URStream URServer::accept() +{ + struct sockaddr addr; + socklen_t addr_len; + int client_fd = ::accept(socket_fd_, &addr, &addr_len); + return URStream(client_fd); +} \ No newline at end of file diff --git a/tests/ur/robot_mode.cpp b/tests/ur/robot_mode.cpp index a13fcb805..4209191bf 100644 --- a/tests/ur/robot_mode.cpp +++ b/tests/ur/robot_mode.cpp @@ -18,7 +18,7 @@ TEST(RobotModeData_V1_X, testRandomDataParsing) ASSERT_EQ(rdt.getNext(), state.real_robot_enabled); ASSERT_EQ(rdt.getNext(), state.robot_power_on); ASSERT_EQ(rdt.getNext(), state.emergency_stopped); - ASSERT_EQ(rdt.getNext(), state.security_stopped); + ASSERT_EQ(rdt.getNext(), state.protective_stopped); ASSERT_EQ(rdt.getNext(), state.program_running); ASSERT_EQ(rdt.getNext(), state.program_paused); ASSERT_EQ(rdt.getNext(), state.robot_mode); From c59bfc78cc6baa9b6595f5aa3b9e42f1fb96bf25 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Thu, 27 Apr 2017 06:40:03 +0200 Subject: [PATCH 046/114] Major refactor --- CMakeLists.txt | 1 + include/ur_modern_driver/pipeline.h | 24 ++ include/ur_modern_driver/ros/action_server.h | 13 +- include/ur_modern_driver/ros/controller.h | 2 + .../ur_modern_driver/ros/hardware_interface.h | 2 + include/ur_modern_driver/ros/io_service.h | 2 + .../ros/trajectory_follower.h | 31 ++- include/ur_modern_driver/tcp_socket.h | 45 ++++ include/ur_modern_driver/ur/commander.h | 38 ++- include/ur_modern_driver/ur/factory.h | 13 + include/ur_modern_driver/ur/producer.h | 39 +-- include/ur_modern_driver/ur/server.h | 19 +- include/ur_modern_driver/ur/stream.h | 51 ++-- src/ros/action_server.cpp | 112 +++++---- src/ros/controller.cpp | 29 ++- src/ros/hardware_interface.cpp | 66 ++--- src/ros/trajectory_follower.cpp | 226 +++++++++++++----- src/ros_main.cpp | 28 ++- src/tcp_socket.cpp | 133 +++++++++++ src/ur/commander.cpp | 146 +++++++---- src/ur/server.cpp | 73 +++--- src/ur/stream.cpp | 129 ++-------- 22 files changed, 812 insertions(+), 410 deletions(-) create mode 100644 include/ur_modern_driver/tcp_socket.h create mode 100644 src/tcp_socket.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 1de272572..be59d00b1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -172,6 +172,7 @@ set(${PROJECT_NAME}_SOURCES src/ur/master_board.cpp src/ur/rt_state.cpp src/ur/messages.cpp + src/tcp_socket.cpp src/ur_driver.cpp src/ur_realtime_communication.cpp src/ur_communication.cpp diff --git a/include/ur_modern_driver/pipeline.h b/include/ur_modern_driver/pipeline.h index 9f487b154..0e5cfdb52 100644 --- a/include/ur_modern_driver/pipeline.h +++ b/include/ur_modern_driver/pipeline.h @@ -23,6 +23,9 @@ class IConsumer virtual void stopConsumer() { } + virtual void onTimeout() + { + } virtual bool consume(shared_ptr product) = 0; }; @@ -59,6 +62,13 @@ class MultiConsumer : public IConsumer con->stopConsumer(); } } + virtual void onTimeout() + { + for(auto &con : consumers_) + { + con->onTimeout(); + } + } bool consume(shared_ptr product) { @@ -93,6 +103,8 @@ template class Pipeline { private: + typedef std::chrono::high_resolution_clock Clock; + typedef Clock::time_point Time; IProducer& producer_; IConsumer& consumer_; BlockingReaderWriterQueue> queue_; @@ -129,6 +141,8 @@ class Pipeline { consumer_.setupConsumer(); unique_ptr product; + Time last_pkg = Clock::now(); + Time last_warn = last_pkg; while (running_) { // 16000us timeout was chosen because we should @@ -136,8 +150,18 @@ class Pipeline // 8ms so double it for some error margin if (!queue_.wait_dequeue_timed(product, std::chrono::milliseconds(16))) { + Time now = Clock::now(); + auto pkg_diff = now - last_pkg; + auto warn_diff = now - last_warn; + if(pkg_diff > std::chrono::seconds(1) && warn_diff > std::chrono::seconds(1)) + { + last_warn = now; + consumer_.onTimeout(); + } continue; } + + last_pkg = Clock::now(); if (!consumer_.consume(std::move(product))) break; } diff --git a/include/ur_modern_driver/ros/action_server.h b/include/ur_modern_driver/ros/action_server.h index dd11de40f..398e4d585 100644 --- a/include/ur_modern_driver/ros/action_server.h +++ b/include/ur_modern_driver/ros/action_server.h @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -17,7 +18,7 @@ #include "ur_modern_driver/ros/trajectory_follower.h" -class ActionServer : public URRTPacketConsumer, public Service +class ActionServer : public Service //,public URRTPacketConsumer { private: typedef control_msgs::FollowJointTrajectoryAction Action; @@ -35,9 +36,11 @@ class ActionServer : public URRTPacketConsumer, public Service GoalHandle curr_gh_; + std::atomic interrupt_traj_; std::atomic has_goal_, running_; std::mutex tj_mutex_; std::condition_variable tj_cv_; + std::thread tj_thread_; TrajectoryFollower& follower_; @@ -50,20 +53,16 @@ class ActionServer : public URRTPacketConsumer, public Service bool validateTrajectory(GoalHandle& gh, Result& res); bool try_execute(GoalHandle& gh, Result& res); + void interruptGoal(GoalHandle& gh); std::vector reorderMap(std::vector goal_joints); - double interp_cubic(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel); void trajectoryThread(); - template - double toSec(U const& u) - { - return std::chrono::duration_cast>(u).count(); - } public: ActionServer(TrajectoryFollower& follower, std::vector& joint_names, double max_velocity); + void start(); virtual void onRobotStateChange(RobotState state); }; \ No newline at end of file diff --git a/include/ur_modern_driver/ros/controller.h b/include/ur_modern_driver/ros/controller.h index eb940d079..3532ee6ae 100644 --- a/include/ur_modern_driver/ros/controller.h +++ b/include/ur_modern_driver/ros/controller.h @@ -34,6 +34,7 @@ class ROSController : private hardware_interface::RobotHW, public URRTPacketCons std::map available_interfaces_; std::atomic service_enabled_; + std::atomic service_cooldown_; // helper functions to map interfaces template @@ -51,6 +52,7 @@ class ROSController : private hardware_interface::RobotHW, public URRTPacketCons void read(RTShared& state); bool update(RTShared& state); bool write(); + void reset(); public: ROSController(URCommander& commander, std::vector& joint_names, double max_vel_change); diff --git a/include/ur_modern_driver/ros/hardware_interface.h b/include/ur_modern_driver/ros/hardware_interface.h index 0c6618d53..325faa4b4 100644 --- a/include/ur_modern_driver/ros/hardware_interface.h +++ b/include/ur_modern_driver/ros/hardware_interface.h @@ -13,6 +13,7 @@ class HardwareInterface virtual bool write() = 0; virtual void start() {} virtual void stop() {} + virtual void reset() {} }; using hardware_interface::JointHandle; @@ -48,6 +49,7 @@ class VelocityInterface : public HardwareInterface, public hardware_interface::V public: VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names, double max_vel_change); virtual bool write(); + virtual void reset(); typedef hardware_interface::VelocityJointInterface parent_type; }; diff --git a/include/ur_modern_driver/ros/io_service.h b/include/ur_modern_driver/ros/io_service.h index 36ce662de..03d993581 100644 --- a/include/ur_modern_driver/ros/io_service.h +++ b/include/ur_modern_driver/ros/io_service.h @@ -37,6 +37,8 @@ class IOService case ur_msgs::SetIO::Request::FUN_SET_FLAG: res = commander_.setFlag(req.pin, flag); break; + default: + LOG_WARN("Invalid setIO function called (%d)", req.fun); } return (resp.success = res); diff --git a/include/ur_modern_driver/ros/trajectory_follower.h b/include/ur_modern_driver/ros/trajectory_follower.h index a1df97f06..a16c3e52d 100644 --- a/include/ur_modern_driver/ros/trajectory_follower.h +++ b/include/ur_modern_driver/ros/trajectory_follower.h @@ -5,21 +5,39 @@ #include #include #include +#include #include +#include "ur_modern_driver/log.h" #include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/server.h" -#include "ur_modern_driver/ur/stream.h" + +struct TrajectoryPoint +{ + std::array positions; + std::array velocities; + std::chrono::microseconds time_from_start; + + TrajectoryPoint() + { + } + + TrajectoryPoint(std::array &pos, std::array &vel, std::chrono::microseconds tfs) + : positions(pos) + , velocities(vel) + , time_from_start(tfs) + { + } +}; class TrajectoryFollower { private: - const int32_t MULT_JOINTSTATE_ = 1000000; double servoj_time_, servoj_lookahead_time_, servoj_gain_; std::atomic running_; std::array last_positions_; URCommander &commander_; URServer server_; - URStream stream_; + int reverse_port_; std::string program_; template @@ -30,15 +48,16 @@ class TrajectoryFollower return s; } + std::string buildProgram(); bool execute(std::array &positions, bool keep_alive); + double interpolate(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel); public: TrajectoryFollower(URCommander &commander, int reverse_port, bool version_3); - std::string buildProgram(bool version_3); - bool start(); bool execute(std::array &positions); + bool execute(std::vector &trajectory, std::atomic &interrupt); void stop(); - void halt(); //maybe + void interrupt(); }; \ No newline at end of file diff --git a/include/ur_modern_driver/tcp_socket.h b/include/ur_modern_driver/tcp_socket.h new file mode 100644 index 000000000..303d49e99 --- /dev/null +++ b/include/ur_modern_driver/tcp_socket.h @@ -0,0 +1,45 @@ +#pragma once +#include +#include +#include +#include +#include +#include + +enum class SocketState +{ + Invalid, + Connected, + Disconnected, + Closed +}; + +class TCPSocket +{ +private: + std::atomic socket_fd_; + std::atomic state_; + +protected: + virtual bool open(int socket_fd, struct sockaddr *address, size_t address_len) + { + return false; + } + + + bool setup(std::string &host, int port); + void close(); + + +public: + TCPSocket(); + virtual ~TCPSocket(); + + SocketState getState() { return state_; } + + int getSocketFD() { return socket_fd_; } + bool setSocketFD(int socket_fd); + + bool read(uint8_t* buf, size_t buf_len, size_t &read); + bool write(const uint8_t* buf, size_t buf_len, size_t &written); +}; diff --git a/include/ur_modern_driver/ur/commander.h b/include/ur_modern_driver/ur/commander.h index da92ae86c..5d81698c2 100644 --- a/include/ur_modern_driver/ur/commander.h +++ b/include/ur_modern_driver/ur/commander.h @@ -11,18 +11,46 @@ class URCommander protected: bool write(std::string& s); + void formatArray(std::ostringstream &out, std::array &values); public: URCommander(URStream& stream) : stream_(stream) { } - virtual bool uploadProg(std::string &s); + virtual bool speedj(std::array &speeds, double acceleration) = 0; + virtual bool setDigitalOut(uint8_t pin, bool value) = 0; + virtual bool setAnalogOut(uint8_t pin, double value) = 0; + + //shared + bool uploadProg(std::string &s); + bool stopj(double a = 10.0); + bool setToolVoltage(uint8_t voltage); + bool setFlag(uint8_t pin, bool value); + bool setPayload(double value); +}; + +class URCommander_V1_X : public URCommander +{ +public: + URCommander_V1_X(URStream& stream) : URCommander(stream) + { + } + + virtual bool speedj(std::array &speeds, double acceleration); + virtual bool setDigitalOut(uint8_t pin, bool value); + virtual bool setAnalogOut(uint8_t pin, double value); +}; + + +class URCommander_V3_X : public URCommander +{ +public: + URCommander_V3_X(URStream& stream) : URCommander(stream) + { + } + virtual bool speedj(std::array &speeds, double acceleration); - virtual bool stopj(double a = 10.0); virtual bool setDigitalOut(uint8_t pin, bool value); virtual bool setAnalogOut(uint8_t pin, double value); - virtual bool setToolVoltage(uint8_t voltage); - virtual bool setFlag(uint8_t pin, bool value); - virtual bool setPayload(double value); }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/factory.h b/include/ur_modern_driver/ur/factory.h index 83f0cd6ff..0b4e6ca78 100644 --- a/include/ur_modern_driver/ur/factory.h +++ b/include/ur_modern_driver/ur/factory.h @@ -71,6 +71,19 @@ class URFactory : private URMessagePacketConsumer prod.teardownProducer(); } + bool isVersion3() + { + return major_version_ == 3; + } + + std::unique_ptr getCommander(URStream &stream) + { + if(major_version_ == 1) + return std::unique_ptr(new URCommander_V1_X(stream)); + else + return std::unique_ptr(new URCommander_V3_X(stream)); + } + std::unique_ptr> getStateParser() { if (major_version_ == 1) diff --git a/include/ur_modern_driver/ur/producer.h b/include/ur_modern_driver/ur/producer.h index 5907f976b..b3bb475bb 100644 --- a/include/ur_modern_driver/ur/producer.h +++ b/include/ur_modern_driver/ur/producer.h @@ -1,4 +1,5 @@ #pragma once +#include #include "ur_modern_driver/pipeline.h" #include "ur_modern_driver/ur/parser.h" #include "ur_modern_driver/ur/stream.h" @@ -9,9 +10,10 @@ class URProducer : public IProducer private: URStream& stream_; URParser& parser_; + std::chrono::seconds timeout_; public: - URProducer(URStream& stream, URParser& parser) : stream_(stream), parser_(parser) + URProducer(URStream& stream, URParser& parser) : stream_(stream), parser_(parser), timeout_(1) { } @@ -32,24 +34,29 @@ class URProducer : public IProducer { // 4KB should be enough to hold any packet received from UR uint8_t buf[4096]; - - // blocking call - ssize_t len = stream_.receive(buf, sizeof(buf)); - - // LOG_DEBUG("Read %d bytes from stream", len); - - if (len == 0) + size_t read = 0; + //expoential backoff reconnects + while(true) { - LOG_WARN("Read nothing from stream"); - return false; - } - else if (len < 0) - { - LOG_WARN("Stream closed"); - return false; + if(stream_.read(buf, sizeof(buf), read)) + { + //reset sleep amount + timeout_ = std::chrono::seconds(1); + break; + } + + if(stream_.closed()) + return false; + + LOG_WARN("Failed to read from stream, reconnecting in %ld seconds...", timeout_.count()); + std::this_thread::sleep_for(timeout_); + auto next = timeout_ * 2; + if(next <= std::chrono::seconds(120)) + timeout_ = next; } - BinParser bp(buf, static_cast(len)); + + BinParser bp(buf, read); return parser_.parse(bp, products); } }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/server.h b/include/ur_modern_driver/ur/server.h index 87408060c..dba5fe542 100644 --- a/include/ur_modern_driver/ur/server.h +++ b/include/ur_modern_driver/ur/server.h @@ -6,14 +6,25 @@ #include #include #include -#include "ur_modern_driver/ur/stream.h" +#include "ur_modern_driver/tcp_socket.h" -class URServer +class URServer : private TCPSocket { private: - int socket_fd_ = -1; + int port_; + SocketState state_; + TCPSocket client_; + +protected: + virtual bool open(int socket_fd, struct sockaddr *address, size_t address_len) + { + return ::bind(socket_fd, address, address_len) == 0; + } public: URServer(int port); - URStream accept(); + std::string getIP(); + bool bind(); + bool accept(); + bool write(const uint8_t* buf, size_t buf_len, size_t &written); }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/stream.h b/include/ur_modern_driver/ur/stream.h index 16719d719..e3bbb2388 100644 --- a/include/ur_modern_driver/ur/stream.h +++ b/include/ur_modern_driver/ur/stream.h @@ -5,56 +5,39 @@ #include #include #include +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/tcp_socket.h" -/// Encapsulates a TCP socket -class URStream +class URStream : private TCPSocket { private: - int socket_fd_ = -1; std::string host_; int port_; + std::mutex write_mutex_, read_mutex_; - std::atomic initialized_; - std::atomic stopping_; - std::mutex send_mutex_, receive_mutex_; - -public: - URStream() - { - } - - URStream(std::string& host, int port) : host_(host), port_(port), initialized_(false), stopping_(false) +protected: + virtual bool open(int socket_fd, struct sockaddr *address, size_t address_len) { + return ::connect(socket_fd, address, address_len) == 0; } - URStream(int socket_fd) : socket_fd_(socket_fd), initialized_(true), stopping_(false) - { - - } - - URStream(URStream&& other) noexcept : socket_fd_(other.socket_fd_), host_(other.host_), initialized_(other.initialized_.load()), stopping_(other.stopping_.load()) +public: + URStream(std::string& host, int port) : host_(host), port_(port) { - } - ~URStream() + bool connect() { - disconnect(); + return TCPSocket::setup(host_, port_); } - - URStream& operator=(URStream&& other) + void disconnect() { - socket_fd_ = std::move(other.socket_fd_); - host_ = std::move(other.host_); - initialized_ = std::move(other.initialized_.load()); - stopping_ = std::move(other.stopping_.load()); - return *this; + LOG_INFO("Disconnecting"); + TCPSocket::close(); } - bool connect(); - void disconnect(); - void reconnect(); + bool closed() { return getState() == SocketState::Closed; } - ssize_t send(const uint8_t* buf, size_t buf_len); - ssize_t receive(uint8_t* buf, size_t buf_len); + bool read(uint8_t* buf, size_t buf_len, size_t &read); + bool write(const uint8_t* buf, size_t buf_len, size_t &written); }; \ No newline at end of file diff --git a/src/ros/action_server.cpp b/src/ros/action_server.cpp index 3b6637dea..629a50f44 100644 --- a/src/ros/action_server.cpp +++ b/src/ros/action_server.cpp @@ -15,7 +15,14 @@ ActionServer::ActionServer(TrajectoryFollower& follower, std::vector lock(tj_mutex_); + Result res; + res.error_code = -100; + res.error_string = "Goal cancelled by client"; + gh.setCanceled(res); } bool ActionServer::validate(GoalHandle& gh, Result& res) @@ -125,9 +139,17 @@ bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res) } } + //todo validate start position? + return true; } +inline std::chrono::microseconds convert(const ros::Duration &dur) +{ + return std::chrono::duration_cast(std::chrono::seconds(dur.sec)) + + std::chrono::duration_cast(std::chrono::nanoseconds(dur.nsec)); +} + bool ActionServer::try_execute(GoalHandle& gh, Result& res) { if(!running_) @@ -137,27 +159,20 @@ bool ActionServer::try_execute(GoalHandle& gh, Result& res) } if(!tj_mutex_.try_lock()) { - has_goal_ = false; - //stop_trajectory(); + interrupt_traj_ = true; res.error_string = "Received another trajectory"; curr_gh_.setAborted(res, res.error_string); tj_mutex_.lock(); + //todo: make configurable + std::this_thread::sleep_for(std::chrono::milliseconds(250)); } //locked here curr_gh_ = gh; + interrupt_traj_ = false; has_goal_ = true; tj_mutex_.unlock(); tj_cv_.notify_one(); -} - -inline double ActionServer::interp_cubic(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel) -{ - using std::pow; - double a = p0_pos; - double b = p0_vel; - double c = (-3 * a + 3 * p1_pos - 2 * T * b - T * p1_vel) / pow(T, 2); - double d = (2 * a - 2 * p1_pos + T * b + T * p1_vel) / pow(T, 3); - return a + b * t + c * pow(t, 2) + d * pow(t, 3); + return true; } std::vector ActionServer::reorderMap(std::vector goal_joints) @@ -179,53 +194,56 @@ std::vector ActionServer::reorderMap(std::vector goal_joint void ActionServer::trajectoryThread() { + follower_.start(); //todo check error + //as_.start(); while(running_) { std::unique_lock lk(tj_mutex_); if(!tj_cv_.wait_for(lk, std::chrono::milliseconds(100), [&]{return running_ && has_goal_;})) continue; - auto g = curr_gh_.getGoal(); - auto const& traj = g->trajectory; - auto const& points = traj.points; - size_t len = points.size(); - auto const& last_point = points[points.size() - 1]; - double end_time = last_point.time_from_start.toSec(); - - auto mapping = reorderMap(traj.joint_names); - std::chrono::high_resolution_clock::time_point t0, t; - t = t0 = std::chrono::high_resolution_clock::now(); + LOG_DEBUG("Trajectory received and accepted"); + curr_gh_.setAccepted(); - size_t i = 0; - while(end_time >= toSec(t - t0) && has_goal_) + auto goal = curr_gh_.getGoal(); + auto mapping = reorderMap(goal->trajectory.joint_names); + std::vector trajectory(goal->trajectory.points.size()); + + for(auto const& point : goal->trajectory.points) { - while(points[i].time_from_start.toSec() <= toSec(t - t0) && i < len) - i++; - - auto const& pp = points[i-1]; - auto const& p = points[i]; - - auto pp_t = pp.time_from_start.toSec(); - auto p_t =p.time_from_start.toSec(); - - std::array pos; - for(size_t j = 0; j < pos.size(); j++) + std::array pos, vel; + for(size_t i = 0; i < 6; i++) { - pos[i] = interp_cubic( - toSec(t - t0) - pp_t, - p_t - pp_t, - pp.positions[j], - p.positions[j], - pp.velocities[j], - p.velocities[j] - ); + //joint names of the goal might have a different ordering compared + //to what URScript expects so need to map between the two + size_t idx = mapping[i]; + pos[idx] = point.positions[i]; + vel[idx] = point.velocities[i]; } + trajectory.push_back(TrajectoryPoint(pos, vel, convert(point.time_from_start))); + } - follower_.execute(pos); - //std::this_thread::sleep_for(std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.))); - t = std::chrono::high_resolution_clock::now(); + Result res; + if(follower_.execute(trajectory, interrupt_traj_)) + { + //interrupted goals must be handled by interrupt trigger + if(!interrupt_traj_) + { + LOG_DEBUG("Trajectory executed successfully"); + res.error_code = Result::SUCCESSFUL; + curr_gh_.setSucceeded(res); + } + } + else + { + LOG_DEBUG("Trajectory failed"); + res.error_code = -100; + res.error_string = "Connection to robot was lost"; + curr_gh_.setAborted(res, res.error_string); } has_goal_ = false; + lk.unlock(); } + follower_.stop(); } \ No newline at end of file diff --git a/src/ros/controller.cpp b/src/ros/controller.cpp index 753f28d30..79102fa2f 100644 --- a/src/ros/controller.cpp +++ b/src/ros/controller.cpp @@ -20,6 +20,8 @@ void ROSController::setupConsumer() void ROSController::doSwitch(const std::list& start_list, const std::list& stop_list) { + LOG_INFO("Switching hardware interface"); + if (active_interface_ != nullptr && stop_list.size() > 0) { LOG_INFO("Stopping active interface"); @@ -54,6 +56,14 @@ bool ROSController::write() return active_interface_->write(); } +void ROSController::reset() +{ + if (active_interface_ == nullptr) + return; + + active_interface_->reset(); +} + void ROSController::read(RTShared& packet) { joint_interface_.update(packet); @@ -68,17 +78,32 @@ bool ROSController::update(RTShared& state) lastUpdate_ = time; read(state); - controller_.update(time, diff); + controller_.update(time, diff, !service_enabled_); //emergency stop and such should not kill the pipeline //but still prevent writes if(!service_enabled_) + { + reset(); + return true; + } + + //allow the controller to update x times before allowing writes again + if(service_cooldown_ > 0) + { + service_cooldown_ -= 1; return true; + } return write(); } void ROSController::onRobotStateChange(RobotState state) { - service_enabled_ = (state == RobotState::Running); + bool next = (state == RobotState::Running); + if(next == service_enabled_) + return; + + service_enabled_ = next; + service_cooldown_ = 125; } \ No newline at end of file diff --git a/src/ros/hardware_interface.cpp b/src/ros/hardware_interface.cpp index 1bf0cc4aa..09a0798f7 100644 --- a/src/ros/hardware_interface.cpp +++ b/src/ros/hardware_interface.cpp @@ -1,61 +1,69 @@ #include "ur_modern_driver/ros/hardware_interface.h" +#include "ur_modern_driver/log.h" JointInterface::JointInterface(std::vector &joint_names) { - for (size_t i = 0; i < 6; i++) - { - registerHandle(hardware_interface::JointStateHandle(joint_names[i], &positions_[i], &velocities_[i], &efforts_[i])); - } + for (size_t i = 0; i < 6; i++) + { + registerHandle(hardware_interface::JointStateHandle(joint_names[i], &positions_[i], &velocities_[i], &efforts_[i])); + } } void JointInterface::update(RTShared &packet) { - positions_ = packet.q_actual; - velocities_ = packet.qd_actual; - efforts_ = packet.i_actual; + positions_ = packet.q_actual; + velocities_ = packet.qd_actual; + efforts_ = packet.i_actual; } WrenchInterface::WrenchInterface() { - registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", "", tcp_.begin(), tcp_.begin() + 3)); -} + registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", "", tcp_.begin(), tcp_.begin() + 3)); +} void WrenchInterface::update(RTShared &packet) { - tcp_ = packet.tcp_force; + tcp_ = packet.tcp_force; } VelocityInterface::VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names, double max_vel_change) - : commander_(commander), max_vel_change_(max_vel_change) + : commander_(commander), max_vel_change_(max_vel_change) { - for (size_t i = 0; i < 6; i++) - { - registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &velocity_cmd_[i])); - } + for (size_t i = 0; i < 6; i++) + { + registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &velocity_cmd_[i])); + } } bool VelocityInterface::write() { - for (size_t i = 0; i < 6; i++) - { - double prev = prev_velocity_cmd_[i]; - double lo = prev - max_vel_change_; - double hi = prev + max_vel_change_; - // clamp value to ±max_vel_change - prev_velocity_cmd_[i] = std::max(lo, std::min(velocity_cmd_[i], hi)); - } + for (size_t i = 0; i < 6; i++) + { + // clamp value to ±max_vel_change + double prev = prev_velocity_cmd_[i]; + double lo = prev - max_vel_change_; + double hi = prev + max_vel_change_; + prev_velocity_cmd_[i] = std::max(lo, std::min(velocity_cmd_[i], hi)); + } + return commander_.speedj(prev_velocity_cmd_, max_vel_change_); +} - return commander_.speedj(prev_velocity_cmd_, max_vel_change_); +void VelocityInterface::reset() +{ + for(auto &val : prev_velocity_cmd_) + { + val = 0; + } } PositionInterface:: PositionInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names) - : commander_(commander) + : commander_(commander) { - for (size_t i = 0; i < 6; i++) - { - registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &position_cmd_[i])); - } + for (size_t i = 0; i < 6; i++) + { + registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &position_cmd_[i])); + } } bool PositionInterface::write() diff --git a/src/ros/trajectory_follower.cpp b/src/ros/trajectory_follower.cpp index ca36e5914..53d966a03 100644 --- a/src/ros/trajectory_follower.cpp +++ b/src/ros/trajectory_follower.cpp @@ -1,83 +1,96 @@ #include #include "ur_modern_driver/ros/trajectory_follower.h" - - -TrajectoryFollower::TrajectoryFollower(URCommander &commander, int reverse_port, bool version_3) - : running_(false) - , commander_(commander) - , server_(reverse_port) - , program_(buildProgram(version_3)) -{ -} + + +static const int32_t MULT_JOINTSTATE_ = 1000000; static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}"); static const std::string SERVO_J_REPLACE("{{SERVO_J_REPLACE}}"); +static const std::string SERVER_IP_REPLACE("{{SERVER_IP_REPLACE}}"); +static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}"); static const std::string POSITION_PROGRAM = R"( def driverProg(): - MULT_jointstate = {{JOINT_STATE_REPLACE}} - - SERVO_IDLE = 0 - SERVO_RUNNING = 1 - cmd_servo_state = SERVO_IDLE - cmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - def set_servo_setpoint(q): - enter_critical - cmd_servo_state = SERVO_RUNNING - cmd_servo_q = q - exit_critical - end - thread servoThread(): - state = SERVO_IDLE - while True: - enter_critical - q = cmd_servo_q - do_brake = False - if (state == SERVO_RUNNING) and (cmd_servo_state == SERVO_IDLE): - do_brake = True - end - state = cmd_servo_state - cmd_servo_state = SERVO_IDLE - exit_critical - if do_brake: - stopj(1.0) - sync() - elif state == SERVO_RUNNING: - servoj(q, {{SERVO_J_REPLACE}}) - else: - sync() - end - end - end + MULT_jointstate = {{JOINT_STATE_REPLACE}} + + SERVO_IDLE = 0 + SERVO_RUNNING = 1 + cmd_servo_state = SERVO_IDLE + cmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + + def set_servo_setpoint(q): + enter_critical + cmd_servo_state = SERVO_RUNNING + cmd_servo_q = q + exit_critical + end + + thread servoThread(): + state = SERVO_IDLE + while True: + enter_critical + q = cmd_servo_q + do_brake = False + if (state == SERVO_RUNNING) and (cmd_servo_state == SERVO_IDLE): + do_brake = True + end + state = cmd_servo_state + cmd_servo_state = SERVO_IDLE + exit_critical + if do_brake: + stopj(1.0) + sync() + elif state == SERVO_RUNNING: + servoj(q, {{SERVO_J_REPLACE}}) + else: + sync() + end + end + end + + socket_open(\"{{SERVER_IP_REPLACE}}\", {{SERVER_PORT_REPLACE}}) thread_servo = run servoThread() keepalive = 1 while keepalive > 0: - params_mult = socket_read_binary_integer(6+1) - if params_mult[0] > 0: - q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate] - keepalive = params_mult[7] - set_servo_setpoint(q) - end + params_mult = socket_read_binary_integer(6+1) + if params_mult[0] > 0: + q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate] + keepalive = params_mult[7] + set_servo_setpoint(q) + end end sleep(.1) socket_close() kill thread_servo end )"; -std::string TrajectoryFollower::buildProgram(bool version_3) + +TrajectoryFollower::TrajectoryFollower(URCommander &commander, int reverse_port, bool version_3) + : running_(false) + , commander_(commander) + , reverse_port_(reverse_port) + , server_(reverse_port) { std::string res(POSITION_PROGRAM); - size_t js_idx = POSITION_PROGRAM.find(JOINT_STATE_REPLACE); - size_t sj_idx = POSITION_PROGRAM.find(SERVO_J_REPLACE); + res.replace(res.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_)); std::ostringstream out; out << "t=" << std::fixed << std::setprecision(4) << servoj_time_; - if(version_3) out << ", lookahead_time=" << servoj_lookahead_time_ << ", gain=" << servoj_gain_; - res.replace(js_idx, JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_)); - res.replace(sj_idx, SERVO_J_REPLACE.length(), out.str()); + res.replace(res.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str()); + + program_ = res; +} + +std::string TrajectoryFollower::buildProgram() +{ + std::string res(program_); + std::string IP(server_.getIP()); + LOG_INFO("Local IP: %s ", IP.c_str()); + res.replace(res.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), "127.0.0.1"); + res.replace(res.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port_)); return res; } @@ -86,12 +99,31 @@ bool TrajectoryFollower::start() if(running_) return true; //not sure - //TODO - std::string prog(""); // buildProg(); + if(!server_.bind()) + { + LOG_ERROR("Failed to bind server"); + return false; + } + + LOG_INFO("Uploading trajectory program to robot"); + + std::string prog(buildProgram()); + //std::string prog = "socket_open(\"127.0.0.1\", 50001)\n"; if(!commander_.uploadProg(prog)) + { + LOG_ERROR("Program upload failed!"); return false; + } - stream_ = std::move(server_.accept()); //todo: pointer instead? + LOG_INFO("Awaiting incomming robot connection"); + + if(!server_.accept()) + { + LOG_ERROR("Failed to accept incomming robot connection"); + return false; + } + + LOG_INFO("Robot successfully connected"); return (running_ = true); } @@ -115,8 +147,18 @@ bool TrajectoryFollower::execute(std::array &positions, bool keep_ali int32_t val = htobe32(static_cast(keep_alive)); append(idx, val); - ssize_t res = stream_.send(buf, sizeof(buf)); - return res > 0 && res == sizeof(buf); + size_t written; + return server_.write(buf, sizeof(buf), written); +} + +double TrajectoryFollower::interpolate(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel) +{ + using std::pow; + double a = p0_pos; + double b = p0_vel; + double c = (-3 * a + 3 * p1_pos - 2 * T * b - T * p1_vel) / pow(T, 2); + double d = (2 * a - 2 * p1_pos + T * b + T * p1_vel) / pow(T, 3); + return a + b * t + c * pow(t, 2) + d * pow(t, 3); } bool TrajectoryFollower::execute(std::array &positions) @@ -124,6 +166,68 @@ bool TrajectoryFollower::execute(std::array &positions) return execute(positions, true); } +bool TrajectoryFollower::execute(std::vector &trajectory, std::atomic &interrupt) +{ + if(!running_) + return false; + + using namespace std::chrono; + typedef duration double_seconds; + typedef high_resolution_clock Clock; + typedef Clock::time_point Time; + + auto const& last = trajectory[trajectory.size()-1]; + auto& prev = trajectory[0]; + + Time t0 = Clock::now(); + Time latest = t0; + + std::array positions; + + for(auto const& point : trajectory) + { + //skip t0 + if(&point == &prev) + continue; + + auto duration = point.time_from_start - prev.time_from_start; + double d_s = duration_cast(duration).count(); + + //interpolation loop + while(!interrupt) + { + latest = Clock::now(); + auto elapsed = latest - t0; + + if(point.time_from_start <= elapsed || last.time_from_start >= elapsed) + break; + + double elapsed_s = duration_cast(elapsed - prev.time_from_start).count(); + //double prev_seconds + for(size_t j = 0; j < positions.size(); j++) + { + positions[j] = interpolate( + elapsed_s, + d_s, + prev.positions[j], + point.positions[j], + prev.velocities[j], + point.velocities[j] + ); + } + + if(!execute(positions, true)) + return false; + + std::this_thread::sleep_for(double_seconds(servoj_time_)); + } + + prev = point; + } + + return true; +} + void TrajectoryFollower::stop() { if(!running_) @@ -132,6 +236,6 @@ void TrajectoryFollower::stop() std::array empty; execute(empty, false); - stream_.disconnect(); + //server_.disconnect(); running_ = false; } \ No newline at end of file diff --git a/src/ros_main.cpp b/src/ros_main.cpp index 7f7679291..efaf1066b 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -6,10 +6,12 @@ #include "ur_modern_driver/log.h" #include "ur_modern_driver/pipeline.h" +#include "ur_modern_driver/ros/action_server.h" +#include "ur_modern_driver/ros/controller.h" #include "ur_modern_driver/ros/io_service.h" #include "ur_modern_driver/ros/mb_publisher.h" -#include "ur_modern_driver/ros/controller.h" #include "ur_modern_driver/ros/rt_publisher.h" +#include "ur_modern_driver/ros/trajectory_follower.h" #include "ur_modern_driver/ros/service_stopper.h" #include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/factory.h" @@ -63,7 +65,7 @@ bool parse_args(ProgArgs &args) return true; } -#include "ur_modern_driver/event_counter.h" +#include "ur_modern_driver/ur/server.h" int main(int argc, char **argv) { @@ -75,6 +77,7 @@ int main(int argc, char **argv) return EXIT_FAILURE; } + URFactory factory(args.host); vector services; @@ -84,17 +87,27 @@ int main(int argc, char **argv) URStream rt_stream(args.host, UR_RT_PORT); URProducer rt_prod(rt_stream, *rt_parser); RTPublisher rt_pub(args.prefix, args.base_frame, args.tool_frame, args.use_ros_control); - URCommander rt_commander(rt_stream); + auto rt_commander = factory.getCommander(rt_stream); vector *> rt_vec{&rt_pub}; + TrajectoryFollower traj_follower(*rt_commander, args.reverse_port, factory.isVersion3()); + ROSController *controller(nullptr); + ActionServer *action_server(nullptr); if (args.use_ros_control) { LOG_INFO("ROS control enabled"); - controller = new ROSController(rt_commander, args.joint_names, args.max_vel_change); + controller = new ROSController(*rt_commander, args.joint_names, args.max_vel_change); rt_vec.push_back(controller); services.push_back(controller); } + else + { + LOG_INFO("ActionServer enabled"); + action_server = new ActionServer(traj_follower, args.joint_names, args.max_velocity); + //rt_vec.push_back(action_server); + services.push_back(action_server); + } MultiConsumer rt_cons(rt_vec); Pipeline rt_pl(rt_prod, rt_cons); @@ -116,8 +129,11 @@ int main(int argc, char **argv) rt_pl.run(); state_pl.run(); - URCommander state_commander(state_stream); - IOService io_service(state_commander); + auto state_commander = factory.getCommander(state_stream); + IOService io_service(*state_commander); + + if(action_server) + action_server->start(); ros::spin(); diff --git a/src/tcp_socket.cpp b/src/tcp_socket.cpp new file mode 100644 index 000000000..01685bffa --- /dev/null +++ b/src/tcp_socket.cpp @@ -0,0 +1,133 @@ +#include +#include +#include +#include + +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/tcp_socket.h" + +TCPSocket::TCPSocket() + : socket_fd_(-1) + , state_(SocketState::Invalid) +{ +} +TCPSocket::~TCPSocket() +{ + close(); +} + +bool TCPSocket::setup(std::string &host, int port) +{ + if(state_ == SocketState::Connected) + return false; + + LOG_INFO("Setting up connection: %s:%d", host.c_str(), port); + + // gethostbyname() is deprecated so use getadderinfo() as described in: + // http://www.beej.us/guide/bgnet/output/html/multipage/syscalls.html#getaddrinfo + + const char *host_name = host.empty() ? nullptr : host.c_str(); + std::string service = std::to_string(port); + struct addrinfo hints, *result; + std::memset(&hints, 0, sizeof(hints)); + + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_STREAM; + hints.ai_flags = AI_PASSIVE; + + if (getaddrinfo(host_name, service.c_str(), &hints, &result) != 0) + { + LOG_ERROR("Failed to get address for %s:%d", host.c_str(), port); + return false; + } + + bool connected = false; + // loop through the list of addresses untill we find one that's connectable + for (struct addrinfo* p = result; p != nullptr; p = p->ai_next) + { + socket_fd_ = ::socket(p->ai_family, p->ai_socktype, p->ai_protocol); + + if (socket_fd_ != -1 && open(socket_fd_, p->ai_addr, p->ai_addrlen)) + { + connected = true; + break; + } + } + + freeaddrinfo(result); + + if(!connected) + { + state_ = SocketState::Invalid; + LOG_ERROR("Connection setup failed for %s:%d", host.c_str(), port); + } + else + { + state_ = SocketState::Connected; + LOG_INFO("Connection established for %s:%d", host.c_str(), port); + } + return connected; +} + +bool TCPSocket::setSocketFD(int socket_fd) +{ + if(state_ == SocketState::Connected) + return false; + socket_fd_ = socket_fd; + state_ = SocketState::Connected; + return true; +} + +void TCPSocket::close() +{ + if(state_ != SocketState::Connected) + return; + state_ = SocketState::Closed; + ::shutdown(socket_fd_, SHUT_RDWR); + socket_fd_ = -1; +} + +bool TCPSocket::read(uint8_t *buf, size_t buf_len, size_t &read) +{ + read = 0; + + if(state_ != SocketState::Connected) + return false; + + ssize_t res = ::recv(socket_fd_, buf, buf_len, 0); + + if(res == 0) + { + state_ = SocketState::Disconnected; + return false; + } + else if(res < 0) + return false; + + read = static_cast(res); + return true; +} + +bool TCPSocket::write(const uint8_t *buf, size_t buf_len, size_t &written) +{ + written = 0; + + if(state_ != SocketState::Connected) + return false; + + size_t remaining = buf_len; + + // handle partial sends + while (written < buf_len) + { + ssize_t sent = ::send(socket_fd_, buf + written, remaining, 0); + + if (sent <= 0) + return false; + + written += sent; + remaining -= sent; + } + + return true; +} \ No newline at end of file diff --git a/src/ur/commander.cpp b/src/ur/commander.cpp index 333bc5265..0fd358e7b 100644 --- a/src/ur/commander.cpp +++ b/src/ur/commander.cpp @@ -3,10 +3,21 @@ bool URCommander::write(std::string& s) { - size_t len = s.size(); - const uint8_t* data = reinterpret_cast(s.c_str()); - ssize_t res = stream_.send(data, len); - return res > 0 && static_cast(res) == len; + size_t len = s.size(); + const uint8_t* data = reinterpret_cast(s.c_str()); + size_t written; + return stream_.write(data, len, written); +} + +void URCommander::formatArray(std::ostringstream &out, std::array &values) +{ + std::string mod("["); + for(auto const& val : values) + { + out << mod << val; + mod = ","; + } + out << "]"; } bool URCommander::uploadProg(std::string &s) @@ -14,59 +25,110 @@ bool URCommander::uploadProg(std::string &s) return write(s); } -bool URCommander::speedj(std::array &speeds, double acceleration) +bool URCommander::setToolVoltage(uint8_t voltage) { - std::ostringstream out; - out << std::fixed << std::setprecision(4); - out << "speedj(["; - std::string mod; - for(auto const& val : speeds) - { - out << mod << val; - mod = ","; - } - out << "]," << acceleration << ")\n"; - std::string s(out.str()); - return write(s); + if(voltage != 0 || voltage != 12 || voltage != 24) + return false; + + std::ostringstream out; + out << "set_tool_voltage(" << (int)voltage << ")\n"; + std::string s(out.str()); + return write(s); } -bool URCommander::stopj(double a) -{ +bool URCommander::setFlag(uint8_t pin, bool value) +{ + std::ostringstream out; + out << "set_flag(" << (int)pin << "," << (value ? "True" : "False") << ")\n"; + std::string s(out.str()); + return write(s); +} +bool URCommander::setPayload(double value) +{ + std::ostringstream out; + out << "set_payload(" << std::fixed << std::setprecision(5) << value << ")\n"; + std::string s(out.str()); + return write(s); } -bool URCommander::setAnalogOut(uint8_t pin, double value) +bool URCommander::stopj(double a) { - std::ostringstream out; - out << "set_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(4) << value << ")\n"; - std::string s(out.str()); - return write(s); + std::ostringstream out; + out << "stopj(" << std::fixed << std::setprecision(5) << a << ")\n"; + std::string s(out.str()); + return write(s); } -bool URCommander::setDigitalOut(uint8_t pin, bool value) + +bool URCommander_V1_X::speedj(std::array &speeds, double acceleration) { - std::ostringstream out; - out << "set_digital_out(" << (int)pin << "," << (value ? "True" : "False") << ")\n"; - std::string s(out.str()); - return write(s); + std::ostringstream out; + out << std::fixed << std::setprecision(5); + out << "speedj("; + formatArray(out, speeds); + out << "," << acceleration << "," << 0.02 << ")\n"; + std::string s(out.str()); + return write(s); +} +bool URCommander_V1_X::setAnalogOut(uint8_t pin, double value) +{ + std::ostringstream out; + out << "set_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(4) << value << ")\n"; + std::string s(out.str()); + return write(s); } -bool URCommander::setToolVoltage(uint8_t voltage) +bool URCommander_V1_X::setDigitalOut(uint8_t pin, bool value) { - + std::ostringstream out; + out << "set_digital_out(" << (int)pin << "," << (value ? "True" : "False") << ")\n"; + std::string s(out.str()); + return write(s); } -bool URCommander::setFlag(uint8_t pin, bool value) +bool URCommander_V3_X::speedj(std::array &speeds, double acceleration) { - std::ostringstream out; - out << "set_flag(" << (int)pin << "," << (value ? "True" : "False") << ")\n"; - std::string s(out.str()); - return write(s); + std::ostringstream out; + out << std::fixed << std::setprecision(5); + out << "speedj("; + formatArray(out, speeds); + out << "," << acceleration << ")\n"; + std::string s(out.str()); + return write(s); } -bool URCommander::setPayload(double value) +bool URCommander_V3_X::setAnalogOut(uint8_t pin, double value) { - std::ostringstream out; - out << "set_payload(" << std::fixed << std::setprecision(4) << value << ")\n"; - std::string s(out.str()); - return write(s); -} \ No newline at end of file + std::ostringstream out; + out << "set_standard_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(5) << value << ")\n"; + std::string s(out.str()); + return write(s); +} + +bool URCommander_V3_X::setDigitalOut(uint8_t pin, bool value) +{ + std::ostringstream out; + std::string func; + + if(pin < 8) + { + func = "set_standard_digital_out"; + } + else if(pin < 16) + { + func = "set_configurable_digital_out"; + pin -= 8; + } + else if(pin < 18) + { + func = "set_tool_digital_out"; + pin -= 16; + } + else + return false; + + + out << func << "(" << (int)pin << "," << (value ? "True" : "False") << ")\n"; + std::string s(out.str()); + return write(s); +} diff --git a/src/ur/server.cpp b/src/ur/server.cpp index d99f1a91d..6c99e138e 100644 --- a/src/ur/server.cpp +++ b/src/ur/server.cpp @@ -5,47 +5,48 @@ #include "ur_modern_driver/ur/server.h" URServer::URServer(int port) + : port_(port) { - std::string service = std::to_string(port); - struct addrinfo hints, *result; - std::memset(&hints, 0, sizeof(hints)); - - hints.ai_family = AF_UNSPEC; - hints.ai_socktype = SOCK_STREAM; - hints.ai_flags = AI_PASSIVE; - - if (getaddrinfo(nullptr, service.c_str(), &hints, &result) != 0) - { - LOG_ERROR("Failed to setup recieving server"); - return; - } - - // loop through the list of addresses untill we find one that's connectable - for (struct addrinfo* p = result; p != nullptr; p = p->ai_next) - { - socket_fd_ = socket(p->ai_family, p->ai_socktype, p->ai_protocol); - - if (socket_fd_ == -1) // socket error? - continue; - - if (bind(socket_fd_, p->ai_addr, p->ai_addrlen) != 0) - continue; - - // disable Nagle's algorithm to ensure we sent packets as fast as possible - int flag = 1; - setsockopt(socket_fd_, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(flag)); - LOG_INFO("Server awaiting connection"); - return; - } +} + +std::string URServer::getIP() +{ + char buf[128]; + int res = ::gethostname(buf, sizeof(buf)); + return std::string(buf); +} + +bool URServer::bind() +{ + std::string empty; + bool res = TCPSocket::setup(empty, port_); + state_ = TCPSocket::getState(); - LOG_ERROR("Failed to setup recieving server"); - std::exit(EXIT_FAILURE); + if(!res) + return false; + + if(::listen(getSocketFD(), 1) < 0) + return false; + + return true; } -URStream URServer::accept() +bool URServer::accept() { + if(state_ != SocketState::Connected || client_.getSocketFD() > 0) + return false; + struct sockaddr addr; socklen_t addr_len; - int client_fd = ::accept(socket_fd_, &addr, &addr_len); - return URStream(client_fd); + int client_fd = ::accept(getSocketFD(), &addr, &addr_len); + + if(client_fd <= 0) + return false; + + return client_.setSocketFD(client_fd); +} + +bool URServer::write(const uint8_t* buf, size_t buf_len, size_t &written) +{ + return client_.write(buf, buf_len, written); } \ No newline at end of file diff --git a/src/ur/stream.cpp b/src/ur/stream.cpp index 240b261c6..05617c55d 100644 --- a/src/ur/stream.cpp +++ b/src/ur/stream.cpp @@ -6,139 +6,38 @@ #include "ur_modern_driver/log.h" #include "ur_modern_driver/ur/stream.h" -bool URStream::connect() +bool URStream::write(const uint8_t* buf, size_t buf_len, size_t &written) { - if (initialized_) - return false; - - LOG_INFO("Connecting to UR @ %s:%d", host_.c_str(), port_); - - // gethostbyname() is deprecated so use getadderinfo() as described in: - // http://www.beej.us/guide/bgnet/output/html/multipage/syscalls.html#getaddrinfo - - std::string service = std::to_string(port_); - struct addrinfo hints, *result; - std::memset(&hints, 0, sizeof(hints)); - - hints.ai_family = AF_UNSPEC; - hints.ai_socktype = SOCK_STREAM; - hints.ai_flags = AI_PASSIVE; - - if (getaddrinfo(host_.c_str(), service.c_str(), &hints, &result) != 0) - { - LOG_ERROR("Failed to get host name"); - return false; - } - - // loop through the list of addresses untill we find one that's connectable - for (struct addrinfo* p = result; p != nullptr; p = p->ai_next) - { - socket_fd_ = socket(p->ai_family, p->ai_socktype, p->ai_protocol); - - if (socket_fd_ == -1) // socket error? - continue; - - if (::connect(socket_fd_, p->ai_addr, p->ai_addrlen) != 0) - { - if (stopping_) - break; - else - continue; // try next addrinfo if connect fails - } - - // disable Nagle's algorithm to ensure we sent packets as fast as possible - int flag = 1; - setsockopt(socket_fd_, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(flag)); - initialized_ = true; - LOG_INFO("Connection successfully established"); - break; - } - - freeaddrinfo(result); - if (!initialized_) - LOG_ERROR("Connection failed"); - - return initialized_; -} - -void URStream::disconnect() -{ - if (!initialized_ || stopping_) - return; - - LOG_INFO("Disconnecting from %s:%d", host_.c_str(), port_); - - stopping_ = true; - close(socket_fd_); - initialized_ = false; -} - -void URStream::reconnect() -{ - disconnect(); - stopping_ = false; - connect(); + std::lock_guard lock(write_mutex_); + return TCPSocket::write(buf, buf_len, written); } -ssize_t URStream::send(const uint8_t* buf, size_t buf_len) +bool URStream::read(uint8_t* buf, size_t buf_len, size_t &total) { - if (!initialized_) - return -1; - if (stopping_) - return 0; - - std::lock_guard lock(send_mutex_); - - size_t total = 0; - size_t remaining = 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) - return stopping_ ? 0 : sent; - total += sent; - remaining -= sent; - } - - return total; -} + std::lock_guard lock(read_mutex_); -ssize_t URStream::receive(uint8_t* buf, size_t buf_len) -{ - if (!initialized_) - return -1; - if (stopping_) - return 0; - - std::lock_guard lock(receive_mutex_); - - size_t remainder = sizeof(int32_t); - uint8_t* buf_pos = buf; bool initial = true; + uint8_t* buf_pos = buf; + size_t remainder = sizeof(int32_t); + size_t read = 0; - do + while(remainder > 0 && TCPSocket::read(buf_pos, remainder, read)) { - ssize_t read = recv(socket_fd_, buf_pos, remainder, 0); - if (read <= 0) // failed reading from socket - return stopping_ ? 0 : read; - if (initial) { remainder = be32toh(*(reinterpret_cast(buf))); if (remainder >= (buf_len - sizeof(int32_t))) { LOG_ERROR("Packet size %zd is larger than buffer %zu, discarding.", remainder, buf_len); - return -1; + return false; } initial = false; } + total += read; buf_pos += read; remainder -= read; - } while (remainder > 0); - - return buf_pos - buf; + } + + return remainder == 0; } \ No newline at end of file From 8d845c6f38a60807dac1a554ed569f471251a9e4 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Fri, 19 May 2017 01:20:30 +0200 Subject: [PATCH 047/114] Action server improvements --- include/ur_modern_driver/ros/action_server.h | 29 +++--- src/ros/action_server.cpp | 100 +++++++++++++++++-- 2 files changed, 109 insertions(+), 20 deletions(-) diff --git a/include/ur_modern_driver/ros/action_server.h b/include/ur_modern_driver/ros/action_server.h index 398e4d585..bf876dccb 100644 --- a/include/ur_modern_driver/ros/action_server.h +++ b/include/ur_modern_driver/ros/action_server.h @@ -1,40 +1,40 @@ #pragma once +#include +#include +#include +#include #include #include #include #include #include #include -#include -#include -#include -#include #include "ur_modern_driver/log.h" +#include "ur_modern_driver/ros/service_stopper.h" +#include "ur_modern_driver/ros/trajectory_follower.h" #include "ur_modern_driver/ur/consumer.h" #include "ur_modern_driver/ur/master_board.h" #include "ur_modern_driver/ur/state.h" -#include "ur_modern_driver/ros/service_stopper.h" -#include "ur_modern_driver/ros/trajectory_follower.h" - -class ActionServer : public Service //,public URRTPacketConsumer +class ActionServer : public Service, public IConsumer { private: typedef control_msgs::FollowJointTrajectoryAction Action; typedef control_msgs::FollowJointTrajectoryResult Result; typedef actionlib::ServerGoalHandle GoalHandle; typedef actionlib::ActionServer Server; - + ros::NodeHandle nh_; Server as_; std::vector joint_names_; std::set joint_set_; double max_velocity_; + RobotState state_; + std::array q_actual_, qd_actual_; - GoalHandle curr_gh_; std::atomic interrupt_traj_; std::atomic has_goal_, running_; @@ -48,7 +48,7 @@ class ActionServer : public Service //,public URRTPacketConsumer void onCancel(GoalHandle gh); bool validate(GoalHandle& gh, Result& res); - bool validateState(GoalHandle& gh, Result& res); + bool validateState(GoalHandle& gh, Result& res); bool validateJoints(GoalHandle& gh, Result& res); bool validateTrajectory(GoalHandle& gh, Result& res); @@ -58,11 +58,16 @@ class ActionServer : public Service //,public URRTPacketConsumer std::vector reorderMap(std::vector goal_joints); void trajectoryThread(); - + bool updateState(RTShared& data); public: ActionServer(TrajectoryFollower& follower, std::vector& joint_names, double max_velocity); void start(); virtual void onRobotStateChange(RobotState 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); }; \ No newline at end of file diff --git a/src/ros/action_server.cpp b/src/ros/action_server.cpp index 629a50f44..1ec7217f7 100644 --- a/src/ros/action_server.cpp +++ b/src/ros/action_server.cpp @@ -12,6 +12,9 @@ ActionServer::ActionServer(TrajectoryFollower& follower, std::vector lock(tj_mutex_); + + Result res; + res.error_code = -100; + res.error_string = "Received another trajectory"; + curr_gh_.setAborted(res, res.error_string); +} + + +bool ActionServer::updateState(RTShared& data) +{ + q_actual_ = data.q_actual; + qd_actual_ = data.qd_actual; + return true; +} + + +bool ActionServer::consume(RTState_V1_6__7& state) +{ + return updateState(state); +} +bool ActionServer::consume(RTState_V1_8& state) +{ + return updateState(state); +} +bool ActionServer::consume(RTState_V3_0__1& state) +{ + return updateState(state); } +bool ActionServer::consume(RTState_V3_2__3& state) +{ + return updateState(state); +} + void ActionServer::onGoal(GoalHandle gh) { Result res; res.error_code = -100; + LOG_INFO("Received new goal"); + if(!validate(gh, res) || !try_execute(gh, res)) + { + LOG_WARN("Goal error: %s", res.error_string.c_str()); gh.setRejected(res, res.error_string); + } } void ActionServer::onCancel(GoalHandle gh) @@ -53,7 +115,7 @@ void ActionServer::onCancel(GoalHandle gh) bool ActionServer::validate(GoalHandle& gh, Result& res) { - return !validateState(gh, res) || !validateJoints(gh, res) || !validateTrajectory(gh, res); + return validateState(gh, res) && validateJoints(gh, res) && validateTrajectory(gh, res); } bool ActionServer::validateState(GoalHandle& gh, Result& res) @@ -100,6 +162,10 @@ bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res) auto goal = gh.getGoal(); res.error_code = Result::INVALID_GOAL; + //must at least have one point + if(goal->trajectory.points.size() < 1) + return false; + for(auto const& point : goal->trajectory.points) { if(point.velocities.size() != joint_names_.size()) @@ -194,28 +260,41 @@ std::vector ActionServer::reorderMap(std::vector goal_joint void ActionServer::trajectoryThread() { + LOG_INFO("Trajectory thread started"); follower_.start(); //todo check error - //as_.start(); while(running_) { std::unique_lock lk(tj_mutex_); if(!tj_cv_.wait_for(lk, std::chrono::milliseconds(100), [&]{return running_ && has_goal_;})) continue; - LOG_DEBUG("Trajectory received and accepted"); + LOG_INFO("Trajectory received and accepted"); curr_gh_.setAccepted(); auto goal = curr_gh_.getGoal(); - auto mapping = reorderMap(goal->trajectory.joint_names); std::vector trajectory(goal->trajectory.points.size()); + + //joint names of the goal might have a different ordering compared + //to what URScript expects so need to map between the two + auto mapping = reorderMap(goal->trajectory.joint_names); + LOG_INFO("Translating trajectory"); + + auto const& fp = goal->trajectory.points[0]; + auto fpt = convert(fp.time_from_start) + + //make sure we have a proper t0 position + if(fpt > std::chrono::microseconds(0)) + { + LOG_INFO("Trajectory without t0 recieved, inserting t0 at currrent position"); + trajectory.push_back(TrajectoryPoint(q_actual_, qd_actual_, std::chrono::microseconds(0))); + } + for(auto const& point : goal->trajectory.points) { std::array pos, vel; for(size_t i = 0; i < 6; i++) { - //joint names of the goal might have a different ordering compared - //to what URScript expects so need to map between the two size_t idx = mapping[i]; pos[idx] = point.positions[i]; vel[idx] = point.velocities[i]; @@ -223,20 +302,25 @@ void ActionServer::trajectoryThread() trajectory.push_back(TrajectoryPoint(pos, vel, convert(point.time_from_start))); } + double t = std::chrono::duration_cast>(trajectory[trajectory.size()-1].time_from_start).count(); + LOG_INFO("Trajectory with %d points and duration of %f constructed, executing now", trajectory.size(), t); + Result res; if(follower_.execute(trajectory, interrupt_traj_)) { //interrupted goals must be handled by interrupt trigger if(!interrupt_traj_) { - LOG_DEBUG("Trajectory executed successfully"); + LOG_INFO("Trajectory executed successfully"); res.error_code = Result::SUCCESSFUL; curr_gh_.setSucceeded(res); } + else + LOG_INFO("Trajectory interrupted"); } else { - LOG_DEBUG("Trajectory failed"); + LOG_INFO("Trajectory failed"); res.error_code = -100; res.error_string = "Connection to robot was lost"; curr_gh_.setAborted(res, res.error_string); From 0c23d0bf3d12d94027e876e6a4b648797ab27a94 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Fri, 19 May 2017 01:22:42 +0200 Subject: [PATCH 048/114] Added set options function for sockets --- include/ur_modern_driver/tcp_socket.h | 1 + include/ur_modern_driver/ur/server.h | 2 ++ include/ur_modern_driver/ur/stream.h | 2 +- src/tcp_socket.cpp | 7 +++++++ src/ur/server.cpp | 8 ++++++++ 5 files changed, 19 insertions(+), 1 deletion(-) diff --git a/include/ur_modern_driver/tcp_socket.h b/include/ur_modern_driver/tcp_socket.h index 303d49e99..982242486 100644 --- a/include/ur_modern_driver/tcp_socket.h +++ b/include/ur_modern_driver/tcp_socket.h @@ -25,6 +25,7 @@ class TCPSocket { return false; } + virtual void setOptions(int socket_fd); bool setup(std::string &host, int port); diff --git a/include/ur_modern_driver/ur/server.h b/include/ur_modern_driver/ur/server.h index dba5fe542..c5c841253 100644 --- a/include/ur_modern_driver/ur/server.h +++ b/include/ur_modern_driver/ur/server.h @@ -20,6 +20,8 @@ class URServer : private TCPSocket { return ::bind(socket_fd, address, address_len) == 0; } + virtual void setOptions(int socket_fd); + public: URServer(int port); diff --git a/include/ur_modern_driver/ur/stream.h b/include/ur_modern_driver/ur/stream.h index e3bbb2388..60619a87a 100644 --- a/include/ur_modern_driver/ur/stream.h +++ b/include/ur_modern_driver/ur/stream.h @@ -32,7 +32,7 @@ class URStream : private TCPSocket } void disconnect() { - LOG_INFO("Disconnecting"); + LOG_INFO("Disconnecting from %s:%d", host_.c_str(), port_); TCPSocket::close(); } diff --git a/src/tcp_socket.cpp b/src/tcp_socket.cpp index 01685bffa..f0049ec4f 100644 --- a/src/tcp_socket.cpp +++ b/src/tcp_socket.cpp @@ -16,6 +16,12 @@ TCPSocket::~TCPSocket() close(); } +void TCPSocket::setOptions(int socket_fd) +{ + int flag = 1; + setsockopt(socket_fd, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(int)); +} + bool TCPSocket::setup(std::string &host, int port) { if(state_ == SocketState::Connected) @@ -63,6 +69,7 @@ bool TCPSocket::setup(std::string &host, int port) } else { + setOptions(socket_fd_); state_ = SocketState::Connected; LOG_INFO("Connection established for %s:%d", host.c_str(), port); } diff --git a/src/ur/server.cpp b/src/ur/server.cpp index 6c99e138e..fd929fed5 100644 --- a/src/ur/server.cpp +++ b/src/ur/server.cpp @@ -9,6 +9,14 @@ URServer::URServer(int port) { } +void URServer::setOptions(int socket_fd) +{ + TCPSocket::setOptions(socket_fd); + + int flag = 1; + setsockopt(socket_fd, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int)); +} + std::string URServer::getIP() { char buf[128]; From ffe0ac170c7b138cd6df7b3f5885cc9a24b64c1e Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Fri, 19 May 2017 01:26:22 +0200 Subject: [PATCH 049/114] Fixed invalid inheritance on ActionServer --- include/ur_modern_driver/ros/action_server.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ur_modern_driver/ros/action_server.h b/include/ur_modern_driver/ros/action_server.h index bf876dccb..f18c6e9ba 100644 --- a/include/ur_modern_driver/ros/action_server.h +++ b/include/ur_modern_driver/ros/action_server.h @@ -17,7 +17,7 @@ #include "ur_modern_driver/ur/master_board.h" #include "ur_modern_driver/ur/state.h" -class ActionServer : public Service, public IConsumer +class ActionServer : public Service, public URRTPacketConsumer { private: typedef control_msgs::FollowJointTrajectoryAction Action; From 502506e7fc09d8051bc12588ee020bae015bc604 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Fri, 19 May 2017 01:28:46 +0200 Subject: [PATCH 050/114] Main --- src/ros_main.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/ros_main.cpp b/src/ros_main.cpp index efaf1066b..38634c9b6 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -30,6 +30,15 @@ static const std::string BASE_FRAME_ARG("~base_frame"); static const std::string TOOL_FRAME_ARG("~tool_frame"); static const std::string JOINT_NAMES_PARAM("hardware_interface/joints"); +static const std::vector DEFAULT_JOINTS = { + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint" +}; + static const int UR_SECONDARY_PORT = 30002; static const int UR_RT_PORT = 30003; @@ -61,7 +70,7 @@ bool parse_args(ProgArgs &args) ros::param::param(PREFIX_ARG, args.prefix, std::string()); ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link"); ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller"); - ros::param::get(JOINT_NAMES_PARAM, args.joint_names); + ros::param::param(JOINT_NAMES_PARAM, args.joint_names, DEFAULT_JOINTS); return true; } @@ -105,7 +114,7 @@ int main(int argc, char **argv) { LOG_INFO("ActionServer enabled"); action_server = new ActionServer(traj_follower, args.joint_names, args.max_velocity); - //rt_vec.push_back(action_server); + rt_vec.push_back(action_server); services.push_back(action_server); } From 0479c047d3a7510c8dfb62673b450b774ba683a6 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Fri, 19 May 2017 01:29:22 +0200 Subject: [PATCH 051/114] Trjaectory follower improvements --- .../ros/trajectory_follower.h | 1 + src/ros/trajectory_follower.cpp | 28 +++++++++++++++++-- 2 files changed, 27 insertions(+), 2 deletions(-) diff --git a/include/ur_modern_driver/ros/trajectory_follower.h b/include/ur_modern_driver/ros/trajectory_follower.h index a16c3e52d..74588af2e 100644 --- a/include/ur_modern_driver/ros/trajectory_follower.h +++ b/include/ur_modern_driver/ros/trajectory_follower.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include diff --git a/src/ros/trajectory_follower.cpp b/src/ros/trajectory_follower.cpp index 53d966a03..c6752b10e 100644 --- a/src/ros/trajectory_follower.cpp +++ b/src/ros/trajectory_follower.cpp @@ -1,3 +1,4 @@ +#include #include #include "ur_modern_driver/ros/trajectory_follower.h" @@ -46,7 +47,7 @@ def driverProg(): end end - socket_open(\"{{SERVER_IP_REPLACE}}\", {{SERVER_PORT_REPLACE}}) + socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}) thread_servo = run servoThread() keepalive = 1 @@ -68,6 +69,9 @@ TrajectoryFollower::TrajectoryFollower(URCommander &commander, int reverse_port, : running_(false) , commander_(commander) , reverse_port_(reverse_port) + , servoj_time_(0.016/4) + , servoj_lookahead_time_(0.03) + , servoj_gain_(300.) , server_(reverse_port) { std::string res(POSITION_PROGRAM); @@ -132,6 +136,8 @@ bool TrajectoryFollower::execute(std::array &positions, bool keep_ali if(!running_) return false; + // LOG_INFO("servoj([%f,%f,%f,%f,%f,%f])", positions[0], positions[1], positions[2], positions[3], positions[4], positions[5]); + last_positions_ = positions; uint8_t buf[sizeof(uint32_t)*7]; @@ -190,18 +196,26 @@ bool TrajectoryFollower::execute(std::vector &trajectory, std:: if(&point == &prev) continue; + if(interrupt) + break; + auto duration = point.time_from_start - prev.time_from_start; double d_s = duration_cast(duration).count(); + LOG_INFO("Point duration: %f", d_s); + //interpolation loop while(!interrupt) { latest = Clock::now(); auto elapsed = latest - t0; - if(point.time_from_start <= elapsed || last.time_from_start >= elapsed) + if(point.time_from_start <= elapsed) break; + if(last.time_from_start <= elapsed) + return true; + double elapsed_s = duration_cast(elapsed - prev.time_from_start).count(); //double prev_seconds for(size_t j = 0; j < positions.size(); j++) @@ -225,6 +239,16 @@ bool TrajectoryFollower::execute(std::vector &trajectory, std:: prev = point; } + //In theory it's possible the last position won't be sent by + //the interpolation loop above but rather some position between + //t[N-1] and t[N] where N is the number of trajectory points. + //To make sure this does not happen the last position is sent + //if not interrupted. + if(!interrupt) + { + return execute(last.positions, true); + } + return true; } From f35f40b45d1163344968531aabca1ce539aa35db Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Fri, 7 Jul 2017 15:26:39 +0200 Subject: [PATCH 052/114] WIP --- include/ur_modern_driver/bin_parser.h | 2 +- .../ros/trajectory_follower.h | 3 +- include/ur_modern_driver/tcp_socket.h | 6 ++- include/ur_modern_driver/ur/producer.h | 4 ++ include/ur_modern_driver/ur/server.h | 2 +- include/ur_modern_driver/ur/state_parser.h | 8 +-- include/ur_modern_driver/ur/stream.h | 2 +- src/ros/action_server.cpp | 54 ++++++++++++------- src/ros/trajectory_follower.cpp | 50 ++++++----------- src/ros_main.cpp | 12 +++-- src/tcp_socket.cpp | 18 +++++++ src/ur/server.cpp | 24 +++++++-- 12 files changed, 114 insertions(+), 71 deletions(-) diff --git a/include/ur_modern_driver/bin_parser.h b/include/ur_modern_driver/bin_parser.h index dbf6df55f..2cfce4595 100644 --- a/include/ur_modern_driver/bin_parser.h +++ b/include/ur_modern_driver/bin_parser.h @@ -68,7 +68,7 @@ class BinParser template T peek() { - assert(buf_pos_ <= buf_end_); + assert(buf_pos_ + sizeof(T) <= buf_end_); T val; std::memcpy(&val, buf_pos_, sizeof(T)); return decode(val); diff --git a/include/ur_modern_driver/ros/trajectory_follower.h b/include/ur_modern_driver/ros/trajectory_follower.h index 74588af2e..be59a5641 100644 --- a/include/ur_modern_driver/ros/trajectory_follower.h +++ b/include/ur_modern_driver/ros/trajectory_follower.h @@ -49,12 +49,11 @@ class TrajectoryFollower return s; } - std::string buildProgram(); bool execute(std::array &positions, bool keep_alive); double interpolate(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel); public: - TrajectoryFollower(URCommander &commander, int reverse_port, bool version_3); + TrajectoryFollower(URCommander &commander, std::string& reverse_ip, int reverse_port, bool version_3); bool start(); bool execute(std::array &positions); diff --git a/include/ur_modern_driver/tcp_socket.h b/include/ur_modern_driver/tcp_socket.h index 982242486..4230ac095 100644 --- a/include/ur_modern_driver/tcp_socket.h +++ b/include/ur_modern_driver/tcp_socket.h @@ -29,8 +29,6 @@ class TCPSocket bool setup(std::string &host, int port); - void close(); - public: TCPSocket(); @@ -41,6 +39,10 @@ class TCPSocket int getSocketFD() { return socket_fd_; } bool setSocketFD(int socket_fd); + std::string getIP(); + bool read(uint8_t* buf, size_t buf_len, size_t &read); bool write(const uint8_t* buf, size_t buf_len, size_t &written); + + void close(); }; diff --git a/include/ur_modern_driver/ur/producer.h b/include/ur_modern_driver/ur/producer.h index b3bb475bb..df4a61a8e 100644 --- a/include/ur_modern_driver/ur/producer.h +++ b/include/ur_modern_driver/ur/producer.h @@ -50,6 +50,10 @@ class URProducer : public IProducer LOG_WARN("Failed to read from stream, reconnecting in %ld seconds...", timeout_.count()); std::this_thread::sleep_for(timeout_); + + if(stream_.connect()) + continue; + auto next = timeout_ * 2; if(next <= std::chrono::seconds(120)) timeout_ = next; diff --git a/include/ur_modern_driver/ur/server.h b/include/ur_modern_driver/ur/server.h index c5c841253..daff4d35b 100644 --- a/include/ur_modern_driver/ur/server.h +++ b/include/ur_modern_driver/ur/server.h @@ -12,7 +12,6 @@ class URServer : private TCPSocket { private: int port_; - SocketState state_; TCPSocket client_; protected: @@ -28,5 +27,6 @@ class URServer : private TCPSocket std::string getIP(); bool bind(); bool accept(); + void disconnectClient(); bool write(const uint8_t* buf, size_t buf_len, size_t &written); }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/state_parser.h b/include/ur_modern_driver/ur/state_parser.h index 1e9331434..494558f60 100644 --- a/include/ur_modern_driver/ur/state_parser.h +++ b/include/ur_modern_driver/ur/state_parser.h @@ -34,7 +34,7 @@ class URStateParser : public URParser bp.parse(type); //quietly ignore the intial version message - if (type == message_type::ROBOT_MESSAGE) + if (type == message_type::ROBOT_MESSAGE || ((int)type) == 5) { bp.consume(); return true; @@ -42,7 +42,7 @@ class URStateParser : public URParser if (type != message_type::ROBOT_STATE) { - LOG_WARN("Invalid message type recieved: %u", static_cast(type)); + LOG_WARN("Invalid state message type recieved: %u", static_cast(type)); return false; } @@ -60,7 +60,7 @@ class URStateParser : public URParser return false; } - LOG_DEBUG("sub-packet size: %" PRIu32, sub_size); + //LOG_DEBUG("sub-packet size: %" PRIu32, sub_size); // deconstruction of a sub parser will increment the position of the parent parser BinParser sbp(bp, sub_size); @@ -73,7 +73,7 @@ class URStateParser : public URParser if (packet == nullptr) { sbp.consume(); - LOG_DEBUG("Skipping sub-packet of type %d", type); + //LOG_DEBUG("Skipping sub-packet of type %d", type); continue; } diff --git a/include/ur_modern_driver/ur/stream.h b/include/ur_modern_driver/ur/stream.h index 60619a87a..3d5bc8125 100644 --- a/include/ur_modern_driver/ur/stream.h +++ b/include/ur_modern_driver/ur/stream.h @@ -8,7 +8,7 @@ #include "ur_modern_driver/log.h" #include "ur_modern_driver/tcp_socket.h" -class URStream : private TCPSocket +class URStream : public TCPSocket { private: std::string host_; diff --git a/src/ros/action_server.cpp b/src/ros/action_server.cpp index 1ec7217f7..fc3f7ad03 100644 --- a/src/ros/action_server.cpp +++ b/src/ros/action_server.cpp @@ -51,12 +51,12 @@ void ActionServer::onRobotStateChange(RobotState state) } interrupt_traj_ = true; - //wait for goal to be interrupted + //wait for goal to be interrupted and automagically unlock when going out of scope std::lock_guard lock(tj_mutex_); Result res; res.error_code = -100; - res.error_string = "Received another trajectory"; + res.error_string = "Robot safety stop"; curr_gh_.setAborted(res, res.error_string); } @@ -212,8 +212,8 @@ bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res) inline std::chrono::microseconds convert(const ros::Duration &dur) { - return std::chrono::duration_cast(std::chrono::seconds(dur.sec)) - + std::chrono::duration_cast(std::chrono::nanoseconds(dur.nsec)); + return std::chrono::duration_cast(std::chrono::seconds(dur.sec) + + std::chrono::nanoseconds(dur.nsec)); } bool ActionServer::try_execute(GoalHandle& gh, Result& res) @@ -261,7 +261,7 @@ std::vector ActionServer::reorderMap(std::vector goal_joint void ActionServer::trajectoryThread() { LOG_INFO("Trajectory thread started"); - follower_.start(); //todo check error + while(running_) { std::unique_lock lk(tj_mutex_); @@ -272,7 +272,8 @@ void ActionServer::trajectoryThread() curr_gh_.setAccepted(); auto goal = curr_gh_.getGoal(); - std::vector trajectory(goal->trajectory.points.size()); + std::vector trajectory; + trajectory.reserve(goal->trajectory.points.size()+1); //joint names of the goal might have a different ordering compared //to what URScript expects so need to map between the two @@ -281,7 +282,7 @@ void ActionServer::trajectoryThread() LOG_INFO("Translating trajectory"); auto const& fp = goal->trajectory.points[0]; - auto fpt = convert(fp.time_from_start) + auto fpt = convert(fp.time_from_start); //make sure we have a proper t0 position if(fpt > std::chrono::microseconds(0)) @@ -290,6 +291,7 @@ void ActionServer::trajectoryThread() trajectory.push_back(TrajectoryPoint(q_actual_, qd_actual_, std::chrono::microseconds(0))); } + int j = 0; for(auto const& point : goal->trajectory.points) { std::array pos, vel; @@ -299,35 +301,49 @@ void ActionServer::trajectoryThread() pos[idx] = point.positions[i]; vel[idx] = point.velocities[i]; } - trajectory.push_back(TrajectoryPoint(pos, vel, convert(point.time_from_start))); + auto t = convert(point.time_from_start); + LOG_DEBUG("P[%d] = %ds, %dns -> %dus", j++, point.time_from_start.sec, point.time_from_start.nsec, t.count()); + trajectory.push_back(TrajectoryPoint(pos, vel, t)); } double t = std::chrono::duration_cast>(trajectory[trajectory.size()-1].time_from_start).count(); - LOG_INFO("Trajectory with %d points and duration of %f constructed, executing now", trajectory.size(), t); + LOG_INFO("Executing trajectory with %d points and duration of %4.3fs", trajectory.size(), t); Result res; - if(follower_.execute(trajectory, interrupt_traj_)) + + if(follower_.start()) { - //interrupted goals must be handled by interrupt trigger - if(!interrupt_traj_) + if(follower_.execute(trajectory, interrupt_traj_)) { - LOG_INFO("Trajectory executed successfully"); - res.error_code = Result::SUCCESSFUL; - curr_gh_.setSucceeded(res); + //interrupted goals must be handled by interrupt trigger + if(!interrupt_traj_) + { + LOG_INFO("Trajectory executed successfully"); + res.error_code = Result::SUCCESSFUL; + curr_gh_.setSucceeded(res); + } + else + LOG_INFO("Trajectory interrupted"); } else - LOG_INFO("Trajectory interrupted"); + { + LOG_INFO("Trajectory failed"); + res.error_code = -100; + res.error_string = "Connection to robot was lost"; + curr_gh_.setAborted(res, res.error_string); + } + + follower_.stop(); } else { - LOG_INFO("Trajectory failed"); + LOG_ERROR("Failed to start trajectory follower!"); res.error_code = -100; - res.error_string = "Connection to robot was lost"; + res.error_string = "Robot connection could not be established"; curr_gh_.setAborted(res, res.error_string); } has_goal_ = false; lk.unlock(); } - follower_.stop(); } \ No newline at end of file diff --git a/src/ros/trajectory_follower.cpp b/src/ros/trajectory_follower.cpp index c6752b10e..099d84ae4 100644 --- a/src/ros/trajectory_follower.cpp +++ b/src/ros/trajectory_follower.cpp @@ -65,7 +65,7 @@ def driverProg(): end )"; -TrajectoryFollower::TrajectoryFollower(URCommander &commander, int reverse_port, bool version_3) +TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string& reverse_ip, int reverse_port, bool version_3) : running_(false) , commander_(commander) , reverse_port_(reverse_port) @@ -75,7 +75,6 @@ TrajectoryFollower::TrajectoryFollower(URCommander &commander, int reverse_port, , server_(reverse_port) { std::string res(POSITION_PROGRAM); - res.replace(res.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_)); std::ostringstream out; @@ -85,17 +84,16 @@ TrajectoryFollower::TrajectoryFollower(URCommander &commander, int reverse_port, res.replace(res.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str()); - program_ = res; -} + if(!server_.bind()) + { + LOG_ERROR("Failed to bind server, the port %d is likely already in use", reverse_port); + std::exit(-1); + } -std::string TrajectoryFollower::buildProgram() -{ - std::string res(program_); - std::string IP(server_.getIP()); - LOG_INFO("Local IP: %s ", IP.c_str()); - res.replace(res.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), "127.0.0.1"); + res.replace(res.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), reverse_ip); res.replace(res.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port_)); - return res; + + program_ = res; } bool TrajectoryFollower::start() @@ -103,23 +101,15 @@ bool TrajectoryFollower::start() if(running_) return true; //not sure - if(!server_.bind()) - { - LOG_ERROR("Failed to bind server"); - return false; - } - LOG_INFO("Uploading trajectory program to robot"); - - std::string prog(buildProgram()); - //std::string prog = "socket_open(\"127.0.0.1\", 50001)\n"; - if(!commander_.uploadProg(prog)) + + if(!commander_.uploadProg(program_)) { LOG_ERROR("Program upload failed!"); return false; } - LOG_INFO("Awaiting incomming robot connection"); + LOG_DEBUG("Awaiting incomming robot connection"); if(!server_.accept()) { @@ -127,7 +117,7 @@ bool TrajectoryFollower::start() return false; } - LOG_INFO("Robot successfully connected"); + LOG_DEBUG("Robot successfully connected"); return (running_ = true); } @@ -182,7 +172,7 @@ bool TrajectoryFollower::execute(std::vector &trajectory, std:: typedef high_resolution_clock Clock; typedef Clock::time_point Time; - auto const& last = trajectory[trajectory.size()-1]; + auto& last = trajectory[trajectory.size()-1]; auto& prev = trajectory[0]; Time t0 = Clock::now(); @@ -202,8 +192,6 @@ bool TrajectoryFollower::execute(std::vector &trajectory, std:: auto duration = point.time_from_start - prev.time_from_start; double d_s = duration_cast(duration).count(); - LOG_INFO("Point duration: %f", d_s); - //interpolation loop while(!interrupt) { @@ -243,13 +231,7 @@ bool TrajectoryFollower::execute(std::vector &trajectory, std:: //the interpolation loop above but rather some position between //t[N-1] and t[N] where N is the number of trajectory points. //To make sure this does not happen the last position is sent - //if not interrupted. - if(!interrupt) - { - return execute(last.positions, true); - } - - return true; + return execute(last.positions, true); } void TrajectoryFollower::stop() @@ -260,6 +242,6 @@ void TrajectoryFollower::stop() std::array empty; execute(empty, false); - //server_.disconnect(); + server_.disconnectClient(); running_ = false; } \ No newline at end of file diff --git a/src/ros_main.cpp b/src/ros_main.cpp index 38634c9b6..dc5255c20 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -74,7 +74,11 @@ bool parse_args(ProgArgs &args) return true; } -#include "ur_modern_driver/ur/server.h" +std::string getLocalIPAccessibleFromHost(std::string& host) +{ + URStream stream(host, UR_RT_PORT); + return stream.connect() ? stream.getIP() : std::string(); +} int main(int argc, char **argv) { @@ -86,9 +90,9 @@ int main(int argc, char **argv) return EXIT_FAILURE; } - + std::string local_ip(getLocalIPAccessibleFromHost(args.host)); + URFactory factory(args.host); - vector services; // RT packets @@ -99,7 +103,7 @@ int main(int argc, char **argv) auto rt_commander = factory.getCommander(rt_stream); vector *> rt_vec{&rt_pub}; - TrajectoryFollower traj_follower(*rt_commander, args.reverse_port, factory.isVersion3()); + TrajectoryFollower traj_follower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3()); ROSController *controller(nullptr); ActionServer *action_server(nullptr); diff --git a/src/tcp_socket.cpp b/src/tcp_socket.cpp index f0049ec4f..a7f8cc455 100644 --- a/src/tcp_socket.cpp +++ b/src/tcp_socket.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include @@ -94,6 +95,23 @@ void TCPSocket::close() socket_fd_ = -1; } +std::string TCPSocket::getIP() +{ + sockaddr_in name; + socklen_t len = sizeof(name); + int res = ::getsockname(socket_fd_, (sockaddr*)&name, &len); + + if(res < 0) + { + LOG_ERROR("Could not get local IP"); + return std::string(); + } + + char buf[128]; + inet_ntop(AF_INET, &name.sin_addr, buf, sizeof(buf)); + return std::string(buf); +} + bool TCPSocket::read(uint8_t *buf, size_t buf_len, size_t &read) { read = 0; diff --git a/src/ur/server.cpp b/src/ur/server.cpp index fd929fed5..1d53ed4d6 100644 --- a/src/ur/server.cpp +++ b/src/ur/server.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include "ur_modern_driver/log.h" #include "ur_modern_driver/ur/server.h" @@ -19,8 +20,18 @@ void URServer::setOptions(int socket_fd) std::string URServer::getIP() { + sockaddr_in name; + socklen_t len = sizeof(name); + int res = ::getsockname(getSocketFD(), (sockaddr*)&name, &len); + + if(res < 0) + { + LOG_ERROR("Could not get local IP"); + return std::string(); + } + char buf[128]; - int res = ::gethostname(buf, sizeof(buf)); + inet_ntop(AF_INET, &name.sin_addr, buf, sizeof(buf)); return std::string(buf); } @@ -28,7 +39,6 @@ bool URServer::bind() { std::string empty; bool res = TCPSocket::setup(empty, port_); - state_ = TCPSocket::getState(); if(!res) return false; @@ -41,7 +51,7 @@ bool URServer::bind() bool URServer::accept() { - if(state_ != SocketState::Connected || client_.getSocketFD() > 0) + if(TCPSocket::getState() != SocketState::Connected || client_.getSocketFD() > 0) return false; struct sockaddr addr; @@ -54,6 +64,14 @@ bool URServer::accept() return client_.setSocketFD(client_fd); } +void URServer::disconnectClient() +{ + if(client_.getState() != SocketState::Connected) + return; + + client_.close(); +} + bool URServer::write(const uint8_t* buf, size_t buf_len, size_t &written) { return client_.write(buf, buf_len, written); From 93bf3487ddb9f0ac4004dbe806fd3b2f805431d0 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Fri, 7 Jul 2017 16:39:34 +0200 Subject: [PATCH 053/114] Fixed trajectory issue --- src/ros/trajectory_follower.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/ros/trajectory_follower.cpp b/src/ros/trajectory_follower.cpp index 099d84ae4..78d448e00 100644 --- a/src/ros/trajectory_follower.cpp +++ b/src/ros/trajectory_follower.cpp @@ -69,7 +69,7 @@ TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string& reve : running_(false) , commander_(commander) , reverse_port_(reverse_port) - , servoj_time_(0.016/4) + , servoj_time_(0.008) , servoj_lookahead_time_(0.03) , servoj_gain_(300.) , server_(reverse_port) @@ -205,7 +205,6 @@ bool TrajectoryFollower::execute(std::vector &trajectory, std:: return true; double elapsed_s = duration_cast(elapsed - prev.time_from_start).count(); - //double prev_seconds for(size_t j = 0; j < positions.size(); j++) { positions[j] = interpolate( @@ -221,7 +220,7 @@ bool TrajectoryFollower::execute(std::vector &trajectory, std:: if(!execute(positions, true)) return false; - std::this_thread::sleep_for(double_seconds(servoj_time_)); + std::this_thread::sleep_for(std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.))); } prev = point; @@ -239,8 +238,8 @@ void TrajectoryFollower::stop() if(!running_) return; - std::array empty; - execute(empty, false); + //std::array empty; + //execute(empty, false); server_.disconnectClient(); running_ = false; From f3e11bfc29692a8abec7ec112ac5bcb66fd1ae55 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Fri, 7 Jul 2017 19:15:17 +0200 Subject: [PATCH 054/114] minor improvements --- include/ur_modern_driver/ur/server.h | 1 + src/ur/server.cpp | 7 +++++++ 2 files changed, 8 insertions(+) diff --git a/include/ur_modern_driver/ur/server.h b/include/ur_modern_driver/ur/server.h index daff4d35b..027e278eb 100644 --- a/include/ur_modern_driver/ur/server.h +++ b/include/ur_modern_driver/ur/server.h @@ -24,6 +24,7 @@ class URServer : private TCPSocket public: URServer(int port); + ~URServer(); std::string getIP(); bool bind(); bool accept(); diff --git a/src/ur/server.cpp b/src/ur/server.cpp index 1d53ed4d6..5530be988 100644 --- a/src/ur/server.cpp +++ b/src/ur/server.cpp @@ -10,6 +10,11 @@ URServer::URServer(int port) { } +URServer::~URServer() +{ + TCPSocket::close(); +} + void URServer::setOptions(int socket_fd) { TCPSocket::setOptions(socket_fd); @@ -61,6 +66,8 @@ bool URServer::accept() if(client_fd <= 0) return false; + setOptions(client_fd); + return client_.setSocketFD(client_fd); } From e4560097c8c89436f996ad8f6f93ac3ebd00f09e Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Fri, 7 Jul 2017 19:15:35 +0200 Subject: [PATCH 055/114] Parameter parsing --- src/ros/trajectory_follower.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/ros/trajectory_follower.cpp b/src/ros/trajectory_follower.cpp index 78d448e00..0093cba46 100644 --- a/src/ros/trajectory_follower.cpp +++ b/src/ros/trajectory_follower.cpp @@ -74,6 +74,11 @@ TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string& reve , servoj_gain_(300.) , server_(reverse_port) { + ros::param::get("~servoj_time", servoj_time_); + ros::param::get("~servoj_lookahead_time", servoj_lookahead_time_); + ros::param::get("~servoj_gain", servoj_gain_); + + std::string res(POSITION_PROGRAM); res.replace(res.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_)); @@ -83,17 +88,15 @@ TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string& reve out << ", lookahead_time=" << servoj_lookahead_time_ << ", gain=" << servoj_gain_; res.replace(res.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str()); + res.replace(res.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), reverse_ip); + res.replace(res.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port_)); + program_ = res; if(!server_.bind()) { LOG_ERROR("Failed to bind server, the port %d is likely already in use", reverse_port); std::exit(-1); } - - res.replace(res.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), reverse_ip); - res.replace(res.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port_)); - - program_ = res; } bool TrajectoryFollower::start() From 210e0086bf7ee8e9e247ee9b4478ac6549ebfe8b Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Fri, 7 Jul 2017 19:15:46 +0200 Subject: [PATCH 056/114] socket improvment --- src/tcp_socket.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/tcp_socket.cpp b/src/tcp_socket.cpp index a7f8cc455..c4f183917 100644 --- a/src/tcp_socket.cpp +++ b/src/tcp_socket.cpp @@ -21,6 +21,7 @@ void TCPSocket::setOptions(int socket_fd) { int flag = 1; setsockopt(socket_fd, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(int)); + setsockopt(socket_fd, IPPROTO_TCP, TCP_QUICKACK, &flag, sizeof(int)); } bool TCPSocket::setup(std::string &host, int port) From 40fc986e7bcc7e6890af1c74d53c57cbdd27d28b Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Fri, 7 Jul 2017 20:08:15 +0200 Subject: [PATCH 057/114] Fixed jitter issues --- src/ur/stream.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/ur/stream.cpp b/src/ur/stream.cpp index 05617c55d..cdb20f2e6 100644 --- a/src/ur/stream.cpp +++ b/src/ur/stream.cpp @@ -23,6 +23,7 @@ bool URStream::read(uint8_t* buf, size_t buf_len, size_t &total) while(remainder > 0 && TCPSocket::read(buf_pos, remainder, read)) { + TCPSocket::setOptions(getSocketFD()); if (initial) { remainder = be32toh(*(reinterpret_cast(buf))); From fdaaacfe2c08e8a0f3ec6f296949075a04ffc081 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Sun, 9 Jul 2017 02:45:02 +0200 Subject: [PATCH 058/114] Clean up of old driver files --- CMakeLists.txt | 16 +- include/ur_modern_driver/packet.h | 8 - include/ur_modern_driver/parser.h | 9 - include/ur_modern_driver/robot_state.h | 233 ----- include/ur_modern_driver/robot_state_RT.h | 119 --- include/ur_modern_driver/ros/robot_hardware.h | 53 -- include/ur_modern_driver/ur_communication.h | 63 -- include/ur_modern_driver/ur_driver.h | 97 -- .../ur_modern_driver/ur_hardware_interface.h | 136 --- .../ur_realtime_communication.h | 73 -- src/do_output.cpp | 62 -- src/ros/robot_hardware.cpp | 30 - src/ur_communication.cpp | 184 ---- src/ur_driver.cpp | 420 --------- src/ur_hardware_interface.cpp | 283 ------ src/ur_realtime_communication.cpp | 211 ----- src/ur_ros_wrapper.cpp | 871 ------------------ 17 files changed, 5 insertions(+), 2863 deletions(-) delete mode 100644 include/ur_modern_driver/packet.h delete mode 100644 include/ur_modern_driver/parser.h delete mode 100644 include/ur_modern_driver/robot_state.h delete mode 100644 include/ur_modern_driver/robot_state_RT.h delete mode 100644 include/ur_modern_driver/ros/robot_hardware.h delete mode 100644 include/ur_modern_driver/ur_communication.h delete mode 100644 include/ur_modern_driver/ur_driver.h delete mode 100644 include/ur_modern_driver/ur_hardware_interface.h delete mode 100644 include/ur_modern_driver/ur_realtime_communication.h delete mode 100644 src/do_output.cpp delete mode 100644 src/ros/robot_hardware.cpp delete mode 100644 src/ur_communication.cpp delete mode 100644 src/ur_driver.cpp delete mode 100644 src/ur_hardware_interface.cpp delete mode 100644 src/ur_realtime_communication.cpp delete mode 100644 src/ur_ros_wrapper.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index be59d00b1..a950358f7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -134,7 +134,7 @@ else() message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler. Suggested solution: update the pkg build-essential ") endif() -set(CMAKE_CXX_FLAGS "-g -Wall -Wextra ${CMAKE_CXX_FLAGS}") +set(CMAKE_CXX_FLAGS "-g -Wall -Wextra -Wno-unused-parameter ${CMAKE_CXX_FLAGS}") ## Specify additional locations of header files ## Your package locations should be listed before other locations @@ -146,7 +146,9 @@ include_directories(include ## Declare a C++ library # Hardware Interface -add_library(ur_hardware_interface src/ur_hardware_interface.cpp) +add_library(ur_hardware_interface + src/ros/hardware_interface.cpp + src/ros/controller.cpp) target_link_libraries(ur_hardware_interface ${catkin_LIBRARIES} ) @@ -159,8 +161,6 @@ target_link_libraries(ur_hardware_interface ## Declare a C++ executable set(${PROJECT_NAME}_SOURCES src/ros/action_server.cpp - src/ros/controller.cpp - src/ros/hardware_interface.cpp src/ros/mb_publisher.cpp src/ros/rt_publisher.cpp src/ros/service_stopper.cpp @@ -172,13 +172,7 @@ set(${PROJECT_NAME}_SOURCES src/ur/master_board.cpp src/ur/rt_state.cpp src/ur/messages.cpp - src/tcp_socket.cpp - src/ur_driver.cpp - src/ur_realtime_communication.cpp - src/ur_communication.cpp - src/robot_state.cpp - src/robot_state_RT.cpp - src/do_output.cpp) + src/tcp_socket.cpp) add_executable(ur_driver ${${PROJECT_NAME}_SOURCES} src/ros_main.cpp) diff --git a/include/ur_modern_driver/packet.h b/include/ur_modern_driver/packet.h deleted file mode 100644 index 521d75bf1..000000000 --- a/include/ur_modern_driver/packet.h +++ /dev/null @@ -1,8 +0,0 @@ -#pragma once -#include "ur_modern_driver/bin_parser.h" - -class Packet -{ -public: - virtual bool parseWith(BinParser& bp) = 0; -}; \ No newline at end of file diff --git a/include/ur_modern_driver/parser.h b/include/ur_modern_driver/parser.h deleted file mode 100644 index e824c57f9..000000000 --- a/include/ur_modern_driver/parser.h +++ /dev/null @@ -1,9 +0,0 @@ -#pragma once -#include "ur_modern_driver/bin_parser.h" -#include "ur_modern_driver/packet.h" - -class Parser -{ -public: - virtual std::unique_ptr parse(BinParser& bp) = 0; -}; diff --git a/include/ur_modern_driver/robot_state.h b/include/ur_modern_driver/robot_state.h deleted file mode 100644 index 621926a1f..000000000 --- a/include/ur_modern_driver/robot_state.h +++ /dev/null @@ -1,233 +0,0 @@ -/* - * robot_state.h - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef ROBOT_STATE_H_ -#define ROBOT_STATE_H_ - -#include -#include -#include -#include -#include -#include -#include - -namespace message_types -{ -enum message_type -{ - ROBOT_STATE = 16, - ROBOT_MESSAGE = 20, - PROGRAM_STATE_MESSAGE = 25 -}; -} -typedef message_types::message_type messageType; - -namespace package_types -{ -enum package_type -{ - ROBOT_MODE_DATA = 0, - JOINT_DATA = 1, - TOOL_DATA = 2, - MASTERBOARD_DATA = 3, - CARTESIAN_INFO = 4, - KINEMATICS_INFO = 5, - CONFIGURATION_DATA = 6, - FORCE_MODE_DATA = 7, - ADDITIONAL_INFO = 8, - CALIBRATION_DATA = 9 -}; -} -typedef package_types::package_type packageType; - -namespace robot_message_types -{ -enum robot_message_type -{ - ROBOT_MESSAGE_TEXT = 0, - ROBOT_MESSAGE_PROGRAM_LABEL = 1, - PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2, - ROBOT_MESSAGE_VERSION = 3, - ROBOT_MESSAGE_SAFETY_MODE = 5, - ROBOT_MESSAGE_ERROR_CODE = 6, - ROBOT_MESSAGE_KEY = 7, - ROBOT_MESSAGE_REQUEST_VALUE = 9, - ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10 -}; -} -typedef robot_message_types::robot_message_type robotMessageType; - -namespace robot_state_type_v18 -{ -enum robot_state_type -{ - ROBOT_RUNNING_MODE = 0, - ROBOT_FREEDRIVE_MODE = 1, - ROBOT_READY_MODE = 2, - ROBOT_INITIALIZING_MODE = 3, - ROBOT_SECURITY_STOPPED_MODE = 4, - ROBOT_EMERGENCY_STOPPED_MODE = 5, - ROBOT_FATAL_ERROR_MODE = 6, - ROBOT_NO_POWER_MODE = 7, - ROBOT_NOT_CONNECTED_MODE = 8, - ROBOT_SHUTDOWN_MODE = 9, - ROBOT_SAFEGUARD_STOP_MODE = 10 -}; -} -typedef robot_state_type_v18::robot_state_type robotStateTypeV18; -namespace robot_state_type_v30 -{ -enum robot_state_type -{ - ROBOT_MODE_DISCONNECTED = 0, - ROBOT_MODE_CONFIRM_SAFETY = 1, - ROBOT_MODE_BOOTING = 2, - ROBOT_MODE_POWER_OFF = 3, - ROBOT_MODE_POWER_ON = 4, - ROBOT_MODE_IDLE = 5, - ROBOT_MODE_BACKDRIVE = 6, - ROBOT_MODE_RUNNING = 7, - ROBOT_MODE_UPDATING_FIRMWARE = 8 -}; -} - -typedef robot_state_type_v30::robot_state_type robotStateTypeV30; - -struct version_message -{ - uint64_t timestamp; - int8_t source; - int8_t robot_message_type; - int8_t project_name_size; - char project_name[15]; - uint8_t major_version; - uint8_t minor_version; - int svn_revision; - char build_date[25]; -}; - -struct masterboard_data -{ - int digitalInputBits; - int digitalOutputBits; - char analogInputRange0; - char analogInputRange1; - double analogInput0; - double analogInput1; - char analogOutputDomain0; - char analogOutputDomain1; - double analogOutput0; - double analogOutput1; - float masterBoardTemperature; - float robotVoltage48V; - float robotCurrent; - float masterIOCurrent; - unsigned char safetyMode; - unsigned char masterOnOffState; - char euromap67InterfaceInstalled; - int euromapInputBits; - int euromapOutputBits; - float euromapVoltage; - float euromapCurrent; -}; - -struct robot_mode_data -{ - uint64_t timestamp; - bool isRobotConnected; - bool isRealRobotEnabled; - bool isPowerOnRobot; - bool isEmergencyStopped; - bool isProtectiveStopped; - bool isProgramRunning; - bool isProgramPaused; - unsigned char robotMode; - unsigned char controlMode; - double targetSpeedFraction; - double speedScaling; -}; - -class RobotState -{ -private: - version_message version_msg_; - masterboard_data mb_data_; - robot_mode_data robot_mode_; - - std::recursive_mutex val_lock_; // Locks the variables while unpack parses data; - - std::condition_variable* pMsg_cond_; // Signals that new vars are available - bool new_data_available_; // to avoid spurious wakes - unsigned char robot_mode_running_; - - double ntohd(uint64_t nf); - -public: - RobotState(std::condition_variable& msg_cond); - ~RobotState(); - double getVersion(); - double getTime(); - std::vector getQTarget(); - int getDigitalInputBits(); - int getDigitalOutputBits(); - char getAnalogInputRange0(); - char getAnalogInputRange1(); - double getAnalogInput0(); - double getAnalogInput1(); - char getAnalogOutputDomain0(); - char getAnalogOutputDomain1(); - double getAnalogOutput0(); - double getAnalogOutput1(); - std::vector getVActual(); - float getMasterBoardTemperature(); - float getRobotVoltage48V(); - float getRobotCurrent(); - float getMasterIOCurrent(); - unsigned char getSafetyMode(); - unsigned char getInReducedMode(); - char getEuromap67InterfaceInstalled(); - int getEuromapInputBits(); - int getEuromapOutputBits(); - float getEuromapVoltage(); - float getEuromapCurrent(); - - bool isRobotConnected(); - bool isRealRobotEnabled(); - bool isPowerOnRobot(); - bool isEmergencyStopped(); - bool isProtectiveStopped(); - bool isProgramRunning(); - bool isProgramPaused(); - unsigned char getRobotMode(); - bool isReady(); - - void setDisconnected(); - - bool getNewDataAvailable(); - void finishedReading(); - - void unpack(uint8_t* buf, unsigned int buf_length); - void unpackRobotMessage(uint8_t* buf, unsigned int offset, uint32_t len); - void unpackRobotMessageVersion(uint8_t* buf, unsigned int offset, uint32_t len); - void unpackRobotState(uint8_t* buf, unsigned int offset, uint32_t len); - void unpackRobotStateMasterboard(uint8_t* buf, unsigned int offset); - void unpackRobotMode(uint8_t* buf, unsigned int offset); -}; - -#endif /* ROBOT_STATE_H_ */ diff --git a/include/ur_modern_driver/robot_state_RT.h b/include/ur_modern_driver/robot_state_RT.h deleted file mode 100644 index bca6faca2..000000000 --- a/include/ur_modern_driver/robot_state_RT.h +++ /dev/null @@ -1,119 +0,0 @@ -/* - * robotStateRT.h - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef ROBOT_STATE_RT_H_ -#define ROBOT_STATE_RT_H_ - -#include -#include -#include -#include -#include -#include -#include - -class RobotStateRT -{ -private: - double version_; // protocol version - - double time_; // Time elapsed since the controller was started - std::vector q_target_; // Target joint positions - std::vector qd_target_; // Target joint velocities - std::vector qdd_target_; // Target joint accelerations - std::vector i_target_; // Target joint currents - std::vector m_target_; // Target joint moments (torques) - std::vector q_actual_; // Actual joint positions - std::vector qd_actual_; // Actual joint velocities - std::vector i_actual_; // Actual joint currents - std::vector i_control_; // Joint control currents - std::vector tool_vector_actual_; // Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry - // and rz is a rotation vector representation of the tool orientation - std::vector tcp_speed_actual_; // Actual speed of the tool given in Cartesian coordinates - std::vector tcp_force_; // Generalised forces in the TC - std::vector tool_vector_target_; // Target Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry - // and rz is a rotation vector representation of the tool orientation - std::vector tcp_speed_target_; // Target speed of the tool given in Cartesian coordinates - std::vector digital_input_bits_; // Current state of the digital inputs. NOTE: these are bits encoded as - // int64_t, e.g. a value of 5 corresponds to bit 0 and bit 2 set high - std::vector motor_temperatures_; // Temperature of each joint in degrees celsius - double controller_timer_; // Controller realtime thread execution time - double robot_mode_; // Robot mode - std::vector joint_modes_; // Joint control modes - double safety_mode_; // Safety mode - std::vector tool_accelerometer_values_; // Tool x,y and z accelerometer values (software version 1.7) - double speed_scaling_; // Speed scaling of the trajectory limiter - double linear_momentum_norm_; // Norm of Cartesian linear momentum - double v_main_; // Masterboard: Main voltage - double v_robot_; // Matorborad: Robot voltage (48V) - double i_robot_; // Masterboard: Robot current - std::vector v_actual_; // Actual joint voltages - - std::mutex val_lock_; // Locks the variables while unpack parses data; - - std::condition_variable* pMsg_cond_; // Signals that new vars are available - bool data_published_; // to avoid spurious wakes - bool controller_updated_; // to avoid spurious wakes - - std::vector unpackVector(uint8_t* buf, int start_index, int nr_of_vals); - std::vector unpackDigitalInputBits(int64_t data); - double ntohd(uint64_t nf); - -public: - RobotStateRT(std::condition_variable& msg_cond); - ~RobotStateRT(); - double getVersion(); - double getTime(); - std::vector getQTarget(); - std::vector getQdTarget(); - std::vector getQddTarget(); - std::vector getITarget(); - std::vector getMTarget(); - std::vector getQActual(); - std::vector getQdActual(); - std::vector getIActual(); - std::vector getIControl(); - std::vector getToolVectorActual(); - std::vector getTcpSpeedActual(); - std::vector getTcpForce(); - std::vector getToolVectorTarget(); - std::vector getTcpSpeedTarget(); - std::vector getDigitalInputBits(); - std::vector getMotorTemperatures(); - double getControllerTimer(); - double getRobotMode(); - std::vector getJointModes(); - double getSafety_mode(); - std::vector getToolAccelerometerValues(); - double getSpeedScaling(); - double getLinearMomentumNorm(); - double getVMain(); - double getVRobot(); - double getIRobot(); - - void setVersion(double ver); - - void setDataPublished(); - bool getDataPublished(); - bool getControllerUpdated(); - void setControllerUpdated(); - std::vector getVActual(); - void unpack(uint8_t* buf); -}; - -#endif /* ROBOT_STATE_RT_H_ */ diff --git a/include/ur_modern_driver/ros/robot_hardware.h b/include/ur_modern_driver/ros/robot_hardware.h deleted file mode 100644 index 47ef455bb..000000000 --- a/include/ur_modern_driver/ros/robot_hardware.h +++ /dev/null @@ -1,53 +0,0 @@ -#pragma once -#include -#include -#include -#include -#include -#include -#include "ur_modern_driver/ros/hardware_interface.h" - -using hardware_interface::RobotHW; -using hardware_interface::ControllerInfo; - -class RobotHardware : public RobotHW -{ -private: - JointInterface joint_interface_; - WrenchInterface wrench_interface_; - PositionInterface position_interface_; - VelocityInterface velocity_interface_; - - HardwareInterface* active_interface_; - std::map available_interfaces_; - - template - void registerHardwareInterface(T* interface) - { - registerInterface(interface); - available_interfaces_[hardware_interface::internal::demangledTypeName()] = interface; - } - -public: - RobotHardware(URCommander& commander, std::vector& joint_names, double max_vel_change) - : joint_interface_(joint_names) - , wrench_interface_() - , position_interface_(commander, joint_interface_, joint_names) - , velocity_interface_(commander, joint_interface_, joint_names, max_vel_change) - { - registerInterface(&joint_interface_); - registerInterface(&wrench_interface_); - - registerHardwareInterface(&position_interface_); - registerHardwareInterface(&velocity_interface_); - } - - // bool canSwitch(const std::list& start_list, const std::list& stop_list) const; - void doSwitch(const std::list& start_list, const std::list& stop_list); - - /// \brief Read the state from the robot hardware. - virtual void read(RTShared& packet); - - /// \brief write the command to the robot hardware. - virtual void write(); -}; \ No newline at end of file diff --git a/include/ur_modern_driver/ur_communication.h b/include/ur_modern_driver/ur_communication.h deleted file mode 100644 index 29460c5a6..000000000 --- a/include/ur_modern_driver/ur_communication.h +++ /dev/null @@ -1,63 +0,0 @@ -/* - * ur_communication.h - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef UR_COMMUNICATION_H_ -#define UR_COMMUNICATION_H_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "do_output.h" -#include "robot_state.h" - -class UrCommunication -{ -private: - int pri_sockfd_, sec_sockfd_; - struct sockaddr_in pri_serv_addr_, sec_serv_addr_; - struct hostent* server_; - bool keepalive_; - std::thread comThread_; - int flag_; - void run(); - -public: - bool connected_; - RobotState* robot_state_; - - UrCommunication(std::condition_variable& msg_cond, std::string host); - bool start(); - void halt(); -}; - -#endif /* UR_COMMUNICATION_H_ */ diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h deleted file mode 100644 index dbfb3bd32..000000000 --- a/include/ur_modern_driver/ur_driver.h +++ /dev/null @@ -1,97 +0,0 @@ -/* - * ur_driver - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef UR_DRIVER_H_ -#define UR_DRIVER_H_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include "do_output.h" -#include "ur_communication.h" -#include "ur_realtime_communication.h" - -#include - -class UrDriver -{ -private: - double maximum_time_step_; - double minimum_payload_; - double maximum_payload_; - std::vector joint_names_; - std::string ip_addr_; - const int MULT_JOINTSTATE_ = 1000000; - const int MULT_TIME_ = 1000000; - const unsigned int REVERSE_PORT_; - int incoming_sockfd_; - int new_sockfd_; - bool reverse_connected_; - double servoj_time_; - bool executing_traj_; - double firmware_version_; - double servoj_lookahead_time_; - double servoj_gain_; - -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.); - bool start(); - void halt(); - - void setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc = 100.); - - bool doTraj(std::vector inp_timestamps, std::vector> inp_positions, - std::vector> inp_velocities); - void servoj(std::vector positions, int keepalive = 1); - - void stopTraj(); - - bool uploadProg(); - bool openServo(); - void closeServo(std::vector positions); - - std::vector interp_cubic(double t, double T, std::vector p0_pos, std::vector p1_pos, - std::vector p0_vel, std::vector p1_vel); - - std::vector getJointNames(); - void setJointNames(std::vector jn); - void setToolVoltage(unsigned int v); - void setFlag(unsigned int n, bool b); - void setDigitalOut(unsigned int n, bool b); - void setAnalogOut(unsigned int n, double f); - bool setPayload(double m); - - void setMinPayload(double m); - void setMaxPayload(double m); - void setServojTime(double t); - void setServojLookahead(double t); - void setServojGain(double g); -}; - -#endif /* UR_DRIVER_H_ */ diff --git a/include/ur_modern_driver/ur_hardware_interface.h b/include/ur_modern_driver/ur_hardware_interface.h deleted file mode 100644 index b2a7db107..000000000 --- a/include/ur_modern_driver/ur_hardware_interface.h +++ /dev/null @@ -1,136 +0,0 @@ -/* - * ur_hardware_control_loop.cpp - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -/* Based on original source from University of Colorado, Boulder. License copied below. */ - -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2015, University of Colorado, Boulder - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Univ of CO, Boulder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - ********************************************************************* - - Author: Dave Coleman - */ - -#ifndef UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H -#define UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include "do_output.h" -#include "ur_driver.h" - -namespace ros_control_ur -{ -// For simulation only - determines how fast a trajectory is followed -static const double POSITION_STEP_FACTOR = 1; -static const double VELOCITY_STEP_FACTOR = 1; - -/// \brief Hardware interface for a robot -class UrHardwareInterface : public hardware_interface::RobotHW -{ -public: - /** - * \brief Constructor - * \param nh - Node handle for topics. - */ - UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot); - - /// \brief Initialize the hardware interface - virtual void init(); - - /// \brief Read the state from the robot hardware. - virtual void read(); - - /// \brief write the command to the robot hardware. - virtual void write(); - - void setMaxVelChange(double inp); - - bool canSwitch(const std::list& start_list, - const std::list& stop_list) const; - void doSwitch(const std::list& start_list, - const std::list& stop_list); - -protected: - // Startup and shutdown of the internal node inside a roscpp program - ros::NodeHandle nh_; - - // Interfaces - hardware_interface::JointStateInterface joint_state_interface_; - hardware_interface::ForceTorqueSensorInterface force_torque_interface_; - hardware_interface::PositionJointInterface position_joint_interface_; - hardware_interface::VelocityJointInterface velocity_joint_interface_; - bool velocity_interface_running_; - bool position_interface_running_; - // Shared memory - std::vector joint_names_; - std::vector joint_position_; - std::vector joint_velocity_; - std::vector joint_effort_; - std::vector joint_position_command_; - std::vector joint_velocity_command_; - std::vector prev_joint_velocity_command_; - std::size_t num_joints_; - double robot_force_[3] = { 0., 0., 0. }; - double robot_torque_[3] = { 0., 0., 0. }; - - double max_vel_change_; - - // Robot API - UrDriver* robot_; -}; -// class - -} // namespace - -#endif diff --git a/include/ur_modern_driver/ur_realtime_communication.h b/include/ur_modern_driver/ur_realtime_communication.h deleted file mode 100644 index 1de9fdb62..000000000 --- a/include/ur_modern_driver/ur_realtime_communication.h +++ /dev/null @@ -1,73 +0,0 @@ -/* - * ur_realtime_communication.h - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef UR_REALTIME_COMMUNICATION_H_ -#define UR_REALTIME_COMMUNICATION_H_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "do_output.h" -#include "robot_state_RT.h" - -class UrRealtimeCommunication -{ -private: - unsigned int safety_count_max_; - int sockfd_; - struct sockaddr_in serv_addr_; - struct hostent* server_; - std::string local_ip_; - bool keepalive_; - std::thread comThread_; - int flag_; - std::recursive_mutex command_string_lock_; - std::string command_; - unsigned int safety_count_; - void run(); - -public: - bool connected_; - RobotStateRT* robot_state_; - - UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host, unsigned int safety_count_max = 12); - bool start(); - void halt(); - void setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc = 100.); - void addCommandToQueue(std::string inp); - void setSafetyCountMax(uint inp); - std::string getLocalIp(); -}; - -#endif /* UR_REALTIME_COMMUNICATION_H_ */ diff --git a/src/do_output.cpp b/src/do_output.cpp deleted file mode 100644 index 765e9b429..000000000 --- a/src/do_output.cpp +++ /dev/null @@ -1,62 +0,0 @@ -/* - * do_output.cpp - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "ur_modern_driver/do_output.h" - -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) -{ -#ifdef ROS_BUILD - ROS_INFO("%s", inp.c_str()); -#else - printf("INFO: %s\n", inp.c_str()); -#endif -} -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) -{ -#ifdef ROS_BUILD - ROS_ERROR("%s", inp.c_str()); -#else - printf("ERROR: %s\n", inp.c_str()); -#endif -} -void print_fatal(std::string inp) -{ -#ifdef ROS_BUILD - ROS_FATAL("%s", inp.c_str()); - ros::shutdown(); -#else - printf("FATAL: %s\n", inp.c_str()); - exit(1); -#endif -} diff --git a/src/ros/robot_hardware.cpp b/src/ros/robot_hardware.cpp deleted file mode 100644 index 008daec27..000000000 --- a/src/ros/robot_hardware.cpp +++ /dev/null @@ -1,30 +0,0 @@ -#include "ur_modern_driver/ros/robot_hardware.h" - -/* -bool RobotHardware::canSwitch(const std::list& start_list, - const std::list& stop_list) const -{ - - bool running = active_interface_ != nullptr; - size_t start_size = start_list.size(); - size_t stop_size = stop_list.size(); - - - for (auto const& ci : stop_list) - { - auto it = interfaces_.find(ci.hardware_interface); - if(it == interfaces_.end() || it->second != active_interface_) - return false; - } - - for (auto const& ci : start_list) - { - auto it = interfaces_.find(ci.hardware_interface); - //we can only start a controller that's already running if we stop it first - if(it == interfaces_.end() || (it->second == active_interface_ && stop_size == 0)) - return false; - } - - return true; -} -*/ diff --git a/src/ur_communication.cpp b/src/ur_communication.cpp deleted file mode 100644 index d7da04f62..000000000 --- a/src/ur_communication.cpp +++ /dev/null @@ -1,184 +0,0 @@ -/* - * ur_communication.cpp - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "ur_modern_driver/ur_communication.h" - -UrCommunication::UrCommunication(std::condition_variable& msg_cond, std::string host) -{ - robot_state_ = new RobotState(msg_cond); - bzero((char*)&pri_serv_addr_, sizeof(pri_serv_addr_)); - bzero((char*)&sec_serv_addr_, sizeof(sec_serv_addr_)); - pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (pri_sockfd_ < 0) - { - print_fatal("ERROR opening socket pri_sockfd"); - } - sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (sec_sockfd_ < 0) - { - print_fatal("ERROR opening socket sec_sockfd"); - } - server_ = gethostbyname(host.c_str()); - if (server_ == NULL) - { - print_fatal("ERROR, unknown host"); - } - pri_serv_addr_.sin_family = AF_INET; - sec_serv_addr_.sin_family = AF_INET; - bcopy((char*)server_->h_addr, (char*)&pri_serv_addr_.sin_addr.s_addr, server_->h_length); - bcopy((char*)server_->h_addr, (char*)&sec_serv_addr_.sin_addr.s_addr, server_->h_length); - pri_serv_addr_.sin_port = htons(30001); - sec_serv_addr_.sin_port = htons(30002); - flag_ = 1; - setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int)); - setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); - setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int)); - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int)); - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); - setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int)); - fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); - connected_ = false; - keepalive_ = false; -} - -bool UrCommunication::start() -{ - keepalive_ = true; - uint8_t buf[512]; - unsigned int bytes_read; - std::string cmd; - bzero(buf, 512); - print_debug("Acquire firmware version: Connecting..."); - if (connect(pri_sockfd_, (struct sockaddr*)&pri_serv_addr_, sizeof(pri_serv_addr_)) < 0) - { - print_fatal("Error connecting to get firmware version"); - return false; - } - print_debug("Acquire firmware version: Got connection"); - bytes_read = read(pri_sockfd_, buf, 512); - setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); - robot_state_->unpack(buf, bytes_read); - // wait for some traffic so the UR socket doesn't die in version 3.1. - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - char tmp[64]; - sprintf(tmp, "Firmware version detected: %.7f", robot_state_->getVersion()); - print_debug(tmp); - close(pri_sockfd_); - - print_debug("Switching to secondary interface for masterboard data: Connecting..."); - - fd_set writefds; - struct timeval timeout; - - connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_, sizeof(sec_serv_addr_)); - FD_ZERO(&writefds); - FD_SET(sec_sockfd_, &writefds); - timeout.tv_sec = 10; - timeout.tv_usec = 0; - select(sec_sockfd_ + 1, NULL, &writefds, NULL, &timeout); - unsigned int flag_len; - getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); - if (flag_ < 0) - { - print_fatal("Error connecting to secondary interface"); - return false; - } - print_debug("Secondary interface: Got connection"); - comThread_ = std::thread(&UrCommunication::run, this); - return true; -} - -void UrCommunication::halt() -{ - keepalive_ = false; - comThread_.join(); -} - -void UrCommunication::run() -{ - uint8_t buf[2048]; - int bytes_read; - bzero(buf, 2048); - struct timeval timeout; - fd_set readfds; - FD_ZERO(&readfds); - FD_SET(sec_sockfd_, &readfds); - connected_ = true; - while (keepalive_) - { - while (connected_ && keepalive_) - { - timeout.tv_sec = 0; // do this each loop as selects modifies timeout - timeout.tv_usec = 500000; // timeout of 0.5 sec - select(sec_sockfd_ + 1, &readfds, NULL, NULL, &timeout); - bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes - if (bytes_read > 0) - { - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); - robot_state_->unpack(buf, bytes_read); - } - else - { - connected_ = false; - robot_state_->setDisconnected(); - close(sec_sockfd_); - } - } - if (keepalive_) - { - // reconnect - print_warning("Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."); - sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (sec_sockfd_ < 0) - { - print_fatal("ERROR opening secondary socket"); - } - flag_ = 1; - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int)); - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); - setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int)); - fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); - while (keepalive_ && !connected_) - { - std::this_thread::sleep_for(std::chrono::seconds(10)); - fd_set writefds; - - connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_, sizeof(sec_serv_addr_)); - FD_ZERO(&writefds); - FD_SET(sec_sockfd_, &writefds); - select(sec_sockfd_ + 1, NULL, &writefds, NULL, NULL); - unsigned int flag_len; - getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); - if (flag_ < 0) - { - print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 " - "seconds..."); - } - else - { - connected_ = true; - print_info("Secondary port: Reconnected"); - } - } - } - } - - // wait for some traffic so the UR socket doesn't die in version 3.1. - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - close(sec_sockfd_); -} diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp deleted file mode 100644 index c5cf3bdd9..000000000 --- a/src/ur_driver.cpp +++ /dev/null @@ -1,420 +0,0 @@ -/* - * ur_driver.cpp - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "ur_modern_driver/ur_driver.h" - -UrDriver::UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host, - unsigned int reverse_port, double servoj_time, unsigned int safety_count_max, double max_time_step, - double min_payload, double max_payload, double servoj_lookahead_time, double servoj_gain) - : REVERSE_PORT_(reverse_port) - , maximum_time_step_(max_time_step) - , minimum_payload_(min_payload) - , maximum_payload_(max_payload) - , servoj_time_(servoj_time) - , servoj_lookahead_time_(servoj_lookahead_time) - , servoj_gain_(servoj_gain) -{ - char buffer[256]; - struct sockaddr_in serv_addr; - int n, flag; - - firmware_version_ = 0; - reverse_connected_ = false; - executing_traj_ = false; - rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host, safety_count_max); - new_sockfd_ = -1; - sec_interface_ = new UrCommunication(msg_cond, host); - - incoming_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (incoming_sockfd_ < 0) - { - print_fatal("ERROR opening socket for reverse communication"); - } - bzero((char*)&serv_addr, sizeof(serv_addr)); - - serv_addr.sin_family = AF_INET; - serv_addr.sin_addr.s_addr = INADDR_ANY; - serv_addr.sin_port = htons(REVERSE_PORT_); - flag = 1; - setsockopt(incoming_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag, sizeof(int)); - setsockopt(incoming_sockfd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int)); - if (bind(incoming_sockfd_, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0) - { - print_fatal("ERROR on binding socket for reverse communication"); - } - listen(incoming_sockfd_, 5); -} - -std::vector UrDriver::interp_cubic(double t, double T, std::vector p0_pos, std::vector p1_pos, - std::vector p0_vel, std::vector p1_vel) -{ - /*Returns positions of the joints at time 't' */ - std::vector positions; - for (unsigned int i = 0; i < p0_pos.size(); i++) - { - double a = p0_pos[i]; - double b = p0_vel[i]; - double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i] - T * p1_vel[i]) / pow(T, 2); - double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i] + T * p1_vel[i]) / pow(T, 3); - positions.push_back(a + b * t + c * pow(t, 2) + d * pow(t, 3)); - } - return positions; -} - -bool UrDriver::doTraj(std::vector inp_timestamps, std::vector> inp_positions, - std::vector> inp_velocities) -{ - std::chrono::high_resolution_clock::time_point t0, t; - std::vector positions; - unsigned int j; - - if (!UrDriver::uploadProg()) - { - return false; - } - executing_traj_ = true; - t0 = std::chrono::high_resolution_clock::now(); - t = t0; - j = 0; - while ((inp_timestamps[inp_timestamps.size() - 1] >= - std::chrono::duration_cast>(t - t0).count()) and - executing_traj_) - { - while (inp_timestamps[j] <= std::chrono::duration_cast>(t - t0).count() && - j < inp_timestamps.size() - 1) - { - j += 1; - } - positions = UrDriver::interp_cubic(std::chrono::duration_cast>(t - t0).count() - - inp_timestamps[j - 1], - inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1], - inp_positions[j], inp_velocities[j - 1], inp_velocities[j]); - UrDriver::servoj(positions); - - // oversample with 4 * sample_time - std::this_thread::sleep_for(std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.))); - t = std::chrono::high_resolution_clock::now(); - } - executing_traj_ = false; - // Signal robot to stop driverProg() - UrDriver::closeServo(positions); - return true; -} - -void UrDriver::servoj(std::vector positions, int keepalive) -{ - if (!reverse_connected_) - { - print_error("UrDriver::servoj called without a reverse connection present. Keepalive: " + - std::to_string(keepalive)); - return; - } - unsigned int bytes_written; - int tmp; - unsigned char buf[28]; - for (int i = 0; i < 6; i++) - { - tmp = htonl((int)(positions[i] * MULT_JOINTSTATE_)); - buf[i * 4] = tmp & 0xff; - buf[i * 4 + 1] = (tmp >> 8) & 0xff; - buf[i * 4 + 2] = (tmp >> 16) & 0xff; - buf[i * 4 + 3] = (tmp >> 24) & 0xff; - } - tmp = htonl((int)keepalive); - buf[6 * 4] = tmp & 0xff; - buf[6 * 4 + 1] = (tmp >> 8) & 0xff; - buf[6 * 4 + 2] = (tmp >> 16) & 0xff; - buf[6 * 4 + 3] = (tmp >> 24) & 0xff; - bytes_written = write(new_sockfd_, buf, 28); -} - -void UrDriver::stopTraj() -{ - executing_traj_ = false; - rt_interface_->addCommandToQueue("stopj(10)\n"); -} - -bool UrDriver::uploadProg() -{ - std::string cmd_str; - char buf[128]; - cmd_str = "def driverProg():\n"; - - sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_); - cmd_str += buf; - - cmd_str += "\tSERVO_IDLE = 0\n"; - cmd_str += "\tSERVO_RUNNING = 1\n"; - cmd_str += "\tcmd_servo_state = SERVO_IDLE\n"; - cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n"; - cmd_str += "\tdef set_servo_setpoint(q):\n"; - cmd_str += "\t\tenter_critical\n"; - cmd_str += "\t\tcmd_servo_state = SERVO_RUNNING\n"; - cmd_str += "\t\tcmd_servo_q = q\n"; - cmd_str += "\t\texit_critical\n"; - cmd_str += "\tend\n"; - cmd_str += "\tthread servoThread():\n"; - cmd_str += "\t\tstate = SERVO_IDLE\n"; - cmd_str += "\t\twhile True:\n"; - cmd_str += "\t\t\tenter_critical\n"; - cmd_str += "\t\t\tq = cmd_servo_q\n"; - cmd_str += "\t\t\tdo_brake = False\n"; - cmd_str += "\t\t\tif (state == SERVO_RUNNING) and "; - cmd_str += "(cmd_servo_state == SERVO_IDLE):\n"; - cmd_str += "\t\t\t\tdo_brake = True\n"; - cmd_str += "\t\t\tend\n"; - cmd_str += "\t\t\tstate = cmd_servo_state\n"; - cmd_str += "\t\t\tcmd_servo_state = SERVO_IDLE\n"; - cmd_str += "\t\t\texit_critical\n"; - cmd_str += "\t\t\tif do_brake:\n"; - cmd_str += "\t\t\t\tstopj(1.0)\n"; - cmd_str += "\t\t\t\tsync()\n"; - cmd_str += "\t\t\telif state == SERVO_RUNNING:\n"; - - if (sec_interface_->robot_state_->getVersion() >= 3.1) - sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=%.4f, gain=%.0f)\n", servoj_time_, servoj_lookahead_time_, - servoj_gain_); - else - sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_); - cmd_str += buf; - - cmd_str += "\t\t\telse:\n"; - cmd_str += "\t\t\t\tsync()\n"; - cmd_str += "\t\t\tend\n"; - cmd_str += "\t\tend\n"; - cmd_str += "\tend\n"; - - sprintf(buf, "\tsocket_open(\"%s\", %i)\n", ip_addr_.c_str(), REVERSE_PORT_); - cmd_str += buf; - - cmd_str += "\tthread_servo = run servoThread()\n"; - cmd_str += "\tkeepalive = 1\n"; - cmd_str += "\twhile keepalive > 0:\n"; - cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1)\n"; - cmd_str += "\t\tif params_mult[0] > 0:\n"; - cmd_str += "\t\t\tq = [params_mult[1] / MULT_jointstate, "; - cmd_str += "params_mult[2] / MULT_jointstate, "; - cmd_str += "params_mult[3] / MULT_jointstate, "; - cmd_str += "params_mult[4] / MULT_jointstate, "; - cmd_str += "params_mult[5] / MULT_jointstate, "; - cmd_str += "params_mult[6] / MULT_jointstate]\n"; - cmd_str += "\t\t\tkeepalive = params_mult[7]\n"; - cmd_str += "\t\t\tset_servo_setpoint(q)\n"; - cmd_str += "\t\tend\n"; - cmd_str += "\tend\n"; - cmd_str += "\tsleep(.1)\n"; - cmd_str += "\tsocket_close()\n"; - cmd_str += "\tkill thread_servo\n"; - cmd_str += "end\n"; - - rt_interface_->addCommandToQueue(cmd_str); - return UrDriver::openServo(); -} - -bool UrDriver::openServo() -{ - struct sockaddr_in cli_addr; - socklen_t clilen; - clilen = sizeof(cli_addr); - new_sockfd_ = accept(incoming_sockfd_, (struct sockaddr*)&cli_addr, &clilen); - if (new_sockfd_ < 0) - { - print_fatal("ERROR on accepting reverse communication"); - return false; - } - reverse_connected_ = true; - return true; -} -void UrDriver::closeServo(std::vector positions) -{ - if (positions.size() != 6) - UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0); - else - UrDriver::servoj(positions, 0); - - reverse_connected_ = false; - close(new_sockfd_); -} - -bool UrDriver::start() -{ - if (!sec_interface_->start()) - return false; - firmware_version_ = sec_interface_->robot_state_->getVersion(); - rt_interface_->robot_state_->setVersion(firmware_version_); - if (!rt_interface_->start()) - return false; - ip_addr_ = rt_interface_->getLocalIp(); - print_debug("Listening on " + ip_addr_ + ":" + std::to_string(REVERSE_PORT_) + "\n"); - return true; -} - -void UrDriver::halt() -{ - if (executing_traj_) - { - UrDriver::stopTraj(); - } - sec_interface_->halt(); - rt_interface_->halt(); - close(incoming_sockfd_); -} - -void UrDriver::setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc) -{ - rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc); -} - -std::vector UrDriver::getJointNames() -{ - return joint_names_; -} - -void UrDriver::setJointNames(std::vector jn) -{ - joint_names_ = jn; -} - -void UrDriver::setToolVoltage(unsigned int v) -{ - char buf[256]; - sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v); - rt_interface_->addCommandToQueue(buf); - print_debug(buf); -} -void UrDriver::setFlag(unsigned int n, bool b) -{ - char buf[256]; - sprintf(buf, "sec setOut():\n\tset_flag(%d, %s)\nend\n", n, b ? "True" : "False"); - rt_interface_->addCommandToQueue(buf); - print_debug(buf); -} -void UrDriver::setDigitalOut(unsigned int n, bool b) -{ - char buf[256]; - if (firmware_version_ < 2) - { - sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n, b ? "True" : "False"); - } - else if (n > 15) - { - sprintf(buf, "sec setOut():\n\tset_tool_digital_out(%d, %s)\nend\n", n - 16, b ? "True" : "False"); - } - else if (n > 7) - { - sprintf(buf, "sec setOut():\n\tset_configurable_digital_out(%d, %s)\nend\n", n - 8, b ? "True" : "False"); - } - else - { - sprintf(buf, "sec setOut():\n\tset_standard_digital_out(%d, %s)\nend\n", n, b ? "True" : "False"); - } - rt_interface_->addCommandToQueue(buf); - print_debug(buf); -} -void UrDriver::setAnalogOut(unsigned int n, double f) -{ - char buf[256]; - if (firmware_version_ < 2) - { - sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f); - } - else - { - sprintf(buf, "sec setOut():\n\tset_standard_analog_out(%d, %1.4f)\nend\n", n, f); - } - - rt_interface_->addCommandToQueue(buf); - print_debug(buf); -} - -bool UrDriver::setPayload(double m) -{ - if ((m < maximum_payload_) && (m > minimum_payload_)) - { - char buf[256]; - sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m); - rt_interface_->addCommandToQueue(buf); - print_debug(buf); - return true; - } - else - return false; -} - -void UrDriver::setMinPayload(double m) -{ - if (m > 0) - { - minimum_payload_ = m; - } - else - { - minimum_payload_ = 0; - } -} -void UrDriver::setMaxPayload(double m) -{ - maximum_payload_ = m; -} -void UrDriver::setServojTime(double t) -{ - if (t > 0.008) - { - servoj_time_ = t; - } - else - { - servoj_time_ = 0.008; - } -} -void UrDriver::setServojLookahead(double t) -{ - if (t > 0.03) - { - if (t < 0.2) - { - servoj_lookahead_time_ = t; - } - else - { - servoj_lookahead_time_ = 0.2; - } - } - else - { - servoj_lookahead_time_ = 0.03; - } -} -void UrDriver::setServojGain(double g) -{ - if (g > 100) - { - if (g < 2000) - { - servoj_gain_ = g; - } - else - { - servoj_gain_ = 2000; - } - } - else - { - servoj_gain_ = 100; - } -} diff --git a/src/ur_hardware_interface.cpp b/src/ur_hardware_interface.cpp deleted file mode 100644 index 06b59f979..000000000 --- a/src/ur_hardware_interface.cpp +++ /dev/null @@ -1,283 +0,0 @@ -/* - * ur_hardware_control_loop.cpp - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -/* Based on original source from University of Colorado, Boulder. License copied below. */ - -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2015, University of Colorado, Boulder - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Univ of CO, Boulder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - ********************************************************************* - - Author: Dave Coleman - */ - -#include - -namespace ros_control_ur -{ -UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) : nh_(nh), robot_(robot) -{ - // Initialize shared memory and interfaces here - init(); // this implementation loads from rosparam - - max_vel_change_ = 0.12; // equivalent of an acceleration of 15 rad/sec^2 - - ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface."); -} - -void UrHardwareInterface::init() -{ - ROS_INFO_STREAM_NAMED("ur_hardware_interface", "Reading rosparams from namespace: " << nh_.getNamespace()); - - // Get joint names - nh_.getParam("hardware_interface/joints", joint_names_); - if (joint_names_.size() == 0) - { - ROS_FATAL_STREAM_NAMED("ur_hardware_interface", - "No joints found on parameter server for controller, did you load the proper yaml file?" - << " Namespace: " << nh_.getNamespace()); - exit(-1); - } - num_joints_ = joint_names_.size(); - - // Resize vectors - joint_position_.resize(num_joints_); - joint_velocity_.resize(num_joints_); - joint_effort_.resize(num_joints_); - joint_position_command_.resize(num_joints_); - joint_velocity_command_.resize(num_joints_); - prev_joint_velocity_command_.resize(num_joints_); - - // Initialize controller - for (std::size_t i = 0; i < num_joints_; ++i) - { - ROS_DEBUG_STREAM_NAMED("ur_hardware_interface", "Loading joint name: " << joint_names_[i]); - - // Create joint state interface - joint_state_interface_.registerHandle(hardware_interface::JointStateHandle(joint_names_[i], &joint_position_[i], - &joint_velocity_[i], &joint_effort_[i])); - - // Create position joint interface - position_joint_interface_.registerHandle(hardware_interface::JointHandle( - joint_state_interface_.getHandle(joint_names_[i]), &joint_position_command_[i])); - - // Create velocity joint interface - velocity_joint_interface_.registerHandle(hardware_interface::JointHandle( - joint_state_interface_.getHandle(joint_names_[i]), &joint_velocity_command_[i])); - prev_joint_velocity_command_[i] = 0.; - } - - // Create force torque interface - force_torque_interface_.registerHandle( - hardware_interface::ForceTorqueSensorHandle("wrench", "", robot_force_, robot_torque_)); - - registerInterface(&joint_state_interface_); // From RobotHW base class. - registerInterface(&position_joint_interface_); // From RobotHW base class. - registerInterface(&velocity_joint_interface_); // From RobotHW base class. - registerInterface(&force_torque_interface_); // From RobotHW base class. - velocity_interface_running_ = false; - position_interface_running_ = false; -} - -void UrHardwareInterface::read() -{ - std::vector pos, vel, current, tcp; - pos = robot_->rt_interface_->robot_state_->getQActual(); - vel = robot_->rt_interface_->robot_state_->getQdActual(); - current = robot_->rt_interface_->robot_state_->getIActual(); - tcp = robot_->rt_interface_->robot_state_->getTcpForce(); - for (std::size_t i = 0; i < num_joints_; ++i) - { - joint_position_[i] = pos[i]; - joint_velocity_[i] = vel[i]; - joint_effort_[i] = current[i]; - } - for (std::size_t i = 0; i < 3; ++i) - { - robot_force_[i] = tcp[i]; - robot_torque_[i] = tcp[i + 3]; - } -} - -void UrHardwareInterface::setMaxVelChange(double inp) -{ - max_vel_change_ = inp; -} - -void UrHardwareInterface::write() -{ - if (velocity_interface_running_) - { - std::vector cmd; - // do some rate limiting - cmd.resize(joint_velocity_command_.size()); - for (unsigned int i = 0; i < joint_velocity_command_.size(); i++) - { - cmd[i] = joint_velocity_command_[i]; - if (cmd[i] > prev_joint_velocity_command_[i] + max_vel_change_) - { - cmd[i] = prev_joint_velocity_command_[i] + max_vel_change_; - } - else if (cmd[i] < prev_joint_velocity_command_[i] - max_vel_change_) - { - cmd[i] = prev_joint_velocity_command_[i] - max_vel_change_; - } - prev_joint_velocity_command_[i] = cmd[i]; - } - robot_->setSpeed(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], max_vel_change_ * 125); - } - else if (position_interface_running_) - { - robot_->servoj(joint_position_command_); - } -} - -bool UrHardwareInterface::canSwitch(const std::list& start_list, - const std::list& stop_list) const -{ - for (std::list::const_iterator controller_it = start_list.begin(); - controller_it != start_list.end(); ++controller_it) - { - if (controller_it->hardware_interface == "hardware_interface::VelocityJointInterface") - { - if (velocity_interface_running_) - { - ROS_ERROR("%s: An interface of that type (%s) is already running", controller_it->name.c_str(), - controller_it->hardware_interface.c_str()); - return false; - } - if (position_interface_running_) - { - bool error = true; - for (std::list::const_iterator stop_controller_it = stop_list.begin(); - stop_controller_it != stop_list.end(); ++stop_controller_it) - { - if (stop_controller_it->hardware_interface == "hardware_interface::PositionJointInterface") - { - error = false; - break; - } - } - if (error) - { - ROS_ERROR("%s (type %s) can not be run simultaneously with a PositionJointInterface", - controller_it->name.c_str(), controller_it->hardware_interface.c_str()); - return false; - } - } - } - else if (controller_it->hardware_interface == "hardware_interface::PositionJointInterface") - { - if (position_interface_running_) - { - ROS_ERROR("%s: An interface of that type (%s) is already running", controller_it->name.c_str(), - controller_it->hardware_interface.c_str()); - return false; - } - if (velocity_interface_running_) - { - bool error = true; - for (std::list::const_iterator stop_controller_it = stop_list.begin(); - stop_controller_it != stop_list.end(); ++stop_controller_it) - { - if (stop_controller_it->hardware_interface == "hardware_interface::VelocityJointInterface") - { - error = false; - break; - } - } - if (error) - { - ROS_ERROR("%s (type %s) can not be run simultaneously with a VelocityJointInterface", - controller_it->name.c_str(), controller_it->hardware_interface.c_str()); - return false; - } - } - } - } - - // we can always stop a controller - return true; -} - -void UrHardwareInterface::doSwitch(const std::list& start_list, - const std::list& stop_list) -{ - for (std::list::const_iterator controller_it = stop_list.begin(); - controller_it != stop_list.end(); ++controller_it) - { - if (controller_it->hardware_interface == "hardware_interface::VelocityJointInterface") - { - velocity_interface_running_ = false; - ROS_DEBUG("Stopping velocity interface"); - } - if (controller_it->hardware_interface == "hardware_interface::PositionJointInterface") - { - position_interface_running_ = false; - std::vector tmp; - robot_->closeServo(tmp); - ROS_DEBUG("Stopping position interface"); - } - } - for (std::list::const_iterator controller_it = start_list.begin(); - controller_it != start_list.end(); ++controller_it) - { - if (controller_it->hardware_interface == "hardware_interface::VelocityJointInterface") - { - velocity_interface_running_ = true; - ROS_DEBUG("Starting velocity interface"); - } - if (controller_it->hardware_interface == "hardware_interface::PositionJointInterface") - { - position_interface_running_ = true; - robot_->uploadProg(); - ROS_DEBUG("Starting position interface"); - } - } -} - -} // namespace diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp deleted file mode 100644 index e947aa9bb..000000000 --- a/src/ur_realtime_communication.cpp +++ /dev/null @@ -1,211 +0,0 @@ -/* - * ur_realtime_communication.cpp - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "ur_modern_driver/ur_realtime_communication.h" - -UrRealtimeCommunication::UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host, - unsigned int safety_count_max) -{ - robot_state_ = new RobotStateRT(msg_cond); - bzero((char*)&serv_addr_, sizeof(serv_addr_)); - sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (sockfd_ < 0) - { - print_fatal("ERROR opening socket"); - } - server_ = gethostbyname(host.c_str()); - if (server_ == NULL) - { - print_fatal("ERROR, no such host"); - } - serv_addr_.sin_family = AF_INET; - bcopy((char*)server_->h_addr, (char*)&serv_addr_.sin_addr.s_addr, server_->h_length); - serv_addr_.sin_port = htons(30003); - flag_ = 1; - setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int)); - setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); - setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int)); - fcntl(sockfd_, F_SETFL, O_NONBLOCK); - connected_ = false; - keepalive_ = false; - safety_count_ = safety_count_max + 1; - safety_count_max_ = safety_count_max; -} - -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_)); - FD_ZERO(&writefds); - FD_SET(sockfd_, &writefds); - timeout.tv_sec = 10; - timeout.tv_usec = 0; - select(sockfd_ + 1, NULL, &writefds, NULL, &timeout); - unsigned int flag_len; - getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); - if (flag_ < 0) - { - print_fatal("Error connecting to RT port 30003"); - return false; - } - sockaddr_in name; - socklen_t namelen = sizeof(name); - int err = getsockname(sockfd_, (sockaddr*)&name, &namelen); - if (err < 0) - { - print_fatal("Could not get local IP"); - close(sockfd_); - return false; - } - char str[18]; - inet_ntop(AF_INET, &name.sin_addr, str, 18); - local_ip_ = str; - comThread_ = std::thread(&UrRealtimeCommunication::run, this); - return true; -} - -void UrRealtimeCommunication::halt() -{ - keepalive_ = false; - comThread_.join(); -} - -void UrRealtimeCommunication::addCommandToQueue(std::string inp) -{ - int bytes_written; - if (inp.back() != '\n') - { - inp.append("\n"); - } - if (connected_) - bytes_written = write(sockfd_, inp.c_str(), inp.length()); - else - print_error("Could not send command \"" + inp + "\". The robot is not connected! Command is discarded"); -} - -void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc) -{ - char cmd[1024]; - if (robot_state_->getVersion() >= 3.1) - { - sprintf(cmd, "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n", q0, q1, q2, q3, q4, q5, acc); - } - else - { - sprintf(cmd, "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n", q0, q1, q2, q3, q4, q5, acc); - } - addCommandToQueue((std::string)(cmd)); - if (q0 != 0. or q1 != 0. or q2 != 0. or q3 != 0. or q4 != 0. or q5 != 0.) - { - // If a joint speed is set, make sure we stop it again after some time if the user doesn't - safety_count_ = 0; - } -} - -void UrRealtimeCommunication::run() -{ - uint8_t buf[2048]; - int bytes_read; - bzero(buf, 2048); - struct timeval timeout; - fd_set readfds; - FD_ZERO(&readfds); - FD_SET(sockfd_, &readfds); - print_debug("Realtime port: Got connection"); - connected_ = true; - while (keepalive_) - { - while (connected_ && keepalive_) - { - timeout.tv_sec = 0; // do this each loop as selects modifies timeout - timeout.tv_usec = 500000; // timeout of 0.5 sec - select(sockfd_ + 1, &readfds, NULL, NULL, &timeout); - bytes_read = read(sockfd_, buf, 2048); - if (bytes_read > 0) - { - setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); - robot_state_->unpack(buf); - if (safety_count_ == safety_count_max_) - { - setSpeed(0., 0., 0., 0., 0., 0.); - } - safety_count_ += 1; - } - else - { - connected_ = false; - close(sockfd_); - } - } - if (keepalive_) - { - // reconnect - print_warning("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."); - sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (sockfd_ < 0) - { - print_fatal("ERROR opening socket"); - } - flag_ = 1; - setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int)); - setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); - - setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int)); - fcntl(sockfd_, F_SETFL, O_NONBLOCK); - while (keepalive_ && !connected_) - { - std::this_thread::sleep_for(std::chrono::seconds(10)); - fd_set writefds; - - connect(sockfd_, (struct sockaddr*)&serv_addr_, sizeof(serv_addr_)); - FD_ZERO(&writefds); - FD_SET(sockfd_, &writefds); - select(sockfd_ + 1, NULL, &writefds, NULL, NULL); - unsigned int flag_len; - getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); - if (flag_ < 0) - { - print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 " - "seconds..."); - } - else - { - connected_ = true; - print_info("Realtime port: Reconnected"); - } - } - } - } - setSpeed(0., 0., 0., 0., 0., 0.); - close(sockfd_); -} - -void UrRealtimeCommunication::setSafetyCountMax(uint inp) -{ - safety_count_max_ = inp; -} - -std::string UrRealtimeCommunication::getLocalIp() -{ - return local_ip_; -} diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp deleted file mode 100644 index 6f3f85404..000000000 --- a/src/ur_ros_wrapper.cpp +++ /dev/null @@ -1,871 +0,0 @@ -/* - * ur_driver.cpp - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "ur_modern_driver/do_output.h" -#include "ur_modern_driver/ur_driver.h" -#include "ur_modern_driver/ur_hardware_interface.h" - -#include -#include -#include -#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" - -/// TF -#include -#include - -class RosWrapper -{ -protected: - UrDriver robot_; - std::condition_variable rt_msg_cond_; - std::condition_variable msg_cond_; - ros::NodeHandle nh_; - actionlib::ActionServer as_; - actionlib::ServerGoalHandle goal_handle_; - bool has_goal_; - control_msgs::FollowJointTrajectoryFeedback feedback_; - control_msgs::FollowJointTrajectoryResult result_; - ros::Subscriber speed_sub_; - ros::Subscriber urscript_sub_; - ros::ServiceServer io_srv_; - ros::ServiceServer payload_srv_; - std::thread* rt_publish_thread_; - std::thread* mb_publish_thread_; - double io_flag_delay_; - double max_velocity_; - std::vector joint_offsets_; - std::string base_frame_; - std::string tool_frame_; - bool use_ros_control_; - std::thread* ros_control_thread_; - boost::shared_ptr hardware_interface_; - boost::shared_ptr controller_manager_; - -public: - RosWrapper(std::string host, int reverse_port) - : as_(nh_, "follow_joint_trajectory", boost::bind(&RosWrapper::goalCB, this, _1), - boost::bind(&RosWrapper::cancelCB, this, _1), false) - , robot_(rt_msg_cond_, msg_cond_, host, reverse_port, 0.03, 300) - , io_flag_delay_(0.05) - , joint_offsets_(6, 0.0) - { - std::string joint_prefix = ""; - std::vector joint_names; - char buf[256]; - - if (ros::param::get("~prefix", joint_prefix)) - { - if (joint_prefix.length() > 0) - { - sprintf(buf, "Setting prefix to %s", joint_prefix.c_str()); - print_info(buf); - } - } - joint_names.push_back(joint_prefix + "shoulder_pan_joint"); - joint_names.push_back(joint_prefix + "shoulder_lift_joint"); - joint_names.push_back(joint_prefix + "elbow_joint"); - joint_names.push_back(joint_prefix + "wrist_1_joint"); - joint_names.push_back(joint_prefix + "wrist_2_joint"); - joint_names.push_back(joint_prefix + "wrist_3_joint"); - robot_.setJointNames(joint_names); - - use_ros_control_ = false; - ros::param::get("~use_ros_control", use_ros_control_); - - if (use_ros_control_) - { - hardware_interface_.reset(new ros_control_ur::UrHardwareInterface(nh_, &robot_)); - controller_manager_.reset(new controller_manager::ControllerManager(hardware_interface_.get(), nh_)); - double max_vel_change = 0.12; // equivalent of an acceleration of 15 rad/sec^2 - if (ros::param::get("~max_acceleration", max_vel_change)) - { - max_vel_change = max_vel_change / 125; - } - sprintf(buf, "Max acceleration set to: %f [rad/sec²]", max_vel_change * 125); - print_debug(buf); - hardware_interface_->setMaxVelChange(max_vel_change); - } - // Using a very high value in order to not limit execution of trajectories being sent from MoveIt! - max_velocity_ = 10.; - if (ros::param::get("~max_velocity", max_velocity_)) - { - sprintf(buf, "Max velocity accepted by ur_driver: %f [rad/s]", max_velocity_); - print_debug(buf); - } - - // Bounds for SetPayload service - // Using a very conservative value as it should be set through the parameter server - double min_payload = 0.; - double max_payload = 1.; - if (ros::param::get("~min_payload", min_payload)) - { - sprintf(buf, "Min payload set to: %f [kg]", min_payload); - print_debug(buf); - } - if (ros::param::get("~max_payload", max_payload)) - { - sprintf(buf, "Max payload set to: %f [kg]", max_payload); - print_debug(buf); - } - robot_.setMinPayload(min_payload); - robot_.setMaxPayload(max_payload); - sprintf(buf, "Bounds for set_payload service calls: [%f, %f]", min_payload, max_payload); - print_debug(buf); - - double servoj_time = 0.008; - if (ros::param::get("~servoj_time", servoj_time)) - { - sprintf(buf, "Servoj_time set to: %f [sec]", servoj_time); - print_debug(buf); - } - robot_.setServojTime(servoj_time); - - double servoj_lookahead_time = 0.03; - if (ros::param::get("~servoj_lookahead_time", servoj_lookahead_time)) - { - sprintf(buf, "Servoj_lookahead_time set to: %f [sec]", servoj_lookahead_time); - print_debug(buf); - } - robot_.setServojLookahead(servoj_lookahead_time); - - double servoj_gain = 300.; - if (ros::param::get("~servoj_gain", servoj_gain)) - { - sprintf(buf, "Servoj_gain set to: %f [sec]", servoj_gain); - print_debug(buf); - } - robot_.setServojGain(servoj_gain); - - // Base and tool frames - base_frame_ = joint_prefix + "base_link"; - tool_frame_ = joint_prefix + "tool0_controller"; - if (ros::param::get("~base_frame", base_frame_)) - { - sprintf(buf, "Base frame set to: %s", base_frame_.c_str()); - print_debug(buf); - } - if (ros::param::get("~tool_frame", tool_frame_)) - { - sprintf(buf, "Tool frame set to: %s", tool_frame_.c_str()); - print_debug(buf); - } - - if (robot_.start()) - { - if (use_ros_control_) - { - ros_control_thread_ = new std::thread(boost::bind(&RosWrapper::rosControlLoop, this)); - print_debug("The control thread for this driver has been started"); - } - else - { - // start actionserver - has_goal_ = false; - as_.start(); - - // subscribe to the data topic of interest - rt_publish_thread_ = new std::thread(boost::bind(&RosWrapper::publishRTMsg, this)); - print_debug("The action server for this driver has been started"); - } - mb_publish_thread_ = new std::thread(boost::bind(&RosWrapper::publishMbMsg, this)); - speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1, &RosWrapper::speedInterface, this); - urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, &RosWrapper::urscriptInterface, this); - - io_srv_ = nh_.advertiseService("ur_driver/set_io", &RosWrapper::setIO, this); - payload_srv_ = nh_.advertiseService("ur_driver/set_payload", &RosWrapper::setPayload, this); - } - } - - void halt() - { - robot_.halt(); - rt_publish_thread_->join(); - } - -private: - void trajThread(std::vector timestamps, std::vector> positions, - std::vector> velocities) - { - robot_.doTraj(timestamps, positions, velocities); - if (has_goal_) - { - result_.error_code = result_.SUCCESSFUL; - goal_handle_.setSucceeded(result_); - has_goal_ = false; - } - } - void goalCB(actionlib::ServerGoalHandle gh) - { - std::string buf; - print_info("on_goal"); - if (!robot_.sec_interface_->robot_state_->isReady()) - { - result_.error_code = -100; // nothing is defined for this...? - - if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) - { - result_.error_string = "Cannot accept new trajectories: Robot arm is not powered on"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - if (!robot_.sec_interface_->robot_state_->isRealRobotEnabled()) - { - result_.error_string = "Cannot accept new trajectories: Robot is not enabled"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - result_.error_string = "Cannot accept new trajectories. (Debug: Robot mode is " + - std::to_string(robot_.sec_interface_->robot_state_->getRobotMode()) + ")"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - if (robot_.sec_interface_->robot_state_->isEmergencyStopped()) - { - result_.error_code = -100; // nothing is defined for this...? - result_.error_string = "Cannot accept new trajectories: Robot is emergency stopped"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - if (robot_.sec_interface_->robot_state_->isProtectiveStopped()) - { - result_.error_code = -100; // nothing is defined for this...? - result_.error_string = "Cannot accept new trajectories: Robot is protective stopped"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - - actionlib::ActionServer::Goal goal = - *gh.getGoal(); // make a copy that we can modify - if (has_goal_) - { - print_warning("Received new goal while still executing previous trajectory. Canceling previous trajectory"); - has_goal_ = false; - robot_.stopTraj(); - result_.error_code = -100; // nothing is defined for this...? - result_.error_string = "Received another trajectory"; - goal_handle_.setAborted(result_, result_.error_string); - std::this_thread::sleep_for(std::chrono::milliseconds(250)); - } - goal_handle_ = gh; - if (!validateJointNames()) - { - std::string outp_joint_names = ""; - for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) - { - outp_joint_names += goal.trajectory.joint_names[i] + " "; - } - result_.error_code = result_.INVALID_JOINTS; - result_.error_string = "Received a goal with incorrect joint names: " + outp_joint_names; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - if (!has_positions()) - { - result_.error_code = result_.INVALID_GOAL; - result_.error_string = "Received a goal without positions"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - - if (!has_velocities()) - { - result_.error_code = result_.INVALID_GOAL; - result_.error_string = "Received a goal without velocities"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - - if (!traj_is_finite()) - { - result_.error_string = "Received a goal with infinities or NaNs"; - result_.error_code = result_.INVALID_GOAL; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - - if (!has_limited_velocities()) - { - result_.error_code = result_.INVALID_GOAL; - result_.error_string = "Received a goal with velocities that are higher than " + std::to_string(max_velocity_); - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - - reorder_traj_joints(goal.trajectory); - - if (!start_positions_match(goal.trajectory, 0.01)) - { - result_.error_code = result_.INVALID_GOAL; - result_.error_string = "Goal start doesn't match current pose"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - - std::vector timestamps; - std::vector> positions, velocities; - if (goal.trajectory.points[0].time_from_start.toSec() != 0.) - { - print_warning("Trajectory's first point should be the current position, with time_from_start set to 0.0 - " - "Inserting point in malformed trajectory"); - timestamps.push_back(0.0); - positions.push_back(robot_.rt_interface_->robot_state_->getQActual()); - velocities.push_back(robot_.rt_interface_->robot_state_->getQdActual()); - } - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) - { - timestamps.push_back(goal.trajectory.points[i].time_from_start.toSec()); - positions.push_back(goal.trajectory.points[i].positions); - velocities.push_back(goal.trajectory.points[i].velocities); - } - - goal_handle_.setAccepted(); - has_goal_ = true; - std::thread(&RosWrapper::trajThread, this, timestamps, positions, velocities).detach(); - } - - void cancelCB(actionlib::ServerGoalHandle gh) - { - // set the action state to preempted - print_info("on_cancel"); - if (has_goal_) - { - if (gh == goal_handle_) - { - robot_.stopTraj(); - has_goal_ = false; - } - } - result_.error_code = -100; // nothing is defined for this...? - result_.error_string = "Goal cancelled by client"; - gh.setCanceled(result_); - } - - bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp) - { - resp.success = true; - // if (req.fun == ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT) { - if (req.fun == 1) - { - robot_.setDigitalOut(req.pin, req.state > 0.0 ? true : false); - } - else if (req.fun == 2) - { - //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_FLAG) { - robot_.setFlag(req.pin, req.state > 0.0 ? true : false); - // According to urdriver.py, set_flag will fail if called to rapidly in succession - ros::Duration(io_flag_delay_).sleep(); - } - else if (req.fun == 3) - { - //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_ANALOG_OUT) { - robot_.setAnalogOut(req.pin, req.state); - } - else if (req.fun == 4) - { - //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_TOOL_VOLTAGE) { - robot_.setToolVoltage((int)req.state); - } - else - { - resp.success = false; - } - return resp.success; - } - - bool setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::SetPayloadResponse& resp) - { - if (robot_.setPayload(req.payload)) - resp.success = true; - else - resp.success = true; - return resp.success; - } - - bool validateJointNames() - { - std::vector actual_joint_names = robot_.getJointNames(); - actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); - if (goal.trajectory.joint_names.size() != actual_joint_names.size()) - return false; - - for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) - { - unsigned int j; - for (j = 0; j < actual_joint_names.size(); j++) - { - if (goal.trajectory.joint_names[i] == actual_joint_names[j]) - break; - } - if (goal.trajectory.joint_names[i] == actual_joint_names[j]) - { - actual_joint_names.erase(actual_joint_names.begin() + j); - } - else - { - return false; - } - } - - return true; - } - - void reorder_traj_joints(trajectory_msgs::JointTrajectory& traj) - { - /* Reorders trajectory - destructive */ - std::vector actual_joint_names = robot_.getJointNames(); - std::vector mapping; - mapping.resize(actual_joint_names.size(), actual_joint_names.size()); - for (unsigned int i = 0; i < traj.joint_names.size(); i++) - { - for (unsigned int j = 0; j < actual_joint_names.size(); j++) - { - if (traj.joint_names[i] == actual_joint_names[j]) - mapping[j] = i; - } - } - traj.joint_names = actual_joint_names; - std::vector new_traj; - for (unsigned int i = 0; i < traj.points.size(); i++) - { - trajectory_msgs::JointTrajectoryPoint new_point; - for (unsigned int j = 0; j < traj.points[i].positions.size(); j++) - { - new_point.positions.push_back(traj.points[i].positions[mapping[j]]); - new_point.velocities.push_back(traj.points[i].velocities[mapping[j]]); - if (traj.points[i].accelerations.size() != 0) - new_point.accelerations.push_back(traj.points[i].accelerations[mapping[j]]); - } - new_point.time_from_start = traj.points[i].time_from_start; - new_traj.push_back(new_point); - } - traj.points = new_traj; - } - - bool has_velocities() - { - actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) - { - if (goal.trajectory.points[i].positions.size() != goal.trajectory.points[i].velocities.size()) - return false; - } - return true; - } - - bool has_positions() - { - actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); - if (goal.trajectory.points.size() == 0) - return false; - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) - { - if (goal.trajectory.points[i].positions.size() != goal.trajectory.joint_names.size()) - return false; - } - return true; - } - - bool start_positions_match(const trajectory_msgs::JointTrajectory& traj, double eps) - { - for (unsigned int i = 0; i < traj.points[0].positions.size(); i++) - { - std::vector qActual = robot_.rt_interface_->robot_state_->getQActual(); - if (fabs(traj.points[0].positions[i] - qActual[i]) > eps) - { - return false; - } - } - return true; - } - - bool has_limited_velocities() - { - actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) - { - for (unsigned int j = 0; j < goal.trajectory.points[i].velocities.size(); j++) - { - if (fabs(goal.trajectory.points[i].velocities[j]) > max_velocity_) - return false; - } - } - return true; - } - - bool traj_is_finite() - { - actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) - { - for (unsigned int j = 0; j < goal.trajectory.points[i].velocities.size(); j++) - { - if (!std::isfinite(goal.trajectory.points[i].positions[j])) - return false; - if (!std::isfinite(goal.trajectory.points[i].velocities[j])) - return false; - } - } - return true; - } - - void speedInterface(const trajectory_msgs::JointTrajectory::Ptr& msg) - { - if (msg->points[0].velocities.size() == 6) - { - double acc = 100; - if (msg->points[0].accelerations.size() > 0) - acc = *std::max_element(msg->points[0].accelerations.begin(), msg->points[0].accelerations.end()); - robot_.setSpeed(msg->points[0].velocities[0], msg->points[0].velocities[1], msg->points[0].velocities[2], - msg->points[0].velocities[3], msg->points[0].velocities[4], msg->points[0].velocities[5], acc); - } - } - void urscriptInterface(const std_msgs::String::ConstPtr& msg) - { - robot_.rt_interface_->addCommandToQueue(msg->data); - } - - void rosControlLoop() - { - ros::Duration elapsed_time; - struct timespec last_time, current_time; - static const double BILLION = 1000000000.0; - - realtime_tools::RealtimePublisher tf_pub(nh_, "/tf", 1); - geometry_msgs::TransformStamped tool_transform; - tool_transform.header.frame_id = base_frame_; - tool_transform.child_frame_id = tool_frame_; - tf_pub.msg_.transforms.push_back(tool_transform); - - realtime_tools::RealtimePublisher tool_vel_pub(nh_, "tool_velocity", 1); - tool_vel_pub.msg_.header.frame_id = base_frame_; - - clock_gettime(CLOCK_MONOTONIC, &last_time); - while (ros::ok()) - { - std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex - std::unique_lock locker(msg_lock); - while (!robot_.rt_interface_->robot_state_->getControllerUpdated()) - { - rt_msg_cond_.wait(locker); - } - // Input - hardware_interface_->read(); - robot_.rt_interface_->robot_state_->setControllerUpdated(); - - // Control - clock_gettime(CLOCK_MONOTONIC, ¤t_time); - elapsed_time = - ros::Duration(current_time.tv_sec - last_time.tv_sec + (current_time.tv_nsec - last_time.tv_nsec) / BILLION); - ros::Time ros_time = ros::Time::now(); - controller_manager_->update(ros_time, elapsed_time); - last_time = current_time; - - // Output - hardware_interface_->write(); - - // Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation - // vector representation of the tool orientation - std::vector tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual(); - - // Compute rotation angle - double rx = tool_vector_actual[3]; - double ry = tool_vector_actual[4]; - double rz = tool_vector_actual[5]; - double angle = std::sqrt(std::pow(rx, 2) + std::pow(ry, 2) + std::pow(rz, 2)); - - // Broadcast transform - if (tf_pub.trylock()) - { - tf_pub.msg_.transforms[0].header.stamp = ros_time; - if (angle < 1e-16) - { - tf_pub.msg_.transforms[0].transform.rotation.x = 0; - tf_pub.msg_.transforms[0].transform.rotation.y = 0; - tf_pub.msg_.transforms[0].transform.rotation.z = 0; - tf_pub.msg_.transforms[0].transform.rotation.w = 1; - } - else - { - tf_pub.msg_.transforms[0].transform.rotation.x = (rx / angle) * std::sin(angle * 0.5); - tf_pub.msg_.transforms[0].transform.rotation.y = (ry / angle) * std::sin(angle * 0.5); - tf_pub.msg_.transforms[0].transform.rotation.z = (rz / angle) * std::sin(angle * 0.5); - tf_pub.msg_.transforms[0].transform.rotation.w = std::cos(angle * 0.5); - } - tf_pub.msg_.transforms[0].transform.translation.x = tool_vector_actual[0]; - tf_pub.msg_.transforms[0].transform.translation.y = tool_vector_actual[1]; - tf_pub.msg_.transforms[0].transform.translation.z = tool_vector_actual[2]; - - tf_pub.unlockAndPublish(); - } - - // Publish tool velocity - std::vector tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual(); - - if (tool_vel_pub.trylock()) - { - tool_vel_pub.msg_.header.stamp = ros_time; - tool_vel_pub.msg_.twist.linear.x = tcp_speed[0]; - tool_vel_pub.msg_.twist.linear.y = tcp_speed[1]; - tool_vel_pub.msg_.twist.linear.z = tcp_speed[2]; - tool_vel_pub.msg_.twist.angular.x = tcp_speed[3]; - tool_vel_pub.msg_.twist.angular.y = tcp_speed[4]; - tool_vel_pub.msg_.twist.angular.z = tcp_speed[5]; - - tool_vel_pub.unlockAndPublish(); - } - } - } - - void publishRTMsg() - { - ros::Publisher joint_pub = nh_.advertise("joint_states", 1); - ros::Publisher wrench_pub = nh_.advertise("wrench", 1); - ros::Publisher tool_vel_pub = nh_.advertise("tool_velocity", 1); - static tf::TransformBroadcaster br; - while (ros::ok()) - { - sensor_msgs::JointState joint_msg; - joint_msg.name = robot_.getJointNames(); - geometry_msgs::WrenchStamped wrench_msg; - geometry_msgs::PoseStamped tool_pose_msg; - std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex - std::unique_lock locker(msg_lock); - while (!robot_.rt_interface_->robot_state_->getDataPublished()) - { - rt_msg_cond_.wait(locker); - } - joint_msg.header.stamp = ros::Time::now(); - joint_msg.position = robot_.rt_interface_->robot_state_->getQActual(); - for (unsigned int i = 0; i < joint_msg.position.size(); i++) - { - joint_msg.position[i] += joint_offsets_[i]; - } - joint_msg.velocity = robot_.rt_interface_->robot_state_->getQdActual(); - joint_msg.effort = robot_.rt_interface_->robot_state_->getIActual(); - joint_pub.publish(joint_msg); - std::vector tcp_force = robot_.rt_interface_->robot_state_->getTcpForce(); - wrench_msg.header.stamp = joint_msg.header.stamp; - wrench_msg.wrench.force.x = tcp_force[0]; - wrench_msg.wrench.force.y = tcp_force[1]; - wrench_msg.wrench.force.z = tcp_force[2]; - wrench_msg.wrench.torque.x = tcp_force[3]; - wrench_msg.wrench.torque.y = tcp_force[4]; - wrench_msg.wrench.torque.z = tcp_force[5]; - wrench_pub.publish(wrench_msg); - - // Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation - // vector representation of the tool orientation - std::vector tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual(); - - // Create quaternion - tf::Quaternion quat; - 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)); - if (angle < 1e-16) - { - quat.setValue(0, 0, 0, 1); - } - else - { - quat.setRotation(tf::Vector3(rx / angle, ry / angle, rz / angle), angle); - } - - // Create and broadcast transform - tf::Transform transform; - transform.setOrigin(tf::Vector3(tool_vector_actual[0], tool_vector_actual[1], tool_vector_actual[2])); - transform.setRotation(quat); - br.sendTransform(tf::StampedTransform(transform, joint_msg.header.stamp, base_frame_, tool_frame_)); - - // Publish tool velocity - std::vector tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual(); - geometry_msgs::TwistStamped tool_twist; - tool_twist.header.frame_id = base_frame_; - tool_twist.header.stamp = joint_msg.header.stamp; - tool_twist.twist.linear.x = tcp_speed[0]; - tool_twist.twist.linear.y = tcp_speed[1]; - tool_twist.twist.linear.z = tcp_speed[2]; - tool_twist.twist.angular.x = tcp_speed[3]; - tool_twist.twist.angular.y = tcp_speed[4]; - tool_twist.twist.angular.z = tcp_speed[5]; - tool_vel_pub.publish(tool_twist); - - robot_.rt_interface_->robot_state_->setDataPublished(); - } - } - - void publishMbMsg() - { - bool warned = false; - ros::Publisher io_pub = nh_.advertise("ur_driver/io_states", 1); - - while (ros::ok()) - { - ur_msgs::IOStates io_msg; - std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex - std::unique_lock locker(msg_lock); - while (!robot_.sec_interface_->robot_state_->getNewDataAvailable()) - { - msg_cond_.wait(locker); - } - int i_max = 10; - if (robot_.sec_interface_->robot_state_->getVersion() > 3.0) - i_max = 18; // From version 3.0, there are up to 18 inputs and outputs - for (unsigned int i = 0; i < i_max; i++) - { - ur_msgs::Digital digi; - digi.pin = i; - digi.state = ((robot_.sec_interface_->robot_state_->getDigitalInputBits() & (1 << i)) >> i); - io_msg.digital_in_states.push_back(digi); - digi.state = ((robot_.sec_interface_->robot_state_->getDigitalOutputBits() & (1 << i)) >> i); - io_msg.digital_out_states.push_back(digi); - } - ur_msgs::Analog ana; - ana.pin = 0; - ana.state = robot_.sec_interface_->robot_state_->getAnalogInput0(); - io_msg.analog_in_states.push_back(ana); - ana.pin = 1; - ana.state = robot_.sec_interface_->robot_state_->getAnalogInput1(); - io_msg.analog_in_states.push_back(ana); - - ana.pin = 0; - ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput0(); - io_msg.analog_out_states.push_back(ana); - ana.pin = 1; - ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput1(); - io_msg.analog_out_states.push_back(ana); - io_pub.publish(io_msg); - - if (robot_.sec_interface_->robot_state_->isEmergencyStopped() or - robot_.sec_interface_->robot_state_->isProtectiveStopped()) - { - if (robot_.sec_interface_->robot_state_->isEmergencyStopped() and !warned) - { - print_error("Emergency stop pressed!"); - } - else if (robot_.sec_interface_->robot_state_->isProtectiveStopped() and !warned) - { - print_error("Robot is protective stopped!"); - } - if (has_goal_) - { - print_error("Aborting trajectory"); - robot_.stopTraj(); - result_.error_code = result_.SUCCESSFUL; - result_.error_string = "Robot was halted"; - goal_handle_.setAborted(result_, result_.error_string); - has_goal_ = false; - } - warned = true; - } - else - warned = false; - - robot_.sec_interface_->robot_state_->finishedReading(); - } - } -}; - -int main(int argc, char** argv) -{ - bool use_sim_time = false; - std::string host; - int reverse_port = 50001; - - ros::init(argc, argv, "ur_driver"); - ros::NodeHandle nh; - if (ros::param::get("use_sim_time", use_sim_time)) - { - print_warning("use_sim_time is set!!"); - } - if (!(ros::param::get("~robot_ip_address", host))) - { - if (argc > 1) - { - print_warning("Please set the parameter robot_ip_address instead of giving it as a command line argument. This " - "method is DEPRECATED"); - host = argv[1]; - } - else - { - print_fatal("Could not get robot ip. Please supply it as command line parameter or on the parameter server as " - "robot_ip"); - exit(1); - } - } - if ((ros::param::get("~reverse_port", reverse_port))) - { - if ((reverse_port <= 0) or (reverse_port >= 65535)) - { - print_warning("Reverse port value is not valid (Use number between 1 and 65534. Using default value of 50001"); - reverse_port = 50001; - } - } - else - reverse_port = 50001; - - RosWrapper interface(host, reverse_port); - - ros::AsyncSpinner spinner(3); - spinner.start(); - - ros::waitForShutdown(); - - interface.halt(); - - exit(0); -} From 577fcdbf986e1e8e4924a1023f996847f2c0b3e5 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Sun, 9 Jul 2017 02:45:58 +0200 Subject: [PATCH 059/114] Fixed compiler warnings --- include/ur_modern_driver/ros/action_server.h | 6 +++--- include/ur_modern_driver/ros/controller.h | 2 +- .../ur_modern_driver/ros/hardware_interface.h | 5 +++-- .../ur_modern_driver/ros/trajectory_follower.h | 4 ++-- include/ur_modern_driver/ur/state_parser.h | 18 +++++++----------- src/ros/action_server.cpp | 8 +++----- src/ros/controller.cpp | 4 ++-- src/ros/hardware_interface.cpp | 10 +++++----- src/ros/trajectory_follower.cpp | 5 ++--- src/ros_main.cpp | 2 +- 10 files changed, 29 insertions(+), 35 deletions(-) diff --git a/include/ur_modern_driver/ros/action_server.h b/include/ur_modern_driver/ros/action_server.h index f18c6e9ba..1d62b8f6e 100644 --- a/include/ur_modern_driver/ros/action_server.h +++ b/include/ur_modern_driver/ros/action_server.h @@ -32,9 +32,6 @@ class ActionServer : public Service, public URRTPacketConsumer std::set joint_set_; double max_velocity_; - RobotState state_; - std::array q_actual_, qd_actual_; - GoalHandle curr_gh_; std::atomic interrupt_traj_; std::atomic has_goal_, running_; @@ -44,6 +41,9 @@ class ActionServer : public Service, public URRTPacketConsumer TrajectoryFollower& follower_; + RobotState state_; + std::array q_actual_, qd_actual_; + void onGoal(GoalHandle gh); void onCancel(GoalHandle gh); diff --git a/include/ur_modern_driver/ros/controller.h b/include/ur_modern_driver/ros/controller.h index 3532ee6ae..b2780d537 100644 --- a/include/ur_modern_driver/ros/controller.h +++ b/include/ur_modern_driver/ros/controller.h @@ -55,7 +55,7 @@ class ROSController : private hardware_interface::RobotHW, public URRTPacketCons void reset(); public: - ROSController(URCommander& commander, std::vector& joint_names, double max_vel_change); + ROSController(URCommander& commander, TrajectoryFollower& follower, std::vector& joint_names, double max_vel_change); virtual ~ROSController() { } // from RobotHW void doSwitch(const std::list& start_list, const std::list& stop_list); diff --git a/include/ur_modern_driver/ros/hardware_interface.h b/include/ur_modern_driver/ros/hardware_interface.h index 325faa4b4..93bd0c729 100644 --- a/include/ur_modern_driver/ros/hardware_interface.h +++ b/include/ur_modern_driver/ros/hardware_interface.h @@ -6,6 +6,7 @@ #include #include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/rt_state.h" +#include "ur_modern_driver/ros/trajectory_follower.h" class HardwareInterface { @@ -56,11 +57,11 @@ class VelocityInterface : public HardwareInterface, public hardware_interface::V class PositionInterface : public HardwareInterface, public hardware_interface::PositionJointInterface { private: - URCommander &commander_; + TrajectoryFollower& follower_; std::array position_cmd_; public: - PositionInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names); + PositionInterface(TrajectoryFollower& follower, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names); virtual bool write(); virtual void start(); virtual void stop(); diff --git a/include/ur_modern_driver/ros/trajectory_follower.h b/include/ur_modern_driver/ros/trajectory_follower.h index be59a5641..da01004dc 100644 --- a/include/ur_modern_driver/ros/trajectory_follower.h +++ b/include/ur_modern_driver/ros/trajectory_follower.h @@ -33,12 +33,12 @@ struct TrajectoryPoint class TrajectoryFollower { private: - double servoj_time_, servoj_lookahead_time_, servoj_gain_; std::atomic running_; std::array last_positions_; URCommander &commander_; URServer server_; - int reverse_port_; + + double servoj_time_, servoj_lookahead_time_, servoj_gain_; std::string program_; template diff --git a/include/ur_modern_driver/ur/state_parser.h b/include/ur_modern_driver/ur/state_parser.h index 494558f60..fc8024250 100644 --- a/include/ur_modern_driver/ur/state_parser.h +++ b/include/ur_modern_driver/ur/state_parser.h @@ -33,19 +33,18 @@ class URStateParser : public URParser bp.parse(packet_size); bp.parse(type); - //quietly ignore the intial version message - if (type == message_type::ROBOT_MESSAGE || ((int)type) == 5) + if (type != message_type::ROBOT_STATE) { + //quietly ignore the intial version message + if(type != message_type::ROBOT_MESSAGE) + { + LOG_WARN("Invalid state message type recieved: %u", static_cast(type)); + } + bp.consume(); return true; } - if (type != message_type::ROBOT_STATE) - { - LOG_WARN("Invalid state message type recieved: %u", static_cast(type)); - return false; - } - while (!bp.empty()) { if (!bp.checkSize(sizeof(uint32_t))) @@ -60,8 +59,6 @@ class URStateParser : public URParser return false; } - //LOG_DEBUG("sub-packet size: %" PRIu32, sub_size); - // deconstruction of a sub parser will increment the position of the parent parser BinParser sbp(bp, sub_size); sbp.consume(sizeof(sub_size)); @@ -73,7 +70,6 @@ class URStateParser : public URParser if (packet == nullptr) { sbp.consume(); - //LOG_DEBUG("Skipping sub-packet of type %d", type); continue; } diff --git a/src/ros/action_server.cpp b/src/ros/action_server.cpp index fc3f7ad03..102df3145 100644 --- a/src/ros/action_server.cpp +++ b/src/ros/action_server.cpp @@ -13,10 +13,10 @@ ActionServer::ActionServer(TrajectoryFollower& follower, std::vectortrajectory.points) { std::array pos, vel; @@ -302,12 +301,11 @@ void ActionServer::trajectoryThread() vel[idx] = point.velocities[i]; } auto t = convert(point.time_from_start); - LOG_DEBUG("P[%d] = %ds, %dns -> %dus", j++, point.time_from_start.sec, point.time_from_start.nsec, t.count()); trajectory.push_back(TrajectoryPoint(pos, vel, t)); } double t = std::chrono::duration_cast>(trajectory[trajectory.size()-1].time_from_start).count(); - LOG_INFO("Executing trajectory with %d points and duration of %4.3fs", trajectory.size(), t); + LOG_INFO("Executing trajectory with %zu points and duration of %4.3fs", trajectory.size(), t); Result res; diff --git a/src/ros/controller.cpp b/src/ros/controller.cpp index 79102fa2f..f8896d751 100644 --- a/src/ros/controller.cpp +++ b/src/ros/controller.cpp @@ -1,10 +1,10 @@ #include "ur_modern_driver/ros/controller.h" -ROSController::ROSController(URCommander &commander, std::vector &joint_names, double max_vel_change) +ROSController::ROSController(URCommander &commander, TrajectoryFollower& follower, std::vector &joint_names, double max_vel_change) : controller_(this, nh_) , joint_interface_(joint_names) , wrench_interface_() - , position_interface_(commander, joint_interface_, joint_names) + , position_interface_(follower, joint_interface_, joint_names) , velocity_interface_(commander, joint_interface_, joint_names, max_vel_change) { registerInterface(&joint_interface_); diff --git a/src/ros/hardware_interface.cpp b/src/ros/hardware_interface.cpp index 09a0798f7..793769bd5 100644 --- a/src/ros/hardware_interface.cpp +++ b/src/ros/hardware_interface.cpp @@ -57,8 +57,8 @@ void VelocityInterface::reset() } -PositionInterface:: PositionInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names) - : commander_(commander) +PositionInterface:: PositionInterface(TrajectoryFollower& follower, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names) + : follower_(follower) { for (size_t i = 0; i < 6; i++) { @@ -68,15 +68,15 @@ PositionInterface:: PositionInterface(URCommander &commander, hardware_interfac bool PositionInterface::write() { - + return follower_.execute(position_cmd_); } void PositionInterface::start() { - + follower_.start(); } void PositionInterface::stop() { - + follower_.stop(); } \ No newline at end of file diff --git a/src/ros/trajectory_follower.cpp b/src/ros/trajectory_follower.cpp index 0093cba46..52b79f2d7 100644 --- a/src/ros/trajectory_follower.cpp +++ b/src/ros/trajectory_follower.cpp @@ -68,11 +68,10 @@ end TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string& reverse_ip, int reverse_port, bool version_3) : running_(false) , commander_(commander) - , reverse_port_(reverse_port) + , server_(reverse_port) , servoj_time_(0.008) , servoj_lookahead_time_(0.03) , servoj_gain_(300.) - , server_(reverse_port) { ros::param::get("~servoj_time", servoj_time_); ros::param::get("~servoj_lookahead_time", servoj_lookahead_time_); @@ -89,7 +88,7 @@ TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string& reve res.replace(res.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str()); res.replace(res.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), reverse_ip); - res.replace(res.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port_)); + res.replace(res.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port)); program_ = res; if(!server_.bind()) diff --git a/src/ros_main.cpp b/src/ros_main.cpp index dc5255c20..f069ac0ec 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -110,7 +110,7 @@ int main(int argc, char **argv) if (args.use_ros_control) { LOG_INFO("ROS control enabled"); - controller = new ROSController(*rt_commander, args.joint_names, args.max_vel_change); + controller = new ROSController(*rt_commander, traj_follower, args.joint_names, args.max_vel_change); rt_vec.push_back(controller); services.push_back(controller); } From 3a5fa23f6b590690df53f4c5450a9ae5e3297bb2 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Sun, 9 Jul 2017 02:54:49 +0200 Subject: [PATCH 060/114] Clang-format run --- include/ur_modern_driver/bin_parser.h | 4 +- include/ur_modern_driver/event_counter.h | 26 ++-- include/ur_modern_driver/pipeline.h | 14 +- include/ur_modern_driver/ros/controller.h | 18 ++- .../ur_modern_driver/ros/hardware_interface.h | 22 ++- include/ur_modern_driver/ros/io_service.h | 20 ++- include/ur_modern_driver/ros/mb_publisher.h | 17 ++- .../ur_modern_driver/ros/service_stopper.h | 32 +++-- .../ros/trajectory_follower.h | 16 +-- include/ur_modern_driver/tcp_socket.h | 25 ++-- include/ur_modern_driver/ur/commander.h | 15 +- include/ur_modern_driver/ur/factory.h | 6 +- include/ur_modern_driver/ur/master_board.h | 3 +- include/ur_modern_driver/ur/producer.h | 15 +- include/ur_modern_driver/ur/robot_mode.h | 3 +- include/ur_modern_driver/ur/server.h | 5 +- include/ur_modern_driver/ur/state.h | 8 +- include/ur_modern_driver/ur/state_parser.h | 4 +- include/ur_modern_driver/ur/stream.h | 13 +- src/ros/action_server.cpp | 128 +++++++++--------- src/ros/controller.cpp | 27 ++-- src/ros/hardware_interface.cpp | 12 +- src/ros/mb_publisher.cpp | 4 +- src/ros/service_stopper.cpp | 15 +- src/ros/trajectory_follower.cpp | 94 ++++++------- src/ros_main.cpp | 32 ++--- src/tcp_socket.cpp | 42 +++--- src/ur/commander.cpp | 21 ++- src/ur/master_board.cpp | 2 +- src/ur/server.cpp | 29 ++-- src/ur/stream.cpp | 10 +- 31 files changed, 341 insertions(+), 341 deletions(-) diff --git a/include/ur_modern_driver/bin_parser.h b/include/ur_modern_driver/bin_parser.h index 2cfce4595..5bac374df 100644 --- a/include/ur_modern_driver/bin_parser.h +++ b/include/ur_modern_driver/bin_parser.h @@ -3,11 +3,11 @@ #include #include #include +#include +#include #include #include #include -#include -#include #include "ur_modern_driver/log.h" #include "ur_modern_driver/types.h" diff --git a/include/ur_modern_driver/event_counter.h b/include/ur_modern_driver/event_counter.h index 2c8ff314b..566ffb267 100644 --- a/include/ur_modern_driver/event_counter.h +++ b/include/ur_modern_driver/event_counter.h @@ -1,11 +1,10 @@ #pragma once -#include #include +#include #include "ur_modern_driver/log.h" #include "ur_modern_driver/ur/consumer.h" - class EventCounter : public URRTPacketConsumer { private: @@ -13,32 +12,31 @@ class EventCounter : public URRTPacketConsumer Clock::time_point events_[250]; size_t idx_ = 0; - Clock::time_point last_; public: void trigger() { - //auto now = Clock::now(); - //LOG_INFO("Time diff: %d ms", std::chrono::duration_cast(now - last_)); - //last_ = now; - //return; + // auto now = Clock::now(); + // LOG_INFO("Time diff: %d ms", std::chrono::duration_cast(now - last_)); + // last_ = now; + // return; events_[idx_] = Clock::now(); idx_ += 1; - if(idx_ > 250) + if (idx_ > 250) { std::chrono::time_point t_min = - std::chrono::time_point::max(); + std::chrono::time_point::max(); std::chrono::time_point t_max = std::chrono::time_point::min(); - - for(auto const& e : events_) + + for (auto const& e : events_) { - if(e < t_min) + if (e < t_min) t_min = e; - if(e > t_max) + if (e > t_max) t_max = e; } @@ -46,7 +44,7 @@ class EventCounter : public URRTPacketConsumer auto secs = std::chrono::duration_cast(diff).count(); auto ms = std::chrono::duration_cast(diff).count(); std::chrono::duration test(t_max - t_min); - LOG_INFO("Recieved 250 messages at %f Hz", (250.0/test.count())); + LOG_INFO("Recieved 250 messages at %f Hz", (250.0 / test.count())); idx_ = 0; } } diff --git a/include/ur_modern_driver/pipeline.h b/include/ur_modern_driver/pipeline.h index 0e5cfdb52..e902ed3c2 100644 --- a/include/ur_modern_driver/pipeline.h +++ b/include/ur_modern_driver/pipeline.h @@ -43,28 +43,28 @@ class MultiConsumer : public IConsumer virtual void setupConsumer() { - for(auto &con : consumers_) + for (auto& con : consumers_) { con->setupConsumer(); } } virtual void teardownConsumer() { - for(auto &con : consumers_) + for (auto& con : consumers_) { con->teardownConsumer(); } } virtual void stopConsumer() { - for(auto &con : consumers_) + for (auto& con : consumers_) { con->stopConsumer(); } } virtual void onTimeout() { - for(auto &con : consumers_) + for (auto& con : consumers_) { con->onTimeout(); } @@ -73,9 +73,9 @@ class MultiConsumer : public IConsumer bool consume(shared_ptr product) { bool res = true; - for(auto &con : consumers_) + for (auto& con : consumers_) { - if(!con->consume(product)) + if (!con->consume(product)) res = false; } return res; @@ -153,7 +153,7 @@ class Pipeline Time now = Clock::now(); auto pkg_diff = now - last_pkg; auto warn_diff = now - last_warn; - if(pkg_diff > std::chrono::seconds(1) && warn_diff > std::chrono::seconds(1)) + if (pkg_diff > std::chrono::seconds(1) && warn_diff > std::chrono::seconds(1)) { last_warn = now; consumer_.onTimeout(); diff --git a/include/ur_modern_driver/ros/controller.h b/include/ur_modern_driver/ros/controller.h index b2780d537..ea4989b16 100644 --- a/include/ur_modern_driver/ros/controller.h +++ b/include/ur_modern_driver/ros/controller.h @@ -1,18 +1,18 @@ #pragma once -#include -#include #include #include #include #include #include #include +#include +#include #include "ur_modern_driver/log.h" +#include "ur_modern_driver/ros/hardware_interface.h" +#include "ur_modern_driver/ros/service_stopper.h" #include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/consumer.h" #include "ur_modern_driver/ur/rt_state.h" -#include "ur_modern_driver/ros/hardware_interface.h" -#include "ur_modern_driver/ros/service_stopper.h" class ROSController : private hardware_interface::RobotHW, public URRTPacketConsumer, public Service { @@ -55,10 +55,14 @@ class ROSController : private hardware_interface::RobotHW, public URRTPacketCons void reset(); public: - ROSController(URCommander& commander, TrajectoryFollower& follower, std::vector& joint_names, double max_vel_change); - virtual ~ROSController() { } + ROSController(URCommander& commander, TrajectoryFollower& follower, std::vector& joint_names, + double max_vel_change); + virtual ~ROSController() + { + } // from RobotHW - void doSwitch(const std::list& start_list, const std::list& stop_list); + void doSwitch(const std::list& start_list, + const std::list& stop_list); // from URRTPacketConsumer virtual void setupConsumer(); virtual bool consume(RTState_V1_6__7& state) diff --git a/include/ur_modern_driver/ros/hardware_interface.h b/include/ur_modern_driver/ros/hardware_interface.h index 93bd0c729..5dd95c1f2 100644 --- a/include/ur_modern_driver/ros/hardware_interface.h +++ b/include/ur_modern_driver/ros/hardware_interface.h @@ -4,17 +4,23 @@ #include #include #include +#include "ur_modern_driver/ros/trajectory_follower.h" #include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/rt_state.h" -#include "ur_modern_driver/ros/trajectory_follower.h" class HardwareInterface { public: virtual bool write() = 0; - virtual void start() {} - virtual void stop() {} - virtual void reset() {} + virtual void start() + { + } + virtual void stop() + { + } + virtual void reset() + { + } }; using hardware_interface::JointHandle; @@ -48,7 +54,8 @@ class VelocityInterface : public HardwareInterface, public hardware_interface::V double max_vel_change_; public: - VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names, double max_vel_change); + VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, + std::vector &joint_names, double max_vel_change); virtual bool write(); virtual void reset(); typedef hardware_interface::VelocityJointInterface parent_type; @@ -57,11 +64,12 @@ class VelocityInterface : public HardwareInterface, public hardware_interface::V class PositionInterface : public HardwareInterface, public hardware_interface::PositionJointInterface { private: - TrajectoryFollower& follower_; + TrajectoryFollower &follower_; std::array position_cmd_; public: - PositionInterface(TrajectoryFollower& follower, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names); + PositionInterface(TrajectoryFollower &follower, hardware_interface::JointStateInterface &js_interface, + std::vector &joint_names); virtual bool write(); virtual void start(); virtual void stop(); diff --git a/include/ur_modern_driver/ros/io_service.h b/include/ur_modern_driver/ros/io_service.h index 03d993581..4f02bb22f 100644 --- a/include/ur_modern_driver/ros/io_service.h +++ b/include/ur_modern_driver/ros/io_service.h @@ -10,11 +10,11 @@ #include "ur_modern_driver/log.h" #include "ur_modern_driver/ur/commander.h" -class IOService +class IOService { private: ros::NodeHandle nh_; - URCommander &commander_; + URCommander& commander_; ros::ServiceServer io_service_; ros::ServiceServer payload_service_; @@ -23,20 +23,20 @@ class IOService LOG_INFO("setIO called with [%d, %d]", req.fun, req.pin); bool res = false; bool flag = req.state > 0.0 ? true : false; - switch(req.fun) + switch (req.fun) { case ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT: res = commander_.setDigitalOut(req.pin, flag); - break; + break; case ur_msgs::SetIO::Request::FUN_SET_ANALOG_OUT: res = commander_.setAnalogOut(req.pin, req.state); - break; + break; case ur_msgs::SetIO::Request::FUN_SET_TOOL_VOLTAGE: res = commander_.setToolVoltage(static_cast(req.state)); - break; + break; case ur_msgs::SetIO::Request::FUN_SET_FLAG: res = commander_.setFlag(req.pin, flag); - break; + break; default: LOG_WARN("Invalid setIO function called (%d)", req.fun); } @@ -47,17 +47,15 @@ class IOService bool setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::SetPayloadResponse& resp) { LOG_INFO("setPayload called"); - //TODO check min and max payload? + // TODO check min and max payload? return (resp.success = commander_.setPayload(req.payload)); } - public: - IOService(URCommander &commander) + IOService(URCommander& commander) : commander_(commander) , io_service_(nh_.advertiseService("ur_driver/set_io", &IOService::setIO, this)) , payload_service_(nh_.advertiseService("ur_driver/set_payload", &IOService::setPayload, this)) { - } }; \ No newline at end of file diff --git a/include/ur_modern_driver/ros/mb_publisher.h b/include/ur_modern_driver/ros/mb_publisher.h index 11c4d8b53..692ecfd6c 100644 --- a/include/ur_modern_driver/ros/mb_publisher.h +++ b/include/ur_modern_driver/ros/mb_publisher.h @@ -16,22 +16,21 @@ class MBPublisher : public URStatePacketConsumer Publisher io_pub_; template - inline void appendDigital(std::vector &vec, std::bitset bits) + inline void appendDigital(std::vector& vec, std::bitset bits) { - for(size_t i = 0; i < N; i++) + for (size_t i = 0; i < N; i++) { - ur_msgs::Digital digi; - digi.pin = static_cast(i); - digi.state = bits.test(i); - vec.push_back(digi); + ur_msgs::Digital digi; + digi.pin = static_cast(i); + digi.state = bits.test(i); + vec.push_back(digi); } } - void publish(ur_msgs::IOStates &io_msg, SharedMasterBoardData& data); + void publish(ur_msgs::IOStates& io_msg, SharedMasterBoardData& data); public: - MBPublisher() - : io_pub_(nh_.advertise("ur_driver/io_states", 1)) + MBPublisher() : io_pub_(nh_.advertise("ur_driver/io_states", 1)) { } diff --git a/include/ur_modern_driver/ros/service_stopper.h b/include/ur_modern_driver/ros/service_stopper.h index af09dfa6e..ffbe10646 100644 --- a/include/ur_modern_driver/ros/service_stopper.h +++ b/include/ur_modern_driver/ros/service_stopper.h @@ -4,7 +4,7 @@ #include "ur_modern_driver/log.h" #include "ur_modern_driver/ur/consumer.h" -enum class RobotState +enum class RobotState { Running, Error, @@ -18,15 +18,16 @@ class Service virtual void onRobotStateChange(RobotState state) = 0; }; -class ServiceStopper : public URStatePacketConsumer { +class ServiceStopper : public URStatePacketConsumer +{ private: ros::NodeHandle nh_; ros::ServiceServer enable_service_; std::vector services_; RobotState last_state_; - + void notify_all(RobotState state); - bool handle(SharedRobotModeData &data, bool error); + bool handle(SharedRobotModeData& data, bool error); bool enableCallback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp); public: @@ -34,19 +35,28 @@ class ServiceStopper : public URStatePacketConsumer { virtual bool consume(RobotModeData_V1_X& data) { - return handle(data, data.robot_mode != robot_mode_V1_X::ROBOT_RUNNING_MODE); + return handle(data, data.robot_mode != robot_mode_V1_X::ROBOT_RUNNING_MODE); } virtual bool consume(RobotModeData_V3_0__1& data) { - return handle(data, data.robot_mode != robot_mode_V3_X::RUNNING); + return handle(data, data.robot_mode != robot_mode_V3_X::RUNNING); } virtual bool consume(RobotModeData_V3_2& data) { - return handle(data, data.robot_mode != robot_mode_V3_X::RUNNING); + return handle(data, data.robot_mode != robot_mode_V3_X::RUNNING); } - //unused - virtual bool consume(MasterBoardData_V1_X& data) { return true; } - virtual bool consume(MasterBoardData_V3_0__1& data) { return true; } - virtual bool consume(MasterBoardData_V3_2& data) { return true; } + // unused + virtual bool consume(MasterBoardData_V1_X& data) + { + return true; + } + virtual bool consume(MasterBoardData_V3_0__1& data) + { + return true; + } + virtual bool consume(MasterBoardData_V3_2& data) + { + return true; + } }; \ No newline at end of file diff --git a/include/ur_modern_driver/ros/trajectory_follower.h b/include/ur_modern_driver/ros/trajectory_follower.h index da01004dc..d6c478999 100644 --- a/include/ur_modern_driver/ros/trajectory_follower.h +++ b/include/ur_modern_driver/ros/trajectory_follower.h @@ -1,13 +1,13 @@ #pragma once +#include #include -#include #include #include #include #include #include -#include +#include #include "ur_modern_driver/log.h" #include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/server.h" @@ -23,14 +23,12 @@ struct TrajectoryPoint } TrajectoryPoint(std::array &pos, std::array &vel, std::chrono::microseconds tfs) - : positions(pos) - , velocities(vel) - , time_from_start(tfs) + : positions(pos), velocities(vel), time_from_start(tfs) { } }; -class TrajectoryFollower +class TrajectoryFollower { private: std::atomic running_; @@ -49,11 +47,11 @@ class TrajectoryFollower return s; } - bool execute(std::array &positions, bool keep_alive); - double interpolate(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel); + bool execute(std::array &positions, bool keep_alive); + double interpolate(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel); public: - TrajectoryFollower(URCommander &commander, std::string& reverse_ip, int reverse_port, bool version_3); + TrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, bool version_3); bool start(); bool execute(std::array &positions); diff --git a/include/ur_modern_driver/tcp_socket.h b/include/ur_modern_driver/tcp_socket.h index 4230ac095..99427b928 100644 --- a/include/ur_modern_driver/tcp_socket.h +++ b/include/ur_modern_driver/tcp_socket.h @@ -1,9 +1,9 @@ -#pragma once +#pragma once #include #include #include -#include #include +#include #include enum class SocketState @@ -26,23 +26,28 @@ class TCPSocket return false; } virtual void setOptions(int socket_fd); - - + bool setup(std::string &host, int port); public: TCPSocket(); virtual ~TCPSocket(); - SocketState getState() { return state_; } - - int getSocketFD() { return socket_fd_; } + SocketState getState() + { + return state_; + } + + int getSocketFD() + { + return socket_fd_; + } bool setSocketFD(int socket_fd); std::string getIP(); - bool read(uint8_t* buf, size_t buf_len, size_t &read); - bool write(const uint8_t* buf, size_t buf_len, size_t &written); + bool read(uint8_t *buf, size_t buf_len, size_t &read); + bool write(const uint8_t *buf, size_t buf_len, size_t &written); - void close(); + void close(); }; diff --git a/include/ur_modern_driver/ur/commander.h b/include/ur_modern_driver/ur/commander.h index 5d81698c2..8fdffac1d 100644 --- a/include/ur_modern_driver/ur/commander.h +++ b/include/ur_modern_driver/ur/commander.h @@ -1,20 +1,20 @@ #pragma once #include -#include #include +#include #include "ur_modern_driver/ur/stream.h" class URCommander { private: - URStream& stream_; + URStream &stream_; protected: - bool write(std::string& s); + bool write(std::string &s); void formatArray(std::ostringstream &out, std::array &values); public: - URCommander(URStream& stream) : stream_(stream) + URCommander(URStream &stream) : stream_(stream) { } @@ -22,7 +22,7 @@ class URCommander virtual bool setDigitalOut(uint8_t pin, bool value) = 0; virtual bool setAnalogOut(uint8_t pin, double value) = 0; - //shared + // shared bool uploadProg(std::string &s); bool stopj(double a = 10.0); bool setToolVoltage(uint8_t voltage); @@ -33,7 +33,7 @@ class URCommander class URCommander_V1_X : public URCommander { public: - URCommander_V1_X(URStream& stream) : URCommander(stream) + URCommander_V1_X(URStream &stream) : URCommander(stream) { } @@ -42,11 +42,10 @@ class URCommander_V1_X : public URCommander virtual bool setAnalogOut(uint8_t pin, double value); }; - class URCommander_V3_X : public URCommander { public: - URCommander_V3_X(URStream& stream) : URCommander(stream) + URCommander_V3_X(URStream &stream) : URCommander(stream) { } diff --git a/include/ur_modern_driver/ur/factory.h b/include/ur_modern_driver/ur/factory.h index 0b4e6ca78..ecf3bed6b 100644 --- a/include/ur_modern_driver/ur/factory.h +++ b/include/ur_modern_driver/ur/factory.h @@ -76,12 +76,12 @@ class URFactory : private URMessagePacketConsumer return major_version_ == 3; } - std::unique_ptr getCommander(URStream &stream) + std::unique_ptr getCommander(URStream& stream) { - if(major_version_ == 1) + if (major_version_ == 1) return std::unique_ptr(new URCommander_V1_X(stream)); else - return std::unique_ptr(new URCommander_V3_X(stream)); + return std::unique_ptr(new URCommander_V3_X(stream)); } std::unique_ptr> getStateParser() diff --git a/include/ur_modern_driver/ur/master_board.h b/include/ur_modern_driver/ur/master_board.h index c625cbf8d..e3163dc2f 100644 --- a/include/ur_modern_driver/ur/master_board.h +++ b/include/ur_modern_driver/ur/master_board.h @@ -1,8 +1,8 @@ #pragma once #include -#include #include +#include #include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/types.h" #include "ur_modern_driver/ur/state.h" @@ -42,7 +42,6 @@ class MasterBoardData_V1_X : public SharedMasterBoardData, public StatePacket virtual bool parseWith(BinParser& bp); virtual bool consumeWith(URStatePacketConsumer& consumer); - std::bitset<10> digital_input_bits; std::bitset<10> digital_output_bits; diff --git a/include/ur_modern_driver/ur/producer.h b/include/ur_modern_driver/ur/producer.h index df4a61a8e..6f08ab6ba 100644 --- a/include/ur_modern_driver/ur/producer.h +++ b/include/ur_modern_driver/ur/producer.h @@ -35,31 +35,30 @@ class URProducer : public IProducer // 4KB should be enough to hold any packet received from UR uint8_t buf[4096]; size_t read = 0; - //expoential backoff reconnects - while(true) + // expoential backoff reconnects + while (true) { - if(stream_.read(buf, sizeof(buf), read)) + if (stream_.read(buf, sizeof(buf), read)) { - //reset sleep amount + // reset sleep amount timeout_ = std::chrono::seconds(1); break; } - if(stream_.closed()) + if (stream_.closed()) return false; LOG_WARN("Failed to read from stream, reconnecting in %ld seconds...", timeout_.count()); std::this_thread::sleep_for(timeout_); - if(stream_.connect()) + if (stream_.connect()) continue; auto next = timeout_ * 2; - if(next <= std::chrono::seconds(120)) + if (next <= std::chrono::seconds(120)) timeout_ = next; } - BinParser bp(buf, read); return parser_.parse(bp, products); } diff --git a/include/ur_modern_driver/ur/robot_mode.h b/include/ur_modern_driver/ur/robot_mode.h index 6c7c6416a..80aa145c8 100644 --- a/include/ur_modern_driver/ur/robot_mode.h +++ b/include/ur_modern_driver/ur/robot_mode.h @@ -16,7 +16,7 @@ class SharedRobotModeData bool real_robot_enabled; bool robot_power_on; bool emergency_stopped; - bool protective_stopped; //AKA security_stopped + bool protective_stopped; // AKA security_stopped bool program_running; bool program_paused; @@ -79,7 +79,6 @@ class RobotModeData_V3_0__1 : public SharedRobotModeData, public StatePacket virtual bool parseWith(BinParser& bp); virtual bool consumeWith(URStatePacketConsumer& consumer); - robot_mode_V3_X robot_mode; robot_control_mode_V3_X control_mode; diff --git a/include/ur_modern_driver/ur/server.h b/include/ur_modern_driver/ur/server.h index 027e278eb..f236b965b 100644 --- a/include/ur_modern_driver/ur/server.h +++ b/include/ur_modern_driver/ur/server.h @@ -2,9 +2,9 @@ #include #include #include +#include #include #include -#include #include #include "ur_modern_driver/tcp_socket.h" @@ -21,7 +21,6 @@ class URServer : private TCPSocket } virtual void setOptions(int socket_fd); - public: URServer(int port); ~URServer(); @@ -29,5 +28,5 @@ class URServer : private TCPSocket bool bind(); bool accept(); void disconnectClient(); - bool write(const uint8_t* buf, size_t buf_len, size_t &written); + bool write(const uint8_t *buf, size_t buf_len, size_t &written); }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/state.h b/include/ur_modern_driver/ur/state.h index 4aeddbaed..d48a87608 100644 --- a/include/ur_modern_driver/ur/state.h +++ b/include/ur_modern_driver/ur/state.h @@ -32,8 +32,12 @@ class URStatePacketConsumer; class StatePacket { public: - StatePacket() {} - virtual ~StatePacket() {} + StatePacket() + { + } + virtual ~StatePacket() + { + } virtual bool parseWith(BinParser& bp) = 0; virtual bool consumeWith(URStatePacketConsumer& consumer) = 0; }; diff --git a/include/ur_modern_driver/ur/state_parser.h b/include/ur_modern_driver/ur/state_parser.h index fc8024250..48eda1d9f 100644 --- a/include/ur_modern_driver/ur/state_parser.h +++ b/include/ur_modern_driver/ur/state_parser.h @@ -35,8 +35,8 @@ class URStateParser : public URParser if (type != message_type::ROBOT_STATE) { - //quietly ignore the intial version message - if(type != message_type::ROBOT_MESSAGE) + // quietly ignore the intial version message + if (type != message_type::ROBOT_MESSAGE) { LOG_WARN("Invalid state message type recieved: %u", static_cast(type)); } diff --git a/include/ur_modern_driver/ur/stream.h b/include/ur_modern_driver/ur/stream.h index 3d5bc8125..913f665e2 100644 --- a/include/ur_modern_driver/ur/stream.h +++ b/include/ur_modern_driver/ur/stream.h @@ -2,8 +2,8 @@ #include #include #include -#include #include +#include #include #include "ur_modern_driver/log.h" #include "ur_modern_driver/tcp_socket.h" @@ -16,7 +16,7 @@ class URStream : public TCPSocket std::mutex write_mutex_, read_mutex_; protected: - virtual bool open(int socket_fd, struct sockaddr *address, size_t address_len) + virtual bool open(int socket_fd, struct sockaddr* address, size_t address_len) { return ::connect(socket_fd, address, address_len) == 0; } @@ -36,8 +36,11 @@ class URStream : public TCPSocket TCPSocket::close(); } - bool closed() { return getState() == SocketState::Closed; } + bool closed() + { + return getState() == SocketState::Closed; + } - bool read(uint8_t* buf, size_t buf_len, size_t &read); - bool write(const uint8_t* buf, size_t buf_len, size_t &written); + bool read(uint8_t* buf, size_t buf_len, size_t& read); + bool write(const uint8_t* buf, size_t buf_len, size_t& written); }; \ No newline at end of file diff --git a/src/ros/action_server.cpp b/src/ros/action_server.cpp index 102df3145..380acd3a3 100644 --- a/src/ros/action_server.cpp +++ b/src/ros/action_server.cpp @@ -1,14 +1,9 @@ -#include #include "ur_modern_driver/ros/action_server.h" +#include ActionServer::ActionServer(TrajectoryFollower& follower, std::vector& joint_names, double max_velocity) - : as_( - nh_, - "follow_joint_trajectory", - boost::bind(&ActionServer::onGoal, this, _1), - boost::bind(&ActionServer::onCancel, this, _1), - false - ) + : as_(nh_, "follow_joint_trajectory", boost::bind(&ActionServer::onGoal, this, _1), + boost::bind(&ActionServer::onCancel, this, _1), false) , joint_names_(joint_names) , joint_set_(joint_names.begin(), joint_names.end()) , max_velocity_(max_velocity) @@ -22,7 +17,7 @@ ActionServer::ActionServer(TrajectoryFollower& follower, std::vector lock(tj_mutex_); Result res; @@ -60,7 +55,6 @@ void ActionServer::onRobotStateChange(RobotState state) curr_gh_.setAborted(res, res.error_string); } - bool ActionServer::updateState(RTShared& data) { q_actual_ = data.q_actual; @@ -68,7 +62,6 @@ bool ActionServer::updateState(RTShared& data) return true; } - bool ActionServer::consume(RTState_V1_6__7& state) { return updateState(state); @@ -86,7 +79,6 @@ bool ActionServer::consume(RTState_V3_2__3& state) return updateState(state); } - void ActionServer::onGoal(GoalHandle gh) { Result res; @@ -94,7 +86,7 @@ void ActionServer::onGoal(GoalHandle gh) LOG_INFO("Received new goal"); - if(!validate(gh, res) || !try_execute(gh, res)) + if (!validate(gh, res) || !try_execute(gh, res)) { LOG_WARN("Goal error: %s", res.error_string.c_str()); gh.setRejected(res, res.error_string); @@ -104,7 +96,7 @@ void ActionServer::onGoal(GoalHandle gh) void ActionServer::onCancel(GoalHandle gh) { interrupt_traj_ = true; - //wait for goal to be interrupted + // wait for goal to be interrupted std::lock_guard lock(tj_mutex_); Result res; @@ -114,22 +106,22 @@ void ActionServer::onCancel(GoalHandle gh) } bool ActionServer::validate(GoalHandle& gh, Result& res) -{ +{ return validateState(gh, res) && validateJoints(gh, res) && validateTrajectory(gh, res); } bool ActionServer::validateState(GoalHandle& gh, Result& res) { - switch(state_) + switch (state_) { case RobotState::EmergencyStopped: res.error_string = "Robot is emergency stopped"; return false; - + case RobotState::ProtectiveStopped: res.error_string = "Robot is protective stopped"; return false; - + case RobotState::Error: res.error_string = "Robot is not ready, check robot_mode"; return false; @@ -149,7 +141,7 @@ bool ActionServer::validateJoints(GoalHandle& gh, Result& res) auto const& joints = goal->trajectory.joint_names; std::set goal_joints(joints.begin(), joints.end()); - if(goal_joints == joint_set_) + if (goal_joints == joint_set_) return true; res.error_code = Result::INVALID_JOINTS; @@ -162,42 +154,42 @@ bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res) auto goal = gh.getGoal(); res.error_code = Result::INVALID_GOAL; - //must at least have one point - if(goal->trajectory.points.size() < 1) + // must at least have one point + if (goal->trajectory.points.size() < 1) return false; - for(auto const& point : goal->trajectory.points) + for (auto const& point : goal->trajectory.points) { - if(point.velocities.size() != joint_names_.size()) + if (point.velocities.size() != joint_names_.size()) { res.error_code = Result::INVALID_GOAL; res.error_string = "Received a goal with an invalid number of velocities"; return false; } - - if(point.positions.size() != joint_names_.size()) + + if (point.positions.size() != joint_names_.size()) { res.error_code = Result::INVALID_GOAL; res.error_string = "Received a goal with an invalid number of positions"; return false; } - - for(auto const& velocity : point.velocities) + + for (auto const& velocity : point.velocities) { - if(!std::isfinite(velocity)) + if (!std::isfinite(velocity)) { res.error_string = "Received a goal with infinities or NaNs in velocity"; return false; } - if(std::fabs(velocity) > max_velocity_) + if (std::fabs(velocity) > max_velocity_) { res.error_string = "Received a goal with velocities that are higher than " + std::to_string(max_velocity_); return false; } } - for(auto const& position : point.positions) + for (auto const& position : point.positions) { - if(!std::isfinite(position)) + if (!std::isfinite(position)) { res.error_string = "Received a goal with infinities or NaNs in positions"; return false; @@ -205,34 +197,34 @@ bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res) } } - //todo validate start position? + // todo validate start position? return true; } -inline std::chrono::microseconds convert(const ros::Duration &dur) +inline std::chrono::microseconds convert(const ros::Duration& dur) { return std::chrono::duration_cast(std::chrono::seconds(dur.sec) + - std::chrono::nanoseconds(dur.nsec)); + std::chrono::nanoseconds(dur.nsec)); } bool ActionServer::try_execute(GoalHandle& gh, Result& res) { - if(!running_) + if (!running_) { res.error_string = "Internal error"; return false; } - if(!tj_mutex_.try_lock()) + if (!tj_mutex_.try_lock()) { interrupt_traj_ = true; res.error_string = "Received another trajectory"; curr_gh_.setAborted(res, res.error_string); tj_mutex_.lock(); - //todo: make configurable + // todo: make configurable std::this_thread::sleep_for(std::chrono::milliseconds(250)); } - //locked here + // locked here curr_gh_ = gh; interrupt_traj_ = false; has_goal_ = true; @@ -244,12 +236,12 @@ bool ActionServer::try_execute(GoalHandle& gh, Result& res) std::vector ActionServer::reorderMap(std::vector goal_joints) { std::vector indecies; - for(auto const& aj : joint_names_) + for (auto const& aj : joint_names_) { size_t j = 0; - for(auto const& gj : goal_joints) + for (auto const& gj : goal_joints) { - if(aj == gj) + if (aj == gj) break; j++; } @@ -261,40 +253,40 @@ std::vector ActionServer::reorderMap(std::vector goal_joint void ActionServer::trajectoryThread() { LOG_INFO("Trajectory thread started"); - - while(running_) + + while (running_) { std::unique_lock lk(tj_mutex_); - if(!tj_cv_.wait_for(lk, std::chrono::milliseconds(100), [&]{return running_ && has_goal_;})) + if (!tj_cv_.wait_for(lk, std::chrono::milliseconds(100), [&] { return running_ && has_goal_; })) continue; - + LOG_INFO("Trajectory received and accepted"); curr_gh_.setAccepted(); auto goal = curr_gh_.getGoal(); std::vector trajectory; - trajectory.reserve(goal->trajectory.points.size()+1); + trajectory.reserve(goal->trajectory.points.size() + 1); - //joint names of the goal might have a different ordering compared - //to what URScript expects so need to map between the two + // joint names of the goal might have a different ordering compared + // to what URScript expects so need to map between the two auto mapping = reorderMap(goal->trajectory.joint_names); - + LOG_INFO("Translating trajectory"); auto const& fp = goal->trajectory.points[0]; auto fpt = convert(fp.time_from_start); - //make sure we have a proper t0 position - if(fpt > std::chrono::microseconds(0)) + // make sure we have a proper t0 position + if (fpt > std::chrono::microseconds(0)) { LOG_INFO("Trajectory without t0 recieved, inserting t0 at currrent position"); trajectory.push_back(TrajectoryPoint(q_actual_, qd_actual_, std::chrono::microseconds(0))); } - for(auto const& point : goal->trajectory.points) + for (auto const& point : goal->trajectory.points) { std::array pos, vel; - for(size_t i = 0; i < 6; i++) + for (size_t i = 0; i < 6; i++) { size_t idx = mapping[i]; pos[idx] = point.positions[i]; @@ -304,17 +296,19 @@ void ActionServer::trajectoryThread() trajectory.push_back(TrajectoryPoint(pos, vel, t)); } - double t = std::chrono::duration_cast>(trajectory[trajectory.size()-1].time_from_start).count(); + double t = + std::chrono::duration_cast>(trajectory[trajectory.size() - 1].time_from_start) + .count(); LOG_INFO("Executing trajectory with %zu points and duration of %4.3fs", trajectory.size(), t); Result res; - if(follower_.start()) + if (follower_.start()) { - if(follower_.execute(trajectory, interrupt_traj_)) + if (follower_.execute(trajectory, interrupt_traj_)) { - //interrupted goals must be handled by interrupt trigger - if(!interrupt_traj_) + // interrupted goals must be handled by interrupt trigger + if (!interrupt_traj_) { LOG_INFO("Trajectory executed successfully"); res.error_code = Result::SUCCESSFUL; @@ -327,7 +321,7 @@ void ActionServer::trajectoryThread() { LOG_INFO("Trajectory failed"); res.error_code = -100; - res.error_string = "Connection to robot was lost"; + res.error_string = "Connection to robot was lost"; curr_gh_.setAborted(res, res.error_string); } @@ -337,7 +331,7 @@ void ActionServer::trajectoryThread() { LOG_ERROR("Failed to start trajectory follower!"); res.error_code = -100; - res.error_string = "Robot connection could not be established"; + res.error_string = "Robot connection could not be established"; curr_gh_.setAborted(res, res.error_string); } diff --git a/src/ros/controller.cpp b/src/ros/controller.cpp index f8896d751..75aa9c3d4 100644 --- a/src/ros/controller.cpp +++ b/src/ros/controller.cpp @@ -1,6 +1,7 @@ #include "ur_modern_driver/ros/controller.h" -ROSController::ROSController(URCommander &commander, TrajectoryFollower& follower, std::vector &joint_names, double max_vel_change) +ROSController::ROSController(URCommander& commander, TrajectoryFollower& follower, + std::vector& joint_names, double max_vel_change) : controller_(this, nh_) , joint_interface_(joint_names) , wrench_interface_() @@ -18,7 +19,8 @@ void ROSController::setupConsumer() lastUpdate_ = ros::Time::now(); } -void ROSController::doSwitch(const std::list& start_list, const std::list& stop_list) +void ROSController::doSwitch(const std::list& start_list, + const std::list& stop_list) { LOG_INFO("Switching hardware interface"); @@ -60,7 +62,7 @@ void ROSController::reset() { if (active_interface_ == nullptr) return; - + active_interface_->reset(); } @@ -70,7 +72,6 @@ void ROSController::read(RTShared& packet) wrench_interface_.update(packet); } - bool ROSController::update(RTShared& state) { auto time = ros::Time::now(); @@ -80,16 +81,16 @@ bool ROSController::update(RTShared& state) read(state); controller_.update(time, diff, !service_enabled_); - //emergency stop and such should not kill the pipeline - //but still prevent writes - if(!service_enabled_) + // emergency stop and such should not kill the pipeline + // but still prevent writes + if (!service_enabled_) { reset(); return true; } - - //allow the controller to update x times before allowing writes again - if(service_cooldown_ > 0) + + // allow the controller to update x times before allowing writes again + if (service_cooldown_ > 0) { service_cooldown_ -= 1; return true; @@ -101,9 +102,9 @@ bool ROSController::update(RTShared& state) void ROSController::onRobotStateChange(RobotState state) { bool next = (state == RobotState::Running); - if(next == service_enabled_) + if (next == service_enabled_) return; - + service_enabled_ = next; - service_cooldown_ = 125; + service_cooldown_ = 125; } \ No newline at end of file diff --git a/src/ros/hardware_interface.cpp b/src/ros/hardware_interface.cpp index 793769bd5..fde4e85be 100644 --- a/src/ros/hardware_interface.cpp +++ b/src/ros/hardware_interface.cpp @@ -19,14 +19,15 @@ void JointInterface::update(RTShared &packet) WrenchInterface::WrenchInterface() { registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", "", tcp_.begin(), tcp_.begin() + 3)); -} +} void WrenchInterface::update(RTShared &packet) { tcp_ = packet.tcp_force; } -VelocityInterface::VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names, double max_vel_change) +VelocityInterface::VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, + std::vector &joint_names, double max_vel_change) : commander_(commander), max_vel_change_(max_vel_change) { for (size_t i = 0; i < 6; i++) @@ -50,14 +51,15 @@ bool VelocityInterface::write() void VelocityInterface::reset() { - for(auto &val : prev_velocity_cmd_) + for (auto &val : prev_velocity_cmd_) { val = 0; } } - -PositionInterface:: PositionInterface(TrajectoryFollower& follower, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names) +PositionInterface::PositionInterface(TrajectoryFollower &follower, + hardware_interface::JointStateInterface &js_interface, + std::vector &joint_names) : follower_(follower) { for (size_t i = 0; i < 6; i++) diff --git a/src/ros/mb_publisher.cpp b/src/ros/mb_publisher.cpp index 1177be568..c0fdc232d 100644 --- a/src/ros/mb_publisher.cpp +++ b/src/ros/mb_publisher.cpp @@ -1,6 +1,6 @@ #include "ur_modern_driver/ros/mb_publisher.h" -inline void appendAnalog(std::vector &vec, double val, uint8_t pin) +inline void appendAnalog(std::vector& vec, double val, uint8_t pin) { ur_msgs::Analog ana; ana.pin = pin; @@ -8,7 +8,7 @@ inline void appendAnalog(std::vector &vec, double val, uint8_t vec.push_back(ana); } -void MBPublisher::publish(ur_msgs::IOStates &io_msg, SharedMasterBoardData& data) +void MBPublisher::publish(ur_msgs::IOStates& io_msg, SharedMasterBoardData& data) { appendAnalog(io_msg.analog_in_states, data.analog_input0, 0); appendAnalog(io_msg.analog_in_states, data.analog_input1, 1); diff --git a/src/ros/service_stopper.cpp b/src/ros/service_stopper.cpp index 53cb43cf3..2f46fd5ba 100644 --- a/src/ros/service_stopper.cpp +++ b/src/ros/service_stopper.cpp @@ -5,10 +5,9 @@ ServiceStopper::ServiceStopper(std::vector services) , services_(services) , last_state_(RobotState::Error) { - //enable_all(); + // enable_all(); } - bool ServiceStopper::enableCallback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp) { notify_all(RobotState::Running); @@ -17,10 +16,10 @@ bool ServiceStopper::enableCallback(std_srvs::EmptyRequest& req, std_srvs::Empty void ServiceStopper::notify_all(RobotState state) { - if(last_state_ == state) + if (last_state_ == state) return; - - for(auto const service : services_) + + for (auto const service : services_) { service->onRobotStateChange(state); } @@ -30,15 +29,15 @@ void ServiceStopper::notify_all(RobotState state) bool ServiceStopper::handle(SharedRobotModeData& data, bool error) { - if(data.emergency_stopped) + if (data.emergency_stopped) { notify_all(RobotState::EmergencyStopped); } - else if(data.protective_stopped) + else if (data.protective_stopped) { notify_all(RobotState::ProtectiveStopped); } - else if(error) + else if (error) { notify_all(RobotState::Error); } diff --git a/src/ros/trajectory_follower.cpp b/src/ros/trajectory_follower.cpp index 52b79f2d7..a1f45d943 100644 --- a/src/ros/trajectory_follower.cpp +++ b/src/ros/trajectory_follower.cpp @@ -1,8 +1,7 @@ -#include -#include #include "ur_modern_driver/ros/trajectory_follower.h" - - +#include +#include + static const int32_t MULT_JOINTSTATE_ = 1000000; static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}"); static const std::string SERVO_J_REPLACE("{{SERVO_J_REPLACE}}"); @@ -65,7 +64,8 @@ def driverProg(): end )"; -TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string& reverse_ip, int reverse_port, bool version_3) +TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, + bool version_3) : running_(false) , commander_(commander) , server_(reverse_port) @@ -77,13 +77,12 @@ TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string& reve ros::param::get("~servoj_lookahead_time", servoj_lookahead_time_); ros::param::get("~servoj_gain", servoj_gain_); - std::string res(POSITION_PROGRAM); res.replace(res.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_)); std::ostringstream out; out << "t=" << std::fixed << std::setprecision(4) << servoj_time_; - if(version_3) + if (version_3) out << ", lookahead_time=" << servoj_lookahead_time_ << ", gain=" << servoj_gain_; res.replace(res.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str()); @@ -91,21 +90,21 @@ TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string& reve res.replace(res.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port)); program_ = res; - if(!server_.bind()) + if (!server_.bind()) { LOG_ERROR("Failed to bind server, the port %d is likely already in use", reverse_port); std::exit(-1); - } + } } bool TrajectoryFollower::start() { - if(running_) - return true; //not sure + if (running_) + return true; // not sure LOG_INFO("Uploading trajectory program to robot"); - - if(!commander_.uploadProg(program_)) + + if (!commander_.uploadProg(program_)) { LOG_ERROR("Program upload failed!"); return false; @@ -113,29 +112,30 @@ bool TrajectoryFollower::start() LOG_DEBUG("Awaiting incomming robot connection"); - if(!server_.accept()) + if (!server_.accept()) { LOG_ERROR("Failed to accept incomming robot connection"); return false; } - + LOG_DEBUG("Robot successfully connected"); return (running_ = true); } bool TrajectoryFollower::execute(std::array &positions, bool keep_alive) { - if(!running_) + if (!running_) return false; - // LOG_INFO("servoj([%f,%f,%f,%f,%f,%f])", positions[0], positions[1], positions[2], positions[3], positions[4], positions[5]); + // LOG_INFO("servoj([%f,%f,%f,%f,%f,%f])", positions[0], positions[1], positions[2], positions[3], positions[4], + // positions[5]); last_positions_ = positions; - uint8_t buf[sizeof(uint32_t)*7]; + uint8_t buf[sizeof(uint32_t) * 7]; uint8_t *idx = buf; - - for(auto const& pos : positions) + + for (auto const &pos : positions) { int32_t val = static_cast(pos * MULT_JOINTSTATE_); val = htobe32(val); @@ -145,7 +145,7 @@ bool TrajectoryFollower::execute(std::array &positions, bool keep_ali int32_t val = htobe32(static_cast(keep_alive)); append(idx, val); - size_t written; + size_t written; return server_.write(buf, sizeof(buf), written); } @@ -166,60 +166,54 @@ bool TrajectoryFollower::execute(std::array &positions) bool TrajectoryFollower::execute(std::vector &trajectory, std::atomic &interrupt) { - if(!running_) + if (!running_) return false; - + using namespace std::chrono; typedef duration double_seconds; typedef high_resolution_clock Clock; typedef Clock::time_point Time; - auto& last = trajectory[trajectory.size()-1]; - auto& prev = trajectory[0]; + auto &last = trajectory[trajectory.size() - 1]; + auto &prev = trajectory[0]; Time t0 = Clock::now(); Time latest = t0; std::array positions; - for(auto const& point : trajectory) + for (auto const &point : trajectory) { - //skip t0 - if(&point == &prev) + // skip t0 + if (&point == &prev) continue; - if(interrupt) + if (interrupt) break; auto duration = point.time_from_start - prev.time_from_start; double d_s = duration_cast(duration).count(); - //interpolation loop - while(!interrupt) + // interpolation loop + while (!interrupt) { latest = Clock::now(); auto elapsed = latest - t0; - if(point.time_from_start <= elapsed) + if (point.time_from_start <= elapsed) break; - if(last.time_from_start <= elapsed) + if (last.time_from_start <= elapsed) return true; double elapsed_s = duration_cast(elapsed - prev.time_from_start).count(); - for(size_t j = 0; j < positions.size(); j++) + for (size_t j = 0; j < positions.size(); j++) { - positions[j] = interpolate( - elapsed_s, - d_s, - prev.positions[j], - point.positions[j], - prev.velocities[j], - point.velocities[j] - ); + positions[j] = + interpolate(elapsed_s, d_s, prev.positions[j], point.positions[j], prev.velocities[j], point.velocities[j]); } - if(!execute(positions, true)) + if (!execute(positions, true)) return false; std::this_thread::sleep_for(std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.))); @@ -228,20 +222,20 @@ bool TrajectoryFollower::execute(std::vector &trajectory, std:: prev = point; } - //In theory it's possible the last position won't be sent by - //the interpolation loop above but rather some position between - //t[N-1] and t[N] where N is the number of trajectory points. - //To make sure this does not happen the last position is sent + // In theory it's possible the last position won't be sent by + // the interpolation loop above but rather some position between + // t[N-1] and t[N] where N is the number of trajectory points. + // To make sure this does not happen the last position is sent return execute(last.positions, true); } void TrajectoryFollower::stop() { - if(!running_) + if (!running_) return; - //std::array empty; - //execute(empty, false); + // std::array empty; + // execute(empty, false); server_.disconnectClient(); running_ = false; diff --git a/src/ros_main.cpp b/src/ros_main.cpp index f069ac0ec..4f2663433 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -11,8 +11,8 @@ #include "ur_modern_driver/ros/io_service.h" #include "ur_modern_driver/ros/mb_publisher.h" #include "ur_modern_driver/ros/rt_publisher.h" -#include "ur_modern_driver/ros/trajectory_follower.h" #include "ur_modern_driver/ros/service_stopper.h" +#include "ur_modern_driver/ros/trajectory_follower.h" #include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/factory.h" #include "ur_modern_driver/ur/messages.h" @@ -30,14 +30,8 @@ static const std::string BASE_FRAME_ARG("~base_frame"); static const std::string TOOL_FRAME_ARG("~tool_frame"); static const std::string JOINT_NAMES_PARAM("hardware_interface/joints"); -static const std::vector DEFAULT_JOINTS = { - "shoulder_pan_joint", - "shoulder_lift_joint", - "elbow_joint", - "wrist_1_joint", - "wrist_2_joint", - "wrist_3_joint" -}; +static const std::vector DEFAULT_JOINTS = { "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", + "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" }; static const int UR_SECONDARY_PORT = 30002; static const int UR_RT_PORT = 30003; @@ -65,7 +59,7 @@ bool parse_args(ProgArgs &args) return false; } ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001)); - ros::param::param(MAX_VEL_CHANGE_ARG, args.max_vel_change, 15.0); // rad/s + ros::param::param(MAX_VEL_CHANGE_ARG, args.max_vel_change, 15.0); // rad/s ros::param::param(ROS_CONTROL_ARG, args.use_ros_control, false); ros::param::param(PREFIX_ARG, args.prefix, std::string()); ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link"); @@ -74,7 +68,7 @@ bool parse_args(ProgArgs &args) return true; } -std::string getLocalIPAccessibleFromHost(std::string& host) +std::string getLocalIPAccessibleFromHost(std::string &host) { URStream stream(host, UR_RT_PORT); return stream.connect() ? stream.getIP() : std::string(); @@ -91,9 +85,9 @@ int main(int argc, char **argv) } std::string local_ip(getLocalIPAccessibleFromHost(args.host)); - + URFactory factory(args.host); - vector services; + vector services; // RT packets auto rt_parser = factory.getRTParser(); @@ -101,7 +95,7 @@ int main(int argc, char **argv) URProducer rt_prod(rt_stream, *rt_parser); RTPublisher rt_pub(args.prefix, args.base_frame, args.tool_frame, args.use_ros_control); auto rt_commander = factory.getCommander(rt_stream); - vector *> rt_vec{&rt_pub}; + vector *> rt_vec{ &rt_pub }; TrajectoryFollower traj_follower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3()); @@ -130,10 +124,10 @@ int main(int argc, char **argv) URStream state_stream(args.host, UR_SECONDARY_PORT); URProducer state_prod(state_stream, *state_parser); MBPublisher state_pub; - + ServiceStopper service_stopper(services); - vector *> state_vec{&state_pub, &service_stopper}; + vector *> state_vec{ &state_pub, &service_stopper }; MultiConsumer state_cons(state_vec); Pipeline state_pl(state_prod, state_cons); @@ -145,7 +139,7 @@ int main(int argc, char **argv) auto state_commander = factory.getCommander(state_stream); IOService io_service(*state_commander); - if(action_server) + if (action_server) action_server->start(); ros::spin(); @@ -154,8 +148,8 @@ int main(int argc, char **argv) rt_pl.stop(); state_pl.stop(); - - if(controller) + + if (controller) delete controller; LOG_INFO("Pipelines shutdown complete"); diff --git a/src/tcp_socket.cpp b/src/tcp_socket.cpp index c4f183917..0bd390abb 100644 --- a/src/tcp_socket.cpp +++ b/src/tcp_socket.cpp @@ -1,16 +1,14 @@ +#include #include #include -#include #include #include #include "ur_modern_driver/log.h" #include "ur_modern_driver/tcp_socket.h" -TCPSocket::TCPSocket() - : socket_fd_(-1) - , state_(SocketState::Invalid) -{ +TCPSocket::TCPSocket() : socket_fd_(-1), state_(SocketState::Invalid) +{ } TCPSocket::~TCPSocket() { @@ -26,7 +24,7 @@ void TCPSocket::setOptions(int socket_fd) bool TCPSocket::setup(std::string &host, int port) { - if(state_ == SocketState::Connected) + if (state_ == SocketState::Connected) return false; LOG_INFO("Setting up connection: %s:%d", host.c_str(), port); @@ -51,11 +49,11 @@ bool TCPSocket::setup(std::string &host, int port) bool connected = 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 && open(socket_fd_, p->ai_addr, p->ai_addrlen)) + if (socket_fd_ != -1 && open(socket_fd_, p->ai_addr, p->ai_addrlen)) { connected = true; break; @@ -63,8 +61,8 @@ bool TCPSocket::setup(std::string &host, int port) } freeaddrinfo(result); - - if(!connected) + + if (!connected) { state_ = SocketState::Invalid; LOG_ERROR("Connection setup failed for %s:%d", host.c_str(), port); @@ -80,7 +78,7 @@ bool TCPSocket::setup(std::string &host, int port) bool TCPSocket::setSocketFD(int socket_fd) { - if(state_ == SocketState::Connected) + if (state_ == SocketState::Connected) return false; socket_fd_ = socket_fd; state_ = SocketState::Connected; @@ -89,7 +87,7 @@ bool TCPSocket::setSocketFD(int socket_fd) void TCPSocket::close() { - if(state_ != SocketState::Connected) + if (state_ != SocketState::Connected) return; state_ = SocketState::Closed; ::shutdown(socket_fd_, SHUT_RDWR); @@ -100,9 +98,9 @@ std::string TCPSocket::getIP() { sockaddr_in name; socklen_t len = sizeof(name); - int res = ::getsockname(socket_fd_, (sockaddr*)&name, &len); + int res = ::getsockname(socket_fd_, (sockaddr *)&name, &len); - if(res < 0) + if (res < 0) { LOG_ERROR("Could not get local IP"); return std::string(); @@ -117,17 +115,17 @@ bool TCPSocket::read(uint8_t *buf, size_t buf_len, size_t &read) { read = 0; - if(state_ != SocketState::Connected) + if (state_ != SocketState::Connected) return false; - + ssize_t res = ::recv(socket_fd_, buf, buf_len, 0); - if(res == 0) + if (res == 0) { state_ = SocketState::Disconnected; return false; } - else if(res < 0) + else if (res < 0) return false; read = static_cast(res); @@ -137,8 +135,8 @@ bool TCPSocket::read(uint8_t *buf, size_t buf_len, size_t &read) bool TCPSocket::write(const uint8_t *buf, size_t buf_len, size_t &written) { written = 0; - - if(state_ != SocketState::Connected) + + if (state_ != SocketState::Connected) return false; size_t remaining = buf_len; @@ -150,10 +148,10 @@ bool TCPSocket::write(const uint8_t *buf, size_t buf_len, size_t &written) if (sent <= 0) return false; - + written += sent; remaining -= sent; } - + return true; } \ No newline at end of file diff --git a/src/ur/commander.cpp b/src/ur/commander.cpp index 0fd358e7b..96fde7d4c 100644 --- a/src/ur/commander.cpp +++ b/src/ur/commander.cpp @@ -1,10 +1,10 @@ #include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/log.h" -bool URCommander::write(std::string& s) +bool URCommander::write(std::string &s) { size_t len = s.size(); - const uint8_t* data = reinterpret_cast(s.c_str()); + const uint8_t *data = reinterpret_cast(s.c_str()); size_t written; return stream_.write(data, len, written); } @@ -12,10 +12,10 @@ bool URCommander::write(std::string& s) void URCommander::formatArray(std::ostringstream &out, std::array &values) { std::string mod("["); - for(auto const& val : values) + for (auto const &val : values) { - out << mod << val; - mod = ","; + out << mod << val; + mod = ","; } out << "]"; } @@ -27,7 +27,7 @@ bool URCommander::uploadProg(std::string &s) bool URCommander::setToolVoltage(uint8_t voltage) { - if(voltage != 0 || voltage != 12 || voltage != 24) + if (voltage != 0 || voltage != 12 || voltage != 24) return false; std::ostringstream out; @@ -59,7 +59,6 @@ bool URCommander::stopj(double a) return write(s); } - bool URCommander_V1_X::speedj(std::array &speeds, double acceleration) { std::ostringstream out; @@ -86,7 +85,6 @@ bool URCommander_V1_X::setDigitalOut(uint8_t pin, bool value) return write(s); } - bool URCommander_V3_X::speedj(std::array &speeds, double acceleration) { std::ostringstream out; @@ -110,16 +108,16 @@ bool URCommander_V3_X::setDigitalOut(uint8_t pin, bool value) std::ostringstream out; std::string func; - if(pin < 8) + if (pin < 8) { func = "set_standard_digital_out"; } - else if(pin < 16) + else if (pin < 16) { func = "set_configurable_digital_out"; pin -= 8; } - else if(pin < 18) + else if (pin < 18) { func = "set_tool_digital_out"; pin -= 16; @@ -127,7 +125,6 @@ bool URCommander_V3_X::setDigitalOut(uint8_t pin, bool value) else return false; - out << func << "(" << (int)pin << "," << (value ? "True" : "False") << ")\n"; std::string s(out.str()); return write(s); diff --git a/src/ur/master_board.cpp b/src/ur/master_board.cpp index 991b6ac85..c6aa9245f 100644 --- a/src/ur/master_board.cpp +++ b/src/ur/master_board.cpp @@ -59,7 +59,7 @@ bool MasterBoardData_V3_0__1::parseWith(BinParser& bp) bp.parse(euromap67_interface_installed); if (euromap67_interface_installed) - { + { if (!bp.checkSize(MasterBoardData_V3_0__1::EURO_SIZE)) return false; diff --git a/src/ur/server.cpp b/src/ur/server.cpp index 5530be988..be19ae07f 100644 --- a/src/ur/server.cpp +++ b/src/ur/server.cpp @@ -1,12 +1,11 @@ -#include -#include +#include "ur_modern_driver/ur/server.h" #include +#include #include +#include #include "ur_modern_driver/log.h" -#include "ur_modern_driver/ur/server.h" -URServer::URServer(int port) - : port_(port) +URServer::URServer(int port) : port_(port) { } @@ -29,7 +28,7 @@ std::string URServer::getIP() socklen_t len = sizeof(name); int res = ::getsockname(getSocketFD(), (sockaddr*)&name, &len); - if(res < 0) + if (res < 0) { LOG_ERROR("Could not get local IP"); return std::string(); @@ -44,11 +43,11 @@ bool URServer::bind() { std::string empty; bool res = TCPSocket::setup(empty, port_); - - if(!res) + + if (!res) return false; - if(::listen(getSocketFD(), 1) < 0) + if (::listen(getSocketFD(), 1) < 0) return false; return true; @@ -56,14 +55,14 @@ bool URServer::bind() bool URServer::accept() { - if(TCPSocket::getState() != SocketState::Connected || client_.getSocketFD() > 0) + if (TCPSocket::getState() != SocketState::Connected || client_.getSocketFD() > 0) return false; - struct sockaddr addr; + struct sockaddr addr; socklen_t addr_len; int client_fd = ::accept(getSocketFD(), &addr, &addr_len); - if(client_fd <= 0) + if (client_fd <= 0) return false; setOptions(client_fd); @@ -73,13 +72,13 @@ bool URServer::accept() void URServer::disconnectClient() { - if(client_.getState() != SocketState::Connected) + if (client_.getState() != SocketState::Connected) return; - + client_.close(); } -bool URServer::write(const uint8_t* buf, size_t buf_len, size_t &written) +bool URServer::write(const uint8_t* buf, size_t buf_len, size_t& written) { return client_.write(buf, buf_len, written); } \ No newline at end of file diff --git a/src/ur/stream.cpp b/src/ur/stream.cpp index cdb20f2e6..b0609da38 100644 --- a/src/ur/stream.cpp +++ b/src/ur/stream.cpp @@ -6,22 +6,22 @@ #include "ur_modern_driver/log.h" #include "ur_modern_driver/ur/stream.h" -bool URStream::write(const uint8_t* buf, size_t buf_len, size_t &written) +bool URStream::write(const uint8_t* buf, size_t buf_len, size_t& written) { std::lock_guard lock(write_mutex_); return TCPSocket::write(buf, buf_len, written); } -bool URStream::read(uint8_t* buf, size_t buf_len, size_t &total) +bool URStream::read(uint8_t* buf, size_t buf_len, size_t& total) { - std::lock_guard lock(read_mutex_); + std::lock_guard lock(read_mutex_); bool initial = true; uint8_t* buf_pos = buf; size_t remainder = sizeof(int32_t); size_t read = 0; - while(remainder > 0 && TCPSocket::read(buf_pos, remainder, read)) + while (remainder > 0 && TCPSocket::read(buf_pos, remainder, read)) { TCPSocket::setOptions(getSocketFD()); if (initial) @@ -39,6 +39,6 @@ bool URStream::read(uint8_t* buf, size_t buf_len, size_t &total) buf_pos += read; remainder -= read; } - + return remainder == 0; } \ No newline at end of file From b63e36b5339fdce5ba90ba01170b9dbf461afeee Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Sun, 9 Jul 2017 02:55:33 +0200 Subject: [PATCH 061/114] removed useless file --- include/ur_modern_driver/do_output.h | 33 ---------------------------- 1 file changed, 33 deletions(-) delete mode 100644 include/ur_modern_driver/do_output.h diff --git a/include/ur_modern_driver/do_output.h b/include/ur_modern_driver/do_output.h deleted file mode 100644 index 44c1e63bf..000000000 --- a/include/ur_modern_driver/do_output.h +++ /dev/null @@ -1,33 +0,0 @@ -/* - * do_output.h - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef UR_DO_OUTPUT_H_ -#define UR_DO_OUTPUT_H_ - -#ifdef ROS_BUILD -#include -#endif -#include - -void print_debug(std::string inp); -void print_info(std::string inp); -void print_warning(std::string inp); -void print_error(std::string inp); -void print_fatal(std::string inp); - -#endif /* UR_DO_OUTPUT_H_ */ From 88c9976e31b3e5b5d0a09aba70f83143155497e4 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Sun, 9 Jul 2017 04:01:16 +0200 Subject: [PATCH 062/114] Removed more old files --- src/robot_state.cpp | 399 --------------------------------- src/robot_state_RT.cpp | 492 ----------------------------------------- 2 files changed, 891 deletions(-) delete mode 100644 src/robot_state.cpp delete mode 100644 src/robot_state_RT.cpp diff --git a/src/robot_state.cpp b/src/robot_state.cpp deleted file mode 100644 index 786517c82..000000000 --- a/src/robot_state.cpp +++ /dev/null @@ -1,399 +0,0 @@ -/* - * robot_state.cpp - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "ur_modern_driver/robot_state.h" - -RobotState::RobotState(std::condition_variable& msg_cond) -{ - version_msg_.major_version = 0; - version_msg_.minor_version = 0; - new_data_available_ = false; - pMsg_cond_ = &msg_cond; - RobotState::setDisconnected(); - robot_mode_running_ = robotStateTypeV30::ROBOT_MODE_RUNNING; -} -double RobotState::ntohd(uint64_t nf) -{ - double x; - nf = be64toh(nf); - memcpy(&x, &nf, sizeof(x)); - return x; -} -void RobotState::unpack(uint8_t* buf, unsigned int buf_length) -{ - /* Returns missing bytes to unpack a message, or 0 if all data was parsed */ - unsigned int offset = 0; - while (buf_length > offset) - { - int len; - unsigned char message_type; - memcpy(&len, &buf[offset], sizeof(len)); - len = ntohl(len); - if (len + offset > buf_length) - { - return; - } - memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type)); - switch (message_type) - { - case messageType::ROBOT_MESSAGE: - RobotState::unpackRobotMessage(buf, offset, - len); //'len' is inclusive the 5 bytes from messageSize and messageType - break; - case messageType::ROBOT_STATE: - RobotState::unpackRobotState(buf, offset, - len); //'len' is inclusive the 5 bytes from messageSize and messageType - break; - case messageType::PROGRAM_STATE_MESSAGE: - // Don't do anything atm... - default: - break; - } - offset += len; - } - return; -} - -void RobotState::unpackRobotMessage(uint8_t* buf, unsigned int offset, uint32_t len) -{ - offset += 5; - uint64_t timestamp; - int8_t source, robot_message_type; - memcpy(×tamp, &buf[offset], sizeof(timestamp)); - offset += sizeof(timestamp); - memcpy(&source, &buf[offset], sizeof(source)); - offset += sizeof(source); - memcpy(&robot_message_type, &buf[offset], sizeof(robot_message_type)); - offset += sizeof(robot_message_type); - switch (robot_message_type) - { - case robotMessageType::ROBOT_MESSAGE_VERSION: - val_lock_.lock(); - version_msg_.timestamp = timestamp; - version_msg_.source = source; - version_msg_.robot_message_type = robot_message_type; - RobotState::unpackRobotMessageVersion(buf, offset, len); - val_lock_.unlock(); - break; - default: - break; - } -} - -void RobotState::unpackRobotState(uint8_t* buf, unsigned int offset, uint32_t len) -{ - offset += 5; - while (offset < len) - { - int32_t length; - uint8_t package_type; - memcpy(&length, &buf[offset], sizeof(length)); - length = ntohl(length); - memcpy(&package_type, &buf[offset + sizeof(length)], sizeof(package_type)); - switch (package_type) - { - case packageType::ROBOT_MODE_DATA: - val_lock_.lock(); - RobotState::unpackRobotMode(buf, offset + 5); - val_lock_.unlock(); - break; - - case packageType::MASTERBOARD_DATA: - val_lock_.lock(); - RobotState::unpackRobotStateMasterboard(buf, offset + 5); - val_lock_.unlock(); - break; - default: - break; - } - offset += length; - } - new_data_available_ = true; - pMsg_cond_->notify_all(); -} - -void RobotState::unpackRobotMessageVersion(uint8_t* buf, unsigned int offset, uint32_t len) -{ - memcpy(&version_msg_.project_name_size, &buf[offset], sizeof(version_msg_.project_name_size)); - offset += sizeof(version_msg_.project_name_size); - memcpy(&version_msg_.project_name, &buf[offset], sizeof(char) * version_msg_.project_name_size); - offset += version_msg_.project_name_size; - version_msg_.project_name[version_msg_.project_name_size] = '\0'; - memcpy(&version_msg_.major_version, &buf[offset], sizeof(version_msg_.major_version)); - offset += sizeof(version_msg_.major_version); - memcpy(&version_msg_.minor_version, &buf[offset], sizeof(version_msg_.minor_version)); - offset += sizeof(version_msg_.minor_version); - memcpy(&version_msg_.svn_revision, &buf[offset], sizeof(version_msg_.svn_revision)); - offset += sizeof(version_msg_.svn_revision); - version_msg_.svn_revision = ntohl(version_msg_.svn_revision); - memcpy(&version_msg_.build_date, &buf[offset], sizeof(char) * len - offset); - version_msg_.build_date[len - offset] = '\0'; - if (version_msg_.major_version < 2) - { - robot_mode_running_ = robotStateTypeV18::ROBOT_RUNNING_MODE; - } -} - -void RobotState::unpackRobotMode(uint8_t* buf, unsigned int offset) -{ - memcpy(&robot_mode_.timestamp, &buf[offset], sizeof(robot_mode_.timestamp)); - offset += sizeof(robot_mode_.timestamp); - uint8_t tmp; - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isRobotConnected = true; - else - robot_mode_.isRobotConnected = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isRealRobotEnabled = true; - else - robot_mode_.isRealRobotEnabled = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - // printf("PowerOnRobot: %d\n", tmp); - if (tmp > 0) - robot_mode_.isPowerOnRobot = true; - else - robot_mode_.isPowerOnRobot = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isEmergencyStopped = true; - else - robot_mode_.isEmergencyStopped = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isProtectiveStopped = true; - else - robot_mode_.isProtectiveStopped = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isProgramRunning = true; - else - robot_mode_.isProgramRunning = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isProgramPaused = true; - else - robot_mode_.isProgramPaused = false; - offset += sizeof(tmp); - memcpy(&robot_mode_.robotMode, &buf[offset], sizeof(robot_mode_.robotMode)); - offset += sizeof(robot_mode_.robotMode); - uint64_t temp; - if (RobotState::getVersion() > 2.) - { - memcpy(&robot_mode_.controlMode, &buf[offset], sizeof(robot_mode_.controlMode)); - offset += sizeof(robot_mode_.controlMode); - memcpy(&temp, &buf[offset], sizeof(temp)); - offset += sizeof(temp); - robot_mode_.targetSpeedFraction = RobotState::ntohd(temp); - } - memcpy(&temp, &buf[offset], sizeof(temp)); - offset += sizeof(temp); - robot_mode_.speedScaling = RobotState::ntohd(temp); -} - -void RobotState::unpackRobotStateMasterboard(uint8_t* buf, unsigned int offset) -{ - if (RobotState::getVersion() < 3.0) - { - int16_t digital_input_bits, digital_output_bits; - memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits)); - offset += sizeof(digital_input_bits); - memcpy(&digital_output_bits, &buf[offset], sizeof(digital_output_bits)); - offset += sizeof(digital_output_bits); - mb_data_.digitalInputBits = ntohs(digital_input_bits); - mb_data_.digitalOutputBits = ntohs(digital_output_bits); - } - else - { - memcpy(&mb_data_.digitalInputBits, &buf[offset], sizeof(mb_data_.digitalInputBits)); - offset += sizeof(mb_data_.digitalInputBits); - mb_data_.digitalInputBits = ntohl(mb_data_.digitalInputBits); - memcpy(&mb_data_.digitalOutputBits, &buf[offset], sizeof(mb_data_.digitalOutputBits)); - offset += sizeof(mb_data_.digitalOutputBits); - mb_data_.digitalOutputBits = ntohl(mb_data_.digitalOutputBits); - } - - memcpy(&mb_data_.analogInputRange0, &buf[offset], sizeof(mb_data_.analogInputRange0)); - offset += sizeof(mb_data_.analogInputRange0); - memcpy(&mb_data_.analogInputRange1, &buf[offset], sizeof(mb_data_.analogInputRange1)); - offset += sizeof(mb_data_.analogInputRange1); - uint64_t temp; - memcpy(&temp, &buf[offset], sizeof(temp)); - offset += sizeof(temp); - mb_data_.analogInput0 = RobotState::ntohd(temp); - memcpy(&temp, &buf[offset], sizeof(temp)); - offset += sizeof(temp); - mb_data_.analogInput1 = RobotState::ntohd(temp); - memcpy(&mb_data_.analogOutputDomain0, &buf[offset], sizeof(mb_data_.analogOutputDomain0)); - offset += sizeof(mb_data_.analogOutputDomain0); - memcpy(&mb_data_.analogOutputDomain1, &buf[offset], sizeof(mb_data_.analogOutputDomain1)); - offset += sizeof(mb_data_.analogOutputDomain1); - memcpy(&temp, &buf[offset], sizeof(temp)); - offset += sizeof(temp); - mb_data_.analogOutput0 = RobotState::ntohd(temp); - memcpy(&temp, &buf[offset], sizeof(temp)); - offset += sizeof(temp); - mb_data_.analogOutput1 = RobotState::ntohd(temp); - - memcpy(&mb_data_.masterBoardTemperature, &buf[offset], sizeof(mb_data_.masterBoardTemperature)); - offset += sizeof(mb_data_.masterBoardTemperature); - mb_data_.masterBoardTemperature = ntohl(mb_data_.masterBoardTemperature); - memcpy(&mb_data_.robotVoltage48V, &buf[offset], sizeof(mb_data_.robotVoltage48V)); - offset += sizeof(mb_data_.robotVoltage48V); - mb_data_.robotVoltage48V = ntohl(mb_data_.robotVoltage48V); - memcpy(&mb_data_.robotCurrent, &buf[offset], sizeof(mb_data_.robotCurrent)); - offset += sizeof(mb_data_.robotCurrent); - mb_data_.robotCurrent = ntohl(mb_data_.robotCurrent); - memcpy(&mb_data_.masterIOCurrent, &buf[offset], sizeof(mb_data_.masterIOCurrent)); - offset += sizeof(mb_data_.masterIOCurrent); - mb_data_.masterIOCurrent = ntohl(mb_data_.masterIOCurrent); - - memcpy(&mb_data_.safetyMode, &buf[offset], sizeof(mb_data_.safetyMode)); - offset += sizeof(mb_data_.safetyMode); - memcpy(&mb_data_.masterOnOffState, &buf[offset], sizeof(mb_data_.masterOnOffState)); - offset += sizeof(mb_data_.masterOnOffState); - - memcpy(&mb_data_.euromap67InterfaceInstalled, &buf[offset], sizeof(mb_data_.euromap67InterfaceInstalled)); - offset += sizeof(mb_data_.euromap67InterfaceInstalled); - if (mb_data_.euromap67InterfaceInstalled != 0) - { - memcpy(&mb_data_.euromapInputBits, &buf[offset], sizeof(mb_data_.euromapInputBits)); - offset += sizeof(mb_data_.euromapInputBits); - mb_data_.euromapInputBits = ntohl(mb_data_.euromapInputBits); - memcpy(&mb_data_.euromapOutputBits, &buf[offset], sizeof(mb_data_.euromapOutputBits)); - offset += sizeof(mb_data_.euromapOutputBits); - mb_data_.euromapOutputBits = ntohl(mb_data_.euromapOutputBits); - if (RobotState::getVersion() < 3.0) - { - int16_t euromap_voltage, euromap_current; - memcpy(&euromap_voltage, &buf[offset], sizeof(euromap_voltage)); - offset += sizeof(euromap_voltage); - memcpy(&euromap_current, &buf[offset], sizeof(euromap_current)); - offset += sizeof(euromap_current); - mb_data_.euromapVoltage = ntohs(euromap_voltage); - mb_data_.euromapCurrent = ntohs(euromap_current); - } - else - { - memcpy(&mb_data_.euromapVoltage, &buf[offset], sizeof(mb_data_.euromapVoltage)); - offset += sizeof(mb_data_.euromapVoltage); - mb_data_.euromapVoltage = ntohl(mb_data_.euromapVoltage); - memcpy(&mb_data_.euromapCurrent, &buf[offset], sizeof(mb_data_.euromapCurrent)); - offset += sizeof(mb_data_.euromapCurrent); - mb_data_.euromapCurrent = ntohl(mb_data_.euromapCurrent); - } - } -} - -double RobotState::getVersion() -{ - double ver; - val_lock_.lock(); - ver = version_msg_.major_version + 0.1 * version_msg_.minor_version + .0000001 * version_msg_.svn_revision; - val_lock_.unlock(); - return ver; -} - -void RobotState::finishedReading() -{ - new_data_available_ = false; -} - -bool RobotState::getNewDataAvailable() -{ - return new_data_available_; -} - -int RobotState::getDigitalInputBits() -{ - return mb_data_.digitalInputBits; -} -int RobotState::getDigitalOutputBits() -{ - return mb_data_.digitalOutputBits; -} -double RobotState::getAnalogInput0() -{ - return mb_data_.analogInput0; -} -double RobotState::getAnalogInput1() -{ - return mb_data_.analogInput1; -} -double RobotState::getAnalogOutput0() -{ - return mb_data_.analogOutput0; -} -double RobotState::getAnalogOutput1() -{ - return mb_data_.analogOutput1; -} -bool RobotState::isRobotConnected() -{ - return robot_mode_.isRobotConnected; -} -bool RobotState::isRealRobotEnabled() -{ - return robot_mode_.isRealRobotEnabled; -} -bool RobotState::isPowerOnRobot() -{ - return robot_mode_.isPowerOnRobot; -} -bool RobotState::isEmergencyStopped() -{ - return robot_mode_.isEmergencyStopped; -} -bool RobotState::isProtectiveStopped() -{ - return robot_mode_.isProtectiveStopped; -} -bool RobotState::isProgramRunning() -{ - return robot_mode_.isProgramRunning; -} -bool RobotState::isProgramPaused() -{ - return robot_mode_.isProgramPaused; -} -unsigned char RobotState::getRobotMode() -{ - return robot_mode_.robotMode; -} -bool RobotState::isReady() -{ - if (robot_mode_.robotMode == robot_mode_running_) - { - return true; - } - return false; -} - -void RobotState::setDisconnected() -{ - robot_mode_.isRobotConnected = false; - robot_mode_.isRealRobotEnabled = false; - robot_mode_.isPowerOnRobot = false; -} diff --git a/src/robot_state_RT.cpp b/src/robot_state_RT.cpp deleted file mode 100644 index 71bb7ba57..000000000 --- a/src/robot_state_RT.cpp +++ /dev/null @@ -1,492 +0,0 @@ -/* - * robotStateRT.cpp - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "ur_modern_driver/robot_state_RT.h" - -RobotStateRT::RobotStateRT(std::condition_variable& msg_cond) -{ - version_ = 0.0; - time_ = 0.0; - q_target_.assign(6, 0.0); - qd_target_.assign(6, 0.0); - qdd_target_.assign(6, 0.0); - i_target_.assign(6, 0.0); - m_target_.assign(6, 0.0); - q_actual_.assign(6, 0.0); - qd_actual_.assign(6, 0.0); - i_actual_.assign(6, 0.0); - i_control_.assign(6, 0.0); - tool_vector_actual_.assign(6, 0.0); - tcp_speed_actual_.assign(6, 0.0); - tcp_force_.assign(6, 0.0); - tool_vector_target_.assign(6, 0.0); - tcp_speed_target_.assign(6, 0.0); - digital_input_bits_.assign(64, false); - motor_temperatures_.assign(6, 0.0); - controller_timer_ = 0.0; - robot_mode_ = 0.0; - joint_modes_.assign(6, 0.0); - safety_mode_ = 0.0; - tool_accelerometer_values_.assign(3, 0.0); - speed_scaling_ = 0.0; - linear_momentum_norm_ = 0.0; - v_main_ = 0.0; - v_robot_ = 0.0; - i_robot_ = 0.0; - v_actual_.assign(6, 0.0); - data_published_ = false; - controller_updated_ = false; - pMsg_cond_ = &msg_cond; -} - -RobotStateRT::~RobotStateRT() -{ - /* Make sure nobody is waiting after this thread is destroyed */ - data_published_ = true; - controller_updated_ = true; - pMsg_cond_->notify_all(); -} - -void RobotStateRT::setDataPublished() -{ - data_published_ = false; -} -bool RobotStateRT::getDataPublished() -{ - return data_published_; -} - -void RobotStateRT::setControllerUpdated() -{ - controller_updated_ = false; -} -bool RobotStateRT::getControllerUpdated() -{ - return controller_updated_; -} - -double RobotStateRT::ntohd(uint64_t nf) -{ - double x; - nf = be64toh(nf); - memcpy(&x, &nf, sizeof(x)); - return x; -} - -std::vector RobotStateRT::unpackVector(uint8_t* buf, int start_index, int nr_of_vals) -{ - uint64_t q; - std::vector ret; - for (int i = 0; i < nr_of_vals; i++) - { - memcpy(&q, &buf[start_index + i * sizeof(q)], sizeof(q)); - ret.push_back(ntohd(q)); - } - return ret; -} - -std::vector RobotStateRT::unpackDigitalInputBits(int64_t data) -{ - std::vector ret; - for (int i = 0; i < 64; i++) - { - ret.push_back((data & (1 << i)) >> i); - } - return ret; -} - -void RobotStateRT::setVersion(double ver) -{ - val_lock_.lock(); - version_ = ver; - val_lock_.unlock(); -} - -double RobotStateRT::getVersion() -{ - double ret; - val_lock_.lock(); - ret = version_; - val_lock_.unlock(); - return ret; -} -double RobotStateRT::getTime() -{ - double ret; - val_lock_.lock(); - ret = time_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getQTarget() -{ - std::vector ret; - val_lock_.lock(); - ret = q_target_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getQdTarget() -{ - std::vector ret; - val_lock_.lock(); - ret = qd_target_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getQddTarget() -{ - std::vector ret; - val_lock_.lock(); - ret = qdd_target_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getITarget() -{ - std::vector ret; - val_lock_.lock(); - ret = i_target_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getMTarget() -{ - std::vector ret; - val_lock_.lock(); - ret = m_target_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getQActual() -{ - std::vector ret; - val_lock_.lock(); - ret = q_actual_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getQdActual() -{ - std::vector ret; - val_lock_.lock(); - ret = qd_actual_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getIActual() -{ - std::vector ret; - val_lock_.lock(); - ret = i_actual_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getIControl() -{ - std::vector ret; - val_lock_.lock(); - ret = i_control_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getToolVectorActual() -{ - std::vector ret; - val_lock_.lock(); - ret = tool_vector_actual_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getTcpSpeedActual() -{ - std::vector ret; - val_lock_.lock(); - ret = tcp_speed_actual_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getTcpForce() -{ - std::vector ret; - val_lock_.lock(); - ret = tcp_force_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getToolVectorTarget() -{ - std::vector ret; - val_lock_.lock(); - ret = tool_vector_target_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getTcpSpeedTarget() -{ - std::vector ret; - val_lock_.lock(); - ret = tcp_speed_target_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getDigitalInputBits() -{ - std::vector ret; - val_lock_.lock(); - ret = digital_input_bits_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getMotorTemperatures() -{ - std::vector ret; - val_lock_.lock(); - ret = motor_temperatures_; - val_lock_.unlock(); - return ret; -} -double RobotStateRT::getControllerTimer() -{ - double ret; - val_lock_.lock(); - ret = controller_timer_; - val_lock_.unlock(); - return ret; -} -double RobotStateRT::getRobotMode() -{ - double ret; - val_lock_.lock(); - ret = robot_mode_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getJointModes() -{ - std::vector ret; - val_lock_.lock(); - ret = joint_modes_; - val_lock_.unlock(); - return ret; -} -double RobotStateRT::getSafety_mode() -{ - double ret; - val_lock_.lock(); - ret = safety_mode_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getToolAccelerometerValues() -{ - std::vector ret; - val_lock_.lock(); - ret = tool_accelerometer_values_; - val_lock_.unlock(); - return ret; -} -double RobotStateRT::getSpeedScaling() -{ - double ret; - val_lock_.lock(); - ret = speed_scaling_; - val_lock_.unlock(); - return ret; -} -double RobotStateRT::getLinearMomentumNorm() -{ - double ret; - val_lock_.lock(); - ret = linear_momentum_norm_; - val_lock_.unlock(); - return ret; -} -double RobotStateRT::getVMain() -{ - double ret; - val_lock_.lock(); - ret = v_main_; - val_lock_.unlock(); - return ret; -} -double RobotStateRT::getVRobot() -{ - double ret; - val_lock_.lock(); - ret = v_robot_; - val_lock_.unlock(); - return ret; -} -double RobotStateRT::getIRobot() -{ - double ret; - val_lock_.lock(); - ret = i_robot_; - val_lock_.unlock(); - return ret; -} -std::vector RobotStateRT::getVActual() -{ - std::vector ret; - val_lock_.lock(); - ret = v_actual_; - val_lock_.unlock(); - return ret; -} -void RobotStateRT::unpack(uint8_t* buf) -{ - int64_t digital_input_bits; - uint64_t unpack_to; - uint16_t offset = 0; - val_lock_.lock(); - int len; - memcpy(&len, &buf[offset], sizeof(len)); - - offset += sizeof(len); - len = ntohl(len); - - // Check the correct message length is received - bool len_good = true; - if (version_ >= 1.6 && version_ < 1.7) - { // v1.6 - if (len != 756) - len_good = false; - } - else if (version_ >= 1.7 && version_ < 1.8) - { // v1.7 - if (len != 764) - len_good = false; - } - else if (version_ >= 1.8 && version_ < 1.9) - { // v1.8 - if (len != 812) - len_good = false; - } - else if (version_ >= 3.0 && version_ < 3.2) - { // v3.0 & v3.1 - if (len != 1044) - len_good = false; - } - else if (version_ >= 3.2 && version_ < 3.3) - { // v3.2 - if (len != 1060) - len_good = false; - } - - if (!len_good) - { - printf("Wrong length of message on RT interface: %i\n", len); - val_lock_.unlock(); - return; - } - - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - time_ = RobotStateRT::ntohd(unpack_to); - offset += sizeof(double); - q_target_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - qd_target_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - qdd_target_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - i_target_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - m_target_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - q_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - qd_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - i_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - if (version_ <= 1.9) - { - if (version_ > 1.6) - tool_accelerometer_values_ = unpackVector(buf, offset, 3); - offset += sizeof(double) * (3 + 15); - tcp_force_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tool_vector_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tcp_speed_actual_ = unpackVector(buf, offset, 6); - } - else - { - i_control_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tool_vector_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tcp_speed_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tcp_force_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tool_vector_target_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tcp_speed_target_ = unpackVector(buf, offset, 6); - } - offset += sizeof(double) * 6; - - memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits)); - digital_input_bits_ = unpackDigitalInputBits(be64toh(digital_input_bits)); - offset += sizeof(double); - motor_temperatures_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - controller_timer_ = ntohd(unpack_to); - if (version_ > 1.6) - { - offset += sizeof(double) * 2; - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - robot_mode_ = ntohd(unpack_to); - if (version_ > 1.7) - { - offset += sizeof(double); - joint_modes_ = unpackVector(buf, offset, 6); - } - } - if (version_ > 1.8) - { - offset += sizeof(double) * 6; - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - safety_mode_ = ntohd(unpack_to); - offset += sizeof(double); - tool_accelerometer_values_ = unpackVector(buf, offset, 3); - offset += sizeof(double) * 3; - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - speed_scaling_ = ntohd(unpack_to); - offset += sizeof(double); - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - linear_momentum_norm_ = ntohd(unpack_to); - offset += sizeof(double); - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - v_main_ = ntohd(unpack_to); - offset += sizeof(double); - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - v_robot_ = ntohd(unpack_to); - offset += sizeof(double); - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - i_robot_ = ntohd(unpack_to); - offset += sizeof(double); - v_actual_ = unpackVector(buf, offset, 6); - } - val_lock_.unlock(); - controller_updated_ = true; - data_published_ = true; - pMsg_cond_->notify_all(); -} From 79fd2ee864c0cb40688252c4c43d8b4bbd14120b Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Sun, 9 Jul 2017 04:02:40 +0200 Subject: [PATCH 063/114] Fixed missing include --- src/ros/trajectory_follower.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/ros/trajectory_follower.cpp b/src/ros/trajectory_follower.cpp index a1f45d943..18ca6912e 100644 --- a/src/ros/trajectory_follower.cpp +++ b/src/ros/trajectory_follower.cpp @@ -1,6 +1,7 @@ #include "ur_modern_driver/ros/trajectory_follower.h" #include #include +#include static const int32_t MULT_JOINTSTATE_ = 1000000; static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}"); From 1e724dcd33d57d47107be6ccc3a412fbd39d5d67 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Sun, 9 Jul 2017 04:03:37 +0200 Subject: [PATCH 064/114] Added bitset parsing and setter for random data --- include/ur_modern_driver/test/random_data.h | 26 +++++++++++++++++---- 1 file changed, 21 insertions(+), 5 deletions(-) diff --git a/include/ur_modern_driver/test/random_data.h b/include/ur_modern_driver/test/random_data.h index 14ae65b0a..4f9838693 100644 --- a/include/ur_modern_driver/test/random_data.h +++ b/include/ur_modern_driver/test/random_data.h @@ -5,31 +5,32 @@ #include #include #include +#include #include "ur_modern_driver/bin_parser.h" class RandomDataTest { private: using random_bytes_engine = std::independent_bits_engine; - uint8_t* _buf; + uint8_t* buf_; BinParser bp_; size_t n_; public: - RandomDataTest(size_t n) : _buf(new uint8_t[n]), bp_(_buf, n), n_(n) + RandomDataTest(size_t n) : buf_(new uint8_t[n]), bp_(buf_, n), n_(n) { random_bytes_engine rbe; - std::generate(_buf, _buf + n, std::ref(rbe)); + std::generate(buf_, buf_ + n, std::ref(rbe)); } ~RandomDataTest() { - delete _buf; + delete buf_; } BinParser getParser(bool skip = false) { - return BinParser(_buf, n_ - (skip ? sizeof(int32_t) : 0)); + return BinParser(buf_, n_ - (skip ? sizeof(int32_t) : 0)); } template @@ -39,6 +40,21 @@ class RandomDataTest bp_.parse(actual); return actual; } + + template + std::bitset getNext() + { + T actual; + bp_.parse(actual); + return std::bitset(actual); + } + + template + void set(T data, size_t pos) + { + std::memcpy(&data, buf_+pos, sizeof(T)); + } + void skip(size_t n) { bp_.consume(n); From 8e8a0428b071fdd087f3d1cbda6b333a969bb7f2 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Sun, 9 Jul 2017 04:04:22 +0200 Subject: [PATCH 065/114] Added testing for master board parsing and fixed unparsed fields --- CMakeLists.txt | 1 + src/ur/master_board.cpp | 4 ++ tests/ur/master_board.cpp | 136 ++++++++++++++++++++++++++++++++++++++ 3 files changed, 141 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index a950358f7..be227ba57 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -218,6 +218,7 @@ install(DIRECTORY include/${PROJECT_NAME}/ set(${PROJECT_NAME}_TEST_SOURCES tests/ur/rt_state.cpp + tests/ur/master_board.cpp tests/ur/robot_mode.cpp) if (CATKIN_ENABLE_TESTING) diff --git a/src/ur/master_board.cpp b/src/ur/master_board.cpp index c6aa9245f..0775a397f 100644 --- a/src/ur/master_board.cpp +++ b/src/ur/master_board.cpp @@ -37,6 +37,8 @@ bool MasterBoardData_V1_X::parseWith(BinParser& bp) if (!bp.checkSize(MasterBoardData_V1_X::EURO_SIZE)) return false; + bp.parse(euromap_input_bits); + bp.parse(euromap_output_bits); bp.parse(euromap_voltage); bp.parse(euromap_current); } @@ -63,6 +65,8 @@ bool MasterBoardData_V3_0__1::parseWith(BinParser& bp) if (!bp.checkSize(MasterBoardData_V3_0__1::EURO_SIZE)) return false; + bp.parse(euromap_input_bits); + bp.parse(euromap_output_bits); bp.parse(euromap_voltage); bp.parse(euromap_current); } diff --git a/tests/ur/master_board.cpp b/tests/ur/master_board.cpp index e69de29bb..2af943faf 100644 --- a/tests/ur/master_board.cpp +++ b/tests/ur/master_board.cpp @@ -0,0 +1,136 @@ +#include "ur_modern_driver/ur/master_board.h" +#include +#include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/test/random_data.h" +#include "ur_modern_driver/test/utils.h" +#include "ur_modern_driver/types.h" + +TEST(MasterBoardData_V1_X, testRandomDataParsing) +{ + RandomDataTest rdt(71); + rdt.set(1, 58); //sets euromap67_interface_installed to true + BinParser bp = rdt.getParser(); + MasterBoardData_V1_X state; + + ASSERT_TRUE(state.parseWith(bp)) << "parse() returned false"; + + ASSERT_EQ((rdt.getNext()), state.digital_input_bits); + ASSERT_EQ((rdt.getNext()), state.digital_output_bits); + ASSERT_EQ(rdt.getNext(), state.analog_input_range0); + ASSERT_EQ(rdt.getNext(), state.analog_input_range1); + ASSERT_EQ(rdt.getNext(), state.analog_input0); + ASSERT_EQ(rdt.getNext(), state.analog_input1); + ASSERT_EQ(rdt.getNext(), state.analog_output_domain0); + ASSERT_EQ(rdt.getNext(), state.analog_output_domain1); + ASSERT_EQ(rdt.getNext(), state.analog_output0); + ASSERT_EQ(rdt.getNext(), state.analog_output1); + ASSERT_EQ(rdt.getNext(), state.master_board_temperature); + ASSERT_EQ(rdt.getNext(), state.robot_voltage_48V); + ASSERT_EQ(rdt.getNext(), state.robot_current); + ASSERT_EQ(rdt.getNext(), state.master_IO_current); + ASSERT_EQ(rdt.getNext(), state.master_safety_state); + ASSERT_EQ(rdt.getNext(), state.master_on_off_state); + ASSERT_EQ(rdt.getNext(), state.euromap67_interface_installed); + ASSERT_EQ(rdt.getNext(), state.euromap_input_bits); + ASSERT_EQ(rdt.getNext(), state.euromap_output_bits); + ASSERT_EQ(rdt.getNext(), state.euromap_voltage); + ASSERT_EQ(rdt.getNext(), state.euromap_current); + + ASSERT_TRUE(bp.empty()) << "Did not consume all data"; +} + +TEST(MasterBoardData_V3_0__1, testRandomDataParsing) +{ + RandomDataTest rdt(83); + rdt.set(1, 62); //sets euromap67_interface_installed to true + BinParser bp = rdt.getParser(); + MasterBoardData_V3_0__1 state; + ASSERT_TRUE(state.parseWith(bp)) << "parse() returned false"; + + ASSERT_EQ((rdt.getNext()), state.digital_input_bits); + ASSERT_EQ((rdt.getNext()), state.digital_output_bits); + ASSERT_EQ(rdt.getNext(), state.analog_input_range0); + ASSERT_EQ(rdt.getNext(), state.analog_input_range1); + ASSERT_EQ(rdt.getNext(), state.analog_input0); + ASSERT_EQ(rdt.getNext(), state.analog_input1); + ASSERT_EQ(rdt.getNext(), state.analog_output_domain0); + ASSERT_EQ(rdt.getNext(), state.analog_output_domain1); + ASSERT_EQ(rdt.getNext(), state.analog_output0); + ASSERT_EQ(rdt.getNext(), state.analog_output1); + ASSERT_EQ(rdt.getNext(), state.master_board_temperature); + ASSERT_EQ(rdt.getNext(), state.robot_voltage_48V); + ASSERT_EQ(rdt.getNext(), state.robot_current); + ASSERT_EQ(rdt.getNext(), state.master_IO_current); + ASSERT_EQ(rdt.getNext(), state.safety_mode); + ASSERT_EQ(rdt.getNext(), state.in_reduced_mode); + ASSERT_EQ(rdt.getNext(), state.euromap67_interface_installed); + ASSERT_EQ(rdt.getNext(), state.euromap_input_bits); + ASSERT_EQ(rdt.getNext(), state.euromap_output_bits); + ASSERT_EQ(rdt.getNext(), state.euromap_voltage); + ASSERT_EQ(rdt.getNext(), state.euromap_current); + + rdt.skip(sizeof(uint32_t)); + + ASSERT_TRUE(bp.empty()) << "Did not consume all data"; +} + +TEST(MasterBoardData_V3_2, testRandomDataParsing) +{ + RandomDataTest rdt(85); + rdt.set(1, 62); //sets euromap67_interface_installed to true + BinParser bp = rdt.getParser(); + MasterBoardData_V3_2 state; + ASSERT_TRUE(state.parseWith(bp)) << "parse() returned false"; + + ASSERT_EQ((rdt.getNext()), state.digital_input_bits); + ASSERT_EQ((rdt.getNext()), state.digital_output_bits); + ASSERT_EQ(rdt.getNext(), state.analog_input_range0); + ASSERT_EQ(rdt.getNext(), state.analog_input_range1); + ASSERT_EQ(rdt.getNext(), state.analog_input0); + ASSERT_EQ(rdt.getNext(), state.analog_input1); + ASSERT_EQ(rdt.getNext(), state.analog_output_domain0); + ASSERT_EQ(rdt.getNext(), state.analog_output_domain1); + ASSERT_EQ(rdt.getNext(), state.analog_output0); + ASSERT_EQ(rdt.getNext(), state.analog_output1); + ASSERT_EQ(rdt.getNext(), state.master_board_temperature); + ASSERT_EQ(rdt.getNext(), state.robot_voltage_48V); + ASSERT_EQ(rdt.getNext(), state.robot_current); + ASSERT_EQ(rdt.getNext(), state.master_IO_current); + ASSERT_EQ(rdt.getNext(), state.safety_mode); + ASSERT_EQ(rdt.getNext(), state.in_reduced_mode); + ASSERT_EQ(rdt.getNext(), state.euromap67_interface_installed); + ASSERT_EQ(rdt.getNext(), state.euromap_input_bits); + ASSERT_EQ(rdt.getNext(), state.euromap_output_bits); + ASSERT_EQ(rdt.getNext(), state.euromap_voltage); + ASSERT_EQ(rdt.getNext(), state.euromap_current); + rdt.skip(sizeof(uint32_t)); + ASSERT_EQ(rdt.getNext(), state.operational_mode_selector_input); + ASSERT_EQ(rdt.getNext(), state.three_position_enabling_device_input); + + ASSERT_TRUE(bp.empty()) << "Did not consume all data"; +} + +TEST(MasterBoardData_V1_X, testTooSmallBuffer) +{ + RandomDataTest rdt(10); + BinParser bp = rdt.getParser(); + MasterBoardData_V1_X state; + EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough"; +} + +TEST(MasterBoardData_V3_0__1, testTooSmallBuffer) +{ + RandomDataTest rdt(10); + BinParser bp = rdt.getParser(); + MasterBoardData_V3_0__1 state; + EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough"; +} + +TEST(MasterBoardData_V3_2, testTooSmallBuffer) +{ + RandomDataTest rdt(10); + BinParser bp = rdt.getParser(); + MasterBoardData_V3_2 state; + EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough"; +} From a170b9f51eb3040fe417121e5b3dd46277dabecc Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Mon, 17 Jul 2017 23:30:39 +0200 Subject: [PATCH 066/114] Fixed issues preventing from compiling on Kinetic --- include/ur_modern_driver/ros/controller.h | 3 +-- include/ur_modern_driver/ros/hardware_interface.h | 5 +++++ src/ros/controller.cpp | 8 +++++--- src/ros/hardware_interface.cpp | 6 ++++++ 4 files changed, 17 insertions(+), 5 deletions(-) diff --git a/include/ur_modern_driver/ros/controller.h b/include/ur_modern_driver/ros/controller.h index ea4989b16..d99dc0752 100644 --- a/include/ur_modern_driver/ros/controller.h +++ b/include/ur_modern_driver/ros/controller.h @@ -1,7 +1,6 @@ #pragma once #include #include -#include #include #include #include @@ -46,7 +45,7 @@ class ROSController : private hardware_interface::RobotHW, public URRTPacketCons void registerControllereInterface(T* interface) { registerInterface(interface); - available_interfaces_[hardware_interface::internal::demangledTypeName()] = interface; + available_interfaces_[T::INTERFACE_NAME] = interface; } void read(RTShared& state); diff --git a/include/ur_modern_driver/ros/hardware_interface.h b/include/ur_modern_driver/ros/hardware_interface.h index 5dd95c1f2..50e311b15 100644 --- a/include/ur_modern_driver/ros/hardware_interface.h +++ b/include/ur_modern_driver/ros/hardware_interface.h @@ -33,7 +33,9 @@ class JointInterface : public hardware_interface::JointStateInterface public: JointInterface(std::vector &joint_names); void update(RTShared &packet); + typedef hardware_interface::JointStateInterface parent_type; + static const std::string INTERFACE_NAME; }; class WrenchInterface : public hardware_interface::ForceTorqueSensorInterface @@ -44,6 +46,7 @@ class WrenchInterface : public hardware_interface::ForceTorqueSensorInterface WrenchInterface(); void update(RTShared &packet); typedef hardware_interface::ForceTorqueSensorInterface parent_type; + static const std::string INTERFACE_NAME; }; class VelocityInterface : public HardwareInterface, public hardware_interface::VelocityJointInterface @@ -59,6 +62,7 @@ class VelocityInterface : public HardwareInterface, public hardware_interface::V virtual bool write(); virtual void reset(); typedef hardware_interface::VelocityJointInterface parent_type; + static const std::string INTERFACE_NAME; }; class PositionInterface : public HardwareInterface, public hardware_interface::PositionJointInterface @@ -75,4 +79,5 @@ class PositionInterface : public HardwareInterface, public hardware_interface::P virtual void stop(); typedef hardware_interface::PositionJointInterface parent_type; + static const std::string INTERFACE_NAME; }; \ No newline at end of file diff --git a/src/ros/controller.cpp b/src/ros/controller.cpp index 75aa9c3d4..3e8b0f084 100644 --- a/src/ros/controller.cpp +++ b/src/ros/controller.cpp @@ -33,21 +33,23 @@ void ROSController::doSwitch(const std::list for (auto const& ci : start_list) { - auto ait = available_interfaces_.find(ci.hardware_interface); + auto ait = available_interfaces_.find(ci.name); if (ait == available_interfaces_.end()) continue; auto new_interface = ait->second; - LOG_INFO("Starting %s", ci.hardware_interface.c_str()); + LOG_INFO("Starting %s", ci.name.c_str()); + active_interface_ = new_interface; new_interface->start(); return; } - LOG_WARN("Failed to start interface!"); + if(start_list.size() > 0) + LOG_WARN("Failed to start interface!"); } bool ROSController::write() diff --git a/src/ros/hardware_interface.cpp b/src/ros/hardware_interface.cpp index fde4e85be..dc1ce9bf9 100644 --- a/src/ros/hardware_interface.cpp +++ b/src/ros/hardware_interface.cpp @@ -1,6 +1,7 @@ #include "ur_modern_driver/ros/hardware_interface.h" #include "ur_modern_driver/log.h" +const std::string JointInterface::INTERFACE_NAME = "joint_state_controller"; JointInterface::JointInterface(std::vector &joint_names) { for (size_t i = 0; i < 6; i++) @@ -16,6 +17,8 @@ void JointInterface::update(RTShared &packet) efforts_ = packet.i_actual; } + +const std::string WrenchInterface::INTERFACE_NAME = "force_torque_sensor_controller"; WrenchInterface::WrenchInterface() { registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", "", tcp_.begin(), tcp_.begin() + 3)); @@ -26,6 +29,8 @@ void WrenchInterface::update(RTShared &packet) tcp_ = packet.tcp_force; } + +const std::string VelocityInterface::INTERFACE_NAME = "vel_based_pos_traj_controller"; VelocityInterface::VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names, double max_vel_change) : commander_(commander), max_vel_change_(max_vel_change) @@ -57,6 +62,7 @@ void VelocityInterface::reset() } } +const std::string PositionInterface::INTERFACE_NAME = "pos_based_pos_traj_controller"; PositionInterface::PositionInterface(TrajectoryFollower &follower, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names) From 6e8b2ef5ca14c266126e82f43fb1ffb1f3f0a839 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Mon, 17 Jul 2017 23:33:18 +0200 Subject: [PATCH 067/114] Fixed minor ur3 launch typos issue #98 --- launch/ur3_ros_control.launch | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/ur3_ros_control.launch b/launch/ur3_ros_control.launch index 5b782aaf1..6e88e9fb2 100644 --- a/launch/ur3_ros_control.launch +++ b/launch/ur3_ros_control.launch @@ -38,11 +38,11 @@ + output="screen" args="joint_state_controller force_torque_sensor_controller vel_based_pos_traj_controller" /> + output="screen" args="load pos_based_pos_traj_controller" /> From 1e4934a1992b31c10c3604af162221057afcd9ee Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 21 Jul 2017 17:32:13 +0200 Subject: [PATCH 068/114] Adds missing param max_velocity. --- src/ros_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/ros_main.cpp b/src/ros_main.cpp index 4f2663433..b680ff6a7 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -60,6 +60,7 @@ bool parse_args(ProgArgs &args) } ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001)); ros::param::param(MAX_VEL_CHANGE_ARG, args.max_vel_change, 15.0); // rad/s + ros::param::param(MAX_VEL_CHANGE_ARG, args.max_velocity, 10.0); ros::param::param(ROS_CONTROL_ARG, args.use_ros_control, false); ros::param::param(PREFIX_ARG, args.prefix, std::string()); ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link"); From 54a852305cf99e7ca730d06d7f7550bb55b78b4f Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 21 Jul 2017 17:33:28 +0200 Subject: [PATCH 069/114] Adds prefix to all joint names. --- src/ros_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/ros_main.cpp b/src/ros_main.cpp index b680ff6a7..8d74e3e84 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -85,6 +85,10 @@ int main(int argc, char **argv) return EXIT_FAILURE; } + //Add prefix to joint names + std::transform (args.joint_names.begin(), args.joint_names.end(), args.joint_names.begin(), + [&args](std::string name){return args.prefix + name;}); + std::string local_ip(getLocalIPAccessibleFromHost(args.host)); URFactory factory(args.host); @@ -156,4 +160,4 @@ int main(int argc, char **argv) LOG_INFO("Pipelines shutdown complete"); return EXIT_SUCCESS; -} \ No newline at end of file +} From 63691e038e393abb531b0fd2e1b90b9ddd889c97 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 21 Jul 2017 17:34:58 +0200 Subject: [PATCH 070/114] Extend error messages Extends error message to print invalid joint names. Adds max_velocity as parameter descriptor in error message. --- src/ros/action_server.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/ros/action_server.cpp b/src/ros/action_server.cpp index 380acd3a3..494d93296 100644 --- a/src/ros/action_server.cpp +++ b/src/ros/action_server.cpp @@ -145,7 +145,11 @@ bool ActionServer::validateJoints(GoalHandle& gh, Result& res) return true; res.error_code = Result::INVALID_JOINTS; - res.error_string = "Invalid joint names for goal"; + res.error_string = "Invalid joint names for goal\n"; + res.error_string += "Expected: "; + std::for_each(goal_joints.begin(), goal_joints.end(), [&res](std::string joint){res.error_string += joint + ", ";}); + res.error_string += "\nFound: "; + std::for_each(joint_set_.begin(), joint_set_.end(), [&res](std::string joint){res.error_string += joint + ", ";}); return false; } @@ -183,7 +187,7 @@ bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res) } if (std::fabs(velocity) > max_velocity_) { - res.error_string = "Received a goal with velocities that are higher than " + std::to_string(max_velocity_); + res.error_string = "Received a goal with velocities that are higher than max_velocity_ " + std::to_string(max_velocity_); return false; } } @@ -338,4 +342,4 @@ void ActionServer::trajectoryThread() has_goal_ = false; lk.unlock(); } -} \ No newline at end of file +} From 4375ffecc6b36379e0591e5b40a9294069be9f58 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 21 Jul 2017 17:37:07 +0200 Subject: [PATCH 071/114] Adds multiple retries to accept reverse connection We observed that the accept failed nondeterministically in rare cases. --- src/ur/server.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/ur/server.cpp b/src/ur/server.cpp index be19ae07f..4da9b2c3d 100644 --- a/src/ur/server.cpp +++ b/src/ur/server.cpp @@ -60,10 +60,14 @@ bool URServer::accept() struct sockaddr addr; socklen_t addr_len; - int client_fd = ::accept(getSocketFD(), &addr, &addr_len); + int client_fd = -1; - if (client_fd <= 0) - return false; + int retry = 0; + while((client_fd = ::accept(getSocketFD(), &addr, &addr_len)) == -1){ + ROS_ERROR_STREAM("Accepting socket connection failed. (errno: " << errno << ")"); + if(retry++ >= 5) + return false; + } setOptions(client_fd); @@ -81,4 +85,4 @@ void URServer::disconnectClient() bool URServer::write(const uint8_t* buf, size_t buf_len, size_t& written) { return client_.write(buf, buf_len, written); -} \ No newline at end of file +} From d3637e9633146c090107b943b652d5fe2ffad038 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 21 Jul 2017 17:45:18 +0200 Subject: [PATCH 072/114] Fixing some typos. --- src/ros/trajectory_follower.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/ros/trajectory_follower.cpp b/src/ros/trajectory_follower.cpp index 18ca6912e..bec6db777 100644 --- a/src/ros/trajectory_follower.cpp +++ b/src/ros/trajectory_follower.cpp @@ -111,11 +111,11 @@ bool TrajectoryFollower::start() return false; } - LOG_DEBUG("Awaiting incomming robot connection"); + LOG_DEBUG("Awaiting incoming robot connection"); if (!server_.accept()) { - LOG_ERROR("Failed to accept incomming robot connection"); + LOG_ERROR("Failed to accept incoming robot connection"); return false; } @@ -240,4 +240,4 @@ void TrajectoryFollower::stop() server_.disconnectClient(); running_ = false; -} \ No newline at end of file +} From 75c3733eb75789761e6383d587c7913b2ad7865e Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 21 Jul 2017 18:02:48 +0200 Subject: [PATCH 073/114] Fix type warning in log output --- include/ur_modern_driver/ur/state_parser.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/include/ur_modern_driver/ur/state_parser.h b/include/ur_modern_driver/ur/state_parser.h index 48eda1d9f..146d93c9a 100644 --- a/include/ur_modern_driver/ur/state_parser.h +++ b/include/ur_modern_driver/ur/state_parser.h @@ -75,7 +75,7 @@ class URStateParser : public URParser if (!packet->parseWith(sbp)) { - LOG_ERROR("Sub-package parsing of type %d failed!", type); + LOG_ERROR("Sub-package parsing of type %d failed!", static_cast(type)); return false; } @@ -83,7 +83,7 @@ class URStateParser : public URParser if (!sbp.empty()) { - LOG_ERROR("Sub-package of type %d was not parsed completely!", type); + LOG_ERROR("Sub-package of type %d was not parsed completely!", static_cast(type)); sbp.debug(); return false; } @@ -95,4 +95,4 @@ class URStateParser : public URParser typedef URStateParser URStateParser_V1_X; typedef URStateParser URStateParser_V3_0__1; -typedef URStateParser URStateParser_V3_2; \ No newline at end of file +typedef URStateParser URStateParser_V3_2; From dbcf8aeb4102978528249a7cd28d0e8e269fc316 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Fri, 21 Jul 2017 19:01:57 +0200 Subject: [PATCH 074/114] Fixed SO_REUSEADDR being set after bind rather than before --- include/ur_modern_driver/ur/server.h | 6 +----- src/ur/server.cpp | 18 +++++++++--------- 2 files changed, 10 insertions(+), 14 deletions(-) diff --git a/include/ur_modern_driver/ur/server.h b/include/ur_modern_driver/ur/server.h index f236b965b..c240ba5ce 100644 --- a/include/ur_modern_driver/ur/server.h +++ b/include/ur_modern_driver/ur/server.h @@ -15,11 +15,7 @@ class URServer : private TCPSocket TCPSocket client_; protected: - virtual bool open(int socket_fd, struct sockaddr *address, size_t address_len) - { - return ::bind(socket_fd, address, address_len) == 0; - } - virtual void setOptions(int socket_fd); + virtual bool open(int socket_fd, struct sockaddr *address, size_t address_len); public: URServer(int port); diff --git a/src/ur/server.cpp b/src/ur/server.cpp index be19ae07f..e2f7b2215 100644 --- a/src/ur/server.cpp +++ b/src/ur/server.cpp @@ -14,14 +14,6 @@ URServer::~URServer() TCPSocket::close(); } -void URServer::setOptions(int socket_fd) -{ - TCPSocket::setOptions(socket_fd); - - int flag = 1; - setsockopt(socket_fd, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int)); -} - std::string URServer::getIP() { sockaddr_in name; @@ -39,6 +31,14 @@ std::string URServer::getIP() return std::string(buf); } + +bool URServer::open(int socket_fd, struct sockaddr *address, size_t address_len) +{ + int flag = 1; + setsockopt(socket_fd, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int)); + return ::bind(socket_fd, address, address_len) == 0; +} + bool URServer::bind() { std::string empty; @@ -65,7 +65,7 @@ bool URServer::accept() if (client_fd <= 0) return false; - setOptions(client_fd); + TCPSocket::setOptions(client_fd); return client_.setSocketFD(client_fd); } From 45480881ffe32ecf5c89358b30443ad766ca9aea Mon Sep 17 00:00:00 2001 From: Henning <1kayser@informatik.uni-hamburg.de> Date: Mon, 24 Jul 2017 12:20:45 +0200 Subject: [PATCH 075/114] Change ROS_ERROR_STREAM to LOG_ERROR. --- src/ur/server.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ur/server.cpp b/src/ur/server.cpp index 4da9b2c3d..cbdaa5b25 100644 --- a/src/ur/server.cpp +++ b/src/ur/server.cpp @@ -64,7 +64,7 @@ bool URServer::accept() int retry = 0; while((client_fd = ::accept(getSocketFD(), &addr, &addr_len)) == -1){ - ROS_ERROR_STREAM("Accepting socket connection failed. (errno: " << errno << ")"); + LOG_ERROR("Accepting socket connection failed. (errno: %d)", errno); if(retry++ >= 5) return false; } From 24b81c6f94561b4e3a5847de7edf3857cdb8ff0a Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Mon, 24 Jul 2017 19:12:00 +0200 Subject: [PATCH 076/114] Started ROS CI config --- .travis.yml | 34 ++++++++++++++++++++++++++++++++++ 1 file changed, 34 insertions(+) create mode 100644 .travis.yml diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 000000000..212ed20db --- /dev/null +++ b/.travis.yml @@ -0,0 +1,34 @@ +# This config file for Travis CI utilizes ros-industrial/industrial_ci package. +# For more info for the package, see https://github.com/ros-industrial/industrial_ci/blob/master/README.rst +sudo: required +dist: trusty +services: + - docker +language: generic +compiler: + - gcc +notifications: + email: + recipients: + - gm130s@gmail.com +env: + matrix: + - ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true + - ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true + - ROS_DISTRO="indigo" PRERELEASE=true + - ROS_DISTRO="jade" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true + - ROS_DISTRO="jade" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true + - ROS_DISTRO="jade" PRERELEASE=true + - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu NOT_TEST_INSTALL=true + - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu NOT_TEST_INSTALL=true + - ROS_DISTRO="kinetic" PRERELEASE=true +matrix: + allow_failures: + - env: ROS_DISTRO="indigo" PRERELEASE=true # Run docker-based ROS prerelease test http://wiki.ros.org/bloom/Tutorials/PrereleaseTest Because we might not want to run prerelease test for all PRs, it's omitted from pass-fail criteria. + - env: ROS_DISTRO="jade" PRERELEASE=true + - env: ROS_DISTRO="kinetic" PRERELEASE=true +install: + - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config +script: + - source .ci_config/travis.sh +# - source ./travis.sh # Enable this when you have a package-local script From 336ad6453a445bc0d6ea29fc5fa8e1fdf6291f4d Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Fri, 28 Jul 2017 01:17:09 +0200 Subject: [PATCH 077/114] Better travis config --- .travis.yml | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/.travis.yml b/.travis.yml index 212ed20db..589d0d570 100644 --- a/.travis.yml +++ b/.travis.yml @@ -7,26 +7,14 @@ services: language: generic compiler: - gcc -notifications: - email: - recipients: - - gm130s@gmail.com env: matrix: - ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true - ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true - - ROS_DISTRO="indigo" PRERELEASE=true - ROS_DISTRO="jade" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true - ROS_DISTRO="jade" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true - - ROS_DISTRO="jade" PRERELEASE=true - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu NOT_TEST_INSTALL=true - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu NOT_TEST_INSTALL=true - - ROS_DISTRO="kinetic" PRERELEASE=true -matrix: - allow_failures: - - env: ROS_DISTRO="indigo" PRERELEASE=true # Run docker-based ROS prerelease test http://wiki.ros.org/bloom/Tutorials/PrereleaseTest Because we might not want to run prerelease test for all PRs, it's omitted from pass-fail criteria. - - env: ROS_DISTRO="jade" PRERELEASE=true - - env: ROS_DISTRO="kinetic" PRERELEASE=true install: - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config script: From 5de8950bdfe307d3ffa7edcbe2f06f531b13e725 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Fri, 28 Jul 2017 01:23:59 +0200 Subject: [PATCH 078/114] travis debugging... --- .travis.yml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.travis.yml b/.travis.yml index 589d0d570..a8c8a81bd 100644 --- a/.travis.yml +++ b/.travis.yml @@ -8,6 +8,11 @@ language: generic compiler: - gcc env: + global: + - VERBOSE_OUTPUT='true' + - POST_PROCESS='echo "post processing reached"' + - BEFORE_SCRIPT='echo current dir: $(pwd)' + matrix: - ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true - ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true From afff1e440b258ec43acfabf43943bfdc1f93f067 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Fri, 28 Jul 2017 01:36:18 +0200 Subject: [PATCH 079/114] Fixed travis "fatal error: std_srvs/Empty.h: No such file or directory" --- package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/package.xml b/package.xml index 5cf0127f3..f1cd9960d 100644 --- a/package.xml +++ b/package.xml @@ -52,6 +52,7 @@ ur_msgs tf realtime_tools + std_srvs hardware_interface controller_manager ros_controllers @@ -66,6 +67,7 @@ ur_description tf realtime_tools + std_srvs From b57e327a0970ab7844181a93a5deff2f92414b5f Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Fri, 28 Jul 2017 02:11:38 +0200 Subject: [PATCH 080/114] Removed jade and kinetic builds from travis for now --- .travis.yml | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/.travis.yml b/.travis.yml index a8c8a81bd..a8a72aa4a 100644 --- a/.travis.yml +++ b/.travis.yml @@ -8,18 +8,14 @@ language: generic compiler: - gcc env: - global: - - VERBOSE_OUTPUT='true' - - POST_PROCESS='echo "post processing reached"' - - BEFORE_SCRIPT='echo current dir: $(pwd)' - matrix: - ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true - ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true - - ROS_DISTRO="jade" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true - - ROS_DISTRO="jade" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true - - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu NOT_TEST_INSTALL=true - - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu NOT_TEST_INSTALL=true +# Disable jade and kinetic for now because ur_description is not released for them +# - ROS_DISTRO="jade" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true +# - ROS_DISTRO="jade" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true +# - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu NOT_TEST_INSTALL=true +# - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu NOT_TEST_INSTALL=true install: - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config script: From f70255926b327b238582c5589266345246aed38c Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Wed, 16 Aug 2017 16:56:21 +0200 Subject: [PATCH 081/114] Implement activation modes of robot_enable service Adds parameter 'require_activation' to configure when the service should be called (Always, Never, OnStartup). --- .../ur_modern_driver/ros/service_stopper.h | 10 ++++++- launch/ur_common.launch | 6 +++- src/ros/service_stopper.cpp | 30 +++++++++++++++++-- 3 files changed, 42 insertions(+), 4 deletions(-) diff --git a/include/ur_modern_driver/ros/service_stopper.h b/include/ur_modern_driver/ros/service_stopper.h index ffbe10646..f0b2c16ef 100644 --- a/include/ur_modern_driver/ros/service_stopper.h +++ b/include/ur_modern_driver/ros/service_stopper.h @@ -12,6 +12,13 @@ enum class RobotState ProtectiveStopped }; +enum class ActivationMode +{ + Always, + Never, + OnStartup +}; + class Service { public: @@ -25,6 +32,7 @@ class ServiceStopper : public URStatePacketConsumer ros::ServiceServer enable_service_; std::vector services_; RobotState last_state_; + ActivationMode activation_mode_; void notify_all(RobotState state); bool handle(SharedRobotModeData& data, bool error); @@ -59,4 +67,4 @@ class ServiceStopper : public URStatePacketConsumer { return true; } -}; \ No newline at end of file +}; diff --git a/launch/ur_common.launch b/launch/ur_common.launch index 765015636..25aa71f24 100644 --- a/launch/ur_common.launch +++ b/launch/ur_common.launch @@ -15,6 +15,9 @@ + + + @@ -30,7 +33,8 @@ - + + diff --git a/src/ros/service_stopper.cpp b/src/ros/service_stopper.cpp index 2f46fd5ba..1d1a3e6ac 100644 --- a/src/ros/service_stopper.cpp +++ b/src/ros/service_stopper.cpp @@ -4,12 +4,33 @@ ServiceStopper::ServiceStopper(std::vector services) : enable_service_(nh_.advertiseService("ur_driver/robot_enable", &ServiceStopper::enableCallback, this)) , services_(services) , last_state_(RobotState::Error) + , activation_mode_(ActivationMode::Always) { - // enable_all(); + std::string mode; + ros::param::param("~require_activation", mode, std::string("Always")); + if (mode == "Never") + { + activation_mode_ = ActivationMode::Never; + notify_all(RobotState::Running); + } + else if (mode == "OnStartup") + { + activation_mode_ = ActivationMode::OnStartup; + } + else if (mode != "Always") + { + LOG_WARN("Found invalid value for param service_stopper_mode: '%s'\nShould be one of Always, Never, OnStartup", mode.c_str()); + mode = "Always"; + } + + LOG_INFO("ActivationMode mode: %s", mode.c_str()); } bool ServiceStopper::enableCallback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp) { + // After the startup call OnStartup and Never behave the same + if (activation_mode_ == ActivationMode::OnStartup) + activation_mode_ = ActivationMode::Never; notify_all(RobotState::Running); return true; } @@ -41,6 +62,11 @@ bool ServiceStopper::handle(SharedRobotModeData& data, bool error) { notify_all(RobotState::Error); } + else if (activation_mode_ == ActivationMode::Never) + { + // No error encountered, the user requested automatic reactivation + notify_all(RobotState::Running); + } return true; -} \ No newline at end of file +} From 231840fabff8ed5671f0eaabf49623c4ea516206 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Thu, 17 Aug 2017 13:39:05 +0200 Subject: [PATCH 082/114] Change default activation mode to 'Never' Maintains default behavior of indigo that no controller activation is required. Enabling required activation can be done by passing 'require_activation' as Always/OnStartup to the ur_common.launch or by modifying corresponding launch files. --- .../ur_modern_driver/ros/service_stopper.h | 2 +- launch/ur_common.launch | 2 +- src/ros/service_stopper.cpp | 21 +++++++++++-------- 3 files changed, 14 insertions(+), 11 deletions(-) diff --git a/include/ur_modern_driver/ros/service_stopper.h b/include/ur_modern_driver/ros/service_stopper.h index f0b2c16ef..aec39a984 100644 --- a/include/ur_modern_driver/ros/service_stopper.h +++ b/include/ur_modern_driver/ros/service_stopper.h @@ -14,8 +14,8 @@ enum class RobotState enum class ActivationMode { - Always, Never, + Always, OnStartup }; diff --git a/launch/ur_common.launch b/launch/ur_common.launch index 25aa71f24..95c3198ad 100644 --- a/launch/ur_common.launch +++ b/launch/ur_common.launch @@ -17,7 +17,7 @@ - + diff --git a/src/ros/service_stopper.cpp b/src/ros/service_stopper.cpp index 1d1a3e6ac..fcbf1a1d0 100644 --- a/src/ros/service_stopper.cpp +++ b/src/ros/service_stopper.cpp @@ -4,26 +4,29 @@ ServiceStopper::ServiceStopper(std::vector services) : enable_service_(nh_.advertiseService("ur_driver/robot_enable", &ServiceStopper::enableCallback, this)) , services_(services) , last_state_(RobotState::Error) - , activation_mode_(ActivationMode::Always) + , activation_mode_(ActivationMode::Never) { std::string mode; - ros::param::param("~require_activation", mode, std::string("Always")); - if (mode == "Never") + ros::param::param("~require_activation", mode, std::string("Never")); + if (mode == "Always") { - activation_mode_ = ActivationMode::Never; - notify_all(RobotState::Running); + activation_mode_ = ActivationMode::Always; } else if (mode == "OnStartup") { activation_mode_ = ActivationMode::OnStartup; } - else if (mode != "Always") + else { - LOG_WARN("Found invalid value for param service_stopper_mode: '%s'\nShould be one of Always, Never, OnStartup", mode.c_str()); - mode = "Always"; + if (mode != "Never") + { + LOG_WARN("Found invalid value for param require_activation: '%s'\nShould be one of Never, OnStartup, Always", mode.c_str()); + mode = "Never"; + } + notify_all(RobotState::Running); } - LOG_INFO("ActivationMode mode: %s", mode.c_str()); + LOG_INFO("Service 'ur_driver/robot_enable' activation mode: %s", mode.c_str()); } bool ServiceStopper::enableCallback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp) From 08affc5e9b52ee2ac9ec082057f367f7a25fd829 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 11 Oct 2017 20:34:19 +0200 Subject: [PATCH 083/114] set(CMAKE_CXX_FLAGS -> add_compile_options Some of the set commands overwrote user-provided values. Also add_compile_options is the much cleaner interface to add the values --- CMakeLists.txt | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index be227ba57..c510ce501 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 2.8.12) project(ur_modern_driver) @@ -127,14 +127,16 @@ include(CheckCXXCompilerFlag) CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) if(COMPILER_SUPPORTS_CXX11) - set(CMAKE_CXX_FLAGS "-std=c++11") + add_compile_options(-std=c++11) elseif(COMPILER_SUPPORTS_CXX0X) - set(CMAKE_CXX_FLAGS "-std=c++0x") + add_compile_options(-std=c++0x) else() message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler. Suggested solution: update the pkg build-essential ") endif() -set(CMAKE_CXX_FLAGS "-g -Wall -Wextra -Wno-unused-parameter ${CMAKE_CXX_FLAGS}") +add_compile_options(-Wall) +add_compile_options(-Wextra) +add_compile_options(-Wno-unused-parameter) ## Specify additional locations of header files ## Your package locations should be listed before other locations From 3bd3c017a5ead4fe8baa8894da41aa9e3b154c27 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 11 Oct 2017 20:36:06 +0200 Subject: [PATCH 084/114] cleanup inconsistent / unused dependencies --- CMakeLists.txt | 2 +- package.xml | 5 +---- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c510ce501..3275e5982 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,7 +16,7 @@ find_package(catkin REQUIRED COMPONENTS geometry_msgs roscpp sensor_msgs - std_msgs + std_srvs trajectory_msgs ur_msgs tf diff --git a/package.xml b/package.xml index f1cd9960d..e451bc36a 100644 --- a/package.xml +++ b/package.xml @@ -47,11 +47,9 @@ geometry_msgs roscpp sensor_msgs - std_msgs trajectory_msgs ur_msgs tf - realtime_tools std_srvs hardware_interface controller_manager @@ -61,14 +59,13 @@ geometry_msgs roscpp sensor_msgs - std_msgs trajectory_msgs ur_msgs ur_description tf - realtime_tools std_srvs + rosunit From 025739648685edb3d1e41f905f3a4c00d871c2d2 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 11 Oct 2017 20:36:33 +0200 Subject: [PATCH 085/114] fix typo in catkin_package ur_hardware_interface is no *dependency* of this package. It's a library *provided* by this package --- CMakeLists.txt | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3275e5982..6e2739433 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -113,9 +113,8 @@ find_package(catkin REQUIRED COMPONENTS ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include -# LIBRARIES ur_modern_driver + LIBRARIES ur_hardware_interface CATKIN_DEPENDS hardware_interface controller_manager actionlib control_msgs geometry_msgs roscpp sensor_msgs trajectory_msgs ur_msgs - DEPENDS ur_hardware_interface ) ########### From e4a503fe5f0466c77a6005d49294f17fc32cd3d2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Fri, 8 Dec 2017 14:05:07 +0100 Subject: [PATCH 086/114] various improvements and fixes for use_ros_control=true (#6) * Find matching hardware_interface using the required type The name of the controller was used in order to find and start the matching hardware interface. In consequence this meant that one could only define one controller for each hardware interface. Now, the controller's required type of hardware interface is used to find and start the matching hardware interface. * separate read & update in controller consume is defined as read+update, but update does not include read in ros_control terminology. * Handle latency in pipeline loop The controllers need to update at a rate of *at least* 125Hz, but the wait_dequeue_timed call could in theory slow the loop down to 62.5Hz. The old ur_modern_driver worked around this problem by sending goals at 4*125Hz. This patch exploits the onTimeout method of a consumer to update with the specified frequency of the control loop, even if no new state message arrived after the previous command. * publish wrench w.r.t. tcp frame The messages had an empty frame_id before and could not be displayed in RViz * support ros_control in indigo --- CMakeLists.txt | 5 ++++ include/ur_modern_driver/pipeline.h | 25 ++++++------------- include/ur_modern_driver/ros/controller.h | 20 +++++++++------ .../ur_modern_driver/ros/hardware_interface.h | 4 +-- src/ros/controller.cpp | 25 ++++++++++++++----- src/ros/hardware_interface.cpp | 14 +++++------ src/ros_main.cpp | 5 +++- 7 files changed, 58 insertions(+), 40 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6e2739433..e37a82084 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -137,6 +137,11 @@ add_compile_options(-Wall) add_compile_options(-Wextra) add_compile_options(-Wno-unused-parameter) +# support indigo's ros_control - This can be removed upon EOL indigo +if("${controller_manager_msgs_VERSION}" VERSION_LESS "0.10.0") + add_definitions(-DUR_ROS_CONTROL_INTERFACE_OLD_ROS_CONTROL) +endif() + ## Specify additional locations of header files ## Your package locations should be listed before other locations # include_directories(include) diff --git a/include/ur_modern_driver/pipeline.h b/include/ur_modern_driver/pipeline.h index e902ed3c2..ca7058a80 100644 --- a/include/ur_modern_driver/pipeline.h +++ b/include/ur_modern_driver/pipeline.h @@ -126,7 +126,7 @@ class Pipeline { if (!queue_.try_enqueue(std::move(p))) { - LOG_ERROR("Pipeline producer owerflowed!"); + LOG_ERROR("Pipeline producer overflowed!"); } } @@ -141,27 +141,18 @@ class Pipeline { consumer_.setupConsumer(); unique_ptr product; - Time last_pkg = Clock::now(); - Time last_warn = last_pkg; while (running_) { - // 16000us timeout was chosen because we should - // roughly recieve messages at 125hz which is every - // 8ms so double it for some error margin - if (!queue_.wait_dequeue_timed(product, std::chrono::milliseconds(16))) + // timeout was chosen because we should receive messages + // at roughly 125hz (every 8ms) and have to update + // the controllers (i.e. the consumer) with *at least* 125Hz + // So we update the consumer more frequently via onTimeout + if (!queue_.wait_dequeue_timed(product, std::chrono::milliseconds(8))) { - Time now = Clock::now(); - auto pkg_diff = now - last_pkg; - auto warn_diff = now - last_warn; - if (pkg_diff > std::chrono::seconds(1) && warn_diff > std::chrono::seconds(1)) - { - last_warn = now; - consumer_.onTimeout(); - } + consumer_.onTimeout(); continue; } - last_pkg = Clock::now(); if (!consumer_.consume(std::move(product))) break; } @@ -201,4 +192,4 @@ class Pipeline pThread_.join(); cThread_.join(); } -}; \ No newline at end of file +}; diff --git a/include/ur_modern_driver/ros/controller.h b/include/ur_modern_driver/ros/controller.h index d99dc0752..df5d6de5f 100644 --- a/include/ur_modern_driver/ros/controller.h +++ b/include/ur_modern_driver/ros/controller.h @@ -49,13 +49,13 @@ class ROSController : private hardware_interface::RobotHW, public URRTPacketCons } void read(RTShared& state); - bool update(RTShared& state); + bool update(); bool write(); void reset(); public: ROSController(URCommander& commander, TrajectoryFollower& follower, std::vector& joint_names, - double max_vel_change); + double max_vel_change, std::string tcp_link); virtual ~ROSController() { } @@ -66,20 +66,26 @@ class ROSController : private hardware_interface::RobotHW, public URRTPacketCons virtual void setupConsumer(); virtual bool consume(RTState_V1_6__7& state) { - return update(state); + read(state); + return update(); } virtual bool consume(RTState_V1_8& state) { - return update(state); + read(state); + return update(); } virtual bool consume(RTState_V3_0__1& state) { - return update(state); + read(state); + return update(); } virtual bool consume(RTState_V3_2__3& state) { - return update(state); + read(state); + return update(); } + virtual void onTimeout(); + virtual void onRobotStateChange(RobotState state); -}; \ No newline at end of file +}; diff --git a/include/ur_modern_driver/ros/hardware_interface.h b/include/ur_modern_driver/ros/hardware_interface.h index 50e311b15..a68a1e6d1 100644 --- a/include/ur_modern_driver/ros/hardware_interface.h +++ b/include/ur_modern_driver/ros/hardware_interface.h @@ -43,7 +43,7 @@ class WrenchInterface : public hardware_interface::ForceTorqueSensorInterface std::array tcp_; public: - WrenchInterface(); + WrenchInterface(std::string tcp_link); void update(RTShared &packet); typedef hardware_interface::ForceTorqueSensorInterface parent_type; static const std::string INTERFACE_NAME; @@ -80,4 +80,4 @@ class PositionInterface : public HardwareInterface, public hardware_interface::P typedef hardware_interface::PositionJointInterface parent_type; static const std::string INTERFACE_NAME; -}; \ No newline at end of file +}; diff --git a/src/ros/controller.cpp b/src/ros/controller.cpp index 3e8b0f084..bc844c46c 100644 --- a/src/ros/controller.cpp +++ b/src/ros/controller.cpp @@ -1,10 +1,10 @@ #include "ur_modern_driver/ros/controller.h" ROSController::ROSController(URCommander& commander, TrajectoryFollower& follower, - std::vector& joint_names, double max_vel_change) + std::vector& joint_names, double max_vel_change, std::string tcp_link) : controller_(this, nh_) , joint_interface_(joint_names) - , wrench_interface_() + , wrench_interface_(tcp_link) , position_interface_(follower, joint_interface_, joint_names) , velocity_interface_(commander, joint_interface_, joint_names, max_vel_change) { @@ -33,7 +33,16 @@ void ROSController::doSwitch(const std::list for (auto const& ci : start_list) { - auto ait = available_interfaces_.find(ci.name); + std::string requested_interface(""); + +#if defined(UR_ROS_CONTROL_INTERFACE_OLD_ROS_CONTROL) + requested_interface = ci.hardware_interface; +#else + if (!ci.claimed_resources.empty()) + requested_interface = ci.claimed_resources[0].hardware_interface; +#endif + + auto ait = available_interfaces_.find(requested_interface); if (ait == available_interfaces_.end()) continue; @@ -74,13 +83,12 @@ void ROSController::read(RTShared& packet) wrench_interface_.update(packet); } -bool ROSController::update(RTShared& state) +bool ROSController::update() { auto time = ros::Time::now(); auto diff = time - lastUpdate_; lastUpdate_ = time; - read(state); controller_.update(time, diff, !service_enabled_); // emergency stop and such should not kill the pipeline @@ -101,6 +109,11 @@ bool ROSController::update(RTShared& state) return write(); } +void ROSController::onTimeout() +{ + update(); +} + void ROSController::onRobotStateChange(RobotState state) { bool next = (state == RobotState::Running); @@ -109,4 +122,4 @@ void ROSController::onRobotStateChange(RobotState state) service_enabled_ = next; service_cooldown_ = 125; -} \ No newline at end of file +} diff --git a/src/ros/hardware_interface.cpp b/src/ros/hardware_interface.cpp index dc1ce9bf9..759eca583 100644 --- a/src/ros/hardware_interface.cpp +++ b/src/ros/hardware_interface.cpp @@ -1,7 +1,7 @@ #include "ur_modern_driver/ros/hardware_interface.h" #include "ur_modern_driver/log.h" -const std::string JointInterface::INTERFACE_NAME = "joint_state_controller"; +const std::string JointInterface::INTERFACE_NAME = "hardware_interface::JointStateInterface"; JointInterface::JointInterface(std::vector &joint_names) { for (size_t i = 0; i < 6; i++) @@ -18,10 +18,10 @@ void JointInterface::update(RTShared &packet) } -const std::string WrenchInterface::INTERFACE_NAME = "force_torque_sensor_controller"; -WrenchInterface::WrenchInterface() +const std::string WrenchInterface::INTERFACE_NAME = "hardware_interface::ForceTorqueSensorInterface"; +WrenchInterface::WrenchInterface(std::string tcp_link) { - registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", "", tcp_.begin(), tcp_.begin() + 3)); + registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", tcp_link, tcp_.begin(), tcp_.begin() + 3)); } void WrenchInterface::update(RTShared &packet) @@ -30,7 +30,7 @@ void WrenchInterface::update(RTShared &packet) } -const std::string VelocityInterface::INTERFACE_NAME = "vel_based_pos_traj_controller"; +const std::string VelocityInterface::INTERFACE_NAME = "hardware_interface::VelocityJointInterface"; VelocityInterface::VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names, double max_vel_change) : commander_(commander), max_vel_change_(max_vel_change) @@ -62,7 +62,7 @@ void VelocityInterface::reset() } } -const std::string PositionInterface::INTERFACE_NAME = "pos_based_pos_traj_controller"; +const std::string PositionInterface::INTERFACE_NAME = "hardware_interface::PositionJointInterface"; PositionInterface::PositionInterface(TrajectoryFollower &follower, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names) @@ -87,4 +87,4 @@ void PositionInterface::start() void PositionInterface::stop() { follower_.stop(); -} \ No newline at end of file +} diff --git a/src/ros_main.cpp b/src/ros_main.cpp index 8d74e3e84..081810886 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -28,6 +28,7 @@ static const std::string MAX_VEL_CHANGE_ARG("~max_vel_change"); 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"); +static const std::string TCP_LINK_ARG("~tcp_link"); static const std::string JOINT_NAMES_PARAM("hardware_interface/joints"); static const std::vector DEFAULT_JOINTS = { "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", @@ -43,6 +44,7 @@ struct ProgArgs std::string prefix; std::string base_frame; std::string tool_frame; + std::string tcp_link; std::vector joint_names; double max_acceleration; double max_velocity; @@ -65,6 +67,7 @@ bool parse_args(ProgArgs &args) ros::param::param(PREFIX_ARG, args.prefix, std::string()); ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link"); ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller"); + ros::param::param(TCP_LINK_ARG, args.tcp_link, args.prefix + "ee_link"); ros::param::param(JOINT_NAMES_PARAM, args.joint_names, DEFAULT_JOINTS); return true; } @@ -109,7 +112,7 @@ int main(int argc, char **argv) if (args.use_ros_control) { LOG_INFO("ROS control enabled"); - controller = new ROSController(*rt_commander, traj_follower, args.joint_names, args.max_vel_change); + controller = new ROSController(*rt_commander, traj_follower, args.joint_names, args.max_vel_change, args.tcp_link); rt_vec.push_back(controller); services.push_back(controller); } From 6dc4a98b9d180404c6c780bf3fd95b2ca050b34b Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Fri, 8 Dec 2017 14:20:59 +0100 Subject: [PATCH 087/114] Enable kinetic builds for travis --- .travis.yml | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/.travis.yml b/.travis.yml index a8a72aa4a..6ca2043b0 100644 --- a/.travis.yml +++ b/.travis.yml @@ -11,11 +11,8 @@ env: matrix: - ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true - ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true -# Disable jade and kinetic for now because ur_description is not released for them -# - ROS_DISTRO="jade" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true -# - ROS_DISTRO="jade" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true -# - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu NOT_TEST_INSTALL=true -# - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu NOT_TEST_INSTALL=true + - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu NOT_TEST_INSTALL=true + - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu NOT_TEST_INSTALL=true install: - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config script: From f93c9bcd65da19a64fc313d2f209ff05fd521dde Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Fri, 8 Dec 2017 14:48:09 +0100 Subject: [PATCH 088/114] Merged README.md from upstream --- README.md | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/README.md b/README.md index 8b355ea1e..3dabb0773 100644 --- a/README.md +++ b/README.md @@ -39,6 +39,8 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design ## Installation +**As the driver communicates with the robot via ethernet and depends on reliable continous communication, it is not possible to reliably control a UR from a virtual machine.** + Just clone the repository into your catkin working directory and make it with ```catkin_make```. Note that this package depends on ur_msgs, hardware_interface, and controller_manager so it cannot directly be used with ROS versions prior to hydro. @@ -49,6 +51,21 @@ The driver is designed to be a drop-in replacement of the ur\_driver package. It If you want to test it in your current setup, just use the modified launch files included in this package instead of those in ur\_bringup. Everything else should work as usual. +If you would like to run this package to connect to the hardware, you only need to run the following launch file. +``` +roslaunch ur_modern_driver urXX_bringup.launch robot_ip:=ROBOT_IP_ADDRESS +``` + +Where ROBOT_IP_ADDRESS is your UR arm's IP and XX is '5' or '10' depending on your robot. The above launch file makes calls to both roscore and the launch file to the urXX_description so that ROS's parameter server has information on your robot arm. If you do not have your ```ur_description``` installed please do so via: +``` +sudo apt install ros--ur-description +``` + +Where is the ROS distribution your machine is running on. You may want to run MoveIt to plan and execute actions on the arm. You can do so by simply entering the following commands after launching ```ur_modern_driver```: +``` +roslaunch urXX_moveit_config ur5_moveit_planning_executing.launch +roslaunch urXX_moveit_config moveit_rviz.launch config:=true +``` --- If you would like to use the ros\_control-based approach, use the launch files urXX\_ros\_control.launch, where XX is '5' or '10' depending on your robot. @@ -143,3 +160,4 @@ Please cite the following report if using this driver The report can be downloaded from http://orbit.dtu.dk/en/publications/optimizing-the-universal-robots-ros-driver(20dde139-7e87-4552-8658-dbf2cdaab24b).html + From ccba7d593b047085b6049c69a3075bbdd6e43a02 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Fri, 8 Dec 2017 14:55:05 +0100 Subject: [PATCH 089/114] Improved README.md --- README.md | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 3dabb0773..129db512f 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,5 @@ -# ur_modern_driver +# ur_modern_driver - Refactored +[![Build Status](https://travis-ci.org/Zagitta/ur_modern_driver.svg?branch=master)](https://travis-ci.org/Zagitta/ur_modern_driver) A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is designed to replace the old driver transparently, while solving some issues, improving usability as well as enabling compatibility of ros_control. @@ -21,7 +22,7 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design * Besides this, the driver subscribes to two new topics: - * */ur\_driver/URScript* : Takes messages of type _std\_msgs/String_ and directly forwards it to the robot. Note that no control is done on the input, so use at your own risk! Inteded for sending movel/movej commands directly to the robot, conveyor tracking and the like. + * ~~*/ur\_driver/URScript* : Takes messages of type _std\_msgs/String_ and directly forwards it to the robot. Note that no control is done on the input, so use at your own risk! Inteded for sending movel/movej commands directly to the robot, conveyor tracking and the like.~~ * */joint\_speed* : Takes messages of type _trajectory\_msgs/JointTrajectory_. Parses the first JointTracetoryPoint and sends the specified joint speeds and accelerations to the robot. This interface is intended for doing visual servoing and other kind of control that requires speed control rather than position control of the robot. Remember to set values for all 6 joints. Ignores the field joint\_names, so set the values in the correct order. @@ -35,7 +36,8 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design * As ros_control continuesly controls the robot, using the teach pendant while a controller is running will cause the controller **on the robot** to crash, as it obviously can't handle conflicting control input from two sources. Thus be sure to stop the running controller **before** moving the robot via the teach pendant: * A list of the loaded and running controllers can be found by a call to the controller_manager ```rosservice call /controller_manager/list_controllers {} ``` * The running position trajectory controller can be stopped with a call to ```rosservice call /universal_robot/controller_manager/switch_controller "start_controllers: - '' stop_controllers: - 'pos_based_pos_traj_controller' strictness: 1" ``` (Remember you can use tab-completion for this) - + +* Added activation modes: `Never`, `Always` and `OnStartup`. Sets wether a call to the `ur_driver/robot_enable` service is required at startup, at startup + on errors or never. Is intended as a safety feature to require some sort of manual intervention in case of driver crashes and robot safety faults. ## Installation From 560affaa78d1a49f8343014cac30ea9ea17b143f Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Fri, 8 Dec 2017 14:57:14 +0100 Subject: [PATCH 090/114] Fixed default tcp_link value --- src/ros_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ros_main.cpp b/src/ros_main.cpp index 081810886..9d037d587 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -67,7 +67,7 @@ bool parse_args(ProgArgs &args) ros::param::param(PREFIX_ARG, args.prefix, std::string()); ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link"); ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller"); - ros::param::param(TCP_LINK_ARG, args.tcp_link, args.prefix + "ee_link"); + ros::param::param(TCP_LINK_ARG, args.tcp_link, args.prefix + "tool0"); ros::param::param(JOINT_NAMES_PARAM, args.joint_names, DEFAULT_JOINTS); return true; } From 24eef75d726924687e606e672fbfc3f5d741a4c8 Mon Sep 17 00:00:00 2001 From: Simon Schmeisser Date: Tue, 2 Jan 2018 20:16:41 +0100 Subject: [PATCH 091/114] Publish industrial_msgs::RobotStatus (#5) --- CMakeLists.txt | 1 + include/ur_modern_driver/ros/mb_publisher.h | 8 ++- include/ur_modern_driver/ros/rt_publisher.h | 2 +- package.xml | 2 + src/ros/mb_publisher.cpp | 62 ++++++++++++++++++++- src/ros/rt_publisher.cpp | 2 +- 6 files changed, 73 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e37a82084..1691e72b0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(catkin REQUIRED COMPONENTS actionlib control_msgs geometry_msgs + industrial_msgs roscpp sensor_msgs std_srvs diff --git a/include/ur_modern_driver/ros/mb_publisher.h b/include/ur_modern_driver/ros/mb_publisher.h index 692ecfd6c..f4b154964 100644 --- a/include/ur_modern_driver/ros/mb_publisher.h +++ b/include/ur_modern_driver/ros/mb_publisher.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -14,6 +15,7 @@ class MBPublisher : public URStatePacketConsumer private: NodeHandle nh_; Publisher io_pub_; + Publisher status_pub_; template inline void appendDigital(std::vector& vec, std::bitset bits) @@ -28,9 +30,13 @@ class MBPublisher : public URStatePacketConsumer } void publish(ur_msgs::IOStates& io_msg, SharedMasterBoardData& data); + void publishRobotStatus(industrial_msgs::RobotStatus& status, const SharedRobotModeData& data) const; + void publishRobotStatus(const RobotModeData_V1_X& data) const; + void publishRobotStatus(const RobotModeData_V3_0__1& data) const; public: MBPublisher() : io_pub_(nh_.advertise("ur_driver/io_states", 1)) + , status_pub_(nh_.advertise("ur_driver/robot_status", 1)) { } @@ -51,4 +57,4 @@ class MBPublisher : public URStatePacketConsumer virtual void stopConsumer() { } -}; \ No newline at end of file +}; diff --git a/include/ur_modern_driver/ros/rt_publisher.h b/include/ur_modern_driver/ros/rt_publisher.h index f4945d9ab..ce9b320a5 100644 --- a/include/ur_modern_driver/ros/rt_publisher.h +++ b/include/ur_modern_driver/ros/rt_publisher.h @@ -71,4 +71,4 @@ class RTPublisher : public URRTPacketConsumer virtual void stopConsumer() { } -}; \ No newline at end of file +}; diff --git a/package.xml b/package.xml index e451bc36a..7e2ac0c59 100644 --- a/package.xml +++ b/package.xml @@ -45,6 +45,7 @@ actionlib control_msgs geometry_msgs + industrial_msgs roscpp sensor_msgs trajectory_msgs @@ -57,6 +58,7 @@ actionlib control_msgs geometry_msgs + industrial_msgs roscpp sensor_msgs trajectory_msgs diff --git a/src/ros/mb_publisher.cpp b/src/ros/mb_publisher.cpp index c0fdc232d..9d9832f0f 100644 --- a/src/ros/mb_publisher.cpp +++ b/src/ros/mb_publisher.cpp @@ -18,6 +18,63 @@ void MBPublisher::publish(ur_msgs::IOStates& io_msg, SharedMasterBoardData& data io_pub_.publish(io_msg); } +void MBPublisher::publishRobotStatus(industrial_msgs::RobotStatus& status, const SharedRobotModeData& data) const +{ + //note that this is true as soon as the drives are powered, + //even if the breakes are still closed + //which is in slight contrast to the comments in the + //message definition + status.drives_powered.val = data.robot_power_on; + + status.e_stopped.val = data.emergency_stopped; + + //I found no way to reliably get information if the robot is moving + //data.programm_running would be true when using this driver to move the robot + //but it would also be true when another programm is running that might not + //in fact contain any movement commands + status.in_motion.val = industrial_msgs::TriState::UNKNOWN; + + //the error code, if any, is not transmitted by this protocol + //it can and should be fetched seperately + status.error_code = 0; + + //note that e-stop is handled by a seperate variable + status.in_error.val = data.protective_stopped; + + status_pub_.publish(status); +} + +void MBPublisher::publishRobotStatus(const RobotModeData_V1_X& data) const +{ + industrial_msgs::RobotStatus msg; + + if (data.robot_mode == robot_mode_V1_X::ROBOT_FREEDRIVE_MODE) + msg.mode.val = industrial_msgs::RobotMode::MANUAL; + else + msg.mode.val = industrial_msgs::RobotMode::AUTO; + + //todo: verify that this correct, there is also ROBOT_READY_MODE + msg.motion_possible.val = (data.robot_mode == robot_mode_V1_X::ROBOT_RUNNING_MODE) + ? industrial_msgs::TriState::ON : industrial_msgs::TriState::OFF; + + publishRobotStatus(msg, data); +} + +void MBPublisher::publishRobotStatus(const RobotModeData_V3_0__1& data) const +{ + industrial_msgs::RobotStatus msg; + + msg.motion_possible.val = (data.robot_mode == robot_mode_V3_X::RUNNING) + ? industrial_msgs::TriState::ON : industrial_msgs::TriState::OFF; + + if (data.control_mode == robot_control_mode_V3_X::TEACH) + msg.mode.val = industrial_msgs::RobotMode::MANUAL; + else + msg.mode.val = industrial_msgs::RobotMode::AUTO; + + publishRobotStatus(msg, data); +} + bool MBPublisher::consume(MasterBoardData_V1_X& data) { ur_msgs::IOStates io_msg; @@ -42,13 +99,16 @@ bool MBPublisher::consume(MasterBoardData_V3_2& data) bool MBPublisher::consume(RobotModeData_V1_X& data) { + publishRobotStatus(data); return true; } bool MBPublisher::consume(RobotModeData_V3_0__1& data) { + publishRobotStatus(data); return true; } bool MBPublisher::consume(RobotModeData_V3_2& data) { + publishRobotStatus(data); return true; -} \ No newline at end of file +} diff --git a/src/ros/rt_publisher.cpp b/src/ros/rt_publisher.cpp index 65d07266e..0709f5067 100644 --- a/src/ros/rt_publisher.cpp +++ b/src/ros/rt_publisher.cpp @@ -116,4 +116,4 @@ bool RTPublisher::consume(RTState_V3_0__1& state) bool RTPublisher::consume(RTState_V3_2__3& state) { return publish(state); -} \ No newline at end of file +} From 6950b3c4bd22d4cf1d6c0dd167a86b0f6e34ab21 Mon Sep 17 00:00:00 2001 From: Jarek Potiuk Date: Tue, 2 Jan 2018 20:22:55 +0100 Subject: [PATCH 092/114] Re-add urscript topic (#7) * Re-added UR script - for custom UR Script execution * Restarting the driver when robot closes the connection on script error. The pipelines work in the way that if the connection is is closed by the control PC, it will not be re-established. This happens for example if you use the URScript topic and upload script that does not compile. The robot will then close the connection, the pipeline will close and any subsequent uploads will fail and noone realises there is a problem. While we could re-establish the connection, I think much better solution is to shutdown the driver in such case. This is much more resilient behaviour as it will clean up any inconsistent driver state. We can utilise "respawn" feature of ROS launch and restart such driver automatically (launch files are updated as part of that change). On top of "production" stability, it allows for much nicer development workflow - you can use URScript topic for development of new scripts and have the driver restart every time you make mistake. Without it, any mistake requires restarting the driver manually. --- CMakeLists.txt | 7 +-- README.md | 2 +- include/ur_modern_driver/pipeline.h | 32 ++++++++++--- .../ur_modern_driver/ros/urscript_handler.h | 24 ++++++++++ include/ur_modern_driver/ur/commander.h | 6 +-- launch/ur10_bringup.launch | 2 + launch/ur10_bringup_compatible.launch | 2 + launch/ur10_bringup_joint_limited.launch | 2 + launch/ur10_ros_control.launch | 2 + launch/ur3_bringup.launch | 2 + launch/ur3_bringup_joint_limited.launch | 2 + launch/ur3_ros_control.launch | 2 + launch/ur5_bringup.launch | 2 + launch/ur5_bringup_compatible.launch | 2 + launch/ur5_bringup_joint_limited.launch | 8 ++-- launch/ur5_ros_control.launch | 2 + launch/ur_common.launch | 4 +- src/ros/urscript_handler.cpp | 42 +++++++++++++++++ src/ros_main.cpp | 47 ++++++++++++++++++- src/ur/commander.cpp | 5 +- 20 files changed, 176 insertions(+), 21 deletions(-) create mode 100644 include/ur_modern_driver/ros/urscript_handler.h create mode 100644 src/ros/urscript_handler.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 1691e72b0..22cf821ea 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -146,15 +146,15 @@ endif() ## Specify additional locations of header files ## Your package locations should be listed before other locations # include_directories(include) -include_directories(include +include_directories(include ${catkin_INCLUDE_DIRS} ) ## Declare a C++ library # Hardware Interface -add_library(ur_hardware_interface - src/ros/hardware_interface.cpp +add_library(ur_hardware_interface + src/ros/hardware_interface.cpp src/ros/controller.cpp) target_link_libraries(ur_hardware_interface ${catkin_LIBRARIES} @@ -172,6 +172,7 @@ set(${PROJECT_NAME}_SOURCES src/ros/rt_publisher.cpp src/ros/service_stopper.cpp src/ros/trajectory_follower.cpp + src/ros/urscript_handler.cpp src/ur/stream.cpp src/ur/server.cpp src/ur/commander.cpp diff --git a/README.md b/README.md index 129db512f..51427a0ed 100644 --- a/README.md +++ b/README.md @@ -22,7 +22,7 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design * Besides this, the driver subscribes to two new topics: - * ~~*/ur\_driver/URScript* : Takes messages of type _std\_msgs/String_ and directly forwards it to the robot. Note that no control is done on the input, so use at your own risk! Inteded for sending movel/movej commands directly to the robot, conveyor tracking and the like.~~ + * */ur\_driver/URScript* : Takes messages of type _std\_msgs/String_ and directly forwards it to the robot. Note that no control is done on the input, so use at your own risk! Inteded for sending movel/movej commands directly to the robot, conveyor tracking and the like. * */joint\_speed* : Takes messages of type _trajectory\_msgs/JointTrajectory_. Parses the first JointTracetoryPoint and sends the specified joint speeds and accelerations to the robot. This interface is intended for doing visual servoing and other kind of control that requires speed control rather than position control of the robot. Remember to set values for all 6 joints. Ignores the field joint\_names, so set the values in the correct order. diff --git a/include/ur_modern_driver/pipeline.h b/include/ur_modern_driver/pipeline.h index ca7058a80..ba710096f 100644 --- a/include/ur_modern_driver/pipeline.h +++ b/include/ur_modern_driver/pipeline.h @@ -99,6 +99,18 @@ class IProducer virtual bool tryGet(std::vector>& products) = 0; }; +class INotifier +{ +public: + virtual void started(std::string name) + { + } + virtual void stopped(std::string name) + { + } +}; + + template class Pipeline { @@ -107,6 +119,8 @@ class Pipeline typedef Clock::time_point Time; IProducer& producer_; IConsumer& consumer_; + std::string name_; + INotifier& notifier_; BlockingReaderWriterQueue> queue_; atomic running_; thread pThread_, cThread_; @@ -126,15 +140,17 @@ class Pipeline { if (!queue_.try_enqueue(std::move(p))) { - LOG_ERROR("Pipeline producer overflowed!"); + LOG_ERROR("Pipeline producer overflowed! <%s>",name_.c_str()); } } products.clear(); } producer_.teardownProducer(); - LOG_DEBUG("Pipline producer ended"); + LOG_DEBUG("Pipeline producer ended! <%s>", name_.c_str()); consumer_.stopConsumer(); + running_ = false; + notifier_.stopped(name_); } void run_consumer() @@ -157,13 +173,15 @@ class Pipeline break; } consumer_.teardownConsumer(); - LOG_DEBUG("Pipline consumer ended"); + LOG_DEBUG("Pipeline consumer ended! <%s>", name_.c_str()); producer_.stopProducer(); + running_ = false; + notifier_.stopped(name_); } public: - Pipeline(IProducer& producer, IConsumer& consumer) - : producer_(producer), consumer_(consumer), queue_{ 32 }, running_{ false } + Pipeline(IProducer& producer, IConsumer& consumer, std::string name, INotifier& notifier) + : producer_(producer), consumer_(consumer), name_(name), notifier_(notifier), queue_{ 32 }, running_{ false } { } @@ -175,6 +193,7 @@ class Pipeline running_ = true; pThread_ = thread(&Pipeline::run_producer, this); cThread_ = thread(&Pipeline::run_consumer, this); + notifier_.started(name_); } void stop() @@ -182,7 +201,7 @@ class Pipeline if (!running_) return; - LOG_DEBUG("Stopping pipeline"); + LOG_DEBUG("Stopping pipeline! <%s>",name_.c_str()); consumer_.stopConsumer(); producer_.stopProducer(); @@ -191,5 +210,6 @@ class Pipeline pThread_.join(); cThread_.join(); + notifier_.stopped(name_); } }; diff --git a/include/ur_modern_driver/ros/urscript_handler.h b/include/ur_modern_driver/ros/urscript_handler.h new file mode 100644 index 000000000..f5190db3e --- /dev/null +++ b/include/ur_modern_driver/ros/urscript_handler.h @@ -0,0 +1,24 @@ +#pragma once + +#include +#include + +#include "std_msgs/String.h" +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/ros/service_stopper.h" +#include "ur_modern_driver/ur/commander.h" + +class URScriptHandler: public Service +{ +private: + + ros::NodeHandle nh_; + URCommander &commander_; + ros::Subscriber urscript_sub_; + RobotState state_; + +public: + URScriptHandler(URCommander &commander); + void urscriptInterface(const std_msgs::String::ConstPtr& msg); + void onRobotStateChange(RobotState state); +}; diff --git a/include/ur_modern_driver/ur/commander.h b/include/ur_modern_driver/ur/commander.h index 8fdffac1d..6a2b48620 100644 --- a/include/ur_modern_driver/ur/commander.h +++ b/include/ur_modern_driver/ur/commander.h @@ -10,7 +10,7 @@ class URCommander URStream &stream_; protected: - bool write(std::string &s); + bool write(const std::string &s); void formatArray(std::ostringstream &out, std::array &values); public: @@ -23,7 +23,7 @@ class URCommander virtual bool setAnalogOut(uint8_t pin, double value) = 0; // shared - bool uploadProg(std::string &s); + bool uploadProg(const std::string &s); bool stopj(double a = 10.0); bool setToolVoltage(uint8_t voltage); bool setFlag(uint8_t pin, bool value); @@ -52,4 +52,4 @@ class URCommander_V3_X : public URCommander virtual bool speedj(std::array &speeds, double acceleration); virtual bool setDigitalOut(uint8_t pin, bool value); virtual bool setAnalogOut(uint8_t pin, double value); -}; \ No newline at end of file +}; diff --git a/launch/ur10_bringup.launch b/launch/ur10_bringup.launch index 58f7abd59..f27efb36c 100644 --- a/launch/ur10_bringup.launch +++ b/launch/ur10_bringup.launch @@ -14,6 +14,7 @@ + @@ -24,6 +25,7 @@ + diff --git a/launch/ur10_bringup_compatible.launch b/launch/ur10_bringup_compatible.launch index 436cb5b76..44387a43f 100644 --- a/launch/ur10_bringup_compatible.launch +++ b/launch/ur10_bringup_compatible.launch @@ -13,6 +13,7 @@ + @@ -25,6 +26,7 @@ + diff --git a/launch/ur10_bringup_joint_limited.launch b/launch/ur10_bringup_joint_limited.launch index 6cc6ca654..1ca2d8b5f 100644 --- a/launch/ur10_bringup_joint_limited.launch +++ b/launch/ur10_bringup_joint_limited.launch @@ -13,6 +13,7 @@ + @@ -20,5 +21,6 @@ + diff --git a/launch/ur10_ros_control.launch b/launch/ur10_ros_control.launch index a63820f62..6eb9dd45e 100644 --- a/launch/ur10_ros_control.launch +++ b/launch/ur10_ros_control.launch @@ -14,6 +14,7 @@ + @@ -31,6 +32,7 @@ + diff --git a/launch/ur3_bringup.launch b/launch/ur3_bringup.launch index 9e7986d4c..f4bc006b0 100644 --- a/launch/ur3_bringup.launch +++ b/launch/ur3_bringup.launch @@ -14,6 +14,7 @@ + @@ -25,6 +26,7 @@ + diff --git a/launch/ur3_bringup_joint_limited.launch b/launch/ur3_bringup_joint_limited.launch index e2c657ee9..9c70c524f 100644 --- a/launch/ur3_bringup_joint_limited.launch +++ b/launch/ur3_bringup_joint_limited.launch @@ -13,6 +13,7 @@ + @@ -20,5 +21,6 @@ + diff --git a/launch/ur3_ros_control.launch b/launch/ur3_ros_control.launch index 6e88e9fb2..2da412354 100644 --- a/launch/ur3_ros_control.launch +++ b/launch/ur3_ros_control.launch @@ -14,6 +14,7 @@ + @@ -31,6 +32,7 @@ + diff --git a/launch/ur5_bringup.launch b/launch/ur5_bringup.launch index eeaa31897..303d253ab 100644 --- a/launch/ur5_bringup.launch +++ b/launch/ur5_bringup.launch @@ -14,6 +14,7 @@ + @@ -25,6 +26,7 @@ + diff --git a/launch/ur5_bringup_compatible.launch b/launch/ur5_bringup_compatible.launch index 31f7b0b69..c8383bff2 100644 --- a/launch/ur5_bringup_compatible.launch +++ b/launch/ur5_bringup_compatible.launch @@ -13,6 +13,7 @@ + @@ -25,6 +26,7 @@ + diff --git a/launch/ur5_bringup_joint_limited.launch b/launch/ur5_bringup_joint_limited.launch index 643f5c487..3640a2d7f 100644 --- a/launch/ur5_bringup_joint_limited.launch +++ b/launch/ur5_bringup_joint_limited.launch @@ -2,23 +2,25 @@ - + - + + + diff --git a/launch/ur5_ros_control.launch b/launch/ur5_ros_control.launch index c240b7b38..ccf5d7924 100644 --- a/launch/ur5_ros_control.launch +++ b/launch/ur5_ros_control.launch @@ -14,6 +14,7 @@ + @@ -31,6 +32,7 @@ + diff --git a/launch/ur_common.launch b/launch/ur_common.launch index 95c3198ad..aad4c6d29 100644 --- a/launch/ur_common.launch +++ b/launch/ur_common.launch @@ -15,6 +15,7 @@ + @@ -25,7 +26,7 @@ - + @@ -36,5 +37,6 @@ + diff --git a/src/ros/urscript_handler.cpp b/src/ros/urscript_handler.cpp new file mode 100644 index 000000000..c2e5fe7eb --- /dev/null +++ b/src/ros/urscript_handler.cpp @@ -0,0 +1,42 @@ +#include "ur_modern_driver/ros/urscript_handler.h" +#include "ur_modern_driver/log.h" + +URScriptHandler::URScriptHandler(URCommander &commander) + : commander_(commander) + , state_(RobotState::Error) +{ + LOG_INFO("Initializing ur_driver/URScript subscriber"); + urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, &URScriptHandler::urscriptInterface, this); + LOG_INFO("The ur_driver/URScript initialized"); +} + +void URScriptHandler::urscriptInterface(const std_msgs::String::ConstPtr& msg) +{ + LOG_INFO("Message received"); + switch (state_) + { + case RobotState::Running: + if (!commander_.uploadProg(msg->data)) + { + LOG_ERROR("Program upload failed!"); + } + break; + case RobotState::EmergencyStopped: + LOG_ERROR("Robot is emergency stopped"); + break; + case RobotState::ProtectiveStopped: + LOG_ERROR("Robot is protective stopped"); + break; + case RobotState::Error: + LOG_ERROR("Robot is not ready, check robot_mode"); + break; + default: + LOG_ERROR("Robot is in undefined state"); + break; + } +} + +void URScriptHandler::onRobotStateChange(RobotState state) +{ + state_ = state; +} diff --git a/src/ros_main.cpp b/src/ros_main.cpp index 9d037d587..123ecc427 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -13,6 +13,7 @@ #include "ur_modern_driver/ros/rt_publisher.h" #include "ur_modern_driver/ros/service_stopper.h" #include "ur_modern_driver/ros/trajectory_follower.h" +#include "ur_modern_driver/ros/urscript_handler.h" #include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/factory.h" #include "ur_modern_driver/ur/messages.h" @@ -30,6 +31,7 @@ static const std::string BASE_FRAME_ARG("~base_frame"); static const std::string TOOL_FRAME_ARG("~tool_frame"); static const std::string TCP_LINK_ARG("~tcp_link"); static const std::string JOINT_NAMES_PARAM("hardware_interface/joints"); +static const std::string SHUTDOWN_ON_DISCONNECT_ARG("~shutdown_on_disconnect"); static const std::vector DEFAULT_JOINTS = { "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" }; @@ -51,8 +53,34 @@ struct ProgArgs double max_vel_change; int32_t reverse_port; bool use_ros_control; + bool shutdown_on_disconnect; }; +class IgnorePipelineStoppedNotifier : public INotifier +{ +public: + void started(std::string name){ + LOG_INFO("Starting pipeline %s", name.c_str()); + } + void stopped(std::string name){ + LOG_INFO("Stopping pipeline %s", name.c_str()); + } +}; + +class ShutdownOnPipelineStoppedNotifier : public INotifier +{ +public: + void started(std::string name){ + LOG_INFO("Starting pipeline %s", name.c_str()); + } + void stopped(std::string name){ + LOG_INFO("Shutting down on stopped pipeline %s", name.c_str()); + ros::shutdown(); + exit(1); + } +}; + + bool parse_args(ProgArgs &args) { if (!ros::param::get(IP_ADDR_ARG, args.host)) @@ -69,6 +97,7 @@ bool parse_args(ProgArgs &args) ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller"); ros::param::param(TCP_LINK_ARG, args.tcp_link, args.prefix + "tool0"); ros::param::param(JOINT_NAMES_PARAM, args.joint_names, DEFAULT_JOINTS); + ros::param::param(SHUTDOWN_ON_DISCONNECT_ARG, args.shutdown_on_disconnect, true); return true; } @@ -107,6 +136,7 @@ int main(int argc, char **argv) TrajectoryFollower traj_follower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3()); + INotifier *notifier(nullptr); ROSController *controller(nullptr); ActionServer *action_server(nullptr); if (args.use_ros_control) @@ -124,8 +154,21 @@ int main(int argc, char **argv) services.push_back(action_server); } + URScriptHandler urscript_handler(*rt_commander); + services.push_back(&urscript_handler); + if (args.shutdown_on_disconnect) + { + LOG_INFO("Notifier: Pipeline disconnect will shutdown the node"); + notifier = new ShutdownOnPipelineStoppedNotifier(); + } + else + { + LOG_INFO("Notifier: Pipeline disconnect will be ignored."); + notifier = new IgnorePipelineStoppedNotifier(); + } + MultiConsumer rt_cons(rt_vec); - Pipeline rt_pl(rt_prod, rt_cons); + Pipeline rt_pl(rt_prod, rt_cons, "RTPacket", *notifier); // Message packets auto state_parser = factory.getStateParser(); @@ -137,7 +180,7 @@ int main(int argc, char **argv) vector *> state_vec{ &state_pub, &service_stopper }; MultiConsumer state_cons(state_vec); - Pipeline state_pl(state_prod, state_cons); + Pipeline state_pl(state_prod, state_cons, "StatePacket", *notifier); LOG_INFO("Starting main loop"); diff --git a/src/ur/commander.cpp b/src/ur/commander.cpp index 96fde7d4c..5c752f07f 100644 --- a/src/ur/commander.cpp +++ b/src/ur/commander.cpp @@ -1,7 +1,7 @@ #include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/log.h" -bool URCommander::write(std::string &s) +bool URCommander::write(const std::string &s) { size_t len = s.size(); const uint8_t *data = reinterpret_cast(s.c_str()); @@ -20,8 +20,9 @@ void URCommander::formatArray(std::ostringstream &out, std::array &va out << "]"; } -bool URCommander::uploadProg(std::string &s) +bool URCommander::uploadProg(const std::string &s) { + LOG_DEBUG("Sending program [%s]",s.c_str()); return write(s); } From bf857557447d106a750a65e5f23892744faac9ce Mon Sep 17 00:00:00 2001 From: "Simon Schmeisser (isys vision)" Date: Mon, 8 Jan 2018 09:14:54 +0100 Subject: [PATCH 093/114] Add support for version 3.5 A new undocumented uchar was added to RobotMode --- include/ur_modern_driver/ur/factory.h | 4 +++- include/ur_modern_driver/ur/robot_mode.h | 15 ++++++++++++++- include/ur_modern_driver/ur/state_parser.h | 1 + src/ur/robot_mode.cpp | 18 +++++++++++++++++- 4 files changed, 35 insertions(+), 3 deletions(-) diff --git a/include/ur_modern_driver/ur/factory.h b/include/ur_modern_driver/ur/factory.h index ecf3bed6b..54c6cdfda 100644 --- a/include/ur_modern_driver/ur/factory.h +++ b/include/ur_modern_driver/ur/factory.h @@ -94,8 +94,10 @@ class URFactory : private URMessagePacketConsumer { if (minor_version_ < 3) return std::unique_ptr>(new URStateParser_V3_0__1); - else + else if (minor_version_ < 5) return std::unique_ptr>(new URStateParser_V3_2); + else + return std::unique_ptr>(new URStateParser_V3_5); } } diff --git a/include/ur_modern_driver/ur/robot_mode.h b/include/ur_modern_driver/ur/robot_mode.h index 80aa145c8..c6a1cfce1 100644 --- a/include/ur_modern_driver/ur/robot_mode.h +++ b/include/ur_modern_driver/ur/robot_mode.h @@ -102,4 +102,17 @@ class RobotModeData_V3_2 : public RobotModeData_V3_0__1 static const size_t SIZE = RobotModeData_V3_0__1::SIZE + sizeof(double); static_assert(RobotModeData_V3_2::SIZE == 41, "RobotModeData_V3_2 has missmatched size"); -}; \ No newline at end of file +}; + +class RobotModeData_V3_5 : public RobotModeData_V3_2 +{ +public: + virtual bool parseWith(BinParser& bp); + virtual bool consumeWith(URStatePacketConsumer& consumer); + + unsigned char unknown_internal_use; + + static const size_t SIZE = RobotModeData_V3_2::SIZE + sizeof(unsigned char); + + static_assert(RobotModeData_V3_5::SIZE == 42, "RobotModeData_V3_5 has missmatched size"); +}; diff --git a/include/ur_modern_driver/ur/state_parser.h b/include/ur_modern_driver/ur/state_parser.h index 146d93c9a..37da43c6d 100644 --- a/include/ur_modern_driver/ur/state_parser.h +++ b/include/ur_modern_driver/ur/state_parser.h @@ -96,3 +96,4 @@ class URStateParser : public URParser typedef URStateParser URStateParser_V1_X; typedef URStateParser URStateParser_V3_0__1; typedef URStateParser URStateParser_V3_2; +typedef URStateParser URStateParser_V3_5; diff --git a/src/ur/robot_mode.cpp b/src/ur/robot_mode.cpp index da3a65ba8..783648ba9 100644 --- a/src/ur/robot_mode.cpp +++ b/src/ur/robot_mode.cpp @@ -54,6 +54,18 @@ bool RobotModeData_V3_2::parseWith(BinParser& bp) return true; } +bool RobotModeData_V3_5::parseWith(BinParser& bp) +{ + if (!bp.checkSize()) + return false; + + RobotModeData_V3_2::parseWith(bp); + + bp.parse(unknown_internal_use); + + return true; +} + bool RobotModeData_V1_X::consumeWith(URStatePacketConsumer& consumer) { return consumer.consume(*this); @@ -65,4 +77,8 @@ bool RobotModeData_V3_0__1::consumeWith(URStatePacketConsumer& consumer) bool RobotModeData_V3_2::consumeWith(URStatePacketConsumer& consumer) { return consumer.consume(*this); -} \ No newline at end of file +} +bool RobotModeData_V3_5::consumeWith(URStatePacketConsumer& consumer) +{ + return consumer.consume(*this); +} From f71c83c649428b76280e63dc0a382dccab184918 Mon Sep 17 00:00:00 2001 From: Simon Schmeisser Date: Thu, 11 Jan 2018 14:46:54 +0100 Subject: [PATCH 094/114] respawn needs to have a value (#12) Respawn defaults to false, so just remove it. --- launch/ur_common.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/ur_common.launch b/launch/ur_common.launch index aad4c6d29..c8cb7bd6e 100644 --- a/launch/ur_common.launch +++ b/launch/ur_common.launch @@ -26,7 +26,7 @@ - + From 5dcef724157f67c30d467549baadf7ad2fbceeb8 Mon Sep 17 00:00:00 2001 From: Jarek Potiuk Date: Fri, 29 Dec 2017 09:37:56 +0100 Subject: [PATCH 095/114] Adds Safe Trajectory Follower implementation Safe Trajectory Follower implements different approach for controlling the robot. Rather than calculate the interpolation steps in the driver and send the small interpolated steps over the network to the URScript program with 500Hz frequency, the coarser MoveIt trajectory is sent (with few Hz) and the interpolation steps are calculated by the URScript. The algorithm for time progress has also built-in protection against any delays induced by load on the driver, network or URControl - it will never "catch-up" dangerously when such delay are introduced, It will rather pause and wait for the next small interpolation step instructions and re-start the move slower - never skipping any interpolated steps. Those changes make Safe Trajectory Follower much more resilient to network communication problems and removes any superficial requirements for the network setup, kernel latency and no-load-requirement for the driver's PC - making it much more suitable for research, development and quick iteration loops. It works reliably even over WiFi. --- CMakeLists.txt | 3 + README.md | 29 +- include/ur_modern_driver/ros/action_server.h | 6 +- .../ros/safe_trajectory_follower.h | 50 ++ .../ros/trajectory_follower.h | 24 +- .../ros/trajectory_follower_interface.h | 32 ++ include/ur_modern_driver/tcp_socket.h | 1 + include/ur_modern_driver/ur/server.h | 5 +- launch/ur_common.launch | 22 +- src/ros/action_server.cpp | 3 +- src/ros/safe_trajectory_follower.cpp | 515 ++++++++++++++++++ src/ros_main.cpp | 24 +- src/tcp_socket.cpp | 10 + src/ur/server.cpp | 41 ++ 14 files changed, 734 insertions(+), 31 deletions(-) create mode 100644 include/ur_modern_driver/ros/safe_trajectory_follower.h create mode 100644 include/ur_modern_driver/ros/trajectory_follower_interface.h create mode 100644 src/ros/safe_trajectory_follower.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 22cf821ea..17a4d4e0f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -137,6 +137,8 @@ endif() add_compile_options(-Wall) add_compile_options(-Wextra) add_compile_options(-Wno-unused-parameter) +add_compile_options(-Wno-ignored-qualifiers) +add_compile_options(-Wno-return-type) # support indigo's ros_control - This can be removed upon EOL indigo if("${controller_manager_msgs_VERSION}" VERSION_LESS "0.10.0") @@ -172,6 +174,7 @@ set(${PROJECT_NAME}_SOURCES src/ros/rt_publisher.cpp src/ros/service_stopper.cpp src/ros/trajectory_follower.cpp + src/ros/safe_trajectory_follower.cpp src/ros/urscript_handler.cpp src/ur/stream.cpp src/ur/server.cpp diff --git a/README.md b/README.md index 51427a0ed..0516a4cd2 100644 --- a/README.md +++ b/README.md @@ -22,9 +22,9 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design * Besides this, the driver subscribes to two new topics: - * */ur\_driver/URScript* : Takes messages of type _std\_msgs/String_ and directly forwards it to the robot. Note that no control is done on the input, so use at your own risk! Inteded for sending movel/movej commands directly to the robot, conveyor tracking and the like. + * */ur\_driver/URScript* : Takes messages of type _std\_msgs/String_ and directly forwards it to the robot. Note that no control is done on the input, so use at your own risk! Intended for sending movel/movej commands directly to the robot, conveyor tracking and the like. - * */joint\_speed* : Takes messages of type _trajectory\_msgs/JointTrajectory_. Parses the first JointTracetoryPoint and sends the specified joint speeds and accelerations to the robot. This interface is intended for doing visual servoing and other kind of control that requires speed control rather than position control of the robot. Remember to set values for all 6 joints. Ignores the field joint\_names, so set the values in the correct order. + * */joint\_speed* : Takes messages of type _trajectory\_msgs/JointTrajectory_. Parses the first JointTrajectoryPoint and sends the specified joint speeds and accelerations to the robot. This interface is intended for doing visual servoing and other kind of control that requires speed control rather than position control of the robot. Remember to set values for all 6 joints. Ignores the field joint\_names, so set the values in the correct order. * Added support for ros_control. * As ros_control wants to have control over the robot at all times, ros_control compatibility is set via a parameter at launch-time. @@ -39,6 +39,31 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design * Added activation modes: `Never`, `Always` and `OnStartup`. Sets wether a call to the `ur_driver/robot_enable` service is required at startup, at startup + on errors or never. Is intended as a safety feature to require some sort of manual intervention in case of driver crashes and robot safety faults. +* **Safe Trajectory Follower** mode of execution. In this mode the real-time control loop for the robot is shifted from the client PC running the driver to URscript executed on the URControl PC of the robot with the following features: + * It works only with */follow\_joint\_trajectory* action for MoveIt integration. It is mutually exclusive with *ros_control* + * It only implements "position + velocity" based control - it uses coarse positions and velocities calculated by MoveIt and performs cubic interpolation of joint positions (Bezier' curve) of positions. The positions are set using servoj command of URScript. + * It is much more safe and resilient to connectivity problems than other methods of ur_modern_driver. It is resilient to additional load on client PC, latency of network and kernel of the PC the driver runs on. This makes it much better suitable for development/research quick iteration loops without separate dedicated physical setup. + * Other methods of controlling the robot by ur_modern_driver are very fragile and require low-latency kernel, separated wired network and ideally no other network activity on the PC where ur_modern_driver runs. + * **Safe Trajectory Follower** will never make dangerous "catch-up" when move is delayed. Other methods prioritise execution time of a move which sometimes might lead to situation where robot gets out of control and speeds up to catch-up - often resulting in Protective Stop. For Safe Trajectory Follower safety of the move has priority over move execution time. + * The amount of TCP/IP communication between the driver and UR robot (URControl PC) is two orders of magnitude (around 100x) less with **Safe Trajectory Follower** than with other methods- coarse MoveIt generated trajectory is sent to robot and cubic interpolation of the move is calculated by the URScript program run on URControl PC. + * Due to communication optimisations and **Safe Trajectory Follower** works reliably even over WiFi connection (!) which is impossible for other methods. + * Time flow for the URScript program might be independent from "real time" and can be controlled to speed up or slow down execution of planned moves. + * The Safe Trajectory Follower requires 3.0+ version firmware of the robot (servoj command must support lookahead_time and servoj_gain parameters) + * There are several parameters that you can use to control Safe Trajectory Follower's behaviour: + * **use_safe_trajectory_follower** - should be set to `true` to enable the follower + * **time_interval** - time interval (in seconds) this is 'simulated' time for interpolation steps of the move. Together with *servoj_time* it can be used to change speed of moves even after they are planned by MoveIt. Default value is 0.008 + * **servoj_time** - time interval (real time) for which each interpolation step controls the robot (using servoj command). See below on examples of setting different time parameters. Default value is 0.008 (corresponds to expected 125Hz frequency of UR robot control) + * **servoj_time_waiting** - time in seconds (real time) of internal active loop while the robot waits for new instructions in case of delays in communication. The smaller value, the faster robot restarts move after delay (but more stress is put on URControl processor). Default value is 0.001 (1000 Hz check frequency) + * **max_waiting_time** - maximum time in seconds (real time) to wait for instructions from the drive before move is aborted. Defaults to 2 seconds. + * **debug** - displays (visible in the log of Polyscope/pendant and URControl PC) helpful information about trajectory points messages exchanged Driver <-> URScript. It is safe to be run with real robot and introduces very small overhead on the execution of the moves + * **more_debug** - displays even more debug information in the logs of Polyscope - details about interpolation loop calculations and execution. It adds a lot of overhead and is not recommended to be used with real robot (It's quite OK to run it with URSim though) + * **servoj_gain** and **servoj_lookahead_time** - useful to control precision and speed of the position moves with servoj command (see URScript documentation for detailes) + +Here are some examples of manipulating the time flow for **Safe Trajectory Follower** mode. You can use other settings but you should do it on your own risk. + * Default mode: *servoj_time* = 0.008, *time_interval* = 0.008 : interpolation time flows with the same speed as real time - moves are executed as planned + * Slow-motion mode: *servoj_time* = 0.008, *time_interval* = 0.004 : interpolation time flows 2x slower than real time, so the move is executed 2x slower than planned. Requires configuring MoveIt to accept much slower moves than expected (otherwise MoveIt cancels such move mid-way) + * Fast-forward mode: *servoj_time* = 0.004, *time_interval* = 0.012 : interpolation time flows 3x faster than real time, so the move is 3x faster than planned. Might violate limits of the robot speed so use carefully and make sure you gradually increase the speed in-between to check how safe it is. + ## Installation **As the driver communicates with the robot via ethernet and depends on reliable continous communication, it is not possible to reliably control a UR from a virtual machine.** diff --git a/include/ur_modern_driver/ros/action_server.h b/include/ur_modern_driver/ros/action_server.h index 1d62b8f6e..fe5056752 100644 --- a/include/ur_modern_driver/ros/action_server.h +++ b/include/ur_modern_driver/ros/action_server.h @@ -39,7 +39,7 @@ class ActionServer : public Service, public URRTPacketConsumer std::condition_variable tj_cv_; std::thread tj_thread_; - TrajectoryFollower& follower_; + TrajectoryFollowerInterface& follower_; RobotState state_; std::array q_actual_, qd_actual_; @@ -61,7 +61,7 @@ class ActionServer : public Service, public URRTPacketConsumer bool updateState(RTShared& data); public: - ActionServer(TrajectoryFollower& follower, std::vector& joint_names, double max_velocity); + ActionServer(TrajectoryFollowerInterface& follower, std::vector& joint_names, double max_velocity); void start(); virtual void onRobotStateChange(RobotState state); @@ -70,4 +70,4 @@ class ActionServer : public Service, public URRTPacketConsumer virtual bool consume(RTState_V1_8& state); virtual bool consume(RTState_V3_0__1& state); virtual bool consume(RTState_V3_2__3& state); -}; \ No newline at end of file +}; diff --git a/include/ur_modern_driver/ros/safe_trajectory_follower.h b/include/ur_modern_driver/ros/safe_trajectory_follower.h new file mode 100644 index 000000000..f3a6bae8e --- /dev/null +++ b/include/ur_modern_driver/ros/safe_trajectory_follower.h @@ -0,0 +1,50 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/ur/commander.h" +#include "ur_modern_driver/ur/server.h" +#include "ur_modern_driver/ros/trajectory_follower_interface.h" + +class SafeTrajectoryFollower: public TrajectoryFollowerInterface +{ +private: + std::atomic running_; + std::array last_positions_; + URCommander &commander_; + URServer server_; + + double time_interval_, servoj_time_, servoj_time_waiting_, max_waiting_time_, \ + servoj_gain_, servoj_lookahead_time_; + bool debug_, more_debug_; + + std::string program_; + + template + size_t append(uint8_t *buffer, T &val) + { + size_t s = sizeof(T); + std::memcpy(buffer, &val, s); + return s; + } + + bool execute(const std::array &positions, + const std::array &velocities, + double sample_number, double time_in_seconds); + +public: + SafeTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, bool version_3); + + bool start(); + bool execute(std::vector &trajectory, std::atomic &interrupt); + void stop(); + + virtual ~SafeTrajectoryFollower() {}; +}; diff --git a/include/ur_modern_driver/ros/trajectory_follower.h b/include/ur_modern_driver/ros/trajectory_follower.h index d6c478999..70dba5ce9 100644 --- a/include/ur_modern_driver/ros/trajectory_follower.h +++ b/include/ur_modern_driver/ros/trajectory_follower.h @@ -11,24 +11,9 @@ #include "ur_modern_driver/log.h" #include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/server.h" +#include "ur_modern_driver/ros/trajectory_follower_interface.h" -struct TrajectoryPoint -{ - std::array positions; - std::array velocities; - std::chrono::microseconds time_from_start; - - TrajectoryPoint() - { - } - - TrajectoryPoint(std::array &pos, std::array &vel, std::chrono::microseconds tfs) - : positions(pos), velocities(vel), time_from_start(tfs) - { - } -}; - -class TrajectoryFollower +class TrajectoryFollower : public TrajectoryFollowerInterface { private: std::atomic running_; @@ -57,5 +42,6 @@ class TrajectoryFollower bool execute(std::array &positions); bool execute(std::vector &trajectory, std::atomic &interrupt); void stop(); - void interrupt(); -}; \ No newline at end of file + + virtual ~TrajectoryFollower() {}; +}; diff --git a/include/ur_modern_driver/ros/trajectory_follower_interface.h b/include/ur_modern_driver/ros/trajectory_follower_interface.h new file mode 100644 index 000000000..a25ddf7d9 --- /dev/null +++ b/include/ur_modern_driver/ros/trajectory_follower_interface.h @@ -0,0 +1,32 @@ +#pragma once + +#include +#include +#include +#include +#include + +struct TrajectoryPoint +{ + std::array positions; + std::array velocities; + std::chrono::microseconds time_from_start; + + TrajectoryPoint() + { + } + + TrajectoryPoint(std::array &pos, std::array &vel, std::chrono::microseconds tfs) + : positions(pos), velocities(vel), time_from_start(tfs) + { + } +}; + +class TrajectoryFollowerInterface +{ +public: + virtual bool start() = 0; + virtual bool execute(std::vector &trajectory, std::atomic &interrupt) = 0; + virtual void stop() = 0; + virtual ~TrajectoryFollowerInterface() {}; +}; diff --git a/include/ur_modern_driver/tcp_socket.h b/include/ur_modern_driver/tcp_socket.h index 99427b928..4ff2f868e 100644 --- a/include/ur_modern_driver/tcp_socket.h +++ b/include/ur_modern_driver/tcp_socket.h @@ -46,6 +46,7 @@ class TCPSocket std::string getIP(); + bool read(char *character); bool read(uint8_t *buf, size_t buf_len, size_t &read); bool write(const uint8_t *buf, size_t buf_len, size_t &written); diff --git a/include/ur_modern_driver/ur/server.h b/include/ur_modern_driver/ur/server.h index c240ba5ce..1809237b5 100644 --- a/include/ur_modern_driver/ur/server.h +++ b/include/ur_modern_driver/ur/server.h @@ -8,6 +8,8 @@ #include #include "ur_modern_driver/tcp_socket.h" +#define MAX_SERVER_BUF_LEN 50 + class URServer : private TCPSocket { private: @@ -24,5 +26,6 @@ class URServer : private TCPSocket bool bind(); bool accept(); void disconnectClient(); + bool readLine(char* buffer, size_t buf_len); bool write(const uint8_t *buf, size_t buf_len, size_t &written); -}; \ No newline at end of file +}; diff --git a/launch/ur_common.launch b/launch/ur_common.launch index c8cb7bd6e..ca6eaec02 100644 --- a/launch/ur_common.launch +++ b/launch/ur_common.launch @@ -2,7 +2,7 @@ @@ -12,14 +12,23 @@ + + + + + + + + + - + @@ -30,10 +39,19 @@ + + + + + + + + + diff --git a/src/ros/action_server.cpp b/src/ros/action_server.cpp index 494d93296..c8546c23e 100644 --- a/src/ros/action_server.cpp +++ b/src/ros/action_server.cpp @@ -1,7 +1,7 @@ #include "ur_modern_driver/ros/action_server.h" #include -ActionServer::ActionServer(TrajectoryFollower& follower, std::vector& joint_names, double max_velocity) +ActionServer::ActionServer(TrajectoryFollowerInterface& follower, std::vector& joint_names, double max_velocity) : as_(nh_, "follow_joint_trajectory", boost::bind(&ActionServer::onGoal, this, _1), boost::bind(&ActionServer::onCancel, this, _1), false) , joint_names_(joint_names) @@ -307,6 +307,7 @@ void ActionServer::trajectoryThread() Result res; + LOG_INFO("Attempting to start follower %p", &follower_); if (follower_.start()) { if (follower_.execute(trajectory, interrupt_traj_)) diff --git a/src/ros/safe_trajectory_follower.cpp b/src/ros/safe_trajectory_follower.cpp new file mode 100644 index 000000000..678c5ac7a --- /dev/null +++ b/src/ros/safe_trajectory_follower.cpp @@ -0,0 +1,515 @@ +#include "ur_modern_driver/ros/safe_trajectory_follower.h" +#include +#include +#include + +static const std::array EMPTY_VALUES = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + +static const std::string TIME_INTERVAL("{{TIME_INTERVAL}}"); +static const std::string SERVOJ_TIME("{{SERVOJ_TIME}}"); +static const std::string SERVOJ_TIME_WAITING("{{SERVOJ_TIME_WAITING}}"); +static const std::string MAX_WAITING_TIME("{{MAX_WAITING_TIME}}"); +static const std::string DEBUG("{{DEBUG}}"); +static const std::string MORE_DEBUG("{{MORE_DEBUG}}"); +static const std::string SERVOJ_GAIN("{{SERVOJ_GAIN}}"); +static const std::string SERVOJ_LOOKAHEAD_TIME("{{SERVOJ_LOOKAHEAD_TIME}}"); +static const std::string REVERSE_IP("{{REVERSE_IP}}"); +static const std::string REVERSE_PORT("{{REVERSE_PORT}}"); +static const std::string POSITION_PROGRAM = R"( +def driveRobotSafeTrajectory(): + + global JOINT_NUM = 6 + global TIME_INTERVAL = {{TIME_INTERVAL}} + global SERVOJ_TIME = {{SERVOJ_TIME}} + global SERVOJ_TIME_WAITING = {{SERVOJ_TIME_WAITING}} + global MAX_WAITING_TIME = {{MAX_WAITING_TIME}} + global DEBUG = {{DEBUG}} + global MORE_DEBUG = {{MORE_DEBUG}} + global EMPTY_VALUES = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + global SERVOJ_GAIN = {{SERVOJ_GAIN}} + global SERVOJ_LOOKAHEAD_TIME = {{SERVOJ_LOOKAHEAD_TIME}} + global CONNECTION_NAME = "reverse_connection" + global REVERSE_IP = "{{REVERSE_IP}}" + global REVERSE_PORT = {{REVERSE_PORT}} + + # NOTE All the global variables here are accessed by different threads + # therefore they should be accessed within critical section. Those variables + # are all prefixed with g_ . Whenever their values are needed they are copied + # to similarly named l_ variable. Copying happens inside the critical section + # and l_values might be used outside of it. This needs to be confirmed with UR + # about the semantics of assignment operator (copying or by reference?). + # Hopefully it's copying :). + # + # Please make sure to keep that pattern and do not access the global variables + # outside of the critical section + # + # TO DO: Are those assignments by references or copies? We will find out soon + # If not then assigning positions would not work either + global g_position_previous = EMPTY_VALUES + global g_position_target = EMPTY_VALUES + global g_position_next = EMPTY_VALUES + + global g_velocity_previous = EMPTY_VALUES + global g_velocity_target = EMPTY_VALUES + global g_velocity_next = EMPTY_VALUES + + global g_time_previous = 0.0 + global g_time_target = 0.0 + global g_time_next = 0.0 + + global g_num_previous = -1 + global g_num_target = -1 + global g_num_next = -1 + + global g_received_waypoints_number = -1 + global g_requested_waypoints_number = -1 + + global g_total_elapsed_time = 0 + + global g_stopping = False + + def send_message(message): + socket_send_string(message, CONNECTION_NAME) + socket_send_byte(10, CONNECTION_NAME) + end + + def is_waypoint_sentinel(waypoint): + local l_previous_index = 2 + while l_previous_index < 1 + JOINT_NUM * 2 + 2: # Note - we do not check first two which are non-zero + if waypoint[l_previous_index] != 0.0: + return False + end + l_previous_index = l_previous_index + 1 + end + return True + end + + def interpolate(time_within_segment, total_segment_time, start_pos, l_end_pos, l_start_vel, end_vel): + local a = start_pos + local b = l_start_vel + local c = (-3 * a + 3 * l_end_pos - 2 * total_segment_time * b - total_segment_time * end_vel) / pow(total_segment_time, 2) + local d = (2 * a - 2 * l_end_pos + total_segment_time * b + total_segment_time * end_vel) / pow(total_segment_time, 3) + return a + b * time_within_segment + c * pow(time_within_segment, 2) + d * pow(time_within_segment, 3) + end + + def add_next_waypoint(waypoint): + enter_critical + + # Rotate the values received so far: + # target -> previous + g_position_previous = g_position_target + g_velocity_previous = g_velocity_target + g_time_previous = g_time_target + g_num_previous = g_num_target + + # next -> previous + g_position_target = g_position_next + g_velocity_target = g_velocity_next + g_time_target = g_time_next + g_num_target = g_num_next + + # Decode the array received into next + # waypoint 0 is number of entries in the received array + g_num_next = waypoint[1] + g_position_next = [waypoint[2], waypoint[3], waypoint[4], waypoint[5], waypoint[6], waypoint[7]] + g_velocity_next = [waypoint[8], waypoint[9], waypoint[10], waypoint[11], waypoint[12], waypoint[13]] + g_time_next = waypoint[14] + + # store latest received waypoint number so that controlling thread knows it's been received already + g_received_waypoints_number = g_num_next + + if DEBUG: + local l_received_waypoints_number = g_received_waypoints_number + local l_waypoint = waypoint + end + + exit_critical + + if DEBUG: + textmsg("Received waypoint:") + textmsg(l_received_waypoints_number) + textmsg(l_waypoint) + end + end + + # Thread controlling the motor. In the loop it checks first if it received the + # requested waypoints and until it does, it syncs doing noting to the motor + # once it received all up to requested waypoints it executes interpolation + # between PREVIOUS AND TARGET points received and rquests the next waypoint request + # to be sent. + thread controllingThread(): + + local l_received_waypoints_number = -1 + local l_requested_waypoints_number = -1 + local l_stopped = False + local l_current_position = get_actual_joint_positions() + + enter_critical + g_requested_waypoints_number = 2 + exit_critical + + while True: + + enter_critical + l_requested_waypoints_number = g_requested_waypoints_number + l_received_waypoints_number = g_received_waypoints_number + exit_critical + + local l_max_waiting_time_left = MAX_WAITING_TIME + # if expected waypoint number not yet received wait so that receiving thread has time to receive it + while l_received_waypoints_number < l_requested_waypoints_number and l_max_waiting_time_left > 0: + + if DEBUG: + textmsg("Waiting for the received waypoints number to catch up (received/requested):") + textmsg(l_received_waypoints_number) + textmsg(l_requested_waypoints_number) + end + # Keep robot in l_current position for short time and check if the next waipoint arrived + servoj(l_current_position,t=SERVOJ_TIME_WAITING,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN) + + enter_critical + l_received_waypoints_number = g_received_waypoints_number + exit_critical + + l_max_waiting_time_left = l_max_waiting_time_left - SERVOJ_TIME_WAITING + + end + + if l_max_waiting_time_left <= 0: + textmsg("Closing the connection on waiting too long.") + socket_close(CONNECTION_NAME) + halt + end + + # OK. We received next point, copy the required global variables into local ones + enter_critical + + local l_start_pos = g_position_previous + local l_start_vel = g_velocity_previous + local l_start_time = g_time_previous + local l_start_num= g_num_previous + local l_end_pos = g_position_target + local l_end_vel = g_velocity_target + local l_end_time = g_time_target + local l_end_num = g_num_target + local l_total_elapsed_time = g_total_elapsed_time + + # Note we deliberately only read "stopping" state here and not update it below + # so that stopping flag takes effect only after one additional interpolation loop is complete + # and all points of the trajectory are processeed + local l_stopping_after_next_interpolation = g_stopping + + # And increasing the global requested number - informs sender thread that it needs to ask for it + g_requested_waypoints_number = g_requested_waypoints_number + 1 + exit_critical + + if DEBUG: + textmsg("Starting interpolation. Segment from/to:") + textmsg(l_start_num) + textmsg(l_end_num) + textmsg("Current time:") + textmsg(l_total_elapsed_time) + textmsg("Starting/Ending segment time:") + textmsg(l_start_time) + textmsg(l_end_time) + end + + l_current_position = l_start_pos + + local l_total_segment_time = l_end_time - l_start_time + + # Here perform the interpolation loop + while l_total_elapsed_time <= l_end_time: + if MORE_DEBUG: + textmsg("Next step of interpolation:") + end + + local l_segment_elapsed_time = l_total_elapsed_time - l_start_time + + # Calculate interpolation for all joints + j = 0 + while j < JOINT_NUM: + l_current_position[j] = interpolate(l_segment_elapsed_time, l_total_segment_time, l_start_pos[j], l_end_pos[j], l_start_vel[j], l_end_vel[j]) + j = j + 1 + end + + if MORE_DEBUG: + textmsg("Next step of interpolated position:") + textmsg(l_current_position) + textmsg("Current time:") + textmsg(l_total_elapsed_time) + textmsg("Running servoj command:") + end + + servoj(l_current_position,t=SERVOJ_TIME,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN) + + enter_critical + g_total_elapsed_time = g_total_elapsed_time + TIME_INTERVAL + l_total_elapsed_time = g_total_elapsed_time + exit_critical + + if MORE_DEBUG: + textmsg("Finishing interpolation step at time:") + textmsg(l_total_elapsed_time) + end + end + if DEBUG: + textmsg("Ending interpolation segment at time:") + textmsg(l_total_elapsed_time) + end + if l_stopping_after_next_interpolation: + textmsg("Stopping the controlling thread on signal from receiving thread after one interpolation loop.") + break + end + end + textmsg("Ending controlling thread") + end + + # This thread sends requested waypoints number to the client when requested number is changed + # It will send all the numbers from [already sent + 1, g_requested_waypoints_number] and waits + # until requested waypoints number increases + thread sendingThread(): + local controlling_thread = run controllingThread() + local l_sent_waypoints_number = -1 + local l_requested_waypoints_number = -1 + local l_stopping = False + + enter_critical + l_requested_waypoints_number = g_requested_waypoints_number + l_stopping = g_stopping + exit_critical + while not l_stopping: + # wait until we have more requested waypoints to send than actually sent ones + while l_sent_waypoints_number == l_requested_waypoints_number and not l_stopping: + sleep(SERVOJ_TIME_WAITING) + + enter_critical + l_requested_waypoints_number = g_requested_waypoints_number + l_stopping = g_stopping + exit_critical + + end + if l_stopping: + break + end + send_message(l_sent_waypoints_number + 1) + l_sent_waypoints_number = l_sent_waypoints_number + 1 + if DEBUG: + textmsg("Sent waypoint request number:") + textmsg(l_sent_waypoints_number) + end + end + textmsg("Joining controlling thread") + join controlling_thread + textmsg("Ending Sending thread") + end + + # Receiving thread - it will receive the next trajectory point over the TCP connection + # It will increase the received waipoints_number on each request. + thread receivingThread(): + local sending_thread = run sendingThread() + while True: + waypoint_received = socket_read_ascii_float(14, CONNECTION_NAME) + if waypoint_received[0] == 0: + # No new waypoint requested for the last 2 seconds + textmsg("Not received trajectory for the last 2 seconds. Quitting") + enter_critical + g_stopping = True + exit_critical + + break + elif waypoint_received[0] != JOINT_NUM * 2 + 2: + textmsg("Received wrong number of floats in trajectory. This is certainly not OK.") + textmsg(waypoint_received[0]) + + enter_critical + g_stopping = True + exit_critical + + break + elif is_waypoint_sentinel(waypoint_received): + add_next_waypoint(waypoint_received) + textmsg("Received sentinel waypoint. Finishing.") + + enter_critical + g_stopping = True + g_received_waypoints_number = g_received_waypoints_number + 1 + exit_critical + + break + end + add_next_waypoint(waypoint_received) + end + textmsg("Joining sendingThread") + join sending_thread + textmsg("Ending Receiving thread") + end + + textmsg("Opening socket") + socket_open(REVERSE_IP, REVERSE_PORT, CONNECTION_NAME) + textmsg("Socket opened") + + receiving_thread = run receivingThread() + textmsg("Joining receiving_thread") + join receiving_thread + textmsg("Closing reverse connection") + socket_close(CONNECTION_NAME) + textmsg("Exiting the program") +end + +)"; + +SafeTrajectoryFollower::SafeTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, + bool version_3) + : running_(false) + , commander_(commander) + , server_(reverse_port) + , time_interval_(0.008) + , servoj_time_(0.008) + , servoj_time_waiting_(0.001) + , max_waiting_time_(2.0) + , servoj_gain_(300.0) + , servoj_lookahead_time_(0.03) + , debug_(false) + , more_debug_(false) +{ + ros::param::get("~time_interval", time_interval_); + ros::param::get("~servoj_time", servoj_time_); + ros::param::get("~servoj_time_waiting", servoj_time_waiting_); + ros::param::get("~max_waiting_time", max_waiting_time_); + ros::param::get("~servoj_gain", servoj_gain_); + ros::param::get("~servoj_lookahead_time", servoj_lookahead_time_); + ros::param::get("~debug", debug_); + ros::param::get("~more_debug", more_debug_); + + std::string res(POSITION_PROGRAM); + std::ostringstream out; + if (!version_3) { + LOG_ERROR("Safe Trajectory Follower only works for interface version > 3"); + std::exit(-1); + } + res.replace(res.find(TIME_INTERVAL), TIME_INTERVAL.length(), std::to_string(time_interval_)); + res.replace(res.find(SERVOJ_TIME_WAITING), SERVOJ_TIME_WAITING.length(), std::to_string(servoj_time_waiting_)); + res.replace(res.find(SERVOJ_TIME), SERVOJ_TIME.length(), std::to_string(servoj_time_)); + res.replace(res.find(MAX_WAITING_TIME), MAX_WAITING_TIME.length(), std::to_string(max_waiting_time_)); + res.replace(res.find(SERVOJ_GAIN), SERVOJ_GAIN.length(), std::to_string(servoj_gain_)); + res.replace(res.find(SERVOJ_LOOKAHEAD_TIME), SERVOJ_LOOKAHEAD_TIME.length(), std::to_string(servoj_lookahead_time_)); + res.replace(res.find(DEBUG), DEBUG.length(), debug_ ? "True" : "False"); + res.replace(res.find(MORE_DEBUG), MORE_DEBUG.length(), more_debug_ ? "True" : "False"); + res.replace(res.find(REVERSE_IP), REVERSE_IP.length(), reverse_ip); + res.replace(res.find(REVERSE_PORT), REVERSE_PORT.length(), std::to_string(reverse_port)); + program_ = res; + + if (!server_.bind()) + { + LOG_ERROR("Failed to bind server, the port %d is likely already in use", reverse_port); + std::exit(-1); + } + LOG_INFO("Safe Trajectory Follower is initialized!"); +} + +bool SafeTrajectoryFollower::start() +{ + LOG_INFO("Starting SafeTrajectoryFollower"); + + if (running_) + return true; // not sure + + LOG_INFO("Uploading trajectory program to robot"); + + if (!commander_.uploadProg(program_)) + { + LOG_ERROR("Program upload failed!"); + return false; + } + + LOG_DEBUG("Awaiting incoming robot connection"); + + if (!server_.accept()) + { + LOG_ERROR("Failed to accept incoming robot connection"); + return false; + } + + LOG_DEBUG("Robot successfully connected"); + return (running_ = true); +} + +bool SafeTrajectoryFollower::execute(const std::array &positions, + const std::array &velocities, + double sample_number, double time_in_seconds) +{ + if (!running_) + return false; + + std::ostringstream out; + + out << "("; + out << sample_number << ","; + for (auto const &pos: positions) + { + out << pos << ","; + } + for (auto const &vel: velocities) + { + out << vel << ","; + } + out << time_in_seconds << ")\r\n"; + + // I know it's ugly but it's the most efficient and fastest way + // We have only ASCII characters and we can cast char -> uint_8 + const std::string tmp = out.str(); + const char *formatted_message = tmp.c_str(); + const uint8_t *buf = (uint8_t *) formatted_message; + + size_t written; + LOG_DEBUG("Sending message %s", formatted_message); + + return server_.write(buf, strlen(formatted_message) + 1, written); +} + +bool SafeTrajectoryFollower::execute(std::vector &trajectory, std::atomic &interrupt) +{ + if (!running_) + return false; + + bool finished = false; + + char* line[MAX_SERVER_BUF_LEN]; + + bool res = true; + + while (!finished && !interrupt) + { + if (!server_.readLine((char *)line, MAX_SERVER_BUF_LEN)) + { + LOG_DEBUG("Connection closed. Finishing!"); + finished = true; + break; + } + unsigned int message_num=atoi((const char *) line); + LOG_DEBUG("Received request %i", message_num); + if (message_num < trajectory.size()) + { + res = execute(trajectory[message_num].positions, trajectory[message_num].velocities, + message_num, trajectory[message_num].time_from_start.count() / 1e6); + } else + { + res = execute(EMPTY_VALUES, EMPTY_VALUES, message_num, 0.0); + } + if (!res) + { + finished = true; + } + } + return res; +} + +void SafeTrajectoryFollower::stop() +{ + if (!running_) + return; + + server_.disconnectClient(); + running_ = false; +} diff --git a/src/ros_main.cpp b/src/ros_main.cpp index 123ecc427..484cdd14c 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -13,6 +13,7 @@ #include "ur_modern_driver/ros/rt_publisher.h" #include "ur_modern_driver/ros/service_stopper.h" #include "ur_modern_driver/ros/trajectory_follower.h" +#include "ur_modern_driver/ros/safe_trajectory_follower.h" #include "ur_modern_driver/ros/urscript_handler.h" #include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/factory.h" @@ -25,6 +26,7 @@ static const std::string IP_ADDR_ARG("~robot_ip_address"); static const std::string REVERSE_PORT_ARG("~reverse_port"); static const std::string ROS_CONTROL_ARG("~use_ros_control"); +static const std::string SAFE_TRAJECTORY_FOLLOWER("~use_safe_trajectory_follower"); static const std::string MAX_VEL_CHANGE_ARG("~max_vel_change"); static const std::string PREFIX_ARG("~prefix"); static const std::string BASE_FRAME_ARG("~base_frame"); @@ -53,6 +55,7 @@ struct ProgArgs double max_vel_change; int32_t reverse_port; bool use_ros_control; + bool use_safe_trajectory_follower; bool shutdown_on_disconnect; }; @@ -92,6 +95,7 @@ bool parse_args(ProgArgs &args) ros::param::param(MAX_VEL_CHANGE_ARG, args.max_vel_change, 15.0); // rad/s ros::param::param(MAX_VEL_CHANGE_ARG, args.max_velocity, 10.0); ros::param::param(ROS_CONTROL_ARG, args.use_ros_control, false); + ros::param::param(SAFE_TRAJECTORY_FOLLOWER, args.use_safe_trajectory_follower, false); ros::param::param(PREFIX_ARG, args.prefix, std::string()); ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link"); ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller"); @@ -134,7 +138,20 @@ int main(int argc, char **argv) auto rt_commander = factory.getCommander(rt_stream); vector *> rt_vec{ &rt_pub }; - TrajectoryFollower traj_follower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3()); + TrajectoryFollowerInterface *traj_follower(nullptr); + + if (args.use_safe_trajectory_follower && !args.use_ros_control) + { + LOG_INFO("Use safe trajectory follower"); + traj_follower = new SafeTrajectoryFollower(*rt_commander, local_ip, args.reverse_port,factory.isVersion3()); + // We are leaking the follower here, but it's OK as this is once-a-lifetime event + } + else + { + LOG_INFO("Use standard trajectory follower"); + traj_follower = new TrajectoryFollower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3()); + // We are leaking the follower here, but it's OK as this is once-a-lifetime event + } INotifier *notifier(nullptr); ROSController *controller(nullptr); @@ -142,14 +159,15 @@ int main(int argc, char **argv) if (args.use_ros_control) { LOG_INFO("ROS control enabled"); - controller = new ROSController(*rt_commander, traj_follower, args.joint_names, args.max_vel_change, args.tcp_link); + // Note - we are sure that TrajectoryFollower is used here (see the args.use_ros_control above) + controller = new ROSController(*rt_commander, *((TrajectoryFollower *) traj_follower), args.joint_names, args.max_vel_change, args.tcp_link); rt_vec.push_back(controller); services.push_back(controller); } else { LOG_INFO("ActionServer enabled"); - action_server = new ActionServer(traj_follower, args.joint_names, args.max_velocity); + action_server = new ActionServer(*traj_follower, args.joint_names, args.max_velocity); rt_vec.push_back(action_server); services.push_back(action_server); } diff --git a/src/tcp_socket.cpp b/src/tcp_socket.cpp index 0bd390abb..619151a83 100644 --- a/src/tcp_socket.cpp +++ b/src/tcp_socket.cpp @@ -111,6 +111,16 @@ std::string TCPSocket::getIP() return std::string(buf); } +bool TCPSocket::read(char *character) +{ + size_t read_chars; + // It's inefficient, but in our case we read very small messages + // and the overhead connected with reading character by character is + // negligible - adding buffering would complicate the code needlessly. + return read((uint8_t *) character, 1, read_chars); +} + + bool TCPSocket::read(uint8_t *buf, size_t buf_len, size_t &read) { read = 0; diff --git a/src/ur/server.cpp b/src/ur/server.cpp index 1d548693f..557ed5da6 100644 --- a/src/ur/server.cpp +++ b/src/ur/server.cpp @@ -86,3 +86,44 @@ bool URServer::write(const uint8_t* buf, size_t buf_len, size_t& written) { return client_.write(buf, buf_len, written); } + +bool URServer::readLine(char* buffer, size_t buf_len) +{ + char *current_pointer = buffer; + char ch; + size_t total_read; + + if (buf_len <= 0 || buffer == NULL) { + return false; + } + + total_read = 0; + for (;;) { + if (client_.read(&ch)) + { + if (total_read < buf_len - 1) // just in case ... + { + total_read ++; + *current_pointer++ = ch; + } + if (ch == '\n') + { + break; + } + } + else + { + if (total_read == 0) + { + return false; + } + else + { + break; + } + } + } + + *current_pointer = '\0'; + return true; +} From a54b97e939848fc725a9b96b2d147a407b46a525 Mon Sep 17 00:00:00 2001 From: Jarek Potiuk Date: Sun, 14 Jan 2018 21:04:41 +0100 Subject: [PATCH 096/114] Renamed Safe Trajectory Follower to Low Bandwidth one --- .gitignore | 1 + CMakeLists.txt | 2 +- README.md | 18 ++++++++--------- ...r.h => lowbandwidth_trajectory_follower.h} | 6 +++--- launch/ur_common.launch | 4 ++-- ...p => lowbandwidth_trajectory_follower.cpp} | 20 +++++++++---------- src/ros_main.cpp | 14 ++++++------- 7 files changed, 33 insertions(+), 32 deletions(-) rename include/ur_modern_driver/ros/{safe_trajectory_follower.h => lowbandwidth_trajectory_follower.h} (82%) rename src/ros/{safe_trajectory_follower.cpp => lowbandwidth_trajectory_follower.cpp} (96%) diff --git a/.gitignore b/.gitignore index 6faf4e14b..1e69c9fb8 100644 --- a/.gitignore +++ b/.gitignore @@ -4,3 +4,4 @@ Makefile cmake_install.cmake install_manifest.txt *~ +.idea diff --git a/CMakeLists.txt b/CMakeLists.txt index 17a4d4e0f..a6a5728c6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -174,7 +174,7 @@ set(${PROJECT_NAME}_SOURCES src/ros/rt_publisher.cpp src/ros/service_stopper.cpp src/ros/trajectory_follower.cpp - src/ros/safe_trajectory_follower.cpp + src/ros/lowbandwidth_trajectory_follower.cpp src/ros/urscript_handler.cpp src/ur/stream.cpp src/ur/server.cpp diff --git a/README.md b/README.md index 0516a4cd2..ec171c78c 100644 --- a/README.md +++ b/README.md @@ -39,18 +39,18 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design * Added activation modes: `Never`, `Always` and `OnStartup`. Sets wether a call to the `ur_driver/robot_enable` service is required at startup, at startup + on errors or never. Is intended as a safety feature to require some sort of manual intervention in case of driver crashes and robot safety faults. -* **Safe Trajectory Follower** mode of execution. In this mode the real-time control loop for the robot is shifted from the client PC running the driver to URscript executed on the URControl PC of the robot with the following features: +* **Low Bandwidth Trajectory Follower** mode of execution. In this mode the real-time control loop for the robot is shifted from the client PC running the driver to URscript executed on the URControl PC of the robot with the following features: * It works only with */follow\_joint\_trajectory* action for MoveIt integration. It is mutually exclusive with *ros_control* * It only implements "position + velocity" based control - it uses coarse positions and velocities calculated by MoveIt and performs cubic interpolation of joint positions (Bezier' curve) of positions. The positions are set using servoj command of URScript. - * It is much more safe and resilient to connectivity problems than other methods of ur_modern_driver. It is resilient to additional load on client PC, latency of network and kernel of the PC the driver runs on. This makes it much better suitable for development/research quick iteration loops without separate dedicated physical setup. + * It is much more resilient to connectivity problems than other methods of ur_modern_driver. It is resilient to additional load on client PC, latency of network and kernel of the PC the driver runs on. This makes it much better suitable for development/research quick iteration loops without separate dedicated physical setup. * Other methods of controlling the robot by ur_modern_driver are very fragile and require low-latency kernel, separated wired network and ideally no other network activity on the PC where ur_modern_driver runs. - * **Safe Trajectory Follower** will never make dangerous "catch-up" when move is delayed. Other methods prioritise execution time of a move which sometimes might lead to situation where robot gets out of control and speeds up to catch-up - often resulting in Protective Stop. For Safe Trajectory Follower safety of the move has priority over move execution time. - * The amount of TCP/IP communication between the driver and UR robot (URControl PC) is two orders of magnitude (around 100x) less with **Safe Trajectory Follower** than with other methods- coarse MoveIt generated trajectory is sent to robot and cubic interpolation of the move is calculated by the URScript program run on URControl PC. - * Due to communication optimisations and **Safe Trajectory Follower** works reliably even over WiFi connection (!) which is impossible for other methods. + * **Low Bandwidth Trajectory Follower** will never make dangerous "catch-up" when move is delayed. Other methods prioritise execution time of a move which sometimes might lead to situation where robot gets out of control and speeds up to catch-up - often resulting in Protective Stop. For Low Bandwith Trajectory Follower predictability of the move has priority over move execution time. + * The amount of TCP/IP communication between the driver and UR robot (URControl PC) is two orders of magnitude (around 100x) less with **Low Bandwidth Trajectory Follower** than with other methods- coarse MoveIt generated trajectory is sent to robot and cubic interpolation of the move is calculated by the URScript program run on URControl PC. + * Due to communication optimisations and **Low Bandwidth Trajectory Follower** works reliably even over WiFi connection (!) which is impossible for other methods. * Time flow for the URScript program might be independent from "real time" and can be controlled to speed up or slow down execution of planned moves. - * The Safe Trajectory Follower requires 3.0+ version firmware of the robot (servoj command must support lookahead_time and servoj_gain parameters) - * There are several parameters that you can use to control Safe Trajectory Follower's behaviour: - * **use_safe_trajectory_follower** - should be set to `true` to enable the follower + * The Low Bandwidth Trajectory Follower requires 3.0+ version firmware of the robot (servoj command must support lookahead_time and servoj_gain parameters) + * There are several parameters that you can use to control Low Bandwidth Trajectory Follower's behaviour: + * **use_lowbandwidth_trajectory_follower** - should be set to `true` to enable the follower * **time_interval** - time interval (in seconds) this is 'simulated' time for interpolation steps of the move. Together with *servoj_time* it can be used to change speed of moves even after they are planned by MoveIt. Default value is 0.008 * **servoj_time** - time interval (real time) for which each interpolation step controls the robot (using servoj command). See below on examples of setting different time parameters. Default value is 0.008 (corresponds to expected 125Hz frequency of UR robot control) * **servoj_time_waiting** - time in seconds (real time) of internal active loop while the robot waits for new instructions in case of delays in communication. The smaller value, the faster robot restarts move after delay (but more stress is put on URControl processor). Default value is 0.001 (1000 Hz check frequency) @@ -59,7 +59,7 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design * **more_debug** - displays even more debug information in the logs of Polyscope - details about interpolation loop calculations and execution. It adds a lot of overhead and is not recommended to be used with real robot (It's quite OK to run it with URSim though) * **servoj_gain** and **servoj_lookahead_time** - useful to control precision and speed of the position moves with servoj command (see URScript documentation for detailes) -Here are some examples of manipulating the time flow for **Safe Trajectory Follower** mode. You can use other settings but you should do it on your own risk. +Here are some examples of manipulating the time flow for **Low Bandwidth Trajectory Follower** mode. You can use other settings but you should do it on your own risk. * Default mode: *servoj_time* = 0.008, *time_interval* = 0.008 : interpolation time flows with the same speed as real time - moves are executed as planned * Slow-motion mode: *servoj_time* = 0.008, *time_interval* = 0.004 : interpolation time flows 2x slower than real time, so the move is executed 2x slower than planned. Requires configuring MoveIt to accept much slower moves than expected (otherwise MoveIt cancels such move mid-way) * Fast-forward mode: *servoj_time* = 0.004, *time_interval* = 0.012 : interpolation time flows 3x faster than real time, so the move is 3x faster than planned. Might violate limits of the robot speed so use carefully and make sure you gradually increase the speed in-between to check how safe it is. diff --git a/include/ur_modern_driver/ros/safe_trajectory_follower.h b/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h similarity index 82% rename from include/ur_modern_driver/ros/safe_trajectory_follower.h rename to include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h index f3a6bae8e..bd52ee4f2 100644 --- a/include/ur_modern_driver/ros/safe_trajectory_follower.h +++ b/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h @@ -13,7 +13,7 @@ #include "ur_modern_driver/ur/server.h" #include "ur_modern_driver/ros/trajectory_follower_interface.h" -class SafeTrajectoryFollower: public TrajectoryFollowerInterface +class LowBandwidthTrajectoryFollower: public TrajectoryFollowerInterface { private: std::atomic running_; @@ -40,11 +40,11 @@ class SafeTrajectoryFollower: public TrajectoryFollowerInterface double sample_number, double time_in_seconds); public: - SafeTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, bool version_3); + LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, bool version_3); bool start(); bool execute(std::vector &trajectory, std::atomic &interrupt); void stop(); - virtual ~SafeTrajectoryFollower() {}; + virtual ~LowBandwidthTrajectoryFollower() {}; }; diff --git a/launch/ur_common.launch b/launch/ur_common.launch index ca6eaec02..ad77d4161 100644 --- a/launch/ur_common.launch +++ b/launch/ur_common.launch @@ -13,7 +13,7 @@ - + @@ -40,7 +40,7 @@ - + diff --git a/src/ros/safe_trajectory_follower.cpp b/src/ros/lowbandwidth_trajectory_follower.cpp similarity index 96% rename from src/ros/safe_trajectory_follower.cpp rename to src/ros/lowbandwidth_trajectory_follower.cpp index 678c5ac7a..1e628b270 100644 --- a/src/ros/safe_trajectory_follower.cpp +++ b/src/ros/lowbandwidth_trajectory_follower.cpp @@ -1,4 +1,4 @@ -#include "ur_modern_driver/ros/safe_trajectory_follower.h" +#include "ur_modern_driver/ros/lowbandwidth_trajectory_follower.h" #include #include #include @@ -16,7 +16,7 @@ static const std::string SERVOJ_LOOKAHEAD_TIME("{{SERVOJ_LOOKAHEAD_TIME}}"); static const std::string REVERSE_IP("{{REVERSE_IP}}"); static const std::string REVERSE_PORT("{{REVERSE_PORT}}"); static const std::string POSITION_PROGRAM = R"( -def driveRobotSafeTrajectory(): +def driveRobotLowBandwidthTrajectory(): global JOINT_NUM = 6 global TIME_INTERVAL = {{TIME_INTERVAL}} @@ -359,7 +359,7 @@ end )"; -SafeTrajectoryFollower::SafeTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, +LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, bool version_3) : running_(false) , commander_(commander) @@ -385,7 +385,7 @@ SafeTrajectoryFollower::SafeTrajectoryFollower(URCommander &commander, std::stri std::string res(POSITION_PROGRAM); std::ostringstream out; if (!version_3) { - LOG_ERROR("Safe Trajectory Follower only works for interface version > 3"); + LOG_ERROR("Low Bandwidth Trajectory Follower only works for interface version > 3"); std::exit(-1); } res.replace(res.find(TIME_INTERVAL), TIME_INTERVAL.length(), std::to_string(time_interval_)); @@ -405,12 +405,12 @@ SafeTrajectoryFollower::SafeTrajectoryFollower(URCommander &commander, std::stri LOG_ERROR("Failed to bind server, the port %d is likely already in use", reverse_port); std::exit(-1); } - LOG_INFO("Safe Trajectory Follower is initialized!"); + LOG_INFO("Low Bandwidth Trajectory Follower is initialized!"); } -bool SafeTrajectoryFollower::start() +bool LowBandwidthTrajectoryFollower::start() { - LOG_INFO("Starting SafeTrajectoryFollower"); + LOG_INFO("Starting LowBandwidthTrajectoryFollower"); if (running_) return true; // not sure @@ -435,7 +435,7 @@ bool SafeTrajectoryFollower::start() return (running_ = true); } -bool SafeTrajectoryFollower::execute(const std::array &positions, +bool LowBandwidthTrajectoryFollower::execute(const std::array &positions, const std::array &velocities, double sample_number, double time_in_seconds) { @@ -468,7 +468,7 @@ bool SafeTrajectoryFollower::execute(const std::array &positions, return server_.write(buf, strlen(formatted_message) + 1, written); } -bool SafeTrajectoryFollower::execute(std::vector &trajectory, std::atomic &interrupt) +bool LowBandwidthTrajectoryFollower::execute(std::vector &trajectory, std::atomic &interrupt) { if (!running_) return false; @@ -505,7 +505,7 @@ bool SafeTrajectoryFollower::execute(std::vector &trajectory, s return res; } -void SafeTrajectoryFollower::stop() +void LowBandwidthTrajectoryFollower::stop() { if (!running_) return; diff --git a/src/ros_main.cpp b/src/ros_main.cpp index 484cdd14c..523f1d21d 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -13,7 +13,7 @@ #include "ur_modern_driver/ros/rt_publisher.h" #include "ur_modern_driver/ros/service_stopper.h" #include "ur_modern_driver/ros/trajectory_follower.h" -#include "ur_modern_driver/ros/safe_trajectory_follower.h" +#include "ur_modern_driver/ros/lowbandwidth_trajectory_follower.h" #include "ur_modern_driver/ros/urscript_handler.h" #include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/factory.h" @@ -26,7 +26,7 @@ static const std::string IP_ADDR_ARG("~robot_ip_address"); static const std::string REVERSE_PORT_ARG("~reverse_port"); static const std::string ROS_CONTROL_ARG("~use_ros_control"); -static const std::string SAFE_TRAJECTORY_FOLLOWER("~use_safe_trajectory_follower"); +static const std::string LOW_BANDWIDTH_TRAJECTORY_FOLLOWER("~use_lowbandwidth_trajectory_follower"); static const std::string MAX_VEL_CHANGE_ARG("~max_vel_change"); static const std::string PREFIX_ARG("~prefix"); static const std::string BASE_FRAME_ARG("~base_frame"); @@ -55,7 +55,7 @@ struct ProgArgs double max_vel_change; int32_t reverse_port; bool use_ros_control; - bool use_safe_trajectory_follower; + bool use_lowbandwidth_trajectory_follower; bool shutdown_on_disconnect; }; @@ -95,7 +95,7 @@ bool parse_args(ProgArgs &args) ros::param::param(MAX_VEL_CHANGE_ARG, args.max_vel_change, 15.0); // rad/s ros::param::param(MAX_VEL_CHANGE_ARG, args.max_velocity, 10.0); ros::param::param(ROS_CONTROL_ARG, args.use_ros_control, false); - ros::param::param(SAFE_TRAJECTORY_FOLLOWER, args.use_safe_trajectory_follower, false); + ros::param::param(LOW_BANDWIDTH_TRAJECTORY_FOLLOWER, args.use_lowbandwidth_trajectory_follower, false); ros::param::param(PREFIX_ARG, args.prefix, std::string()); ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link"); ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller"); @@ -140,10 +140,10 @@ int main(int argc, char **argv) TrajectoryFollowerInterface *traj_follower(nullptr); - if (args.use_safe_trajectory_follower && !args.use_ros_control) + if (args.use_lowbandwidth_trajectory_follower && !args.use_ros_control) { - LOG_INFO("Use safe trajectory follower"); - traj_follower = new SafeTrajectoryFollower(*rt_commander, local_ip, args.reverse_port,factory.isVersion3()); + LOG_INFO("Use low bandwidth trajectory follower"); + traj_follower = new LowBandwidthTrajectoryFollower(*rt_commander, local_ip, args.reverse_port,factory.isVersion3()); // We are leaking the follower here, but it's OK as this is once-a-lifetime event } else From 0611afcf40b494e39ab418eebb9c1700d14fbac6 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Wed, 17 Jan 2018 19:01:01 +0100 Subject: [PATCH 097/114] Create issue_template.md --- .github/issue_template.md | 37 +++++++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) create mode 100644 .github/issue_template.md diff --git a/.github/issue_template.md b/.github/issue_template.md new file mode 100644 index 000000000..956bb03e0 --- /dev/null +++ b/.github/issue_template.md @@ -0,0 +1,37 @@ +Your issue may already be reported! +Please search on the [issue track](../) before creating one. + +## Expected Behavior + + + +## Current Behavior + + + +## Possible Solution + + + +## Steps to Reproduce (for bugs) + + +1. +2. +3. +4. + +## Context + + + +## Your Environment + +* Version used: +* ROS version: +* Operating System and version: + +## Log file +``` +Paste your DEBUG logfile content here +``` From 42b336bbf58d744b0c2d500f246151ed8125dc0a Mon Sep 17 00:00:00 2001 From: Jarek Potiuk Date: Sat, 27 Jan 2018 13:14:38 +0100 Subject: [PATCH 098/114] Faster loading speed and better precision for LBTF Low Bandwidth Trajectory Follower has now much less comments and debugging information - which means that it will parse on UR robot in around 400 ms instead of 700 ms. It also has an adjustment loop in case the robot does not reach any of the trajectory points specified. It will catch-up at every trajectory point sent by MoveIt and will try to get as close as possible to the desired pointi (it will check if all joints are within MAX_JOINT_DIFFERENCE from the desired positions) --- README.md | 4 +- .../ros/lowbandwidth_trajectory_follower.h | 2 +- launch/ur_common.launch | 2 + src/ros/lowbandwidth_trajectory_follower.cpp | 169 +++--------------- 4 files changed, 24 insertions(+), 153 deletions(-) diff --git a/README.md b/README.md index ec171c78c..a48c59173 100644 --- a/README.md +++ b/README.md @@ -55,10 +55,8 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design * **servoj_time** - time interval (real time) for which each interpolation step controls the robot (using servoj command). See below on examples of setting different time parameters. Default value is 0.008 (corresponds to expected 125Hz frequency of UR robot control) * **servoj_time_waiting** - time in seconds (real time) of internal active loop while the robot waits for new instructions in case of delays in communication. The smaller value, the faster robot restarts move after delay (but more stress is put on URControl processor). Default value is 0.001 (1000 Hz check frequency) * **max_waiting_time** - maximum time in seconds (real time) to wait for instructions from the drive before move is aborted. Defaults to 2 seconds. - * **debug** - displays (visible in the log of Polyscope/pendant and URControl PC) helpful information about trajectory points messages exchanged Driver <-> URScript. It is safe to be run with real robot and introduces very small overhead on the execution of the moves - * **more_debug** - displays even more debug information in the logs of Polyscope - details about interpolation loop calculations and execution. It adds a lot of overhead and is not recommended to be used with real robot (It's quite OK to run it with URSim though) * **servoj_gain** and **servoj_lookahead_time** - useful to control precision and speed of the position moves with servoj command (see URScript documentation for detailes) - + * **max_joint_difference** - maximum allowed difference between target and actual joints - checked at every trajectory step Here are some examples of manipulating the time flow for **Low Bandwidth Trajectory Follower** mode. You can use other settings but you should do it on your own risk. * Default mode: *servoj_time* = 0.008, *time_interval* = 0.008 : interpolation time flows with the same speed as real time - moves are executed as planned * Slow-motion mode: *servoj_time* = 0.008, *time_interval* = 0.004 : interpolation time flows 2x slower than real time, so the move is executed 2x slower than planned. Requires configuring MoveIt to accept much slower moves than expected (otherwise MoveIt cancels such move mid-way) diff --git a/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h b/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h index bd52ee4f2..003d07155 100644 --- a/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h +++ b/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h @@ -22,7 +22,7 @@ class LowBandwidthTrajectoryFollower: public TrajectoryFollowerInterface URServer server_; double time_interval_, servoj_time_, servoj_time_waiting_, max_waiting_time_, \ - servoj_gain_, servoj_lookahead_time_; + servoj_gain_, servoj_lookahead_time_, max_joint_difference_; bool debug_, more_debug_; std::string program_; diff --git a/launch/ur_common.launch b/launch/ur_common.launch index ad77d4161..2387b00dc 100644 --- a/launch/ur_common.launch +++ b/launch/ur_common.launch @@ -22,6 +22,7 @@ + @@ -52,6 +53,7 @@ + diff --git a/src/ros/lowbandwidth_trajectory_follower.cpp b/src/ros/lowbandwidth_trajectory_follower.cpp index 1e628b270..c3cdd9f14 100644 --- a/src/ros/lowbandwidth_trajectory_follower.cpp +++ b/src/ros/lowbandwidth_trajectory_follower.cpp @@ -9,65 +9,41 @@ static const std::string TIME_INTERVAL("{{TIME_INTERVAL}}"); static const std::string SERVOJ_TIME("{{SERVOJ_TIME}}"); static const std::string SERVOJ_TIME_WAITING("{{SERVOJ_TIME_WAITING}}"); static const std::string MAX_WAITING_TIME("{{MAX_WAITING_TIME}}"); -static const std::string DEBUG("{{DEBUG}}"); -static const std::string MORE_DEBUG("{{MORE_DEBUG}}"); static const std::string SERVOJ_GAIN("{{SERVOJ_GAIN}}"); static const std::string SERVOJ_LOOKAHEAD_TIME("{{SERVOJ_LOOKAHEAD_TIME}}"); static const std::string REVERSE_IP("{{REVERSE_IP}}"); static const std::string REVERSE_PORT("{{REVERSE_PORT}}"); +static const std::string MAX_JOINT_DIFFERENCE("{{MAX_JOINT_DIFFERENCE}}"); static const std::string POSITION_PROGRAM = R"( def driveRobotLowBandwidthTrajectory(): - global JOINT_NUM = 6 global TIME_INTERVAL = {{TIME_INTERVAL}} global SERVOJ_TIME = {{SERVOJ_TIME}} global SERVOJ_TIME_WAITING = {{SERVOJ_TIME_WAITING}} global MAX_WAITING_TIME = {{MAX_WAITING_TIME}} - global DEBUG = {{DEBUG}} - global MORE_DEBUG = {{MORE_DEBUG}} global EMPTY_VALUES = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] global SERVOJ_GAIN = {{SERVOJ_GAIN}} global SERVOJ_LOOKAHEAD_TIME = {{SERVOJ_LOOKAHEAD_TIME}} global CONNECTION_NAME = "reverse_connection" global REVERSE_IP = "{{REVERSE_IP}}" global REVERSE_PORT = {{REVERSE_PORT}} - - # NOTE All the global variables here are accessed by different threads - # therefore they should be accessed within critical section. Those variables - # are all prefixed with g_ . Whenever their values are needed they are copied - # to similarly named l_ variable. Copying happens inside the critical section - # and l_values might be used outside of it. This needs to be confirmed with UR - # about the semantics of assignment operator (copying or by reference?). - # Hopefully it's copying :). - # - # Please make sure to keep that pattern and do not access the global variables - # outside of the critical section - # - # TO DO: Are those assignments by references or copies? We will find out soon - # If not then assigning positions would not work either + global MAX_JOINT_DIFFERENCE = {{MAX_JOINT_DIFFERENCE}} global g_position_previous = EMPTY_VALUES global g_position_target = EMPTY_VALUES global g_position_next = EMPTY_VALUES - global g_velocity_previous = EMPTY_VALUES global g_velocity_target = EMPTY_VALUES global g_velocity_next = EMPTY_VALUES - global g_time_previous = 0.0 global g_time_target = 0.0 global g_time_next = 0.0 - global g_num_previous = -1 global g_num_target = -1 global g_num_next = -1 - global g_received_waypoints_number = -1 global g_requested_waypoints_number = -1 - global g_total_elapsed_time = 0 - global g_stopping = False - def send_message(message): socket_send_string(message, CONNECTION_NAME) socket_send_byte(10, CONNECTION_NAME) @@ -75,7 +51,7 @@ def driveRobotLowBandwidthTrajectory(): def is_waypoint_sentinel(waypoint): local l_previous_index = 2 - while l_previous_index < 1 + JOINT_NUM * 2 + 2: # Note - we do not check first two which are non-zero + while l_previous_index < 1 + JOINT_NUM * 2 + 2: if waypoint[l_previous_index] != 0.0: return False end @@ -84,6 +60,18 @@ def driveRobotLowBandwidthTrajectory(): return True end + def is_final_position_reached(position): + local l_current_position = get_actual_joint_positions() + local l_index = 0 + while l_index < JOINT_NUM: + if norm(position[l_index] - l_current_position[l_index]) > MAX_JOINT_DIFFERENCE: + return False + end + l_index = l_index + 1 + end + return True + end + def interpolate(time_within_segment, total_segment_time, start_pos, l_end_pos, l_start_vel, end_vel): local a = start_pos local b = l_start_vel @@ -94,96 +82,48 @@ def driveRobotLowBandwidthTrajectory(): def add_next_waypoint(waypoint): enter_critical - - # Rotate the values received so far: - # target -> previous g_position_previous = g_position_target g_velocity_previous = g_velocity_target g_time_previous = g_time_target g_num_previous = g_num_target - - # next -> previous g_position_target = g_position_next g_velocity_target = g_velocity_next g_time_target = g_time_next g_num_target = g_num_next - - # Decode the array received into next - # waypoint 0 is number of entries in the received array g_num_next = waypoint[1] g_position_next = [waypoint[2], waypoint[3], waypoint[4], waypoint[5], waypoint[6], waypoint[7]] g_velocity_next = [waypoint[8], waypoint[9], waypoint[10], waypoint[11], waypoint[12], waypoint[13]] g_time_next = waypoint[14] - - # store latest received waypoint number so that controlling thread knows it's been received already g_received_waypoints_number = g_num_next - - if DEBUG: - local l_received_waypoints_number = g_received_waypoints_number - local l_waypoint = waypoint - end - exit_critical - - if DEBUG: - textmsg("Received waypoint:") - textmsg(l_received_waypoints_number) - textmsg(l_waypoint) - end end - # Thread controlling the motor. In the loop it checks first if it received the - # requested waypoints and until it does, it syncs doing noting to the motor - # once it received all up to requested waypoints it executes interpolation - # between PREVIOUS AND TARGET points received and rquests the next waypoint request - # to be sent. thread controllingThread(): - local l_received_waypoints_number = -1 local l_requested_waypoints_number = -1 local l_stopped = False local l_current_position = get_actual_joint_positions() - enter_critical g_requested_waypoints_number = 2 exit_critical - while True: - enter_critical l_requested_waypoints_number = g_requested_waypoints_number - l_received_waypoints_number = g_received_waypoints_number exit_critical - local l_max_waiting_time_left = MAX_WAITING_TIME - # if expected waypoint number not yet received wait so that receiving thread has time to receive it while l_received_waypoints_number < l_requested_waypoints_number and l_max_waiting_time_left > 0: - - if DEBUG: - textmsg("Waiting for the received waypoints number to catch up (received/requested):") - textmsg(l_received_waypoints_number) - textmsg(l_requested_waypoints_number) - end - # Keep robot in l_current position for short time and check if the next waipoint arrived servoj(l_current_position,t=SERVOJ_TIME_WAITING,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN) - enter_critical l_received_waypoints_number = g_received_waypoints_number exit_critical - l_max_waiting_time_left = l_max_waiting_time_left - SERVOJ_TIME_WAITING - end - if l_max_waiting_time_left <= 0: textmsg("Closing the connection on waiting too long.") socket_close(CONNECTION_NAME) halt end - - # OK. We received next point, copy the required global variables into local ones enter_critical - local l_start_pos = g_position_previous local l_start_vel = g_velocity_previous local l_start_time = g_time_previous @@ -193,81 +133,39 @@ def driveRobotLowBandwidthTrajectory(): local l_end_time = g_time_target local l_end_num = g_num_target local l_total_elapsed_time = g_total_elapsed_time - - # Note we deliberately only read "stopping" state here and not update it below - # so that stopping flag takes effect only after one additional interpolation loop is complete - # and all points of the trajectory are processeed local l_stopping_after_next_interpolation = g_stopping - - # And increasing the global requested number - informs sender thread that it needs to ask for it g_requested_waypoints_number = g_requested_waypoints_number + 1 exit_critical - if DEBUG: - textmsg("Starting interpolation. Segment from/to:") - textmsg(l_start_num) - textmsg(l_end_num) - textmsg("Current time:") - textmsg(l_total_elapsed_time) - textmsg("Starting/Ending segment time:") - textmsg(l_start_time) - textmsg(l_end_time) - end - l_current_position = l_start_pos local l_total_segment_time = l_end_time - l_start_time - # Here perform the interpolation loop while l_total_elapsed_time <= l_end_time: - if MORE_DEBUG: - textmsg("Next step of interpolation:") - end - local l_segment_elapsed_time = l_total_elapsed_time - l_start_time - - # Calculate interpolation for all joints j = 0 while j < JOINT_NUM: l_current_position[j] = interpolate(l_segment_elapsed_time, l_total_segment_time, l_start_pos[j], l_end_pos[j], l_start_vel[j], l_end_vel[j]) j = j + 1 end - if MORE_DEBUG: - textmsg("Next step of interpolated position:") - textmsg(l_current_position) - textmsg("Current time:") - textmsg(l_total_elapsed_time) - textmsg("Running servoj command:") - end - servoj(l_current_position,t=SERVOJ_TIME,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN) - enter_critical g_total_elapsed_time = g_total_elapsed_time + TIME_INTERVAL l_total_elapsed_time = g_total_elapsed_time exit_critical - if MORE_DEBUG: - textmsg("Finishing interpolation step at time:") - textmsg(l_total_elapsed_time) - end end - if DEBUG: - textmsg("Ending interpolation segment at time:") - textmsg(l_total_elapsed_time) + while not is_final_position_reached(l_current_position): + servoj(l_current_position,t=SERVOJ_TIME,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN) end if l_stopping_after_next_interpolation: textmsg("Stopping the controlling thread on signal from receiving thread after one interpolation loop.") break end end - textmsg("Ending controlling thread") end - # This thread sends requested waypoints number to the client when requested number is changed - # It will send all the numbers from [already sent + 1, g_requested_waypoints_number] and waits - # until requested waypoints number increases thread sendingThread(): local controlling_thread = run controllingThread() local l_sent_waypoints_number = -1 @@ -279,7 +177,6 @@ def driveRobotLowBandwidthTrajectory(): l_stopping = g_stopping exit_critical while not l_stopping: - # wait until we have more requested waypoints to send than actually sent ones while l_sent_waypoints_number == l_requested_waypoints_number and not l_stopping: sleep(SERVOJ_TIME_WAITING) @@ -294,65 +191,42 @@ def driveRobotLowBandwidthTrajectory(): end send_message(l_sent_waypoints_number + 1) l_sent_waypoints_number = l_sent_waypoints_number + 1 - if DEBUG: - textmsg("Sent waypoint request number:") - textmsg(l_sent_waypoints_number) - end end - textmsg("Joining controlling thread") join controlling_thread - textmsg("Ending Sending thread") end - # Receiving thread - it will receive the next trajectory point over the TCP connection - # It will increase the received waipoints_number on each request. thread receivingThread(): local sending_thread = run sendingThread() while True: waypoint_received = socket_read_ascii_float(14, CONNECTION_NAME) if waypoint_received[0] == 0: - # No new waypoint requested for the last 2 seconds textmsg("Not received trajectory for the last 2 seconds. Quitting") enter_critical g_stopping = True exit_critical - break elif waypoint_received[0] != JOINT_NUM * 2 + 2: textmsg("Received wrong number of floats in trajectory. This is certainly not OK.") textmsg(waypoint_received[0]) - enter_critical g_stopping = True exit_critical - break elif is_waypoint_sentinel(waypoint_received): add_next_waypoint(waypoint_received) - textmsg("Received sentinel waypoint. Finishing.") - enter_critical g_stopping = True g_received_waypoints_number = g_received_waypoints_number + 1 exit_critical - break end add_next_waypoint(waypoint_received) end - textmsg("Joining sendingThread") join sending_thread - textmsg("Ending Receiving thread") end - - textmsg("Opening socket") socket_open(REVERSE_IP, REVERSE_PORT, CONNECTION_NAME) - textmsg("Socket opened") - receiving_thread = run receivingThread() - textmsg("Joining receiving_thread") join receiving_thread - textmsg("Closing reverse connection") socket_close(CONNECTION_NAME) textmsg("Exiting the program") end @@ -370,8 +244,7 @@ LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander &comm , max_waiting_time_(2.0) , servoj_gain_(300.0) , servoj_lookahead_time_(0.03) - , debug_(false) - , more_debug_(false) + , max_joint_difference_(0.01) { ros::param::get("~time_interval", time_interval_); ros::param::get("~servoj_time", servoj_time_); @@ -379,8 +252,7 @@ LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander &comm ros::param::get("~max_waiting_time", max_waiting_time_); ros::param::get("~servoj_gain", servoj_gain_); ros::param::get("~servoj_lookahead_time", servoj_lookahead_time_); - ros::param::get("~debug", debug_); - ros::param::get("~more_debug", more_debug_); + ros::param::get("~max_joint_difference", max_joint_difference_); std::string res(POSITION_PROGRAM); std::ostringstream out; @@ -394,10 +266,9 @@ LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander &comm res.replace(res.find(MAX_WAITING_TIME), MAX_WAITING_TIME.length(), std::to_string(max_waiting_time_)); res.replace(res.find(SERVOJ_GAIN), SERVOJ_GAIN.length(), std::to_string(servoj_gain_)); res.replace(res.find(SERVOJ_LOOKAHEAD_TIME), SERVOJ_LOOKAHEAD_TIME.length(), std::to_string(servoj_lookahead_time_)); - res.replace(res.find(DEBUG), DEBUG.length(), debug_ ? "True" : "False"); - res.replace(res.find(MORE_DEBUG), MORE_DEBUG.length(), more_debug_ ? "True" : "False"); res.replace(res.find(REVERSE_IP), REVERSE_IP.length(), reverse_ip); res.replace(res.find(REVERSE_PORT), REVERSE_PORT.length(), std::to_string(reverse_port)); + res.replace(res.find(MAX_JOINT_DIFFERENCE), MAX_JOINT_DIFFERENCE.length(), std::to_string(max_joint_difference_)); program_ = res; if (!server_.bind()) From 2ace46d824754eebda5c295258c7967fc5ac30ca Mon Sep 17 00:00:00 2001 From: Jarek Potiuk Date: Sun, 4 Feb 2018 22:38:11 +0100 Subject: [PATCH 099/114] Bring all parameters in example launch files to common/latest parameters --- launch/ur10_bringup.launch | 25 ++++++++++++++++++++-- launch/ur10_bringup_compatible.launch | 27 +++++++++++++++++++++--- launch/ur10_bringup_joint_limited.launch | 24 +++++++++++++++++++-- launch/ur3_bringup.launch | 20 ++++++++++++++++++ launch/ur3_bringup_joint_limited.launch | 20 ++++++++++++++++++ launch/ur5_bringup.launch | 26 ++++++++++++++++++++--- launch/ur5_bringup_compatible.launch | 27 +++++++++++++++++++++--- launch/ur5_bringup_joint_limited.launch | 20 ++++++++++++++++++ 8 files changed, 176 insertions(+), 13 deletions(-) diff --git a/launch/ur10_bringup.launch b/launch/ur10_bringup.launch index f27efb36c..34582d9c2 100644 --- a/launch/ur10_bringup.launch +++ b/launch/ur10_bringup.launch @@ -7,24 +7,45 @@ ur10_bringup.launch robot_ip:= --> - + + + + + + + + + + + - + + + + + + + + + + + + diff --git a/launch/ur10_bringup_compatible.launch b/launch/ur10_bringup_compatible.launch index 44387a43f..45e0ebdb5 100644 --- a/launch/ur10_bringup_compatible.launch +++ b/launch/ur10_bringup_compatible.launch @@ -7,25 +7,46 @@ ur10_bringup.launch robot_ip:= --> - + + + + + + + + + + + + - + - + + + + + + + + + + + diff --git a/launch/ur10_bringup_joint_limited.launch b/launch/ur10_bringup_joint_limited.launch index 1ca2d8b5f..756807b0d 100644 --- a/launch/ur10_bringup_joint_limited.launch +++ b/launch/ur10_bringup_joint_limited.launch @@ -2,17 +2,27 @@ - + + + + + + + + + + + @@ -21,6 +31,16 @@ + + + + + + + + + + diff --git a/launch/ur3_bringup.launch b/launch/ur3_bringup.launch index f4bc006b0..22e426bce 100644 --- a/launch/ur3_bringup.launch +++ b/launch/ur3_bringup.launch @@ -14,6 +14,16 @@ + + + + + + + + + + @@ -26,6 +36,16 @@ + + + + + + + + + + diff --git a/launch/ur3_bringup_joint_limited.launch b/launch/ur3_bringup_joint_limited.launch index 9c70c524f..5f994f1d6 100644 --- a/launch/ur3_bringup_joint_limited.launch +++ b/launch/ur3_bringup_joint_limited.launch @@ -13,6 +13,16 @@ + + + + + + + + + + @@ -21,6 +31,16 @@ + + + + + + + + + + diff --git a/launch/ur5_bringup.launch b/launch/ur5_bringup.launch index 303d253ab..eafad382e 100644 --- a/launch/ur5_bringup.launch +++ b/launch/ur5_bringup.launch @@ -7,25 +7,45 @@ ur5_bringup.launch robot_ip:= --> - + + + + + + + + + + + - + - + + + + + + + + + + + diff --git a/launch/ur5_bringup_compatible.launch b/launch/ur5_bringup_compatible.launch index c8383bff2..ddd0b9267 100644 --- a/launch/ur5_bringup_compatible.launch +++ b/launch/ur5_bringup_compatible.launch @@ -7,25 +7,46 @@ ur5_bringup.launch robot_ip:= --> - + + + + + + + + + + + + - + - + + + + + + + + + + + diff --git a/launch/ur5_bringup_joint_limited.launch b/launch/ur5_bringup_joint_limited.launch index 3640a2d7f..09a0022c8 100644 --- a/launch/ur5_bringup_joint_limited.launch +++ b/launch/ur5_bringup_joint_limited.launch @@ -13,6 +13,16 @@ + + + + + + + + + + @@ -21,6 +31,16 @@ + + + + + + + + + + From b4718ab4d251e5b564e85feb8ccc5185a8dd4780 Mon Sep 17 00:00:00 2001 From: Jarek Potiuk Date: Sun, 4 Feb 2018 22:39:07 +0100 Subject: [PATCH 100/114] Only do adjustment of position at the very end of the move --- src/ros/lowbandwidth_trajectory_follower.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/ros/lowbandwidth_trajectory_follower.cpp b/src/ros/lowbandwidth_trajectory_follower.cpp index c3cdd9f14..75c47f790 100644 --- a/src/ros/lowbandwidth_trajectory_follower.cpp +++ b/src/ros/lowbandwidth_trajectory_follower.cpp @@ -148,7 +148,6 @@ def driveRobotLowBandwidthTrajectory(): l_current_position[j] = interpolate(l_segment_elapsed_time, l_total_segment_time, l_start_pos[j], l_end_pos[j], l_start_vel[j], l_end_vel[j]) j = j + 1 end - servoj(l_current_position,t=SERVOJ_TIME,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN) enter_critical g_total_elapsed_time = g_total_elapsed_time + TIME_INTERVAL @@ -156,11 +155,12 @@ def driveRobotLowBandwidthTrajectory(): exit_critical end - while not is_final_position_reached(l_current_position): - servoj(l_current_position,t=SERVOJ_TIME,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN) - end if l_stopping_after_next_interpolation: - textmsg("Stopping the controlling thread on signal from receiving thread after one interpolation loop.") + while not is_final_position_reached(l_end_pos): + textmsg("Catching up on final position not reached first time.") + servoj(l_end_pos,t=SERVOJ_TIME,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN) + end + textmsg("Finishing the controlling thread. Final position reached.") break end end From 8f5df0145f1770cd48c32c353213a3f7a384c0e9 Mon Sep 17 00:00:00 2001 From: Jarek Potiuk Date: Sun, 4 Feb 2018 23:21:01 +0100 Subject: [PATCH 101/114] Removed unused debug/more_debug options --- .../ur_modern_driver/ros/lowbandwidth_trajectory_follower.h | 1 - launch/ur_common.launch | 4 ---- 2 files changed, 5 deletions(-) diff --git a/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h b/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h index 003d07155..7b1f610e6 100644 --- a/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h +++ b/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h @@ -23,7 +23,6 @@ class LowBandwidthTrajectoryFollower: public TrajectoryFollowerInterface double time_interval_, servoj_time_, servoj_time_waiting_, max_waiting_time_, \ servoj_gain_, servoj_lookahead_time_, max_joint_difference_; - bool debug_, more_debug_; std::string program_; diff --git a/launch/ur_common.launch b/launch/ur_common.launch index 2387b00dc..37eea204e 100644 --- a/launch/ur_common.launch +++ b/launch/ur_common.launch @@ -18,8 +18,6 @@ - - @@ -49,8 +47,6 @@ - - From 992c661e9e65435bcede95f5521259f58c614ef1 Mon Sep 17 00:00:00 2001 From: Jarek Potiuk Date: Sun, 4 Feb 2018 23:48:51 +0100 Subject: [PATCH 102/114] Removed unused append method --- .../ros/lowbandwidth_trajectory_follower.h | 8 -------- 1 file changed, 8 deletions(-) diff --git a/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h b/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h index 7b1f610e6..e95800262 100644 --- a/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h +++ b/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h @@ -26,14 +26,6 @@ class LowBandwidthTrajectoryFollower: public TrajectoryFollowerInterface std::string program_; - template - size_t append(uint8_t *buffer, T &val) - { - size_t s = sizeof(T); - std::memcpy(buffer, &val, s); - return s; - } - bool execute(const std::array &positions, const std::array &velocities, double sample_number, double time_in_seconds); From db61edfe5ba98df403bbd59624aa62fa988c1551 Mon Sep 17 00:00:00 2001 From: Jarek Potiuk Date: Mon, 5 Feb 2018 00:03:48 +0100 Subject: [PATCH 103/114] Remove added warning options. Those warning options are separated out to #8 pull request so there is no point in adding them here as well. --- CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a6a5728c6..bc43d3fdf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -137,8 +137,6 @@ endif() add_compile_options(-Wall) add_compile_options(-Wextra) add_compile_options(-Wno-unused-parameter) -add_compile_options(-Wno-ignored-qualifiers) -add_compile_options(-Wno-return-type) # support indigo's ros_control - This can be removed upon EOL indigo if("${controller_manager_msgs_VERSION}" VERSION_LESS "0.10.0") From d7d84caee79513fbd1da44eb2b3c8a8ce9f13f8d Mon Sep 17 00:00:00 2001 From: Miguel Prada Date: Mon, 12 Feb 2018 23:04:00 +0100 Subject: [PATCH 104/114] Add time parameter to speedj for UR software >= 3.3 Fixes #15. Related to https://github.com/ThomasTimm/ur_modern_driver/issues/92 --- include/ur_modern_driver/ur/commander.h | 22 ++++++++++++++++- include/ur_modern_driver/ur/factory.h | 4 +++- src/ur/commander.cpp | 32 +++++++++++++++++-------- 3 files changed, 46 insertions(+), 12 deletions(-) diff --git a/include/ur_modern_driver/ur/commander.h b/include/ur_modern_driver/ur/commander.h index 6a2b48620..0a4cf6f71 100644 --- a/include/ur_modern_driver/ur/commander.h +++ b/include/ur_modern_driver/ur/commander.h @@ -49,7 +49,27 @@ class URCommander_V3_X : public URCommander { } - virtual bool speedj(std::array &speeds, double acceleration); + virtual bool speedj(std::array &speeds, double acceleration) = 0; virtual bool setDigitalOut(uint8_t pin, bool value); virtual bool setAnalogOut(uint8_t pin, double value); }; + +class URCommander_V3_1__2 : public URCommander_V3_X +{ +public: + URCommander_V3_1__2(URStream &stream) : URCommander_V3_X(stream) + { + } + + virtual bool speedj(std::array &speeds, double acceleration); +}; + +class URCommander_V3_3 : public URCommander_V3_X +{ +public: + URCommander_V3_3(URStream &stream) : URCommander_V3_X(stream) + { + } + + virtual bool speedj(std::array &speeds, double acceleration); +}; diff --git a/include/ur_modern_driver/ur/factory.h b/include/ur_modern_driver/ur/factory.h index 54c6cdfda..1f1c1ef8e 100644 --- a/include/ur_modern_driver/ur/factory.h +++ b/include/ur_modern_driver/ur/factory.h @@ -80,8 +80,10 @@ class URFactory : private URMessagePacketConsumer { if (major_version_ == 1) return std::unique_ptr(new URCommander_V1_X(stream)); + else if (minor_version_ < 3) + return std::unique_ptr(new URCommander_V3_1__2(stream)); else - return std::unique_ptr(new URCommander_V3_X(stream)); + return std::unique_ptr(new URCommander_V3_3(stream)); } std::unique_ptr> getStateParser() diff --git a/src/ur/commander.cpp b/src/ur/commander.cpp index 5c752f07f..9b85865c4 100644 --- a/src/ur/commander.cpp +++ b/src/ur/commander.cpp @@ -86,16 +86,6 @@ bool URCommander_V1_X::setDigitalOut(uint8_t pin, bool value) return write(s); } -bool URCommander_V3_X::speedj(std::array &speeds, double acceleration) -{ - std::ostringstream out; - out << std::fixed << std::setprecision(5); - out << "speedj("; - formatArray(out, speeds); - out << "," << acceleration << ")\n"; - std::string s(out.str()); - return write(s); -} bool URCommander_V3_X::setAnalogOut(uint8_t pin, double value) { std::ostringstream out; @@ -130,3 +120,25 @@ bool URCommander_V3_X::setDigitalOut(uint8_t pin, bool value) std::string s(out.str()); return write(s); } + +bool URCommander_V3_1__2::speedj(std::array &speeds, double acceleration) +{ + std::ostringstream out; + out << std::fixed << std::setprecision(5); + out << "speedj("; + formatArray(out, speeds); + out << "," << acceleration << ")\n"; + std::string s(out.str()); + return write(s); +} + +bool URCommander_V3_3::speedj(std::array &speeds, double acceleration) +{ + std::ostringstream out; + out << std::fixed << std::setprecision(5); + out << "speedj("; + formatArray(out, speeds); + out << "," << acceleration << "," << 0.008 << ")\n"; + std::string s(out.str()); + return write(s); +} From a2137b4762a69b056da4ec06233b199daaef4439 Mon Sep 17 00:00:00 2001 From: Miguel Prada Date: Thu, 15 Feb 2018 09:28:37 +0100 Subject: [PATCH 105/114] Replace ros_controllers metapackage dependency for controller packages --- package.xml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 7e2ac0c59..d58d4a92d 100644 --- a/package.xml +++ b/package.xml @@ -54,7 +54,9 @@ std_srvs hardware_interface controller_manager - ros_controllers + force_torque_sensor_controller + joint_state_controller + joint_trajectory_controller actionlib control_msgs geometry_msgs From 24d4406c91f417c2d7c25e15548a760aabcba526 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Thu, 1 Mar 2018 10:07:44 +0100 Subject: [PATCH 106/114] Fixed compatability issue Pre-refactor version of URScript topic used to automatically append newline when not the last character of the message however that was not the case for the new version. --- src/ros/urscript_handler.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/ros/urscript_handler.cpp b/src/ros/urscript_handler.cpp index c2e5fe7eb..3b5dafdce 100644 --- a/src/ros/urscript_handler.cpp +++ b/src/ros/urscript_handler.cpp @@ -13,10 +13,14 @@ URScriptHandler::URScriptHandler(URCommander &commander) void URScriptHandler::urscriptInterface(const std_msgs::String::ConstPtr& msg) { LOG_INFO("Message received"); + std::string str(msg->data); + if (str.back() != '\n') + str.append('\n'); + switch (state_) { case RobotState::Running: - if (!commander_.uploadProg(msg->data)) + if (!commander_.uploadProg(str)) { LOG_ERROR("Program upload failed!"); } From 7eaa51839a52bbea865e35f490762ff00d4939d1 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Thu, 1 Mar 2018 10:58:16 +0100 Subject: [PATCH 107/114] Broken build fix, oops! --- src/ros/urscript_handler.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ros/urscript_handler.cpp b/src/ros/urscript_handler.cpp index 3b5dafdce..565bf92b0 100644 --- a/src/ros/urscript_handler.cpp +++ b/src/ros/urscript_handler.cpp @@ -15,7 +15,7 @@ void URScriptHandler::urscriptInterface(const std_msgs::String::ConstPtr& msg) LOG_INFO("Message received"); std::string str(msg->data); if (str.back() != '\n') - str.append('\n'); + str.append("\n"); switch (state_) { From a9530c858617c6e54085f6718fca0f815b2965da Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?J=C3=B8rgen=20Borgesen?= Date: Tue, 20 Mar 2018 21:34:13 +0100 Subject: [PATCH 108/114] Add robot_state_publisher as a dependency (#20) Adds robot_state_publisher to package.xml. The package is used in launch files and should be defined as a dependency. --- package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/package.xml b/package.xml index d58d4a92d..828927d14 100644 --- a/package.xml +++ b/package.xml @@ -68,6 +68,7 @@ ur_description tf std_srvs + robot_state_publisher rosunit From 71bca7abf7e476b0814e7254ca5701ca1c00a3b1 Mon Sep 17 00:00:00 2001 From: Jarek Potiuk Date: Sat, 24 Feb 2018 13:33:04 +0100 Subject: [PATCH 109/114] Refactored intialisation of ROSController/ActionServer --- include/ur_modern_driver/ros/action_server.h | 4 +-- ...=> action_trajectory_follower_interface.h} | 4 +-- .../ros/lowbandwidth_trajectory_follower.h | 4 +-- .../ros/trajectory_follower.h | 4 +-- src/ros/action_server.cpp | 2 +- src/ros_main.cpp | 32 +++++++++---------- 6 files changed, 24 insertions(+), 26 deletions(-) rename include/ur_modern_driver/ros/{trajectory_follower_interface.h => action_trajectory_follower_interface.h} (87%) diff --git a/include/ur_modern_driver/ros/action_server.h b/include/ur_modern_driver/ros/action_server.h index fe5056752..d741714f6 100644 --- a/include/ur_modern_driver/ros/action_server.h +++ b/include/ur_modern_driver/ros/action_server.h @@ -39,7 +39,7 @@ class ActionServer : public Service, public URRTPacketConsumer std::condition_variable tj_cv_; std::thread tj_thread_; - TrajectoryFollowerInterface& follower_; + ActionTrajectoryFollowerInterface& follower_; RobotState state_; std::array q_actual_, qd_actual_; @@ -61,7 +61,7 @@ class ActionServer : public Service, public URRTPacketConsumer bool updateState(RTShared& data); public: - ActionServer(TrajectoryFollowerInterface& follower, std::vector& joint_names, double max_velocity); + ActionServer(ActionTrajectoryFollowerInterface& follower, std::vector& joint_names, double max_velocity); void start(); virtual void onRobotStateChange(RobotState state); diff --git a/include/ur_modern_driver/ros/trajectory_follower_interface.h b/include/ur_modern_driver/ros/action_trajectory_follower_interface.h similarity index 87% rename from include/ur_modern_driver/ros/trajectory_follower_interface.h rename to include/ur_modern_driver/ros/action_trajectory_follower_interface.h index a25ddf7d9..9b5427fa2 100644 --- a/include/ur_modern_driver/ros/trajectory_follower_interface.h +++ b/include/ur_modern_driver/ros/action_trajectory_follower_interface.h @@ -22,11 +22,11 @@ struct TrajectoryPoint } }; -class TrajectoryFollowerInterface +class ActionTrajectoryFollowerInterface { public: virtual bool start() = 0; virtual bool execute(std::vector &trajectory, std::atomic &interrupt) = 0; virtual void stop() = 0; - virtual ~TrajectoryFollowerInterface() {}; + virtual ~ActionTrajectoryFollowerInterface() {}; }; diff --git a/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h b/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h index e95800262..6442dcb38 100644 --- a/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h +++ b/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h @@ -11,9 +11,9 @@ #include "ur_modern_driver/log.h" #include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/server.h" -#include "ur_modern_driver/ros/trajectory_follower_interface.h" +#include "ur_modern_driver/ros/action_trajectory_follower_interface.h" -class LowBandwidthTrajectoryFollower: public TrajectoryFollowerInterface +class LowBandwidthTrajectoryFollower: public ActionTrajectoryFollowerInterface { private: std::atomic running_; diff --git a/include/ur_modern_driver/ros/trajectory_follower.h b/include/ur_modern_driver/ros/trajectory_follower.h index 70dba5ce9..af3747403 100644 --- a/include/ur_modern_driver/ros/trajectory_follower.h +++ b/include/ur_modern_driver/ros/trajectory_follower.h @@ -11,9 +11,9 @@ #include "ur_modern_driver/log.h" #include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/server.h" -#include "ur_modern_driver/ros/trajectory_follower_interface.h" +#include "ur_modern_driver/ros/action_trajectory_follower_interface.h" -class TrajectoryFollower : public TrajectoryFollowerInterface +class TrajectoryFollower : public ActionTrajectoryFollowerInterface { private: std::atomic running_; diff --git a/src/ros/action_server.cpp b/src/ros/action_server.cpp index c8546c23e..8616c35c2 100644 --- a/src/ros/action_server.cpp +++ b/src/ros/action_server.cpp @@ -1,7 +1,7 @@ #include "ur_modern_driver/ros/action_server.h" #include -ActionServer::ActionServer(TrajectoryFollowerInterface& follower, std::vector& joint_names, double max_velocity) +ActionServer::ActionServer(ActionTrajectoryFollowerInterface& follower, std::vector& joint_names, double max_velocity) : as_(nh_, "follow_joint_trajectory", boost::bind(&ActionServer::onGoal, this, _1), boost::bind(&ActionServer::onCancel, this, _1), false) , joint_names_(joint_names) diff --git a/src/ros_main.cpp b/src/ros_main.cpp index 523f1d21d..5343334d5 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -138,35 +138,33 @@ int main(int argc, char **argv) auto rt_commander = factory.getCommander(rt_stream); vector *> rt_vec{ &rt_pub }; - TrajectoryFollowerInterface *traj_follower(nullptr); - - if (args.use_lowbandwidth_trajectory_follower && !args.use_ros_control) - { - LOG_INFO("Use low bandwidth trajectory follower"); - traj_follower = new LowBandwidthTrajectoryFollower(*rt_commander, local_ip, args.reverse_port,factory.isVersion3()); - // We are leaking the follower here, but it's OK as this is once-a-lifetime event - } - else - { - LOG_INFO("Use standard trajectory follower"); - traj_follower = new TrajectoryFollower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3()); - // We are leaking the follower here, but it's OK as this is once-a-lifetime event - } - INotifier *notifier(nullptr); ROSController *controller(nullptr); ActionServer *action_server(nullptr); if (args.use_ros_control) { LOG_INFO("ROS control enabled"); - // Note - we are sure that TrajectoryFollower is used here (see the args.use_ros_control above) - controller = new ROSController(*rt_commander, *((TrajectoryFollower *) traj_follower), args.joint_names, args.max_vel_change, args.tcp_link); + TrajectoryFollower *traj_follower = new TrajectoryFollower( + *rt_commander, local_ip, args.reverse_port, factory.isVersion3()); + controller = new ROSController(*rt_commander, *traj_follower, args.joint_names, args.max_vel_change, args.tcp_link); rt_vec.push_back(controller); services.push_back(controller); } else { LOG_INFO("ActionServer enabled"); + ActionTrajectoryFollowerInterface *traj_follower(nullptr); + if (args.use_lowbandwidth_trajectory_follower) + { + LOG_INFO("Use low bandwidth trajectory follower"); + traj_follower = new LowBandwidthTrajectoryFollower(*rt_commander, + local_ip, args.reverse_port,factory.isVersion3()); + } + else + { + LOG_INFO("Use standard trajectory follower"); + traj_follower = new TrajectoryFollower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3()); + } action_server = new ActionServer(*traj_follower, args.joint_names, args.max_velocity); rt_vec.push_back(action_server); services.push_back(action_server); From 924c3710414a3a50ca071ff9ee90df3d45be6031 Mon Sep 17 00:00:00 2001 From: Jarek Potiuk Date: Sat, 24 Feb 2018 20:45:26 +0100 Subject: [PATCH 110/114] Updated README.md to include duration monitoring remarks --- README.md | 40 +++++++++++++++++++++++++--------------- 1 file changed, 25 insertions(+), 15 deletions(-) diff --git a/README.md b/README.md index a48c59173..3682d70c6 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # ur_modern_driver - Refactored [![Build Status](https://travis-ci.org/Zagitta/ur_modern_driver.svg?branch=master)](https://travis-ci.org/Zagitta/ur_modern_driver) -A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is designed to replace the old driver transparently, while solving some issues, improving usability as well as enabling compatibility of ros_control. +A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is designed to replace the old driver transparently, while solving some issues, improving usability as well as enabling compatibility of ros_control. ## Improvements @@ -26,8 +26,8 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design * */joint\_speed* : Takes messages of type _trajectory\_msgs/JointTrajectory_. Parses the first JointTrajectoryPoint and sends the specified joint speeds and accelerations to the robot. This interface is intended for doing visual servoing and other kind of control that requires speed control rather than position control of the robot. Remember to set values for all 6 joints. Ignores the field joint\_names, so set the values in the correct order. -* Added support for ros_control. - * As ros_control wants to have control over the robot at all times, ros_control compatibility is set via a parameter at launch-time. +* Added support for ros_control. + * As ros_control wants to have control over the robot at all times, ros_control compatibility is set via a parameter at launch-time. * With ros_control active, the driver doesn't open the action_lib interface nor publish joint_states or wrench msgs. This is handled by ros_control instead. * Currently two controllers are available, both controlling the joint position of the robot, useable for trajectroy execution * The velocity based controller sends joint speed commands to the robot, using the speedj command @@ -39,7 +39,7 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design * Added activation modes: `Never`, `Always` and `OnStartup`. Sets wether a call to the `ur_driver/robot_enable` service is required at startup, at startup + on errors or never. Is intended as a safety feature to require some sort of manual intervention in case of driver crashes and robot safety faults. -* **Low Bandwidth Trajectory Follower** mode of execution. In this mode the real-time control loop for the robot is shifted from the client PC running the driver to URscript executed on the URControl PC of the robot with the following features: +* **Low Bandwidth Trajectory Follower** mode of execution. (only works when `ros_control` is set to `false`)In this mode the real-time control loop for the robot is shifted from the client PC running the driver to URscript executed on the URControl PC of the robot with the following features: * It works only with */follow\_joint\_trajectory* action for MoveIt integration. It is mutually exclusive with *ros_control* * It only implements "position + velocity" based control - it uses coarse positions and velocities calculated by MoveIt and performs cubic interpolation of joint positions (Bezier' curve) of positions. The positions are set using servoj command of URScript. * It is much more resilient to connectivity problems than other methods of ur_modern_driver. It is resilient to additional load on client PC, latency of network and kernel of the PC the driver runs on. This makes it much better suitable for development/research quick iteration loops without separate dedicated physical setup. @@ -59,12 +59,22 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design * **max_joint_difference** - maximum allowed difference between target and actual joints - checked at every trajectory step Here are some examples of manipulating the time flow for **Low Bandwidth Trajectory Follower** mode. You can use other settings but you should do it on your own risk. * Default mode: *servoj_time* = 0.008, *time_interval* = 0.008 : interpolation time flows with the same speed as real time - moves are executed as planned - * Slow-motion mode: *servoj_time* = 0.008, *time_interval* = 0.004 : interpolation time flows 2x slower than real time, so the move is executed 2x slower than planned. Requires configuring MoveIt to accept much slower moves than expected (otherwise MoveIt cancels such move mid-way) + * Slow-motion mode: *servoj_time* = 0.008, *time_interval* = 0.004 : interpolation time flows 2x slower than real time, so the move is executed 2x slower than planned. Requires configuring MoveIt to accept much slower moves than expected (otherwise MoveIt cancels such move mid-way) * Fast-forward mode: *servoj_time* = 0.004, *time_interval* = 0.012 : interpolation time flows 3x faster than real time, so the move is 3x faster than planned. Might violate limits of the robot speed so use carefully and make sure you gradually increase the speed in-between to check how safe it is. +NOTE! In case you use Low Bandwidth Trajectory Follower and you experience MoveIt to cancel robot moves prematurely +because of too long move duration, you should increase tolerance of duration monitoring of MoveIt trajectory execution +You can find the configuration usually in trajectory_execution.launch.xml in generated moveit config - there are +parameters that configure scaling and margin for allowed execution time among others. +The relevant parameters are `trajectory_execution/allowed_execution_duration_scaling` (default 1.2) and +`trajectory_execution/allowed_goal_duration_margin` (default 0.5). The first one is factor that scales execution time, +the second is margin that is added on top of the scaled one. You can increase either of those values to make moveit +executor more "tolerant" to execution delays. There is also another parameter: +`trajectory_execution/execution_duration_monitoring`. You can set it to false to disable duration monitoring completely. + ## Installation -**As the driver communicates with the robot via ethernet and depends on reliable continous communication, it is not possible to reliably control a UR from a virtual machine.** +**As the driver communicates with the robot via ethernet and depends on reliable continous communication, it is not possible to reliably control a UR from a virtual machine.** Just clone the repository into your catkin working directory and make it with ```catkin_make```. @@ -112,7 +122,7 @@ The position based controller *should* stay closer to the commanded path, while **Note** that the PID values are not optimally tweaked as of this moment. -To use ros_control together with MoveIt, be sure to add the desired controller to the ```controllers.yaml``` in the urXX_moveit_config/config folder. Add the following +To use ros_control together with MoveIt, be sure to add the desired controller to the ```controllers.yaml``` in the urXX_moveit_config/config folder. Add the following ``` controller_list: - name: /vel_based_pos_traj_controller #or /pos_based_pos_traj_controller @@ -133,7 +143,7 @@ controller_list: Each robot from UR is calibrated individually, so there is a small error (in the order of millimeters) between the end-effector reported by the URDF models in https://github.com/ros-industrial/universal_robot/tree/indigo-devel/ur_description and the end-effector as reported by the controller itself. -This driver broadcasts a transformation between the base link and the end-effector as reported by the UR. The default frame names are: *base* and *tool0_controller*. +This driver broadcasts a transformation between the base link and the end-effector as reported by the UR. The default frame names are: *base* and *tool0_controller*. To use the *tool0_controller* frame in a URDF, there needs to be a link with that name connected to *base*. For example: @@ -171,16 +181,16 @@ Should be compatible with all robots and control boxes with the newest firmware. *Simulated UR5 running 1.6.08725 - + #Credits Please cite the following report if using this driver -@techreport{andersen2015optimizing, - title = {Optimizing the Universal Robots ROS driver.}, - institution = {Technical University of Denmark, Department of Electrical Engineering}, - author = {Andersen, Thomas Timm}, - year = {2015}, - url = {http://orbit.dtu.dk/en/publications/optimizing-the-universal-robots-ros-driver(20dde139-7e87-4552-8658-dbf2cdaab24b).html} +@techreport{andersen2015optimizing, + title = {Optimizing the Universal Robots ROS driver.}, + institution = {Technical University of Denmark, Department of Electrical Engineering}, + author = {Andersen, Thomas Timm}, + year = {2015}, + url = {http://orbit.dtu.dk/en/publications/optimizing-the-universal-robots-ros-driver(20dde139-7e87-4552-8658-dbf2cdaab24b).html} } From 5c9ffe2ba1cedd51165cc83edfdd54abdd0f1efb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?J=C3=B8rn=20Sandvik=20Nilsson?= Date: Mon, 26 Mar 2018 11:39:32 +0200 Subject: [PATCH 111/114] Run io serivce operations as secondary urscripts --- src/ur/commander.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/ur/commander.cpp b/src/ur/commander.cpp index 9b85865c4..44f58670d 100644 --- a/src/ur/commander.cpp +++ b/src/ur/commander.cpp @@ -73,7 +73,7 @@ bool URCommander_V1_X::speedj(std::array &speeds, double acceleration bool URCommander_V1_X::setAnalogOut(uint8_t pin, double value) { std::ostringstream out; - out << "set_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(4) << value << ")\n"; + out << "sec io_fun():\n" << "set_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(4) << value << ")\n" << "end\n"; std::string s(out.str()); return write(s); } @@ -81,7 +81,7 @@ bool URCommander_V1_X::setAnalogOut(uint8_t pin, double value) bool URCommander_V1_X::setDigitalOut(uint8_t pin, bool value) { std::ostringstream out; - out << "set_digital_out(" << (int)pin << "," << (value ? "True" : "False") << ")\n"; + out << "sec io_fun():\n" << "set_digital_out(" << (int)pin << "," << (value ? "True" : "False") << ")\n" << "end\n"; std::string s(out.str()); return write(s); } @@ -89,7 +89,7 @@ bool URCommander_V1_X::setDigitalOut(uint8_t pin, bool value) bool URCommander_V3_X::setAnalogOut(uint8_t pin, double value) { std::ostringstream out; - out << "set_standard_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(5) << value << ")\n"; + out << "sec io_fun():\n" << "set_standard_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(5) << value << ")\n" << "end\n"; std::string s(out.str()); return write(s); } @@ -116,7 +116,7 @@ bool URCommander_V3_X::setDigitalOut(uint8_t pin, bool value) else return false; - out << func << "(" << (int)pin << "," << (value ? "True" : "False") << ")\n"; + out << "sec io_fun():\n" << func << "(" << (int)pin << "," << (value ? "True" : "False") << ")\n" << "end\n"; std::string s(out.str()); return write(s); } From 64f752f744f3cbcca42fcf0842d7360464218444 Mon Sep 17 00:00:00 2001 From: Miguel Prada Date: Mon, 23 Apr 2018 16:38:10 +0200 Subject: [PATCH 112/114] Initialize VelocityInterface::prev_velocity_cmd_ with zeroes --- src/ros/hardware_interface.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/ros/hardware_interface.cpp b/src/ros/hardware_interface.cpp index 759eca583..11b5b5f4e 100644 --- a/src/ros/hardware_interface.cpp +++ b/src/ros/hardware_interface.cpp @@ -17,7 +17,6 @@ void JointInterface::update(RTShared &packet) efforts_ = packet.i_actual; } - const std::string WrenchInterface::INTERFACE_NAME = "hardware_interface::ForceTorqueSensorInterface"; WrenchInterface::WrenchInterface(std::string tcp_link) { @@ -29,11 +28,10 @@ void WrenchInterface::update(RTShared &packet) tcp_ = packet.tcp_force; } - const std::string VelocityInterface::INTERFACE_NAME = "hardware_interface::VelocityJointInterface"; VelocityInterface::VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names, double max_vel_change) - : commander_(commander), max_vel_change_(max_vel_change) + : commander_(commander), max_vel_change_(max_vel_change), prev_velocity_cmd_({ 0, 0, 0, 0, 0, 0 }) { for (size_t i = 0; i < 6; i++) { From c72ec5bdedc105afc20e0984ec491a92cd3ef5fc Mon Sep 17 00:00:00 2001 From: axelschroth Date: Fri, 18 May 2018 13:54:45 +0200 Subject: [PATCH 113/114] Fixed tcp socket: shutdown() does not destroy socket descriptor -> use close() --- src/tcp_socket.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/tcp_socket.cpp b/src/tcp_socket.cpp index 619151a83..6a057bdf9 100644 --- a/src/tcp_socket.cpp +++ b/src/tcp_socket.cpp @@ -90,7 +90,7 @@ void TCPSocket::close() if (state_ != SocketState::Connected) return; state_ = SocketState::Closed; - ::shutdown(socket_fd_, SHUT_RDWR); + ::close(socket_fd_); socket_fd_ = -1; } @@ -164,4 +164,4 @@ bool TCPSocket::write(const uint8_t *buf, size_t buf_len, size_t &written) } return true; -} \ No newline at end of file +} From a2f5f4bed593057fa686d6c903fb41f14509917f Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Tue, 5 Jun 2018 21:54:13 +0200 Subject: [PATCH 114/114] Added additional questions to issue template. Added platform, CB version and connection setup questions to issue template based on G.A. vd. Hoorn's response to #23 --- .github/issue_template.md | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/.github/issue_template.md b/.github/issue_template.md index 956bb03e0..b4fc1f32a 100644 --- a/.github/issue_template.md +++ b/.github/issue_template.md @@ -27,9 +27,12 @@ Please search on the [issue track](../) before creating one. ## Your Environment +* Operating System and version: +* Platform (ie: PC or embedded arm): * Version used: * ROS version: -* Operating System and version: +* UR CB version: +* Connection setup (wired or wireless): ## Log file ```