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:
@@ -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();
|
||||
}
|
||||
};
|
||||
Reference in New Issue
Block a user