Improved pipeline design

include/ur_modern_driver/pipeline.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <atomic>
+#include <chrono>
 #include <thread>
 #include <vector>
 #include "ur_modern_driver/log.h"
@@ -23,7 +24,52 @@ public:
   {
   }
 
-  virtual bool consume(unique_ptr<T> product) = 0;
+  virtual bool consume(shared_ptr<T> product) = 0;
+};
+
+template <typename T>
+class MultiConsumer : public IConsumer<T>
+{
+private:
+  std::vector<IConsumer<T>*> consumers_;
+
+public:
+  MultiConsumer(std::vector<IConsumer<T>*> 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<T> product)
+  {
+    bool res = true;
+    for(auto &con : consumers_)
+    {
+      if(!con->consume(product))
+        res = false;
+    }
+    return res;
+  }
 };
 
 template <typename T>
@@ -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;
       }

include/ur_modern_driver/ur/consumer.h

@@ -10,7 +10,7 @@
 class URRTPacketConsumer : public IConsumer<RTPacket>
 {
 public:
-  virtual bool consume(unique_ptr<RTPacket> packet)
+  virtual bool consume(shared_ptr<RTPacket> packet)
   {
     return packet->consumeWith(*this);
   }
@@ -24,7 +24,7 @@ public:
 class URStatePacketConsumer : public IConsumer<StatePacket>
 {
 public:
-  virtual bool consume(unique_ptr<StatePacket> packet)
+  virtual bool consume(shared_ptr<StatePacket> packet)
   {
     return packet->consumeWith(*this);
   }
@@ -41,7 +41,7 @@ public:
 class URMessagePacketConsumer : public IConsumer<MessagePacket>
 {
 public:
-  virtual bool consume(unique_ptr<MessagePacket> packet)
+  virtual bool consume(shared_ptr<MessagePacket> packet)
   {
     return packet->consumeWith(*this);
   }

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;
 };