RequestQueue.cpp

Go to the documentation of this file.

#include "RequestQueue.h"
 
#include <boost/lockfree/policies.hpp>
#include <boost/lockfree/spsc_queue.hpp>
 
#include "RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h"
 
#include <armarx/control/ethercat/ErrorReporting.h>
#include <armarx/control/ethercat/EtherCATState.h>
 
#include "requests/ChangeStateRequest.h"
#include "requests/ReadStatesRequest.h"
#include "requests/RegisterReadRequest.h"
#include "requests/RegisterResetRequest.h"
#include "requests/SDOUpdateRequest.h"
 
namespace armarx::control::ethercat
{
    struct RequestQueue::QueueImpl
    {
        static constexpr size_t queueSize = 100;
 
        boost::lockfree::spsc_queue<std::shared_ptr<RequestBase>,
                                    boost::lockfree::capacity<queueSize>>
            queue;
    };
 
    RequestQueue::RequestQueue() : pimpl(std::make_unique<QueueImpl>())
    {
    }
 
    RequestQueue::~RequestQueue() = default; // Need for std::unique_ptr to incomplete type
 
    RequestQueue&
    RequestQueue::operator=(RequestQueue rhs)
    {
        swap(pimpl, rhs.pimpl);
        return *this;
    }
 
    std::shared_ptr<RequestBase>
    RequestQueue::getNextRequest()
    {
        std::shared_ptr<RequestBase> request;
        if (!pimpl->queue.pop(request))
        {
            return nullptr;
        }
        return request;
    }
 
    void
    RequestQueue::postReply(std::shared_ptr<RequestBase>&& request)
    {
        request->setProcessed();
        requestCV.notify_all();
    }
 
    bool
    RequestQueue::postRequest(const std::shared_ptr<RequestBase>& request)
    {
        static std::mutex mtx;
        std::unique_lock<std::mutex> lck(mtx);
 
        if (pimpl->queue.write_available() < 10)
        {
            GENERAL_ERROR("Queue is running full.");
        }
 
 
        pimpl->queue.push(request);
        requestCV.wait(lck,
                       [&, request]() { return request->isProcessed() || request->hasFailed(); });
        return !request->hasFailed();
    }
 
    bool
    RequestQueue::postSDOReadRequest(SDOIdentifier sdoIdentifier,
                                     int* buflen,
                                     unsigned char* buf,
                                     bool completeAccess)
    {
        std::shared_ptr<SDOUpdateRequest> request =
            std::make_shared<SDOUpdateRequest>(sdoIdentifier, buflen, buf, true, completeAccess);
        return postSDOUpdateRequest(request);
    }
 
    bool
    RequestQueue::postSDOWriteRequest(SDOIdentifier sdoIdentifier,
                                      int buflen,
                                      unsigned char* buf,
                                      bool completeAccess)
    {
        std::shared_ptr<SDOUpdateRequest> request =
            std::make_shared<SDOUpdateRequest>(sdoIdentifier, &buflen, buf, false, completeAccess);
        return postSDOUpdateRequest(request);
    }
 
    bool
    RequestQueue::postChangeStateRequest(std::uint16_t slaveIndex,
                                         EtherCATState state,
                                         bool validate,
                                         EtherCATState* actualState)
    {
        std::shared_ptr<ChangeStateRequest> request =
            std::make_shared<ChangeStateRequest>(slaveIndex, state, validate, actualState);
        return postRequest(request);
    }
 
    bool
    RequestQueue::postReadStatesRequest(EtherCATState* state)
    {
        auto request = std::make_shared<ReadStatesRequest>(state);
        return postRequest(request);
    }
 
    bool
    RequestQueue::postRegisterResetRequest(uint16_t slaveIndex)
    {
        auto request = std::make_shared<RegisterResetRequest>(slaveIndex);
        return postRequest(request);
    }
 
    bool
    RequestQueue::postRegisterReadRequest(std::vector<RegisterDataList>& registerData)
    {
        auto request = std::make_shared<RegisterReadRequest>(&registerData);
        return postRequest(request);
    }
 
    bool
    RequestQueue::postRegisterReadRequest(EtherCATFrameList* frames,
                                          std::uint16_t amountFramesToRead)
    {
        auto request = std::make_shared<RegisterReadRequest>(frames, amountFramesToRead);
        return postRequest(request);
    }
 
    bool
    RequestQueue::postSDOUpdateRequest(const std::shared_ptr<SDOUpdateRequest>& request)
    {
        return postRequest(request);
    }
} // namespace armarx::control::ethercat