568 lines
16 KiB
C++
Executable File
568 lines
16 KiB
C++
Executable File
#pragma once
|
|
|
|
|
|
|
|
#include "generic_ftc.h"
|
|
#include "abstract_requester.h"
|
|
#include "generic_ftc_threading.h"
|
|
|
|
#include <DDS/Core/core.hpp>
|
|
#include <DDS/FTS/connext.h>
|
|
#include <DDS/Generic/generic_logger.h>
|
|
#include <DDS/Generic/generic_base.h>
|
|
#include <DDS/QosProfile/DDS_DefaultQos.h>
|
|
|
|
#include <QObject>
|
|
#include <QMetaMethod>
|
|
#include <QTimer>
|
|
|
|
#include <queue>
|
|
#include <future>
|
|
|
|
|
|
namespace FTC
|
|
{
|
|
template <class C>
|
|
class HasStatusFunction
|
|
{
|
|
template <class T>
|
|
static std::true_type testSignature(void (T::*)(const Core::FtReplyState &));
|
|
|
|
template <class T>
|
|
static decltype(testSignature(&T::replyState)) test(std::nullptr_t);
|
|
|
|
template <class T>
|
|
static std::false_type test(...);
|
|
|
|
public:
|
|
using type = decltype(test<C>(nullptr));
|
|
static const bool value = type::value;
|
|
};
|
|
|
|
template <class C>
|
|
class HasRequestId
|
|
{
|
|
template <class T>
|
|
static std::true_type testSignature(const int32_t &(T::*)() const);
|
|
template <class T>
|
|
static std::true_type testSignature(int32_t (T::*)() const);
|
|
|
|
template <class T>
|
|
static decltype(testSignature(&T::requestId)) test(std::nullptr_t);
|
|
|
|
template <class T>
|
|
static std::false_type test(...);
|
|
|
|
public:
|
|
using type = decltype(test<C>(nullptr));
|
|
static const bool value = type::value;
|
|
};
|
|
|
|
template<typename GENERIC_FTC>
|
|
struct TopicNames
|
|
{};
|
|
|
|
template<typename TOPICTYPE_REQUEST, typename TOPICTYPE_REPLY, typename DUMMY>
|
|
class GenericFtcImpl final : public GenericFtc<TOPICTYPE_REQUEST, TOPICTYPE_REPLY>, public Threading
|
|
{
|
|
public:
|
|
using RequestTopic = TOPICTYPE_REQUEST;
|
|
using ReplyTopic = TOPICTYPE_REPLY;
|
|
using TopicNameHelper = TopicNames<GenericFtcImpl<RequestTopic, ReplyTopic, DUMMY>>;
|
|
|
|
GenericFtcImpl(const dds::domain::DomainParticipant &domainParticipant,
|
|
const dds::pub::Publisher &ddsPublisher,
|
|
const dds::sub::Subscriber &ddsSubscriber,
|
|
QObject *objToInform,
|
|
const std::string &partitionType,
|
|
int partitionId)
|
|
: GenericFtc<RequestTopic, ReplyTopic>()
|
|
, m_objToInform(objToInform)
|
|
, m_requesterParams(domainParticipant)
|
|
, m_partitionType(partitionType)
|
|
, m_partitionId(partitionId)
|
|
, m_logger(DDS::getLogger(TopicNameHelper::functionTopicName(), DDS::DataType::FUNCTION_TOPIC_CLIENT))
|
|
{
|
|
|
|
|
|
Q_ASSERT(domainParticipant != nullptr);
|
|
Q_ASSERT(ddsPublisher != nullptr);
|
|
Q_ASSERT(ddsSubscriber != nullptr);
|
|
Q_ASSERT(objToInform != nullptr);
|
|
|
|
|
|
m_requesterParams.request_topic_name(TopicNameHelper::requestTopicName());
|
|
m_requesterParams.reply_topic_name(TopicNameHelper::replyTopicName());
|
|
m_requesterParams.publisher(ddsPublisher);
|
|
m_requesterParams.subscriber(ddsSubscriber);
|
|
m_requesterParams.datareader_qos(DDS::QosProfile::dataReaderDefault(DDS::QosProfile::TopicType::FunctionTopic));
|
|
m_requesterParams.datawriter_qos(DDS::QosProfile::dataWriterDefault(DDS::QosProfile::TopicType::FunctionTopic));
|
|
DDS::QosProfile::print(m_requesterParams.datareader_qos(), m_logger);
|
|
DDS::QosProfile::print(m_requesterParams.datawriter_qos(), m_logger);
|
|
|
|
DEBUG_LOG(m_logger) << "=> Static request using request/reply topic names: "
|
|
<< TopicNameHelper::requestTopicName()
|
|
<< ", reply: "
|
|
<< TopicNameHelper::replyTopicName();
|
|
|
|
qRegisterMetaType<ReplyTopic>(TopicNameHelper::replyTopicTypeName());
|
|
preInit();
|
|
}
|
|
|
|
|
|
~GenericFtcImpl()
|
|
{
|
|
Q_ASSERT(!m_dying);
|
|
m_dying = true;
|
|
|
|
decltype(m_modernRequests) modernRequests;
|
|
decltype(m_classicRequests) classicRequests;
|
|
{
|
|
{
|
|
std::lock_guard<std::mutex> l(m_conditionMutex);
|
|
setWorkAvailable(false);
|
|
}
|
|
|
|
std::lock_guard<std::mutex> l(m_requestMutex);
|
|
Q_ASSERT(m_classicRequests.empty());
|
|
Q_ASSERT(m_modernRequests.empty());
|
|
|
|
classicRequests.swap(m_classicRequests);
|
|
modernRequests.swap(m_modernRequests);
|
|
}
|
|
|
|
for (; !modernRequests.empty(); modernRequests.pop())
|
|
{
|
|
ModernRequest<ReplyTopic> &request = modernRequests.front();
|
|
fprintf(stderr, "[!] GenericFtcImpl::dtor: there was a modern request in the queue while the Ftc is being destroyed. Throwing!\n");
|
|
Q_ASSERT(false);
|
|
request.promise.set_exception(std::make_exception_ptr(std::runtime_error("Session aborted.")));
|
|
emit request.requestWatcher->finished();
|
|
}
|
|
|
|
for (; !classicRequests.empty(); classicRequests.pop())
|
|
{
|
|
ClassicRequest &request = classicRequests.front();
|
|
fprintf(stderr, "[!] GenericFtcImpl::dtor: there was a classic request in the queue while the Ftc is being destroyed. Throwing!\n");
|
|
Q_ASSERT(false);
|
|
sendError(request, 0, true );
|
|
}
|
|
}
|
|
|
|
|
|
|
|
struct ClassicRequest
|
|
{
|
|
connext::WriteSample<RequestTopic> sample;
|
|
QMetaMethod replySlot;
|
|
QMetaMethod errorSlot;
|
|
int uniqueId = 0;
|
|
};
|
|
|
|
|
|
bool requestDataClassic(const RequestTopic &requestTopic,
|
|
const char *replySlot,
|
|
const char *errorSlot,
|
|
int &uniqueId) override
|
|
{
|
|
Q_ASSERT(!m_dying);
|
|
|
|
ClassicRequest request;
|
|
if (!checkSlots(request, replySlot, errorSlot))
|
|
return false;
|
|
request.sample.set_data(requestTopic);
|
|
request.uniqueId = ++m_uniqueId;
|
|
uniqueId = request.uniqueId;
|
|
|
|
createThread();
|
|
|
|
{
|
|
std::lock_guard<std::mutex> l(m_requestMutex);
|
|
m_classicRequests.push(request);
|
|
}
|
|
|
|
runThread();
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
|
|
template<typename ReplyTopic>
|
|
struct ModernRequest
|
|
{
|
|
std::promise<ReplyTopic> promise;
|
|
connext::WriteSample<RequestTopic> sample;
|
|
Request2 *requestWatcher;
|
|
};
|
|
|
|
|
|
std::future<ReplyTopic> requestDataModern(const RequestTopic &requestTopic, Request2 *requestWatcher) override
|
|
{
|
|
Q_ASSERT(!m_dying);
|
|
|
|
std::promise<ReplyTopic> promise;
|
|
std::future<ReplyTopic> future = promise.get_future();
|
|
|
|
connext::WriteSample<RequestTopic> sample;
|
|
sample.set_data(requestTopic);
|
|
|
|
createThread();
|
|
|
|
{
|
|
std::lock_guard<std::mutex> l(m_requestMutex);
|
|
m_modernRequests.push({ std::move(promise), sample, requestWatcher });
|
|
}
|
|
|
|
runThread();
|
|
|
|
return future;
|
|
}
|
|
|
|
|
|
private:
|
|
QString functionTopicName() const override
|
|
{
|
|
return QString::fromUtf8(TopicNameHelper::functionTopicName());
|
|
}
|
|
|
|
|
|
int partitionId() const override
|
|
{
|
|
return m_partitionId;
|
|
}
|
|
|
|
|
|
QString partitionType() const override
|
|
{
|
|
return QString::fromStdString(m_partitionType);
|
|
}
|
|
|
|
|
|
std::unique_ptr<connext::Requester<RequestTopic, ReplyTopic> > requester;
|
|
|
|
|
|
void cleanupThread() override
|
|
{
|
|
|
|
requester.reset();
|
|
}
|
|
|
|
|
|
void threadDone() override
|
|
{
|
|
|
|
|
|
|
|
|
|
|
|
for (; !m_modernRequests.empty(); m_modernRequests.pop())
|
|
{
|
|
ModernRequest<ReplyTopic> &request = m_modernRequests.front();
|
|
fprintf(stderr, "[!] GenericFtcImpl::threadDone: there was a modern request in the queue while the thread already ended. Throwing!\n");
|
|
Q_ASSERT(false);
|
|
request.promise.set_exception(std::make_exception_ptr(std::runtime_error("Worker thread timed out.")));
|
|
}
|
|
|
|
for (; !m_classicRequests.empty(); m_classicRequests.pop())
|
|
{
|
|
ClassicRequest &request = m_classicRequests.front();
|
|
fprintf(stderr, "[!] GenericFtcImpl::dtor: there was a classic request in the queue while the thread already ended. Throwing!\n");
|
|
Q_ASSERT(false);
|
|
sendError(request, 0, true );
|
|
}
|
|
}
|
|
|
|
|
|
void work() override
|
|
{
|
|
try
|
|
{
|
|
decltype(m_classicRequests) classicRequests;
|
|
decltype(m_modernRequests) modernRequests;
|
|
|
|
Q_ASSERT(isWorkAvailable());
|
|
if (!isWorkAvailable())
|
|
{
|
|
fprintf(stderr, "[!] GenericFtc::work: called while marked as no work available! run? %d (%s)\n", isRun(), threadName().toStdString().c_str());
|
|
}
|
|
setWorkAvailable(false);
|
|
|
|
{
|
|
std::lock_guard<std::mutex> l(m_requestMutex);
|
|
|
|
if (!m_classicRequests.empty())
|
|
{
|
|
m_classicRequests.swap(classicRequests);
|
|
}
|
|
if (!m_modernRequests.empty())
|
|
{
|
|
m_modernRequests.swap(modernRequests);
|
|
}
|
|
}
|
|
|
|
cancellationPoint();
|
|
|
|
if (classicRequests.empty() && modernRequests.empty())
|
|
{
|
|
fprintf(stderr, "[!] GenericFtc::work: called without having any requests! (%s)\n", threadName().toStdString().c_str());
|
|
Q_ASSERT(false);
|
|
return;
|
|
}
|
|
|
|
if (!requester)
|
|
{
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
requester.reset(new connext::Requester<RequestTopic, ReplyTopic>(m_requesterParams));
|
|
}
|
|
|
|
for (; !classicRequests.empty(); classicRequests.pop())
|
|
{
|
|
cancellationPoint();
|
|
ClassicRequest &request = classicRequests.front();
|
|
requester->send_request(request.sample);
|
|
cancellationPoint();
|
|
getRequestedDataClassic(*requester.get(), request);
|
|
cancellationPoint();
|
|
}
|
|
|
|
for (; !modernRequests.empty(); modernRequests.pop())
|
|
{
|
|
cancellationPoint();
|
|
ModernRequest<ReplyTopic> &request = modernRequests.front();
|
|
requester->send_request(request.sample);
|
|
cancellationPoint();
|
|
try
|
|
{
|
|
getModernRequestedData(*requester.get(), request);
|
|
}
|
|
catch (...)
|
|
{
|
|
request.promise.set_exception(std::current_exception());
|
|
|
|
QTimer::singleShot(0, request.requestWatcher, &Request2::finished);
|
|
}
|
|
cancellationPoint();
|
|
}
|
|
}
|
|
catch (const dds::core::Exception &ex)
|
|
{
|
|
fprintf(stderr, "[!] GenericFtc::work: dds::core::exception: %s (%s)\n", ex.what(), threadName().toStdString().c_str());
|
|
}
|
|
catch (const std::exception &ex)
|
|
{
|
|
fprintf(stderr, "[!] GenericFtc::work: exception: %s (%s)\n", ex.what(), threadName().toStdString().c_str());
|
|
}
|
|
catch(...)
|
|
{
|
|
fprintf(stderr, "[!] GenericFtc::work: unknown exception occured !!! (%s)\n", threadName().toStdString().c_str());
|
|
}
|
|
}
|
|
|
|
|
|
template <typename T, typename std::enable_if<HasStatusFunction<T>::value, int>::type = 0>
|
|
static inline bool isInProgress(const connext::Sample<T> &reply)
|
|
{
|
|
return (reply.data().replyState() == Core::FtReplyState::InProgress);
|
|
}
|
|
|
|
|
|
template <typename T, typename std::enable_if<!HasStatusFunction<T>::value, int>::type = 0>
|
|
static inline bool isInProgress(const connext::Sample<T> &)
|
|
{
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
|
|
void getRequestedDataClassic(connext::Requester<RequestTopic, ReplyTopic> &requester, ClassicRequest &request)
|
|
{
|
|
|
|
#ifdef WIN32
|
|
const dds::core::Duration MAX_WAIT(200);
|
|
#else
|
|
const dds::core::Duration MAX_WAIT(20);
|
|
#endif
|
|
|
|
bool inProgress = true;
|
|
while (inProgress && isRun())
|
|
{
|
|
DEBUG_LOG(m_logger) << "Waiting for reply ";
|
|
|
|
connext::Sample<ReplyTopic> reply;
|
|
|
|
if (!requester.receive_reply(reply, MAX_WAIT))
|
|
{
|
|
WARNING_LOG(m_logger) << "Timed out waiting for reply";
|
|
sendError(request, 3);
|
|
return;
|
|
}
|
|
|
|
|
|
if (reply.info().valid())
|
|
{
|
|
if (request.sample.info().identity() != reply.info()->related_original_publication_virtual_sample_identity())
|
|
{
|
|
|
|
INFO_LOG(m_logger) << "#receiveReply - outdated. sample id: " << connext::DebugInfo::toReadable(request.sample.info().identity())
|
|
<< ", virutal sample id: " << connext::DebugInfo::toReadable(reply.info()->related_original_publication_virtual_sample_identity());
|
|
continue;
|
|
}
|
|
inProgress = isInProgress(reply);
|
|
if (request.replySlot.isValid())
|
|
{
|
|
INFO_LOG(m_logger) << "#receiveReply for " << TopicNameHelper::replyTopicTypeName();
|
|
|
|
request.replySlot.invoke(m_objToInform, Qt::QueuedConnection,
|
|
QArgument<ReplyTopic>(TopicNameHelper::replyTopicTypeName(), reply.data()),
|
|
Q_ARG(int, request.uniqueId),
|
|
Q_ARG(bool, !inProgress));
|
|
}
|
|
else
|
|
{
|
|
WARNING_LOG(m_logger) << "reply slot doesn't exist for " << dds::topic::topic_type_name<ReplyTopic>::value();
|
|
}
|
|
if (!inProgress)
|
|
return;
|
|
}
|
|
else
|
|
WARNING_LOG(m_logger) << "No valid data available ";
|
|
|
|
|
|
|
|
}
|
|
}
|
|
|
|
|
|
void getModernRequestedData(connext::Requester<RequestTopic, ReplyTopic> &requester, ModernRequest<ReplyTopic> &request) noexcept(false)
|
|
{
|
|
|
|
#ifdef WIN32
|
|
const dds::core::Duration MAX_WAIT(200);
|
|
#else
|
|
const dds::core::Duration MAX_WAIT(20);
|
|
#endif
|
|
|
|
while (isRun())
|
|
{
|
|
connext::Sample<ReplyTopic> reply;
|
|
|
|
if (!requester.receive_reply(reply, MAX_WAIT))
|
|
{
|
|
WARNING_LOG(m_logger) << "Timeout while waiting for reply!";
|
|
throw std::runtime_error("Timeout while waiting for reply!");
|
|
}
|
|
|
|
|
|
|
|
if (!reply.info().valid())
|
|
{
|
|
WARNING_LOG(m_logger) << "No valid data available!";
|
|
|
|
throw std::runtime_error("No valid data available!");
|
|
}
|
|
|
|
if (request.sample.info().identity() != reply.info()->related_original_publication_virtual_sample_identity())
|
|
{
|
|
|
|
INFO_LOG(m_logger) << "#receiveReply - outdated. sample id: " << connext::DebugInfo::toReadable(request.sample.info().identity())
|
|
<< ", virutal sample id: " << connext::DebugInfo::toReadable(reply.info()->related_original_publication_virtual_sample_identity());
|
|
continue;
|
|
}
|
|
|
|
Q_ASSERT(!isInProgress(reply));
|
|
if (isInProgress(reply))
|
|
{
|
|
ERROR_LOG(m_logger) << "Used ModernRequest for a request with FtReplyState! Your data will most likely be imcomplete!";
|
|
}
|
|
|
|
INFO_LOG(m_logger) << "#receiveReply for " << TopicNameHelper::replyTopicTypeName();
|
|
|
|
request.promise.set_value(reply.data());
|
|
|
|
QTimer::singleShot(0, request.requestWatcher, &Request2::finished);
|
|
|
|
return;
|
|
|
|
|
|
|
|
}
|
|
}
|
|
|
|
|
|
void sendErrorInternal(const QMetaMethod &errorSlot, int reason, int uniqueId, int requestId, bool directConnection = false)
|
|
{
|
|
if (errorSlot.isValid())
|
|
{
|
|
errorSlot.invoke(m_objToInform, directConnection ? Qt::DirectConnection : Qt::QueuedConnection,
|
|
Q_ARG(int, reason),
|
|
Q_ARG(int, uniqueId),
|
|
Q_ARG(int, requestId));
|
|
}
|
|
}
|
|
|
|
|
|
template < typename T = TOPICTYPE_REQUEST, typename std::enable_if < HasRequestId<T>::value, int >::type = 0 >
|
|
void sendError(const ClassicRequest &request, int reason, bool directConnection = false)
|
|
{
|
|
sendErrorInternal(request.errorSlot, reason, request.uniqueId, request.sample.data().requestId(), directConnection);
|
|
}
|
|
|
|
|
|
template < typename T = TOPICTYPE_REQUEST, typename std::enable_if < !HasRequestId<T>::value, int >::type = 0 >
|
|
void sendError(const ClassicRequest &request, int reason, bool directConnection = false)
|
|
{
|
|
sendErrorInternal(request.errorSlot, reason, request.uniqueId, -1, directConnection);
|
|
}
|
|
|
|
|
|
bool checkSlots(ClassicRequest &request, const char *replySlot, const char *errorSlot)
|
|
{
|
|
const QByteArray baReply = QByteArray::number(QSIGNAL_CODE) +
|
|
"signalSignature(" +
|
|
TopicNameHelper::replyTopicTypeName() +
|
|
",int,bool)";
|
|
request.replySlot = DDS::Helper::getMethod(m_logger, m_objToInform, replySlot, baReply.data());
|
|
if (!request.replySlot.isValid())
|
|
return false;
|
|
|
|
if (errorSlot)
|
|
{
|
|
const QByteArray baError = QByteArray::number(QSIGNAL_CODE) + "signalSignature(int,int,int)";
|
|
request.errorSlot = DDS::Helper::getMethod(m_logger, m_objToInform, errorSlot, baError.data());
|
|
if (!request.errorSlot.isValid())
|
|
return false;
|
|
}
|
|
else
|
|
request.errorSlot = QMetaMethod();
|
|
return true;
|
|
}
|
|
|
|
|
|
|
|
|
|
QObject *m_objToInform;
|
|
int m_uniqueId { 0 };
|
|
|
|
|
|
std::queue<ModernRequest<ReplyTopic> > m_modernRequests;
|
|
std::queue<ClassicRequest> m_classicRequests;
|
|
|
|
connext::RequesterParams m_requesterParams;
|
|
bool m_dying { false };
|
|
std::string m_partitionType;
|
|
int m_partitionId;
|
|
std::mutex m_requestMutex;
|
|
NGL::INGLLogger &m_logger;
|
|
};
|
|
|
|
}
|