1
0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 01:50:46 +02:00

Adopted roscpp code style and naming convention

This commit is contained in:
Simon Rasmussen
2017-03-01 13:59:48 +01:00
parent e4bc40fc09
commit 474f469e97
44 changed files with 5097 additions and 4891 deletions

View File

@@ -1,116 +1,134 @@
#pragma once
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/queue/readerwriterqueue.h"
#include <atomic>
#include <thread>
#include <vector>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/queue/readerwriterqueue.h"
using namespace moodycamel;
using namespace std;
template <typename T>
class IConsumer {
class IConsumer
{
public:
virtual void setup_consumer() {}
virtual void teardown_consumer() {}
virtual void stop_consumer() {}
virtual void setupConsumer()
{
}
virtual void teardownConsumer()
{
}
virtual void stopConsumer()
{
}
virtual bool consume(unique_ptr<T> product) = 0;
virtual bool consume(unique_ptr<T> product) = 0;
};
template <typename T>
class IProducer {
class IProducer
{
public:
virtual void setup_producer() {}
virtual void teardown_producer() {}
virtual void stop_producer() {}
virtual void setupProducer()
{
}
virtual void teardownProducer()
{
}
virtual void stopProducer()
{
}
virtual bool try_get(std::vector<unique_ptr<T> >& products) = 0;
virtual bool tryGet(std::vector<unique_ptr<T>>& products) = 0;
};
template <typename T>
class Pipeline {
class Pipeline
{
private:
IProducer<T>& _producer;
IConsumer<T>& _consumer;
BlockingReaderWriterQueue<unique_ptr<T> > _queue;
atomic<bool> _running;
thread _pThread, _cThread;
IProducer<T>& producer_;
IConsumer<T>& consumer_;
BlockingReaderWriterQueue<unique_ptr<T>> queue_;
atomic<bool> running_;
thread pThread_, cThread_;
void run_producer()
void run_producer()
{
producer_.setupProducer();
std::vector<unique_ptr<T>> products;
while (running_)
{
_producer.setup_producer();
std::vector<unique_ptr<T> > 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<T> product;
while (running_)
{
_consumer.setup_consumer();
unique_ptr<T> 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<T>& producer, IConsumer<T>& consumer)
: _producer(producer)
, _consumer(consumer)
, _queue{ 32 }
, _running{ false }
{
}
Pipeline(IProducer<T>& producer, IConsumer<T>& 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();
}
};