CanRxTxThreads.hpp

// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
 
#ifndef __CAN_RX_TH_THREADS_HPP__
#define __CAN_RX_TH_THREADS_HPP__
 
#include <mutex>
#include <string>
#include <unordered_map>
#include <vector>
 
#include <yarp/os/Bottle.h>
#include <yarp/os/Contactable.h>
#include <yarp/os/PortWriterBuffer.h>
#include <yarp/os/Stamp.h>
#include <yarp/os/Thread.h>
 
#include <yarp/dev/CanBusInterface.h>
 
#include "ICanBusSharer.hpp"
 
namespace roboticslab
{
 
class CanReaderWriterThread : public yarp::os::Thread
{
public:
    CanReaderWriterThread(const std::string & type, const std::string & id, double delay, unsigned int bufferSize)
        : iCanBus(nullptr), iCanBufferFactory(nullptr),
          dumpPort(nullptr), dumpWriter(nullptr), dumpMutex(nullptr), busLoadMonitor(nullptr),
          bufferSize(bufferSize), delay(delay), type(type), id(id)
    { }
 
    ~CanReaderWriterThread() override = default;
 
    bool threadInit() override
    { canBuffer = iCanBufferFactory->createBuffer(bufferSize); return true; }
 
    void threadRelease() override
    { iCanBufferFactory->destroyBuffer(canBuffer); }
 
    void beforeStart() override;
 
    void afterStart(bool success) override;
 
    void onStop() override;
 
    void setCanHandles(yarp::dev::ICanBus * iCanBus, yarp::dev::ICanBufferFactory * iCanBufferFactory)
    {
        this->iCanBus = iCanBus; this->iCanBufferFactory = iCanBufferFactory;
    }
 
    void attachDumpWriter(yarp::os::Contactable * dumpPort,
                          yarp::os::PortWriterBuffer<yarp::os::Bottle> * dumpWriter,
                          std::mutex * dumpMutex)
    {
        this->dumpPort = dumpPort; this->dumpWriter = dumpWriter; this->dumpMutex = dumpMutex;
    }
 
    void attachBusLoadMonitor(ICanMessageNotifier * busLoadMonitor)
    { this->busLoadMonitor = busLoadMonitor; }
 
protected:
    static void dumpMessage(const can_message & msg, yarp::os::Bottle & b);
 
    yarp::dev::ICanBus * iCanBus;
    yarp::dev::ICanBufferFactory * iCanBufferFactory;
    yarp::dev::CanBuffer canBuffer;
 
    yarp::os::Contactable * dumpPort;
    yarp::os::PortWriterBuffer<yarp::os::Bottle> * dumpWriter;
    std::mutex * dumpMutex;
 
    yarp::os::Stamp lastStamp;
 
    ICanMessageNotifier * busLoadMonitor;
 
    unsigned int bufferSize;
    double delay;
 
private:
    std::string type;
    std::string id;
};
 
class CanReaderThread : public CanReaderWriterThread
{
public:
    CanReaderThread(const std::string & id, double delay, unsigned int bufferSize);
 
    void registerHandle(ICanBusSharer * p);
 
    const std::vector<ICanBusSharer *> & getHandles() const
    { return handles; }
 
    void attachCanNotifier(ICanMessageNotifier * canMessageNotifier)
    { this->canMessageNotifier = canMessageNotifier; }
 
    void run() override;
 
private:
    std::vector<ICanBusSharer *> handles;
    std::unordered_map<unsigned int, ICanBusSharer *> canIdToHandle;
    ICanMessageNotifier * canMessageNotifier;
};
 
class CanWriterThread : public CanReaderWriterThread
{
public:
    CanWriterThread(const std::string & id, double delay, unsigned int bufferSize);
 
    ~CanWriterThread() override;
 
    ICanSenderDelegate * getDelegate()
    { return sender; }
 
    void flush();
 
    void run() override;
 
private:
    void handlePartialWrite(unsigned int sent);
 
    unsigned int preparedMessages;
    ICanSenderDelegate * sender;
    mutable std::mutex bufferMutex;
};
 
} // namespace roboticslab
 
#endif // __CAN_RX_TH_THREADS_HPP__