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 0000000..ac2fe53 --- /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 0000000..76d802e --- /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 0000000..c375710 --- /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 0000000..ec465d6 --- /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 0000000..53b5232 --- /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 0000000..88a5ed6 --- /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 0000000..a7c53a8 --- /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 0000000..f44d57f --- /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 0000000..0d96b88 --- /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 0000000..8c51443 --- /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 53b5232..0000000 --- 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 0000000..7f316dc --- /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 0000000..e85b2dd --- /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 0000000..31ec391 --- /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 0000000..13b1952 --- /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 0000000..bc56aa9 --- /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 0000000..f76e8a6 --- /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 0000000..084211a --- /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 0000000..0e7ceea --- /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 0000000..4fea01d --- /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 0000000..aad336d --- /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 0000000..460bdf1 --- /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 0000000..d7a1222 --- /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 0000000..573b3d4 --- /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 0000000..b86b519 --- /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 0000000..f1ca009 --- /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 f44d57f..02083d5 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 @@ public: _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 @@ public: 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 0d96b88..3837402 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 f1ca009..bd51dae 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 460bdf1..3631992 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 a7c53a8..fad5ef9 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 02083d5..d8cde1b 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 @@ public: 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 @@ public: template T peek() { + assert(_buf_pos <= _buf_end); return decode(*(reinterpret_cast(_buf_pos))); } @@ -93,8 +98,12 @@ public: 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 @@ public: } } - void skip(size_t bytes) { + void consume() { + _buf_pos = _buf_end; + } + void consume(size_t bytes) { _buf_pos += bytes; } @@ -123,4 +135,12 @@ public: 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 | 40 ++------ src/ur/state.cpp | 111 ++++++++++++++------- src/ur/stream.cpp | 35 ++++++- 10 files changed, 258 insertions(+), 97 deletions(-) diff --git a/include/ur_modern_driver/ur/master_board.h b/include/ur_modern_driver/ur/master_board.h index e85b2dd..39317eb 100644 --- a/include/ur_modern_driver/ur/master_board.h +++ b/include/ur_modern_driver/ur/master_board.h @@ -60,7 +60,7 @@ public: + 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 @@ public: 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 31ec391..838c312 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 @@ public: 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 573b3d4..9cd8067 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 f76e8a6..085d42f 100644 --- a/include/ur_modern_driver/ur/state.h +++ b/include/ur_modern_driver/ur/state.h @@ -37,7 +37,7 @@ protected: 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 @@ public: 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 @@ public: //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 88a5ed6..a6ac0d5 100644 --- a/include/ur_modern_driver/ur/stream.h +++ b/include/ur_modern_driver/ur/stream.h @@ -21,6 +21,10 @@ public: _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 084211a..9c0f865 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 0e7ceea..42ceb8a 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 bd51dae..87fb6ed 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(buf, sizeof(buf)); - //blocking call - ssize_t len = _stream.receive(pos, size); - - if(len < 1) { - LOG_DEBUG("Read nothing from stream"); - return std::unique_ptr(nullptr); - } + LOG_DEBUG("Read %d bytes from stream", len); - 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)); + 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 3631992..851c906 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; +} + +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.parse_with(bp); + break; + case package_type::MASTERBOARD_DATA: + 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: + 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; } -bool RobotState_V1_6__7::parse_package(BinParser &bp) { - //todo: size checks - package_type type; - bp.parse(type); + +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::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 + 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 package type parsed: %" PRIu8 "", type); + 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) { - 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; + 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 fad5ef9..f66482b 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 3837402..db0f5b6 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 d8cde1b..c7f5d7c 100644 --- a/include/ur_modern_driver/bin_parser.h +++ b/include/ur_modern_driver/bin_parser.h @@ -141,6 +141,6 @@ public: } 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 ac2fe53..b718f62 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 @@ private: 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 @@ private: 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 @@ public: _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 0000000..5b2a373 --- /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 0000000..a09186f --- /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 39317eb..0a7daf1 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 @@ public: 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 @@ public: + 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 @@ public: 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 838c312..97315de 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 0000000..77dc9b7 --- /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 9cd8067..00a0bac 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); - } +class URParser { +public: + virtual bool parse(BinParser &bp, std::vector> &results) = 0; }; - - -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 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 b86b519..35be085 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 13b1952..bce22af 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 @@ public: 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 0000000..3a3831a --- /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 bc56aa9..c749ebb 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 @@ public: 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 @@ public: 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 @@ public: 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 085d42f..d37cbf1 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 { +class StatePacket { 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); -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 0000000..df5a7d6 --- /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 9c0f865..c8d947b 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 42ceb8a..e079f8d 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 87fb6ed..0000000 --- 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 4fea01d..1c62406 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 aad336d..596134d 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 851c906..fe5af1b 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 0000000..3f59c93 --- /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 0000000..3289174 --- /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 0000000..c0db690 --- /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 0000000..c2dc749 --- /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 | 922 +++++------ .../queue/readerwriterqueue.h | 1278 ++++++++-------- include/ur_modern_driver/robot_state.h | 282 ++-- 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 | 72 +- 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 | 56 +- include/ur_modern_driver/ur_driver.h | 118 +- .../ur_modern_driver/ur_hardware_interface.h | 97 +- .../ur_realtime_communication.h | 80 +- src/do_output.cpp | 39 +- src/robot_state.cpp | 671 ++++---- src/robot_state_RT.cpp | 766 +++++----- src/ros/rt_publisher.cpp | 32 +- src/ros_main.cpp | 93 +- 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 | 638 ++++---- src/ur_hardware_interface.cpp | 389 ++--- src/ur_realtime_communication.cpp | 313 ++-- src/ur_ros_wrapper.cpp | 1356 ++++++++--------- 46 files changed, 4476 insertions(+), 4212 deletions(-) create mode 100644 .clang-format diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..95f7eb5 --- /dev/null +++ b/.clang-format @@ -0,0 +1,95 @@ +--- +Language: Cpp +# BasedOnStyle: WebKit +AccessModifierOffset: -4 +AlignAfterOpenBracket: DontAlign +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlinesLeft: false +AlignOperands: false +AlignTrailingComments: false +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: All +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakTemplateDeclarations: false +BinPackArguments: true +BinPackParameters: true +BraceWrapping: + AfterClass: false + AfterControlStatement: false + AfterEnum: false + AfterFunction: true + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + BeforeCatch: false + BeforeElse: false + IndentBraces: false +BreakBeforeBinaryOperators: All +BreakBeforeBraces: WebKit +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: true +BreakAfterJavaFieldAnnotations: false +BreakStringLiterals: true +ColumnLimit: 0 +CommentPragmas: '^ IWYU pragma:' +ConstructorInitializerAllOnOneLineOrOnePerLine: false +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: false +DerivePointerAlignment: false +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] +IncludeCategories: + - Regex: '^"(llvm|llvm-c|clang|clang-c)/' + Priority: 2 + - Regex: '^(<|"(gtest|isl|json)/)' + Priority: 3 + - Regex: '.*' + Priority: 1 +IncludeIsMainRegex: '$' +IndentCaseLabels: false +IndentWidth: 4 +IndentWrappedFunctionNames: false +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: true +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: Inner +ObjCBlockIndentWidth: 4 +ObjCSpaceAfterProperty: true +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 60 +PointerAlignment: Left +ReflowComments: true +SortIncludes: true +SpaceAfterCStyleCast: false +SpaceBeforeAssignmentOperators: true +SpaceBeforeParens: ControlStatements +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 1 +SpacesInAngles: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Cpp03 +TabWidth: 8 +UseTab: Never +... + diff --git a/include/ur_modern_driver/bin_parser.h b/include/ur_modern_driver/bin_parser.h index c7f5d7c..12d49cb 100644 --- a/include/ur_modern_driver/bin_parser.h +++ b/include/ur_modern_driver/bin_parser.h @@ -1,146 +1,171 @@ #pragma once -#include -#include +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/types.h" +#include #include #include +#include +#include #include -#include -#include "ur_modern_driver/types.h" -#include "ur_modern_driver/log.h" class BinParser { private: uint8_t *_buf_pos, *_buf_end; - BinParser &_parent; + BinParser& _parent; public: - BinParser(uint8_t *buffer, size_t buf_len) : - _buf_pos(buffer), - _buf_end(buffer+buf_len), - _parent(*this) { - assert(_buf_pos <= _buf_end); - } - - BinParser(BinParser &parent, size_t sub_len) : - _buf_pos(parent._buf_pos), - _buf_end(parent._buf_pos+sub_len), - _parent(parent) { - assert(_buf_pos <= _buf_end); - } + BinParser(uint8_t* buffer, size_t buf_len) + : _buf_pos(buffer) + , _buf_end(buffer + buf_len) + , _parent(*this) + { + assert(_buf_pos <= _buf_end); + } - ~BinParser() { + BinParser(BinParser& parent, size_t sub_len) + : _buf_pos(parent._buf_pos) + , _buf_end(parent._buf_pos + sub_len) + , _parent(parent) + { + assert(_buf_pos <= _buf_end); + } + + ~BinParser() + { _parent._buf_pos = _buf_pos; - } - + } //Decode from network encoding (big endian) to host encoding - template - T decode(T val) { + template + T decode(T val) + { return val; } - uint16_t decode(uint16_t val) { + uint16_t decode(uint16_t val) + { return be16toh(val); } - uint32_t decode(uint32_t val) { + uint32_t decode(uint32_t val) + { return be32toh(val); } - uint64_t decode(uint64_t val) { + uint64_t decode(uint64_t val) + { return be64toh(val); } - int16_t decode(int16_t val) { + int16_t decode(int16_t val) + { return be16toh(val); } - int32_t decode(int32_t val) { + int32_t decode(int32_t val) + { return be32toh(val); } - int64_t decode(int64_t val) { + int64_t decode(int64_t val) + { return be64toh(val); } - float decode(float val) { + float decode(float val) + { return be32toh(val); } - double decode(double val) { + double decode(double val) + { return be64toh(val); } - - template - T peek() { + template + T peek() + { assert(_buf_pos <= _buf_end); return decode(*(reinterpret_cast(_buf_pos))); } - template - void parse(T &val) { + template + void parse(T& val) + { val = peek(); _buf_pos += sizeof(T); } - // UR uses 1 byte for boolean values but sizeof(bool) is implementation + // UR uses 1 byte for boolean values but sizeof(bool) is implementation // defined so we must ensure they're parsed as uint8_t on all compilers - void parse(bool &val) { + void parse(bool& val) + { uint8_t inner; parse(inner); - val = inner != 0; + val = inner != 0; } // Explicit parsing order of fields to avoid issues with struct layout - void parse(double3_t &val) { + void parse(double3_t& val) + { parse(val.x); parse(val.y); parse(val.z); } // Explicit parsing order of fields to avoid issues with struct layout - void parse(cartesian_coord_t &val) { + void parse(cartesian_coord_t& val) + { parse(val.position); parse(val.rotation); } - void parse_remainder(std::string &val) { + void parse_remainder(std::string& val) + { parse(val, size_t(_buf_end - _buf_pos)); } - void parse(std::string &val, size_t len) { + void parse(std::string& val, size_t len) + { val.assign(reinterpret_cast(_buf_pos), len); _buf_pos += len; } - // Special string parse function that assumes uint8_t len followed by chars - void parse(std::string &val) { + // Special string parse function that assumes uint8_t len followed by chars + void parse(std::string& val) + { uint8_t len; parse(len); parse(val, size_t(len)); } - template - void parse(T (&array)[N]) { - for(size_t i = 0; i < N; i++) { + template + void parse(T (&array)[N]) + { + for (size_t i = 0; i < N; i++) { parse(array[i]); } } - void consume() { + void consume() + { _buf_pos = _buf_end; } - void consume(size_t bytes) { + void consume(size_t bytes) + { _buf_pos += bytes; } - bool check_size(size_t bytes) { + bool check_size(size_t bytes) + { return bytes <= size_t(_buf_end - _buf_pos); } - template - bool check_size(void) { + template + bool check_size(void) + { return check_size(T::SIZE); } - bool empty() { + bool empty() + { return _buf_pos == _buf_end; } - void debug() { + void debug() + { LOG_DEBUG("BinParser: %p - %p (%zu bytes)", _buf_pos, _buf_end, _buf_end - _buf_pos); } }; diff --git a/include/ur_modern_driver/do_output.h b/include/ur_modern_driver/do_output.h index d52420e..44c1e63 100644 --- a/include/ur_modern_driver/do_output.h +++ b/include/ur_modern_driver/do_output.h @@ -30,5 +30,4 @@ void print_warning(std::string inp); void print_error(std::string inp); void print_fatal(std::string inp); - #endif /* UR_DO_OUTPUT_H_ */ diff --git a/include/ur_modern_driver/log.h b/include/ur_modern_driver/log.h index db0f5b6..b66267d 100644 --- a/include/ur_modern_driver/log.h +++ b/include/ur_modern_driver/log.h @@ -2,20 +2,20 @@ #include #ifdef ROS_BUILD - #include +#include - #define LOG_DEBUG ROS_DEBUG - #define LOG_WARN ROS_WARN - #define LOG_INFO ROS_INFO - #define LOG_ERROR ROS_ERROR - #define LOG_FATAL ROS_FATAL +#define LOG_DEBUG ROS_DEBUG +#define LOG_WARN ROS_WARN +#define LOG_INFO ROS_INFO +#define LOG_ERROR ROS_ERROR +#define LOG_FATAL ROS_FATAL #else - #define LOG_DEBUG(format, ...) printf("[DEBUG]: " format "\n", ##__VA_ARGS__) - #define LOG_WARN(format, ...) printf("[WARNING]: " format "\n", ##__VA_ARGS__) - #define LOG_INFO(format, ...) printf("[INFO]: " format "\n", ##__VA_ARGS__) - #define LOG_ERROR(format, ...) printf("[ERROR]: " format "\n", ##__VA_ARGS__) - #define LOG_FATAL(format, ...) printf("[FATAL]: " format "\n", ##__VA_ARGS__) +#define LOG_DEBUG(format, ...) printf("[DEBUG]: " format "\n", ##__VA_ARGS__) +#define LOG_WARN(format, ...) printf("[WARNING]: " format "\n", ##__VA_ARGS__) +#define LOG_INFO(format, ...) printf("[INFO]: " format "\n", ##__VA_ARGS__) +#define LOG_ERROR(format, ...) printf("[ERROR]: " format "\n", ##__VA_ARGS__) +#define LOG_FATAL(format, ...) printf("[FATAL]: " format "\n", ##__VA_ARGS__) #endif \ No newline at end of file diff --git a/include/ur_modern_driver/packet.h b/include/ur_modern_driver/packet.h index 7f316dc..541e3e5 100644 --- a/include/ur_modern_driver/packet.h +++ b/include/ur_modern_driver/packet.h @@ -3,5 +3,5 @@ class Packet { public: - virtual bool parse_with(BinParser &bp) = 0; + virtual bool parse_with(BinParser& bp) = 0; }; \ No newline at end of file diff --git a/include/ur_modern_driver/parser.h b/include/ur_modern_driver/parser.h index d7a1222..9997515 100644 --- a/include/ur_modern_driver/parser.h +++ b/include/ur_modern_driver/parser.h @@ -4,5 +4,5 @@ class Parser { public: - virtual std::unique_ptr parse(BinParser &bp) = 0; + virtual std::unique_ptr parse(BinParser& bp) = 0; }; diff --git a/include/ur_modern_driver/pipeline.h b/include/ur_modern_driver/pipeline.h index b718f62..13a9b1b 100644 --- a/include/ur_modern_driver/pipeline.h +++ b/include/ur_modern_driver/pipeline.h @@ -1,21 +1,20 @@ #pragma once -#include -#include -#include #include "ur_modern_driver/log.h" #include "ur_modern_driver/queue/readerwriterqueue.h" +#include +#include +#include -using namespace moodycamel; +using namespace moodycamel; using namespace std; - template class IConsumer { public: - virtual void setup_consumer() { } - virtual void teardown_consumer() { } - virtual void stop_consumer() { } + virtual void setup_consumer() {} + virtual void teardown_consumer() {} + virtual void stop_consumer() {} virtual bool consume(unique_ptr product) = 0; }; @@ -23,33 +22,33 @@ public: template class IProducer { public: - virtual void setup_producer() { } - virtual void teardown_producer() { } - virtual void stop_producer() { } + virtual void setup_producer() {} + virtual void teardown_producer() {} + virtual void stop_producer() {} - virtual bool try_get(std::vector> &products) = 0; + virtual bool try_get(std::vector >& products) = 0; }; - template class Pipeline { private: - IProducer &_producer; - IConsumer &_consumer; - BlockingReaderWriterQueue> _queue; + IProducer& _producer; + IConsumer& _consumer; + BlockingReaderWriterQueue > _queue; atomic _running; thread _pThread, _cThread; - void run_producer() { + void run_producer() + { _producer.setup_producer(); - std::vector> products; - while(_running) { - if(!_producer.try_get(products)) { + std::vector > products; + while (_running) { + if (!_producer.try_get(products)) { break; } - - for(auto &p : products) { - if(!_queue.try_enqueue(std::move(p))) { + + for (auto& p : products) { + if (!_queue.try_enqueue(std::move(p))) { LOG_WARN("Pipeline owerflowed!"); } } @@ -60,27 +59,31 @@ private: //todo cleanup } - void run_consumer() { + void run_consumer() + { _consumer.setup_consumer(); unique_ptr product; - while(_running) { + while (_running) { _queue.wait_dequeue(product); - if(!_consumer.consume(std::move(product))) + if (!_consumer.consume(std::move(product))) break; } _consumer.teardown_consumer(); //todo cleanup } -public: - Pipeline(IProducer &producer, IConsumer &consumer) - : _producer(producer), - _consumer(consumer), - _queue{32}, - _running{false} - { } - void run() { - if(_running) +public: + Pipeline(IProducer& producer, IConsumer& consumer) + : _producer(producer) + , _consumer(consumer) + , _queue{ 32 } + , _running{ false } + { + } + + void run() + { + if (_running) return; _running = true; @@ -88,10 +91,11 @@ public: _cThread = thread(&Pipeline::run_consumer, this); } - void stop() { - if(!_running) + void stop() + { + if (!_running) return; - + _consumer.stop_consumer(); _producer.stop_producer(); diff --git a/include/ur_modern_driver/queue/atomicops.h b/include/ur_modern_driver/queue/atomicops.h index c375710..af0089e 100644 --- a/include/ur_modern_driver/queue/atomicops.h +++ b/include/ur_modern_driver/queue/atomicops.h @@ -12,10 +12,10 @@ // Uses the AE_* prefix for macros (historical reasons), and the "moodycamel" namespace for symbols. #include -#include #include #include #include +#include // Platform detection #if defined(__INTEL_COMPILER) @@ -38,22 +38,19 @@ #define AE_ARCH_UNKNOWN #endif - // AE_UNUSED #define AE_UNUSED(x) ((void)x) - // AE_FORCEINLINE #if defined(AE_VCPP) || defined(AE_ICC) #define AE_FORCEINLINE __forceinline #elif defined(AE_GCC) -//#define AE_FORCEINLINE __attribute__((always_inline)) +//#define AE_FORCEINLINE __attribute__((always_inline)) #define AE_FORCEINLINE inline #else #define AE_FORCEINLINE inline #endif - // AE_ALIGN #if defined(AE_VCPP) || defined(AE_ICC) #define AE_ALIGN(x) __declspec(align(x)) @@ -64,24 +61,23 @@ #define AE_ALIGN(x) __attribute__((aligned(x))) #endif - // Portable atomic fences implemented below: namespace moodycamel { enum memory_order { - memory_order_relaxed, - memory_order_acquire, - memory_order_release, - memory_order_acq_rel, - memory_order_seq_cst, + memory_order_relaxed, + memory_order_acquire, + memory_order_release, + memory_order_acq_rel, + memory_order_seq_cst, - // memory_order_sync: Forces a full sync: - // #LoadLoad, #LoadStore, #StoreStore, and most significantly, #StoreLoad - memory_order_sync = memory_order_seq_cst + // memory_order_sync: Forces a full sync: + // #LoadLoad, #LoadStore, #StoreStore, and most significantly, #StoreLoad + memory_order_sync = memory_order_seq_cst }; -} // end namespace moodycamel +} // end namespace moodycamel #if (defined(AE_VCPP) && (_MSC_VER < 1700 || defined(__cplusplus_cli))) || defined(AE_ICC) // VS2010 and ICC13 don't support std::atomic_*_fence, implement our own fences @@ -100,10 +96,9 @@ enum memory_order { #define AeLiteSync __lwsync #endif - #ifdef AE_VCPP #pragma warning(push) -#pragma warning(disable: 4365) // Disable erroneous 'conversion from long to unsigned int, signed/unsigned mismatch' error when using `assert` +#pragma warning(disable : 4365) // Disable erroneous 'conversion from long to unsigned int, signed/unsigned mismatch' error when using `assert` #ifdef __cplusplus_cli #pragma managed(push, off) #endif @@ -113,14 +108,24 @@ namespace moodycamel { AE_FORCEINLINE void compiler_fence(memory_order order) { - switch (order) { - case memory_order_relaxed: break; - case memory_order_acquire: _ReadBarrier(); break; - case memory_order_release: _WriteBarrier(); break; - case memory_order_acq_rel: _ReadWriteBarrier(); break; - case memory_order_seq_cst: _ReadWriteBarrier(); break; - default: assert(false); - } + switch (order) { + case memory_order_relaxed: + break; + case memory_order_acquire: + _ReadBarrier(); + break; + case memory_order_release: + _WriteBarrier(); + break; + case memory_order_acq_rel: + _ReadWriteBarrier(); + break; + case memory_order_seq_cst: + _ReadWriteBarrier(); + break; + default: + assert(false); + } } // x86/x64 have a strong memory model -- all loads and stores have @@ -129,51 +134,60 @@ AE_FORCEINLINE void compiler_fence(memory_order order) #if defined(AE_ARCH_X86) || defined(AE_ARCH_X64) AE_FORCEINLINE void fence(memory_order order) { - switch (order) { - case memory_order_relaxed: break; - case memory_order_acquire: _ReadBarrier(); break; - case memory_order_release: _WriteBarrier(); break; - case memory_order_acq_rel: _ReadWriteBarrier(); break; - case memory_order_seq_cst: - _ReadWriteBarrier(); - AeFullSync(); - _ReadWriteBarrier(); - break; - default: assert(false); - } + switch (order) { + case memory_order_relaxed: + break; + case memory_order_acquire: + _ReadBarrier(); + break; + case memory_order_release: + _WriteBarrier(); + break; + case memory_order_acq_rel: + _ReadWriteBarrier(); + break; + case memory_order_seq_cst: + _ReadWriteBarrier(); + AeFullSync(); + _ReadWriteBarrier(); + break; + default: + assert(false); + } } #else AE_FORCEINLINE void fence(memory_order order) { - // Non-specialized arch, use heavier memory barriers everywhere just in case :-( - switch (order) { - case memory_order_relaxed: - break; - case memory_order_acquire: - _ReadBarrier(); - AeLiteSync(); - _ReadBarrier(); - break; - case memory_order_release: - _WriteBarrier(); - AeLiteSync(); - _WriteBarrier(); - break; - case memory_order_acq_rel: - _ReadWriteBarrier(); - AeLiteSync(); - _ReadWriteBarrier(); - break; - case memory_order_seq_cst: - _ReadWriteBarrier(); - AeFullSync(); - _ReadWriteBarrier(); - break; - default: assert(false); - } + // Non-specialized arch, use heavier memory barriers everywhere just in case :-( + switch (order) { + case memory_order_relaxed: + break; + case memory_order_acquire: + _ReadBarrier(); + AeLiteSync(); + _ReadBarrier(); + break; + case memory_order_release: + _WriteBarrier(); + AeLiteSync(); + _WriteBarrier(); + break; + case memory_order_acq_rel: + _ReadWriteBarrier(); + AeLiteSync(); + _ReadWriteBarrier(); + break; + case memory_order_seq_cst: + _ReadWriteBarrier(); + AeFullSync(); + _ReadWriteBarrier(); + break; + default: + assert(false); + } } #endif -} // end namespace moodycamel +} // end namespace moodycamel #else // Use standard library of atomics #include @@ -182,33 +196,52 @@ namespace moodycamel { AE_FORCEINLINE void compiler_fence(memory_order order) { - switch (order) { - case memory_order_relaxed: break; - case memory_order_acquire: std::atomic_signal_fence(std::memory_order_acquire); break; - case memory_order_release: std::atomic_signal_fence(std::memory_order_release); break; - case memory_order_acq_rel: std::atomic_signal_fence(std::memory_order_acq_rel); break; - case memory_order_seq_cst: std::atomic_signal_fence(std::memory_order_seq_cst); break; - default: assert(false); - } + switch (order) { + case memory_order_relaxed: + break; + case memory_order_acquire: + std::atomic_signal_fence(std::memory_order_acquire); + break; + case memory_order_release: + std::atomic_signal_fence(std::memory_order_release); + break; + case memory_order_acq_rel: + std::atomic_signal_fence(std::memory_order_acq_rel); + break; + case memory_order_seq_cst: + std::atomic_signal_fence(std::memory_order_seq_cst); + break; + default: + assert(false); + } } AE_FORCEINLINE void fence(memory_order order) { - switch (order) { - case memory_order_relaxed: break; - case memory_order_acquire: std::atomic_thread_fence(std::memory_order_acquire); break; - case memory_order_release: std::atomic_thread_fence(std::memory_order_release); break; - case memory_order_acq_rel: std::atomic_thread_fence(std::memory_order_acq_rel); break; - case memory_order_seq_cst: std::atomic_thread_fence(std::memory_order_seq_cst); break; - default: assert(false); - } + switch (order) { + case memory_order_relaxed: + break; + case memory_order_acquire: + std::atomic_thread_fence(std::memory_order_acquire); + break; + case memory_order_release: + std::atomic_thread_fence(std::memory_order_release); + break; + case memory_order_acq_rel: + std::atomic_thread_fence(std::memory_order_acq_rel); + break; + case memory_order_seq_cst: + std::atomic_thread_fence(std::memory_order_seq_cst); + break; + default: + assert(false); + } } -} // end namespace moodycamel +} // end namespace moodycamel #endif - #if !defined(AE_VCPP) || (_MSC_VER >= 1700 && !defined(__cplusplus_cli)) #define AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC #endif @@ -223,102 +256,126 @@ AE_FORCEINLINE void fence(memory_order order) // The guarantee of atomicity is only made for types that already have atomic load and store guarantees // at the hardware level -- on most platforms this generally means aligned pointers and integers (only). namespace moodycamel { -template -class weak_atomic -{ +template +class weak_atomic { public: - weak_atomic() { } + weak_atomic() {} #ifdef AE_VCPP -#pragma warning(disable: 4100) // Get rid of (erroneous) 'unreferenced formal parameter' warning +#pragma warning(disable : 4100) // Get rid of (erroneous) 'unreferenced formal parameter' warning #endif - template weak_atomic(U&& x) : value(std::forward(x)) { } + template + weak_atomic(U&& x) + : value(std::forward(x)) + { + } #ifdef __cplusplus_cli - // Work around bug with universal reference/nullptr combination that only appears when /clr is on - weak_atomic(nullptr_t) : value(nullptr) { } + // Work around bug with universal reference/nullptr combination that only appears when /clr is on + weak_atomic(nullptr_t) + : value(nullptr) + { + } #endif - weak_atomic(weak_atomic const& other) : value(other.value) { } - weak_atomic(weak_atomic&& other) : value(std::move(other.value)) { } + weak_atomic(weak_atomic const& other) + : value(other.value) + { + } + weak_atomic(weak_atomic&& other) + : value(std::move(other.value)) + { + } #ifdef AE_VCPP -#pragma warning(default: 4100) +#pragma warning(default : 4100) #endif - AE_FORCEINLINE operator T() const { return load(); } + AE_FORCEINLINE operator T() const + { + return load(); + } - #ifndef AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC - template AE_FORCEINLINE weak_atomic const& operator=(U&& x) { value = std::forward(x); return *this; } - AE_FORCEINLINE weak_atomic const& operator=(weak_atomic const& other) { value = other.value; return *this; } - - AE_FORCEINLINE T load() const { return value; } - - AE_FORCEINLINE T fetch_add_acquire(T increment) - { -#if defined(AE_ARCH_X64) || defined(AE_ARCH_X86) - if (sizeof(T) == 4) return _InterlockedExchangeAdd((long volatile*)&value, (long)increment); -#if defined(_M_AMD64) - else if (sizeof(T) == 8) return _InterlockedExchangeAdd64((long long volatile*)&value, (long long)increment); -#endif -#else -#error Unsupported platform -#endif - assert(false && "T must be either a 32 or 64 bit type"); - return value; - } - - AE_FORCEINLINE T fetch_add_release(T increment) - { -#if defined(AE_ARCH_X64) || defined(AE_ARCH_X86) - if (sizeof(T) == 4) return _InterlockedExchangeAdd((long volatile*)&value, (long)increment); -#if defined(_M_AMD64) - else if (sizeof(T) == 8) return _InterlockedExchangeAdd64((long long volatile*)&value, (long long)increment); -#endif -#else -#error Unsupported platform -#endif - assert(false && "T must be either a 32 or 64 bit type"); - return value; - } -#else - template - AE_FORCEINLINE weak_atomic const& operator=(U&& x) - { - value.store(std::forward(x), std::memory_order_relaxed); - return *this; - } - - AE_FORCEINLINE weak_atomic const& operator=(weak_atomic const& other) - { - value.store(other.value.load(std::memory_order_relaxed), std::memory_order_relaxed); - return *this; - } + template + AE_FORCEINLINE weak_atomic const& operator=(U&& x) + { + value = std::forward(x); + return *this; + } + AE_FORCEINLINE weak_atomic const& operator=(weak_atomic const& other) + { + value = other.value; + return *this; + } - AE_FORCEINLINE T load() const { return value.load(std::memory_order_relaxed); } - - AE_FORCEINLINE T fetch_add_acquire(T increment) - { - return value.fetch_add(increment, std::memory_order_acquire); - } - - AE_FORCEINLINE T fetch_add_release(T increment) - { - return value.fetch_add(increment, std::memory_order_release); - } + AE_FORCEINLINE T load() const { return value; } + + AE_FORCEINLINE T fetch_add_acquire(T increment) + { +#if defined(AE_ARCH_X64) || defined(AE_ARCH_X86) + if (sizeof(T) == 4) + return _InterlockedExchangeAdd((long volatile*)&value, (long)increment); +#if defined(_M_AMD64) + else if (sizeof(T) == 8) + return _InterlockedExchangeAdd64((long long volatile*)&value, (long long)increment); +#endif +#else +#error Unsupported platform +#endif + assert(false && "T must be either a 32 or 64 bit type"); + return value; + } + + AE_FORCEINLINE T fetch_add_release(T increment) + { +#if defined(AE_ARCH_X64) || defined(AE_ARCH_X86) + if (sizeof(T) == 4) + return _InterlockedExchangeAdd((long volatile*)&value, (long)increment); +#if defined(_M_AMD64) + else if (sizeof(T) == 8) + return _InterlockedExchangeAdd64((long long volatile*)&value, (long long)increment); +#endif +#else +#error Unsupported platform +#endif + assert(false && "T must be either a 32 or 64 bit type"); + return value; + } +#else + template + AE_FORCEINLINE weak_atomic const& operator=(U&& x) + { + value.store(std::forward(x), std::memory_order_relaxed); + return *this; + } + + AE_FORCEINLINE weak_atomic const& operator=(weak_atomic const& other) + { + value.store(other.value.load(std::memory_order_relaxed), std::memory_order_relaxed); + return *this; + } + + AE_FORCEINLINE T load() const { return value.load(std::memory_order_relaxed); } + + AE_FORCEINLINE T fetch_add_acquire(T increment) + { + return value.fetch_add(increment, std::memory_order_acquire); + } + + AE_FORCEINLINE T fetch_add_release(T increment) + { + return value.fetch_add(increment, std::memory_order_release); + } #endif - private: #ifndef AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC - // No std::atomic support, but still need to circumvent compiler optimizations. - // `volatile` will make memory access slow, but is guaranteed to be reliable. - volatile T value; + // No std::atomic support, but still need to circumvent compiler optimizations. + // `volatile` will make memory access slow, but is guaranteed to be reliable. + volatile T value; #else - std::atomic value; + std::atomic value; #endif }; -} // end namespace moodycamel - - +} // end namespace moodycamel // Portable single-producer, single-consumer semaphore below: @@ -329,11 +386,11 @@ private: // I know this is an ugly hack but it still beats polluting the global // namespace with thousands of generic names or adding a .cpp for nothing. extern "C" { - struct _SECURITY_ATTRIBUTES; - __declspec(dllimport) void* __stdcall CreateSemaphoreW(_SECURITY_ATTRIBUTES* lpSemaphoreAttributes, long lInitialCount, long lMaximumCount, const wchar_t* lpName); - __declspec(dllimport) int __stdcall CloseHandle(void* hObject); - __declspec(dllimport) unsigned long __stdcall WaitForSingleObject(void* hHandle, unsigned long dwMilliseconds); - __declspec(dllimport) int __stdcall ReleaseSemaphore(void* hSemaphore, long lReleaseCount, long* lpPreviousCount); +struct _SECURITY_ATTRIBUTES; +__declspec(dllimport) void* __stdcall CreateSemaphoreW(_SECURITY_ATTRIBUTES* lpSemaphoreAttributes, long lInitialCount, long lMaximumCount, const wchar_t* lpName); +__declspec(dllimport) int __stdcall CloseHandle(void* hObject); +__declspec(dllimport) unsigned long __stdcall WaitForSingleObject(void* hHandle, unsigned long dwMilliseconds); +__declspec(dllimport) int __stdcall ReleaseSemaphore(void* hSemaphore, long lReleaseCount, long* lpPreviousCount); } #elif defined(__MACH__) #include @@ -341,320 +398,305 @@ extern "C" { #include #endif -namespace moodycamel -{ - // Code in the spsc_sema namespace below is an adaptation of Jeff Preshing's - // portable + lightweight semaphore implementations, originally from - // https://github.com/preshing/cpp11-on-multicore/blob/master/common/sema.h - // LICENSE: - // Copyright (c) 2015 Jeff Preshing - // - // This software is provided 'as-is', without any express or implied - // warranty. In no event will the authors be held liable for any damages - // arising from the use of this software. - // - // Permission is granted to anyone to use this software for any purpose, - // including commercial applications, and to alter it and redistribute it - // freely, subject to the following restrictions: - // - // 1. The origin of this software must not be misrepresented; you must not - // claim that you wrote the original software. If you use this software - // in a product, an acknowledgement in the product documentation would be - // appreciated but is not required. - // 2. Altered source versions must be plainly marked as such, and must not be - // misrepresented as being the original software. - // 3. This notice may not be removed or altered from any source distribution. - namespace spsc_sema - { +namespace moodycamel { +// Code in the spsc_sema namespace below is an adaptation of Jeff Preshing's +// portable + lightweight semaphore implementations, originally from +// https://github.com/preshing/cpp11-on-multicore/blob/master/common/sema.h +// LICENSE: +// Copyright (c) 2015 Jeff Preshing +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgement in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +namespace spsc_sema { #if defined(_WIN32) - class Semaphore - { - private: - void* m_hSema; - - Semaphore(const Semaphore& other); - Semaphore& operator=(const Semaphore& other); + class Semaphore { + private: + void* m_hSema; - public: - Semaphore(int initialCount = 0) - { - assert(initialCount >= 0); - const long maxLong = 0x7fffffff; - m_hSema = CreateSemaphoreW(nullptr, initialCount, maxLong, nullptr); - } + Semaphore(const Semaphore& other); + Semaphore& operator=(const Semaphore& other); - ~Semaphore() - { - CloseHandle(m_hSema); - } + public: + Semaphore(int initialCount = 0) + { + assert(initialCount >= 0); + const long maxLong = 0x7fffffff; + m_hSema = CreateSemaphoreW(nullptr, initialCount, maxLong, nullptr); + } - void wait() - { - const unsigned long infinite = 0xffffffff; - WaitForSingleObject(m_hSema, infinite); - } + ~Semaphore() + { + CloseHandle(m_hSema); + } - bool try_wait() - { - const unsigned long RC_WAIT_TIMEOUT = 0x00000102; - return WaitForSingleObject(m_hSema, 0) != RC_WAIT_TIMEOUT; - } + void wait() + { + const unsigned long infinite = 0xffffffff; + WaitForSingleObject(m_hSema, infinite); + } - bool timed_wait(std::uint64_t usecs) - { - const unsigned long RC_WAIT_TIMEOUT = 0x00000102; - return WaitForSingleObject(m_hSema, (unsigned long)(usecs / 1000)) != RC_WAIT_TIMEOUT; - } + bool try_wait() + { + const unsigned long RC_WAIT_TIMEOUT = 0x00000102; + return WaitForSingleObject(m_hSema, 0) != RC_WAIT_TIMEOUT; + } - void signal(int count = 1) - { - ReleaseSemaphore(m_hSema, count, nullptr); - } - }; + bool timed_wait(std::uint64_t usecs) + { + const unsigned long RC_WAIT_TIMEOUT = 0x00000102; + return WaitForSingleObject(m_hSema, (unsigned long)(usecs / 1000)) != RC_WAIT_TIMEOUT; + } + + void signal(int count = 1) + { + ReleaseSemaphore(m_hSema, count, nullptr); + } + }; #elif defined(__MACH__) - //--------------------------------------------------------- - // Semaphore (Apple iOS and OSX) - // Can't use POSIX semaphores due to http://lists.apple.com/archives/darwin-kernel/2009/Apr/msg00010.html - //--------------------------------------------------------- - class Semaphore - { - private: - semaphore_t m_sema; + //--------------------------------------------------------- + // Semaphore (Apple iOS and OSX) + // Can't use POSIX semaphores due to http://lists.apple.com/archives/darwin-kernel/2009/Apr/msg00010.html + //--------------------------------------------------------- + class Semaphore { + private: + semaphore_t m_sema; - Semaphore(const Semaphore& other); - Semaphore& operator=(const Semaphore& other); + Semaphore(const Semaphore& other); + Semaphore& operator=(const Semaphore& other); - public: - Semaphore(int initialCount = 0) - { - assert(initialCount >= 0); - semaphore_create(mach_task_self(), &m_sema, SYNC_POLICY_FIFO, initialCount); - } + public: + Semaphore(int initialCount = 0) + { + assert(initialCount >= 0); + semaphore_create(mach_task_self(), &m_sema, SYNC_POLICY_FIFO, initialCount); + } - ~Semaphore() - { - semaphore_destroy(mach_task_self(), m_sema); - } + ~Semaphore() + { + semaphore_destroy(mach_task_self(), m_sema); + } - void wait() - { - semaphore_wait(m_sema); - } + void wait() + { + semaphore_wait(m_sema); + } - bool try_wait() - { - return timed_wait(0); - } + bool try_wait() + { + return timed_wait(0); + } - bool timed_wait(std::int64_t timeout_usecs) - { - mach_timespec_t ts; - ts.tv_sec = timeout_usecs / 1000000; - ts.tv_nsec = (timeout_usecs % 1000000) * 1000; + bool timed_wait(std::int64_t timeout_usecs) + { + mach_timespec_t ts; + ts.tv_sec = timeout_usecs / 1000000; + ts.tv_nsec = (timeout_usecs % 1000000) * 1000; - // added in OSX 10.10: https://developer.apple.com/library/prerelease/mac/documentation/General/Reference/APIDiffsMacOSX10_10SeedDiff/modules/Darwin.html - kern_return_t rc = semaphore_timedwait(m_sema, ts); + // added in OSX 10.10: https://developer.apple.com/library/prerelease/mac/documentation/General/Reference/APIDiffsMacOSX10_10SeedDiff/modules/Darwin.html + kern_return_t rc = semaphore_timedwait(m_sema, ts); - return rc != KERN_OPERATION_TIMED_OUT; - } + return rc != KERN_OPERATION_TIMED_OUT; + } - void signal() - { - semaphore_signal(m_sema); - } + void signal() + { + semaphore_signal(m_sema); + } - void signal(int count) - { - while (count-- > 0) - { - semaphore_signal(m_sema); - } - } - }; + void signal(int count) + { + while (count-- > 0) { + semaphore_signal(m_sema); + } + } + }; #elif defined(__unix__) - //--------------------------------------------------------- - // Semaphore (POSIX, Linux) - //--------------------------------------------------------- - class Semaphore - { - private: - sem_t m_sema; + //--------------------------------------------------------- + // Semaphore (POSIX, Linux) + //--------------------------------------------------------- + class Semaphore { + private: + sem_t m_sema; - Semaphore(const Semaphore& other); - Semaphore& operator=(const Semaphore& other); + Semaphore(const Semaphore& other); + Semaphore& operator=(const Semaphore& other); - public: - Semaphore(int initialCount = 0) - { - assert(initialCount >= 0); - sem_init(&m_sema, 0, initialCount); - } + public: + Semaphore(int initialCount = 0) + { + assert(initialCount >= 0); + sem_init(&m_sema, 0, initialCount); + } - ~Semaphore() - { - sem_destroy(&m_sema); - } + ~Semaphore() + { + sem_destroy(&m_sema); + } - void wait() - { - // http://stackoverflow.com/questions/2013181/gdb-causes-sem-wait-to-fail-with-eintr-error - int rc; - do - { - rc = sem_wait(&m_sema); - } - while (rc == -1 && errno == EINTR); - } + void wait() + { + // http://stackoverflow.com/questions/2013181/gdb-causes-sem-wait-to-fail-with-eintr-error + int rc; + do { + rc = sem_wait(&m_sema); + } while (rc == -1 && errno == EINTR); + } - bool try_wait() - { - int rc; - do { - rc = sem_trywait(&m_sema); - } while (rc == -1 && errno == EINTR); - return !(rc == -1 && errno == EAGAIN); - } + bool try_wait() + { + int rc; + do { + rc = sem_trywait(&m_sema); + } while (rc == -1 && errno == EINTR); + return !(rc == -1 && errno == EAGAIN); + } - bool timed_wait(std::uint64_t usecs) - { - struct timespec ts; - const int usecs_in_1_sec = 1000000; - const int nsecs_in_1_sec = 1000000000; - clock_gettime(CLOCK_REALTIME, &ts); - ts.tv_sec += usecs / usecs_in_1_sec; - ts.tv_nsec += (usecs % usecs_in_1_sec) * 1000; - // sem_timedwait bombs if you have more than 1e9 in tv_nsec - // so we have to clean things up before passing it in - if (ts.tv_nsec > nsecs_in_1_sec) { - ts.tv_nsec -= nsecs_in_1_sec; - ++ts.tv_sec; - } + bool timed_wait(std::uint64_t usecs) + { + struct timespec ts; + const int usecs_in_1_sec = 1000000; + const int nsecs_in_1_sec = 1000000000; + clock_gettime(CLOCK_REALTIME, &ts); + ts.tv_sec += usecs / usecs_in_1_sec; + ts.tv_nsec += (usecs % usecs_in_1_sec) * 1000; + // sem_timedwait bombs if you have more than 1e9 in tv_nsec + // so we have to clean things up before passing it in + if (ts.tv_nsec > nsecs_in_1_sec) { + ts.tv_nsec -= nsecs_in_1_sec; + ++ts.tv_sec; + } - int rc; - do { - rc = sem_timedwait(&m_sema, &ts); - } while (rc == -1 && errno == EINTR); - return !(rc == -1 && errno == ETIMEDOUT); - } + int rc; + do { + rc = sem_timedwait(&m_sema, &ts); + } while (rc == -1 && errno == EINTR); + return !(rc == -1 && errno == ETIMEDOUT); + } - void signal() - { - sem_post(&m_sema); - } + void signal() + { + sem_post(&m_sema); + } - void signal(int count) - { - while (count-- > 0) - { - sem_post(&m_sema); - } - } - }; + void signal(int count) + { + while (count-- > 0) { + sem_post(&m_sema); + } + } + }; #else #error Unsupported platform! (No semaphore wrapper available) #endif - //--------------------------------------------------------- - // LightweightSemaphore - //--------------------------------------------------------- - class LightweightSemaphore - { - public: - typedef std::make_signed::type ssize_t; - - private: - weak_atomic m_count; - Semaphore m_sema; + //--------------------------------------------------------- + // LightweightSemaphore + //--------------------------------------------------------- + class LightweightSemaphore { + public: + typedef std::make_signed::type ssize_t; - bool waitWithPartialSpinning(std::int64_t timeout_usecs = -1) - { - ssize_t oldCount; - // Is there a better way to set the initial spin count? - // If we lower it to 1000, testBenaphore becomes 15x slower on my Core i7-5930K Windows PC, - // as threads start hitting the kernel semaphore. - int spin = 10000; - while (--spin >= 0) - { - if (m_count.load() > 0) - { - m_count.fetch_add_acquire(-1); - return true; - } - compiler_fence(memory_order_acquire); // Prevent the compiler from collapsing the loop. - } - oldCount = m_count.fetch_add_acquire(-1); - if (oldCount > 0) - return true; - if (timeout_usecs < 0) - { - m_sema.wait(); - return true; - } - if (m_sema.timed_wait(timeout_usecs)) - return true; - // At this point, we've timed out waiting for the semaphore, but the - // count is still decremented indicating we may still be waiting on - // it. So we have to re-adjust the count, but only if the semaphore - // wasn't signaled enough times for us too since then. If it was, we - // need to release the semaphore too. - while (true) - { - oldCount = m_count.fetch_add_release(1); - if (oldCount < 0) - return false; // successfully restored things to the way they were - // Oh, the producer thread just signaled the semaphore after all. Try again: - oldCount = m_count.fetch_add_acquire(-1); - if (oldCount > 0 && m_sema.try_wait()) - return true; - } - } + private: + weak_atomic m_count; + Semaphore m_sema; - public: - LightweightSemaphore(ssize_t initialCount = 0) : m_count(initialCount) - { - assert(initialCount >= 0); - } + bool waitWithPartialSpinning(std::int64_t timeout_usecs = -1) + { + ssize_t oldCount; + // Is there a better way to set the initial spin count? + // If we lower it to 1000, testBenaphore becomes 15x slower on my Core i7-5930K Windows PC, + // as threads start hitting the kernel semaphore. + int spin = 10000; + while (--spin >= 0) { + if (m_count.load() > 0) { + m_count.fetch_add_acquire(-1); + return true; + } + compiler_fence(memory_order_acquire); // Prevent the compiler from collapsing the loop. + } + oldCount = m_count.fetch_add_acquire(-1); + if (oldCount > 0) + return true; + if (timeout_usecs < 0) { + m_sema.wait(); + return true; + } + if (m_sema.timed_wait(timeout_usecs)) + return true; + // At this point, we've timed out waiting for the semaphore, but the + // count is still decremented indicating we may still be waiting on + // it. So we have to re-adjust the count, but only if the semaphore + // wasn't signaled enough times for us too since then. If it was, we + // need to release the semaphore too. + while (true) { + oldCount = m_count.fetch_add_release(1); + if (oldCount < 0) + return false; // successfully restored things to the way they were + // Oh, the producer thread just signaled the semaphore after all. Try again: + oldCount = m_count.fetch_add_acquire(-1); + if (oldCount > 0 && m_sema.try_wait()) + return true; + } + } - bool tryWait() - { - if (m_count.load() > 0) - { - m_count.fetch_add_acquire(-1); - return true; - } - return false; - } + public: + LightweightSemaphore(ssize_t initialCount = 0) + : m_count(initialCount) + { + assert(initialCount >= 0); + } - void wait() - { - if (!tryWait()) - waitWithPartialSpinning(); - } + bool tryWait() + { + if (m_count.load() > 0) { + m_count.fetch_add_acquire(-1); + return true; + } + return false; + } - bool wait(std::int64_t timeout_usecs) - { - return tryWait() || waitWithPartialSpinning(timeout_usecs); - } + void wait() + { + if (!tryWait()) + waitWithPartialSpinning(); + } - void signal(ssize_t count = 1) - { - assert(count >= 0); - ssize_t oldCount = m_count.fetch_add_release(count); - assert(oldCount >= -1); - if (oldCount < 0) - { - m_sema.signal(1); - } - } - - ssize_t availableApprox() const - { - ssize_t count = m_count.load(); - return count > 0 ? count : 0; - } - }; - } // end namespace spsc_sema -} // end namespace moodycamel + bool wait(std::int64_t timeout_usecs) + { + return tryWait() || waitWithPartialSpinning(timeout_usecs); + } + + void signal(ssize_t count = 1) + { + assert(count >= 0); + ssize_t oldCount = m_count.fetch_add_release(count); + assert(oldCount >= -1); + if (oldCount < 0) { + m_sema.signal(1); + } + } + + ssize_t availableApprox() const + { + ssize_t count = m_count.load(); + return count > 0 ? count : 0; + } + }; +} // end namespace spsc_sema +} // end namespace moodycamel #if defined(AE_VCPP) && (_MSC_VER < 1700 || defined(__cplusplus_cli)) #pragma warning(pop) diff --git a/include/ur_modern_driver/queue/readerwriterqueue.h b/include/ur_modern_driver/queue/readerwriterqueue.h index ec465d6..1503dcb 100644 --- a/include/ur_modern_driver/queue/readerwriterqueue.h +++ b/include/ur_modern_driver/queue/readerwriterqueue.h @@ -5,18 +5,17 @@ #pragma once #include "atomicops.h" +#include +#include +#include // For malloc/free/abort & size_t +#include +#include #include #include -#include -#include -#include -#include -#include // For malloc/free/abort & size_t #if __cplusplus > 199711L || _MSC_VER >= 1700 // C++11 or VS2012 #include #endif - // A lock-free queue for a single-consumer, single-producer architecture. // The queue is also wait-free in the common path (except if more memory // needs to be allocated, in which case malloc is called). @@ -42,773 +41,750 @@ #ifdef AE_VCPP #pragma warning(push) -#pragma warning(disable: 4324) // structure was padded due to __declspec(align()) -#pragma warning(disable: 4820) // padding was added -#pragma warning(disable: 4127) // conditional expression is constant +#pragma warning(disable : 4324) // structure was padded due to __declspec(align()) +#pragma warning(disable : 4820) // padding was added +#pragma warning(disable : 4127) // conditional expression is constant #endif namespace moodycamel { -template -class ReaderWriterQueue -{ - // Design: Based on a queue-of-queues. The low-level queues are just - // circular buffers with front and tail indices indicating where the - // next element to dequeue is and where the next element can be enqueued, - // respectively. Each low-level queue is called a "block". Each block - // wastes exactly one element's worth of space to keep the design simple - // (if front == tail then the queue is empty, and can't be full). - // The high-level queue is a circular linked list of blocks; again there - // is a front and tail, but this time they are pointers to the blocks. - // The front block is where the next element to be dequeued is, provided - // the block is not empty. The back block is where elements are to be - // enqueued, provided the block is not full. - // The producer thread owns all the tail indices/pointers. The consumer - // thread owns all the front indices/pointers. Both threads read each - // other's variables, but only the owning thread updates them. E.g. After - // the consumer reads the producer's tail, the tail may change before the - // consumer is done dequeuing an object, but the consumer knows the tail - // will never go backwards, only forwards. - // If there is no room to enqueue an object, an additional block (of - // equal size to the last block) is added. Blocks are never removed. +template +class ReaderWriterQueue { + // Design: Based on a queue-of-queues. The low-level queues are just + // circular buffers with front and tail indices indicating where the + // next element to dequeue is and where the next element can be enqueued, + // respectively. Each low-level queue is called a "block". Each block + // wastes exactly one element's worth of space to keep the design simple + // (if front == tail then the queue is empty, and can't be full). + // The high-level queue is a circular linked list of blocks; again there + // is a front and tail, but this time they are pointers to the blocks. + // The front block is where the next element to be dequeued is, provided + // the block is not empty. The back block is where elements are to be + // enqueued, provided the block is not full. + // The producer thread owns all the tail indices/pointers. The consumer + // thread owns all the front indices/pointers. Both threads read each + // other's variables, but only the owning thread updates them. E.g. After + // the consumer reads the producer's tail, the tail may change before the + // consumer is done dequeuing an object, but the consumer knows the tail + // will never go backwards, only forwards. + // If there is no room to enqueue an object, an additional block (of + // equal size to the last block) is added. Blocks are never removed. public: - // Constructs a queue that can hold maxSize elements without further - // allocations. If more than MAX_BLOCK_SIZE elements are requested, - // then several blocks of MAX_BLOCK_SIZE each are reserved (including - // at least one extra buffer block). - explicit ReaderWriterQueue(size_t maxSize = 15) + // Constructs a queue that can hold maxSize elements without further + // allocations. If more than MAX_BLOCK_SIZE elements are requested, + // then several blocks of MAX_BLOCK_SIZE each are reserved (including + // at least one extra buffer block). + explicit ReaderWriterQueue(size_t maxSize = 15) #ifndef NDEBUG - : enqueuing(false) - ,dequeuing(false) + : enqueuing(false) + , dequeuing(false) #endif - { - assert(maxSize > 0); - assert(MAX_BLOCK_SIZE == ceilToPow2(MAX_BLOCK_SIZE) && "MAX_BLOCK_SIZE must be a power of 2"); - assert(MAX_BLOCK_SIZE >= 2 && "MAX_BLOCK_SIZE must be at least 2"); - - Block* firstBlock = nullptr; - - largestBlockSize = ceilToPow2(maxSize + 1); // We need a spare slot to fit maxSize elements in the block - if (largestBlockSize > MAX_BLOCK_SIZE * 2) { - // We need a spare block in case the producer is writing to a different block the consumer is reading from, and - // wants to enqueue the maximum number of elements. We also need a spare element in each block to avoid the ambiguity - // between front == tail meaning "empty" and "full". - // So the effective number of slots that are guaranteed to be usable at any time is the block size - 1 times the - // number of blocks - 1. Solving for maxSize and applying a ceiling to the division gives us (after simplifying): - size_t initialBlockCount = (maxSize + MAX_BLOCK_SIZE * 2 - 3) / (MAX_BLOCK_SIZE - 1); - largestBlockSize = MAX_BLOCK_SIZE; - Block* lastBlock = nullptr; - for (size_t i = 0; i != initialBlockCount; ++i) { - auto block = make_block(largestBlockSize); - if (block == nullptr) { + { + assert(maxSize > 0); + assert(MAX_BLOCK_SIZE == ceilToPow2(MAX_BLOCK_SIZE) && "MAX_BLOCK_SIZE must be a power of 2"); + assert(MAX_BLOCK_SIZE >= 2 && "MAX_BLOCK_SIZE must be at least 2"); + + Block* firstBlock = nullptr; + + largestBlockSize = ceilToPow2(maxSize + 1); // We need a spare slot to fit maxSize elements in the block + if (largestBlockSize > MAX_BLOCK_SIZE * 2) { + // We need a spare block in case the producer is writing to a different block the consumer is reading from, and + // wants to enqueue the maximum number of elements. We also need a spare element in each block to avoid the ambiguity + // between front == tail meaning "empty" and "full". + // So the effective number of slots that are guaranteed to be usable at any time is the block size - 1 times the + // number of blocks - 1. Solving for maxSize and applying a ceiling to the division gives us (after simplifying): + size_t initialBlockCount = (maxSize + MAX_BLOCK_SIZE * 2 - 3) / (MAX_BLOCK_SIZE - 1); + largestBlockSize = MAX_BLOCK_SIZE; + Block* lastBlock = nullptr; + for (size_t i = 0; i != initialBlockCount; ++i) { + auto block = make_block(largestBlockSize); + if (block == nullptr) { #ifdef MOODYCAMEL_EXCEPTIONS_ENABLED - throw std::bad_alloc(); + throw std::bad_alloc(); #else - abort(); + abort(); #endif - } - if (firstBlock == nullptr) { - firstBlock = block; - } - else { - lastBlock->next = block; - } - lastBlock = block; - block->next = firstBlock; - } - } - else { - firstBlock = make_block(largestBlockSize); - if (firstBlock == nullptr) { + } + if (firstBlock == nullptr) { + firstBlock = block; + } else { + lastBlock->next = block; + } + lastBlock = block; + block->next = firstBlock; + } + } else { + firstBlock = make_block(largestBlockSize); + if (firstBlock == nullptr) { #ifdef MOODYCAMEL_EXCEPTIONS_ENABLED - throw std::bad_alloc(); + throw std::bad_alloc(); #else - abort(); + abort(); #endif - } - firstBlock->next = firstBlock; - } - frontBlock = firstBlock; - tailBlock = firstBlock; - - // Make sure the reader/writer threads will have the initialized memory setup above: - fence(memory_order_sync); - } + } + firstBlock->next = firstBlock; + } + frontBlock = firstBlock; + tailBlock = firstBlock; - // Note: The queue should not be accessed concurrently while it's - // being deleted. It's up to the user to synchronize this. - ~ReaderWriterQueue() - { - // Make sure we get the latest version of all variables from other CPUs: - fence(memory_order_sync); + // Make sure the reader/writer threads will have the initialized memory setup above: + fence(memory_order_sync); + } - // Destroy any remaining objects in queue and free memory - Block* frontBlock_ = frontBlock; - Block* block = frontBlock_; - do { - Block* nextBlock = block->next; - size_t blockFront = block->front; - size_t blockTail = block->tail; + // Note: The queue should not be accessed concurrently while it's + // being deleted. It's up to the user to synchronize this. + ~ReaderWriterQueue() + { + // Make sure we get the latest version of all variables from other CPUs: + fence(memory_order_sync); - for (size_t i = blockFront; i != blockTail; i = (i + 1) & block->sizeMask) { - auto element = reinterpret_cast(block->data + i * sizeof(T)); - element->~T(); - (void)element; - } - - auto rawBlock = block->rawThis; - block->~Block(); - std::free(rawBlock); - block = nextBlock; - } while (block != frontBlock_); - } + // Destroy any remaining objects in queue and free memory + Block* frontBlock_ = frontBlock; + Block* block = frontBlock_; + do { + Block* nextBlock = block->next; + size_t blockFront = block->front; + size_t blockTail = block->tail; + for (size_t i = blockFront; i != blockTail; i = (i + 1) & block->sizeMask) { + auto element = reinterpret_cast(block->data + i * sizeof(T)); + element->~T(); + (void)element; + } - // Enqueues a copy of element if there is room in the queue. - // Returns true if the element was enqueued, false otherwise. - // Does not allocate memory. - AE_FORCEINLINE bool try_enqueue(T const& element) - { - return inner_enqueue(element); - } + auto rawBlock = block->rawThis; + block->~Block(); + std::free(rawBlock); + block = nextBlock; + } while (block != frontBlock_); + } - // Enqueues a moved copy of element if there is room in the queue. - // Returns true if the element was enqueued, false otherwise. - // Does not allocate memory. - AE_FORCEINLINE bool try_enqueue(T&& element) - { - return inner_enqueue(std::forward(element)); - } + // Enqueues a copy of element if there is room in the queue. + // Returns true if the element was enqueued, false otherwise. + // Does not allocate memory. + AE_FORCEINLINE bool try_enqueue(T const& element) + { + return inner_enqueue(element); + } + // Enqueues a moved copy of element if there is room in the queue. + // Returns true if the element was enqueued, false otherwise. + // Does not allocate memory. + AE_FORCEINLINE bool try_enqueue(T&& element) + { + return inner_enqueue(std::forward(element)); + } - // Enqueues a copy of element on the queue. - // Allocates an additional block of memory if needed. - // Only fails (returns false) if memory allocation fails. - AE_FORCEINLINE bool enqueue(T const& element) - { - return inner_enqueue(element); - } + // Enqueues a copy of element on the queue. + // Allocates an additional block of memory if needed. + // Only fails (returns false) if memory allocation fails. + AE_FORCEINLINE bool enqueue(T const& element) + { + return inner_enqueue(element); + } - // Enqueues a moved copy of element on the queue. - // Allocates an additional block of memory if needed. - // Only fails (returns false) if memory allocation fails. - AE_FORCEINLINE bool enqueue(T&& element) - { - return inner_enqueue(std::forward(element)); - } + // Enqueues a moved copy of element on the queue. + // Allocates an additional block of memory if needed. + // Only fails (returns false) if memory allocation fails. + AE_FORCEINLINE bool enqueue(T&& element) + { + return inner_enqueue(std::forward(element)); + } - - // Attempts to dequeue an element; if the queue is empty, - // returns false instead. If the queue has at least one element, - // moves front to result using operator=, then returns true. - template - bool try_dequeue(U& result) - { + // Attempts to dequeue an element; if the queue is empty, + // returns false instead. If the queue has at least one element, + // moves front to result using operator=, then returns true. + template + bool try_dequeue(U& result) + { #ifndef NDEBUG - ReentrantGuard guard(this->dequeuing); + ReentrantGuard guard(this->dequeuing); #endif - // High-level pseudocode: - // Remember where the tail block is - // If the front block has an element in it, dequeue it - // Else - // If front block was the tail block when we entered the function, return false - // Else advance to next block and dequeue the item there + // High-level pseudocode: + // Remember where the tail block is + // If the front block has an element in it, dequeue it + // Else + // If front block was the tail block when we entered the function, return false + // Else advance to next block and dequeue the item there - // Note that we have to use the value of the tail block from before we check if the front - // block is full or not, in case the front block is empty and then, before we check if the - // tail block is at the front block or not, the producer fills up the front block *and - // moves on*, which would make us skip a filled block. Seems unlikely, but was consistently - // reproducible in practice. - // In order to avoid overhead in the common case, though, we do a double-checked pattern - // where we have the fast path if the front block is not empty, then read the tail block, - // then re-read the front block and check if it's not empty again, then check if the tail - // block has advanced. - - Block* frontBlock_ = frontBlock.load(); - size_t blockTail = frontBlock_->localTail; - size_t blockFront = frontBlock_->front.load(); - - if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) { - fence(memory_order_acquire); - - non_empty_front_block: - // Front block not empty, dequeue from here - auto element = reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); - result = std::move(*element); - element->~T(); + // Note that we have to use the value of the tail block from before we check if the front + // block is full or not, in case the front block is empty and then, before we check if the + // tail block is at the front block or not, the producer fills up the front block *and + // moves on*, which would make us skip a filled block. Seems unlikely, but was consistently + // reproducible in practice. + // In order to avoid overhead in the common case, though, we do a double-checked pattern + // where we have the fast path if the front block is not empty, then read the tail block, + // then re-read the front block and check if it's not empty again, then check if the tail + // block has advanced. - blockFront = (blockFront + 1) & frontBlock_->sizeMask; + Block* frontBlock_ = frontBlock.load(); + size_t blockTail = frontBlock_->localTail; + size_t blockFront = frontBlock_->front.load(); - fence(memory_order_release); - frontBlock_->front = blockFront; - } - else if (frontBlock_ != tailBlock.load()) { - fence(memory_order_acquire); + if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) { + fence(memory_order_acquire); - frontBlock_ = frontBlock.load(); - blockTail = frontBlock_->localTail = frontBlock_->tail.load(); - blockFront = frontBlock_->front.load(); - fence(memory_order_acquire); - - if (blockFront != blockTail) { - // Oh look, the front block isn't empty after all - goto non_empty_front_block; - } - - // Front block is empty but there's another block ahead, advance to it - Block* nextBlock = frontBlock_->next; - // Don't need an acquire fence here since next can only ever be set on the tailBlock, - // and we're not the tailBlock, and we did an acquire earlier after reading tailBlock which - // ensures next is up-to-date on this CPU in case we recently were at tailBlock. + non_empty_front_block: + // Front block not empty, dequeue from here + auto element = reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); + result = std::move(*element); + element->~T(); - size_t nextBlockFront = nextBlock->front.load(); - size_t nextBlockTail = nextBlock->localTail = nextBlock->tail.load(); - fence(memory_order_acquire); + blockFront = (blockFront + 1) & frontBlock_->sizeMask; - // Since the tailBlock is only ever advanced after being written to, - // we know there's for sure an element to dequeue on it - assert(nextBlockFront != nextBlockTail); - AE_UNUSED(nextBlockTail); + fence(memory_order_release); + frontBlock_->front = blockFront; + } else if (frontBlock_ != tailBlock.load()) { + fence(memory_order_acquire); - // We're done with this block, let the producer use it if it needs - fence(memory_order_release); // Expose possibly pending changes to frontBlock->front from last dequeue - frontBlock = frontBlock_ = nextBlock; + frontBlock_ = frontBlock.load(); + blockTail = frontBlock_->localTail = frontBlock_->tail.load(); + blockFront = frontBlock_->front.load(); + fence(memory_order_acquire); - compiler_fence(memory_order_release); // Not strictly needed + if (blockFront != blockTail) { + // Oh look, the front block isn't empty after all + goto non_empty_front_block; + } - auto element = reinterpret_cast(frontBlock_->data + nextBlockFront * sizeof(T)); - - result = std::move(*element); - element->~T(); + // Front block is empty but there's another block ahead, advance to it + Block* nextBlock = frontBlock_->next; + // Don't need an acquire fence here since next can only ever be set on the tailBlock, + // and we're not the tailBlock, and we did an acquire earlier after reading tailBlock which + // ensures next is up-to-date on this CPU in case we recently were at tailBlock. - nextBlockFront = (nextBlockFront + 1) & frontBlock_->sizeMask; - - fence(memory_order_release); - frontBlock_->front = nextBlockFront; - } - else { - // No elements in current block and no other block to advance to - return false; - } + size_t nextBlockFront = nextBlock->front.load(); + size_t nextBlockTail = nextBlock->localTail = nextBlock->tail.load(); + fence(memory_order_acquire); - return true; - } + // Since the tailBlock is only ever advanced after being written to, + // we know there's for sure an element to dequeue on it + assert(nextBlockFront != nextBlockTail); + AE_UNUSED(nextBlockTail); + // We're done with this block, let the producer use it if it needs + fence(memory_order_release); // Expose possibly pending changes to frontBlock->front from last dequeue + frontBlock = frontBlock_ = nextBlock; - // Returns a pointer to the front element in the queue (the one that - // would be removed next by a call to `try_dequeue` or `pop`). If the - // queue appears empty at the time the method is called, nullptr is - // returned instead. - // Must be called only from the consumer thread. - T* peek() - { + compiler_fence(memory_order_release); // Not strictly needed + + auto element = reinterpret_cast(frontBlock_->data + nextBlockFront * sizeof(T)); + + result = std::move(*element); + element->~T(); + + nextBlockFront = (nextBlockFront + 1) & frontBlock_->sizeMask; + + fence(memory_order_release); + frontBlock_->front = nextBlockFront; + } else { + // No elements in current block and no other block to advance to + return false; + } + + return true; + } + + // Returns a pointer to the front element in the queue (the one that + // would be removed next by a call to `try_dequeue` or `pop`). If the + // queue appears empty at the time the method is called, nullptr is + // returned instead. + // Must be called only from the consumer thread. + T* peek() + { #ifndef NDEBUG - ReentrantGuard guard(this->dequeuing); + ReentrantGuard guard(this->dequeuing); #endif - // See try_dequeue() for reasoning + // See try_dequeue() for reasoning - Block* frontBlock_ = frontBlock.load(); - size_t blockTail = frontBlock_->localTail; - size_t blockFront = frontBlock_->front.load(); - - if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) { - fence(memory_order_acquire); - non_empty_front_block: - return reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); - } - else if (frontBlock_ != tailBlock.load()) { - fence(memory_order_acquire); - frontBlock_ = frontBlock.load(); - blockTail = frontBlock_->localTail = frontBlock_->tail.load(); - blockFront = frontBlock_->front.load(); - fence(memory_order_acquire); - - if (blockFront != blockTail) { - goto non_empty_front_block; - } - - Block* nextBlock = frontBlock_->next; - - size_t nextBlockFront = nextBlock->front.load(); - fence(memory_order_acquire); + Block* frontBlock_ = frontBlock.load(); + size_t blockTail = frontBlock_->localTail; + size_t blockFront = frontBlock_->front.load(); - assert(nextBlockFront != nextBlock->tail.load()); - return reinterpret_cast(nextBlock->data + nextBlockFront * sizeof(T)); - } - - return nullptr; - } - - // Removes the front element from the queue, if any, without returning it. - // Returns true on success, or false if the queue appeared empty at the time - // `pop` was called. - bool pop() - { + if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) { + fence(memory_order_acquire); + non_empty_front_block: + return reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); + } else if (frontBlock_ != tailBlock.load()) { + fence(memory_order_acquire); + frontBlock_ = frontBlock.load(); + blockTail = frontBlock_->localTail = frontBlock_->tail.load(); + blockFront = frontBlock_->front.load(); + fence(memory_order_acquire); + + if (blockFront != blockTail) { + goto non_empty_front_block; + } + + Block* nextBlock = frontBlock_->next; + + size_t nextBlockFront = nextBlock->front.load(); + fence(memory_order_acquire); + + assert(nextBlockFront != nextBlock->tail.load()); + return reinterpret_cast(nextBlock->data + nextBlockFront * sizeof(T)); + } + + return nullptr; + } + + // Removes the front element from the queue, if any, without returning it. + // Returns true on success, or false if the queue appeared empty at the time + // `pop` was called. + bool pop() + { #ifndef NDEBUG - ReentrantGuard guard(this->dequeuing); + ReentrantGuard guard(this->dequeuing); #endif - // See try_dequeue() for reasoning - - Block* frontBlock_ = frontBlock.load(); - size_t blockTail = frontBlock_->localTail; - size_t blockFront = frontBlock_->front.load(); - - if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) { - fence(memory_order_acquire); - - non_empty_front_block: - auto element = reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); - element->~T(); + // See try_dequeue() for reasoning - blockFront = (blockFront + 1) & frontBlock_->sizeMask; + Block* frontBlock_ = frontBlock.load(); + size_t blockTail = frontBlock_->localTail; + size_t blockFront = frontBlock_->front.load(); - fence(memory_order_release); - frontBlock_->front = blockFront; - } - else if (frontBlock_ != tailBlock.load()) { - fence(memory_order_acquire); - frontBlock_ = frontBlock.load(); - blockTail = frontBlock_->localTail = frontBlock_->tail.load(); - blockFront = frontBlock_->front.load(); - fence(memory_order_acquire); - - if (blockFront != blockTail) { - goto non_empty_front_block; - } - - // Front block is empty but there's another block ahead, advance to it - Block* nextBlock = frontBlock_->next; - - size_t nextBlockFront = nextBlock->front.load(); - size_t nextBlockTail = nextBlock->localTail = nextBlock->tail.load(); - fence(memory_order_acquire); + if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) { + fence(memory_order_acquire); - assert(nextBlockFront != nextBlockTail); - AE_UNUSED(nextBlockTail); + non_empty_front_block: + auto element = reinterpret_cast(frontBlock_->data + blockFront * sizeof(T)); + element->~T(); - fence(memory_order_release); - frontBlock = frontBlock_ = nextBlock; + blockFront = (blockFront + 1) & frontBlock_->sizeMask; - compiler_fence(memory_order_release); + fence(memory_order_release); + frontBlock_->front = blockFront; + } else if (frontBlock_ != tailBlock.load()) { + fence(memory_order_acquire); + frontBlock_ = frontBlock.load(); + blockTail = frontBlock_->localTail = frontBlock_->tail.load(); + blockFront = frontBlock_->front.load(); + fence(memory_order_acquire); - auto element = reinterpret_cast(frontBlock_->data + nextBlockFront * sizeof(T)); - element->~T(); + if (blockFront != blockTail) { + goto non_empty_front_block; + } - nextBlockFront = (nextBlockFront + 1) & frontBlock_->sizeMask; - - fence(memory_order_release); - frontBlock_->front = nextBlockFront; - } - else { - // No elements in current block and no other block to advance to - return false; - } + // Front block is empty but there's another block ahead, advance to it + Block* nextBlock = frontBlock_->next; - return true; - } - - // Returns the approximate number of items currently in the queue. - // Safe to call from both the producer and consumer threads. - inline size_t size_approx() const - { - size_t result = 0; - Block* frontBlock_ = frontBlock.load(); - Block* block = frontBlock_; - do { - fence(memory_order_acquire); - size_t blockFront = block->front.load(); - size_t blockTail = block->tail.load(); - result += (blockTail - blockFront) & block->sizeMask; - block = block->next.load(); - } while (block != frontBlock_); - return result; - } + size_t nextBlockFront = nextBlock->front.load(); + size_t nextBlockTail = nextBlock->localTail = nextBlock->tail.load(); + fence(memory_order_acquire); + assert(nextBlockFront != nextBlockTail); + AE_UNUSED(nextBlockTail); + + fence(memory_order_release); + frontBlock = frontBlock_ = nextBlock; + + compiler_fence(memory_order_release); + + auto element = reinterpret_cast(frontBlock_->data + nextBlockFront * sizeof(T)); + element->~T(); + + nextBlockFront = (nextBlockFront + 1) & frontBlock_->sizeMask; + + fence(memory_order_release); + frontBlock_->front = nextBlockFront; + } else { + // No elements in current block and no other block to advance to + return false; + } + + return true; + } + + // Returns the approximate number of items currently in the queue. + // Safe to call from both the producer and consumer threads. + inline size_t size_approx() const + { + size_t result = 0; + Block* frontBlock_ = frontBlock.load(); + Block* block = frontBlock_; + do { + fence(memory_order_acquire); + size_t blockFront = block->front.load(); + size_t blockTail = block->tail.load(); + result += (blockTail - blockFront) & block->sizeMask; + block = block->next.load(); + } while (block != frontBlock_); + return result; + } private: - enum AllocationMode { CanAlloc, CannotAlloc }; + enum AllocationMode { CanAlloc, + CannotAlloc }; - template - bool inner_enqueue(U&& element) - { + template + bool inner_enqueue(U&& element) + { #ifndef NDEBUG - ReentrantGuard guard(this->enqueuing); + ReentrantGuard guard(this->enqueuing); #endif - // High-level pseudocode (assuming we're allowed to alloc a new block): - // If room in tail block, add to tail - // Else check next block - // If next block is not the head block, enqueue on next block - // Else create a new block and enqueue there - // Advance tail to the block we just enqueued to + // High-level pseudocode (assuming we're allowed to alloc a new block): + // If room in tail block, add to tail + // Else check next block + // If next block is not the head block, enqueue on next block + // Else create a new block and enqueue there + // Advance tail to the block we just enqueued to - Block* tailBlock_ = tailBlock.load(); - size_t blockFront = tailBlock_->localFront; - size_t blockTail = tailBlock_->tail.load(); + Block* tailBlock_ = tailBlock.load(); + size_t blockFront = tailBlock_->localFront; + size_t blockTail = tailBlock_->tail.load(); - size_t nextBlockTail = (blockTail + 1) & tailBlock_->sizeMask; - if (nextBlockTail != blockFront || nextBlockTail != (tailBlock_->localFront = tailBlock_->front.load())) { - fence(memory_order_acquire); - // This block has room for at least one more element - char* location = tailBlock_->data + blockTail * sizeof(T); - new (location) T(std::forward(element)); + size_t nextBlockTail = (blockTail + 1) & tailBlock_->sizeMask; + if (nextBlockTail != blockFront || nextBlockTail != (tailBlock_->localFront = tailBlock_->front.load())) { + fence(memory_order_acquire); + // This block has room for at least one more element + char* location = tailBlock_->data + blockTail * sizeof(T); + new (location) T(std::forward(element)); - fence(memory_order_release); - tailBlock_->tail = nextBlockTail; - } - else { - fence(memory_order_acquire); - if (tailBlock_->next.load() != frontBlock) { - // Note that the reason we can't advance to the frontBlock and start adding new entries there - // is because if we did, then dequeue would stay in that block, eventually reading the new values, - // instead of advancing to the next full block (whose values were enqueued first and so should be - // consumed first). - - fence(memory_order_acquire); // Ensure we get latest writes if we got the latest frontBlock + fence(memory_order_release); + tailBlock_->tail = nextBlockTail; + } else { + fence(memory_order_acquire); + if (tailBlock_->next.load() != frontBlock) { + // Note that the reason we can't advance to the frontBlock and start adding new entries there + // is because if we did, then dequeue would stay in that block, eventually reading the new values, + // instead of advancing to the next full block (whose values were enqueued first and so should be + // consumed first). - // tailBlock is full, but there's a free block ahead, use it - Block* tailBlockNext = tailBlock_->next.load(); - size_t nextBlockFront = tailBlockNext->localFront = tailBlockNext->front.load(); - nextBlockTail = tailBlockNext->tail.load(); - fence(memory_order_acquire); + fence(memory_order_acquire); // Ensure we get latest writes if we got the latest frontBlock - // This block must be empty since it's not the head block and we - // go through the blocks in a circle - assert(nextBlockFront == nextBlockTail); - tailBlockNext->localFront = nextBlockFront; + // tailBlock is full, but there's a free block ahead, use it + Block* tailBlockNext = tailBlock_->next.load(); + size_t nextBlockFront = tailBlockNext->localFront = tailBlockNext->front.load(); + nextBlockTail = tailBlockNext->tail.load(); + fence(memory_order_acquire); - char* location = tailBlockNext->data + nextBlockTail * sizeof(T); - new (location) T(std::forward(element)); + // This block must be empty since it's not the head block and we + // go through the blocks in a circle + assert(nextBlockFront == nextBlockTail); + tailBlockNext->localFront = nextBlockFront; - tailBlockNext->tail = (nextBlockTail + 1) & tailBlockNext->sizeMask; + char* location = tailBlockNext->data + nextBlockTail * sizeof(T); + new (location) T(std::forward(element)); - fence(memory_order_release); - tailBlock = tailBlockNext; - } - else if (canAlloc == CanAlloc) { - // tailBlock is full and there's no free block ahead; create a new block - auto newBlockSize = largestBlockSize >= MAX_BLOCK_SIZE ? largestBlockSize : largestBlockSize * 2; - auto newBlock = make_block(newBlockSize); - if (newBlock == nullptr) { - // Could not allocate a block! - return false; - } - largestBlockSize = newBlockSize; + tailBlockNext->tail = (nextBlockTail + 1) & tailBlockNext->sizeMask; - new (newBlock->data) T(std::forward(element)); + fence(memory_order_release); + tailBlock = tailBlockNext; + } else if (canAlloc == CanAlloc) { + // tailBlock is full and there's no free block ahead; create a new block + auto newBlockSize = largestBlockSize >= MAX_BLOCK_SIZE ? largestBlockSize : largestBlockSize * 2; + auto newBlock = make_block(newBlockSize); + if (newBlock == nullptr) { + // Could not allocate a block! + return false; + } + largestBlockSize = newBlockSize; - assert(newBlock->front == 0); - newBlock->tail = newBlock->localTail = 1; + new (newBlock->data) T(std::forward(element)); - newBlock->next = tailBlock_->next.load(); - tailBlock_->next = newBlock; + assert(newBlock->front == 0); + newBlock->tail = newBlock->localTail = 1; - // Might be possible for the dequeue thread to see the new tailBlock->next - // *without* seeing the new tailBlock value, but this is OK since it can't - // advance to the next block until tailBlock is set anyway (because the only - // case where it could try to read the next is if it's already at the tailBlock, - // and it won't advance past tailBlock in any circumstance). - - fence(memory_order_release); - tailBlock = newBlock; - } - else if (canAlloc == CannotAlloc) { - // Would have had to allocate a new block to enqueue, but not allowed - return false; - } - else { - assert(false && "Should be unreachable code"); - return false; - } - } + newBlock->next = tailBlock_->next.load(); + tailBlock_->next = newBlock; - return true; - } + // Might be possible for the dequeue thread to see the new tailBlock->next + // *without* seeing the new tailBlock value, but this is OK since it can't + // advance to the next block until tailBlock is set anyway (because the only + // case where it could try to read the next is if it's already at the tailBlock, + // and it won't advance past tailBlock in any circumstance). + fence(memory_order_release); + tailBlock = newBlock; + } else if (canAlloc == CannotAlloc) { + // Would have had to allocate a new block to enqueue, but not allowed + return false; + } else { + assert(false && "Should be unreachable code"); + return false; + } + } - // Disable copying - ReaderWriterQueue(ReaderWriterQueue const&) { } + return true; + } - // Disable assignment - ReaderWriterQueue& operator=(ReaderWriterQueue const&) { } + // Disable copying + ReaderWriterQueue(ReaderWriterQueue const&) {} + // Disable assignment + ReaderWriterQueue& operator=(ReaderWriterQueue const&) {} + AE_FORCEINLINE static size_t ceilToPow2(size_t x) + { + // From http://graphics.stanford.edu/~seander/bithacks.html#RoundUpPowerOf2 + --x; + x |= x >> 1; + x |= x >> 2; + x |= x >> 4; + for (size_t i = 1; i < sizeof(size_t); i <<= 1) { + x |= x >> (i << 3); + } + ++x; + return x; + } + + template + static AE_FORCEINLINE char* align_for(char* ptr) + { + const std::size_t alignment = std::alignment_of::value; + return ptr + (alignment - (reinterpret_cast(ptr) % alignment)) % alignment; + } - AE_FORCEINLINE static size_t ceilToPow2(size_t x) - { - // From http://graphics.stanford.edu/~seander/bithacks.html#RoundUpPowerOf2 - --x; - x |= x >> 1; - x |= x >> 2; - x |= x >> 4; - for (size_t i = 1; i < sizeof(size_t); i <<= 1) { - x |= x >> (i << 3); - } - ++x; - return x; - } - - template - static AE_FORCEINLINE char* align_for(char* ptr) - { - const std::size_t alignment = std::alignment_of::value; - return ptr + (alignment - (reinterpret_cast(ptr) % alignment)) % alignment; - } private: #ifndef NDEBUG - struct ReentrantGuard - { - ReentrantGuard(bool& _inSection) - : inSection(_inSection) - { - assert(!inSection && "ReaderWriterQueue does not support enqueuing or dequeuing elements from other elements' ctors and dtors"); - inSection = true; - } + struct ReentrantGuard { + ReentrantGuard(bool& _inSection) + : inSection(_inSection) + { + assert(!inSection && "ReaderWriterQueue does not support enqueuing or dequeuing elements from other elements' ctors and dtors"); + inSection = true; + } - ~ReentrantGuard() { inSection = false; } + ~ReentrantGuard() { inSection = false; } - private: - ReentrantGuard& operator=(ReentrantGuard const&); + private: + ReentrantGuard& operator=(ReentrantGuard const&); - private: - bool& inSection; - }; + private: + bool& inSection; + }; #endif - struct Block - { - // Avoid false-sharing by putting highly contended variables on their own cache lines - weak_atomic front; // (Atomic) Elements are read from here - size_t localTail; // An uncontended shadow copy of tail, owned by the consumer - - char cachelineFiller0[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic) - sizeof(size_t)]; - weak_atomic tail; // (Atomic) Elements are enqueued here - size_t localFront; - - char cachelineFiller1[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic) - sizeof(size_t)]; // next isn't very contended, but we don't want it on the same cache line as tail (which is) - weak_atomic next; // (Atomic) - - char* data; // Contents (on heap) are aligned to T's alignment + struct Block { + // Avoid false-sharing by putting highly contended variables on their own cache lines + weak_atomic front; // (Atomic) Elements are read from here + size_t localTail; // An uncontended shadow copy of tail, owned by the consumer - const size_t sizeMask; + char cachelineFiller0[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic) - sizeof(size_t)]; + weak_atomic tail; // (Atomic) Elements are enqueued here + size_t localFront; + char cachelineFiller1[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic) - sizeof(size_t)]; // next isn't very contended, but we don't want it on the same cache line as tail (which is) + weak_atomic next; // (Atomic) - // size must be a power of two (and greater than 0) - Block(size_t const& _size, char* _rawThis, char* _data) - : front(0), localTail(0), tail(0), localFront(0), next(nullptr), data(_data), sizeMask(_size - 1), rawThis(_rawThis) - { - } + char* data; // Contents (on heap) are aligned to T's alignment - private: - // C4512 - Assignment operator could not be generated - Block& operator=(Block const&); + const size_t sizeMask; - public: - char* rawThis; - }; - - - static Block* make_block(size_t capacity) - { - // Allocate enough memory for the block itself, as well as all the elements it will contain - auto size = sizeof(Block) + std::alignment_of::value - 1; - size += sizeof(T) * capacity + std::alignment_of::value - 1; - auto newBlockRaw = static_cast(std::malloc(size)); - if (newBlockRaw == nullptr) { - return nullptr; - } - - auto newBlockAligned = align_for(newBlockRaw); - auto newBlockData = align_for(newBlockAligned + sizeof(Block)); - return new (newBlockAligned) Block(capacity, newBlockRaw, newBlockData); - } + // size must be a power of two (and greater than 0) + Block(size_t const& _size, char* _rawThis, char* _data) + : front(0) + , localTail(0) + , tail(0) + , localFront(0) + , next(nullptr) + , data(_data) + , sizeMask(_size - 1) + , rawThis(_rawThis) + { + } + + private: + // C4512 - Assignment operator could not be generated + Block& operator=(Block const&); + + public: + char* rawThis; + }; + + static Block* make_block(size_t capacity) + { + // Allocate enough memory for the block itself, as well as all the elements it will contain + auto size = sizeof(Block) + std::alignment_of::value - 1; + size += sizeof(T) * capacity + std::alignment_of::value - 1; + auto newBlockRaw = static_cast(std::malloc(size)); + if (newBlockRaw == nullptr) { + return nullptr; + } + + auto newBlockAligned = align_for(newBlockRaw); + auto newBlockData = align_for(newBlockAligned + sizeof(Block)); + return new (newBlockAligned) Block(capacity, newBlockRaw, newBlockData); + } private: - weak_atomic frontBlock; // (Atomic) Elements are enqueued to this block - - char cachelineFiller[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic)]; - weak_atomic tailBlock; // (Atomic) Elements are dequeued from this block + weak_atomic frontBlock; // (Atomic) Elements are enqueued to this block - size_t largestBlockSize; + char cachelineFiller[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic)]; + weak_atomic tailBlock; // (Atomic) Elements are dequeued from this block + + size_t largestBlockSize; #ifndef NDEBUG - bool enqueuing; - bool dequeuing; + bool enqueuing; + bool dequeuing; #endif }; // Like ReaderWriterQueue, but also providees blocking operations -template -class BlockingReaderWriterQueue -{ +template +class BlockingReaderWriterQueue { private: - typedef ::moodycamel::ReaderWriterQueue ReaderWriterQueue; - + typedef ::moodycamel::ReaderWriterQueue ReaderWriterQueue; + public: - explicit BlockingReaderWriterQueue(size_t maxSize = 15) - : inner(maxSize) - { } + explicit BlockingReaderWriterQueue(size_t maxSize = 15) + : inner(maxSize) + { + } - - // Enqueues a copy of element if there is room in the queue. - // Returns true if the element was enqueued, false otherwise. - // Does not allocate memory. - AE_FORCEINLINE bool try_enqueue(T const& element) - { - if (inner.try_enqueue(element)) { - sema.signal(); - return true; - } - return false; - } + // Enqueues a copy of element if there is room in the queue. + // Returns true if the element was enqueued, false otherwise. + // Does not allocate memory. + AE_FORCEINLINE bool try_enqueue(T const& element) + { + if (inner.try_enqueue(element)) { + sema.signal(); + return true; + } + return false; + } - // Enqueues a moved copy of element if there is room in the queue. - // Returns true if the element was enqueued, false otherwise. - // Does not allocate memory. - AE_FORCEINLINE bool try_enqueue(T&& element) - { - if (inner.try_enqueue(std::forward(element))) { - sema.signal(); - return true; - } - return false; - } + // Enqueues a moved copy of element if there is room in the queue. + // Returns true if the element was enqueued, false otherwise. + // Does not allocate memory. + AE_FORCEINLINE bool try_enqueue(T&& element) + { + if (inner.try_enqueue(std::forward(element))) { + sema.signal(); + return true; + } + return false; + } + // Enqueues a copy of element on the queue. + // Allocates an additional block of memory if needed. + // Only fails (returns false) if memory allocation fails. + AE_FORCEINLINE bool enqueue(T const& element) + { + if (inner.enqueue(element)) { + sema.signal(); + return true; + } + return false; + } - // Enqueues a copy of element on the queue. - // Allocates an additional block of memory if needed. - // Only fails (returns false) if memory allocation fails. - AE_FORCEINLINE bool enqueue(T const& element) - { - if (inner.enqueue(element)) { - sema.signal(); - return true; - } - return false; - } + // Enqueues a moved copy of element on the queue. + // Allocates an additional block of memory if needed. + // Only fails (returns false) if memory allocation fails. + AE_FORCEINLINE bool enqueue(T&& element) + { + if (inner.enqueue(std::forward(element))) { + sema.signal(); + return true; + } + return false; + } - // Enqueues a moved copy of element on the queue. - // Allocates an additional block of memory if needed. - // Only fails (returns false) if memory allocation fails. - AE_FORCEINLINE bool enqueue(T&& element) - { - if (inner.enqueue(std::forward(element))) { - sema.signal(); - return true; - } - return false; - } + // Attempts to dequeue an element; if the queue is empty, + // returns false instead. If the queue has at least one element, + // moves front to result using operator=, then returns true. + template + bool try_dequeue(U& result) + { + if (sema.tryWait()) { + bool success = inner.try_dequeue(result); + assert(success); + AE_UNUSED(success); + return true; + } + return false; + } + // Attempts to dequeue an element; if the queue is empty, + // waits until an element is available, then dequeues it. + template + void wait_dequeue(U& result) + { + sema.wait(); + bool success = inner.try_dequeue(result); + AE_UNUSED(result); + assert(success); + AE_UNUSED(success); + } - // Attempts to dequeue an element; if the queue is empty, - // returns false instead. If the queue has at least one element, - // moves front to result using operator=, then returns true. - template - bool try_dequeue(U& result) - { - if (sema.tryWait()) { - bool success = inner.try_dequeue(result); - assert(success); - AE_UNUSED(success); - return true; - } - return false; - } - - - // Attempts to dequeue an element; if the queue is empty, - // waits until an element is available, then dequeues it. - template - void wait_dequeue(U& result) - { - sema.wait(); - bool success = inner.try_dequeue(result); - AE_UNUSED(result); - assert(success); - AE_UNUSED(success); - } - - - // Attempts to dequeue an element; if the queue is empty, - // waits until an element is available up to the specified timeout, - // then dequeues it and returns true, or returns false if the timeout - // expires before an element can be dequeued. - // Using a negative timeout indicates an indefinite timeout, - // and is thus functionally equivalent to calling wait_dequeue. - template - bool wait_dequeue_timed(U& result, std::int64_t timeout_usecs) - { - if (!sema.wait(timeout_usecs)) { - return false; - } - bool success = inner.try_dequeue(result); - AE_UNUSED(result); - assert(success); - AE_UNUSED(success); - return true; - } - + // Attempts to dequeue an element; if the queue is empty, + // waits until an element is available up to the specified timeout, + // then dequeues it and returns true, or returns false if the timeout + // expires before an element can be dequeued. + // Using a negative timeout indicates an indefinite timeout, + // and is thus functionally equivalent to calling wait_dequeue. + template + bool wait_dequeue_timed(U& result, std::int64_t timeout_usecs) + { + if (!sema.wait(timeout_usecs)) { + return false; + } + bool success = inner.try_dequeue(result); + AE_UNUSED(result); + assert(success); + AE_UNUSED(success); + return true; + } #if __cplusplus > 199711L || _MSC_VER >= 1700 - // Attempts to dequeue an element; if the queue is empty, - // waits until an element is available up to the specified timeout, - // then dequeues it and returns true, or returns false if the timeout - // expires before an element can be dequeued. - // Using a negative timeout indicates an indefinite timeout, - // and is thus functionally equivalent to calling wait_dequeue. - template - inline bool wait_dequeue_timed(U& result, std::chrono::duration const& timeout) - { + // Attempts to dequeue an element; if the queue is empty, + // waits until an element is available up to the specified timeout, + // then dequeues it and returns true, or returns false if the timeout + // expires before an element can be dequeued. + // Using a negative timeout indicates an indefinite timeout, + // and is thus functionally equivalent to calling wait_dequeue. + template + inline bool wait_dequeue_timed(U& result, std::chrono::duration const& timeout) + { return wait_dequeue_timed(result, std::chrono::duration_cast(timeout).count()); - } + } #endif + // Returns a pointer to the front element in the queue (the one that + // would be removed next by a call to `try_dequeue` or `pop`). If the + // queue appears empty at the time the method is called, nullptr is + // returned instead. + // Must be called only from the consumer thread. + AE_FORCEINLINE T* peek() + { + return inner.peek(); + } - // Returns a pointer to the front element in the queue (the one that - // would be removed next by a call to `try_dequeue` or `pop`). If the - // queue appears empty at the time the method is called, nullptr is - // returned instead. - // Must be called only from the consumer thread. - AE_FORCEINLINE T* peek() - { - return inner.peek(); - } - - // Removes the front element from the queue, if any, without returning it. - // Returns true on success, or false if the queue appeared empty at the time - // `pop` was called. - AE_FORCEINLINE bool pop() - { - if (sema.tryWait()) { - bool result = inner.pop(); - assert(result); - AE_UNUSED(result); - return true; - } - return false; - } - - // Returns the approximate number of items currently in the queue. - // Safe to call from both the producer and consumer threads. - AE_FORCEINLINE size_t size_approx() const - { - return sema.availableApprox(); - } + // Removes the front element from the queue, if any, without returning it. + // Returns true on success, or false if the queue appeared empty at the time + // `pop` was called. + AE_FORCEINLINE bool pop() + { + if (sema.tryWait()) { + bool result = inner.pop(); + assert(result); + AE_UNUSED(result); + return true; + } + return false; + } + // Returns the approximate number of items currently in the queue. + // Safe to call from both the producer and consumer threads. + AE_FORCEINLINE size_t size_approx() const + { + return sema.availableApprox(); + } private: - // Disable copying & assignment - BlockingReaderWriterQueue(ReaderWriterQueue const&) { } - BlockingReaderWriterQueue& operator=(ReaderWriterQueue const&) { } - + // Disable copying & assignment + BlockingReaderWriterQueue(ReaderWriterQueue const&) {} + BlockingReaderWriterQueue& operator=(ReaderWriterQueue const&) {} + private: - ReaderWriterQueue inner; - spsc_sema::LightweightSemaphore sema; + ReaderWriterQueue inner; + spsc_sema::LightweightSemaphore sema; }; -} // end namespace moodycamel +} // end namespace moodycamel #ifdef AE_VCPP #pragma warning(pop) diff --git a/include/ur_modern_driver/robot_state.h b/include/ur_modern_driver/robot_state.h index 159974b..a35da44 100644 --- a/include/ur_modern_driver/robot_state.h +++ b/include/ur_modern_driver/robot_state.h @@ -19,200 +19,202 @@ #ifndef ROBOT_STATE_H_ #define ROBOT_STATE_H_ +#include #include -#include +#include +#include #include #include -#include -#include -#include +#include namespace message_types { enum message_type { - ROBOT_STATE = 16, ROBOT_MESSAGE = 20, PROGRAM_STATE_MESSAGE = 25 + ROBOT_STATE = 16, + ROBOT_MESSAGE = 20, + PROGRAM_STATE_MESSAGE = 25 }; } typedef message_types::message_type messageType; namespace package_types { enum package_type { - ROBOT_MODE_DATA = 0, - JOINT_DATA = 1, - TOOL_DATA = 2, - MASTERBOARD_DATA = 3, - CARTESIAN_INFO = 4, - KINEMATICS_INFO = 5, - CONFIGURATION_DATA = 6, - FORCE_MODE_DATA = 7, - ADDITIONAL_INFO = 8, - CALIBRATION_DATA = 9 + ROBOT_MODE_DATA = 0, + JOINT_DATA = 1, + TOOL_DATA = 2, + MASTERBOARD_DATA = 3, + CARTESIAN_INFO = 4, + KINEMATICS_INFO = 5, + CONFIGURATION_DATA = 6, + FORCE_MODE_DATA = 7, + ADDITIONAL_INFO = 8, + CALIBRATION_DATA = 9 }; } typedef package_types::package_type packageType; namespace robot_message_types { enum robot_message_type { - ROBOT_MESSAGE_TEXT = 0, - ROBOT_MESSAGE_PROGRAM_LABEL = 1, - PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2, - ROBOT_MESSAGE_VERSION = 3, - ROBOT_MESSAGE_SAFETY_MODE = 5, - ROBOT_MESSAGE_ERROR_CODE = 6, - ROBOT_MESSAGE_KEY = 7, - ROBOT_MESSAGE_REQUEST_VALUE = 9, - ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10 + ROBOT_MESSAGE_TEXT = 0, + ROBOT_MESSAGE_PROGRAM_LABEL = 1, + PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2, + ROBOT_MESSAGE_VERSION = 3, + ROBOT_MESSAGE_SAFETY_MODE = 5, + ROBOT_MESSAGE_ERROR_CODE = 6, + ROBOT_MESSAGE_KEY = 7, + ROBOT_MESSAGE_REQUEST_VALUE = 9, + ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10 }; } typedef robot_message_types::robot_message_type robotMessageType; namespace robot_state_type_v18 { enum robot_state_type { - ROBOT_RUNNING_MODE = 0, - ROBOT_FREEDRIVE_MODE = 1, - ROBOT_READY_MODE = 2, - ROBOT_INITIALIZING_MODE = 3, - ROBOT_SECURITY_STOPPED_MODE = 4, - ROBOT_EMERGENCY_STOPPED_MODE = 5, - ROBOT_FATAL_ERROR_MODE = 6, - ROBOT_NO_POWER_MODE = 7, - ROBOT_NOT_CONNECTED_MODE = 8, - ROBOT_SHUTDOWN_MODE = 9, - ROBOT_SAFEGUARD_STOP_MODE = 10 + ROBOT_RUNNING_MODE = 0, + ROBOT_FREEDRIVE_MODE = 1, + ROBOT_READY_MODE = 2, + ROBOT_INITIALIZING_MODE = 3, + ROBOT_SECURITY_STOPPED_MODE = 4, + ROBOT_EMERGENCY_STOPPED_MODE = 5, + ROBOT_FATAL_ERROR_MODE = 6, + ROBOT_NO_POWER_MODE = 7, + ROBOT_NOT_CONNECTED_MODE = 8, + ROBOT_SHUTDOWN_MODE = 9, + ROBOT_SAFEGUARD_STOP_MODE = 10 }; } typedef robot_state_type_v18::robot_state_type robotStateTypeV18; namespace robot_state_type_v30 { enum robot_state_type { - ROBOT_MODE_DISCONNECTED = 0, - ROBOT_MODE_CONFIRM_SAFETY = 1, - ROBOT_MODE_BOOTING = 2, - ROBOT_MODE_POWER_OFF = 3, - ROBOT_MODE_POWER_ON = 4, - ROBOT_MODE_IDLE = 5, - ROBOT_MODE_BACKDRIVE = 6, - ROBOT_MODE_RUNNING = 7, - ROBOT_MODE_UPDATING_FIRMWARE = 8 + ROBOT_MODE_DISCONNECTED = 0, + ROBOT_MODE_CONFIRM_SAFETY = 1, + ROBOT_MODE_BOOTING = 2, + ROBOT_MODE_POWER_OFF = 3, + ROBOT_MODE_POWER_ON = 4, + ROBOT_MODE_IDLE = 5, + ROBOT_MODE_BACKDRIVE = 6, + ROBOT_MODE_RUNNING = 7, + ROBOT_MODE_UPDATING_FIRMWARE = 8 }; } typedef robot_state_type_v30::robot_state_type robotStateTypeV30; struct version_message { - uint64_t timestamp; - int8_t source; - int8_t robot_message_type; - int8_t project_name_size; - char project_name[15]; - uint8_t major_version; - uint8_t minor_version; - int svn_revision; - char build_date[25]; + uint64_t timestamp; + int8_t source; + int8_t robot_message_type; + int8_t project_name_size; + char project_name[15]; + uint8_t major_version; + uint8_t minor_version; + int svn_revision; + char build_date[25]; }; struct masterboard_data { - int digitalInputBits; - int digitalOutputBits; - char analogInputRange0; - char analogInputRange1; - double analogInput0; - double analogInput1; - char analogOutputDomain0; - char analogOutputDomain1; - double analogOutput0; - double analogOutput1; - float masterBoardTemperature; - float robotVoltage48V; - float robotCurrent; - float masterIOCurrent; - unsigned char safetyMode; - unsigned char masterOnOffState; - char euromap67InterfaceInstalled; - int euromapInputBits; - int euromapOutputBits; - float euromapVoltage; - float euromapCurrent; + int digitalInputBits; + int digitalOutputBits; + char analogInputRange0; + char analogInputRange1; + double analogInput0; + double analogInput1; + char analogOutputDomain0; + char analogOutputDomain1; + double analogOutput0; + double analogOutput1; + float masterBoardTemperature; + float robotVoltage48V; + float robotCurrent; + float masterIOCurrent; + unsigned char safetyMode; + unsigned char masterOnOffState; + char euromap67InterfaceInstalled; + int euromapInputBits; + int euromapOutputBits; + float euromapVoltage; + float euromapCurrent; }; struct robot_mode_data { - uint64_t timestamp; - bool isRobotConnected; - bool isRealRobotEnabled; - bool isPowerOnRobot; - bool isEmergencyStopped; - bool isProtectiveStopped; - bool isProgramRunning; - bool isProgramPaused; - unsigned char robotMode; - unsigned char controlMode; - double targetSpeedFraction; - double speedScaling; + uint64_t timestamp; + bool isRobotConnected; + bool isRealRobotEnabled; + bool isPowerOnRobot; + bool isEmergencyStopped; + bool isProtectiveStopped; + bool isProgramRunning; + bool isProgramPaused; + unsigned char robotMode; + unsigned char controlMode; + double targetSpeedFraction; + double speedScaling; }; class RobotState { private: - version_message version_msg_; - masterboard_data mb_data_; - robot_mode_data robot_mode_; + version_message version_msg_; + masterboard_data mb_data_; + robot_mode_data robot_mode_; - std::recursive_mutex val_lock_; // Locks the variables while unpack parses data; + std::recursive_mutex val_lock_; // Locks the variables while unpack parses data; - std::condition_variable* pMsg_cond_; //Signals that new vars are available - bool new_data_available_; //to avoid spurious wakes - unsigned char robot_mode_running_; + std::condition_variable* pMsg_cond_; //Signals that new vars are available + bool new_data_available_; //to avoid spurious wakes + unsigned char robot_mode_running_; - double ntohd(uint64_t nf); + double ntohd(uint64_t nf); public: - RobotState(std::condition_variable& msg_cond); - ~RobotState(); - double getVersion(); - double getTime(); - std::vector getQTarget(); - int getDigitalInputBits(); - int getDigitalOutputBits(); - char getAnalogInputRange0(); - char getAnalogInputRange1(); - double getAnalogInput0(); - double getAnalogInput1(); - char getAnalogOutputDomain0(); - char getAnalogOutputDomain1(); - double getAnalogOutput0(); - double getAnalogOutput1(); - std::vector getVActual(); - float getMasterBoardTemperature(); - float getRobotVoltage48V(); - float getRobotCurrent(); - float getMasterIOCurrent(); - unsigned char getSafetyMode(); - unsigned char getInReducedMode(); - char getEuromap67InterfaceInstalled(); - int getEuromapInputBits(); - int getEuromapOutputBits(); - float getEuromapVoltage(); - float getEuromapCurrent(); + RobotState(std::condition_variable& msg_cond); + ~RobotState(); + double getVersion(); + double getTime(); + std::vector getQTarget(); + int getDigitalInputBits(); + int getDigitalOutputBits(); + char getAnalogInputRange0(); + char getAnalogInputRange1(); + double getAnalogInput0(); + double getAnalogInput1(); + char getAnalogOutputDomain0(); + char getAnalogOutputDomain1(); + double getAnalogOutput0(); + double getAnalogOutput1(); + std::vector getVActual(); + float getMasterBoardTemperature(); + float getRobotVoltage48V(); + float getRobotCurrent(); + float getMasterIOCurrent(); + unsigned char getSafetyMode(); + unsigned char getInReducedMode(); + char getEuromap67InterfaceInstalled(); + int getEuromapInputBits(); + int getEuromapOutputBits(); + float getEuromapVoltage(); + float getEuromapCurrent(); - bool isRobotConnected(); - bool isRealRobotEnabled(); - bool isPowerOnRobot(); - bool isEmergencyStopped(); - bool isProtectiveStopped(); - bool isProgramRunning(); - bool isProgramPaused(); - unsigned char getRobotMode(); - bool isReady(); + bool isRobotConnected(); + bool isRealRobotEnabled(); + bool isPowerOnRobot(); + bool isEmergencyStopped(); + bool isProtectiveStopped(); + bool isProgramRunning(); + bool isProgramPaused(); + unsigned char getRobotMode(); + bool isReady(); - void setDisconnected(); + void setDisconnected(); - bool getNewDataAvailable(); - void finishedReading(); + bool getNewDataAvailable(); + void finishedReading(); - void unpack(uint8_t * buf, unsigned int buf_length); - void unpackRobotMessage(uint8_t * buf, unsigned int offset, uint32_t len); - void unpackRobotMessageVersion(uint8_t * buf, unsigned int offset, - uint32_t len); - void unpackRobotState(uint8_t * buf, unsigned int offset, uint32_t len); - void unpackRobotStateMasterboard(uint8_t * buf, unsigned int offset); - void unpackRobotMode(uint8_t * buf, unsigned int offset); + void unpack(uint8_t* buf, unsigned int buf_length); + void unpackRobotMessage(uint8_t* buf, unsigned int offset, uint32_t len); + void unpackRobotMessageVersion(uint8_t* buf, unsigned int offset, + uint32_t len); + void unpackRobotState(uint8_t* buf, unsigned int offset, uint32_t len); + void unpackRobotStateMasterboard(uint8_t* buf, unsigned int offset); + void unpackRobotMode(uint8_t* buf, unsigned int offset); }; #endif /* ROBOT_STATE_H_ */ diff --git a/include/ur_modern_driver/robot_state_RT.h b/include/ur_modern_driver/robot_state_RT.h index 1f5b2af..3d3b2e8 100644 --- a/include/ur_modern_driver/robot_state_RT.h +++ b/include/ur_modern_driver/robot_state_RT.h @@ -19,98 +19,98 @@ #ifndef ROBOT_STATE_RT_H_ #define ROBOT_STATE_RT_H_ +#include #include -#include -#include -#include #include #include -#include +#include +#include +#include class RobotStateRT { private: - double version_; //protocol version + double version_; //protocol version - double time_; //Time elapsed since the controller was started - std::vector q_target_; //Target joint positions - std::vector qd_target_; //Target joint velocities - std::vector qdd_target_; //Target joint accelerations - std::vector i_target_; //Target joint currents - std::vector m_target_; //Target joint moments (torques) - std::vector q_actual_; //Actual joint positions - std::vector qd_actual_; //Actual joint velocities - std::vector i_actual_; //Actual joint currents - std::vector i_control_; //Joint control currents - std::vector tool_vector_actual_; //Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation - std::vector tcp_speed_actual_; //Actual speed of the tool given in Cartesian coordinates - std::vector tcp_force_; //Generalised forces in the TC - std::vector tool_vector_target_; //Target Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation - std::vector tcp_speed_target_; //Target speed of the tool given in Cartesian coordinates - std::vector digital_input_bits_; //Current state of the digital inputs. NOTE: these are bits encoded as int64_t, e.g. a value of 5 corresponds to bit 0 and bit 2 set high - std::vector motor_temperatures_; //Temperature of each joint in degrees celsius - double controller_timer_; //Controller realtime thread execution time - double robot_mode_; //Robot mode - std::vector joint_modes_; //Joint control modes - double safety_mode_; //Safety mode - std::vector tool_accelerometer_values_; //Tool x,y and z accelerometer values (software version 1.7) - double speed_scaling_; //Speed scaling of the trajectory limiter - double linear_momentum_norm_; //Norm of Cartesian linear momentum - double v_main_; //Masterboard: Main voltage - double v_robot_; //Matorborad: Robot voltage (48V) - double i_robot_; //Masterboard: Robot current - std::vector v_actual_; //Actual joint voltages + double time_; //Time elapsed since the controller was started + std::vector q_target_; //Target joint positions + std::vector qd_target_; //Target joint velocities + std::vector qdd_target_; //Target joint accelerations + std::vector i_target_; //Target joint currents + std::vector m_target_; //Target joint moments (torques) + std::vector q_actual_; //Actual joint positions + std::vector qd_actual_; //Actual joint velocities + std::vector i_actual_; //Actual joint currents + std::vector i_control_; //Joint control currents + std::vector tool_vector_actual_; //Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation + std::vector tcp_speed_actual_; //Actual speed of the tool given in Cartesian coordinates + std::vector tcp_force_; //Generalised forces in the TC + std::vector tool_vector_target_; //Target Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation + std::vector tcp_speed_target_; //Target speed of the tool given in Cartesian coordinates + std::vector digital_input_bits_; //Current state of the digital inputs. NOTE: these are bits encoded as int64_t, e.g. a value of 5 corresponds to bit 0 and bit 2 set high + std::vector motor_temperatures_; //Temperature of each joint in degrees celsius + double controller_timer_; //Controller realtime thread execution time + double robot_mode_; //Robot mode + std::vector joint_modes_; //Joint control modes + double safety_mode_; //Safety mode + std::vector tool_accelerometer_values_; //Tool x,y and z accelerometer values (software version 1.7) + double speed_scaling_; //Speed scaling of the trajectory limiter + double linear_momentum_norm_; //Norm of Cartesian linear momentum + double v_main_; //Masterboard: Main voltage + double v_robot_; //Matorborad: Robot voltage (48V) + double i_robot_; //Masterboard: Robot current + std::vector v_actual_; //Actual joint voltages - std::mutex val_lock_; // Locks the variables while unpack parses data; + std::mutex val_lock_; // Locks the variables while unpack parses data; - std::condition_variable* pMsg_cond_; //Signals that new vars are available - bool data_published_; //to avoid spurious wakes - bool controller_updated_; //to avoid spurious wakes + std::condition_variable* pMsg_cond_; //Signals that new vars are available + bool data_published_; //to avoid spurious wakes + bool controller_updated_; //to avoid spurious wakes - std::vector unpackVector(uint8_t * buf, int start_index, - int nr_of_vals); - std::vector unpackDigitalInputBits(int64_t data); - double ntohd(uint64_t nf); + std::vector unpackVector(uint8_t* buf, int start_index, + int nr_of_vals); + std::vector unpackDigitalInputBits(int64_t data); + double ntohd(uint64_t nf); public: - RobotStateRT(std::condition_variable& msg_cond); - ~RobotStateRT(); - double getVersion(); - double getTime(); - std::vector getQTarget(); - std::vector getQdTarget(); - std::vector getQddTarget(); - std::vector getITarget(); - std::vector getMTarget(); - std::vector getQActual(); - std::vector getQdActual(); - std::vector getIActual(); - std::vector getIControl(); - std::vector getToolVectorActual(); - std::vector getTcpSpeedActual(); - std::vector getTcpForce(); - std::vector getToolVectorTarget(); - std::vector getTcpSpeedTarget(); - std::vector getDigitalInputBits(); - std::vector getMotorTemperatures(); - double getControllerTimer(); - double getRobotMode(); - std::vector getJointModes(); - double getSafety_mode(); - std::vector getToolAccelerometerValues(); - double getSpeedScaling(); - double getLinearMomentumNorm(); - double getVMain(); - double getVRobot(); - double getIRobot(); + RobotStateRT(std::condition_variable& msg_cond); + ~RobotStateRT(); + double getVersion(); + double getTime(); + std::vector getQTarget(); + std::vector getQdTarget(); + std::vector getQddTarget(); + std::vector getITarget(); + std::vector getMTarget(); + std::vector getQActual(); + std::vector getQdActual(); + std::vector getIActual(); + std::vector getIControl(); + std::vector getToolVectorActual(); + std::vector getTcpSpeedActual(); + std::vector getTcpForce(); + std::vector getToolVectorTarget(); + std::vector getTcpSpeedTarget(); + std::vector getDigitalInputBits(); + std::vector getMotorTemperatures(); + double getControllerTimer(); + double getRobotMode(); + std::vector getJointModes(); + double getSafety_mode(); + std::vector getToolAccelerometerValues(); + double getSpeedScaling(); + double getLinearMomentumNorm(); + double getVMain(); + double getVRobot(); + double getIRobot(); - void setVersion(double ver); + void setVersion(double ver); - void setDataPublished(); - bool getDataPublished(); - bool getControllerUpdated(); - void setControllerUpdated(); - std::vector getVActual(); - void unpack(uint8_t * buf); + void setDataPublished(); + bool getDataPublished(); + bool getControllerUpdated(); + void setControllerUpdated(); + std::vector getVActual(); + void unpack(uint8_t* buf); }; #endif /* ROBOT_STATE_RT_H_ */ diff --git a/include/ur_modern_driver/ros/converter.h b/include/ur_modern_driver/ros/converter.h index 3f59c93..6f70f09 100644 --- a/include/ur_modern_driver/ros/converter.h +++ b/include/ur_modern_driver/ros/converter.h @@ -1,2 +1 @@ #pragma once - diff --git a/include/ur_modern_driver/ros/rt_publisher.h b/include/ur_modern_driver/ros/rt_publisher.h index 3289174..0737e61 100644 --- a/include/ur_modern_driver/ros/rt_publisher.h +++ b/include/ur_modern_driver/ros/rt_publisher.h @@ -1,12 +1,12 @@ #pragma once #include -#include -#include -#include -#include #include #include +#include +#include +#include +#include #include "ur_modern_driver/ur/consumer.h" @@ -26,34 +26,34 @@ private: NodeHandle _nh; Publisher _joint_pub; Publisher _wrench_pub; - Publisher _tool_vel_pub; + Publisher _tool_vel_pub; std::vector _joint_names; std::string _base_frame; - bool publish_joints(RTShared &packet, ros::Time &t); - bool publish_wrench(RTShared &packet, ros::Time &t); - bool publish_tool(RTShared &packet, ros::Time &t); + bool publish_joints(RTShared& packet, ros::Time& t); + bool publish_wrench(RTShared& packet, ros::Time& t); + bool publish_tool(RTShared& packet, ros::Time& t); - bool publish(RTShared &packet); + bool publish(RTShared& packet); public: - RTPublisher(std::string &joint_prefix, std::string &base_frame) : - _joint_pub(_nh.advertise("joint_states", 1)), - _wrench_pub(_nh.advertise("wrench", 1)), - _tool_vel_pub(_nh.advertise("tool_velocity", 1)), - _base_frame(base_frame) + RTPublisher(std::string& joint_prefix, std::string& base_frame) + : _joint_pub(_nh.advertise("joint_states", 1)) + , _wrench_pub(_nh.advertise("wrench", 1)) + , _tool_vel_pub(_nh.advertise("tool_velocity", 1)) + , _base_frame(base_frame) { - for(auto const& j : JOINTS) { + for (auto const& j : JOINTS) { _joint_names.push_back(joint_prefix + j); } } - virtual bool consume(RTState_V1_6__7 &state); - virtual bool consume(RTState_V1_8 &state); - virtual bool consume(RTState_V3_0__1 &state); - virtual bool consume(RTState_V3_2__3 &state); + virtual bool consume(RTState_V1_6__7& state); + virtual bool consume(RTState_V1_8& state); + virtual bool consume(RTState_V3_0__1& state); + virtual bool consume(RTState_V3_2__3& state); - virtual void setup_consumer() { } - virtual void teardown_consumer() { } - virtual void stop_consumer() { } + virtual void setup_consumer() {} + virtual void teardown_consumer() {} + virtual void stop_consumer() {} }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/consumer.h b/include/ur_modern_driver/ur/consumer.h index 5b2a373..655e8bb 100644 --- a/include/ur_modern_driver/ur/consumer.h +++ b/include/ur_modern_driver/ur/consumer.h @@ -1,46 +1,47 @@ #pragma once #include "ur_modern_driver/pipeline.h" -#include "ur_modern_driver/ur/state.h" #include "ur_modern_driver/ur/master_board.h" +#include "ur_modern_driver/ur/messages.h" #include "ur_modern_driver/ur/robot_mode.h" #include "ur_modern_driver/ur/rt_state.h" -#include "ur_modern_driver/ur/messages.h" +#include "ur_modern_driver/ur/state.h" class URRTPacketConsumer : public IConsumer { public: - virtual bool consume(unique_ptr packet) { + virtual bool consume(unique_ptr packet) + { return packet->consume_with(*this); } - virtual bool consume(RTState_V1_6__7 &state) = 0; - virtual bool consume(RTState_V1_8 &state) = 0; - virtual bool consume(RTState_V3_0__1 &state) = 0; - virtual bool consume(RTState_V3_2__3 &state) = 0; + virtual bool consume(RTState_V1_6__7& state) = 0; + virtual bool consume(RTState_V1_8& state) = 0; + virtual bool consume(RTState_V3_0__1& state) = 0; + virtual bool consume(RTState_V3_2__3& state) = 0; }; - class URStatePacketConsumer : public IConsumer { public: - virtual bool consume(unique_ptr packet) { + virtual bool consume(unique_ptr packet) + { return packet->consume_with(*this); } - virtual bool consume(MasterBoardData_V1_X &data) = 0; - virtual bool consume(MasterBoardData_V3_0__1 &data) = 0; - virtual bool consume(MasterBoardData_V3_2 &data) = 0; - - virtual bool consume(RobotModeData_V1_X &data) = 0; - virtual bool consume(RobotModeData_V3_0__1 &data) = 0; - virtual bool consume(RobotModeData_V3_2 &data) = 0; -}; + virtual bool consume(MasterBoardData_V1_X& data) = 0; + virtual bool consume(MasterBoardData_V3_0__1& data) = 0; + virtual bool consume(MasterBoardData_V3_2& data) = 0; + virtual bool consume(RobotModeData_V1_X& data) = 0; + virtual bool consume(RobotModeData_V3_0__1& data) = 0; + virtual bool consume(RobotModeData_V3_2& data) = 0; +}; class URMessagePacketConsumer : public IConsumer { public: - virtual bool consume(unique_ptr packet) { + virtual bool consume(unique_ptr packet) + { return packet->consume_with(*this); } - virtual bool consume(VersionMessage &message) = 0; + virtual bool consume(VersionMessage& message) = 0; }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/factory.h b/include/ur_modern_driver/ur/factory.h index a09186f..d3266ea 100644 --- a/include/ur_modern_driver/ur/factory.h +++ b/include/ur_modern_driver/ur/factory.h @@ -1,14 +1,13 @@ #pragma once -#include -#include "ur_modern_driver/ur/stream.h" #include "ur_modern_driver/ur/consumer.h" -#include "ur_modern_driver/ur/producer.h" -#include "ur_modern_driver/ur/parser.h" -#include "ur_modern_driver/ur/state_parser.h" -#include "ur_modern_driver/ur/rt_parser.h" #include "ur_modern_driver/ur/messages_parser.h" - +#include "ur_modern_driver/ur/parser.h" +#include "ur_modern_driver/ur/producer.h" +#include "ur_modern_driver/ur/rt_parser.h" +#include "ur_modern_driver/ur/state_parser.h" +#include "ur_modern_driver/ur/stream.h" +#include class URFactory : private URMessagePacketConsumer { private: @@ -18,7 +17,8 @@ private: uint8_t _major_version; uint8_t _minor_version; - bool consume(VersionMessage &vm) { + bool consume(VersionMessage& vm) + { LOG_INFO("Got VersionMessage:"); LOG_INFO("project name: %s", vm.project_name.c_str()); LOG_INFO("version: %u.%u.%d", vm.major_version, vm.minor_version, vm.svn_version); @@ -26,31 +26,33 @@ private: _major_version = vm.major_version; _minor_version = vm.minor_version; - + return true; } - void setup_consumer() { } - void teardown_consumer() { } - void stop_consumer() { } + void setup_consumer() {} + void teardown_consumer() {} + void stop_consumer() {} public: - URFactory(std::string &host) : _stream(host, 30001) { + URFactory(std::string& host) + : _stream(host, 30001) + { URProducer p(_stream, _parser); - std::vector> results; + std::vector > results; p.setup_producer(); - if(!p.try_get(results) || results.size() == 0) { - LOG_FATAL("No version message received, init failed!"); + if (!p.try_get(results) || results.size() == 0) { + LOG_FATAL("No version message received, init failed!"); std::exit(EXIT_FAILURE); } - - for(auto const& p : results) { + + for (auto const& p : results) { p->consume_with(*this); } - if(_major_version == 0 && _minor_version == 0) { + if (_major_version == 0 && _minor_version == 0) { LOG_FATAL("No version message received, init failed!"); std::exit(EXIT_FAILURE); } @@ -58,28 +60,30 @@ public: p.teardown_producer(); } - std::unique_ptr> get_state_parser() { - if(_major_version == 1) { - return std::unique_ptr>(new URStateParser_V1_X); + std::unique_ptr > get_state_parser() + { + if (_major_version == 1) { + return std::unique_ptr >(new URStateParser_V1_X); } else { - if(_minor_version < 3) - return std::unique_ptr>(new URStateParser_V3_0__1); + if (_minor_version < 3) + return std::unique_ptr >(new URStateParser_V3_0__1); else - return std::unique_ptr>(new URStateParser_V3_2); + return std::unique_ptr >(new URStateParser_V3_2); } } - std::unique_ptr> get_rt_parser() { - if(_major_version == 1) { - if(_minor_version < 8) - return std::unique_ptr>(new URRTStateParser_V1_6__7); + std::unique_ptr > get_rt_parser() + { + if (_major_version == 1) { + if (_minor_version < 8) + return std::unique_ptr >(new URRTStateParser_V1_6__7); else - return std::unique_ptr>(new URRTStateParser_V1_8); + return std::unique_ptr >(new URRTStateParser_V1_8); } else { - if(_minor_version < 3) - return std::unique_ptr>(new URRTStateParser_V3_0__1); - else - return std::unique_ptr>(new URRTStateParser_V3_2__3); + if (_minor_version < 3) + return std::unique_ptr >(new URRTStateParser_V3_0__1); + else + return std::unique_ptr >(new URRTStateParser_V3_2__3); } } }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/master_board.h b/include/ur_modern_driver/ur/master_board.h index 0a7daf1..5f23397 100644 --- a/include/ur_modern_driver/ur/master_board.h +++ b/include/ur_modern_driver/ur/master_board.h @@ -1,23 +1,22 @@ #pragma once +#include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/types.h" +#include "ur_modern_driver/ur/state.h" #include #include -#include "ur_modern_driver/types.h" -#include "ur_modern_driver/bin_parser.h" -#include "ur_modern_driver/ur/state.h" - class SharedMasterBoardData { public: - virtual bool parse_with(BinParser &bp); - - int8_t analog_input_range0; + virtual bool parse_with(BinParser& bp); + + int8_t analog_input_range0; int8_t analog_input_range1; - double analog_input0; + double analog_input0; double analog_input1; - int8_t analog_output_domain0; + int8_t analog_output_domain0; int8_t analog_output_domain1; - double analog_output0; + double analog_output0; double analog_output1; float master_board_temperature; float robot_voltage_48V; @@ -30,7 +29,7 @@ public: int32_t euromap_input_bits; int32_t euromap_output_bits; - static const size_t SIZE = sizeof(double) * 4 + static const size_t SIZE = sizeof(double) * 4 + sizeof(int8_t) * 4 + sizeof(float) * 4 + sizeof(uint8_t); @@ -40,8 +39,8 @@ public: class MasterBoardData_V1_X : public SharedMasterBoardData, public StatePacket { public: - virtual bool parse_with(BinParser &bp); - virtual bool consume_with(URStatePacketConsumer &consumer); + virtual bool parse_with(BinParser& bp); + virtual bool consume_with(URStatePacketConsumer& consumer); int16_t digital_input_bits; int16_t digital_output_bits; @@ -53,7 +52,6 @@ public: int16_t euromap_voltage; int16_t euromap_current; - static const size_t SIZE = SharedMasterBoardData::SIZE + sizeof(int16_t) * 2 + sizeof(uint8_t) * 2; @@ -64,8 +62,8 @@ public: class MasterBoardData_V3_0__1 : public SharedMasterBoardData, public StatePacket { public: - virtual bool parse_with(BinParser &bp); - virtual bool consume_with(URStatePacketConsumer &consumer); + virtual bool parse_with(BinParser& bp); + virtual bool consume_with(URStatePacketConsumer& consumer); int32_t digital_input_bits; int32_t digital_output_bits; @@ -77,7 +75,6 @@ public: float euromap_voltage; float euromap_current; - static const size_t SIZE = SharedMasterBoardData::SIZE + sizeof(int32_t) * 2 + sizeof(uint8_t) * 2 @@ -89,8 +86,8 @@ public: class MasterBoardData_V3_2 : public MasterBoardData_V3_0__1 { public: - virtual bool parse_with(BinParser &bp); - virtual bool consume_with(URStatePacketConsumer &consumer); + virtual bool parse_with(BinParser& bp); + virtual bool consume_with(URStatePacketConsumer& consumer); uint8_t operational_mode_selector_input; uint8_t three_position_enabling_device_input; diff --git a/include/ur_modern_driver/ur/messages.h b/include/ur_modern_driver/ur/messages.h index 97315de..dafad61 100644 --- a/include/ur_modern_driver/ur/messages.h +++ b/include/ur_modern_driver/ur/messages.h @@ -1,30 +1,33 @@ #pragma once +#include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/pipeline.h" #include #include -#include "ur_modern_driver/pipeline.h" -#include "ur_modern_driver/bin_parser.h" - enum class robot_message_type : uint8_t { - ROBOT_MESSAGE_TEXT = 0, - ROBOT_MESSAGE_PROGRAM_LABEL = 1, - PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2, - ROBOT_MESSAGE_VERSION = 3, - ROBOT_MESSAGE_SAFETY_MODE = 5, - ROBOT_MESSAGE_ERROR_CODE = 6, - ROBOT_MESSAGE_KEY = 7, - ROBOT_MESSAGE_REQUEST_VALUE = 9, - ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10 + ROBOT_MESSAGE_TEXT = 0, + ROBOT_MESSAGE_PROGRAM_LABEL = 1, + PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2, + ROBOT_MESSAGE_VERSION = 3, + ROBOT_MESSAGE_SAFETY_MODE = 5, + ROBOT_MESSAGE_ERROR_CODE = 6, + ROBOT_MESSAGE_KEY = 7, + ROBOT_MESSAGE_REQUEST_VALUE = 9, + ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10 }; class URMessagePacketConsumer; class MessagePacket { public: - MessagePacket(uint64_t timestamp, uint8_t source) : timestamp(timestamp), source(source) { } - virtual bool parse_with(BinParser &bp) = 0; - virtual bool consume_with(URMessagePacketConsumer &consumer) = 0; + MessagePacket(uint64_t timestamp, uint8_t source) + : timestamp(timestamp) + , source(source) + { + } + virtual bool parse_with(BinParser& bp) = 0; + virtual bool consume_with(URMessagePacketConsumer& consumer) = 0; uint64_t timestamp; uint8_t source; @@ -32,10 +35,13 @@ public: class VersionMessage : public MessagePacket { public: - VersionMessage(uint64_t timestamp, uint8_t source) : MessagePacket(timestamp, source) { } - - virtual bool parse_with(BinParser &bp); - virtual bool consume_with(URMessagePacketConsumer &consumer); + VersionMessage(uint64_t timestamp, uint8_t source) + : MessagePacket(timestamp, source) + { + } + + virtual bool parse_with(BinParser& bp); + virtual bool consume_with(URMessagePacketConsumer& consumer); std::string project_name; uint8_t major_version; diff --git a/include/ur_modern_driver/ur/messages_parser.h b/include/ur_modern_driver/ur/messages_parser.h index 77dc9b7..b0a781f 100644 --- a/include/ur_modern_driver/ur/messages_parser.h +++ b/include/ur_modern_driver/ur/messages_parser.h @@ -1,20 +1,21 @@ #pragma once -#include -#include "ur_modern_driver/log.h" #include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/log.h" #include "ur_modern_driver/pipeline.h" -#include "ur_modern_driver/ur/parser.h" #include "ur_modern_driver/ur/messages.h" +#include "ur_modern_driver/ur/parser.h" +#include class URMessageParser : public URParser { public: - bool parse(BinParser &bp, std::vector> &results) { + bool parse(BinParser& bp, std::vector >& results) + { int32_t packet_size; message_type type; bp.parse(packet_size); bp.parse(type); - if(type != message_type::ROBOT_MESSAGE) { + if (type != message_type::ROBOT_MESSAGE) { LOG_WARN("Invalid message type recieved: %u", static_cast(type)); return false; } @@ -30,16 +31,16 @@ public: std::unique_ptr result; bool parsed = false; - switch(message_type) { - case robot_message_type::ROBOT_MESSAGE_VERSION: { - VersionMessage *vm = new VersionMessage(timestamp, source); - parsed = vm->parse_with(bp); - result.reset(vm); - break; - } + switch (message_type) { + case robot_message_type::ROBOT_MESSAGE_VERSION: { + VersionMessage* vm = new VersionMessage(timestamp, source); + parsed = vm->parse_with(bp); + result.reset(vm); + break; + } - default: - return false; + default: + return false; } results.push_back(std::move(result)); diff --git a/include/ur_modern_driver/ur/parser.h b/include/ur_modern_driver/ur/parser.h index 00a0bac..ea53b40 100644 --- a/include/ur_modern_driver/ur/parser.h +++ b/include/ur_modern_driver/ur/parser.h @@ -1,10 +1,10 @@ #pragma once -#include -#include "ur_modern_driver/pipeline.h" #include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/pipeline.h" +#include template class URParser { public: - virtual bool parse(BinParser &bp, std::vector> &results) = 0; + virtual bool parse(BinParser& bp, std::vector >& results) = 0; }; diff --git a/include/ur_modern_driver/ur/producer.h b/include/ur_modern_driver/ur/producer.h index 35be085..cfbe95c 100644 --- a/include/ur_modern_driver/ur/producer.h +++ b/include/ur_modern_driver/ur/producer.h @@ -1,30 +1,36 @@ #pragma once #include "ur_modern_driver/pipeline.h" -#include "ur_modern_driver/ur/stream.h" #include "ur_modern_driver/ur/parser.h" +#include "ur_modern_driver/ur/stream.h" template class URProducer : public IProducer { private: - URStream &_stream; - URParser &_parser; + URStream& _stream; + URParser& _parser; public: - URProducer(URStream &stream, URParser &parser) - : _stream(stream), - _parser(parser) { } - - void setup_producer() { + URProducer(URStream& stream, URParser& parser) + : _stream(stream) + , _parser(parser) + { + } + + void setup_producer() + { _stream.connect(); } - void teardown_producer() { + void teardown_producer() + { _stream.disconnect(); } - void stop_producer() { + void stop_producer() + { _stream.disconnect(); } - - bool try_get(std::vector> &products) { + + bool try_get(std::vector >& products) + { //4KB should be enough to hold any packet received from UR uint8_t buf[4096]; @@ -33,7 +39,7 @@ public: //LOG_DEBUG("Read %d bytes from stream", len); - if(len < 1) { + if (len < 1) { LOG_WARN("Read nothing from stream"); return false; } diff --git a/include/ur_modern_driver/ur/robot_mode.h b/include/ur_modern_driver/ur/robot_mode.h index bce22af..b5307b9 100644 --- a/include/ur_modern_driver/ur/robot_mode.h +++ b/include/ur_modern_driver/ur/robot_mode.h @@ -1,14 +1,14 @@ #pragma once +#include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/types.h" +#include "ur_modern_driver/ur/state.h" #include #include -#include "ur_modern_driver/types.h" -#include "ur_modern_driver/bin_parser.h" -#include "ur_modern_driver/ur/state.h" class SharedRobotModeData { public: - virtual bool parse_with(BinParser &bp); + virtual bool parse_with(BinParser& bp); uint64_t timestamp; bool physical_robot_connected; @@ -23,31 +23,30 @@ public: enum class robot_mode_V1_X : uint8_t { ROBOT_RUNNING_MODE = 0, - ROBOT_FREEDRIVE_MODE = 1, - ROBOT_READY_MODE = 2, - ROBOT_INITIALIZING_MODE = 3, - ROBOT_SECURITY_STOPPED_MODE = 4, - ROBOT_EMERGENCY_STOPPED_MODE = 5, - ROBOT_FATAL_ERROR_MODE = 6, - ROBOT_NO_POWER_MODE = 7, - ROBOT_NOT_CONNECTED_MODE = 8, - ROBOT_SHUTDOWN_MODE = 9, - ROBOT_SAFEGUARD_STOP_MODE = 10 + ROBOT_FREEDRIVE_MODE = 1, + ROBOT_READY_MODE = 2, + ROBOT_INITIALIZING_MODE = 3, + ROBOT_SECURITY_STOPPED_MODE = 4, + ROBOT_EMERGENCY_STOPPED_MODE = 5, + ROBOT_FATAL_ERROR_MODE = 6, + ROBOT_NO_POWER_MODE = 7, + ROBOT_NOT_CONNECTED_MODE = 8, + ROBOT_SHUTDOWN_MODE = 9, + ROBOT_SAFEGUARD_STOP_MODE = 10 }; class RobotModeData_V1_X : public SharedRobotModeData, public StatePacket { public: - virtual bool parse_with(BinParser &bp); - virtual bool consume_with(URStatePacketConsumer &consumer); - + virtual bool parse_with(BinParser& bp); + virtual bool consume_with(URStatePacketConsumer& consumer); bool security_stopped; robot_mode_V1_X robot_mode; double speed_fraction; - static const size_t SIZE = SharedRobotModeData::SIZE + static const size_t SIZE = SharedRobotModeData::SIZE + sizeof(uint8_t) - + sizeof(robot_mode_V1_X) + + sizeof(robot_mode_V1_X) + sizeof(double); static_assert(RobotModeData_V1_X::SIZE == 24, "RobotModeData_V1_X has missmatched size"); @@ -55,14 +54,14 @@ public: enum class robot_mode_V3_X : uint8_t { DISCONNECTED = 0, - CONFIRM_SAFETY = 1, - BOOTING = 2, - POWER_OFF = 3, - POWER_ON = 4, - IDLE = 5, - BACKDRIVE = 6, - RUNNING = 7, - UPDATING_FIRMWARE = 8 + CONFIRM_SAFETY = 1, + BOOTING = 2, + POWER_OFF = 3, + POWER_ON = 4, + IDLE = 5, + BACKDRIVE = 6, + RUNNING = 7, + UPDATING_FIRMWARE = 8 }; enum class robot_control_mode_V3_X : uint8_t { @@ -74,9 +73,8 @@ enum class robot_control_mode_V3_X : uint8_t { class RobotModeData_V3_0__1 : public SharedRobotModeData, public StatePacket { public: - virtual bool parse_with(BinParser &bp); - virtual bool consume_with(URStatePacketConsumer &consumer); - + virtual bool parse_with(BinParser& bp); + virtual bool consume_with(URStatePacketConsumer& consumer); bool protective_stopped; @@ -85,10 +83,10 @@ public: double target_speed_fraction; double speed_scaling; - - static const size_t SIZE = SharedRobotModeData::SIZE - + sizeof(uint8_t) - + sizeof(robot_mode_V3_X) + + static const size_t SIZE = SharedRobotModeData::SIZE + + sizeof(uint8_t) + + sizeof(robot_mode_V3_X) + sizeof(robot_control_mode_V3_X) + sizeof(double) + sizeof(double); @@ -98,14 +96,13 @@ public: class RobotModeData_V3_2 : public RobotModeData_V3_0__1 { public: - virtual bool parse_with(BinParser &bp); - virtual bool consume_with(URStatePacketConsumer &consumer); - + virtual bool parse_with(BinParser& bp); + virtual bool consume_with(URStatePacketConsumer& consumer); double target_speed_fraction_limit; static const size_t SIZE = RobotModeData_V3_0__1::SIZE + sizeof(double); - static_assert(RobotModeData_V3_2::SIZE == 41, "RobotModeData_V3_2 has missmatched size"); + static_assert(RobotModeData_V3_2::SIZE == 41, "RobotModeData_V3_2 has missmatched size"); }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/rt_parser.h b/include/ur_modern_driver/ur/rt_parser.h index 3a3831a..e0cde0d 100644 --- a/include/ur_modern_driver/ur/rt_parser.h +++ b/include/ur_modern_driver/ur/rt_parser.h @@ -1,31 +1,31 @@ #pragma once -#include -#include "ur_modern_driver/log.h" #include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/log.h" #include "ur_modern_driver/pipeline.h" #include "ur_modern_driver/ur/parser.h" #include "ur_modern_driver/ur/rt_state.h" - +#include template class URRTStateParser : public URParser { public: - bool parse(BinParser &bp, std::vector> &results) { + bool parse(BinParser& bp, std::vector >& results) + { int32_t packet_size = bp.peek(); - if(!bp.check_size(packet_size)) { + if (!bp.check_size(packet_size)) { LOG_ERROR("Buffer len shorter than expected packet length"); - return false; + return false; } bp.parse(packet_size); //consumes the peeked data std::unique_ptr packet(new T); - if(!packet->parse_with(bp)) + if (!packet->parse_with(bp)) return false; - + results.push_back(std::move(packet)); - + return true; } }; diff --git a/include/ur_modern_driver/ur/rt_state.h b/include/ur_modern_driver/ur/rt_state.h index c749ebb..92cd299 100644 --- a/include/ur_modern_driver/ur/rt_state.h +++ b/include/ur_modern_driver/ur/rt_state.h @@ -1,23 +1,23 @@ #pragma once -#include -#include -#include "ur_modern_driver/types.h" #include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/pipeline.h" +#include "ur_modern_driver/types.h" +#include +#include class URRTPacketConsumer; class RTPacket { public: - virtual bool parse_with(BinParser &bp) = 0; - virtual bool consume_with(URRTPacketConsumer &consumer) = 0; + virtual bool parse_with(BinParser& bp) = 0; + virtual bool consume_with(URRTPacketConsumer& consumer) = 0; }; class RTShared { protected: - bool parse_shared1(BinParser &bp); - bool parse_shared2(BinParser &bp); + bool parse_shared1(BinParser& bp); + bool parse_shared2(BinParser& bp); public: double time; @@ -43,18 +43,16 @@ public: double controller_time; double robot_mode; - - static const size_t SIZE = sizeof(double) * 3 + static const size_t SIZE = sizeof(double) * 3 + sizeof(double[6]) * 10 + sizeof(cartesian_coord_t) * 2 + sizeof(uint64_t); - }; class RTState_V1_6__7 : public RTShared, public RTPacket { public: - bool parse_with(BinParser &bp); - virtual bool consume_with(URRTPacketConsumer &consumer); + bool parse_with(BinParser& bp); + virtual bool consume_with(URRTPacketConsumer& consumer); double3_t tool_accelerometer_values; @@ -66,8 +64,8 @@ public: class RTState_V1_8 : public RTState_V1_6__7 { public: - bool parse_with(BinParser &bp); - virtual bool consume_with(URRTPacketConsumer &consumer); + bool parse_with(BinParser& bp); + virtual bool consume_with(URRTPacketConsumer& consumer); double joint_modes[6]; @@ -79,14 +77,13 @@ public: class RTState_V3_0__1 : public RTShared, public RTPacket { public: - bool parse_with(BinParser &bp); - virtual bool consume_with(URRTPacketConsumer &consumer); + bool parse_with(BinParser& bp); + virtual bool consume_with(URRTPacketConsumer& consumer); double i_control[6]; cartesian_coord_t tool_vector_target; cartesian_coord_t tcp_speed_target; - double joint_modes[6]; double safety_mode; double3_t tool_accelerometer_values; @@ -97,8 +94,7 @@ public: double i_robot; double v_actual[6]; - - static const size_t SIZE = RTShared::SIZE + static const size_t SIZE = RTShared::SIZE + sizeof(double[6]) * 3 + sizeof(double3_t) + sizeof(cartesian_coord_t) * 2 @@ -109,15 +105,15 @@ public: class RTState_V3_2__3 : public RTState_V3_0__1 { public: - bool parse_with(BinParser &bp); - virtual bool consume_with(URRTPacketConsumer &consumer); + bool parse_with(BinParser& bp); + virtual bool consume_with(URRTPacketConsumer& consumer); uint64_t digital_outputs; double program_state; - static const size_t SIZE = RTState_V3_0__1::SIZE + static const size_t SIZE = RTState_V3_0__1::SIZE + sizeof(uint64_t) + sizeof(double); - static_assert(RTState_V3_2__3::SIZE == 936, "RTState_V3_2__3 has mismatched size!"); + static_assert(RTState_V3_2__3::SIZE == 936, "RTState_V3_2__3 has mismatched size!"); }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/state.h b/include/ur_modern_driver/ur/state.h index d37cbf1..0ac8e69 100644 --- a/include/ur_modern_driver/ur/state.h +++ b/include/ur_modern_driver/ur/state.h @@ -1,27 +1,27 @@ #pragma once -#include -#include -#include "ur_modern_driver/pipeline.h" #include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/log.h" +#include "ur_modern_driver/pipeline.h" +#include +#include enum class package_type : uint8_t { - ROBOT_MODE_DATA = 0, - JOINT_DATA = 1, - TOOL_DATA = 2, - MASTERBOARD_DATA = 3, - CARTESIAN_INFO = 4, - KINEMATICS_INFO = 5, - CONFIGURATION_DATA = 6, - FORCE_MODE_DATA = 7, - ADDITIONAL_INFO = 8, - CALIBRATION_DATA = 9 + ROBOT_MODE_DATA = 0, + JOINT_DATA = 1, + TOOL_DATA = 2, + MASTERBOARD_DATA = 3, + CARTESIAN_INFO = 4, + KINEMATICS_INFO = 5, + CONFIGURATION_DATA = 6, + FORCE_MODE_DATA = 7, + ADDITIONAL_INFO = 8, + CALIBRATION_DATA = 9 }; enum class message_type : uint8_t { - ROBOT_STATE = 16, - ROBOT_MESSAGE = 20, + ROBOT_STATE = 16, + ROBOT_MESSAGE = 20, PROGRAM_STATE_MESSAGE = 25 }; @@ -29,6 +29,6 @@ class URStatePacketConsumer; class StatePacket { public: - virtual bool parse_with(BinParser &bp) = 0; - virtual bool consume_with(URStatePacketConsumer &consumer) = 0; + virtual bool parse_with(BinParser& bp) = 0; + virtual bool consume_with(URStatePacketConsumer& consumer) = 0; }; diff --git a/include/ur_modern_driver/ur/state_parser.h b/include/ur_modern_driver/ur/state_parser.h index df5a7d6..a68a611 100644 --- a/include/ur_modern_driver/ur/state_parser.h +++ b/include/ur_modern_driver/ur/state_parser.h @@ -1,73 +1,74 @@ #pragma once -#include -#include "ur_modern_driver/log.h" #include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/log.h" #include "ur_modern_driver/pipeline.h" -#include "ur_modern_driver/ur/parser.h" -#include "ur_modern_driver/ur/state.h" #include "ur_modern_driver/ur/master_board.h" +#include "ur_modern_driver/ur/parser.h" #include "ur_modern_driver/ur/robot_mode.h" - +#include "ur_modern_driver/ur/state.h" +#include template class URStateParser : public URParser { private: - StatePacket* from_type(package_type type) { - switch(type) { - case package_type::ROBOT_MODE_DATA: - return new RMD; - case package_type::MASTERBOARD_DATA: - return new MBD; - default: - return nullptr; + StatePacket* from_type(package_type type) + { + switch (type) { + case package_type::ROBOT_MODE_DATA: + return new RMD; + case package_type::MASTERBOARD_DATA: + return new MBD; + default: + return nullptr; } } public: - bool parse(BinParser &bp, std::vector> &results) { + bool parse(BinParser& bp, std::vector >& results) + { int32_t packet_size; message_type type; bp.parse(packet_size); bp.parse(type); - if(type != message_type::ROBOT_STATE) { + if (type != message_type::ROBOT_STATE) { LOG_WARN("Invalid message type recieved: %u", static_cast(type)); return false; } - while(!bp.empty()) { - if(!bp.check_size(sizeof(uint32_t))){ + while (!bp.empty()) { + if (!bp.check_size(sizeof(uint32_t))) { LOG_ERROR("Failed to read sub-package length, there's likely a parsing error"); - return false; + return false; } uint32_t sub_size = bp.peek(); - if(!bp.check_size(static_cast(sub_size))) { + if (!bp.check_size(static_cast(sub_size))) { LOG_WARN("Invalid sub-package size of %" PRIu32 " received!", sub_size); return false; } - - //deconstruction of a sub parser will increment the position of the parent parser + + //deconstruction of a sub parser will increment the position of the parent parser BinParser sbp(bp, sub_size); sbp.consume(sizeof(sub_size)); package_type type; sbp.parse(type); - + std::unique_ptr packet(from_type(type)); - if(packet == nullptr) { + if (packet == nullptr) { sbp.consume(); LOG_INFO("Skipping sub-packet of type %d", type); continue; } - if(!packet->parse_with(sbp)) { + if (!packet->parse_with(sbp)) { LOG_ERROR("Sub-package parsing of type %d failed!", type); return false; } results.push_back(std::move(packet)); - if(!sbp.empty()) { + if (!sbp.empty()) { LOG_ERROR("Sub-package was not parsed completely!"); return false; } @@ -77,6 +78,6 @@ public: } }; -typedef URStateParser URStateParser_V1_X; -typedef URStateParser URStateParser_V3_0__1; -typedef URStateParser URStateParser_V3_2; \ No newline at end of file +typedef URStateParser URStateParser_V1_X; +typedef URStateParser URStateParser_V3_0__1; +typedef URStateParser URStateParser_V3_2; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/stream.h b/include/ur_modern_driver/ur/stream.h index a6ac0d5..7d7363b 100644 --- a/include/ur_modern_driver/ur/stream.h +++ b/include/ur_modern_driver/ur/stream.h @@ -1,11 +1,11 @@ #pragma once -#include #include -#include -#include #include +#include +#include +#include -/// Encapsulates a TCP socket +/// Encapsulates a TCP socket class URStream { private: int _socket_fd = -1; @@ -16,19 +16,22 @@ private: std::atomic _stopping; public: - URStream(std::string &host, int port) - : _host(host), - _port(port), - _initialized(false), - _stopping(false) {} - - ~URStream() { + URStream(std::string& host, int port) + : _host(host) + , _port(port) + , _initialized(false) + , _stopping(false) + { + } + + ~URStream() + { disconnect(); } bool connect(); void disconnect(); - ssize_t send(uint8_t *buf, size_t buf_len); - ssize_t receive(uint8_t *buf, size_t buf_len); + ssize_t send(uint8_t* buf, size_t buf_len); + ssize_t receive(uint8_t* buf, size_t buf_len); }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur_communication.h b/include/ur_modern_driver/ur_communication.h index 5173c45..cad98e1 100644 --- a/include/ur_modern_driver/ur_communication.h +++ b/include/ur_modern_driver/ur_communication.h @@ -19,46 +19,44 @@ #ifndef UR_COMMUNICATION_H_ #define UR_COMMUNICATION_H_ -#include "robot_state.h" #include "do_output.h" -#include -#include -#include -#include -#include -#include -#include +#include "robot_state.h" +#include #include -#include -#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include - +#include +#include +#include +#include class UrCommunication { private: - int pri_sockfd_, sec_sockfd_; - struct sockaddr_in pri_serv_addr_, sec_serv_addr_; - struct hostent *server_; - bool keepalive_; - std::thread comThread_; - int flag_; - void run(); + int pri_sockfd_, sec_sockfd_; + struct sockaddr_in pri_serv_addr_, sec_serv_addr_; + struct hostent* server_; + bool keepalive_; + std::thread comThread_; + int flag_; + void run(); public: - bool connected_; - RobotState* robot_state_; - - UrCommunication(std::condition_variable& msg_cond, std::string host); - bool start(); - void halt(); + bool connected_; + RobotState* robot_state_; + UrCommunication(std::condition_variable& msg_cond, std::string host); + bool start(); + void halt(); }; #endif /* UR_COMMUNICATION_H_ */ diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h index 0f392fa..1d414c1 100644 --- a/include/ur_modern_driver/ur_driver.h +++ b/include/ur_modern_driver/ur_driver.h @@ -19,83 +19,81 @@ #ifndef UR_DRIVER_H_ #define UR_DRIVER_H_ -#include -#include -#include "ur_realtime_communication.h" -#include "ur_communication.h" #include "do_output.h" -#include +#include "ur_communication.h" +#include "ur_realtime_communication.h" +#include #include -#include -#include -#include +#include #include +#include +#include +#include +#include #include - class UrDriver { private: - double maximum_time_step_; - double minimum_payload_; - double maximum_payload_; - std::vector joint_names_; - std::string ip_addr_; - const int MULT_JOINTSTATE_ = 1000000; - const int MULT_TIME_ = 1000000; - const unsigned int REVERSE_PORT_; - int incoming_sockfd_; - int new_sockfd_; - bool reverse_connected_; - double servoj_time_; - bool executing_traj_; - double firmware_version_; - double servoj_lookahead_time_; - double servoj_gain_; + double maximum_time_step_; + double minimum_payload_; + double maximum_payload_; + std::vector joint_names_; + std::string ip_addr_; + const int MULT_JOINTSTATE_ = 1000000; + const int MULT_TIME_ = 1000000; + const unsigned int REVERSE_PORT_; + int incoming_sockfd_; + int new_sockfd_; + bool reverse_connected_; + double servoj_time_; + bool executing_traj_; + double firmware_version_; + double servoj_lookahead_time_; + double servoj_gain_; + public: - UrRealtimeCommunication* rt_interface_; - UrCommunication* sec_interface_; + UrRealtimeCommunication* rt_interface_; + UrCommunication* sec_interface_; - UrDriver(std::condition_variable& rt_msg_cond, - std::condition_variable& msg_cond, std::string host, - unsigned int reverse_port = 50007, double servoj_time = 0.016, unsigned int safety_count_max = - 12, double max_time_step = 0.08, double min_payload = 0., - double max_payload = 1., double servoj_lookahead_time=0.03, double servoj_gain=300.); - bool start(); - void halt(); + UrDriver(std::condition_variable& rt_msg_cond, + std::condition_variable& msg_cond, std::string host, + unsigned int reverse_port = 50007, double servoj_time = 0.016, unsigned int safety_count_max = 12, double max_time_step = 0.08, double min_payload = 0., + double max_payload = 1., double servoj_lookahead_time = 0.03, double servoj_gain = 300.); + bool start(); + void halt(); - void setSpeed(double q0, double q1, double q2, double q3, double q4, - double q5, double acc = 100.); + void setSpeed(double q0, double q1, double q2, double q3, double q4, + double q5, double acc = 100.); - bool doTraj(std::vector inp_timestamps, - std::vector > inp_positions, - std::vector > inp_velocities); - void servoj(std::vector positions, int keepalive = 1); + bool doTraj(std::vector inp_timestamps, + std::vector > inp_positions, + std::vector > inp_velocities); + void servoj(std::vector positions, int keepalive = 1); - void stopTraj(); + void stopTraj(); - bool uploadProg(); - bool openServo(); - void closeServo(std::vector positions); + bool uploadProg(); + bool openServo(); + void closeServo(std::vector positions); - std::vector interp_cubic(double t, double T, - std::vector p0_pos, std::vector p1_pos, - std::vector p0_vel, std::vector p1_vel); + std::vector interp_cubic(double t, double T, + std::vector p0_pos, std::vector p1_pos, + std::vector p0_vel, std::vector p1_vel); - std::vector getJointNames(); - void setJointNames(std::vector jn); - void setToolVoltage(unsigned int v); - void setFlag(unsigned int n, bool b); - void setDigitalOut(unsigned int n, bool b); - void setAnalogOut(unsigned int n, double f); - bool setPayload(double m); - - void setMinPayload(double m); - void setMaxPayload(double m); - void setServojTime(double t); - void setServojLookahead(double t); - void setServojGain(double g); + std::vector getJointNames(); + void setJointNames(std::vector jn); + void setToolVoltage(unsigned int v); + void setFlag(unsigned int n, bool b); + void setDigitalOut(unsigned int n, bool b); + void setAnalogOut(unsigned int n, double f); + bool setPayload(double m); + void setMinPayload(double m); + void setMaxPayload(double m); + void setServojTime(double t); + void setServojLookahead(double t); + void setServojGain(double g); }; #endif /* UR_DRIVER_H_ */ diff --git a/include/ur_modern_driver/ur_hardware_interface.h b/include/ur_modern_driver/ur_hardware_interface.h index ac9f310..0724703 100644 --- a/include/ur_modern_driver/ur_hardware_interface.h +++ b/include/ur_modern_driver/ur_hardware_interface.h @@ -58,16 +58,16 @@ #ifndef UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H #define UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H -#include -#include -#include -#include -#include -#include -#include -#include #include "do_output.h" #include "ur_driver.h" +#include +#include +#include +#include +#include +#include +#include +#include namespace ros_control_ur { @@ -76,64 +76,61 @@ static const double POSITION_STEP_FACTOR = 1; static const double VELOCITY_STEP_FACTOR = 1; /// \brief Hardware interface for a robot -class UrHardwareInterface: public hardware_interface::RobotHW { +class UrHardwareInterface : public hardware_interface::RobotHW { public: - - /** + /** * \brief Constructor * \param nh - Node handle for topics. */ - UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot); + UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot); - /// \brief Initialize the hardware interface - virtual void init(); + /// \brief Initialize the hardware interface + virtual void init(); - /// \brief Read the state from the robot hardware. - virtual void read(); + /// \brief Read the state from the robot hardware. + virtual void read(); - /// \brief write the command to the robot hardware. - virtual void write(); + /// \brief write the command to the robot hardware. + virtual void write(); - void setMaxVelChange(double inp); + void setMaxVelChange(double inp); - bool canSwitch( - const std::list &start_list, - const std::list &stop_list) const; - void doSwitch(const std::list&start_list, - const std::list&stop_list); + bool canSwitch( + const std::list& start_list, + const std::list& stop_list) const; + void doSwitch(const std::list& start_list, + const std::list& stop_list); protected: + // Startup and shutdown of the internal node inside a roscpp program + ros::NodeHandle nh_; - // Startup and shutdown of the internal node inside a roscpp program - ros::NodeHandle nh_; + // Interfaces + hardware_interface::JointStateInterface joint_state_interface_; + hardware_interface::ForceTorqueSensorInterface force_torque_interface_; + hardware_interface::PositionJointInterface position_joint_interface_; + hardware_interface::VelocityJointInterface velocity_joint_interface_; + bool velocity_interface_running_; + bool position_interface_running_; + // Shared memory + std::vector joint_names_; + std::vector joint_position_; + std::vector joint_velocity_; + std::vector joint_effort_; + std::vector joint_position_command_; + std::vector joint_velocity_command_; + std::vector prev_joint_velocity_command_; + std::size_t num_joints_; + double robot_force_[3] = { 0., 0., 0. }; + double robot_torque_[3] = { 0., 0., 0. }; - // Interfaces - hardware_interface::JointStateInterface joint_state_interface_; - hardware_interface::ForceTorqueSensorInterface force_torque_interface_; - hardware_interface::PositionJointInterface position_joint_interface_; - hardware_interface::VelocityJointInterface velocity_joint_interface_; - bool velocity_interface_running_; - bool position_interface_running_; - // Shared memory - std::vector joint_names_; - std::vector joint_position_; - std::vector joint_velocity_; - std::vector joint_effort_; - std::vector joint_position_command_; - std::vector joint_velocity_command_; - std::vector prev_joint_velocity_command_; - std::size_t num_joints_; - double robot_force_[3] = { 0., 0., 0. }; - double robot_torque_[3] = { 0., 0., 0. }; - - double max_vel_change_; - - // Robot API - UrDriver* robot_; + double max_vel_change_; + // Robot API + UrDriver* robot_; }; // class -}// namespace +} // namespace #endif diff --git a/include/ur_modern_driver/ur_realtime_communication.h b/include/ur_modern_driver/ur_realtime_communication.h index 9d6b445..61e5474 100644 --- a/include/ur_modern_driver/ur_realtime_communication.h +++ b/include/ur_modern_driver/ur_realtime_communication.h @@ -19,58 +19,56 @@ #ifndef UR_REALTIME_COMMUNICATION_H_ #define UR_REALTIME_COMMUNICATION_H_ -#include "robot_state_RT.h" #include "do_output.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "robot_state_RT.h" #include +#include #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include +#include +#include +#include +#include class UrRealtimeCommunication { private: - unsigned int safety_count_max_; - int sockfd_; - struct sockaddr_in serv_addr_; - struct hostent *server_; - std::string local_ip_; - bool keepalive_; - std::thread comThread_; - int flag_; - std::recursive_mutex command_string_lock_; - std::string command_; - unsigned int safety_count_; - void run(); - + unsigned int safety_count_max_; + int sockfd_; + struct sockaddr_in serv_addr_; + struct hostent* server_; + std::string local_ip_; + bool keepalive_; + std::thread comThread_; + int flag_; + std::recursive_mutex command_string_lock_; + std::string command_; + unsigned int safety_count_; + void run(); public: - bool connected_; - RobotStateRT* robot_state_; - - UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host, - unsigned int safety_count_max = 12); - bool start(); - void halt(); - void setSpeed(double q0, double q1, double q2, double q3, double q4, - double q5, double acc = 100.); - void addCommandToQueue(std::string inp); - void setSafetyCountMax(uint inp); - std::string getLocalIp(); + bool connected_; + RobotStateRT* robot_state_; + UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host, + unsigned int safety_count_max = 12); + bool start(); + void halt(); + void setSpeed(double q0, double q1, double q2, double q3, double q4, + double q5, double acc = 100.); + void addCommandToQueue(std::string inp); + void setSafetyCountMax(uint inp); + std::string getLocalIp(); }; #endif /* UR_REALTIME_COMMUNICATION_H_ */ diff --git a/src/do_output.cpp b/src/do_output.cpp index 048cce1..f4b0b40 100644 --- a/src/do_output.cpp +++ b/src/do_output.cpp @@ -18,40 +18,45 @@ #include "ur_modern_driver/do_output.h" -void print_debug(std::string inp) { +void print_debug(std::string inp) +{ #ifdef ROS_BUILD - ROS_DEBUG("%s", inp.c_str()); + ROS_DEBUG("%s", inp.c_str()); #else - printf("DEBUG: %s\n", inp.c_str()); + printf("DEBUG: %s\n", inp.c_str()); #endif } -void print_info(std::string inp) { +void print_info(std::string inp) +{ #ifdef ROS_BUILD - ROS_INFO("%s", inp.c_str()); + ROS_INFO("%s", inp.c_str()); #else - printf("INFO: %s\n", inp.c_str()); + printf("INFO: %s\n", inp.c_str()); #endif } -void print_warning(std::string inp) { +void print_warning(std::string inp) +{ #ifdef ROS_BUILD - ROS_WARN("%s", inp.c_str()); + ROS_WARN("%s", inp.c_str()); #else - printf("WARNING: %s\n", inp.c_str()); + printf("WARNING: %s\n", inp.c_str()); #endif } -void print_error(std::string inp) { +void print_error(std::string inp) +{ #ifdef ROS_BUILD - ROS_ERROR("%s", inp.c_str()); + ROS_ERROR("%s", inp.c_str()); #else - printf("ERROR: %s\n", inp.c_str()); + printf("ERROR: %s\n", inp.c_str()); #endif } -void print_fatal(std::string inp) { +void print_fatal(std::string inp) +{ #ifdef ROS_BUILD - ROS_FATAL("%s", inp.c_str()); - ros::shutdown(); + ROS_FATAL("%s", inp.c_str()); + ros::shutdown(); #else - printf("FATAL: %s\n", inp.c_str()); - exit(1); + printf("FATAL: %s\n", inp.c_str()); + exit(1); #endif } diff --git a/src/robot_state.cpp b/src/robot_state.cpp index a7eb589..743c2c2 100644 --- a/src/robot_state.cpp +++ b/src/robot_state.cpp @@ -18,370 +18,391 @@ #include "ur_modern_driver/robot_state.h" -RobotState::RobotState(std::condition_variable& msg_cond) { - version_msg_.major_version = 0; - version_msg_.minor_version = 0; - new_data_available_ = false; - pMsg_cond_ = &msg_cond; - RobotState::setDisconnected(); - robot_mode_running_ = robotStateTypeV30::ROBOT_MODE_RUNNING; +RobotState::RobotState(std::condition_variable& msg_cond) +{ + version_msg_.major_version = 0; + version_msg_.minor_version = 0; + new_data_available_ = false; + pMsg_cond_ = &msg_cond; + RobotState::setDisconnected(); + robot_mode_running_ = robotStateTypeV30::ROBOT_MODE_RUNNING; } -double RobotState::ntohd(uint64_t nf) { - double x; - nf = be64toh(nf); - memcpy(&x, &nf, sizeof(x)); - return x; +double RobotState::ntohd(uint64_t nf) +{ + double x; + nf = be64toh(nf); + memcpy(&x, &nf, sizeof(x)); + return x; } -void RobotState::unpack(uint8_t* buf, unsigned int buf_length) { - /* Returns missing bytes to unpack a message, or 0 if all data was parsed */ - unsigned int offset = 0; - while (buf_length > offset) { - int len; - unsigned char message_type; - memcpy(&len, &buf[offset], sizeof(len)); - len = ntohl(len); - if (len + offset > buf_length) { - return; - } - memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type)); - switch (message_type) { - case messageType::ROBOT_MESSAGE: - RobotState::unpackRobotMessage(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType - break; - case messageType::ROBOT_STATE: - RobotState::unpackRobotState(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType - break; - case messageType::PROGRAM_STATE_MESSAGE: - //Don't do anything atm... - default: - break; - } - offset += len; - - } - return; +void RobotState::unpack(uint8_t* buf, unsigned int buf_length) +{ + /* Returns missing bytes to unpack a message, or 0 if all data was parsed */ + unsigned int offset = 0; + while (buf_length > offset) { + int len; + unsigned char message_type; + memcpy(&len, &buf[offset], sizeof(len)); + len = ntohl(len); + if (len + offset > buf_length) { + return; + } + memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type)); + switch (message_type) { + case messageType::ROBOT_MESSAGE: + RobotState::unpackRobotMessage(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType + break; + case messageType::ROBOT_STATE: + RobotState::unpackRobotState(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType + break; + case messageType::PROGRAM_STATE_MESSAGE: + //Don't do anything atm... + default: + break; + } + offset += len; + } + return; } -void RobotState::unpackRobotMessage(uint8_t * buf, unsigned int offset, - uint32_t len) { - offset += 5; - uint64_t timestamp; - int8_t source, robot_message_type; - memcpy(×tamp, &buf[offset], sizeof(timestamp)); - offset += sizeof(timestamp); - memcpy(&source, &buf[offset], sizeof(source)); - offset += sizeof(source); - memcpy(&robot_message_type, &buf[offset], sizeof(robot_message_type)); - offset += sizeof(robot_message_type); - switch (robot_message_type) { - case robotMessageType::ROBOT_MESSAGE_VERSION: - val_lock_.lock(); - version_msg_.timestamp = timestamp; - version_msg_.source = source; - version_msg_.robot_message_type = robot_message_type; - RobotState::unpackRobotMessageVersion(buf, offset, len); - val_lock_.unlock(); - break; - default: - break; - } - +void RobotState::unpackRobotMessage(uint8_t* buf, unsigned int offset, + uint32_t len) +{ + offset += 5; + uint64_t timestamp; + int8_t source, robot_message_type; + memcpy(×tamp, &buf[offset], sizeof(timestamp)); + offset += sizeof(timestamp); + memcpy(&source, &buf[offset], sizeof(source)); + offset += sizeof(source); + memcpy(&robot_message_type, &buf[offset], sizeof(robot_message_type)); + offset += sizeof(robot_message_type); + switch (robot_message_type) { + case robotMessageType::ROBOT_MESSAGE_VERSION: + val_lock_.lock(); + version_msg_.timestamp = timestamp; + version_msg_.source = source; + version_msg_.robot_message_type = robot_message_type; + RobotState::unpackRobotMessageVersion(buf, offset, len); + val_lock_.unlock(); + break; + default: + break; + } } -void RobotState::unpackRobotState(uint8_t * buf, unsigned int offset, - uint32_t len) { - offset += 5; - while (offset < len) { - int32_t length; - uint8_t package_type; - memcpy(&length, &buf[offset], sizeof(length)); - length = ntohl(length); - memcpy(&package_type, &buf[offset + sizeof(length)], - sizeof(package_type)); - switch (package_type) { - case packageType::ROBOT_MODE_DATA: - val_lock_.lock(); - RobotState::unpackRobotMode(buf, offset + 5); - val_lock_.unlock(); - break; - - case packageType::MASTERBOARD_DATA: - val_lock_.lock(); - RobotState::unpackRobotStateMasterboard(buf, offset + 5); - val_lock_.unlock(); - break; - default: - break; - } - offset += length; - } - new_data_available_ = true; - pMsg_cond_->notify_all(); +void RobotState::unpackRobotState(uint8_t* buf, unsigned int offset, + uint32_t len) +{ + offset += 5; + while (offset < len) { + int32_t length; + uint8_t package_type; + memcpy(&length, &buf[offset], sizeof(length)); + length = ntohl(length); + memcpy(&package_type, &buf[offset + sizeof(length)], + sizeof(package_type)); + switch (package_type) { + case packageType::ROBOT_MODE_DATA: + val_lock_.lock(); + RobotState::unpackRobotMode(buf, offset + 5); + val_lock_.unlock(); + break; + case packageType::MASTERBOARD_DATA: + val_lock_.lock(); + RobotState::unpackRobotStateMasterboard(buf, offset + 5); + val_lock_.unlock(); + break; + default: + break; + } + offset += length; + } + new_data_available_ = true; + pMsg_cond_->notify_all(); } -void RobotState::unpackRobotMessageVersion(uint8_t * buf, unsigned int offset, - uint32_t len) { - memcpy(&version_msg_.project_name_size, &buf[offset], - sizeof(version_msg_.project_name_size)); - offset += sizeof(version_msg_.project_name_size); - memcpy(&version_msg_.project_name, &buf[offset], - sizeof(char) * version_msg_.project_name_size); - offset += version_msg_.project_name_size; - version_msg_.project_name[version_msg_.project_name_size] = '\0'; - memcpy(&version_msg_.major_version, &buf[offset], - sizeof(version_msg_.major_version)); - offset += sizeof(version_msg_.major_version); - memcpy(&version_msg_.minor_version, &buf[offset], - sizeof(version_msg_.minor_version)); - offset += sizeof(version_msg_.minor_version); - memcpy(&version_msg_.svn_revision, &buf[offset], - sizeof(version_msg_.svn_revision)); - offset += sizeof(version_msg_.svn_revision); - version_msg_.svn_revision = ntohl(version_msg_.svn_revision); - memcpy(&version_msg_.build_date, &buf[offset], sizeof(char) * len - offset); - version_msg_.build_date[len - offset] = '\0'; - if (version_msg_.major_version < 2) { - robot_mode_running_ = robotStateTypeV18::ROBOT_RUNNING_MODE; - } +void RobotState::unpackRobotMessageVersion(uint8_t* buf, unsigned int offset, + uint32_t len) +{ + memcpy(&version_msg_.project_name_size, &buf[offset], + sizeof(version_msg_.project_name_size)); + offset += sizeof(version_msg_.project_name_size); + memcpy(&version_msg_.project_name, &buf[offset], + sizeof(char) * version_msg_.project_name_size); + offset += version_msg_.project_name_size; + version_msg_.project_name[version_msg_.project_name_size] = '\0'; + memcpy(&version_msg_.major_version, &buf[offset], + sizeof(version_msg_.major_version)); + offset += sizeof(version_msg_.major_version); + memcpy(&version_msg_.minor_version, &buf[offset], + sizeof(version_msg_.minor_version)); + offset += sizeof(version_msg_.minor_version); + memcpy(&version_msg_.svn_revision, &buf[offset], + sizeof(version_msg_.svn_revision)); + offset += sizeof(version_msg_.svn_revision); + version_msg_.svn_revision = ntohl(version_msg_.svn_revision); + memcpy(&version_msg_.build_date, &buf[offset], sizeof(char) * len - offset); + version_msg_.build_date[len - offset] = '\0'; + if (version_msg_.major_version < 2) { + robot_mode_running_ = robotStateTypeV18::ROBOT_RUNNING_MODE; + } } -void RobotState::unpackRobotMode(uint8_t * buf, unsigned int offset) { - memcpy(&robot_mode_.timestamp, &buf[offset], sizeof(robot_mode_.timestamp)); - offset += sizeof(robot_mode_.timestamp); - uint8_t tmp; - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isRobotConnected = true; - else - robot_mode_.isRobotConnected = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isRealRobotEnabled = true; - else - robot_mode_.isRealRobotEnabled = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - //printf("PowerOnRobot: %d\n", tmp); - if (tmp > 0) - robot_mode_.isPowerOnRobot = true; - else - robot_mode_.isPowerOnRobot = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isEmergencyStopped = true; - else - robot_mode_.isEmergencyStopped = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isProtectiveStopped = true; - else - robot_mode_.isProtectiveStopped = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isProgramRunning = true; - else - robot_mode_.isProgramRunning = false; - offset += sizeof(tmp); - memcpy(&tmp, &buf[offset], sizeof(tmp)); - if (tmp > 0) - robot_mode_.isProgramPaused = true; - else - robot_mode_.isProgramPaused = false; - offset += sizeof(tmp); - memcpy(&robot_mode_.robotMode, &buf[offset], sizeof(robot_mode_.robotMode)); - offset += sizeof(robot_mode_.robotMode); - uint64_t temp; - if (RobotState::getVersion() > 2.) { - memcpy(&robot_mode_.controlMode, &buf[offset], - sizeof(robot_mode_.controlMode)); - offset += sizeof(robot_mode_.controlMode); - memcpy(&temp, &buf[offset], sizeof(temp)); - offset += sizeof(temp); - robot_mode_.targetSpeedFraction = RobotState::ntohd(temp); - } - memcpy(&temp, &buf[offset], sizeof(temp)); - offset += sizeof(temp); - robot_mode_.speedScaling = RobotState::ntohd(temp); +void RobotState::unpackRobotMode(uint8_t* buf, unsigned int offset) +{ + memcpy(&robot_mode_.timestamp, &buf[offset], sizeof(robot_mode_.timestamp)); + offset += sizeof(robot_mode_.timestamp); + uint8_t tmp; + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isRobotConnected = true; + else + robot_mode_.isRobotConnected = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isRealRobotEnabled = true; + else + robot_mode_.isRealRobotEnabled = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + //printf("PowerOnRobot: %d\n", tmp); + if (tmp > 0) + robot_mode_.isPowerOnRobot = true; + else + robot_mode_.isPowerOnRobot = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isEmergencyStopped = true; + else + robot_mode_.isEmergencyStopped = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isProtectiveStopped = true; + else + robot_mode_.isProtectiveStopped = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isProgramRunning = true; + else + robot_mode_.isProgramRunning = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isProgramPaused = true; + else + robot_mode_.isProgramPaused = false; + offset += sizeof(tmp); + memcpy(&robot_mode_.robotMode, &buf[offset], sizeof(robot_mode_.robotMode)); + offset += sizeof(robot_mode_.robotMode); + uint64_t temp; + if (RobotState::getVersion() > 2.) { + memcpy(&robot_mode_.controlMode, &buf[offset], + sizeof(robot_mode_.controlMode)); + offset += sizeof(robot_mode_.controlMode); + memcpy(&temp, &buf[offset], sizeof(temp)); + offset += sizeof(temp); + robot_mode_.targetSpeedFraction = RobotState::ntohd(temp); + } + memcpy(&temp, &buf[offset], sizeof(temp)); + offset += sizeof(temp); + robot_mode_.speedScaling = RobotState::ntohd(temp); } -void RobotState::unpackRobotStateMasterboard(uint8_t * buf, - unsigned int offset) { - if (RobotState::getVersion() < 3.0) { - int16_t digital_input_bits, digital_output_bits; - memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits)); - offset += sizeof(digital_input_bits); - memcpy(&digital_output_bits, &buf[offset], sizeof(digital_output_bits)); - offset += sizeof(digital_output_bits); - mb_data_.digitalInputBits = ntohs(digital_input_bits); - mb_data_.digitalOutputBits = ntohs(digital_output_bits); - } else { - memcpy(&mb_data_.digitalInputBits, &buf[offset], - sizeof(mb_data_.digitalInputBits)); - offset += sizeof(mb_data_.digitalInputBits); - mb_data_.digitalInputBits = ntohl(mb_data_.digitalInputBits); - memcpy(&mb_data_.digitalOutputBits, &buf[offset], - sizeof(mb_data_.digitalOutputBits)); - offset += sizeof(mb_data_.digitalOutputBits); - mb_data_.digitalOutputBits = ntohl(mb_data_.digitalOutputBits); - } +void RobotState::unpackRobotStateMasterboard(uint8_t* buf, + unsigned int offset) +{ + if (RobotState::getVersion() < 3.0) { + int16_t digital_input_bits, digital_output_bits; + memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits)); + offset += sizeof(digital_input_bits); + memcpy(&digital_output_bits, &buf[offset], sizeof(digital_output_bits)); + offset += sizeof(digital_output_bits); + mb_data_.digitalInputBits = ntohs(digital_input_bits); + mb_data_.digitalOutputBits = ntohs(digital_output_bits); + } else { + memcpy(&mb_data_.digitalInputBits, &buf[offset], + sizeof(mb_data_.digitalInputBits)); + offset += sizeof(mb_data_.digitalInputBits); + mb_data_.digitalInputBits = ntohl(mb_data_.digitalInputBits); + memcpy(&mb_data_.digitalOutputBits, &buf[offset], + sizeof(mb_data_.digitalOutputBits)); + offset += sizeof(mb_data_.digitalOutputBits); + mb_data_.digitalOutputBits = ntohl(mb_data_.digitalOutputBits); + } - memcpy(&mb_data_.analogInputRange0, &buf[offset], - sizeof(mb_data_.analogInputRange0)); - offset += sizeof(mb_data_.analogInputRange0); - memcpy(&mb_data_.analogInputRange1, &buf[offset], - sizeof(mb_data_.analogInputRange1)); - offset += sizeof(mb_data_.analogInputRange1); - uint64_t temp; - memcpy(&temp, &buf[offset], sizeof(temp)); - offset += sizeof(temp); - mb_data_.analogInput0 = RobotState::ntohd(temp); - memcpy(&temp, &buf[offset], sizeof(temp)); - offset += sizeof(temp); - mb_data_.analogInput1 = RobotState::ntohd(temp); - memcpy(&mb_data_.analogOutputDomain0, &buf[offset], - sizeof(mb_data_.analogOutputDomain0)); - offset += sizeof(mb_data_.analogOutputDomain0); - memcpy(&mb_data_.analogOutputDomain1, &buf[offset], - sizeof(mb_data_.analogOutputDomain1)); - offset += sizeof(mb_data_.analogOutputDomain1); - memcpy(&temp, &buf[offset], sizeof(temp)); - offset += sizeof(temp); - mb_data_.analogOutput0 = RobotState::ntohd(temp); - memcpy(&temp, &buf[offset], sizeof(temp)); - offset += sizeof(temp); - mb_data_.analogOutput1 = RobotState::ntohd(temp); + memcpy(&mb_data_.analogInputRange0, &buf[offset], + sizeof(mb_data_.analogInputRange0)); + offset += sizeof(mb_data_.analogInputRange0); + memcpy(&mb_data_.analogInputRange1, &buf[offset], + sizeof(mb_data_.analogInputRange1)); + offset += sizeof(mb_data_.analogInputRange1); + uint64_t temp; + memcpy(&temp, &buf[offset], sizeof(temp)); + offset += sizeof(temp); + mb_data_.analogInput0 = RobotState::ntohd(temp); + memcpy(&temp, &buf[offset], sizeof(temp)); + offset += sizeof(temp); + mb_data_.analogInput1 = RobotState::ntohd(temp); + memcpy(&mb_data_.analogOutputDomain0, &buf[offset], + sizeof(mb_data_.analogOutputDomain0)); + offset += sizeof(mb_data_.analogOutputDomain0); + memcpy(&mb_data_.analogOutputDomain1, &buf[offset], + sizeof(mb_data_.analogOutputDomain1)); + offset += sizeof(mb_data_.analogOutputDomain1); + memcpy(&temp, &buf[offset], sizeof(temp)); + offset += sizeof(temp); + mb_data_.analogOutput0 = RobotState::ntohd(temp); + memcpy(&temp, &buf[offset], sizeof(temp)); + offset += sizeof(temp); + mb_data_.analogOutput1 = RobotState::ntohd(temp); - memcpy(&mb_data_.masterBoardTemperature, &buf[offset], - sizeof(mb_data_.masterBoardTemperature)); - offset += sizeof(mb_data_.masterBoardTemperature); - mb_data_.masterBoardTemperature = ntohl(mb_data_.masterBoardTemperature); - memcpy(&mb_data_.robotVoltage48V, &buf[offset], - sizeof(mb_data_.robotVoltage48V)); - offset += sizeof(mb_data_.robotVoltage48V); - mb_data_.robotVoltage48V = ntohl(mb_data_.robotVoltage48V); - memcpy(&mb_data_.robotCurrent, &buf[offset], sizeof(mb_data_.robotCurrent)); - offset += sizeof(mb_data_.robotCurrent); - mb_data_.robotCurrent = ntohl(mb_data_.robotCurrent); - memcpy(&mb_data_.masterIOCurrent, &buf[offset], - sizeof(mb_data_.masterIOCurrent)); - offset += sizeof(mb_data_.masterIOCurrent); - mb_data_.masterIOCurrent = ntohl(mb_data_.masterIOCurrent); + memcpy(&mb_data_.masterBoardTemperature, &buf[offset], + sizeof(mb_data_.masterBoardTemperature)); + offset += sizeof(mb_data_.masterBoardTemperature); + mb_data_.masterBoardTemperature = ntohl(mb_data_.masterBoardTemperature); + memcpy(&mb_data_.robotVoltage48V, &buf[offset], + sizeof(mb_data_.robotVoltage48V)); + offset += sizeof(mb_data_.robotVoltage48V); + mb_data_.robotVoltage48V = ntohl(mb_data_.robotVoltage48V); + memcpy(&mb_data_.robotCurrent, &buf[offset], sizeof(mb_data_.robotCurrent)); + offset += sizeof(mb_data_.robotCurrent); + mb_data_.robotCurrent = ntohl(mb_data_.robotCurrent); + memcpy(&mb_data_.masterIOCurrent, &buf[offset], + sizeof(mb_data_.masterIOCurrent)); + offset += sizeof(mb_data_.masterIOCurrent); + mb_data_.masterIOCurrent = ntohl(mb_data_.masterIOCurrent); - memcpy(&mb_data_.safetyMode, &buf[offset], sizeof(mb_data_.safetyMode)); - offset += sizeof(mb_data_.safetyMode); - memcpy(&mb_data_.masterOnOffState, &buf[offset], - sizeof(mb_data_.masterOnOffState)); - offset += sizeof(mb_data_.masterOnOffState); + memcpy(&mb_data_.safetyMode, &buf[offset], sizeof(mb_data_.safetyMode)); + offset += sizeof(mb_data_.safetyMode); + memcpy(&mb_data_.masterOnOffState, &buf[offset], + sizeof(mb_data_.masterOnOffState)); + offset += sizeof(mb_data_.masterOnOffState); - memcpy(&mb_data_.euromap67InterfaceInstalled, &buf[offset], - sizeof(mb_data_.euromap67InterfaceInstalled)); - offset += sizeof(mb_data_.euromap67InterfaceInstalled); - if (mb_data_.euromap67InterfaceInstalled != 0) { - memcpy(&mb_data_.euromapInputBits, &buf[offset], - sizeof(mb_data_.euromapInputBits)); - offset += sizeof(mb_data_.euromapInputBits); - mb_data_.euromapInputBits = ntohl(mb_data_.euromapInputBits); - memcpy(&mb_data_.euromapOutputBits, &buf[offset], - sizeof(mb_data_.euromapOutputBits)); - offset += sizeof(mb_data_.euromapOutputBits); - mb_data_.euromapOutputBits = ntohl(mb_data_.euromapOutputBits); - if (RobotState::getVersion() < 3.0) { - int16_t euromap_voltage, euromap_current; - memcpy(&euromap_voltage, &buf[offset], sizeof(euromap_voltage)); - offset += sizeof(euromap_voltage); - memcpy(&euromap_current, &buf[offset], sizeof(euromap_current)); - offset += sizeof(euromap_current); - mb_data_.euromapVoltage = ntohs(euromap_voltage); - mb_data_.euromapCurrent = ntohs(euromap_current); - } else { - memcpy(&mb_data_.euromapVoltage, &buf[offset], - sizeof(mb_data_.euromapVoltage)); - offset += sizeof(mb_data_.euromapVoltage); - mb_data_.euromapVoltage = ntohl(mb_data_.euromapVoltage); - memcpy(&mb_data_.euromapCurrent, &buf[offset], - sizeof(mb_data_.euromapCurrent)); - offset += sizeof(mb_data_.euromapCurrent); - mb_data_.euromapCurrent = ntohl(mb_data_.euromapCurrent); - } - - } + memcpy(&mb_data_.euromap67InterfaceInstalled, &buf[offset], + sizeof(mb_data_.euromap67InterfaceInstalled)); + offset += sizeof(mb_data_.euromap67InterfaceInstalled); + if (mb_data_.euromap67InterfaceInstalled != 0) { + memcpy(&mb_data_.euromapInputBits, &buf[offset], + sizeof(mb_data_.euromapInputBits)); + offset += sizeof(mb_data_.euromapInputBits); + mb_data_.euromapInputBits = ntohl(mb_data_.euromapInputBits); + memcpy(&mb_data_.euromapOutputBits, &buf[offset], + sizeof(mb_data_.euromapOutputBits)); + offset += sizeof(mb_data_.euromapOutputBits); + mb_data_.euromapOutputBits = ntohl(mb_data_.euromapOutputBits); + if (RobotState::getVersion() < 3.0) { + int16_t euromap_voltage, euromap_current; + memcpy(&euromap_voltage, &buf[offset], sizeof(euromap_voltage)); + offset += sizeof(euromap_voltage); + memcpy(&euromap_current, &buf[offset], sizeof(euromap_current)); + offset += sizeof(euromap_current); + mb_data_.euromapVoltage = ntohs(euromap_voltage); + mb_data_.euromapCurrent = ntohs(euromap_current); + } else { + memcpy(&mb_data_.euromapVoltage, &buf[offset], + sizeof(mb_data_.euromapVoltage)); + offset += sizeof(mb_data_.euromapVoltage); + mb_data_.euromapVoltage = ntohl(mb_data_.euromapVoltage); + memcpy(&mb_data_.euromapCurrent, &buf[offset], + sizeof(mb_data_.euromapCurrent)); + offset += sizeof(mb_data_.euromapCurrent); + mb_data_.euromapCurrent = ntohl(mb_data_.euromapCurrent); + } + } } -double RobotState::getVersion() { - double ver; - val_lock_.lock(); - ver = version_msg_.major_version + 0.1 * version_msg_.minor_version - + .0000001 * version_msg_.svn_revision; - val_lock_.unlock(); - return ver; - +double RobotState::getVersion() +{ + double ver; + val_lock_.lock(); + ver = version_msg_.major_version + 0.1 * version_msg_.minor_version + + .0000001 * version_msg_.svn_revision; + val_lock_.unlock(); + return ver; } -void RobotState::finishedReading() { - new_data_available_ = false; +void RobotState::finishedReading() +{ + new_data_available_ = false; } -bool RobotState::getNewDataAvailable() { - return new_data_available_; +bool RobotState::getNewDataAvailable() +{ + return new_data_available_; } -int RobotState::getDigitalInputBits() { - return mb_data_.digitalInputBits; +int RobotState::getDigitalInputBits() +{ + return mb_data_.digitalInputBits; } -int RobotState::getDigitalOutputBits() { - return mb_data_.digitalOutputBits; +int RobotState::getDigitalOutputBits() +{ + return mb_data_.digitalOutputBits; } -double RobotState::getAnalogInput0() { - return mb_data_.analogInput0; +double RobotState::getAnalogInput0() +{ + return mb_data_.analogInput0; } -double RobotState::getAnalogInput1() { - return mb_data_.analogInput1; +double RobotState::getAnalogInput1() +{ + return mb_data_.analogInput1; } -double RobotState::getAnalogOutput0() { - return mb_data_.analogOutput0; - +double RobotState::getAnalogOutput0() +{ + return mb_data_.analogOutput0; } -double RobotState::getAnalogOutput1() { - return mb_data_.analogOutput1; +double RobotState::getAnalogOutput1() +{ + return mb_data_.analogOutput1; } -bool RobotState::isRobotConnected() { - return robot_mode_.isRobotConnected; +bool RobotState::isRobotConnected() +{ + return robot_mode_.isRobotConnected; } -bool RobotState::isRealRobotEnabled() { - return robot_mode_.isRealRobotEnabled; +bool RobotState::isRealRobotEnabled() +{ + return robot_mode_.isRealRobotEnabled; } -bool RobotState::isPowerOnRobot() { - return robot_mode_.isPowerOnRobot; +bool RobotState::isPowerOnRobot() +{ + return robot_mode_.isPowerOnRobot; } -bool RobotState::isEmergencyStopped() { - return robot_mode_.isEmergencyStopped; +bool RobotState::isEmergencyStopped() +{ + return robot_mode_.isEmergencyStopped; } -bool RobotState::isProtectiveStopped() { - return robot_mode_.isProtectiveStopped; +bool RobotState::isProtectiveStopped() +{ + return robot_mode_.isProtectiveStopped; } -bool RobotState::isProgramRunning() { - return robot_mode_.isProgramRunning; +bool RobotState::isProgramRunning() +{ + return robot_mode_.isProgramRunning; } -bool RobotState::isProgramPaused() { - return robot_mode_.isProgramPaused; +bool RobotState::isProgramPaused() +{ + return robot_mode_.isProgramPaused; } -unsigned char RobotState::getRobotMode() { - return robot_mode_.robotMode; +unsigned char RobotState::getRobotMode() +{ + return robot_mode_.robotMode; } -bool RobotState::isReady() { - if (robot_mode_.robotMode == robot_mode_running_) { - return true; - } - return false; +bool RobotState::isReady() +{ + if (robot_mode_.robotMode == robot_mode_running_) { + return true; + } + return false; } -void RobotState::setDisconnected() { - robot_mode_.isRobotConnected = false; - robot_mode_.isRealRobotEnabled = false; - robot_mode_.isPowerOnRobot = false; +void RobotState::setDisconnected() +{ + robot_mode_.isRobotConnected = false; + robot_mode_.isRealRobotEnabled = false; + robot_mode_.isPowerOnRobot = false; } diff --git a/src/robot_state_RT.cpp b/src/robot_state_RT.cpp index b170a04..46467fe 100644 --- a/src/robot_state_RT.cpp +++ b/src/robot_state_RT.cpp @@ -18,420 +18,458 @@ #include "ur_modern_driver/robot_state_RT.h" -RobotStateRT::RobotStateRT(std::condition_variable& msg_cond) { - version_ = 0.0; - time_ = 0.0; - q_target_.assign(6, 0.0); - qd_target_.assign(6, 0.0); - qdd_target_.assign(6, 0.0); - i_target_.assign(6, 0.0); - m_target_.assign(6, 0.0); - q_actual_.assign(6, 0.0); - qd_actual_.assign(6, 0.0); - i_actual_.assign(6, 0.0); - i_control_.assign(6, 0.0); - tool_vector_actual_.assign(6, 0.0); - tcp_speed_actual_.assign(6, 0.0); - tcp_force_.assign(6, 0.0); - tool_vector_target_.assign(6, 0.0); - tcp_speed_target_.assign(6, 0.0); - digital_input_bits_.assign(64, false); - motor_temperatures_.assign(6, 0.0); - controller_timer_ = 0.0; - robot_mode_ = 0.0; - joint_modes_.assign(6, 0.0); - safety_mode_ = 0.0; - tool_accelerometer_values_.assign(3, 0.0); - speed_scaling_ = 0.0; - linear_momentum_norm_ = 0.0; - v_main_ = 0.0; - v_robot_ = 0.0; - i_robot_ = 0.0; - v_actual_.assign(6, 0.0); - data_published_ = false; - controller_updated_ = false; - pMsg_cond_ = &msg_cond; +RobotStateRT::RobotStateRT(std::condition_variable& msg_cond) +{ + version_ = 0.0; + time_ = 0.0; + q_target_.assign(6, 0.0); + qd_target_.assign(6, 0.0); + qdd_target_.assign(6, 0.0); + i_target_.assign(6, 0.0); + m_target_.assign(6, 0.0); + q_actual_.assign(6, 0.0); + qd_actual_.assign(6, 0.0); + i_actual_.assign(6, 0.0); + i_control_.assign(6, 0.0); + tool_vector_actual_.assign(6, 0.0); + tcp_speed_actual_.assign(6, 0.0); + tcp_force_.assign(6, 0.0); + tool_vector_target_.assign(6, 0.0); + tcp_speed_target_.assign(6, 0.0); + digital_input_bits_.assign(64, false); + motor_temperatures_.assign(6, 0.0); + controller_timer_ = 0.0; + robot_mode_ = 0.0; + joint_modes_.assign(6, 0.0); + safety_mode_ = 0.0; + tool_accelerometer_values_.assign(3, 0.0); + speed_scaling_ = 0.0; + linear_momentum_norm_ = 0.0; + v_main_ = 0.0; + v_robot_ = 0.0; + i_robot_ = 0.0; + v_actual_.assign(6, 0.0); + data_published_ = false; + controller_updated_ = false; + pMsg_cond_ = &msg_cond; } -RobotStateRT::~RobotStateRT() { - /* Make sure nobody is waiting after this thread is destroyed */ - data_published_ = true; - controller_updated_ = true; - pMsg_cond_->notify_all(); +RobotStateRT::~RobotStateRT() +{ + /* Make sure nobody is waiting after this thread is destroyed */ + data_published_ = true; + controller_updated_ = true; + pMsg_cond_->notify_all(); } -void RobotStateRT::setDataPublished() { - data_published_ = false; +void RobotStateRT::setDataPublished() +{ + data_published_ = false; } -bool RobotStateRT::getDataPublished() { - return data_published_; +bool RobotStateRT::getDataPublished() +{ + return data_published_; } -void RobotStateRT::setControllerUpdated() { - controller_updated_ = false; +void RobotStateRT::setControllerUpdated() +{ + controller_updated_ = false; } -bool RobotStateRT::getControllerUpdated() { - return controller_updated_; +bool RobotStateRT::getControllerUpdated() +{ + return controller_updated_; } -double RobotStateRT::ntohd(uint64_t nf) { - double x; - nf = be64toh(nf); - memcpy(&x, &nf, sizeof(x)); - return x; +double RobotStateRT::ntohd(uint64_t nf) +{ + double x; + nf = be64toh(nf); + memcpy(&x, &nf, sizeof(x)); + return x; } -std::vector RobotStateRT::unpackVector(uint8_t * buf, int start_index, - int nr_of_vals) { - uint64_t q; - std::vector ret; - for (int i = 0; i < nr_of_vals; i++) { - memcpy(&q, &buf[start_index + i * sizeof(q)], sizeof(q)); - ret.push_back(ntohd(q)); - } - return ret; +std::vector RobotStateRT::unpackVector(uint8_t* buf, int start_index, + int nr_of_vals) +{ + uint64_t q; + std::vector ret; + for (int i = 0; i < nr_of_vals; i++) { + memcpy(&q, &buf[start_index + i * sizeof(q)], sizeof(q)); + ret.push_back(ntohd(q)); + } + return ret; } -std::vector RobotStateRT::unpackDigitalInputBits(int64_t data) { - std::vector ret; - for (int i = 0; i < 64; i++) { - ret.push_back((data & (1 << i)) >> i); - } - return ret; +std::vector RobotStateRT::unpackDigitalInputBits(int64_t data) +{ + std::vector ret; + for (int i = 0; i < 64; i++) { + ret.push_back((data & (1 << i)) >> i); + } + return ret; } -void RobotStateRT::setVersion(double ver) { - val_lock_.lock(); - version_ = ver; - val_lock_.unlock(); +void RobotStateRT::setVersion(double ver) +{ + val_lock_.lock(); + version_ = ver; + val_lock_.unlock(); } -double RobotStateRT::getVersion() { - double ret; - val_lock_.lock(); - ret = version_; - val_lock_.unlock(); - return ret; +double RobotStateRT::getVersion() +{ + double ret; + val_lock_.lock(); + ret = version_; + val_lock_.unlock(); + return ret; } -double RobotStateRT::getTime() { - double ret; - val_lock_.lock(); - ret = time_; - val_lock_.unlock(); - return ret; +double RobotStateRT::getTime() +{ + double ret; + val_lock_.lock(); + ret = time_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getQTarget() { - std::vector ret; - val_lock_.lock(); - ret = q_target_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getQTarget() +{ + std::vector ret; + val_lock_.lock(); + ret = q_target_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getQdTarget() { - std::vector ret; - val_lock_.lock(); - ret = qd_target_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getQdTarget() +{ + std::vector ret; + val_lock_.lock(); + ret = qd_target_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getQddTarget() { - std::vector ret; - val_lock_.lock(); - ret = qdd_target_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getQddTarget() +{ + std::vector ret; + val_lock_.lock(); + ret = qdd_target_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getITarget() { - std::vector ret; - val_lock_.lock(); - ret = i_target_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getITarget() +{ + std::vector ret; + val_lock_.lock(); + ret = i_target_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getMTarget() { - std::vector ret; - val_lock_.lock(); - ret = m_target_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getMTarget() +{ + std::vector ret; + val_lock_.lock(); + ret = m_target_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getQActual() { - std::vector ret; - val_lock_.lock(); - ret = q_actual_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getQActual() +{ + std::vector ret; + val_lock_.lock(); + ret = q_actual_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getQdActual() { - std::vector ret; - val_lock_.lock(); - ret = qd_actual_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getQdActual() +{ + std::vector ret; + val_lock_.lock(); + ret = qd_actual_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getIActual() { - std::vector ret; - val_lock_.lock(); - ret = i_actual_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getIActual() +{ + std::vector ret; + val_lock_.lock(); + ret = i_actual_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getIControl() { - std::vector ret; - val_lock_.lock(); - ret = i_control_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getIControl() +{ + std::vector ret; + val_lock_.lock(); + ret = i_control_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getToolVectorActual() { - std::vector ret; - val_lock_.lock(); - ret = tool_vector_actual_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getToolVectorActual() +{ + std::vector ret; + val_lock_.lock(); + ret = tool_vector_actual_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getTcpSpeedActual() { - std::vector ret; - val_lock_.lock(); - ret = tcp_speed_actual_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getTcpSpeedActual() +{ + std::vector ret; + val_lock_.lock(); + ret = tcp_speed_actual_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getTcpForce() { - std::vector ret; - val_lock_.lock(); - ret = tcp_force_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getTcpForce() +{ + std::vector ret; + val_lock_.lock(); + ret = tcp_force_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getToolVectorTarget() { - std::vector ret; - val_lock_.lock(); - ret = tool_vector_target_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getToolVectorTarget() +{ + std::vector ret; + val_lock_.lock(); + ret = tool_vector_target_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getTcpSpeedTarget() { - std::vector ret; - val_lock_.lock(); - ret = tcp_speed_target_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getTcpSpeedTarget() +{ + std::vector ret; + val_lock_.lock(); + ret = tcp_speed_target_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getDigitalInputBits() { - std::vector ret; - val_lock_.lock(); - ret = digital_input_bits_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getDigitalInputBits() +{ + std::vector ret; + val_lock_.lock(); + ret = digital_input_bits_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getMotorTemperatures() { - std::vector ret; - val_lock_.lock(); - ret = motor_temperatures_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getMotorTemperatures() +{ + std::vector ret; + val_lock_.lock(); + ret = motor_temperatures_; + val_lock_.unlock(); + return ret; } -double RobotStateRT::getControllerTimer() { - double ret; - val_lock_.lock(); - ret = controller_timer_; - val_lock_.unlock(); - return ret; +double RobotStateRT::getControllerTimer() +{ + double ret; + val_lock_.lock(); + ret = controller_timer_; + val_lock_.unlock(); + return ret; } -double RobotStateRT::getRobotMode() { - double ret; - val_lock_.lock(); - ret = robot_mode_; - val_lock_.unlock(); - return ret; +double RobotStateRT::getRobotMode() +{ + double ret; + val_lock_.lock(); + ret = robot_mode_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getJointModes() { - std::vector ret; - val_lock_.lock(); - ret = joint_modes_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getJointModes() +{ + std::vector ret; + val_lock_.lock(); + ret = joint_modes_; + val_lock_.unlock(); + return ret; } -double RobotStateRT::getSafety_mode() { - double ret; - val_lock_.lock(); - ret = safety_mode_; - val_lock_.unlock(); - return ret; +double RobotStateRT::getSafety_mode() +{ + double ret; + val_lock_.lock(); + ret = safety_mode_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getToolAccelerometerValues() { - std::vector ret; - val_lock_.lock(); - ret = tool_accelerometer_values_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getToolAccelerometerValues() +{ + std::vector ret; + val_lock_.lock(); + ret = tool_accelerometer_values_; + val_lock_.unlock(); + return ret; } -double RobotStateRT::getSpeedScaling() { - double ret; - val_lock_.lock(); - ret = speed_scaling_; - val_lock_.unlock(); - return ret; +double RobotStateRT::getSpeedScaling() +{ + double ret; + val_lock_.lock(); + ret = speed_scaling_; + val_lock_.unlock(); + return ret; } -double RobotStateRT::getLinearMomentumNorm() { - double ret; - val_lock_.lock(); - ret = linear_momentum_norm_; - val_lock_.unlock(); - return ret; +double RobotStateRT::getLinearMomentumNorm() +{ + double ret; + val_lock_.lock(); + ret = linear_momentum_norm_; + val_lock_.unlock(); + return ret; } -double RobotStateRT::getVMain() { - double ret; - val_lock_.lock(); - ret = v_main_; - val_lock_.unlock(); - return ret; +double RobotStateRT::getVMain() +{ + double ret; + val_lock_.lock(); + ret = v_main_; + val_lock_.unlock(); + return ret; } -double RobotStateRT::getVRobot() { - double ret; - val_lock_.lock(); - ret = v_robot_; - val_lock_.unlock(); - return ret; +double RobotStateRT::getVRobot() +{ + double ret; + val_lock_.lock(); + ret = v_robot_; + val_lock_.unlock(); + return ret; } -double RobotStateRT::getIRobot() { - double ret; - val_lock_.lock(); - ret = i_robot_; - val_lock_.unlock(); - return ret; +double RobotStateRT::getIRobot() +{ + double ret; + val_lock_.lock(); + ret = i_robot_; + val_lock_.unlock(); + return ret; } -std::vector RobotStateRT::getVActual() { - std::vector ret; - val_lock_.lock(); - ret = v_actual_; - val_lock_.unlock(); - return ret; +std::vector RobotStateRT::getVActual() +{ + std::vector ret; + val_lock_.lock(); + ret = v_actual_; + val_lock_.unlock(); + return ret; } -void RobotStateRT::unpack(uint8_t * buf) { - int64_t digital_input_bits; - uint64_t unpack_to; - uint16_t offset = 0; - val_lock_.lock(); - int len; - memcpy(&len, &buf[offset], sizeof(len)); +void RobotStateRT::unpack(uint8_t* buf) +{ + int64_t digital_input_bits; + uint64_t unpack_to; + uint16_t offset = 0; + val_lock_.lock(); + int len; + memcpy(&len, &buf[offset], sizeof(len)); - offset += sizeof(len); - len = ntohl(len); + offset += sizeof(len); + len = ntohl(len); - //Check the correct message length is received - bool len_good = true; - if (version_ >= 1.6 && version_ < 1.7) { //v1.6 - if (len != 756) - len_good = false; - } else if (version_ >= 1.7 && version_ < 1.8) { //v1.7 - if (len != 764) - len_good = false; - } else if (version_ >= 1.8 && version_ < 1.9) { //v1.8 - if (len != 812) - len_good = false; - } else if (version_ >= 3.0 && version_ < 3.2) { //v3.0 & v3.1 - if (len != 1044) - len_good = false; - } else if (version_ >= 3.2 && version_ < 3.3) { //v3.2 - if (len != 1060) - len_good = false; - } + //Check the correct message length is received + bool len_good = true; + if (version_ >= 1.6 && version_ < 1.7) { //v1.6 + if (len != 756) + len_good = false; + } else if (version_ >= 1.7 && version_ < 1.8) { //v1.7 + if (len != 764) + len_good = false; + } else if (version_ >= 1.8 && version_ < 1.9) { //v1.8 + if (len != 812) + len_good = false; + } else if (version_ >= 3.0 && version_ < 3.2) { //v3.0 & v3.1 + if (len != 1044) + len_good = false; + } else if (version_ >= 3.2 && version_ < 3.3) { //v3.2 + if (len != 1060) + len_good = false; + } - if (!len_good) { - printf("Wrong length of message on RT interface: %i\n", len); - val_lock_.unlock(); - return; - } + if (!len_good) { + printf("Wrong length of message on RT interface: %i\n", len); + val_lock_.unlock(); + return; + } - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - time_ = RobotStateRT::ntohd(unpack_to); - offset += sizeof(double); - q_target_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - qd_target_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - qdd_target_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - i_target_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - m_target_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - q_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - qd_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - i_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - if (version_ <= 1.9) { - if (version_ > 1.6) - tool_accelerometer_values_ = unpackVector(buf, offset, 3); - offset += sizeof(double) * (3 + 15); - tcp_force_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tool_vector_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tcp_speed_actual_ = unpackVector(buf, offset, 6); - } else { - i_control_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tool_vector_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tcp_speed_actual_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tcp_force_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tool_vector_target_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - tcp_speed_target_ = unpackVector(buf, offset, 6); - } - offset += sizeof(double) * 6; - - memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits)); - digital_input_bits_ = unpackDigitalInputBits(be64toh(digital_input_bits)); - offset += sizeof(double); - motor_temperatures_ = unpackVector(buf, offset, 6); - offset += sizeof(double) * 6; - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - controller_timer_ = ntohd(unpack_to); - if (version_ > 1.6) { - offset += sizeof(double) * 2; - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - robot_mode_ = ntohd(unpack_to); - if (version_ > 1.7) { - offset += sizeof(double); - joint_modes_ = unpackVector(buf, offset, 6); - } - } - if (version_ > 1.8) { - offset += sizeof(double) * 6; - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - safety_mode_ = ntohd(unpack_to); - offset += sizeof(double); - tool_accelerometer_values_ = unpackVector(buf, offset, 3); - offset += sizeof(double) * 3; - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - speed_scaling_ = ntohd(unpack_to); - offset += sizeof(double); - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - linear_momentum_norm_ = ntohd(unpack_to); - offset += sizeof(double); - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - v_main_ = ntohd(unpack_to); - offset += sizeof(double); - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - v_robot_ = ntohd(unpack_to); - offset += sizeof(double); - memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); - i_robot_ = ntohd(unpack_to); - offset += sizeof(double); - v_actual_ = unpackVector(buf, offset, 6); - } - val_lock_.unlock(); - controller_updated_ = true; - data_published_ = true; - pMsg_cond_->notify_all(); + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); + time_ = RobotStateRT::ntohd(unpack_to); + offset += sizeof(double); + q_target_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + qd_target_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + qdd_target_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + i_target_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + m_target_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + q_actual_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + qd_actual_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + i_actual_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + if (version_ <= 1.9) { + if (version_ > 1.6) + tool_accelerometer_values_ = unpackVector(buf, offset, 3); + offset += sizeof(double) * (3 + 15); + tcp_force_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + tool_vector_actual_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + tcp_speed_actual_ = unpackVector(buf, offset, 6); + } else { + i_control_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + tool_vector_actual_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + tcp_speed_actual_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + tcp_force_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + tool_vector_target_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + tcp_speed_target_ = unpackVector(buf, offset, 6); + } + offset += sizeof(double) * 6; + memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits)); + digital_input_bits_ = unpackDigitalInputBits(be64toh(digital_input_bits)); + offset += sizeof(double); + motor_temperatures_ = unpackVector(buf, offset, 6); + offset += sizeof(double) * 6; + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); + controller_timer_ = ntohd(unpack_to); + if (version_ > 1.6) { + offset += sizeof(double) * 2; + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); + robot_mode_ = ntohd(unpack_to); + if (version_ > 1.7) { + offset += sizeof(double); + joint_modes_ = unpackVector(buf, offset, 6); + } + } + if (version_ > 1.8) { + offset += sizeof(double) * 6; + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); + safety_mode_ = ntohd(unpack_to); + offset += sizeof(double); + tool_accelerometer_values_ = unpackVector(buf, offset, 3); + offset += sizeof(double) * 3; + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); + speed_scaling_ = ntohd(unpack_to); + offset += sizeof(double); + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); + linear_momentum_norm_ = ntohd(unpack_to); + offset += sizeof(double); + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); + v_main_ = ntohd(unpack_to); + offset += sizeof(double); + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); + v_robot_ = ntohd(unpack_to); + offset += sizeof(double); + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); + i_robot_ = ntohd(unpack_to); + offset += sizeof(double); + v_actual_ = unpackVector(buf, offset, 6); + } + val_lock_.unlock(); + controller_updated_ = true; + data_published_ = true; + pMsg_cond_->notify_all(); } - diff --git a/src/ros/rt_publisher.cpp b/src/ros/rt_publisher.cpp index c0db690..af5a4a4 100644 --- a/src/ros/rt_publisher.cpp +++ b/src/ros/rt_publisher.cpp @@ -1,17 +1,18 @@ #include "ur_modern_driver/ros/rt_publisher.h" -bool RTPublisher::publish_joints(RTShared &packet, ros::Time &t) { - sensor_msgs::JointState joint_msg; +bool RTPublisher::publish_joints(RTShared& packet, ros::Time& t) +{ + sensor_msgs::JointState joint_msg; joint_msg.header.stamp = t; joint_msg.name = _joint_names; - for(auto const& q : packet.q_actual) { + for (auto const& q : packet.q_actual) { joint_msg.position.push_back(q); } - for(auto const& qd : packet.qd_actual) { + for (auto const& qd : packet.qd_actual) { joint_msg.velocity.push_back(qd); } - for(auto const& i : packet.i_actual) { + for (auto const& i : packet.i_actual) { joint_msg.effort.push_back(i); } @@ -20,7 +21,8 @@ bool RTPublisher::publish_joints(RTShared &packet, ros::Time &t) { return true; } -bool RTPublisher::publish_wrench(RTShared &packet, ros::Time &t) { +bool RTPublisher::publish_wrench(RTShared& packet, ros::Time& t) +{ geometry_msgs::WrenchStamped wrench_msg; wrench_msg.header.stamp = t; @@ -36,7 +38,8 @@ bool RTPublisher::publish_wrench(RTShared &packet, ros::Time &t) { return true; } -bool RTPublisher::publish_tool(RTShared &packet, ros::Time &t) { +bool RTPublisher::publish_tool(RTShared& packet, ros::Time& t) +{ geometry_msgs::TwistStamped tool_twist; tool_twist.header.stamp = t; @@ -55,20 +58,25 @@ bool RTPublisher::publish_tool(RTShared &packet, ros::Time &t) { return true; } -bool RTPublisher::publish(RTShared &packet) { +bool RTPublisher::publish(RTShared& packet) +{ ros::Time time = ros::Time::now(); return publish_joints(packet, time) && publish_wrench(packet, time) && publish_tool(packet, time); } -bool RTPublisher::consume(RTState_V1_6__7 &state) { +bool RTPublisher::consume(RTState_V1_6__7& state) +{ return publish(state); } -bool RTPublisher::consume(RTState_V1_8 &state) { +bool RTPublisher::consume(RTState_V1_8& state) +{ return publish(state); } -bool RTPublisher::consume(RTState_V3_0__1 &state) { +bool RTPublisher::consume(RTState_V3_0__1& state) +{ return publish(state); } -bool RTPublisher::consume(RTState_V3_2__3 &state) { +bool RTPublisher::consume(RTState_V3_2__3& state) +{ return publish(state); } \ No newline at end of file diff --git a/src/ros_main.cpp b/src/ros_main.cpp index c2dc749..48cf6b6 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -1,18 +1,18 @@ -#include -#include #include -#include +#include #include +#include +#include #include "ur_modern_driver/log.h" #include "ur_modern_driver/pipeline.h" -#include "ur_modern_driver/ur/producer.h" -#include "ur_modern_driver/ur/state.h" -#include "ur_modern_driver/ur/rt_state.h" -#include "ur_modern_driver/ur/parser.h" -#include "ur_modern_driver/ur/messages.h" -#include "ur_modern_driver/ur/factory.h" #include "ur_modern_driver/ros/rt_publisher.h" +#include "ur_modern_driver/ur/factory.h" +#include "ur_modern_driver/ur/messages.h" +#include "ur_modern_driver/ur/parser.h" +#include "ur_modern_driver/ur/producer.h" +#include "ur_modern_driver/ur/rt_state.h" +#include "ur_modern_driver/ur/state.h" static const std::string IP_ADDR_ARG("~robot_ip_address"); static const std::string REVERSE_PORT_ARG("~reverse_port"); @@ -21,48 +21,48 @@ static const std::string PREFIX_ARG("~prefix"); static const std::string BASE_FRAME_ARG("~base_frame"); static const std::string TOOL_FRAME_ARG("~tool_frame"); - struct ProgArgs { public: - std::string host; - std::string prefix; - std::string base_frame; - std::string tool_frame; - int32_t reverse_port; - bool use_sim_time; + std::string host; + std::string prefix; + std::string base_frame; + std::string tool_frame; + int32_t reverse_port; + bool use_sim_time; }; -bool parse_args(ProgArgs &args) { - if(!ros::param::get(IP_ADDR_ARG, args.host)) { - LOG_ERROR("robot_ip_address parameter must be set!"); - return false; - } - ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001)); - ros::param::param(SIM_TIME_ARG, args.use_sim_time, false); - ros::param::param(PREFIX_ARG, args.prefix, std::string()); - ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link"); - ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller"); - return true; +bool parse_args(ProgArgs& args) +{ + if (!ros::param::get(IP_ADDR_ARG, args.host)) { + LOG_ERROR("robot_ip_address parameter must be set!"); + return false; + } + ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001)); + ros::param::param(SIM_TIME_ARG, args.use_sim_time, false); + ros::param::param(PREFIX_ARG, args.prefix, std::string()); + ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link"); + ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller"); + return true; } - -int main(int argc, char **argv) { +int main(int argc, char** argv) +{ ros::init(argc, argv, "ur_driver"); - ProgArgs args; - if(!parse_args(args)) { - return EXIT_FAILURE; - } + ProgArgs args; + if (!parse_args(args)) { + return EXIT_FAILURE; + } - URFactory factory(args.host); + URFactory factory(args.host); - auto parser = factory.get_rt_parser(); + auto parser = factory.get_rt_parser(); - URStream s2(args.host, 30003); - URProducer p2(s2, *parser); + URStream s2(args.host, 30003); + URProducer p2(s2, *parser); - /* + /* p2.setup_producer(); while(true) { @@ -77,18 +77,17 @@ int main(int argc, char **argv) { p2.teardown_producer(); */ - RTPublisher pub(args.prefix, args.base_frame); + RTPublisher pub(args.prefix, args.base_frame); - Pipeline pl(p2, pub); + Pipeline pl(p2, pub); - pl.run(); + pl.run(); - while(ros::ok()) { - std::this_thread::sleep_for(std::chrono::milliseconds(33)); - } + while (ros::ok()) { + std::this_thread::sleep_for(std::chrono::milliseconds(33)); + } - pl.stop(); + pl.stop(); - - return EXIT_SUCCESS; + return EXIT_SUCCESS; } \ No newline at end of file diff --git a/src/ur/master_board.cpp b/src/ur/master_board.cpp index c8d947b..046d0ee 100644 --- a/src/ur/master_board.cpp +++ b/src/ur/master_board.cpp @@ -1,11 +1,12 @@ #include "ur_modern_driver/ur/master_board.h" #include "ur_modern_driver/ur/consumer.h" -bool SharedMasterBoardData::parse_with(BinParser &bp) { +bool SharedMasterBoardData::parse_with(BinParser& bp) +{ bp.parse(analog_input_range0); bp.parse(analog_input_range1); bp.parse(analog_input0); - bp.parse(analog_input1); + bp.parse(analog_input1); bp.parse(analog_output_domain0); bp.parse(analog_output_domain1); bp.parse(analog_output0); @@ -17,8 +18,9 @@ bool SharedMasterBoardData::parse_with(BinParser &bp) { return true; } -bool MasterBoardData_V1_X::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool MasterBoardData_V1_X::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; bp.parse(digital_input_bits); @@ -30,8 +32,8 @@ bool MasterBoardData_V1_X::parse_with(BinParser &bp) { bp.parse(master_on_off_state); bp.parse(euromap67_interface_installed); - if(euromap67_interface_installed) { - if(!bp.check_size(MasterBoardData_V1_X::EURO_SIZE)) + if (euromap67_interface_installed) { + if (!bp.check_size(MasterBoardData_V1_X::EURO_SIZE)) return false; bp.parse(euromap_voltage); @@ -41,23 +43,24 @@ bool MasterBoardData_V1_X::parse_with(BinParser &bp) { return true; } -bool MasterBoardData_V3_0__1::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool MasterBoardData_V3_0__1::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; bp.parse(digital_input_bits); bp.parse(digital_output_bits); - + SharedMasterBoardData::parse_with(bp); bp.parse(safety_mode); bp.parse(in_reduced_mode); bp.parse(euromap67_interface_installed); - if(euromap67_interface_installed) { - if(!bp.check_size(MasterBoardData_V3_0__1::EURO_SIZE)) + if (euromap67_interface_installed) { + if (!bp.check_size(MasterBoardData_V3_0__1::EURO_SIZE)) return false; - + bp.parse(euromap_voltage); bp.parse(euromap_current); } @@ -67,10 +70,9 @@ bool MasterBoardData_V3_0__1::parse_with(BinParser &bp) { return true; } - - -bool MasterBoardData_V3_2::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool MasterBoardData_V3_2::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; MasterBoardData_V3_0__1::parse_with(bp); @@ -81,16 +83,15 @@ bool MasterBoardData_V3_2::parse_with(BinParser &bp) { return true; } - - - - -bool MasterBoardData_V1_X::consume_with(URStatePacketConsumer &consumer) { +bool MasterBoardData_V1_X::consume_with(URStatePacketConsumer& consumer) +{ return consumer.consume(*this); } -bool MasterBoardData_V3_0__1::consume_with(URStatePacketConsumer &consumer) { +bool MasterBoardData_V3_0__1::consume_with(URStatePacketConsumer& consumer) +{ return consumer.consume(*this); } -bool MasterBoardData_V3_2::consume_with(URStatePacketConsumer &consumer) { +bool MasterBoardData_V3_2::consume_with(URStatePacketConsumer& consumer) +{ return consumer.consume(*this); } \ No newline at end of file diff --git a/src/ur/messages.cpp b/src/ur/messages.cpp index e079f8d..b532792 100644 --- a/src/ur/messages.cpp +++ b/src/ur/messages.cpp @@ -1,9 +1,9 @@ #include "ur_modern_driver/ur/messages.h" #include "ur_modern_driver/ur/consumer.h" +bool VersionMessage::parse_with(BinParser& bp) +{ -bool VersionMessage::parse_with(BinParser &bp) { - bp.parse(project_name); bp.parse(major_version); bp.parse(minor_version); @@ -11,10 +11,10 @@ bool VersionMessage::parse_with(BinParser &bp) { bp.consume(sizeof(uint32_t)); //undocumented field?? bp.parse_remainder(build_date); - return true; //not really possible to check dynamic size packets + return true; //not really possible to check dynamic size packets } - -bool VersionMessage::consume_with(URMessagePacketConsumer &consumer) { +bool VersionMessage::consume_with(URMessagePacketConsumer& consumer) +{ return consumer.consume(*this); } \ No newline at end of file diff --git a/src/ur/robot_mode.cpp b/src/ur/robot_mode.cpp index 1c62406..be0770f 100644 --- a/src/ur/robot_mode.cpp +++ b/src/ur/robot_mode.cpp @@ -1,8 +1,8 @@ #include "ur_modern_driver/ur/robot_mode.h" #include "ur_modern_driver/ur/consumer.h" - -bool SharedRobotModeData::parse_with(BinParser &bp) { +bool SharedRobotModeData::parse_with(BinParser& bp) +{ bp.parse(timestamp); bp.parse(physical_robot_connected); bp.parse(real_robot_enabled); @@ -11,9 +11,9 @@ bool SharedRobotModeData::parse_with(BinParser &bp) { return true; } - -bool RobotModeData_V1_X::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool RobotModeData_V1_X::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; SharedRobotModeData::parse_with(bp); @@ -27,13 +27,13 @@ bool RobotModeData_V1_X::parse_with(BinParser &bp) { return true; } - -bool RobotModeData_V3_0__1::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool RobotModeData_V3_0__1::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; SharedRobotModeData::parse_with(bp); - + bp.parse(protective_stopped); bp.parse(program_running); bp.parse(program_paused); @@ -45,8 +45,9 @@ bool RobotModeData_V3_0__1::parse_with(BinParser &bp) { return true; } -bool RobotModeData_V3_2::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool RobotModeData_V3_2::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; RobotModeData_V3_0__1::parse_with(bp); @@ -56,15 +57,15 @@ bool RobotModeData_V3_2::parse_with(BinParser &bp) { return true; } - - - -bool RobotModeData_V1_X::consume_with(URStatePacketConsumer &consumer) { +bool RobotModeData_V1_X::consume_with(URStatePacketConsumer& consumer) +{ return consumer.consume(*this); } -bool RobotModeData_V3_0__1::consume_with(URStatePacketConsumer &consumer) { +bool RobotModeData_V3_0__1::consume_with(URStatePacketConsumer& consumer) +{ return consumer.consume(*this); } -bool RobotModeData_V3_2::consume_with(URStatePacketConsumer &consumer) { +bool RobotModeData_V3_2::consume_with(URStatePacketConsumer& consumer) +{ return consumer.consume(*this); } \ No newline at end of file diff --git a/src/ur/rt_state.cpp b/src/ur/rt_state.cpp index 596134d..f13aa73 100644 --- a/src/ur/rt_state.cpp +++ b/src/ur/rt_state.cpp @@ -1,8 +1,8 @@ #include "ur_modern_driver/ur/rt_state.h" #include "ur_modern_driver/ur/consumer.h" - -bool RTShared::parse_shared1(BinParser &bp) { +bool RTShared::parse_shared1(BinParser& bp) +{ bp.parse(time); bp.parse(q_target); bp.parse(qd_target); @@ -15,7 +15,8 @@ bool RTShared::parse_shared1(BinParser &bp) { return true; } -bool RTShared::parse_shared2(BinParser &bp) { +bool RTShared::parse_shared2(BinParser& bp) +{ bp.parse(digital_input); bp.parse(motor_temperatures); bp.parse(controller_time); @@ -23,13 +24,13 @@ bool RTShared::parse_shared2(BinParser &bp) { return true; } - -bool RTState_V1_6__7::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool RTState_V1_6__7::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; parse_shared1(bp); - + bp.parse(tool_accelerometer_values); bp.parse(tcp_force); bp.parse(tool_vector_actual); @@ -40,8 +41,9 @@ bool RTState_V1_6__7::parse_with(BinParser &bp) { return true; } -bool RTState_V1_8::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool RTState_V1_8::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; RTState_V1_6__7::parse_with(bp); @@ -51,8 +53,9 @@ bool RTState_V1_8::parse_with(BinParser &bp) { return true; } -bool RTState_V3_0__1::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool RTState_V3_0__1::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; parse_shared1(bp); @@ -74,7 +77,7 @@ bool RTState_V3_0__1::parse_with(BinParser &bp) { bp.parse(speed_scaling); bp.parse(linear_momentum_norm); bp.consume(sizeof(double)); //skip undocumented - bp.consume(sizeof(double)); //skip undocumented + bp.consume(sizeof(double)); //skip undocumented bp.parse(v_main); bp.parse(v_robot); bp.parse(i_robot); @@ -83,8 +86,9 @@ bool RTState_V3_0__1::parse_with(BinParser &bp) { return true; } -bool RTState_V3_2__3::parse_with(BinParser &bp) { - if(!bp.check_size()) +bool RTState_V3_2__3::parse_with(BinParser& bp) +{ + if (!bp.check_size()) return false; RTState_V3_0__1::parse_with(bp); @@ -95,15 +99,19 @@ bool RTState_V3_2__3::parse_with(BinParser &bp) { return true; } -bool RTState_V1_6__7::consume_with(URRTPacketConsumer &consumer) { +bool RTState_V1_6__7::consume_with(URRTPacketConsumer& consumer) +{ return consumer.consume(*this); } -bool RTState_V1_8::consume_with(URRTPacketConsumer &consumer) { +bool RTState_V1_8::consume_with(URRTPacketConsumer& consumer) +{ return consumer.consume(*this); } -bool RTState_V3_0__1::consume_with(URRTPacketConsumer &consumer) { +bool RTState_V3_0__1::consume_with(URRTPacketConsumer& consumer) +{ return consumer.consume(*this); } -bool RTState_V3_2__3::consume_with(URRTPacketConsumer &consumer) { +bool RTState_V3_2__3::consume_with(URRTPacketConsumer& consumer) +{ return consumer.consume(*this); } \ No newline at end of file diff --git a/src/ur/state.cpp b/src/ur/state.cpp index fe5af1b..0b4f4f2 100644 --- a/src/ur/state.cpp +++ b/src/ur/state.cpp @@ -1,7 +1,5 @@ -#include "ur_modern_driver/log.h" #include "ur_modern_driver/ur/state.h" - - +#include "ur_modern_driver/log.h" //StatePacket::~StatePacket() { } diff --git a/src/ur/stream.cpp b/src/ur/stream.cpp index f66482b..d0fd10f 100644 --- a/src/ur/stream.cpp +++ b/src/ur/stream.cpp @@ -1,13 +1,14 @@ #include -#include -#include #include +#include +#include -#include "ur_modern_driver/ur/stream.h" #include "ur_modern_driver/log.h" +#include "ur_modern_driver/ur/stream.h" -bool URStream::connect() { - if(_initialized) +bool URStream::connect() +{ + if (_initialized) return false; LOG_INFO("Connecting to UR @ %s:%d", _host.c_str(), _port); @@ -23,20 +24,20 @@ bool URStream::connect() { hints.ai_socktype = SOCK_STREAM; hints.ai_flags = AI_PASSIVE; - if(getaddrinfo(_host.c_str(), service.c_str(), &hints, &result) != 0) { - LOG_ERROR("Failed to get host name"); + if (getaddrinfo(_host.c_str(), service.c_str(), &hints, &result) != 0) { + LOG_ERROR("Failed to get host name"); return false; } - + //loop through the list of addresses untill we find one that's connectable - for(struct addrinfo *p = result; p != nullptr; p = p->ai_next) { + for (struct addrinfo* p = result; p != nullptr; p = p->ai_next) { _socket_fd = socket(p->ai_family, p->ai_socktype, p->ai_protocol); - if(_socket_fd == -1) //socket error? + if (_socket_fd == -1) //socket error? continue; - if(::connect(_socket_fd, p->ai_addr, p->ai_addrlen) != 0) { - if(_stopping) + if (::connect(_socket_fd, p->ai_addr, p->ai_addrlen) != 0) { + if (_stopping) break; else continue; //try next addrinfo if connect fails @@ -51,14 +52,15 @@ bool URStream::connect() { } freeaddrinfo(result); - if(!_initialized) + if (!_initialized) LOG_ERROR("Connection failed"); return _initialized; } -void URStream::disconnect() { - if(!_initialized || _stopping) +void URStream::disconnect() +{ + if (!_initialized || _stopping) return; _stopping = true; @@ -66,10 +68,11 @@ void URStream::disconnect() { _initialized = false; } -ssize_t URStream::send(uint8_t *buf, size_t buf_len) { - if(!_initialized) +ssize_t URStream::send(uint8_t* buf, size_t buf_len) +{ + if (!_initialized) return -1; - if(_stopping) + if (_stopping) return 0; size_t total = 0; @@ -77,9 +80,9 @@ ssize_t URStream::send(uint8_t *buf, size_t buf_len) { //TODO: handle reconnect? //handle partial sends - while(total < buf_len) { - ssize_t sent = ::send(_socket_fd, buf+total, remaining, 0); - if(sent <= 0) + while (total < buf_len) { + ssize_t sent = ::send(_socket_fd, buf + total, remaining, 0); + if (sent <= 0) return _stopping ? 0 : sent; total += sent; remaining -= sent; @@ -88,24 +91,25 @@ ssize_t URStream::send(uint8_t *buf, size_t buf_len) { return total; } -ssize_t URStream::receive(uint8_t *buf, size_t buf_len) { - if(!_initialized) +ssize_t URStream::receive(uint8_t* buf, size_t buf_len) +{ + if (!_initialized) return -1; - if(_stopping) + if (_stopping) return 0; size_t remainder = sizeof(int32_t); - uint8_t *buf_pos = buf; + uint8_t* buf_pos = buf; bool initial = true; do { ssize_t read = recv(_socket_fd, buf_pos, remainder, 0); - if(read <= 0) //failed reading from socket - return _stopping ? 0 : read; - - if(initial) { + if (read <= 0) //failed reading from socket + return _stopping ? 0 : read; + + if (initial) { remainder = be32toh(*(reinterpret_cast(buf))); - if(remainder >= (buf_len - sizeof(int32_t))) { + if (remainder >= (buf_len - sizeof(int32_t))) { LOG_ERROR("Packet size %d is larger than buffer %d, discarding.", remainder, buf_len); return -1; } @@ -113,8 +117,8 @@ ssize_t URStream::receive(uint8_t *buf, size_t buf_len) { } buf_pos += read; - remainder -= read; - } while(remainder > 0); + remainder -= read; + } while (remainder > 0); return buf_pos - buf; } \ No newline at end of file diff --git a/src/ur_communication.cpp b/src/ur_communication.cpp index cf04cc3..b883004 100644 --- a/src/ur_communication.cpp +++ b/src/ur_communication.cpp @@ -19,163 +19,167 @@ #include "ur_modern_driver/ur_communication.h" UrCommunication::UrCommunication(std::condition_variable& msg_cond, - std::string host) { - robot_state_ = new RobotState(msg_cond); - bzero((char *) &pri_serv_addr_, sizeof(pri_serv_addr_)); - bzero((char *) &sec_serv_addr_, sizeof(sec_serv_addr_)); - pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (pri_sockfd_ < 0) { - print_fatal("ERROR opening socket pri_sockfd"); - } - sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (sec_sockfd_ < 0) { - print_fatal("ERROR opening socket sec_sockfd"); - } - server_ = gethostbyname(host.c_str()); - if (server_ == NULL) { - print_fatal("ERROR, unknown host"); - } - pri_serv_addr_.sin_family = AF_INET; - sec_serv_addr_.sin_family = AF_INET; - bcopy((char *) server_->h_addr, (char *)&pri_serv_addr_.sin_addr.s_addr, server_->h_length); - bcopy((char *) server_->h_addr, (char *)&sec_serv_addr_.sin_addr.s_addr, server_->h_length); - pri_serv_addr_.sin_port = htons(30001); - sec_serv_addr_.sin_port = htons(30002); - flag_ = 1; - setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, - sizeof(int)); - setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, - sizeof(int)); - setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, - sizeof(int)); - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, - sizeof(int)); - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, - sizeof(int)); - setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, - sizeof(int)); - fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); - connected_ = false; - keepalive_ = false; + std::string host) +{ + robot_state_ = new RobotState(msg_cond); + bzero((char*)&pri_serv_addr_, sizeof(pri_serv_addr_)); + bzero((char*)&sec_serv_addr_, sizeof(sec_serv_addr_)); + pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (pri_sockfd_ < 0) { + print_fatal("ERROR opening socket pri_sockfd"); + } + sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (sec_sockfd_ < 0) { + print_fatal("ERROR opening socket sec_sockfd"); + } + server_ = gethostbyname(host.c_str()); + if (server_ == NULL) { + print_fatal("ERROR, unknown host"); + } + pri_serv_addr_.sin_family = AF_INET; + sec_serv_addr_.sin_family = AF_INET; + bcopy((char*)server_->h_addr, (char*)&pri_serv_addr_.sin_addr.s_addr, server_->h_length); + bcopy((char*)server_->h_addr, (char*)&sec_serv_addr_.sin_addr.s_addr, server_->h_length); + pri_serv_addr_.sin_port = htons(30001); + sec_serv_addr_.sin_port = htons(30002); + flag_ = 1; + setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, + sizeof(int)); + setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, + sizeof(int)); + setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, + sizeof(int)); + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, + sizeof(int)); + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, + sizeof(int)); + setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, + sizeof(int)); + fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); + connected_ = false; + keepalive_ = false; } -bool UrCommunication::start() { - keepalive_ = true; - uint8_t buf[512]; - unsigned int bytes_read; - std::string cmd; - bzero(buf, 512); - print_debug("Acquire firmware version: Connecting..."); - if (connect(pri_sockfd_, (struct sockaddr *) &pri_serv_addr_, - sizeof(pri_serv_addr_)) < 0) { - print_fatal("Error connecting to get firmware version"); - return false; - } - print_debug("Acquire firmware version: Got connection"); - bytes_read = read(pri_sockfd_, buf, 512); - setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, - sizeof(int)); - robot_state_->unpack(buf, bytes_read); - //wait for some traffic so the UR socket doesn't die in version 3.1. - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - char tmp[64]; - sprintf(tmp, "Firmware version detected: %.7f", robot_state_->getVersion()); - print_debug(tmp); - close(pri_sockfd_); +bool UrCommunication::start() +{ + keepalive_ = true; + uint8_t buf[512]; + unsigned int bytes_read; + std::string cmd; + bzero(buf, 512); + print_debug("Acquire firmware version: Connecting..."); + if (connect(pri_sockfd_, (struct sockaddr*)&pri_serv_addr_, + sizeof(pri_serv_addr_)) + < 0) { + print_fatal("Error connecting to get firmware version"); + return false; + } + print_debug("Acquire firmware version: Got connection"); + bytes_read = read(pri_sockfd_, buf, 512); + setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, + sizeof(int)); + robot_state_->unpack(buf, bytes_read); + //wait for some traffic so the UR socket doesn't die in version 3.1. + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + char tmp[64]; + sprintf(tmp, "Firmware version detected: %.7f", robot_state_->getVersion()); + print_debug(tmp); + close(pri_sockfd_); - print_debug( - "Switching to secondary interface for masterboard data: Connecting..."); + print_debug( + "Switching to secondary interface for masterboard data: Connecting..."); - fd_set writefds; - struct timeval timeout; + fd_set writefds; + struct timeval timeout; - connect(sec_sockfd_, (struct sockaddr *) &sec_serv_addr_, - sizeof(sec_serv_addr_)); - FD_ZERO(&writefds); - FD_SET(sec_sockfd_, &writefds); - timeout.tv_sec = 10; - timeout.tv_usec = 0; - select(sec_sockfd_ + 1, NULL, &writefds, NULL, &timeout); - unsigned int flag_len; - getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); - if (flag_ < 0) { - print_fatal("Error connecting to secondary interface"); - return false; - } - print_debug("Secondary interface: Got connection"); - comThread_ = std::thread(&UrCommunication::run, this); - return true; + connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_, + sizeof(sec_serv_addr_)); + FD_ZERO(&writefds); + FD_SET(sec_sockfd_, &writefds); + timeout.tv_sec = 10; + timeout.tv_usec = 0; + select(sec_sockfd_ + 1, NULL, &writefds, NULL, &timeout); + unsigned int flag_len; + getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); + if (flag_ < 0) { + print_fatal("Error connecting to secondary interface"); + return false; + } + print_debug("Secondary interface: Got connection"); + comThread_ = std::thread(&UrCommunication::run, this); + return true; } -void UrCommunication::halt() { - keepalive_ = false; - comThread_.join(); +void UrCommunication::halt() +{ + keepalive_ = false; + comThread_.join(); } -void UrCommunication::run() { - uint8_t buf[2048]; - int bytes_read; - bzero(buf, 2048); - struct timeval timeout; - fd_set readfds; - FD_ZERO(&readfds); - FD_SET(sec_sockfd_, &readfds); - connected_ = true; - while (keepalive_) { - while (connected_ && keepalive_) { - timeout.tv_sec = 0; //do this each loop as selects modifies timeout - timeout.tv_usec = 500000; // timeout of 0.5 sec - select(sec_sockfd_ + 1, &readfds, NULL, NULL, &timeout); - bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes - if (bytes_read > 0) { - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, - (char *) &flag_, sizeof(int)); - robot_state_->unpack(buf, bytes_read); - } else { - connected_ = false; - robot_state_->setDisconnected(); - close(sec_sockfd_); - } - } - if (keepalive_) { - //reconnect - print_warning("Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."); - sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (sec_sockfd_ < 0) { - print_fatal("ERROR opening secondary socket"); - } - flag_ = 1; - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, - sizeof(int)); - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, - sizeof(int)); - setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, - sizeof(int)); - fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); - while (keepalive_ && !connected_) { - std::this_thread::sleep_for(std::chrono::seconds(10)); - fd_set writefds; +void UrCommunication::run() +{ + uint8_t buf[2048]; + int bytes_read; + bzero(buf, 2048); + struct timeval timeout; + fd_set readfds; + FD_ZERO(&readfds); + FD_SET(sec_sockfd_, &readfds); + connected_ = true; + while (keepalive_) { + while (connected_ && keepalive_) { + timeout.tv_sec = 0; //do this each loop as selects modifies timeout + timeout.tv_usec = 500000; // timeout of 0.5 sec + select(sec_sockfd_ + 1, &readfds, NULL, NULL, &timeout); + bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes + if (bytes_read > 0) { + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, + (char*)&flag_, sizeof(int)); + robot_state_->unpack(buf, bytes_read); + } else { + connected_ = false; + robot_state_->setDisconnected(); + close(sec_sockfd_); + } + } + if (keepalive_) { + //reconnect + print_warning("Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."); + sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (sec_sockfd_ < 0) { + print_fatal("ERROR opening secondary socket"); + } + flag_ = 1; + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, + sizeof(int)); + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, + sizeof(int)); + setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, + sizeof(int)); + fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); + while (keepalive_ && !connected_) { + std::this_thread::sleep_for(std::chrono::seconds(10)); + fd_set writefds; - connect(sec_sockfd_, (struct sockaddr *) &sec_serv_addr_, - sizeof(sec_serv_addr_)); - FD_ZERO(&writefds); - FD_SET(sec_sockfd_, &writefds); - select(sec_sockfd_ + 1, NULL, &writefds, NULL, NULL); - unsigned int flag_len; - getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, - &flag_len); - if (flag_ < 0) { - print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds..."); - } else { - connected_ = true; - print_info("Secondary port: Reconnected"); - } - } - } - } + connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_, + sizeof(sec_serv_addr_)); + FD_ZERO(&writefds); + FD_SET(sec_sockfd_, &writefds); + select(sec_sockfd_ + 1, NULL, &writefds, NULL, NULL); + unsigned int flag_len; + getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, + &flag_len); + if (flag_ < 0) { + print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds..."); + } else { + connected_ = true; + print_info("Secondary port: Reconnected"); + } + } + } + } - //wait for some traffic so the UR socket doesn't die in version 3.1. - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - close(sec_sockfd_); + //wait for some traffic so the UR socket doesn't die in version 3.1. + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + close(sec_sockfd_); } - diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index 2208115..08a535a 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -19,364 +19,396 @@ #include "ur_modern_driver/ur_driver.h" UrDriver::UrDriver(std::condition_variable& rt_msg_cond, - std::condition_variable& msg_cond, std::string host, - unsigned int reverse_port, double servoj_time, - unsigned int safety_count_max, double max_time_step, double min_payload, - double max_payload, double servoj_lookahead_time, double servoj_gain) : - REVERSE_PORT_(reverse_port), maximum_time_step_(max_time_step), minimum_payload_( - min_payload), maximum_payload_(max_payload), servoj_time_( - servoj_time), servoj_lookahead_time_(servoj_lookahead_time), servoj_gain_(servoj_gain) { - char buffer[256]; - struct sockaddr_in serv_addr; - int n, flag; + std::condition_variable& msg_cond, std::string host, + unsigned int reverse_port, double servoj_time, + unsigned int safety_count_max, double max_time_step, double min_payload, + double max_payload, double servoj_lookahead_time, double servoj_gain) + : REVERSE_PORT_(reverse_port) + , maximum_time_step_(max_time_step) + , minimum_payload_( + min_payload) + , maximum_payload_(max_payload) + , servoj_time_( + servoj_time) + , servoj_lookahead_time_(servoj_lookahead_time) + , servoj_gain_(servoj_gain) +{ + char buffer[256]; + struct sockaddr_in serv_addr; + int n, flag; - firmware_version_ = 0; - reverse_connected_ = false; - executing_traj_ = false; - rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host, - safety_count_max); - new_sockfd_ = -1; - sec_interface_ = new UrCommunication(msg_cond, host); + firmware_version_ = 0; + reverse_connected_ = false; + executing_traj_ = false; + rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host, + safety_count_max); + new_sockfd_ = -1; + sec_interface_ = new UrCommunication(msg_cond, host); - incoming_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (incoming_sockfd_ < 0) { - print_fatal("ERROR opening socket for reverse communication"); - } - bzero((char *) &serv_addr, sizeof(serv_addr)); + incoming_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (incoming_sockfd_ < 0) { + print_fatal("ERROR opening socket for reverse communication"); + } + bzero((char*)&serv_addr, sizeof(serv_addr)); - serv_addr.sin_family = AF_INET; - serv_addr.sin_addr.s_addr = INADDR_ANY; - serv_addr.sin_port = htons(REVERSE_PORT_); - flag = 1; - setsockopt(incoming_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag, - sizeof(int)); - setsockopt(incoming_sockfd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int)); - if (bind(incoming_sockfd_, (struct sockaddr *) &serv_addr, - sizeof(serv_addr)) < 0) { - print_fatal("ERROR on binding socket for reverse communication"); - } - listen(incoming_sockfd_, 5); + serv_addr.sin_family = AF_INET; + serv_addr.sin_addr.s_addr = INADDR_ANY; + serv_addr.sin_port = htons(REVERSE_PORT_); + flag = 1; + setsockopt(incoming_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag, + sizeof(int)); + setsockopt(incoming_sockfd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int)); + if (bind(incoming_sockfd_, (struct sockaddr*)&serv_addr, + sizeof(serv_addr)) + < 0) { + print_fatal("ERROR on binding socket for reverse communication"); + } + listen(incoming_sockfd_, 5); } std::vector UrDriver::interp_cubic(double t, double T, - std::vector p0_pos, std::vector p1_pos, - std::vector p0_vel, std::vector p1_vel) { - /*Returns positions of the joints at time 't' */ - std::vector positions; - for (unsigned int i = 0; i < p0_pos.size(); i++) { - double a = p0_pos[i]; - double b = p0_vel[i]; - double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i] - - T * p1_vel[i]) / pow(T, 2); - double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i] - + T * p1_vel[i]) / pow(T, 3); - positions.push_back(a + b * t + c * pow(t, 2) + d * pow(t, 3)); - } - return positions; + std::vector p0_pos, std::vector p1_pos, + std::vector p0_vel, std::vector p1_vel) +{ + /*Returns positions of the joints at time 't' */ + std::vector positions; + for (unsigned int i = 0; i < p0_pos.size(); i++) { + double a = p0_pos[i]; + double b = p0_vel[i]; + double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i] + - T * p1_vel[i]) + / pow(T, 2); + double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i] + + T * p1_vel[i]) + / pow(T, 3); + positions.push_back(a + b * t + c * pow(t, 2) + d * pow(t, 3)); + } + return positions; } bool UrDriver::doTraj(std::vector inp_timestamps, - std::vector > inp_positions, - std::vector > inp_velocities) { - std::chrono::high_resolution_clock::time_point t0, t; - std::vector positions; - unsigned int j; + std::vector > inp_positions, + std::vector > inp_velocities) +{ + std::chrono::high_resolution_clock::time_point t0, t; + std::vector positions; + unsigned int j; - if (!UrDriver::uploadProg()) { - return false; - } - executing_traj_ = true; - t0 = std::chrono::high_resolution_clock::now(); - t = t0; - j = 0; - while ((inp_timestamps[inp_timestamps.size() - 1] - >= std::chrono::duration_cast>(t - t0).count()) - and executing_traj_) { - while (inp_timestamps[j] - <= std::chrono::duration_cast>( - t - t0).count() && j < inp_timestamps.size() - 1) { - j += 1; - } - positions = UrDriver::interp_cubic( - std::chrono::duration_cast>( - t - t0).count() - inp_timestamps[j - 1], - inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1], - inp_positions[j], inp_velocities[j - 1], inp_velocities[j]); - UrDriver::servoj(positions); + if (!UrDriver::uploadProg()) { + return false; + } + executing_traj_ = true; + t0 = std::chrono::high_resolution_clock::now(); + t = t0; + j = 0; + while ((inp_timestamps[inp_timestamps.size() - 1] + >= std::chrono::duration_cast >(t - t0).count()) + and executing_traj_) { + while (inp_timestamps[j] + <= std::chrono::duration_cast >( + t - t0) + .count() + && j < inp_timestamps.size() - 1) { + j += 1; + } + positions = UrDriver::interp_cubic( + std::chrono::duration_cast >( + t - t0) + .count() + - inp_timestamps[j - 1], + inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1], + inp_positions[j], inp_velocities[j - 1], inp_velocities[j]); + UrDriver::servoj(positions); - // oversample with 4 * sample_time - std::this_thread::sleep_for( - std::chrono::milliseconds((int) ((servoj_time_ * 1000) / 4.))); - t = std::chrono::high_resolution_clock::now(); - } - executing_traj_ = false; - //Signal robot to stop driverProg() - UrDriver::closeServo(positions); - return true; + // oversample with 4 * sample_time + std::this_thread::sleep_for( + std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.))); + t = std::chrono::high_resolution_clock::now(); + } + executing_traj_ = false; + //Signal robot to stop driverProg() + UrDriver::closeServo(positions); + return true; } -void UrDriver::servoj(std::vector positions, int keepalive) { - if (!reverse_connected_) { - print_error( - "UrDriver::servoj called without a reverse connection present. Keepalive: " - + std::to_string(keepalive)); - return; - } - unsigned int bytes_written; - int tmp; - unsigned char buf[28]; - for (int i = 0; i < 6; i++) { - tmp = htonl((int) (positions[i] * MULT_JOINTSTATE_)); - buf[i * 4] = tmp & 0xff; - buf[i * 4 + 1] = (tmp >> 8) & 0xff; - buf[i * 4 + 2] = (tmp >> 16) & 0xff; - buf[i * 4 + 3] = (tmp >> 24) & 0xff; - } - tmp = htonl((int) keepalive); - buf[6 * 4] = tmp & 0xff; - buf[6 * 4 + 1] = (tmp >> 8) & 0xff; - buf[6 * 4 + 2] = (tmp >> 16) & 0xff; - buf[6 * 4 + 3] = (tmp >> 24) & 0xff; - bytes_written = write(new_sockfd_, buf, 28); +void UrDriver::servoj(std::vector positions, int keepalive) +{ + if (!reverse_connected_) { + print_error( + "UrDriver::servoj called without a reverse connection present. Keepalive: " + + std::to_string(keepalive)); + return; + } + unsigned int bytes_written; + int tmp; + unsigned char buf[28]; + for (int i = 0; i < 6; i++) { + tmp = htonl((int)(positions[i] * MULT_JOINTSTATE_)); + buf[i * 4] = tmp & 0xff; + buf[i * 4 + 1] = (tmp >> 8) & 0xff; + buf[i * 4 + 2] = (tmp >> 16) & 0xff; + buf[i * 4 + 3] = (tmp >> 24) & 0xff; + } + tmp = htonl((int)keepalive); + buf[6 * 4] = tmp & 0xff; + buf[6 * 4 + 1] = (tmp >> 8) & 0xff; + buf[6 * 4 + 2] = (tmp >> 16) & 0xff; + buf[6 * 4 + 3] = (tmp >> 24) & 0xff; + bytes_written = write(new_sockfd_, buf, 28); } -void UrDriver::stopTraj() { - executing_traj_ = false; - rt_interface_->addCommandToQueue("stopj(10)\n"); +void UrDriver::stopTraj() +{ + executing_traj_ = false; + rt_interface_->addCommandToQueue("stopj(10)\n"); } -bool UrDriver::uploadProg() { - std::string cmd_str; - char buf[128]; - cmd_str = "def driverProg():\n"; +bool UrDriver::uploadProg() +{ + std::string cmd_str; + char buf[128]; + cmd_str = "def driverProg():\n"; - sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_); - cmd_str += buf; + sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_); + cmd_str += buf; - cmd_str += "\tSERVO_IDLE = 0\n"; - cmd_str += "\tSERVO_RUNNING = 1\n"; - cmd_str += "\tcmd_servo_state = SERVO_IDLE\n"; - cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n"; - cmd_str += "\tdef set_servo_setpoint(q):\n"; - cmd_str += "\t\tenter_critical\n"; - cmd_str += "\t\tcmd_servo_state = SERVO_RUNNING\n"; - cmd_str += "\t\tcmd_servo_q = q\n"; - cmd_str += "\t\texit_critical\n"; - cmd_str += "\tend\n"; - cmd_str += "\tthread servoThread():\n"; - cmd_str += "\t\tstate = SERVO_IDLE\n"; - cmd_str += "\t\twhile True:\n"; - cmd_str += "\t\t\tenter_critical\n"; - cmd_str += "\t\t\tq = cmd_servo_q\n"; - cmd_str += "\t\t\tdo_brake = False\n"; - cmd_str += "\t\t\tif (state == SERVO_RUNNING) and "; - cmd_str += "(cmd_servo_state == SERVO_IDLE):\n"; - cmd_str += "\t\t\t\tdo_brake = True\n"; - cmd_str += "\t\t\tend\n"; - cmd_str += "\t\t\tstate = cmd_servo_state\n"; - cmd_str += "\t\t\tcmd_servo_state = SERVO_IDLE\n"; - cmd_str += "\t\t\texit_critical\n"; - cmd_str += "\t\t\tif do_brake:\n"; - cmd_str += "\t\t\t\tstopj(1.0)\n"; - cmd_str += "\t\t\t\tsync()\n"; - cmd_str += "\t\t\telif state == SERVO_RUNNING:\n"; + cmd_str += "\tSERVO_IDLE = 0\n"; + cmd_str += "\tSERVO_RUNNING = 1\n"; + cmd_str += "\tcmd_servo_state = SERVO_IDLE\n"; + cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n"; + cmd_str += "\tdef set_servo_setpoint(q):\n"; + cmd_str += "\t\tenter_critical\n"; + cmd_str += "\t\tcmd_servo_state = SERVO_RUNNING\n"; + cmd_str += "\t\tcmd_servo_q = q\n"; + cmd_str += "\t\texit_critical\n"; + cmd_str += "\tend\n"; + cmd_str += "\tthread servoThread():\n"; + cmd_str += "\t\tstate = SERVO_IDLE\n"; + cmd_str += "\t\twhile True:\n"; + cmd_str += "\t\t\tenter_critical\n"; + cmd_str += "\t\t\tq = cmd_servo_q\n"; + cmd_str += "\t\t\tdo_brake = False\n"; + cmd_str += "\t\t\tif (state == SERVO_RUNNING) and "; + cmd_str += "(cmd_servo_state == SERVO_IDLE):\n"; + cmd_str += "\t\t\t\tdo_brake = True\n"; + cmd_str += "\t\t\tend\n"; + cmd_str += "\t\t\tstate = cmd_servo_state\n"; + cmd_str += "\t\t\tcmd_servo_state = SERVO_IDLE\n"; + cmd_str += "\t\t\texit_critical\n"; + cmd_str += "\t\t\tif do_brake:\n"; + cmd_str += "\t\t\t\tstopj(1.0)\n"; + cmd_str += "\t\t\t\tsync()\n"; + cmd_str += "\t\t\telif state == SERVO_RUNNING:\n"; - if (sec_interface_->robot_state_->getVersion() >= 3.1) - sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=%.4f, gain=%.0f)\n", - servoj_time_, servoj_lookahead_time_, servoj_gain_); - else - sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_); - cmd_str += buf; + if (sec_interface_->robot_state_->getVersion() >= 3.1) + sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=%.4f, gain=%.0f)\n", + servoj_time_, servoj_lookahead_time_, servoj_gain_); + else + sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_); + cmd_str += buf; - cmd_str += "\t\t\telse:\n"; - cmd_str += "\t\t\t\tsync()\n"; - cmd_str += "\t\t\tend\n"; - cmd_str += "\t\tend\n"; - cmd_str += "\tend\n"; + cmd_str += "\t\t\telse:\n"; + cmd_str += "\t\t\t\tsync()\n"; + cmd_str += "\t\t\tend\n"; + cmd_str += "\t\tend\n"; + cmd_str += "\tend\n"; - sprintf(buf, "\tsocket_open(\"%s\", %i)\n", ip_addr_.c_str(), - REVERSE_PORT_); - cmd_str += buf; + sprintf(buf, "\tsocket_open(\"%s\", %i)\n", ip_addr_.c_str(), + REVERSE_PORT_); + cmd_str += buf; - cmd_str += "\tthread_servo = run servoThread()\n"; - cmd_str += "\tkeepalive = 1\n"; - cmd_str += "\twhile keepalive > 0:\n"; - cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1)\n"; - cmd_str += "\t\tif params_mult[0] > 0:\n"; - cmd_str += "\t\t\tq = [params_mult[1] / MULT_jointstate, "; - cmd_str += "params_mult[2] / MULT_jointstate, "; - cmd_str += "params_mult[3] / MULT_jointstate, "; - cmd_str += "params_mult[4] / MULT_jointstate, "; - cmd_str += "params_mult[5] / MULT_jointstate, "; - cmd_str += "params_mult[6] / MULT_jointstate]\n"; - cmd_str += "\t\t\tkeepalive = params_mult[7]\n"; - cmd_str += "\t\t\tset_servo_setpoint(q)\n"; - cmd_str += "\t\tend\n"; - cmd_str += "\tend\n"; - cmd_str += "\tsleep(.1)\n"; - cmd_str += "\tsocket_close()\n"; - cmd_str += "\tkill thread_servo\n"; - cmd_str += "end\n"; + cmd_str += "\tthread_servo = run servoThread()\n"; + cmd_str += "\tkeepalive = 1\n"; + cmd_str += "\twhile keepalive > 0:\n"; + cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1)\n"; + cmd_str += "\t\tif params_mult[0] > 0:\n"; + cmd_str += "\t\t\tq = [params_mult[1] / MULT_jointstate, "; + cmd_str += "params_mult[2] / MULT_jointstate, "; + cmd_str += "params_mult[3] / MULT_jointstate, "; + cmd_str += "params_mult[4] / MULT_jointstate, "; + cmd_str += "params_mult[5] / MULT_jointstate, "; + cmd_str += "params_mult[6] / MULT_jointstate]\n"; + cmd_str += "\t\t\tkeepalive = params_mult[7]\n"; + cmd_str += "\t\t\tset_servo_setpoint(q)\n"; + cmd_str += "\t\tend\n"; + cmd_str += "\tend\n"; + cmd_str += "\tsleep(.1)\n"; + cmd_str += "\tsocket_close()\n"; + cmd_str += "\tkill thread_servo\n"; + cmd_str += "end\n"; - rt_interface_->addCommandToQueue(cmd_str); - return UrDriver::openServo(); + rt_interface_->addCommandToQueue(cmd_str); + return UrDriver::openServo(); } -bool UrDriver::openServo() { - struct sockaddr_in cli_addr; - socklen_t clilen; - clilen = sizeof(cli_addr); - new_sockfd_ = accept(incoming_sockfd_, (struct sockaddr *) &cli_addr, - &clilen); - if (new_sockfd_ < 0) { - print_fatal("ERROR on accepting reverse communication"); - return false; - } - reverse_connected_ = true; - return true; +bool UrDriver::openServo() +{ + struct sockaddr_in cli_addr; + socklen_t clilen; + clilen = sizeof(cli_addr); + new_sockfd_ = accept(incoming_sockfd_, (struct sockaddr*)&cli_addr, + &clilen); + if (new_sockfd_ < 0) { + print_fatal("ERROR on accepting reverse communication"); + return false; + } + reverse_connected_ = true; + return true; } -void UrDriver::closeServo(std::vector positions) { - if (positions.size() != 6) - UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0); - else - UrDriver::servoj(positions, 0); +void UrDriver::closeServo(std::vector positions) +{ + if (positions.size() != 6) + UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0); + else + UrDriver::servoj(positions, 0); - reverse_connected_ = false; - close(new_sockfd_); + reverse_connected_ = false; + close(new_sockfd_); } -bool UrDriver::start() { - if (!sec_interface_->start()) - return false; - firmware_version_ = sec_interface_->robot_state_->getVersion(); - rt_interface_->robot_state_->setVersion(firmware_version_); - if (!rt_interface_->start()) - return false; - ip_addr_ = rt_interface_->getLocalIp(); - print_debug( - "Listening on " + ip_addr_ + ":" + std::to_string(REVERSE_PORT_) - + "\n"); - return true; - +bool UrDriver::start() +{ + if (!sec_interface_->start()) + return false; + firmware_version_ = sec_interface_->robot_state_->getVersion(); + rt_interface_->robot_state_->setVersion(firmware_version_); + if (!rt_interface_->start()) + return false; + ip_addr_ = rt_interface_->getLocalIp(); + print_debug( + "Listening on " + ip_addr_ + ":" + std::to_string(REVERSE_PORT_) + + "\n"); + return true; } -void UrDriver::halt() { - if (executing_traj_) { - UrDriver::stopTraj(); - } - sec_interface_->halt(); - rt_interface_->halt(); - close(incoming_sockfd_); +void UrDriver::halt() +{ + if (executing_traj_) { + UrDriver::stopTraj(); + } + sec_interface_->halt(); + rt_interface_->halt(); + close(incoming_sockfd_); } void UrDriver::setSpeed(double q0, double q1, double q2, double q3, double q4, - double q5, double acc) { - rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc); + double q5, double acc) +{ + rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc); } -std::vector UrDriver::getJointNames() { - return joint_names_; +std::vector UrDriver::getJointNames() +{ + return joint_names_; } -void UrDriver::setJointNames(std::vector jn) { - joint_names_ = jn; +void UrDriver::setJointNames(std::vector jn) +{ + joint_names_ = jn; } -void UrDriver::setToolVoltage(unsigned int v) { - char buf[256]; - sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v); - rt_interface_->addCommandToQueue(buf); - print_debug(buf); +void UrDriver::setToolVoltage(unsigned int v) +{ + char buf[256]; + sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v); + rt_interface_->addCommandToQueue(buf); + print_debug(buf); } -void UrDriver::setFlag(unsigned int n, bool b) { - char buf[256]; - sprintf(buf, "sec setOut():\n\tset_flag(%d, %s)\nend\n", n, - b ? "True" : "False"); - rt_interface_->addCommandToQueue(buf); - print_debug(buf); +void UrDriver::setFlag(unsigned int n, bool b) +{ + char buf[256]; + sprintf(buf, "sec setOut():\n\tset_flag(%d, %s)\nend\n", n, + b ? "True" : "False"); + rt_interface_->addCommandToQueue(buf); + print_debug(buf); } -void UrDriver::setDigitalOut(unsigned int n, bool b) { - char buf[256]; - if (firmware_version_ < 2) { - sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n, - b ? "True" : "False"); +void UrDriver::setDigitalOut(unsigned int n, bool b) +{ + char buf[256]; + if (firmware_version_ < 2) { + sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n, + b ? "True" : "False"); } else if (n > 15) { sprintf(buf, - "sec setOut():\n\tset_tool_digital_out(%d, %s)\nend\n", - n - 16, b ? "True" : "False"); - } else if (n > 7) { + "sec setOut():\n\tset_tool_digital_out(%d, %s)\nend\n", + n - 16, b ? "True" : "False"); + } else if (n > 7) { sprintf(buf, "sec setOut():\n\tset_configurable_digital_out(%d, %s)\nend\n", - n - 8, b ? "True" : "False"); - - } else { - sprintf(buf, "sec setOut():\n\tset_standard_digital_out(%d, %s)\nend\n", - n, b ? "True" : "False"); - - } - rt_interface_->addCommandToQueue(buf); - print_debug(buf); + n - 8, b ? "True" : "False"); + } else { + sprintf(buf, "sec setOut():\n\tset_standard_digital_out(%d, %s)\nend\n", + n, b ? "True" : "False"); + } + rt_interface_->addCommandToQueue(buf); + print_debug(buf); } -void UrDriver::setAnalogOut(unsigned int n, double f) { - char buf[256]; - if (firmware_version_ < 2) { - sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f); - } else { - sprintf(buf, "sec setOut():\n\tset_standard_analog_out(%d, %1.4f)\nend\n", n, f); - } +void UrDriver::setAnalogOut(unsigned int n, double f) +{ + char buf[256]; + if (firmware_version_ < 2) { + sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f); + } else { + sprintf(buf, "sec setOut():\n\tset_standard_analog_out(%d, %1.4f)\nend\n", n, f); + } - rt_interface_->addCommandToQueue(buf); - print_debug(buf); + rt_interface_->addCommandToQueue(buf); + print_debug(buf); } -bool UrDriver::setPayload(double m) { - if ((m < maximum_payload_) && (m > minimum_payload_)) { - char buf[256]; - sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m); - rt_interface_->addCommandToQueue(buf); - print_debug(buf); - return true; - } else - return false; +bool UrDriver::setPayload(double m) +{ + if ((m < maximum_payload_) && (m > minimum_payload_)) { + char buf[256]; + sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m); + rt_interface_->addCommandToQueue(buf); + print_debug(buf); + return true; + } else + return false; } -void UrDriver::setMinPayload(double m) { - if (m > 0) { - minimum_payload_ = m; - } else { - minimum_payload_ = 0; - } - +void UrDriver::setMinPayload(double m) +{ + if (m > 0) { + minimum_payload_ = m; + } else { + minimum_payload_ = 0; + } } -void UrDriver::setMaxPayload(double m) { - maximum_payload_ = m; +void UrDriver::setMaxPayload(double m) +{ + maximum_payload_ = m; } -void UrDriver::setServojTime(double t) { - if (t > 0.008) { - servoj_time_ = t; - } else { - servoj_time_ = 0.008; - } +void UrDriver::setServojTime(double t) +{ + if (t > 0.008) { + servoj_time_ = t; + } else { + servoj_time_ = 0.008; + } } -void UrDriver::setServojLookahead(double t){ - if (t > 0.03) { - if (t < 0.2) { - servoj_lookahead_time_ = t; - } else { - servoj_lookahead_time_ = 0.2; - } - } else { - servoj_lookahead_time_ = 0.03; - } +void UrDriver::setServojLookahead(double t) +{ + if (t > 0.03) { + if (t < 0.2) { + servoj_lookahead_time_ = t; + } else { + servoj_lookahead_time_ = 0.2; + } + } else { + servoj_lookahead_time_ = 0.03; + } } -void UrDriver::setServojGain(double g){ - if (g > 100) { - if (g < 2000) { - servoj_gain_ = g; - } else { - servoj_gain_ = 2000; - } - } else { - servoj_gain_ = 100; - } +void UrDriver::setServojGain(double g) +{ + if (g > 100) { + if (g < 2000) { + servoj_gain_ = g; + } else { + servoj_gain_ = 2000; + } + } else { + servoj_gain_ = 100; + } } diff --git a/src/ur_hardware_interface.cpp b/src/ur_hardware_interface.cpp index ea66395..152452c 100644 --- a/src/ur_hardware_interface.cpp +++ b/src/ur_hardware_interface.cpp @@ -59,225 +59,226 @@ namespace ros_control_ur { -UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) : - nh_(nh), robot_(robot) { - // Initialize shared memory and interfaces here - init(); // this implementation loads from rosparam +UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) + : nh_(nh) + , robot_(robot) +{ + // Initialize shared memory and interfaces here + init(); // this implementation loads from rosparam - max_vel_change_ = 0.12; // equivalent of an acceleration of 15 rad/sec^2 + max_vel_change_ = 0.12; // equivalent of an acceleration of 15 rad/sec^2 - ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface."); + ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface."); } -void UrHardwareInterface::init() { - ROS_INFO_STREAM_NAMED("ur_hardware_interface", - "Reading rosparams from namespace: " << nh_.getNamespace()); +void UrHardwareInterface::init() +{ + ROS_INFO_STREAM_NAMED("ur_hardware_interface", + "Reading rosparams from namespace: " << nh_.getNamespace()); - // Get joint names - nh_.getParam("hardware_interface/joints", joint_names_); - if (joint_names_.size() == 0) { - ROS_FATAL_STREAM_NAMED("ur_hardware_interface", - "No joints found on parameter server for controller, did you load the proper yaml file?" << " Namespace: " << nh_.getNamespace()); - exit(-1); - } - num_joints_ = joint_names_.size(); + // Get joint names + nh_.getParam("hardware_interface/joints", joint_names_); + if (joint_names_.size() == 0) { + ROS_FATAL_STREAM_NAMED("ur_hardware_interface", + "No joints found on parameter server for controller, did you load the proper yaml file?" + << " Namespace: " << nh_.getNamespace()); + exit(-1); + } + num_joints_ = joint_names_.size(); - // Resize vectors - joint_position_.resize(num_joints_); - joint_velocity_.resize(num_joints_); - joint_effort_.resize(num_joints_); - joint_position_command_.resize(num_joints_); - joint_velocity_command_.resize(num_joints_); - prev_joint_velocity_command_.resize(num_joints_); + // Resize vectors + joint_position_.resize(num_joints_); + joint_velocity_.resize(num_joints_); + joint_effort_.resize(num_joints_); + joint_position_command_.resize(num_joints_); + joint_velocity_command_.resize(num_joints_); + prev_joint_velocity_command_.resize(num_joints_); - // Initialize controller - for (std::size_t i = 0; i < num_joints_; ++i) { - ROS_DEBUG_STREAM_NAMED("ur_hardware_interface", - "Loading joint name: " << joint_names_[i]); + // Initialize controller + for (std::size_t i = 0; i < num_joints_; ++i) { + ROS_DEBUG_STREAM_NAMED("ur_hardware_interface", + "Loading joint name: " << joint_names_[i]); - // Create joint state interface - joint_state_interface_.registerHandle( - hardware_interface::JointStateHandle(joint_names_[i], - &joint_position_[i], &joint_velocity_[i], - &joint_effort_[i])); + // Create joint state interface + joint_state_interface_.registerHandle( + hardware_interface::JointStateHandle(joint_names_[i], + &joint_position_[i], &joint_velocity_[i], + &joint_effort_[i])); - // Create position joint interface - position_joint_interface_.registerHandle( - hardware_interface::JointHandle( - joint_state_interface_.getHandle(joint_names_[i]), - &joint_position_command_[i])); + // Create position joint interface + position_joint_interface_.registerHandle( + hardware_interface::JointHandle( + joint_state_interface_.getHandle(joint_names_[i]), + &joint_position_command_[i])); - // Create velocity joint interface - velocity_joint_interface_.registerHandle( - hardware_interface::JointHandle( - joint_state_interface_.getHandle(joint_names_[i]), - &joint_velocity_command_[i])); - prev_joint_velocity_command_[i] = 0.; - } + // Create velocity joint interface + velocity_joint_interface_.registerHandle( + hardware_interface::JointHandle( + joint_state_interface_.getHandle(joint_names_[i]), + &joint_velocity_command_[i])); + prev_joint_velocity_command_[i] = 0.; + } - // Create force torque interface - force_torque_interface_.registerHandle( - hardware_interface::ForceTorqueSensorHandle("wrench", "", - robot_force_, robot_torque_)); + // Create force torque interface + force_torque_interface_.registerHandle( + hardware_interface::ForceTorqueSensorHandle("wrench", "", + robot_force_, robot_torque_)); - registerInterface(&joint_state_interface_); // From RobotHW base class. - registerInterface(&position_joint_interface_); // From RobotHW base class. - registerInterface(&velocity_joint_interface_); // From RobotHW base class. - registerInterface(&force_torque_interface_); // From RobotHW base class. - velocity_interface_running_ = false; - position_interface_running_ = false; + registerInterface(&joint_state_interface_); // From RobotHW base class. + registerInterface(&position_joint_interface_); // From RobotHW base class. + registerInterface(&velocity_joint_interface_); // From RobotHW base class. + registerInterface(&force_torque_interface_); // From RobotHW base class. + velocity_interface_running_ = false; + position_interface_running_ = false; } -void UrHardwareInterface::read() { - std::vector pos, vel, current, tcp; - pos = robot_->rt_interface_->robot_state_->getQActual(); - vel = robot_->rt_interface_->robot_state_->getQdActual(); - current = robot_->rt_interface_->robot_state_->getIActual(); - tcp = robot_->rt_interface_->robot_state_->getTcpForce(); - for (std::size_t i = 0; i < num_joints_; ++i) { - joint_position_[i] = pos[i]; - joint_velocity_[i] = vel[i]; - joint_effort_[i] = current[i]; - } - for (std::size_t i = 0; i < 3; ++i) { - robot_force_[i] = tcp[i]; - robot_torque_[i] = tcp[i + 3]; - } - +void UrHardwareInterface::read() +{ + std::vector pos, vel, current, tcp; + pos = robot_->rt_interface_->robot_state_->getQActual(); + vel = robot_->rt_interface_->robot_state_->getQdActual(); + current = robot_->rt_interface_->robot_state_->getIActual(); + tcp = robot_->rt_interface_->robot_state_->getTcpForce(); + for (std::size_t i = 0; i < num_joints_; ++i) { + joint_position_[i] = pos[i]; + joint_velocity_[i] = vel[i]; + joint_effort_[i] = current[i]; + } + for (std::size_t i = 0; i < 3; ++i) { + robot_force_[i] = tcp[i]; + robot_torque_[i] = tcp[i + 3]; + } } -void UrHardwareInterface::setMaxVelChange(double inp) { - max_vel_change_ = inp; +void UrHardwareInterface::setMaxVelChange(double inp) +{ + max_vel_change_ = inp; } -void UrHardwareInterface::write() { - if (velocity_interface_running_) { - std::vector cmd; - //do some rate limiting - cmd.resize(joint_velocity_command_.size()); - for (unsigned int i = 0; i < joint_velocity_command_.size(); i++) { - cmd[i] = joint_velocity_command_[i]; - if (cmd[i] > prev_joint_velocity_command_[i] + max_vel_change_) { - cmd[i] = prev_joint_velocity_command_[i] + max_vel_change_; - } else if (cmd[i] - < prev_joint_velocity_command_[i] - max_vel_change_) { - cmd[i] = prev_joint_velocity_command_[i] - max_vel_change_; - } - prev_joint_velocity_command_[i] = cmd[i]; - } - robot_->setSpeed(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], max_vel_change_*125); - } else if (position_interface_running_) { - robot_->servoj(joint_position_command_); - } +void UrHardwareInterface::write() +{ + if (velocity_interface_running_) { + std::vector cmd; + //do some rate limiting + cmd.resize(joint_velocity_command_.size()); + for (unsigned int i = 0; i < joint_velocity_command_.size(); i++) { + cmd[i] = joint_velocity_command_[i]; + if (cmd[i] > prev_joint_velocity_command_[i] + max_vel_change_) { + cmd[i] = prev_joint_velocity_command_[i] + max_vel_change_; + } else if (cmd[i] + < prev_joint_velocity_command_[i] - max_vel_change_) { + cmd[i] = prev_joint_velocity_command_[i] - max_vel_change_; + } + prev_joint_velocity_command_[i] = cmd[i]; + } + robot_->setSpeed(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], max_vel_change_ * 125); + } else if (position_interface_running_) { + robot_->servoj(joint_position_command_); + } } bool UrHardwareInterface::canSwitch( - const std::list &start_list, - const std::list &stop_list) const { - for (std::list::const_iterator controller_it = - start_list.begin(); controller_it != start_list.end(); - ++controller_it) { - if (controller_it->hardware_interface - == "hardware_interface::VelocityJointInterface") { - if (velocity_interface_running_) { - ROS_ERROR( - "%s: An interface of that type (%s) is already running", - controller_it->name.c_str(), - controller_it->hardware_interface.c_str()); - return false; - } - if (position_interface_running_) { - bool error = true; - for (std::list::const_iterator stop_controller_it = - stop_list.begin(); - stop_controller_it != stop_list.end(); - ++stop_controller_it) { - if (stop_controller_it->hardware_interface - == "hardware_interface::PositionJointInterface") { - error = false; - break; - } - } - if (error) { - ROS_ERROR( - "%s (type %s) can not be run simultaneously with a PositionJointInterface", - controller_it->name.c_str(), - controller_it->hardware_interface.c_str()); - return false; - } - } - } else if (controller_it->hardware_interface - == "hardware_interface::PositionJointInterface") { - if (position_interface_running_) { - ROS_ERROR( - "%s: An interface of that type (%s) is already running", - controller_it->name.c_str(), - controller_it->hardware_interface.c_str()); - return false; - } - if (velocity_interface_running_) { - bool error = true; - for (std::list::const_iterator stop_controller_it = - stop_list.begin(); - stop_controller_it != stop_list.end(); - ++stop_controller_it) { - if (stop_controller_it->hardware_interface - == "hardware_interface::VelocityJointInterface") { - error = false; - break; - } - } - if (error) { - ROS_ERROR( - "%s (type %s) can not be run simultaneously with a VelocityJointInterface", - controller_it->name.c_str(), - controller_it->hardware_interface.c_str()); - return false; - } - } - } - } + const std::list& start_list, + const std::list& stop_list) const +{ + for (std::list::const_iterator controller_it = start_list.begin(); controller_it != start_list.end(); + ++controller_it) { + if (controller_it->hardware_interface + == "hardware_interface::VelocityJointInterface") { + if (velocity_interface_running_) { + ROS_ERROR( + "%s: An interface of that type (%s) is already running", + controller_it->name.c_str(), + controller_it->hardware_interface.c_str()); + return false; + } + if (position_interface_running_) { + bool error = true; + for (std::list::const_iterator stop_controller_it = stop_list.begin(); + stop_controller_it != stop_list.end(); + ++stop_controller_it) { + if (stop_controller_it->hardware_interface + == "hardware_interface::PositionJointInterface") { + error = false; + break; + } + } + if (error) { + ROS_ERROR( + "%s (type %s) can not be run simultaneously with a PositionJointInterface", + controller_it->name.c_str(), + controller_it->hardware_interface.c_str()); + return false; + } + } + } else if (controller_it->hardware_interface + == "hardware_interface::PositionJointInterface") { + if (position_interface_running_) { + ROS_ERROR( + "%s: An interface of that type (%s) is already running", + controller_it->name.c_str(), + controller_it->hardware_interface.c_str()); + return false; + } + if (velocity_interface_running_) { + bool error = true; + for (std::list::const_iterator stop_controller_it = stop_list.begin(); + stop_controller_it != stop_list.end(); + ++stop_controller_it) { + if (stop_controller_it->hardware_interface + == "hardware_interface::VelocityJointInterface") { + error = false; + break; + } + } + if (error) { + ROS_ERROR( + "%s (type %s) can not be run simultaneously with a VelocityJointInterface", + controller_it->name.c_str(), + controller_it->hardware_interface.c_str()); + return false; + } + } + } + } -// we can always stop a controller - return true; + // we can always stop a controller + return true; } void UrHardwareInterface::doSwitch( - const std::list& start_list, - const std::list& stop_list) { - for (std::list::const_iterator controller_it = - stop_list.begin(); controller_it != stop_list.end(); - ++controller_it) { - if (controller_it->hardware_interface - == "hardware_interface::VelocityJointInterface") { - velocity_interface_running_ = false; - ROS_DEBUG("Stopping velocity interface"); - } - if (controller_it->hardware_interface - == "hardware_interface::PositionJointInterface") { - position_interface_running_ = false; - std::vector tmp; - robot_->closeServo(tmp); - ROS_DEBUG("Stopping position interface"); - } - } - for (std::list::const_iterator controller_it = - start_list.begin(); controller_it != start_list.end(); - ++controller_it) { - if (controller_it->hardware_interface - == "hardware_interface::VelocityJointInterface") { - velocity_interface_running_ = true; - ROS_DEBUG("Starting velocity interface"); - } - if (controller_it->hardware_interface - == "hardware_interface::PositionJointInterface") { - position_interface_running_ = true; - robot_->uploadProg(); - ROS_DEBUG("Starting position interface"); - } - } - + const std::list& start_list, + const std::list& stop_list) +{ + for (std::list::const_iterator controller_it = stop_list.begin(); controller_it != stop_list.end(); + ++controller_it) { + if (controller_it->hardware_interface + == "hardware_interface::VelocityJointInterface") { + velocity_interface_running_ = false; + ROS_DEBUG("Stopping velocity interface"); + } + if (controller_it->hardware_interface + == "hardware_interface::PositionJointInterface") { + position_interface_running_ = false; + std::vector tmp; + robot_->closeServo(tmp); + ROS_DEBUG("Stopping position interface"); + } + } + for (std::list::const_iterator controller_it = start_list.begin(); controller_it != start_list.end(); + ++controller_it) { + if (controller_it->hardware_interface + == "hardware_interface::VelocityJointInterface") { + velocity_interface_running_ = true; + ROS_DEBUG("Starting velocity interface"); + } + if (controller_it->hardware_interface + == "hardware_interface::PositionJointInterface") { + position_interface_running_ = true; + robot_->uploadProg(); + ROS_DEBUG("Starting position interface"); + } + } } } // namespace - diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp index 90f0cfe..b45224b 100644 --- a/src/ur_realtime_communication.cpp +++ b/src/ur_realtime_communication.cpp @@ -19,175 +19,182 @@ #include "ur_modern_driver/ur_realtime_communication.h" UrRealtimeCommunication::UrRealtimeCommunication( - std::condition_variable& msg_cond, std::string host, - unsigned int safety_count_max) { - robot_state_ = new RobotStateRT(msg_cond); - bzero((char *) &serv_addr_, sizeof(serv_addr_)); - sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (sockfd_ < 0) { - print_fatal("ERROR opening socket"); - } - server_ = gethostbyname(host.c_str()); - if (server_ == NULL) { - print_fatal("ERROR, no such host"); - } - serv_addr_.sin_family = AF_INET; - bcopy((char *) server_->h_addr, (char *)&serv_addr_.sin_addr.s_addr, server_->h_length); - serv_addr_.sin_port = htons(30003); - flag_ = 1; - setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int)); - setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, sizeof(int)); - setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int)); - fcntl(sockfd_, F_SETFL, O_NONBLOCK); - connected_ = false; - keepalive_ = false; - safety_count_ = safety_count_max + 1; - safety_count_max_ = safety_count_max; + std::condition_variable& msg_cond, std::string host, + unsigned int safety_count_max) +{ + robot_state_ = new RobotStateRT(msg_cond); + bzero((char*)&serv_addr_, sizeof(serv_addr_)); + sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (sockfd_ < 0) { + print_fatal("ERROR opening socket"); + } + server_ = gethostbyname(host.c_str()); + if (server_ == NULL) { + print_fatal("ERROR, no such host"); + } + serv_addr_.sin_family = AF_INET; + bcopy((char*)server_->h_addr, (char*)&serv_addr_.sin_addr.s_addr, server_->h_length); + serv_addr_.sin_port = htons(30003); + flag_ = 1; + setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int)); + setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); + setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int)); + fcntl(sockfd_, F_SETFL, O_NONBLOCK); + connected_ = false; + keepalive_ = false; + safety_count_ = safety_count_max + 1; + safety_count_max_ = safety_count_max; } -bool UrRealtimeCommunication::start() { - fd_set writefds; - struct timeval timeout; +bool UrRealtimeCommunication::start() +{ + fd_set writefds; + struct timeval timeout; - keepalive_ = true; - print_debug("Realtime port: Connecting..."); + keepalive_ = true; + print_debug("Realtime port: Connecting..."); - connect(sockfd_, (struct sockaddr *) &serv_addr_, sizeof(serv_addr_)); - FD_ZERO(&writefds); - FD_SET(sockfd_, &writefds); - timeout.tv_sec = 10; - timeout.tv_usec = 0; - select(sockfd_ + 1, NULL, &writefds, NULL, &timeout); - unsigned int flag_len; - getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); - if (flag_ < 0) { - print_fatal("Error connecting to RT port 30003"); - return false; - } - sockaddr_in name; - socklen_t namelen = sizeof(name); - int err = getsockname(sockfd_, (sockaddr*) &name, &namelen); - if (err < 0) { - print_fatal("Could not get local IP"); - close(sockfd_); - return false; - } - char str[18]; - inet_ntop(AF_INET, &name.sin_addr, str, 18); - local_ip_ = str; - comThread_ = std::thread(&UrRealtimeCommunication::run, this); - return true; + connect(sockfd_, (struct sockaddr*)&serv_addr_, sizeof(serv_addr_)); + FD_ZERO(&writefds); + FD_SET(sockfd_, &writefds); + timeout.tv_sec = 10; + timeout.tv_usec = 0; + select(sockfd_ + 1, NULL, &writefds, NULL, &timeout); + unsigned int flag_len; + getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); + if (flag_ < 0) { + print_fatal("Error connecting to RT port 30003"); + return false; + } + sockaddr_in name; + socklen_t namelen = sizeof(name); + int err = getsockname(sockfd_, (sockaddr*)&name, &namelen); + if (err < 0) { + print_fatal("Could not get local IP"); + close(sockfd_); + return false; + } + char str[18]; + inet_ntop(AF_INET, &name.sin_addr, str, 18); + local_ip_ = str; + comThread_ = std::thread(&UrRealtimeCommunication::run, this); + return true; } -void UrRealtimeCommunication::halt() { - keepalive_ = false; - comThread_.join(); +void UrRealtimeCommunication::halt() +{ + keepalive_ = false; + comThread_.join(); } -void UrRealtimeCommunication::addCommandToQueue(std::string inp) { - int bytes_written; - if (inp.back() != '\n') { - inp.append("\n"); - } - if (connected_) - bytes_written = write(sockfd_, inp.c_str(), inp.length()); - else - print_error("Could not send command \"" +inp + "\". The robot is not connected! Command is discarded" ); +void UrRealtimeCommunication::addCommandToQueue(std::string inp) +{ + int bytes_written; + if (inp.back() != '\n') { + inp.append("\n"); + } + if (connected_) + bytes_written = write(sockfd_, inp.c_str(), inp.length()); + else + print_error("Could not send command \"" + inp + "\". The robot is not connected! Command is discarded"); } void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2, - double q3, double q4, double q5, double acc) { - char cmd[1024]; - if( robot_state_->getVersion() >= 3.1 ) { - sprintf(cmd, - "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n", - q0, q1, q2, q3, q4, q5, acc); - } - else { - sprintf(cmd, - "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n", - q0, q1, q2, q3, q4, q5, acc); - } - addCommandToQueue((std::string) (cmd)); - if (q0 != 0. or q1 != 0. or q2 != 0. or q3 != 0. or q4 != 0. or q5 != 0.) { - //If a joint speed is set, make sure we stop it again after some time if the user doesn't - safety_count_ = 0; - } + double q3, double q4, double q5, double acc) +{ + char cmd[1024]; + if (robot_state_->getVersion() >= 3.1) { + sprintf(cmd, + "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n", + q0, q1, q2, q3, q4, q5, acc); + } else { + sprintf(cmd, + "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n", + q0, q1, q2, q3, q4, q5, acc); + } + addCommandToQueue((std::string)(cmd)); + if (q0 != 0. or q1 != 0. or q2 != 0. or q3 != 0. or q4 != 0. or q5 != 0.) { + //If a joint speed is set, make sure we stop it again after some time if the user doesn't + safety_count_ = 0; + } } -void UrRealtimeCommunication::run() { - uint8_t buf[2048]; - int bytes_read; - bzero(buf, 2048); - struct timeval timeout; - fd_set readfds; - FD_ZERO(&readfds); - FD_SET(sockfd_, &readfds); - print_debug("Realtime port: Got connection"); - connected_ = true; - while (keepalive_) { - while (connected_ && keepalive_) { - timeout.tv_sec = 0; //do this each loop as selects modifies timeout - timeout.tv_usec = 500000; // timeout of 0.5 sec - select(sockfd_ + 1, &readfds, NULL, NULL, &timeout); - bytes_read = read(sockfd_, buf, 2048); - if (bytes_read > 0) { - setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, - sizeof(int)); - robot_state_->unpack(buf); - if (safety_count_ == safety_count_max_) { - setSpeed(0., 0., 0., 0., 0., 0.); - } - safety_count_ += 1; - } else { - connected_ = false; - close(sockfd_); - } - } - if (keepalive_) { - //reconnect - print_warning("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."); - sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (sockfd_ < 0) { - print_fatal("ERROR opening socket"); - } - flag_ = 1; - setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, - sizeof(int)); - setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, - sizeof(int)); - - setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, - sizeof(int)); - fcntl(sockfd_, F_SETFL, O_NONBLOCK); - while (keepalive_ && !connected_) { - std::this_thread::sleep_for(std::chrono::seconds(10)); - fd_set writefds; +void UrRealtimeCommunication::run() +{ + uint8_t buf[2048]; + int bytes_read; + bzero(buf, 2048); + struct timeval timeout; + fd_set readfds; + FD_ZERO(&readfds); + FD_SET(sockfd_, &readfds); + print_debug("Realtime port: Got connection"); + connected_ = true; + while (keepalive_) { + while (connected_ && keepalive_) { + timeout.tv_sec = 0; //do this each loop as selects modifies timeout + timeout.tv_usec = 500000; // timeout of 0.5 sec + select(sockfd_ + 1, &readfds, NULL, NULL, &timeout); + bytes_read = read(sockfd_, buf, 2048); + if (bytes_read > 0) { + setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, + sizeof(int)); + robot_state_->unpack(buf); + if (safety_count_ == safety_count_max_) { + setSpeed(0., 0., 0., 0., 0., 0.); + } + safety_count_ += 1; + } else { + connected_ = false; + close(sockfd_); + } + } + if (keepalive_) { + //reconnect + print_warning("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."); + sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (sockfd_ < 0) { + print_fatal("ERROR opening socket"); + } + flag_ = 1; + setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, + sizeof(int)); + setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, + sizeof(int)); - connect(sockfd_, (struct sockaddr *) &serv_addr_, - sizeof(serv_addr_)); - FD_ZERO(&writefds); - FD_SET(sockfd_, &writefds); - select(sockfd_ + 1, NULL, &writefds, NULL, NULL); - unsigned int flag_len; - getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); - if (flag_ < 0) { - print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds..."); - } else { - connected_ = true; - print_info("Realtime port: Reconnected"); - } - } - } - } - setSpeed(0., 0., 0., 0., 0., 0.); - close(sockfd_); + setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, + sizeof(int)); + fcntl(sockfd_, F_SETFL, O_NONBLOCK); + while (keepalive_ && !connected_) { + std::this_thread::sleep_for(std::chrono::seconds(10)); + fd_set writefds; + + connect(sockfd_, (struct sockaddr*)&serv_addr_, + sizeof(serv_addr_)); + FD_ZERO(&writefds); + FD_SET(sockfd_, &writefds); + select(sockfd_ + 1, NULL, &writefds, NULL, NULL); + unsigned int flag_len; + getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); + if (flag_ < 0) { + print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds..."); + } else { + connected_ = true; + print_info("Realtime port: Reconnected"); + } + } + } + } + setSpeed(0., 0., 0., 0., 0., 0.); + close(sockfd_); } -void UrRealtimeCommunication::setSafetyCountMax(uint inp) { - safety_count_max_ = inp; +void UrRealtimeCommunication::setSafetyCountMax(uint inp) +{ + safety_count_max_ = inp; } -std::string UrRealtimeCommunication::getLocalIp() { - return local_ip_; +std::string UrRealtimeCommunication::getLocalIp() +{ + return local_ip_; } diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index ab74534..92fe341 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -16,40 +16,40 @@ * limitations under the License. */ +#include "ur_modern_driver/do_output.h" #include "ur_modern_driver/ur_driver.h" #include "ur_modern_driver/ur_hardware_interface.h" -#include "ur_modern_driver/do_output.h" -#include -#include -#include -#include -#include #include -#include #include +#include +#include +#include +#include +#include #include +#include -#include "ros/ros.h" -#include -#include "sensor_msgs/JointState.h" -#include "geometry_msgs/WrenchStamped.h" -#include "geometry_msgs/PoseStamped.h" -#include "control_msgs/FollowJointTrajectoryAction.h" #include "actionlib/server/action_server.h" #include "actionlib/server/server_goal_handle.h" +#include "control_msgs/FollowJointTrajectoryAction.h" +#include "geometry_msgs/PoseStamped.h" +#include "geometry_msgs/WrenchStamped.h" +#include "ros/ros.h" +#include "sensor_msgs/JointState.h" +#include "std_msgs/String.h" #include "trajectory_msgs/JointTrajectoryPoint.h" +#include "ur_msgs/Analog.h" +#include "ur_msgs/Digital.h" +#include "ur_msgs/IOStates.h" #include "ur_msgs/SetIO.h" +#include "ur_msgs/SetIORequest.h" +#include "ur_msgs/SetIOResponse.h" #include "ur_msgs/SetPayload.h" #include "ur_msgs/SetPayloadRequest.h" #include "ur_msgs/SetPayloadResponse.h" -#include "ur_msgs/SetIORequest.h" -#include "ur_msgs/SetIOResponse.h" -#include "ur_msgs/IOStates.h" -#include "ur_msgs/Digital.h" -#include "ur_msgs/Analog.h" -#include "std_msgs/String.h" #include #include +#include /// TF #include @@ -57,125 +57,129 @@ class RosWrapper { protected: - UrDriver robot_; - std::condition_variable rt_msg_cond_; - std::condition_variable msg_cond_; - ros::NodeHandle nh_; - actionlib::ActionServer as_; - actionlib::ServerGoalHandle goal_handle_; - bool has_goal_; - control_msgs::FollowJointTrajectoryFeedback feedback_; - control_msgs::FollowJointTrajectoryResult result_; - ros::Subscriber speed_sub_; - ros::Subscriber urscript_sub_; - ros::ServiceServer io_srv_; - ros::ServiceServer payload_srv_; - std::thread* rt_publish_thread_; - std::thread* mb_publish_thread_; - double io_flag_delay_; - double max_velocity_; - std::vector joint_offsets_; + UrDriver robot_; + std::condition_variable rt_msg_cond_; + std::condition_variable msg_cond_; + ros::NodeHandle nh_; + actionlib::ActionServer as_; + actionlib::ServerGoalHandle goal_handle_; + bool has_goal_; + control_msgs::FollowJointTrajectoryFeedback feedback_; + control_msgs::FollowJointTrajectoryResult result_; + ros::Subscriber speed_sub_; + ros::Subscriber urscript_sub_; + ros::ServiceServer io_srv_; + ros::ServiceServer payload_srv_; + std::thread* rt_publish_thread_; + std::thread* mb_publish_thread_; + double io_flag_delay_; + double max_velocity_; + std::vector joint_offsets_; std::string base_frame_; std::string tool_frame_; - bool use_ros_control_; - std::thread* ros_control_thread_; - boost::shared_ptr hardware_interface_; - boost::shared_ptr controller_manager_; + bool use_ros_control_; + std::thread* ros_control_thread_; + boost::shared_ptr hardware_interface_; + boost::shared_ptr controller_manager_; public: - RosWrapper(std::string host, int reverse_port) : - as_(nh_, "follow_joint_trajectory", - boost::bind(&RosWrapper::goalCB, this, _1), - boost::bind(&RosWrapper::cancelCB, this, _1), false), robot_( - rt_msg_cond_, msg_cond_, host, reverse_port, 0.03, 300), io_flag_delay_(0.05), joint_offsets_( - 6, 0.0) { + RosWrapper(std::string host, int reverse_port) + : as_(nh_, "follow_joint_trajectory", + boost::bind(&RosWrapper::goalCB, this, _1), + boost::bind(&RosWrapper::cancelCB, this, _1), false) + , robot_( + rt_msg_cond_, msg_cond_, host, reverse_port, 0.03, 300) + , io_flag_delay_(0.05) + , joint_offsets_( + 6, 0.0) + { - std::string joint_prefix = ""; - std::vector joint_names; - char buf[256]; + std::string joint_prefix = ""; + std::vector joint_names; + char buf[256]; - if (ros::param::get("~prefix", joint_prefix)) { - if (joint_prefix.length() > 0) { - sprintf(buf, "Setting prefix to %s", joint_prefix.c_str()); - print_info(buf); - } + if (ros::param::get("~prefix", joint_prefix)) { + if (joint_prefix.length() > 0) { + sprintf(buf, "Setting prefix to %s", joint_prefix.c_str()); + print_info(buf); + } } - joint_names.push_back(joint_prefix + "shoulder_pan_joint"); - joint_names.push_back(joint_prefix + "shoulder_lift_joint"); - joint_names.push_back(joint_prefix + "elbow_joint"); - joint_names.push_back(joint_prefix + "wrist_1_joint"); - joint_names.push_back(joint_prefix + "wrist_2_joint"); - joint_names.push_back(joint_prefix + "wrist_3_joint"); - robot_.setJointNames(joint_names); + joint_names.push_back(joint_prefix + "shoulder_pan_joint"); + joint_names.push_back(joint_prefix + "shoulder_lift_joint"); + joint_names.push_back(joint_prefix + "elbow_joint"); + joint_names.push_back(joint_prefix + "wrist_1_joint"); + joint_names.push_back(joint_prefix + "wrist_2_joint"); + joint_names.push_back(joint_prefix + "wrist_3_joint"); + robot_.setJointNames(joint_names); - use_ros_control_ = false; - ros::param::get("~use_ros_control", use_ros_control_); + use_ros_control_ = false; + ros::param::get("~use_ros_control", use_ros_control_); - if (use_ros_control_) { - hardware_interface_.reset( - new ros_control_ur::UrHardwareInterface(nh_, &robot_)); - controller_manager_.reset( - new controller_manager::ControllerManager( - hardware_interface_.get(), nh_)); - double max_vel_change = 0.12; // equivalent of an acceleration of 15 rad/sec^2 - if (ros::param::get("~max_acceleration", max_vel_change)) { - max_vel_change = max_vel_change / 125; - } - sprintf(buf, "Max acceleration set to: %f [rad/sec²]", - max_vel_change * 125); - print_debug(buf); - hardware_interface_->setMaxVelChange(max_vel_change); - } - //Using a very high value in order to not limit execution of trajectories being sent from MoveIt! - max_velocity_ = 10.; - if (ros::param::get("~max_velocity", max_velocity_)) { - sprintf(buf, "Max velocity accepted by ur_driver: %f [rad/s]", - max_velocity_); - print_debug(buf); - } + if (use_ros_control_) { + hardware_interface_.reset( + new ros_control_ur::UrHardwareInterface(nh_, &robot_)); + controller_manager_.reset( + new controller_manager::ControllerManager( + hardware_interface_.get(), nh_)); + double max_vel_change = 0.12; // equivalent of an acceleration of 15 rad/sec^2 + if (ros::param::get("~max_acceleration", max_vel_change)) { + max_vel_change = max_vel_change / 125; + } + sprintf(buf, "Max acceleration set to: %f [rad/sec²]", + max_vel_change * 125); + print_debug(buf); + hardware_interface_->setMaxVelChange(max_vel_change); + } + //Using a very high value in order to not limit execution of trajectories being sent from MoveIt! + max_velocity_ = 10.; + if (ros::param::get("~max_velocity", max_velocity_)) { + sprintf(buf, "Max velocity accepted by ur_driver: %f [rad/s]", + max_velocity_); + print_debug(buf); + } - //Bounds for SetPayload service - //Using a very conservative value as it should be set through the parameter server - double min_payload = 0.; - double max_payload = 1.; - if (ros::param::get("~min_payload", min_payload)) { - sprintf(buf, "Min payload set to: %f [kg]", min_payload); - print_debug(buf); - } - if (ros::param::get("~max_payload", max_payload)) { - sprintf(buf, "Max payload set to: %f [kg]", max_payload); - print_debug(buf); - } - robot_.setMinPayload(min_payload); - robot_.setMaxPayload(max_payload); - sprintf(buf, "Bounds for set_payload service calls: [%f, %f]", - min_payload, max_payload); - print_debug(buf); + //Bounds for SetPayload service + //Using a very conservative value as it should be set through the parameter server + double min_payload = 0.; + double max_payload = 1.; + if (ros::param::get("~min_payload", min_payload)) { + sprintf(buf, "Min payload set to: %f [kg]", min_payload); + print_debug(buf); + } + if (ros::param::get("~max_payload", max_payload)) { + sprintf(buf, "Max payload set to: %f [kg]", max_payload); + print_debug(buf); + } + robot_.setMinPayload(min_payload); + robot_.setMaxPayload(max_payload); + sprintf(buf, "Bounds for set_payload service calls: [%f, %f]", + min_payload, max_payload); + print_debug(buf); - double servoj_time = 0.008; - if (ros::param::get("~servoj_time", servoj_time)) { - sprintf(buf, "Servoj_time set to: %f [sec]", servoj_time); - print_debug(buf); - } - robot_.setServojTime(servoj_time); + double servoj_time = 0.008; + if (ros::param::get("~servoj_time", servoj_time)) { + sprintf(buf, "Servoj_time set to: %f [sec]", servoj_time); + print_debug(buf); + } + robot_.setServojTime(servoj_time); - double servoj_lookahead_time = 0.03; - if (ros::param::get("~servoj_lookahead_time", servoj_lookahead_time)) { - sprintf(buf, "Servoj_lookahead_time set to: %f [sec]", servoj_lookahead_time); - print_debug(buf); - } - robot_.setServojLookahead(servoj_lookahead_time); + double servoj_lookahead_time = 0.03; + if (ros::param::get("~servoj_lookahead_time", servoj_lookahead_time)) { + sprintf(buf, "Servoj_lookahead_time set to: %f [sec]", servoj_lookahead_time); + print_debug(buf); + } + robot_.setServojLookahead(servoj_lookahead_time); - double servoj_gain = 300.; - if (ros::param::get("~servoj_gain", servoj_gain)) { - sprintf(buf, "Servoj_gain set to: %f [sec]", servoj_gain); - print_debug(buf); - } - robot_.setServojGain(servoj_gain); + double servoj_gain = 300.; + if (ros::param::get("~servoj_gain", servoj_gain)) { + sprintf(buf, "Servoj_gain set to: %f [sec]", servoj_gain); + print_debug(buf); + } + robot_.setServojGain(servoj_gain); //Base and tool frames base_frame_ = joint_prefix + "base_link"; - tool_frame_ = joint_prefix + "tool0_controller"; + tool_frame_ = joint_prefix + "tool0_controller"; if (ros::param::get("~base_frame", base_frame_)) { sprintf(buf, "Base frame set to: %s", base_frame_.c_str()); print_debug(buf); @@ -185,506 +189,496 @@ public: print_debug(buf); } - if (robot_.start()) { - if (use_ros_control_) { - ros_control_thread_ = new std::thread( - boost::bind(&RosWrapper::rosControlLoop, this)); - print_debug( - "The control thread for this driver has been started"); - } else { - //start actionserver - has_goal_ = false; - as_.start(); + if (robot_.start()) { + if (use_ros_control_) { + ros_control_thread_ = new std::thread( + boost::bind(&RosWrapper::rosControlLoop, this)); + print_debug( + "The control thread for this driver has been started"); + } else { + //start actionserver + has_goal_ = false; + as_.start(); - //subscribe to the data topic of interest - rt_publish_thread_ = new std::thread( - boost::bind(&RosWrapper::publishRTMsg, this)); - print_debug( - "The action server for this driver has been started"); - } - mb_publish_thread_ = new std::thread( - boost::bind(&RosWrapper::publishMbMsg, this)); - speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1, - &RosWrapper::speedInterface, this); - urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, - &RosWrapper::urscriptInterface, this); + //subscribe to the data topic of interest + rt_publish_thread_ = new std::thread( + boost::bind(&RosWrapper::publishRTMsg, this)); + print_debug( + "The action server for this driver has been started"); + } + mb_publish_thread_ = new std::thread( + boost::bind(&RosWrapper::publishMbMsg, this)); + speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1, + &RosWrapper::speedInterface, this); + urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, + &RosWrapper::urscriptInterface, this); - io_srv_ = nh_.advertiseService("ur_driver/set_io", - &RosWrapper::setIO, this); - payload_srv_ = nh_.advertiseService("ur_driver/set_payload", - &RosWrapper::setPayload, this); - } - } + io_srv_ = nh_.advertiseService("ur_driver/set_io", + &RosWrapper::setIO, this); + payload_srv_ = nh_.advertiseService("ur_driver/set_payload", + &RosWrapper::setPayload, this); + } + } - void halt() { - robot_.halt(); - rt_publish_thread_->join(); + void halt() + { + robot_.halt(); + rt_publish_thread_->join(); + } - } private: - void trajThread(std::vector timestamps, - std::vector > positions, - std::vector > velocities) { + void trajThread(std::vector timestamps, + std::vector > positions, + std::vector > velocities) + { - robot_.doTraj(timestamps, positions, velocities); - if (has_goal_) { - result_.error_code = result_.SUCCESSFUL; - goal_handle_.setSucceeded(result_); - has_goal_ = false; - } - } - void goalCB( - actionlib::ServerGoalHandle< - control_msgs::FollowJointTrajectoryAction> gh) { - std::string buf; - print_info("on_goal"); - if (!robot_.sec_interface_->robot_state_->isReady()) { - result_.error_code = -100; //nothing is defined for this...? + robot_.doTraj(timestamps, positions, velocities); + if (has_goal_) { + result_.error_code = result_.SUCCESSFUL; + goal_handle_.setSucceeded(result_); + has_goal_ = false; + } + } + void goalCB( + actionlib::ServerGoalHandle gh) + { + std::string buf; + print_info("on_goal"); + if (!robot_.sec_interface_->robot_state_->isReady()) { + result_.error_code = -100; //nothing is defined for this...? - if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) { - result_.error_string = - "Cannot accept new trajectories: Robot arm is not powered on"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - if (!robot_.sec_interface_->robot_state_->isRealRobotEnabled()) { - result_.error_string = - "Cannot accept new trajectories: Robot is not enabled"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - result_.error_string = - "Cannot accept new trajectories. (Debug: Robot mode is " - + std::to_string( - robot_.sec_interface_->robot_state_->getRobotMode()) - + ")"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - if (robot_.sec_interface_->robot_state_->isEmergencyStopped()) { - result_.error_code = -100; //nothing is defined for this...? - result_.error_string = - "Cannot accept new trajectories: Robot is emergency stopped"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - if (robot_.sec_interface_->robot_state_->isProtectiveStopped()) { - result_.error_code = -100; //nothing is defined for this...? - result_.error_string = - "Cannot accept new trajectories: Robot is protective stopped"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } + if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) { + result_.error_string = "Cannot accept new trajectories: Robot arm is not powered on"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + if (!robot_.sec_interface_->robot_state_->isRealRobotEnabled()) { + result_.error_string = "Cannot accept new trajectories: Robot is not enabled"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + result_.error_string = "Cannot accept new trajectories. (Debug: Robot mode is " + + std::to_string( + robot_.sec_interface_->robot_state_->getRobotMode()) + + ")"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + if (robot_.sec_interface_->robot_state_->isEmergencyStopped()) { + result_.error_code = -100; //nothing is defined for this...? + result_.error_string = "Cannot accept new trajectories: Robot is emergency stopped"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + if (robot_.sec_interface_->robot_state_->isProtectiveStopped()) { + result_.error_code = -100; //nothing is defined for this...? + result_.error_string = "Cannot accept new trajectories: Robot is protective stopped"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } - actionlib::ActionServer::Goal goal = - *gh.getGoal(); //make a copy that we can modify - if (has_goal_) { - print_warning( - "Received new goal while still executing previous trajectory. Canceling previous trajectory"); - has_goal_ = false; - robot_.stopTraj(); - result_.error_code = -100; //nothing is defined for this...? - result_.error_string = "Received another trajectory"; - goal_handle_.setAborted(result_, result_.error_string); - std::this_thread::sleep_for(std::chrono::milliseconds(250)); - } - goal_handle_ = gh; - if (!validateJointNames()) { - std::string outp_joint_names = ""; - for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); - i++) { - outp_joint_names += goal.trajectory.joint_names[i] + " "; - } - result_.error_code = result_.INVALID_JOINTS; - result_.error_string = - "Received a goal with incorrect joint names: " - + outp_joint_names; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - if (!has_positions()) { - result_.error_code = result_.INVALID_GOAL; - result_.error_string = "Received a goal without positions"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - - if (!has_velocities()) { - result_.error_code = result_.INVALID_GOAL; - result_.error_string = "Received a goal without velocities"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } + actionlib::ActionServer::Goal goal = *gh.getGoal(); //make a copy that we can modify + if (has_goal_) { + print_warning( + "Received new goal while still executing previous trajectory. Canceling previous trajectory"); + has_goal_ = false; + robot_.stopTraj(); + result_.error_code = -100; //nothing is defined for this...? + result_.error_string = "Received another trajectory"; + goal_handle_.setAborted(result_, result_.error_string); + std::this_thread::sleep_for(std::chrono::milliseconds(250)); + } + goal_handle_ = gh; + if (!validateJointNames()) { + std::string outp_joint_names = ""; + for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); + i++) { + outp_joint_names += goal.trajectory.joint_names[i] + " "; + } + result_.error_code = result_.INVALID_JOINTS; + result_.error_string = "Received a goal with incorrect joint names: " + + outp_joint_names; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + if (!has_positions()) { + result_.error_code = result_.INVALID_GOAL; + result_.error_string = "Received a goal without positions"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } - if (!traj_is_finite()) { - result_.error_string = "Received a goal with infinities or NaNs"; - result_.error_code = result_.INVALID_GOAL; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } + if (!has_velocities()) { + result_.error_code = result_.INVALID_GOAL; + result_.error_string = "Received a goal without velocities"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } - if (!has_limited_velocities()) { - result_.error_code = result_.INVALID_GOAL; - result_.error_string = - "Received a goal with velocities that are higher than " - + std::to_string(max_velocity_); - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } + if (!traj_is_finite()) { + result_.error_string = "Received a goal with infinities or NaNs"; + result_.error_code = result_.INVALID_GOAL; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } - reorder_traj_joints(goal.trajectory); - - if (!start_positions_match(goal.trajectory, 0.01)) { - result_.error_code = result_.INVALID_GOAL; - result_.error_string = "Goal start doesn't match current pose"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } + if (!has_limited_velocities()) { + result_.error_code = result_.INVALID_GOAL; + result_.error_string = "Received a goal with velocities that are higher than " + + std::to_string(max_velocity_); + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } - std::vector timestamps; - std::vector > positions, velocities; - if (goal.trajectory.points[0].time_from_start.toSec() != 0.) { - print_warning( - "Trajectory's first point should be the current position, with time_from_start set to 0.0 - Inserting point in malformed trajectory"); - timestamps.push_back(0.0); - positions.push_back( - robot_.rt_interface_->robot_state_->getQActual()); - velocities.push_back( - robot_.rt_interface_->robot_state_->getQdActual()); - } - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { - timestamps.push_back( - goal.trajectory.points[i].time_from_start.toSec()); - positions.push_back(goal.trajectory.points[i].positions); - velocities.push_back(goal.trajectory.points[i].velocities); + reorder_traj_joints(goal.trajectory); - } + if (!start_positions_match(goal.trajectory, 0.01)) { + result_.error_code = result_.INVALID_GOAL; + result_.error_string = "Goal start doesn't match current pose"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } - goal_handle_.setAccepted(); - has_goal_ = true; - std::thread(&RosWrapper::trajThread, this, timestamps, positions, - velocities).detach(); - } + std::vector timestamps; + std::vector > positions, velocities; + if (goal.trajectory.points[0].time_from_start.toSec() != 0.) { + print_warning( + "Trajectory's first point should be the current position, with time_from_start set to 0.0 - Inserting point in malformed trajectory"); + timestamps.push_back(0.0); + positions.push_back( + robot_.rt_interface_->robot_state_->getQActual()); + velocities.push_back( + robot_.rt_interface_->robot_state_->getQdActual()); + } + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { + timestamps.push_back( + goal.trajectory.points[i].time_from_start.toSec()); + positions.push_back(goal.trajectory.points[i].positions); + velocities.push_back(goal.trajectory.points[i].velocities); + } - void cancelCB( - actionlib::ServerGoalHandle< - control_msgs::FollowJointTrajectoryAction> gh) { - // set the action state to preempted - print_info("on_cancel"); - if (has_goal_) { - if (gh == goal_handle_) { - robot_.stopTraj(); - has_goal_ = false; - } - } - result_.error_code = -100; //nothing is defined for this...? - result_.error_string = "Goal cancelled by client"; - gh.setCanceled(result_); - } + goal_handle_.setAccepted(); + has_goal_ = true; + std::thread(&RosWrapper::trajThread, this, timestamps, positions, + velocities) + .detach(); + } - bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp) { - resp.success = true; - //if (req.fun == ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT) { - if (req.fun == 1) { - robot_.setDigitalOut(req.pin, req.state > 0.0 ? true : false); - } else if (req.fun == 2) { - //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_FLAG) { - robot_.setFlag(req.pin, req.state > 0.0 ? true : false); - //According to urdriver.py, set_flag will fail if called to rapidly in succession - ros::Duration(io_flag_delay_).sleep(); - } else if (req.fun == 3) { - //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_ANALOG_OUT) { - robot_.setAnalogOut(req.pin, req.state); - } else if (req.fun == 4) { - //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_TOOL_VOLTAGE) { - robot_.setToolVoltage((int) req.state); - } else { - resp.success = false; - } - return resp.success; - } + void cancelCB( + actionlib::ServerGoalHandle gh) + { + // set the action state to preempted + print_info("on_cancel"); + if (has_goal_) { + if (gh == goal_handle_) { + robot_.stopTraj(); + has_goal_ = false; + } + } + result_.error_code = -100; //nothing is defined for this...? + result_.error_string = "Goal cancelled by client"; + gh.setCanceled(result_); + } - bool setPayload(ur_msgs::SetPayloadRequest& req, - ur_msgs::SetPayloadResponse& resp) { - if (robot_.setPayload(req.payload)) - resp.success = true; - else - resp.success = true; - return resp.success; - } + bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp) + { + resp.success = true; + //if (req.fun == ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT) { + if (req.fun == 1) { + robot_.setDigitalOut(req.pin, req.state > 0.0 ? true : false); + } else if (req.fun == 2) { + //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_FLAG) { + robot_.setFlag(req.pin, req.state > 0.0 ? true : false); + //According to urdriver.py, set_flag will fail if called to rapidly in succession + ros::Duration(io_flag_delay_).sleep(); + } else if (req.fun == 3) { + //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_ANALOG_OUT) { + robot_.setAnalogOut(req.pin, req.state); + } else if (req.fun == 4) { + //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_TOOL_VOLTAGE) { + robot_.setToolVoltage((int)req.state); + } else { + resp.success = false; + } + return resp.success; + } - bool validateJointNames() { - std::vector actual_joint_names = robot_.getJointNames(); - actionlib::ActionServer::Goal goal = - *goal_handle_.getGoal(); - if (goal.trajectory.joint_names.size() != actual_joint_names.size()) - return false; + bool setPayload(ur_msgs::SetPayloadRequest& req, + ur_msgs::SetPayloadResponse& resp) + { + if (robot_.setPayload(req.payload)) + resp.success = true; + else + resp.success = true; + return resp.success; + } - for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) { - unsigned int j; - for (j = 0; j < actual_joint_names.size(); j++) { - if (goal.trajectory.joint_names[i] == actual_joint_names[j]) - break; - } - if (goal.trajectory.joint_names[i] == actual_joint_names[j]) { - actual_joint_names.erase(actual_joint_names.begin() + j); - } else { - return false; - } - } + bool validateJointNames() + { + std::vector actual_joint_names = robot_.getJointNames(); + actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); + if (goal.trajectory.joint_names.size() != actual_joint_names.size()) + return false; - return true; - } + for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) { + unsigned int j; + for (j = 0; j < actual_joint_names.size(); j++) { + if (goal.trajectory.joint_names[i] == actual_joint_names[j]) + break; + } + if (goal.trajectory.joint_names[i] == actual_joint_names[j]) { + actual_joint_names.erase(actual_joint_names.begin() + j); + } else { + return false; + } + } - void reorder_traj_joints(trajectory_msgs::JointTrajectory& traj) { - /* Reorders trajectory - destructive */ - std::vector actual_joint_names = robot_.getJointNames(); - std::vector mapping; - mapping.resize(actual_joint_names.size(), actual_joint_names.size()); - for (unsigned int i = 0; i < traj.joint_names.size(); i++) { - for (unsigned int j = 0; j < actual_joint_names.size(); j++) { - if (traj.joint_names[i] == actual_joint_names[j]) - mapping[j] = i; - } - } - traj.joint_names = actual_joint_names; - std::vector new_traj; - for (unsigned int i = 0; i < traj.points.size(); i++) { - trajectory_msgs::JointTrajectoryPoint new_point; - for (unsigned int j = 0; j < traj.points[i].positions.size(); j++) { - new_point.positions.push_back( - traj.points[i].positions[mapping[j]]); - new_point.velocities.push_back( - traj.points[i].velocities[mapping[j]]); - if (traj.points[i].accelerations.size() != 0) - new_point.accelerations.push_back( - traj.points[i].accelerations[mapping[j]]); - } - new_point.time_from_start = traj.points[i].time_from_start; - new_traj.push_back(new_point); - } - traj.points = new_traj; - } + return true; + } - bool has_velocities() { - actionlib::ActionServer::Goal goal = - *goal_handle_.getGoal(); - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { - if (goal.trajectory.points[i].positions.size() - != goal.trajectory.points[i].velocities.size()) - return false; - } - return true; - } + void reorder_traj_joints(trajectory_msgs::JointTrajectory& traj) + { + /* Reorders trajectory - destructive */ + std::vector actual_joint_names = robot_.getJointNames(); + std::vector mapping; + mapping.resize(actual_joint_names.size(), actual_joint_names.size()); + for (unsigned int i = 0; i < traj.joint_names.size(); i++) { + for (unsigned int j = 0; j < actual_joint_names.size(); j++) { + if (traj.joint_names[i] == actual_joint_names[j]) + mapping[j] = i; + } + } + traj.joint_names = actual_joint_names; + std::vector new_traj; + for (unsigned int i = 0; i < traj.points.size(); i++) { + trajectory_msgs::JointTrajectoryPoint new_point; + for (unsigned int j = 0; j < traj.points[i].positions.size(); j++) { + new_point.positions.push_back( + traj.points[i].positions[mapping[j]]); + new_point.velocities.push_back( + traj.points[i].velocities[mapping[j]]); + if (traj.points[i].accelerations.size() != 0) + new_point.accelerations.push_back( + traj.points[i].accelerations[mapping[j]]); + } + new_point.time_from_start = traj.points[i].time_from_start; + new_traj.push_back(new_point); + } + traj.points = new_traj; + } - bool has_positions() { - actionlib::ActionServer::Goal goal = - *goal_handle_.getGoal(); - if (goal.trajectory.points.size() == 0) - return false; - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { - if (goal.trajectory.points[i].positions.size() - != goal.trajectory.joint_names.size()) - return false; - } - return true; - } + bool has_velocities() + { + actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { + if (goal.trajectory.points[i].positions.size() + != goal.trajectory.points[i].velocities.size()) + return false; + } + return true; + } - bool start_positions_match(const trajectory_msgs::JointTrajectory &traj, double eps) - { - for (unsigned int i = 0; i < traj.points[0].positions.size(); i++) - { - std::vector qActual = robot_.rt_interface_->robot_state_->getQActual(); - if( fabs(traj.points[0].positions[i] - qActual[i]) > eps ) - { - return false; - } - } - return true; - } + bool has_positions() + { + actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); + if (goal.trajectory.points.size() == 0) + return false; + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { + if (goal.trajectory.points[i].positions.size() + != goal.trajectory.joint_names.size()) + return false; + } + return true; + } - bool has_limited_velocities() { - actionlib::ActionServer::Goal goal = - *goal_handle_.getGoal(); - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { - for (unsigned int j = 0; - j < goal.trajectory.points[i].velocities.size(); j++) { - if (fabs(goal.trajectory.points[i].velocities[j]) - > max_velocity_) - return false; - } - } - return true; - } + bool start_positions_match(const trajectory_msgs::JointTrajectory& traj, double eps) + { + for (unsigned int i = 0; i < traj.points[0].positions.size(); i++) { + std::vector qActual = robot_.rt_interface_->robot_state_->getQActual(); + if (fabs(traj.points[0].positions[i] - qActual[i]) > eps) { + return false; + } + } + return true; + } - bool traj_is_finite() { - actionlib::ActionServer::Goal goal = - *goal_handle_.getGoal(); - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { - for (unsigned int j = 0; - j < goal.trajectory.points[i].velocities.size(); j++) { - if (!std::isfinite(goal.trajectory.points[i].positions[j])) - return false; - if (!std::isfinite(goal.trajectory.points[i].velocities[j])) - return false; - } - } - return true; - } + bool has_limited_velocities() + { + actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { + for (unsigned int j = 0; + j < goal.trajectory.points[i].velocities.size(); j++) { + if (fabs(goal.trajectory.points[i].velocities[j]) + > max_velocity_) + return false; + } + } + return true; + } - void speedInterface(const trajectory_msgs::JointTrajectory::Ptr& msg) { - if (msg->points[0].velocities.size() == 6) { - double acc = 100; - if (msg->points[0].accelerations.size() > 0) - acc = *std::max_element(msg->points[0].accelerations.begin(), - msg->points[0].accelerations.end()); - robot_.setSpeed(msg->points[0].velocities[0], - msg->points[0].velocities[1], msg->points[0].velocities[2], - msg->points[0].velocities[3], msg->points[0].velocities[4], - msg->points[0].velocities[5], acc); - } + bool traj_is_finite() + { + actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { + for (unsigned int j = 0; + j < goal.trajectory.points[i].velocities.size(); j++) { + if (!std::isfinite(goal.trajectory.points[i].positions[j])) + return false; + if (!std::isfinite(goal.trajectory.points[i].velocities[j])) + return false; + } + } + return true; + } - } - void urscriptInterface(const std_msgs::String::ConstPtr& msg) { + void speedInterface(const trajectory_msgs::JointTrajectory::Ptr& msg) + { + if (msg->points[0].velocities.size() == 6) { + double acc = 100; + if (msg->points[0].accelerations.size() > 0) + acc = *std::max_element(msg->points[0].accelerations.begin(), + msg->points[0].accelerations.end()); + robot_.setSpeed(msg->points[0].velocities[0], + msg->points[0].velocities[1], msg->points[0].velocities[2], + msg->points[0].velocities[3], msg->points[0].velocities[4], + msg->points[0].velocities[5], acc); + } + } + void urscriptInterface(const std_msgs::String::ConstPtr& msg) + { - robot_.rt_interface_->addCommandToQueue(msg->data); + robot_.rt_interface_->addCommandToQueue(msg->data); + } - } + void rosControlLoop() + { + ros::Duration elapsed_time; + struct timespec last_time, current_time; + static const double BILLION = 1000000000.0; - void rosControlLoop() { - ros::Duration elapsed_time; - struct timespec last_time, current_time; - static const double BILLION = 1000000000.0; + realtime_tools::RealtimePublisher tf_pub(nh_, "/tf", 1); + geometry_msgs::TransformStamped tool_transform; + tool_transform.header.frame_id = base_frame_; + tool_transform.child_frame_id = tool_frame_; + tf_pub.msg_.transforms.push_back(tool_transform); - realtime_tools::RealtimePublisher tf_pub( nh_, "/tf", 1 ); - geometry_msgs::TransformStamped tool_transform; - tool_transform.header.frame_id = base_frame_; - tool_transform.child_frame_id = tool_frame_; - tf_pub.msg_.transforms.push_back( tool_transform ); + realtime_tools::RealtimePublisher tool_vel_pub(nh_, "tool_velocity", 1); + tool_vel_pub.msg_.header.frame_id = base_frame_; - realtime_tools::RealtimePublisher tool_vel_pub( nh_, "tool_velocity", 1 ); - tool_vel_pub.msg_.header.frame_id = base_frame_; + clock_gettime(CLOCK_MONOTONIC, &last_time); + while (ros::ok()) { + std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex + std::unique_lock locker(msg_lock); + while (!robot_.rt_interface_->robot_state_->getControllerUpdated()) { + rt_msg_cond_.wait(locker); + } + // Input + hardware_interface_->read(); + robot_.rt_interface_->robot_state_->setControllerUpdated(); + // Control + clock_gettime(CLOCK_MONOTONIC, ¤t_time); + elapsed_time = ros::Duration(current_time.tv_sec - last_time.tv_sec + (current_time.tv_nsec - last_time.tv_nsec) / BILLION); + ros::Time ros_time = ros::Time::now(); + controller_manager_->update(ros_time, elapsed_time); + last_time = current_time; - clock_gettime(CLOCK_MONOTONIC, &last_time); - while (ros::ok()) { - std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex - std::unique_lock locker(msg_lock); - while (!robot_.rt_interface_->robot_state_->getControllerUpdated()) { - rt_msg_cond_.wait(locker); - } - // Input - hardware_interface_->read(); - robot_.rt_interface_->robot_state_->setControllerUpdated(); + // Output + hardware_interface_->write(); - // Control - clock_gettime(CLOCK_MONOTONIC, ¤t_time); - elapsed_time = ros::Duration(current_time.tv_sec - last_time.tv_sec + (current_time.tv_nsec - last_time.tv_nsec)/ BILLION); - ros::Time ros_time = ros::Time::now(); - controller_manager_->update(ros_time, elapsed_time); - last_time = current_time; + // Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation + std::vector tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual(); - // Output - hardware_interface_->write(); + // Compute rotation angle + double rx = tool_vector_actual[3]; + double ry = tool_vector_actual[4]; + double rz = tool_vector_actual[5]; + double angle = std::sqrt(std::pow(rx, 2) + std::pow(ry, 2) + std::pow(rz, 2)); - // Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation - std::vector tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual(); + // Broadcast transform + if (tf_pub.trylock()) { + tf_pub.msg_.transforms[0].header.stamp = ros_time; + if (angle < 1e-16) { + tf_pub.msg_.transforms[0].transform.rotation.x = 0; + tf_pub.msg_.transforms[0].transform.rotation.y = 0; + tf_pub.msg_.transforms[0].transform.rotation.z = 0; + tf_pub.msg_.transforms[0].transform.rotation.w = 1; + } else { + tf_pub.msg_.transforms[0].transform.rotation.x = (rx / angle) * std::sin(angle * 0.5); + tf_pub.msg_.transforms[0].transform.rotation.y = (ry / angle) * std::sin(angle * 0.5); + tf_pub.msg_.transforms[0].transform.rotation.z = (rz / angle) * std::sin(angle * 0.5); + tf_pub.msg_.transforms[0].transform.rotation.w = std::cos(angle * 0.5); + } + tf_pub.msg_.transforms[0].transform.translation.x = tool_vector_actual[0]; + tf_pub.msg_.transforms[0].transform.translation.y = tool_vector_actual[1]; + tf_pub.msg_.transforms[0].transform.translation.z = tool_vector_actual[2]; - // Compute rotation angle - double rx = tool_vector_actual[3]; - double ry = tool_vector_actual[4]; - double rz = tool_vector_actual[5]; - double angle = std::sqrt(std::pow(rx,2) + std::pow(ry,2) + std::pow(rz,2)); + tf_pub.unlockAndPublish(); + } - // Broadcast transform - if( tf_pub.trylock() ) - { - tf_pub.msg_.transforms[0].header.stamp = ros_time; - if (angle < 1e-16) { - tf_pub.msg_.transforms[0].transform.rotation.x = 0; - tf_pub.msg_.transforms[0].transform.rotation.y = 0; - tf_pub.msg_.transforms[0].transform.rotation.z = 0; - tf_pub.msg_.transforms[0].transform.rotation.w = 1; - } else { - tf_pub.msg_.transforms[0].transform.rotation.x = (rx/angle) * std::sin(angle*0.5); - tf_pub.msg_.transforms[0].transform.rotation.y = (ry/angle) * std::sin(angle*0.5); - tf_pub.msg_.transforms[0].transform.rotation.z = (rz/angle) * std::sin(angle*0.5); - tf_pub.msg_.transforms[0].transform.rotation.w = std::cos(angle*0.5); - } - tf_pub.msg_.transforms[0].transform.translation.x = tool_vector_actual[0]; - tf_pub.msg_.transforms[0].transform.translation.y = tool_vector_actual[1]; - tf_pub.msg_.transforms[0].transform.translation.z = tool_vector_actual[2]; + //Publish tool velocity + std::vector tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual(); - tf_pub.unlockAndPublish(); - } + if (tool_vel_pub.trylock()) { + tool_vel_pub.msg_.header.stamp = ros_time; + tool_vel_pub.msg_.twist.linear.x = tcp_speed[0]; + tool_vel_pub.msg_.twist.linear.y = tcp_speed[1]; + tool_vel_pub.msg_.twist.linear.z = tcp_speed[2]; + tool_vel_pub.msg_.twist.angular.x = tcp_speed[3]; + tool_vel_pub.msg_.twist.angular.y = tcp_speed[4]; + tool_vel_pub.msg_.twist.angular.z = tcp_speed[5]; - //Publish tool velocity - std::vector tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual(); + tool_vel_pub.unlockAndPublish(); + } + } + } - if( tool_vel_pub.trylock() ) - { - tool_vel_pub.msg_.header.stamp = ros_time; - tool_vel_pub.msg_.twist.linear.x = tcp_speed[0]; - tool_vel_pub.msg_.twist.linear.y = tcp_speed[1]; - tool_vel_pub.msg_.twist.linear.z = tcp_speed[2]; - tool_vel_pub.msg_.twist.angular.x = tcp_speed[3]; - tool_vel_pub.msg_.twist.angular.y = tcp_speed[4]; - tool_vel_pub.msg_.twist.angular.z = tcp_speed[5]; - - tool_vel_pub.unlockAndPublish(); - } - - } - } - - void publishRTMsg() { - ros::Publisher joint_pub = nh_.advertise( - "joint_states", 1); - ros::Publisher wrench_pub = nh_.advertise( - "wrench", 1); + void publishRTMsg() + { + ros::Publisher joint_pub = nh_.advertise( + "joint_states", 1); + ros::Publisher wrench_pub = nh_.advertise( + "wrench", 1); ros::Publisher tool_vel_pub = nh_.advertise("tool_velocity", 1); static tf::TransformBroadcaster br; - while (ros::ok()) { - sensor_msgs::JointState joint_msg; - joint_msg.name = robot_.getJointNames(); - geometry_msgs::WrenchStamped wrench_msg; + while (ros::ok()) { + sensor_msgs::JointState joint_msg; + joint_msg.name = robot_.getJointNames(); + geometry_msgs::WrenchStamped wrench_msg; geometry_msgs::PoseStamped tool_pose_msg; - std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex - std::unique_lock locker(msg_lock); - while (!robot_.rt_interface_->robot_state_->getDataPublished()) { - rt_msg_cond_.wait(locker); - } - joint_msg.header.stamp = ros::Time::now(); - joint_msg.position = - robot_.rt_interface_->robot_state_->getQActual(); - for (unsigned int i = 0; i < joint_msg.position.size(); i++) { - joint_msg.position[i] += joint_offsets_[i]; - } - joint_msg.velocity = - robot_.rt_interface_->robot_state_->getQdActual(); - joint_msg.effort = robot_.rt_interface_->robot_state_->getIActual(); - joint_pub.publish(joint_msg); - std::vector tcp_force = - robot_.rt_interface_->robot_state_->getTcpForce(); - wrench_msg.header.stamp = joint_msg.header.stamp; - wrench_msg.wrench.force.x = tcp_force[0]; - wrench_msg.wrench.force.y = tcp_force[1]; - wrench_msg.wrench.force.z = tcp_force[2]; - wrench_msg.wrench.torque.x = tcp_force[3]; - wrench_msg.wrench.torque.y = tcp_force[4]; - wrench_msg.wrench.torque.z = tcp_force[5]; - wrench_pub.publish(wrench_msg); + std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex + std::unique_lock locker(msg_lock); + while (!robot_.rt_interface_->robot_state_->getDataPublished()) { + rt_msg_cond_.wait(locker); + } + joint_msg.header.stamp = ros::Time::now(); + joint_msg.position = robot_.rt_interface_->robot_state_->getQActual(); + for (unsigned int i = 0; i < joint_msg.position.size(); i++) { + joint_msg.position[i] += joint_offsets_[i]; + } + joint_msg.velocity = robot_.rt_interface_->robot_state_->getQdActual(); + joint_msg.effort = robot_.rt_interface_->robot_state_->getIActual(); + joint_pub.publish(joint_msg); + std::vector tcp_force = robot_.rt_interface_->robot_state_->getTcpForce(); + wrench_msg.header.stamp = joint_msg.header.stamp; + wrench_msg.wrench.force.x = tcp_force[0]; + wrench_msg.wrench.force.y = tcp_force[1]; + wrench_msg.wrench.force.z = tcp_force[2]; + wrench_msg.wrench.torque.x = tcp_force[3]; + wrench_msg.wrench.torque.y = tcp_force[4]; + wrench_msg.wrench.torque.z = tcp_force[5]; + wrench_pub.publish(wrench_msg); // Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation std::vector tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual(); @@ -694,11 +688,11 @@ private: double rx = tool_vector_actual[3]; double ry = tool_vector_actual[4]; double rz = tool_vector_actual[5]; - double angle = std::sqrt(std::pow(rx,2) + std::pow(ry,2) + std::pow(rz,2)); + double angle = std::sqrt(std::pow(rx, 2) + std::pow(ry, 2) + std::pow(rz, 2)); if (angle < 1e-16) { quat.setValue(0, 0, 0, 1); } else { - quat.setRotation(tf::Vector3(rx/angle, ry/angle, rz/angle), angle); + quat.setRotation(tf::Vector3(rx / angle, ry / angle, rz / angle), angle); } //Create and broadcast transform @@ -708,8 +702,7 @@ private: br.sendTransform(tf::StampedTransform(transform, joint_msg.header.stamp, base_frame_, tool_frame_)); //Publish tool velocity - std::vector tcp_speed = - robot_.rt_interface_->robot_state_->getTcpSpeedActual(); + std::vector tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual(); geometry_msgs::TwistStamped tool_twist; tool_twist.header.frame_id = base_frame_; tool_twist.header.stamp = joint_msg.header.stamp; @@ -721,119 +714,118 @@ private: tool_twist.twist.angular.z = tcp_speed[5]; tool_vel_pub.publish(tool_twist); - robot_.rt_interface_->robot_state_->setDataPublished(); - } - } + robot_.rt_interface_->robot_state_->setDataPublished(); + } + } - void publishMbMsg() { - bool warned = false; - ros::Publisher io_pub = nh_.advertise( - "ur_driver/io_states", 1); + void publishMbMsg() + { + bool warned = false; + ros::Publisher io_pub = nh_.advertise( + "ur_driver/io_states", 1); - while (ros::ok()) { - ur_msgs::IOStates io_msg; - std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex - std::unique_lock locker(msg_lock); - while (!robot_.sec_interface_->robot_state_->getNewDataAvailable()) { - msg_cond_.wait(locker); - } - int i_max = 10; - if (robot_.sec_interface_->robot_state_->getVersion() > 3.0) - i_max = 18; // From version 3.0, there are up to 18 inputs and outputs - for (unsigned int i = 0; i < i_max; i++) { - ur_msgs::Digital digi; - digi.pin = i; - digi.state = - ((robot_.sec_interface_->robot_state_->getDigitalInputBits() - & (1 << i)) >> i); - io_msg.digital_in_states.push_back(digi); - digi.state = - ((robot_.sec_interface_->robot_state_->getDigitalOutputBits() - & (1 << i)) >> i); - io_msg.digital_out_states.push_back(digi); - } - ur_msgs::Analog ana; - ana.pin = 0; - ana.state = robot_.sec_interface_->robot_state_->getAnalogInput0(); - io_msg.analog_in_states.push_back(ana); - ana.pin = 1; - ana.state = robot_.sec_interface_->robot_state_->getAnalogInput1(); - io_msg.analog_in_states.push_back(ana); + while (ros::ok()) { + ur_msgs::IOStates io_msg; + std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex + std::unique_lock locker(msg_lock); + while (!robot_.sec_interface_->robot_state_->getNewDataAvailable()) { + msg_cond_.wait(locker); + } + int i_max = 10; + if (robot_.sec_interface_->robot_state_->getVersion() > 3.0) + i_max = 18; // From version 3.0, there are up to 18 inputs and outputs + for (unsigned int i = 0; i < i_max; i++) { + ur_msgs::Digital digi; + digi.pin = i; + digi.state = ((robot_.sec_interface_->robot_state_->getDigitalInputBits() + & (1 << i)) + >> i); + io_msg.digital_in_states.push_back(digi); + digi.state = ((robot_.sec_interface_->robot_state_->getDigitalOutputBits() + & (1 << i)) + >> i); + io_msg.digital_out_states.push_back(digi); + } + ur_msgs::Analog ana; + ana.pin = 0; + ana.state = robot_.sec_interface_->robot_state_->getAnalogInput0(); + io_msg.analog_in_states.push_back(ana); + ana.pin = 1; + ana.state = robot_.sec_interface_->robot_state_->getAnalogInput1(); + io_msg.analog_in_states.push_back(ana); - ana.pin = 0; - ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput0(); - io_msg.analog_out_states.push_back(ana); - ana.pin = 1; - ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput1(); - io_msg.analog_out_states.push_back(ana); - io_pub.publish(io_msg); + ana.pin = 0; + ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput0(); + io_msg.analog_out_states.push_back(ana); + ana.pin = 1; + ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput1(); + io_msg.analog_out_states.push_back(ana); + io_pub.publish(io_msg); - if (robot_.sec_interface_->robot_state_->isEmergencyStopped() - or robot_.sec_interface_->robot_state_->isProtectiveStopped()) { - if (robot_.sec_interface_->robot_state_->isEmergencyStopped() - and !warned) { - print_error("Emergency stop pressed!"); - } else if (robot_.sec_interface_->robot_state_->isProtectiveStopped() - and !warned) { - print_error("Robot is protective stopped!"); - } - if (has_goal_) { - print_error("Aborting trajectory"); - robot_.stopTraj(); - result_.error_code = result_.SUCCESSFUL; - result_.error_string = "Robot was halted"; - goal_handle_.setAborted(result_, result_.error_string); - has_goal_ = false; - } - warned = true; - } else - warned = false; - - robot_.sec_interface_->robot_state_->finishedReading(); - - } - } + if (robot_.sec_interface_->robot_state_->isEmergencyStopped() + or robot_.sec_interface_->robot_state_->isProtectiveStopped()) { + if (robot_.sec_interface_->robot_state_->isEmergencyStopped() + and !warned) { + print_error("Emergency stop pressed!"); + } else if (robot_.sec_interface_->robot_state_->isProtectiveStopped() + and !warned) { + print_error("Robot is protective stopped!"); + } + if (has_goal_) { + print_error("Aborting trajectory"); + robot_.stopTraj(); + result_.error_code = result_.SUCCESSFUL; + result_.error_string = "Robot was halted"; + goal_handle_.setAborted(result_, result_.error_string); + has_goal_ = false; + } + warned = true; + } else + warned = false; + robot_.sec_interface_->robot_state_->finishedReading(); + } + } }; -int main(int argc, char **argv) { - bool use_sim_time = false; - std::string host; - int reverse_port = 50001; +int main(int argc, char** argv) +{ + bool use_sim_time = false; + std::string host; + int reverse_port = 50001; - ros::init(argc, argv, "ur_driver"); - ros::NodeHandle nh; - if (ros::param::get("use_sim_time", use_sim_time)) { - print_warning("use_sim_time is set!!"); - } - if (!(ros::param::get("~robot_ip_address", host))) { - if (argc > 1) { - print_warning( - "Please set the parameter robot_ip_address instead of giving it as a command line argument. This method is DEPRECATED"); - host = argv[1]; - } else { - print_fatal( - "Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip"); - exit(1); - } + ros::init(argc, argv, "ur_driver"); + ros::NodeHandle nh; + if (ros::param::get("use_sim_time", use_sim_time)) { + print_warning("use_sim_time is set!!"); + } + if (!(ros::param::get("~robot_ip_address", host))) { + if (argc > 1) { + print_warning( + "Please set the parameter robot_ip_address instead of giving it as a command line argument. This method is DEPRECATED"); + host = argv[1]; + } else { + print_fatal( + "Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip"); + exit(1); + } + } + if ((ros::param::get("~reverse_port", reverse_port))) { + if ((reverse_port <= 0) or (reverse_port >= 65535)) { + print_warning("Reverse port value is not valid (Use number between 1 and 65534. Using default value of 50001"); + reverse_port = 50001; + } + } else + reverse_port = 50001; - } - if ((ros::param::get("~reverse_port", reverse_port))) { - if((reverse_port <= 0) or (reverse_port >= 65535)) { - print_warning("Reverse port value is not valid (Use number between 1 and 65534. Using default value of 50001"); - reverse_port = 50001; - } - } else - reverse_port = 50001; + RosWrapper interface(host, reverse_port); - RosWrapper interface(host, reverse_port); + ros::AsyncSpinner spinner(3); + spinner.start(); - ros::AsyncSpinner spinner(3); - spinner.start(); + ros::waitForShutdown(); - ros::waitForShutdown(); + interface.halt(); - interface.halt(); - - exit(0); + exit(0); } 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 0737e61..0870526 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 @@ private: 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 af5a4a4..d58b023 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 13a9b1b..df56421 100644 --- a/include/ur_modern_driver/pipeline.h +++ b/include/ur_modern_driver/pipeline.h @@ -49,7 +49,7 @@ private: for (auto& p : products) { if (!_queue.try_enqueue(std::move(p))) { - LOG_WARN("Pipeline owerflowed!"); + LOG_ERROR("Pipeline owerflowed!"); } } @@ -57,6 +57,7 @@ private: } _producer.teardown_producer(); //todo cleanup + LOG_DEBUG("Pipline producer ended"); } void run_consumer() @@ -64,12 +65,18 @@ private: _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 @@ public: 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 d0fd10f..f05adf8 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 92cd299..5574293 100644 --- a/include/ur_modern_driver/ur/rt_state.h +++ b/include/ur_modern_driver/ur/rt_state.h @@ -33,6 +33,9 @@ public: //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 cfbe95c..7db11c9 100644 --- a/include/ur_modern_driver/ur/producer.h +++ b/include/ur_modern_driver/ur/producer.h @@ -16,18 +16,9 @@ public: { } - 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 @@ public: //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 48cf6b6..adf7a99 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 f13aa73..fe405af 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 12d49cb..2bc102e 100644 --- a/include/ur_modern_driver/bin_parser.h +++ b/include/ur_modern_driver/bin_parser.h @@ -66,20 +66,14 @@ public: { 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 @@ public: _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 f05adf8..955d1f3 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 df56421..199345a 100644 --- a/include/ur_modern_driver/pipeline.h +++ b/include/ur_modern_driver/pipeline.h @@ -49,15 +49,15 @@ private: 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 @@ private: 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 8c51443..86d3a24 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 5574293..ea2dd5e 100644 --- a/include/ur_modern_driver/ur/rt_state.h +++ b/include/ur_modern_driver/ur/rt_state.h @@ -16,8 +16,8 @@ public: 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 @@ public: 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 fe405af..2264835 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 | 119 +- include/ur_modern_driver/bin_parser.h | 293 ++-- include/ur_modern_driver/packet.h | 5 +- include/ur_modern_driver/parser.h | 5 +- include/ur_modern_driver/pipeline.h | 174 +- include/ur_modern_driver/queue/atomicops.h | 850 +++++----- .../queue/readerwriterqueue.h | 1433 ++++++++-------- include/ur_modern_driver/robot_state.h | 321 ++-- 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 | 55 +- include/ur_modern_driver/ur/factory.h | 139 +- include/ur_modern_driver/ur/master_board.h | 120 +- include/ur_modern_driver/ur/messages.h | 68 +- include/ur_modern_driver/ur/messages_parser.h | 80 +- include/ur_modern_driver/ur/parser.h | 7 +- include/ur_modern_driver/ur/producer.h | 73 +- include/ur_modern_driver/ur/robot_mode.h | 143 +- include/ur_modern_driver/ur/rt_parser.h | 40 +- include/ur_modern_driver/ur/rt_state.h | 149 +- include/ur_modern_driver/ur/state.h | 43 +- include/ur_modern_driver/ur/state_parser.h | 131 +- include/ur_modern_driver/ur/stream.h | 43 +- include/ur_modern_driver/ur_communication.h | 41 +- include/ur_modern_driver/ur_driver.h | 114 +- .../ur_modern_driver/ur_hardware_interface.h | 94 +- .../ur_realtime_communication.h | 59 +- src/do_output.cpp | 24 +- src/robot_state.cpp | 577 ++++--- src/robot_state_RT.cpp | 665 ++++---- src/ros/rt_publisher.cpp | 151 +- 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 | 142 +- src/ur/state.cpp | 28 +- src/ur/stream.cpp | 175 +- src/ur_communication.cpp | 291 ++-- src/ur_driver.cpp | 602 +++---- src/ur_hardware_interface.cpp | 367 ++-- src/ur_realtime_communication.cpp | 305 ++-- src/ur_ros_wrapper.cpp | 1504 +++++++++-------- 44 files changed, 5097 insertions(+), 4891 deletions(-) diff --git a/.clang-format b/.clang-format index 95f7eb5..e03f80b 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 -SpacesInAngles: false -SpacesInContainerLiterals: true -SpacesInCStyleCastParentheses: false -SpacesInParentheses: false -SpacesInSquareBrackets: false -Standard: Cpp03 -TabWidth: 8 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +PointerBindsToType: false +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 UseTab: Never +BreakBeforeBraces: Allman +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +ContinuationIndentWidth: 4 ... - diff --git a/include/ur_modern_driver/bin_parser.h b/include/ur_modern_driver/bin_parser.h index 2bc102e..a4f9372 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(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(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; - } + ~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); - } + // 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 + 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); - } + 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)); - } + 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; - } + // 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(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); - } + // 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_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; - } + 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)); - } + // 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]) + template + void parse(T (&array)[N]) + { + for (size_t i = 0; i < N; i++) { - for (size_t i = 0; i < N; i++) { - parse(array[i]); - } + parse(array[i]); } + } - void consume() - { - _buf_pos = _buf_end; - } - void consume(size_t bytes) - { - _buf_pos += bytes; - } + 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 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; - } + bool empty() + { + return buf_pos_ == buf_end_; + } - void debug() - { - LOG_DEBUG("BinParser: %p - %p (%zu bytes)", _buf_pos, _buf_end, _buf_end - _buf_pos); - } + 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 541e3e5..521d75b 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 9997515..e824c57 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 199345a..49b1c9e 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 void setupConsumer() + { + } + virtual void teardownConsumer() + { + } + virtual void stopConsumer() + { + } - virtual bool consume(unique_ptr product) = 0; + 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 void setupProducer() + { + } + virtual void teardownProducer() + { + } + virtual void stopProducer() + { + } - virtual bool try_get(std::vector >& products) = 0; + virtual bool tryGet(std::vector>& products) = 0; }; template -class Pipeline { +class Pipeline +{ private: - IProducer& _producer; - IConsumer& _consumer; - BlockingReaderWriterQueue > _queue; - atomic _running; - thread _pThread, _cThread; + IProducer& producer_; + IConsumer& consumer_; + BlockingReaderWriterQueue> queue_; + atomic running_; + thread pThread_, cThread_; - void run_producer() + void run_producer() + { + producer_.setupProducer(); + std::vector> products; + while (running_) { - _producer.setup_producer(); - std::vector > products; - while (_running) { - if (!_producer.try_get(products)) { - break; - } + if (!producer_.tryGet(products)) + { + break; + } - for (auto& p : products) { - if (!_queue.try_enqueue(std::move(p))) { - LOG_ERROR("Pipeline producer owerflowed!"); - } - } - - products.clear(); + 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 af0089e..8cbf463 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 { +namespace moodycamel +{ +enum memory_order +{ + memory_order_relaxed, + memory_order_acquire, + memory_order_release, + memory_order_acq_rel, + memory_order_seq_cst, -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_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; - } + 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 load() const + { + return value; + } - AE_FORCEINLINE T fetch_add_acquire(T increment) - { + AE_FORCEINLINE T fetch_add_acquire(T increment) + { #if defined(AE_ARCH_X64) || defined(AE_ARCH_X86) - if (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; - } + 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 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 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_acquire(T increment) + { + return value.fetch_add(increment, std::memory_order_acquire); + } - AE_FORCEINLINE T fetch_add_release(T increment) - { - return value.fetch_add(increment, std::memory_order_release); - } + AE_FORCEINLINE T 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 @@ private: // namespace with thousands of generic names or adding a .cpp for nothing. extern "C" { struct _SECURITY_ATTRIBUTES; -__declspec(dllimport) void* __stdcall CreateSemaphoreW(_SECURITY_ATTRIBUTES* lpSemaphoreAttributes, long lInitialCount, long lMaximumCount, const wchar_t* lpName); +__declspec(dllimport) void* __stdcall CreateSemaphoreW(_SECURITY_ATTRIBUTES* lpSemaphoreAttributes, long lInitialCount, + long lMaximumCount, const wchar_t* lpName); __declspec(dllimport) int __stdcall CloseHandle(void* hObject); __declspec(dllimport) unsigned long __stdcall WaitForSingleObject(void* hHandle, unsigned long dwMilliseconds); __declspec(dllimport) int __stdcall ReleaseSemaphore(void* hSemaphore, long lReleaseCount, long* lpPreviousCount); @@ -398,7 +412,8 @@ __declspec(dllimport) int __stdcall ReleaseSemaphore(void* hSemaphore, long lRel #include #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; +class Semaphore +{ +private: + void* m_hSema; - Semaphore(const Semaphore& other); - Semaphore& operator=(const Semaphore& other); + Semaphore(const Semaphore& other); + Semaphore& operator=(const Semaphore& other); - public: - Semaphore(int initialCount = 0) - { - assert(initialCount >= 0); - const long maxLong = 0x7fffffff; - m_hSema = CreateSemaphoreW(nullptr, initialCount, maxLong, nullptr); - } +public: + Semaphore(int initialCount = 0) + { + assert(initialCount >= 0); + const long maxLong = 0x7fffffff; + m_hSema = CreateSemaphoreW(nullptr, initialCount, maxLong, nullptr); + } - ~Semaphore() - { - CloseHandle(m_hSema); - } + ~Semaphore() + { + CloseHandle(m_hSema); + } - void wait() - { - const unsigned long infinite = 0xffffffff; - WaitForSingleObject(m_hSema, infinite); - } + 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 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; - } + 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); - } - }; + void signal(int count = 1) + { + ReleaseSemaphore(m_hSema, count, nullptr); + } +}; #elif defined(__MACH__) - //--------------------------------------------------------- - // Semaphore (Apple iOS and OSX) - // Can't use POSIX semaphores due to http://lists.apple.com/archives/darwin-kernel/2009/Apr/msg00010.html - //--------------------------------------------------------- - class Semaphore { - private: - semaphore_t m_sema; +//--------------------------------------------------------- +// Semaphore (Apple iOS and OSX) +// Can't use POSIX semaphores due to http://lists.apple.com/archives/darwin-kernel/2009/Apr/msg00010.html +//--------------------------------------------------------- +class Semaphore +{ +private: + semaphore_t m_sema; - Semaphore(const Semaphore& other); - Semaphore& operator=(const Semaphore& other); + Semaphore(const Semaphore& other); + Semaphore& operator=(const Semaphore& other); - public: - Semaphore(int initialCount = 0) - { - assert(initialCount >= 0); - semaphore_create(mach_task_self(), &m_sema, SYNC_POLICY_FIFO, initialCount); - } +public: + Semaphore(int initialCount = 0) + { + assert(initialCount >= 0); + semaphore_create(mach_task_self(), &m_sema, SYNC_POLICY_FIFO, initialCount); + } - ~Semaphore() - { - semaphore_destroy(mach_task_self(), m_sema); - } + ~Semaphore() + { + semaphore_destroy(mach_task_self(), m_sema); + } - void wait() - { - semaphore_wait(m_sema); - } + void wait() + { + semaphore_wait(m_sema); + } - bool try_wait() - { - return timed_wait(0); - } + bool try_wait() + { + return timed_wait(0); + } - bool timed_wait(std::int64_t timeout_usecs) - { - mach_timespec_t ts; - ts.tv_sec = timeout_usecs / 1000000; - ts.tv_nsec = (timeout_usecs % 1000000) * 1000; + bool timed_wait(std::int64_t timeout_usecs) + { + mach_timespec_t ts; + ts.tv_sec = timeout_usecs / 1000000; + ts.tv_nsec = (timeout_usecs % 1000000) * 1000; - // added in OSX 10.10: https://developer.apple.com/library/prerelease/mac/documentation/General/Reference/APIDiffsMacOSX10_10SeedDiff/modules/Darwin.html - kern_return_t rc = semaphore_timedwait(m_sema, ts); + // added in OSX 10.10: + // https://developer.apple.com/library/prerelease/mac/documentation/General/Reference/APIDiffsMacOSX10_10SeedDiff/modules/Darwin.html + kern_return_t rc = semaphore_timedwait(m_sema, ts); - return rc != KERN_OPERATION_TIMED_OUT; - } + return rc != KERN_OPERATION_TIMED_OUT; + } - void signal() - { - semaphore_signal(m_sema); - } + void signal() + { + semaphore_signal(m_sema); + } - void signal(int count) - { - while (count-- > 0) { - semaphore_signal(m_sema); - } - } - }; + void signal(int count) + { + while (count-- > 0) + { + semaphore_signal(m_sema); + } + } +}; #elif defined(__unix__) - //--------------------------------------------------------- - // Semaphore (POSIX, Linux) - //--------------------------------------------------------- - class Semaphore { - private: - sem_t m_sema; +//--------------------------------------------------------- +// Semaphore (POSIX, Linux) +//--------------------------------------------------------- +class Semaphore +{ +private: + sem_t m_sema; - Semaphore(const Semaphore& other); - Semaphore& operator=(const Semaphore& other); + Semaphore(const Semaphore& other); + Semaphore& operator=(const Semaphore& other); - public: - Semaphore(int initialCount = 0) - { - assert(initialCount >= 0); - sem_init(&m_sema, 0, initialCount); - } +public: + Semaphore(int initialCount = 0) + { + assert(initialCount >= 0); + sem_init(&m_sema, 0, initialCount); + } - ~Semaphore() - { - sem_destroy(&m_sema); - } + ~Semaphore() + { + sem_destroy(&m_sema); + } - void wait() - { - // http://stackoverflow.com/questions/2013181/gdb-causes-sem-wait-to-fail-with-eintr-error - int rc; - do { - rc = sem_wait(&m_sema); - } while (rc == -1 && errno == EINTR); - } + void wait() + { + // http://stackoverflow.com/questions/2013181/gdb-causes-sem-wait-to-fail-with-eintr-error + int rc; + do + { + rc = sem_wait(&m_sema); + } while (rc == -1 && errno == EINTR); + } - bool try_wait() - { - int rc; - do { - rc = sem_trywait(&m_sema); - } while (rc == -1 && errno == EINTR); - return !(rc == -1 && errno == EAGAIN); - } + bool try_wait() + { + int rc; + do + { + rc = sem_trywait(&m_sema); + } while (rc == -1 && errno == EINTR); + return !(rc == -1 && errno == EAGAIN); + } - bool timed_wait(std::uint64_t usecs) - { - struct timespec ts; - const int usecs_in_1_sec = 1000000; - const int nsecs_in_1_sec = 1000000000; - clock_gettime(CLOCK_REALTIME, &ts); - ts.tv_sec += usecs / usecs_in_1_sec; - ts.tv_nsec += (usecs % usecs_in_1_sec) * 1000; - // sem_timedwait bombs if you have more than 1e9 in tv_nsec - // so we have to clean things up before passing it in - if (ts.tv_nsec > nsecs_in_1_sec) { - ts.tv_nsec -= nsecs_in_1_sec; - ++ts.tv_sec; - } + bool timed_wait(std::uint64_t usecs) + { + struct timespec ts; + const int usecs_in_1_sec = 1000000; + const int nsecs_in_1_sec = 1000000000; + clock_gettime(CLOCK_REALTIME, &ts); + ts.tv_sec += usecs / usecs_in_1_sec; + ts.tv_nsec += (usecs % usecs_in_1_sec) * 1000; + // sem_timedwait bombs if you have more than 1e9 in tv_nsec + // so we have to clean things up before passing it in + if (ts.tv_nsec > nsecs_in_1_sec) + { + ts.tv_nsec -= nsecs_in_1_sec; + ++ts.tv_sec; + } - int rc; - do { - rc = sem_timedwait(&m_sema, &ts); - } while (rc == -1 && errno == EINTR); - return !(rc == -1 && errno == ETIMEDOUT); - } + int rc; + do + { + rc = sem_timedwait(&m_sema, &ts); + } while (rc == -1 && errno == EINTR); + return !(rc == -1 && errno == ETIMEDOUT); + } - void signal() - { - sem_post(&m_sema); - } + void signal() + { + sem_post(&m_sema); + } - void signal(int count) - { - while (count-- > 0) { - sem_post(&m_sema); - } - } - }; + void signal(int count) + { + while (count-- > 0) + { + sem_post(&m_sema); + } + } +}; #else #error Unsupported platform! (No semaphore wrapper available) #endif - //--------------------------------------------------------- - // LightweightSemaphore - //--------------------------------------------------------- - class LightweightSemaphore { - public: - typedef std::make_signed::type ssize_t; +//--------------------------------------------------------- +// LightweightSemaphore +//--------------------------------------------------------- +class LightweightSemaphore +{ +public: + typedef std::make_signed::type ssize_t; - private: - weak_atomic m_count; - Semaphore m_sema; +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; - } - } + 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); - } +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; - } + bool tryWait() + { + if (m_count.load() > 0) + { + m_count.fetch_add_acquire(-1); + return true; + } + return false; + } - void wait() - { - if (!tryWait()) - waitWithPartialSpinning(); - } + void wait() + { + if (!tryWait()) + waitWithPartialSpinning(); + } - bool wait(std::int64_t timeout_usecs) - { - return tryWait() || waitWithPartialSpinning(timeout_usecs); - } + 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); - } - } + 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 + 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 1503dcb..fdfcdf8 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) { -#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) + // 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(!inSection && "ReaderWriterQueue does not support enqueuing or dequeuing elements from other elements' ctors and dtors"); - inSection = true; +#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; - ~ReentrantGuard() { inSection = false; } + // Make sure the reader/writer threads will have the initialized memory setup above: + fence(memory_order_sync); + } - private: - ReentrantGuard& operator=(ReentrantGuard const&); + // 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); - private: - bool& inSection; - }; + // 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 - 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 + // 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 - char cachelineFiller0[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic) - sizeof(size_t)]; - weak_atomic tail; // (Atomic) Elements are enqueued here - size_t localFront; + // 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. - 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) + Block* frontBlock_ = frontBlock.load(); + size_t blockTail = frontBlock_->localTail; + size_t blockFront = frontBlock_->front.load(); - 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) + if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load())) { - // 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; - } + fence(memory_order_acquire); - auto newBlockAligned = align_for(newBlockRaw); - auto newBlockData = align_for(newBlockAligned + sizeof(Block)); - return new (newBlockAligned) Block(capacity, newBlockRaw, newBlockData); + 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: - weak_atomic frontBlock; // (Atomic) Elements are enqueued to this block + enum AllocationMode + { + CanAlloc, + CannotAlloc + }; - char cachelineFiller[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic)]; - weak_atomic tailBlock; // (Atomic) Elements are dequeued from this block + template + bool inner_enqueue(U&& element) + { +#ifndef NDEBUG + ReentrantGuard guard(this->enqueuing); +#endif - size_t largestBlockSize; + // 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; + 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) - { - } + 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) + // 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; } + 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) + // 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; } + 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) + // 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; + 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) + // 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 (inner.enqueue(std::forward(element))) { - sema.signal(); - return true; - } - return false; + 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) + // 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()) { - if (sema.tryWait()) { - bool success = inner.try_dequeue(result); - assert(success); - AE_UNUSED(success); - return true; - } - return false; + 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, 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) + // 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() - { - return inner.peek(); - } + // Returns a pointer to the front element in the queue (the one that + // would be removed next by a call to `try_dequeue` or `pop`). If the + // queue appears empty at the time the method is called, nullptr is + // returned instead. + // Must be called only from the consumer thread. + AE_FORCEINLINE T* peek() + { + return inner.peek(); + } - // Removes the front element from the queue, if any, without returning it. - // Returns true on success, or false if the queue appeared empty at the time - // `pop` was called. - AE_FORCEINLINE bool pop() + // 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()) { - if (sema.tryWait()) { - bool result = inner.pop(); - assert(result); - AE_UNUSED(result); - return true; - } - return false; + 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 a35da44..621926a 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(); + RobotState(std::condition_variable& msg_cond); + ~RobotState(); + double getVersion(); + double getTime(); + std::vector getQTarget(); + int getDigitalInputBits(); + int getDigitalOutputBits(); + char getAnalogInputRange0(); + char getAnalogInputRange1(); + double getAnalogInput0(); + double getAnalogInput1(); + char getAnalogOutputDomain0(); + char getAnalogOutputDomain1(); + double getAnalogOutput0(); + double getAnalogOutput1(); + std::vector getVActual(); + float getMasterBoardTemperature(); + float getRobotVoltage48V(); + float getRobotCurrent(); + float getMasterIOCurrent(); + unsigned char getSafetyMode(); + unsigned char getInReducedMode(); + char getEuromap67InterfaceInstalled(); + int getEuromapInputBits(); + int getEuromapOutputBits(); + float getEuromapVoltage(); + float getEuromapCurrent(); - bool isRobotConnected(); - bool isRealRobotEnabled(); - bool isPowerOnRobot(); - bool isEmergencyStopped(); - bool isProtectiveStopped(); - bool isProgramRunning(); - bool isProgramPaused(); - unsigned char getRobotMode(); - bool isReady(); + bool isRobotConnected(); + bool isRealRobotEnabled(); + bool isPowerOnRobot(); + bool isEmergencyStopped(); + bool isProtectiveStopped(); + bool isProgramRunning(); + bool isProgramPaused(); + unsigned char getRobotMode(); + bool isReady(); - void setDisconnected(); + void setDisconnected(); - bool getNewDataAvailable(); - void finishedReading(); + bool getNewDataAvailable(); + void finishedReading(); - void unpack(uint8_t* buf, unsigned int buf_length); - void unpackRobotMessage(uint8_t* buf, unsigned int offset, uint32_t len); - void unpackRobotMessageVersion(uint8_t* buf, unsigned int offset, - uint32_t len); - void unpackRobotState(uint8_t* buf, unsigned int offset, uint32_t len); - void unpackRobotStateMasterboard(uint8_t* buf, unsigned int offset); - void unpackRobotMode(uint8_t* buf, unsigned int offset); + void unpack(uint8_t* buf, unsigned int buf_length); + void unpackRobotMessage(uint8_t* buf, unsigned int offset, uint32_t len); + void unpackRobotMessageVersion(uint8_t* buf, unsigned int offset, uint32_t len); + void unpackRobotState(uint8_t* buf, unsigned int offset, uint32_t len); + void unpackRobotStateMasterboard(uint8_t* buf, unsigned int offset); + void unpackRobotMode(uint8_t* buf, unsigned int offset); }; #endif /* ROBOT_STATE_H_ */ diff --git a/include/ur_modern_driver/robot_state_RT.h b/include/ur_modern_driver/robot_state_RT.h index 3d3b2e8..bca6fac 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 0870526..a3fbb78 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 86d3a24..34cb905 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 655e8bb..991401a 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(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; + 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(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(MasterBoardData_V1_X& data) = 0; + virtual bool consume(MasterBoardData_V3_0__1& data) = 0; + virtual bool consume(MasterBoardData_V3_2& data) = 0; - virtual bool consume(RobotModeData_V1_X& data) = 0; - virtual bool consume(RobotModeData_V3_0__1& data) = 0; - virtual bool consume(RobotModeData_V3_2& data) = 0; + virtual bool consume(RobotModeData_V1_X& data) = 0; + virtual bool consume(RobotModeData_V3_0__1& data) = 0; + virtual bool consume(RobotModeData_V3_2& data) = 0; }; -class URMessagePacketConsumer : public IConsumer { +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 d3266ea..83d89ab 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) + URFactory(std::string& host) : stream_(host, 30001) + { + URProducer prod(stream_, parser_); + std::vector> results; + + prod.setupProducer(); + + if (!prod.tryGet(results) || results.size() == 0) { - 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(); + LOG_FATAL("No version message received, init failed!"); + std::exit(EXIT_FAILURE); } - std::unique_ptr > get_state_parser() + for (auto const& p : results) { - 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); - } + p->consumeWith(*this); } - std::unique_ptr > get_rt_parser() + if (major_version_ == 0 && minor_version_ == 0) { - 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); - } + LOG_FATAL("No version message received, init failed!"); + std::exit(EXIT_FAILURE); } + + prod.teardownProducer(); + } + + std::unique_ptr> getStateParser() + { + 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> 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 (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 5f23397..a0d9e7a 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); + 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; + 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; + bool euromap67_interface_installed; - //euromap fields are dynamic so don't include in SIZE - int32_t euromap_input_bits; - int32_t euromap_output_bits; + // 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 SIZE = sizeof(double) * 4 + sizeof(int8_t) * 4 + sizeof(float) * 4 + sizeof(uint8_t); - static const size_t EURO_SIZE = sizeof(int32_t) * 2; + 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 dafad61..b693c1e 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; + 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; + uint64_t timestamp; + uint8_t source; }; -class VersionMessage : public MessagePacket { +class VersionMessage : public MessagePacket +{ public: - VersionMessage(uint64_t timestamp, uint8_t source) - : MessagePacket(timestamp, source) - { - } + VersionMessage(uint64_t timestamp, uint8_t source) : MessagePacket(timestamp, source) + { + } - virtual bool parse_with(BinParser& bp); - virtual bool consume_with(URMessagePacketConsumer& consumer); + virtual bool parseWith(BinParser& bp); + virtual bool consumeWith(URMessagePacketConsumer& consumer); - std::string project_name; - uint8_t major_version; - uint8_t minor_version; - int32_t svn_version; - std::string build_date; + 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 b0a781f..80d5806 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 ea53b40..4eb9026 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 7db11c9..5907f97 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; + } + else if (len < 0) + { + LOG_WARN("Stream closed"); + return false; } - 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 == 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); - } + 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 b5307b9..90c9eb8 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 e0cde0d..952cd56 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) + bool parse(BinParser& bp, std::vector>& results) + { + int32_t packet_size = bp.peek(); + + if (!bp.checkSize(packet_size)) { - 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; + 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->parseWith(bp)) + return false; + + results.push_back(std::move(packet)); + + 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 ea2dd5e..bb4e9e9 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]; + 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 + // gap here depending on version - double tcp_force[6]; + 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; + // 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 + // gap here depending on version - uint64_t digital_inputs; - double motor_temperatures[6]; - double controller_time; - double robot_mode; + 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); + 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); + 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 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]; + 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 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!"); + 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 0ac8e69..b76d34e 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 a68a611..031efb2 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) + 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) { - 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; + LOG_WARN("Invalid message type recieved: %u", static_cast(type)); + return false; } + + 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 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->parseWith(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; diff --git a/include/ur_modern_driver/ur/stream.h b/include/ur_modern_driver/ur/stream.h index 7d7363b..6990c4a 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 cad98e1..29460c5 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 1d414c1..dbfb3bd 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 +#include +#include +#include +#include +#include +#include +#include #include "do_output.h" #include "ur_communication.h" #include "ur_realtime_communication.h" -#include -#include -#include -#include -#include -#include -#include -#include #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 0724703..b2a7db1 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_; + // Startup and shutdown of the internal node inside a roscpp program + ros::NodeHandle nh_; - // Interfaces - hardware_interface::JointStateInterface joint_state_interface_; - hardware_interface::ForceTorqueSensorInterface force_torque_interface_; - hardware_interface::PositionJointInterface position_joint_interface_; - hardware_interface::VelocityJointInterface velocity_joint_interface_; - bool velocity_interface_running_; - bool position_interface_running_; - // Shared memory - std::vector joint_names_; - std::vector joint_position_; - std::vector joint_velocity_; - std::vector joint_effort_; - std::vector joint_position_command_; - std::vector joint_velocity_command_; - std::vector prev_joint_velocity_command_; - std::size_t num_joints_; - double robot_force_[3] = { 0., 0., 0. }; - double robot_torque_[3] = { 0., 0., 0. }; + // Interfaces + hardware_interface::JointStateInterface joint_state_interface_; + hardware_interface::ForceTorqueSensorInterface force_torque_interface_; + hardware_interface::PositionJointInterface position_joint_interface_; + hardware_interface::VelocityJointInterface velocity_joint_interface_; + bool velocity_interface_running_; + bool position_interface_running_; + // Shared memory + std::vector joint_names_; + std::vector joint_position_; + std::vector joint_velocity_; + std::vector joint_effort_; + std::vector joint_position_command_; + std::vector joint_velocity_command_; + std::vector prev_joint_velocity_command_; + std::size_t num_joints_; + double robot_force_[3] = { 0., 0., 0. }; + double robot_torque_[3] = { 0., 0., 0. }; - double max_vel_change_; + double max_vel_change_; - // Robot API - UrDriver* robot_; + // 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 61e5474..1de9fdb 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 f4b0b40..765e9b4 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 743c2c2..786517c 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; + 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(); - version_msg_.timestamp = timestamp; - version_msg_.source = source; - version_msg_.robot_message_type = robot_message_type; - RobotState::unpackRobotMessageVersion(buf, offset, len); + RobotState::unpackRobotMode(buf, offset + 5); val_lock_.unlock(); break; - default: + + 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) +void RobotState::unpackRobotMessageVersion(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; - } + 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_.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); } - - 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); - } + 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 46467fe..71bb7ba 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) +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; + 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; - } - - 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); - } + if (!len_good) + { + printf("Wrong length of message on RT interface: %i\n", len); val_lock_.unlock(); - controller_updated_ = true; - data_published_ = true; - pMsg_cond_->notify_all(); + 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(); } diff --git a/src/ros/rt_publisher.cpp b/src/ros/rt_publisher.cpp index d58b023..26a283b 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; + 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); - } + 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); + joint_pub_.publish(joint_msg); - return true; + 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]; + 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; + 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; + 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; + 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]; + 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; + 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 adf7a99..09b142a 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 046d0ee..ecada69 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 b532792..5cfc440 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 be0770f..5bee4cf 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 2264835..18d6822 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; + if (!bp.checkSize()) + return false; - parse_shared1(bp); + 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); + 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); + 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); + 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; + 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 0b4f4f2..66af615 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 955d1f3..819651e 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); + // 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 (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"); + 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; + } - return _initialized; + freeaddrinfo(result); + if (!initialized_) + LOG_ERROR("Connection failed"); + + 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; + if (!initialized_) + return -1; + if (stopping_) + return 0; - size_t total = 0; - size_t remaining = buf_len; + 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; - } + // 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; + 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; + } + initial = false; + } - size_t remainder = sizeof(int32_t); - uint8_t* buf_pos = buf; - bool initial = true; + buf_pos += read; + remainder -= read; + } while (remainder > 0); - 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; + return buf_pos - buf; } \ No newline at end of file diff --git a/src/ur_communication.cpp b/src/ur_communication.cpp index b883004..d7da04f 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; - - 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"); - } - } - } + 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; - //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_); + 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 index 08a535a..c5cf3bd 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 152452c..06b59f9 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 { - // 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()); + 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 b45224b..e947aa9 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_); - } - } - 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"); - } - } + 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_); + } } - setSpeed(0., 0., 0., 0., 0., 0.); - 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; + 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 92fe341..6f3f854 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) + 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); - std::string joint_prefix = ""; - std::vector joint_names; - char buf[256]; + use_ros_control_ = false; + ros::param::get("~use_ros_control", use_ros_control_); - 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); - } + 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); } - void halt() + // 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)) { - robot_.halt(); - rt_publish_thread_->join(); + 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) + void trajThread(std::vector timestamps, std::vector> positions, + std::vector> velocities) + { + robot_.doTraj(timestamps, positions, velocities); + if (has_goal_) { - - robot_.doTraj(timestamps, positions, velocities); - if (has_goal_) { - result_.error_code = result_.SUCCESSFUL; - goal_handle_.setSucceeded(result_); - has_goal_ = false; - } + result_.error_code = result_.SUCCESSFUL; + goal_handle_.setSucceeded(result_); + has_goal_ = false; } - void goalCB( - actionlib::ServerGoalHandle gh) + } + void goalCB(actionlib::ServerGoalHandle gh) + { + std::string buf; + print_info("on_goal"); + if (!robot_.sec_interface_->robot_state_->isReady()) { - std::string buf; - print_info("on_goal"); - if (!robot_.sec_interface_->robot_state_->isReady()) { - result_.error_code = -100; //nothing is defined for this...? + result_.error_code = -100; // nothing is defined for this...? - if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) { - 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(); + 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; } - void cancelCB( - actionlib::ServerGoalHandle gh) + actionlib::ActionServer::Goal goal = + *gh.getGoal(); // make a copy that we can modify + if (has_goal_) { - // 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_); + 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; } - bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp) + if (!has_velocities()) { - 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; + 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; } - bool setPayload(ur_msgs::SetPayloadRequest& req, - ur_msgs::SetPayloadResponse& resp) + if (!traj_is_finite()) { - if (robot_.setPayload(req.payload)) - resp.success = true; + 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 - 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; - } + { + 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]; - return true; + 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 reorder_traj_joints(trajectory_msgs::JointTrajectory& traj) + 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()) { - /* 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; - } + 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); - bool has_velocities() + // 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()) { - 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; + 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!"); } - 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; + else if (robot_.sec_interface_->robot_state_->isProtectiveStopped() and !warned) + { + print_error("Robot is protective stopped!"); } - 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(); + 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; + 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!!"); + 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("~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); - } + else + { + print_fatal("Could not get robot ip. Please supply it as command line parameter or on the parameter server as " + "robot_ip"); + exit(1); } - if ((ros::param::get("~reverse_port", reverse_port))) { - if ((reverse_port <= 0) or (reverse_port >= 65535)) { - print_warning("Reverse port value is not valid (Use number between 1 and 65534. Using default value of 50001"); - reverse_port = 50001; - } - } else - reverse_port = 50001; + } + if ((ros::param::get("~reverse_port", reverse_port))) + { + if ((reverse_port <= 0) or (reverse_port >= 65535)) + { + print_warning("Reverse port value is not valid (Use number between 1 and 65534. Using default value of 50001"); + reverse_port = 50001; + } + } + else + reverse_port = 50001; - RosWrapper interface(host, reverse_port); + RosWrapper interface(host, reverse_port); - ros::AsyncSpinner spinner(3); - spinner.start(); + ros::AsyncSpinner spinner(3); + spinner.start(); - ros::waitForShutdown(); + ros::waitForShutdown(); - interface.halt(); + interface.halt(); - exit(0); + exit(0); } 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 66af615..0000000 --- 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 e9f3b83..7f1badf 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 0000000..14ae65b --- /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 0000000..edbb531 --- /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 0000000..92cbc41 --- /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 0000000..e69de29 diff --git a/tests/ur/robot_mode.cpp b/tests/ur/robot_mode.cpp new file mode 100644 index 0000000..170e0cd --- /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 0000000..c24c72a --- /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 a4f9372..16378a0 100644 --- a/include/ur_modern_driver/bin_parser.h +++ b/include/ur_modern_driver/bin_parser.h @@ -136,7 +136,7 @@ public: } 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 bb4e9e9..fdc93bc 100644 --- a/include/ur_modern_driver/ur/rt_state.h +++ b/include/ur_modern_driver/ur/rt_state.h @@ -23,18 +23,18 @@ protected: 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 @@ public: // 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 @@ public: 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 @@ public: 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 @@ public: 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 16378a0..e5692f5 100644 --- a/include/ur_modern_driver/bin_parser.h +++ b/include/ur_modern_driver/bin_parser.h @@ -136,7 +136,7 @@ public: } 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 edbb531..83b6684 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 170e0cd..a13fcb8 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 c24c72a..5c7920e 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 e5692f5..dbf6df5 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 @@ public: } } + 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 a0d9e7a..c625cbf 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 @@ public: 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 @@ public: 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 ecada69..991b6ac 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 49b1c9e..9f487b1 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 @@ public: { } - 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 @@ private: { // 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 991401a..6617a80 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 @@ public: 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 @@ public: 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 b76d34e..4aeddba 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 6990c4a..eac3c09 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 @@ private: 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 @@ public: 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 819651e..240b261 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 031efb2..1e93314 100644 --- a/include/ur_modern_driver/ur/state_parser.h +++ b/include/ur_modern_driver/ur/state_parser.h @@ -33,6 +33,13 @@ public: 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 @@ public: 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 @@ public: 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 @@ public: 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 0000000..6325f8c --- /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 0000000..d9e16d2 --- /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 26a283b..f9ab2ab 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 0000000..e970e30 --- /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 0000000..be59376 --- /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 0000000..11c4d8b --- /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 0000000..9664432 --- /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 0000000..ad9f2ae --- /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 0000000..1177be5 --- /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 0000000..cddcaff --- /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 7f1badf..944df13 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 0000000..2c8ff31 --- /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 83d89ab..83f0cd6 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 @@ private: } 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 09b142a..1c660dd 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 @@ public: 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(); + //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; - URStream s2(args.host, 30003); - URProducer p2(s2, *parser); - RTPublisher pub(args.prefix, args.base_frame, args.tool_frame); + URCommander rt_commander(rt_stream); + vector*> rt_vec; - Pipeline pl(p2, pub); - - 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 6f70f09..0000000 --- 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 e970e30..dfb36c7 100644 --- a/include/ur_modern_driver/ros/hardware_interface.h +++ b/include/ur_modern_driver/ros/hardware_interface.h @@ -48,10 +48,11 @@ public: 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 @@ public: 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 @@ public: 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 9664432..5e7c566 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 @@ private: 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 @@ public: , 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 a3fbb78..f4945d9 100644 --- a/include/ur_modern_driver/ros/rt_publisher.h +++ b/include/ur_modern_driver/ros/rt_publisher.h @@ -31,6 +31,7 @@ private: 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 @@ private: 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 cddcaff..fce866d 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 f9ab2ab..65d0726 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 1c660dd..b08e90b 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 944df13..1de2725 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 0000000..dd11de4 --- /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 0000000..eb940d0 --- /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 dfb36c7..0c6618d 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 be59376..36ce662 100644 --- a/include/ur_modern_driver/ros/io_service.h +++ b/include/ur_modern_driver/ros/io_service.h @@ -35,18 +35,18 @@ private: 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 5e7c566..47ef455 100644 --- a/include/ur_modern_driver/ros/robot_hardware.h +++ b/include/ur_modern_driver/ros/robot_hardware.h @@ -29,11 +29,11 @@ private: } 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 ad9f2ae..0000000 --- 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 0000000..af09dfa --- /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 0000000..a1df97f --- /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 6325f8c..da92ae8 100644 --- a/include/ur_modern_driver/ur/commander.h +++ b/include/ur_modern_driver/ur/commander.h @@ -17,11 +17,12 @@ public: { } + 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 90c9eb8..6c7c641 100644 --- a/include/ur_modern_driver/ur/robot_mode.h +++ b/include/ur_modern_driver/ur/robot_mode.h @@ -16,6 +16,7 @@ public: 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 @@ public: 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 @@ public: 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 0000000..8740806 --- /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 eac3c09..16719d7 100644 --- a/include/ur_modern_driver/ur/stream.h +++ b/include/ur_modern_driver/ur/stream.h @@ -19,15 +19,38 @@ private: 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 0000000..3b6637d --- /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 0000000..753f28d --- /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 0000000..1bf0cc4 --- /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 fce866d..008daec 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 0000000..53cb43c --- /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 0000000..ca36e59 --- /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 b08e90b..7f76792 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 @@ public: 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 d9e16d2..333bc52 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 5bee4cf..da3a65b 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 0000000..d99f1a9 --- /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 a13fcb8..4209191 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 | 57 ++--- 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 | 164 +++++++++---- src/ur/server.cpp | 75 +++--- src/ur/stream.cpp | 129 ++-------- 22 files changed, 825 insertions(+), 423 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 1de2725..be59d00 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 9f487b1..0e5cfdb 100644 --- a/include/ur_modern_driver/pipeline.h +++ b/include/ur_modern_driver/pipeline.h @@ -23,6 +23,9 @@ public: virtual void stopConsumer() { } + virtual void onTimeout() + { + } virtual bool consume(shared_ptr product) = 0; }; @@ -59,6 +62,13 @@ public: 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 @@ private: { 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 @@ private: // 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 dd11de4..398e4d5 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 @@ private: 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 @@ private: 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 eb940d0..3532ee6 100644 --- a/include/ur_modern_driver/ros/controller.h +++ b/include/ur_modern_driver/ros/controller.h @@ -34,6 +34,7 @@ private: std::map available_interfaces_; std::atomic service_enabled_; + std::atomic service_cooldown_; // helper functions to map interfaces template @@ -51,6 +52,7 @@ private: 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 0c6618d..325faa4 100644 --- a/include/ur_modern_driver/ros/hardware_interface.h +++ b/include/ur_modern_driver/ros/hardware_interface.h @@ -13,6 +13,7 @@ public: virtual bool write() = 0; virtual void start() {} virtual void stop() {} + virtual void reset() {} }; using hardware_interface::JointHandle; @@ -48,6 +49,7 @@ private: 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 36ce662..03d9935 100644 --- a/include/ur_modern_driver/ros/io_service.h +++ b/include/ur_modern_driver/ros/io_service.h @@ -37,6 +37,8 @@ private: 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 a1df97f..a16c3e5 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 @@ private: 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 0000000..303d49e --- /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 da92ae8..5d81698 100644 --- a/include/ur_modern_driver/ur/commander.h +++ b/include/ur_modern_driver/ur/commander.h @@ -11,18 +11,46 @@ private: 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 83f0cd6..0b4e6ca 100644 --- a/include/ur_modern_driver/ur/factory.h +++ b/include/ur_modern_driver/ur/factory.h @@ -71,6 +71,19 @@ public: 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 5907f97..b3bb475 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 @@ public: { // 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 8740806..dba5fe5 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 16719d7..e3bbb23 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_; +protected: + virtual bool open(int socket_fd, struct sockaddr *address, size_t address_len) + { + return ::connect(socket_fd, address, address_len) == 0; + } public: - URStream() + URStream(std::string& host, int port) : host_(host), port_(port) { } - URStream(std::string& host, int port) : host_(host), port_(port), initialized_(false), stopping_(false) + bool connect() { + return TCPSocket::setup(host_, port_); + } + void disconnect() + { + LOG_INFO("Disconnecting"); + TCPSocket::close(); } - URStream(int socket_fd) : socket_fd_(socket_fd), initialized_(true), stopping_(false) - { - - } + bool closed() { return getState() == SocketState::Closed; } - 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(); - - 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 3b6637d..629a50f 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(); + LOG_DEBUG("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()); - 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_) + 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 753f28d..79102fa 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 1bf0cc4..09a0798 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 ca36e59..53d966a 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}} + 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 + 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(!commander_.uploadProg(prog)) + if(!server_.bind()) + { + LOG_ERROR("Failed to bind server"); return false; + } - stream_ = std::move(server_.accept()); //todo: pointer instead? + 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; + } + + 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 7f76792..efaf106 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 0000000..01685bf --- /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 333bc52..0fd358e 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) -{ - 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) { - -} + 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::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); + 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 + std::ostringstream out; + out << "set_payload(" << std::fixed << std::setprecision(5) << value << ")\n"; + std::string s(out.str()); + return write(s); +} + +bool URCommander::stopj(double a) +{ + std::ostringstream out; + out << "stopj(" << std::fixed << std::setprecision(5) << a << ")\n"; + std::string s(out.str()); + return write(s); +} + + +bool URCommander_V1_X::speedj(std::array &speeds, double acceleration) +{ + 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_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_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; + 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 d99f1a9..6c99e13 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; - } - - LOG_ERROR("Failed to setup recieving server"); - std::exit(EXIT_FAILURE); } -URStream URServer::accept() +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(); + + if(!res) + return false; + + if(::listen(getSocketFD(), 1) < 0) + return false; + + return true; +} + +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 240b261..05617c5 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_; + std::lock_guard lock(write_mutex_); + return TCPSocket::write(buf, buf_len, written); } -void URStream::disconnect() +bool URStream::read(uint8_t* buf, size_t buf_len, size_t &total) { - if (!initialized_ || stopping_) - return; + std::lock_guard lock(read_mutex_); - LOG_INFO("Disconnecting from %s:%d", host_.c_str(), port_); - - stopping_ = true; - close(socket_fd_); - initialized_ = false; -} - -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; - - // 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_) - 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 | 31 +++--- src/ros/action_server.cpp | 100 +++++++++++++++++-- 2 files changed, 110 insertions(+), 21 deletions(-) diff --git a/include/ur_modern_driver/ros/action_server.h b/include/ur_modern_driver/ros/action_server.h index 398e4d5..bf876dc 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_; - + RobotState state_; + std::array q_actual_, qd_actual_; + GoalHandle curr_gh_; std::atomic interrupt_traj_; std::atomic has_goal_, running_; @@ -48,7 +48,7 @@ private: 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 @@ private: 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 629a50f..1ec7217 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 303d49e..9822424 100644 --- a/include/ur_modern_driver/tcp_socket.h +++ b/include/ur_modern_driver/tcp_socket.h @@ -25,6 +25,7 @@ protected: { 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 dba5fe5..c5c8412 100644 --- a/include/ur_modern_driver/ur/server.h +++ b/include/ur_modern_driver/ur/server.h @@ -20,6 +20,8 @@ protected: { 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 e3bbb23..60619a8 100644 --- a/include/ur_modern_driver/ur/stream.h +++ b/include/ur_modern_driver/ur/stream.h @@ -32,7 +32,7 @@ public: } 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 01685bf..f0049ec 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 6c99e13..fd929fe 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 bf876dc..f18c6e9 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 efaf106..38634c9 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 a16c3e5..74588af 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 53d966a..c6752b1 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 dbf6df5..2cfce45 100644 --- a/include/ur_modern_driver/bin_parser.h +++ b/include/ur_modern_driver/bin_parser.h @@ -68,7 +68,7 @@ public: 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 74588af..be59a56 100644 --- a/include/ur_modern_driver/ros/trajectory_follower.h +++ b/include/ur_modern_driver/ros/trajectory_follower.h @@ -49,12 +49,11 @@ private: 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 9822424..4230ac0 100644 --- a/include/ur_modern_driver/tcp_socket.h +++ b/include/ur_modern_driver/tcp_socket.h @@ -29,8 +29,6 @@ protected: bool setup(std::string &host, int port); - void close(); - public: TCPSocket(); @@ -41,6 +39,10 @@ public: 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 b3bb475..df4a61a 100644 --- a/include/ur_modern_driver/ur/producer.h +++ b/include/ur_modern_driver/ur/producer.h @@ -50,6 +50,10 @@ public: 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 c5c8412..daff4d3 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 @@ public: 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 1e93314..494558f 100644 --- a/include/ur_modern_driver/ur/state_parser.h +++ b/include/ur_modern_driver/ur/state_parser.h @@ -34,7 +34,7 @@ public: 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 @@ public: 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 @@ public: 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 @@ public: 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 60619a8..3d5bc81 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 1ec7217..fc3f7ad 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 c6752b1..099d84a 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 38634c9..dc5255c 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 f0049ec..a7f8cc4 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 fd929fe..1d53ed4 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 099d84a..78d448e 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 daff4d3..027e278 100644 --- a/include/ur_modern_driver/ur/server.h +++ b/include/ur_modern_driver/ur/server.h @@ -24,6 +24,7 @@ protected: 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 1d53ed4..5530be9 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 78d448e..0093cba 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 a7f8cc4..c4f1839 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 05617c5..cdb20f2 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 be59d00..a950358 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 521d75b..0000000 --- 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 e824c57..0000000 --- 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 621926a..0000000 --- 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 bca6fac..0000000 --- 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 47ef455..0000000 --- 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 29460c5..0000000 --- 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 dbfb3bd..0000000 --- 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 b2a7db1..0000000 --- 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 1de9fdb..0000000 --- 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 765e9b4..0000000 --- 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 008daec..0000000 --- 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 d7da04f..0000000 --- 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 c5cf3bd..0000000 --- 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 06b59f9..0000000 --- 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 e947aa9..0000000 --- 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 6f3f854..0000000 --- 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 +++-- .../ros/trajectory_follower.h | 4 ++-- include/ur_modern_driver/ur/state_parser.h | 20 ++++++++----------- 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, 30 insertions(+), 36 deletions(-) diff --git a/include/ur_modern_driver/ros/action_server.h b/include/ur_modern_driver/ros/action_server.h index f18c6e9..1d62b8f 100644 --- a/include/ur_modern_driver/ros/action_server.h +++ b/include/ur_modern_driver/ros/action_server.h @@ -32,9 +32,6 @@ private: 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 @@ private: 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 3532ee6..b2780d5 100644 --- a/include/ur_modern_driver/ros/controller.h +++ b/include/ur_modern_driver/ros/controller.h @@ -55,7 +55,7 @@ private: 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 325faa4..93bd0c7 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 @@ public: 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 be59a56..da01004 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 494558f..fc80242 100644 --- a/include/ur_modern_driver/ur/state_parser.h +++ b/include/ur_modern_driver/ur/state_parser.h @@ -33,17 +33,16 @@ public: bp.parse(packet_size); bp.parse(type); - //quietly ignore the intial version message - if (type == message_type::ROBOT_MESSAGE || ((int)type) == 5) - { - bp.consume(); - return true; - } - if (type != message_type::ROBOT_STATE) { - LOG_WARN("Invalid state message type recieved: %u", static_cast(type)); - return false; + //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; } while (!bp.empty()) @@ -60,8 +59,6 @@ public: 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 @@ public: 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 fc3f7ad..102df31 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 79102fa..f8896d7 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 09a0798..793769b 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 0093cba..52b79f2 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 dc5255c..f069ac0 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 | 33 +++-- src/ur/stream.cpp | 10 +- 31 files changed, 343 insertions(+), 343 deletions(-) diff --git a/include/ur_modern_driver/bin_parser.h b/include/ur_modern_driver/bin_parser.h index 2cfce45..5bac374 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 2c8ff31..566ffb2 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 @@ private: 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 @@ public: 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 0e5cfdb..e902ed3 100644 --- a/include/ur_modern_driver/pipeline.h +++ b/include/ur_modern_driver/pipeline.h @@ -43,28 +43,28 @@ public: 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 @@ public: 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 @@ private: 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 b2780d5..ea4989b 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 @@ private: 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 93bd0c7..5dd95c1 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 @@ private: 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 @@ public: 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 03d9935..4f02bb2 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 @@ private: 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 @@ private: 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 11c4d8b..692ecfd 100644 --- a/include/ur_modern_driver/ros/mb_publisher.h +++ b/include/ur_modern_driver/ros/mb_publisher.h @@ -16,22 +16,21 @@ private: 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 af09dfa..ffbe106 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 @@ public: 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 @@ public: 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 da01004..d6c4789 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 @@ private: 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 4230ac0..99427b9 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 @@ protected: 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 5d81698..8fdffac 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 @@ public: 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 @@ public: 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 @@ public: 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 0b4e6ca..ecf3bed 100644 --- a/include/ur_modern_driver/ur/factory.h +++ b/include/ur_modern_driver/ur/factory.h @@ -76,12 +76,12 @@ public: 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 c625cbf..e3163dc 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 @@ public: 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 df4a61a..6f08ab6 100644 --- a/include/ur_modern_driver/ur/producer.h +++ b/include/ur_modern_driver/ur/producer.h @@ -35,31 +35,30 @@ public: // 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 6c7c641..80aa145 100644 --- a/include/ur_modern_driver/ur/robot_mode.h +++ b/include/ur_modern_driver/ur/robot_mode.h @@ -16,7 +16,7 @@ public: 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 @@ public: 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 027e278..f236b96 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 @@ protected: } virtual void setOptions(int socket_fd); - public: URServer(int port); ~URServer(); @@ -29,5 +28,5 @@ public: 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 4aeddba..d48a876 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 fc80242..48eda1d 100644 --- a/include/ur_modern_driver/ur/state_parser.h +++ b/include/ur_modern_driver/ur/state_parser.h @@ -35,8 +35,8 @@ public: 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 3d5bc81..913f665 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 @@ private: 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 @@ public: 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 102df31..380acd3 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 f8896d7..75aa9c3 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 793769b..fde4e85 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 1177be5..c0fdc23 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 53cb43c..2f46fd5 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 52b79f2..a1f45d9 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 f069ac0..4f26634 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 c4f1839..0bd390a 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 0fd358e..96fde7d 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 991b6ac..c6aa924 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 5530be9..be19ae0 100644 --- a/src/ur/server.cpp +++ b/src/ur/server.cpp @@ -1,12 +1,11 @@ -#include -#include -#include -#include -#include "ur_modern_driver/log.h" #include "ur_modern_driver/ur/server.h" +#include +#include +#include +#include +#include "ur_modern_driver/log.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 cdb20f2..b0609da 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 44c1e63..0000000 --- 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 786517c..0000000 --- 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 71bb7ba..0000000 --- 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 a1f45d9..18ca691 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 14ae65b..4f98386 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 @@ public: 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 a950358..be227ba 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 c6aa924..0775a39 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 e69de29..2af943f 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 ea4989b..d99dc07 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 @@ private: 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 5dd95c1..50e311b 100644 --- a/include/ur_modern_driver/ros/hardware_interface.h +++ b/include/ur_modern_driver/ros/hardware_interface.h @@ -33,7 +33,9 @@ private: 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 @@ public: 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 @@ public: 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 @@ public: 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 75aa9c3..3e8b0f0 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 fde4e85..dc1ce9b 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 5b782aa..6e88e9f 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 4f26634..b680ff6 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 b680ff6..8d74e3e 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 380acd3..494d932 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 be19ae0..4da9b2c 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 18ca691..bec6db7 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 48eda1d..146d93c 100644 --- a/include/ur_modern_driver/ur/state_parser.h +++ b/include/ur_modern_driver/ur/state_parser.h @@ -75,7 +75,7 @@ public: 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 @@ public: 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 @@ public: typedef URStateParser URStateParser_V1_X; typedef URStateParser URStateParser_V3_0__1; -typedef URStateParser URStateParser_V3_2; \ No newline at end of file +typedef URStateParser URStateParser_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 f236b96..c240ba5 100644 --- a/include/ur_modern_driver/ur/server.h +++ b/include/ur_modern_driver/ur/server.h @@ -15,11 +15,7 @@ private: 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 be19ae0..e2f7b22 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 4da9b2c..cbdaa5b 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 0000000..212ed20 --- /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 212ed20..589d0d5 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 589d0d5..a8c8a81 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 5cf0127..f1cd996 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 a8c8a81..a8a72aa 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 ffbe106..f0b2c16 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 @@ private: 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 @@ public: { return true; } -}; \ No newline at end of file +}; diff --git a/launch/ur_common.launch b/launch/ur_common.launch index 7650156..25aa71f 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 2f46fd5..1d1a3e6 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 f0b2c16..aec39a9 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 25aa71f..95c3198 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 1d1a3e6..fcbf1a1 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 be227ba..c510ce5 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 c510ce5..3275e59 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 f1cd996..e451bc3 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 3275e59..6e27394 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 6e27394..e37a820 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 e902ed3..ca7058a 100644 --- a/include/ur_modern_driver/pipeline.h +++ b/include/ur_modern_driver/pipeline.h @@ -126,7 +126,7 @@ private: { if (!queue_.try_enqueue(std::move(p))) { - LOG_ERROR("Pipeline producer owerflowed!"); + LOG_ERROR("Pipeline producer overflowed!"); } } @@ -141,27 +141,18 @@ private: { 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 @@ public: 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 d99dc07..df5d6de 100644 --- a/include/ur_modern_driver/ros/controller.h +++ b/include/ur_modern_driver/ros/controller.h @@ -49,13 +49,13 @@ private: } 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 @@ public: 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 50e311b..a68a1e6 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 @@ public: 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 3e8b0f0..bc844c4 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 dc1ce9b..759eca5 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 8d74e3e..0818108 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 @@ public: 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 a8a72aa..6ca2043 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 8b355ea..3dabb07 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 3dabb07..129db51 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 0818108..9d037d5 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 e37a820..1691e72 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 692ecfd..f4b1549 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 @@ private: } 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 @@ public: 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 f4945d9..ce9b320 100644 --- a/include/ur_modern_driver/ros/rt_publisher.h +++ b/include/ur_modern_driver/ros/rt_publisher.h @@ -71,4 +71,4 @@ public: virtual void stopConsumer() { } -}; \ No newline at end of file +}; diff --git a/package.xml b/package.xml index e451bc3..7e2ac0c 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 c0fdc23..9d9832f 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 65d0726..0709f50 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 1691e72..22cf821 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 129db51..51427a0 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 ca7058a..ba71009 100644 --- a/include/ur_modern_driver/pipeline.h +++ b/include/ur_modern_driver/pipeline.h @@ -99,6 +99,18 @@ public: 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 @@ private: 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 @@ private: { 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 @@ private: 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 @@ public: running_ = true; pThread_ = thread(&Pipeline::run_producer, this); cThread_ = thread(&Pipeline::run_consumer, this); + notifier_.started(name_); } void stop() @@ -182,7 +201,7 @@ public: if (!running_) return; - LOG_DEBUG("Stopping pipeline"); + LOG_DEBUG("Stopping pipeline! <%s>",name_.c_str()); consumer_.stopConsumer(); producer_.stopProducer(); @@ -191,5 +210,6 @@ public: 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 0000000..f5190db --- /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 8fdffac..6a2b486 100644 --- a/include/ur_modern_driver/ur/commander.h +++ b/include/ur_modern_driver/ur/commander.h @@ -10,7 +10,7 @@ private: 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 @@ public: 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 @@ public: 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 58f7abd..f27efb3 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 436cb5b..44387a4 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 6cc6ca6..1ca2d8b 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 a63820f..6eb9dd4 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 9e7986d..f4bc006 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 e2c657e..9c70c52 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 6e88e9f..2da4123 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 eeaa318..303d253 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 31f7b0b..c8383bf 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 643f5c4..3640a2d 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 c240b7b..ccf5d79 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 95c3198..aad4c6d 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 0000000..c2e5fe7 --- /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 9d037d5..123ecc4 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 @@ public: 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 96fde7d..5c752f0 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 ecf3bed..54c6cdf 100644 --- a/include/ur_modern_driver/ur/factory.h +++ b/include/ur_modern_driver/ur/factory.h @@ -94,8 +94,10 @@ public: { 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 80aa145..c6a1cfc 100644 --- a/include/ur_modern_driver/ur/robot_mode.h +++ b/include/ur_modern_driver/ur/robot_mode.h @@ -102,4 +102,17 @@ public: 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 146d93c..37da43c 100644 --- a/include/ur_modern_driver/ur/state_parser.h +++ b/include/ur_modern_driver/ur/state_parser.h @@ -96,3 +96,4 @@ public: 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 da3a65b..783648b 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 aad4c6d..c8cb7bd 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 22cf821..17a4d4e 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 51427a0..0516a4c 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 1d62b8f..fe50567 100644 --- a/include/ur_modern_driver/ros/action_server.h +++ b/include/ur_modern_driver/ros/action_server.h @@ -39,7 +39,7 @@ private: 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 @@ private: 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 @@ public: 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 0000000..f3a6bae --- /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 d6c4789..70dba5c 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 @@ public: 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 0000000..a25ddf7 --- /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 99427b9..4ff2f86 100644 --- a/include/ur_modern_driver/tcp_socket.h +++ b/include/ur_modern_driver/tcp_socket.h @@ -46,6 +46,7 @@ public: 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 c240ba5..1809237 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 @@ public: 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 c8cb7bd..ca6eaec 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 494d932..c8546c2 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 0000000..678c5ac --- /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 123ecc4..484cdd1 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 @@ public: 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 0bd390a..619151a 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 1d54869..557ed5d 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 6faf4e1..1e69c9f 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 17a4d4e..a6a5728 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 0516a4c..ec171c7 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 f3a6bae..bd52ee4 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 @@ private: 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 ca6eaec..ad77d41 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 678c5ac..1e628b2 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 484cdd1..523f1d2 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 @@ public: 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 0000000..956bb03 --- /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 ec171c7..a48c591 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 bd52ee4..003d071 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 @@ private: 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 ad77d41..2387b00 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 1e628b2..c3cdd9f 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 f27efb3..34582d9 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 44387a4..45e0ebd 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 1ca2d8b..756807b 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 f4bc006..22e426b 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 9c70c52..5f994f1 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 303d253..eafad38 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 c8383bf..ddd0b92 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 3640a2d..09a0022 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 c3cdd9f..75c47f7 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 003d071..7b1f610 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 @@ private: 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 2387b00..37eea20 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 7b1f610..e958002 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 @@ private: 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 a6a5728..bc43d3f 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 6a2b486..0a4cf6f 100644 --- a/include/ur_modern_driver/ur/commander.h +++ b/include/ur_modern_driver/ur/commander.h @@ -49,7 +49,27 @@ public: { } - 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 54c6cdf..1f1c1ef 100644 --- a/include/ur_modern_driver/ur/factory.h +++ b/include/ur_modern_driver/ur/factory.h @@ -80,8 +80,10 @@ public: { 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 5c752f0..9b85865 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 7e2ac0c..d58d4a9 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 c2e5fe7..3b5dafd 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 3b5dafd..565bf92 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 d58d4a9..828927d 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 fe50567..d741714 100644 --- a/include/ur_modern_driver/ros/action_server.h +++ b/include/ur_modern_driver/ros/action_server.h @@ -39,7 +39,7 @@ private: 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 @@ private: 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 a25ddf7..9b5427f 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 e958002..6442dcb 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 70dba5c..af37474 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 c8546c2..8616c35 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 523f1d2..5343334 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 a48c591..3682d70 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 9b85865..44f5867 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 759eca5..11b5b5f 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 619151a..6a057bd 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 956bb03..b4fc1f3 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 ```