Merge branch 'multiple-buses' into apocanlypse

libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp

@@ -3,14 +3,12 @@
 #ifndef __CAN_BUS_CONTROLBOARD_HPP__
 #define __CAN_BUS_CONTROLBOARD_HPP__
 
+#include <string>
 #include <vector>
 
-#include <yarp/dev/CanBusInterface.h>
 #include <yarp/dev/ControlBoardInterfaces.h>
-#include <yarp/dev/PolyDriver.h>
 #include <yarp/dev/PolyDriverList.h>
 
-#include "ICanBusSharer.hpp"
 #include "PositionDirectThread.hpp"
 #include "DeviceMapper.hpp"
 #include "CanRxTxThreads.hpp"
@@ -21,10 +19,6 @@
 #define DEFAULT_LIN_INTERP_BUFFER_SIZE 1
 #define DEFAULT_LIN_INTERP_MODE "pt"
 
-#define DEFAULT_CAN_RX_BUFFER_SIZE 500
-#define DEFAULT_CAN_TX_BUFFER_SIZE 500
-#define DEFAULT_CAN_RX_PERIOD_MS -1
-#define DEFAULT_CAN_TX_PERIOD_MS 1.0
 #define DEFAULT_CAN_SDO_TIMEOUT_MS 25.0 // FIXME unused
 #define DEFAULT_CAN_DRIVE_STATE_TIMEOUT 2.5 // FIXME unused
 
@@ -64,8 +58,7 @@ class CanBusControlboard : public yarp::dev::DeviceDriver,
 public:
 
     CanBusControlboard()
-        : iCanBus(nullptr), canReaderThread(nullptr), canWriterThread(nullptr),
-          posdThread(nullptr), linInterpPeriodMs(0), linInterpBufferSize(0)
+        : posdThread(nullptr), linInterpPeriodMs(0), linInterpBufferSize(0)
     { }
 
     // -------- DeviceDriver declarations. Implementation in DeviceDriverImpl.cpp --------
@@ -314,16 +307,18 @@ class CanBusControlboard : public yarp::dev::DeviceDriver,
 
 private:
 
-    yarp::dev::PolyDriver canBusDevice;
-    yarp::dev::ICanBus * iCanBus;
+    struct CanThreads
+    {
+        std::string busName;
+        CanReaderThread * reader = nullptr;
+        CanWriterThread * writer = nullptr;
+    };
 
+    yarp::dev::PolyDriverList busDevices;
     yarp::dev::PolyDriverList nodeDevices;
 
     DeviceMapper deviceMapper;
-    std::vector<ICanBusSharer *> iCanBusSharers;
-
-    CanReaderThread * canReaderThread;
-    CanWriterThread * canWriterThread;
+    std::vector<CanThreads> canThreads;
 
     PositionDirectThread * posdThread;
     int linInterpPeriodMs;

libraries/YarpPlugins/CanBusControlboard/CanRxTxThreads.cpp

@@ -10,17 +10,17 @@
 
 using namespace roboticslab;
 
-CanReaderThread::CanReaderThread(const std::string & id, const std::vector<ICanBusSharer *> & iCanBusSharers)
+CanReaderThread::CanReaderThread(const std::string & id)
     : CanReaderWriterThread("read", id)
+{}
+
+void CanReaderThread::registerHandle(ICanBusSharer * p)
 {
-    for (auto p : iCanBusSharers)
-    {
-        canIdToHandle[p->getId()] = p;
+    canIdToHandle[p->getId()] = p;
 
-        for (auto id : p->getAdditionalIds())
-        {
-            canIdToHandle[id] = p;
-        }
+    for (auto id : p->getAdditionalIds())
+    {
+        canIdToHandle[id] = p;
     }
 }
 
@@ -33,7 +33,7 @@ void CanReaderThread::run()
     {
         //-- Lend CPU time to write threads.
         // https://github.com/roboticslab-uc3m/yarp-devices/issues/191
-        yarp::os::Time::delay(period);
+        yarp::os::Time::delay(delay);
 
         //-- Return immediately if there is nothing to be read (non-blocking call), return false on errors.
         ok = iCanBus->canRead(canBuffer, bufferSize, &read);
@@ -67,19 +67,15 @@ void CanReaderThread::run()
 
 CanWriterThread::CanWriterThread(const std::string & id)
     : CanReaderWriterThread("write", id),
-      sender(0),
+      sender(nullptr),
       preparedMessages(0)
 {}
 
 // -----------------------------------------------------------------------------
 
 CanWriterThread::~CanWriterThread()
 {
-    if (sender)
-    {
-        delete sender;
-        sender = 0;
-    }
+    delete sender;
 }
 
 // -----------------------------------------------------------------------------
@@ -93,7 +89,7 @@ void CanWriterThread::run()
     {
         //-- Lend CPU time to read threads.
         // https://github.com/roboticslab-uc3m/yarp-devices/issues/191
-        yarp::os::Time::delay(period);
+        yarp::os::Time::delay(delay);
 
         std::lock_guard<std::mutex> lock(bufferMutex);
 

libraries/YarpPlugins/CanBusControlboard/CanRxTxThreads.hpp

@@ -7,7 +7,6 @@
 #include <map>
 #include <mutex>
 #include <string>
-#include <vector>
 
 #include <yarp/os/Thread.h>
 #include <yarp/dev/CanBusInterface.h>
@@ -27,7 +26,7 @@ class CanReaderWriterThread : public yarp::os::Thread
 {
 public:
     CanReaderWriterThread(const std::string & type, const std::string & id)
-        : iCanBus(0), iCanBufferFactory(0), type(type), id(id), bufferSize(0), period(0.0)
+        : iCanBus(nullptr), iCanBufferFactory(nullptr), type(type), id(id), bufferSize(0), delay(0.0)
     {}
 
     virtual bool threadInit() override
@@ -48,16 +47,16 @@ class CanReaderWriterThread : public yarp::os::Thread
     virtual void setCanHandles(yarp::dev::ICanBus * iCanBus, yarp::dev::ICanBufferFactory * iCanBufferFactory, unsigned int bufferSize)
     { this->iCanBus = iCanBus; this->iCanBufferFactory = iCanBufferFactory; this->bufferSize = bufferSize; }
 
-    void setPeriod(double periodMs)
-    { period = periodMs < 0 ? std::numeric_limits<double>::min() : periodMs * 0.001; }
+    void setDelay(double delay)
+    { this->delay = delay <= 0 ? std::numeric_limits<double>::min() : delay; }
 
 protected:
     yarp::dev::ICanBus * iCanBus;
     yarp::dev::ICanBufferFactory * iCanBufferFactory;
     yarp::dev::CanBuffer canBuffer;
 
     unsigned int bufferSize;
-    double period;
+    double delay;
 
 private:
     std::string type;
@@ -70,7 +69,8 @@ class CanReaderWriterThread : public yarp::os::Thread
 class CanReaderThread : public CanReaderWriterThread
 {
 public:
-    CanReaderThread(const std::string & id, const std::vector<ICanBusSharer *> & iCanBusSharers);
+    CanReaderThread(const std::string & id);
+    void registerHandle(ICanBusSharer * p);
     virtual void run() override;
 
 private: