Continues migration with DDS, Common
This commit is contained in:
torsten committed
1 parent
9b15cee841
commit
fa48db111e
668 files changed
+111841
-2
No files matched your search
Executable
+45
@@ -0,0 +1,45 @@
|
||||
#
|
||||
# $Id$
|
||||
# $HeadURL$
|
||||
# $Author$
|
||||
#
|
||||
|
||||
ADD_LIBRARY(ddsfts_connext INTERFACE)
|
||||
TARGET_INCLUDE_DIRECTORIES(ddsfts_connext
|
||||
INTERFACE
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../../
|
||||
)
|
||||
TARGET_LINK_LIBRARIES(ddsfts_connext
|
||||
INTERFACE
|
||||
RTI::nddscpp2
|
||||
)
|
||||
SET(SRCS
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/connext.h
|
||||
)
|
||||
TARGET_SOURCES(ddsfts_connext INTERFACE ${SRCS})
|
||||
|
||||
|
||||
SET(SRCS
|
||||
generic_fts.h ;# Interface
|
||||
generic_fts_impl.h ;# Implementation, using internal class GenericFTCInternal
|
||||
generic_fts_internal.h
|
||||
generic_fts_internal.cpp
|
||||
)
|
||||
|
||||
NGL_ADD_LIBRARY2(WS AAR SV HLA ddsfts SHARED ${SRCS})
|
||||
NGL_TARGET_INCLUDE_DIRECTORIES(
|
||||
INTERFACE
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../../
|
||||
)
|
||||
NGL_TARGET_LINK_LIBRARIES(ddsfts
|
||||
PRIVATE
|
||||
RTI::nddscore
|
||||
ProjectConfig
|
||||
ddsQoS
|
||||
PUBLIC
|
||||
Qt5::Core
|
||||
RTI::nddsc
|
||||
RTI::nddscpp2
|
||||
ddsgen
|
||||
ddsfts_connext
|
||||
)
|
||||
Executable
+502
@@ -0,0 +1,502 @@
|
||||
|
||||
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <dds/core/cond/GuardCondition.hpp>
|
||||
#include <dds/core/cond/StatusCondition.hpp>
|
||||
#include <dds/core/cond/WaitSet.hpp>
|
||||
#include <dds/domain/DomainParticipant.hpp>
|
||||
#include <dds/pub/DataWriter.hpp>
|
||||
#include <dds/pub/Publisher.hpp>
|
||||
#include <dds/sub/DataReader.hpp>
|
||||
#include <dds/sub/SampleInfo.hpp>
|
||||
#include <dds/sub/Subscriber.hpp>
|
||||
#include <dds/topic/ContentFilteredTopic.hpp>
|
||||
#include <dds/topic/find.hpp>
|
||||
#include <rti/pub/WriteParams.hpp>
|
||||
|
||||
|
||||
#include <NGLLogger/NGLLoggerBootstrap.h>
|
||||
|
||||
namespace connext
|
||||
{
|
||||
|
||||
namespace DebugInfo
|
||||
{
|
||||
inline QByteArray toReadable(const rti::core::Guid &guid)
|
||||
{
|
||||
const QByteArray str(reinterpret_cast<const char *>(&guid[0]), 16);
|
||||
return str.toHex();
|
||||
}
|
||||
inline QByteArray toReadable(const rti::core::SequenceNumber &seqNo,
|
||||
const rti::core::Guid &guid)
|
||||
{
|
||||
return "SeqNo: " + QByteArray::number(seqNo.value()) +
|
||||
", Guid-DW: " + toReadable(guid);
|
||||
}
|
||||
inline QByteArray toReadable(const rti::core::SampleIdentity &identiy)
|
||||
{
|
||||
return toReadable(identiy.sequence_number(), identiy.writer_guid());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
template<typename T>
|
||||
dds::topic::Topic<T> getTopic(const dds::domain::DomainParticipant &participant,
|
||||
const std::string &topicName)
|
||||
{
|
||||
using DataTopic = dds::topic::Topic<T>;
|
||||
auto ret = dds::topic::find<DataTopic>(participant, topicName);
|
||||
if (ret != nullptr)
|
||||
return ret;
|
||||
return DataTopic(participant, topicName);
|
||||
}
|
||||
|
||||
|
||||
template<typename T>
|
||||
class SampleBase
|
||||
{
|
||||
protected:
|
||||
SampleBase() = default;
|
||||
virtual ~SampleBase() = default;
|
||||
|
||||
public:
|
||||
void set_data(const T &data)
|
||||
{
|
||||
m_data = data;
|
||||
}
|
||||
T &data()
|
||||
{
|
||||
return m_data;
|
||||
}
|
||||
const T &data() const
|
||||
{
|
||||
return m_data;
|
||||
}
|
||||
protected:
|
||||
T m_data;
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
class WriteSample : public SampleBase<T>
|
||||
{
|
||||
public:
|
||||
WriteSample() = default;
|
||||
|
||||
void set_info(const rti::pub::WriteParams &info)
|
||||
{
|
||||
m_info = info;
|
||||
}
|
||||
rti::pub::WriteParams &info()
|
||||
{
|
||||
return m_info;
|
||||
}
|
||||
const rti::pub::WriteParams &info() const
|
||||
{
|
||||
return m_info;
|
||||
}
|
||||
|
||||
private:
|
||||
rti::pub::WriteParams m_info;
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
class WriteSampleRef
|
||||
{
|
||||
public:
|
||||
WriteSampleRef()
|
||||
: m_data_ptr(nullptr)
|
||||
, m_info_ptr(nullptr)
|
||||
{}
|
||||
|
||||
void set_data(T *data_ptr)
|
||||
{
|
||||
m_data_ptr = data_ptr;
|
||||
}
|
||||
|
||||
T &data()
|
||||
{
|
||||
return *m_data_ptr;
|
||||
}
|
||||
|
||||
void set_info(rti::pub::WriteParams *info_ptr)
|
||||
{
|
||||
m_info_ptr = info_ptr;
|
||||
}
|
||||
|
||||
rti::pub::WriteParams &info()
|
||||
{
|
||||
return *m_info_ptr;
|
||||
}
|
||||
|
||||
private:
|
||||
T *m_data_ptr;
|
||||
rti::pub::WriteParams *m_info_ptr;
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
class Sample : public SampleBase<T>
|
||||
{
|
||||
public:
|
||||
Sample() = default;
|
||||
|
||||
void set_info(const dds::sub::SampleInfo &info)
|
||||
{
|
||||
m_info = info;
|
||||
}
|
||||
dds::sub::SampleInfo &info()
|
||||
{
|
||||
return m_info;
|
||||
}
|
||||
const dds::sub::SampleInfo &info() const
|
||||
{
|
||||
return m_info;
|
||||
}
|
||||
|
||||
void set_identity(const rti::core::SampleIdentity &identity)
|
||||
{
|
||||
m_identity = identity;
|
||||
}
|
||||
|
||||
const rti::core::SampleIdentity &identity() const
|
||||
{
|
||||
return m_identity;
|
||||
}
|
||||
|
||||
private:
|
||||
dds::sub::SampleInfo m_info;
|
||||
rti::core::SampleIdentity m_identity;
|
||||
};
|
||||
|
||||
class RequesterParams
|
||||
{
|
||||
public:
|
||||
explicit RequesterParams(const dds::domain::DomainParticipant &participant)
|
||||
: m_participant(participant)
|
||||
, m_publisher(nullptr)
|
||||
, m_subscriber(nullptr)
|
||||
{}
|
||||
virtual ~RequesterParams() = default;
|
||||
|
||||
const std::string &request_topic_name() const
|
||||
{
|
||||
return m_request_topic_name;
|
||||
}
|
||||
void request_topic_name(const std::string &request_topic_name)
|
||||
{
|
||||
m_request_topic_name = request_topic_name;
|
||||
}
|
||||
|
||||
const std::string &reply_topic_name() const
|
||||
{
|
||||
return m_reply_topic_name;
|
||||
}
|
||||
void reply_topic_name(const std::string &reply_topic_name)
|
||||
{
|
||||
m_reply_topic_name = reply_topic_name;
|
||||
}
|
||||
|
||||
const dds::domain::DomainParticipant &domainParticipant() const
|
||||
{
|
||||
return m_participant;
|
||||
}
|
||||
|
||||
const dds::pub::Publisher &publisher() const
|
||||
{
|
||||
return m_publisher;
|
||||
}
|
||||
void publisher(const dds::pub::Publisher &publisher)
|
||||
{
|
||||
m_publisher = publisher;
|
||||
}
|
||||
|
||||
const dds::sub::Subscriber &subscriber() const
|
||||
{
|
||||
return m_subscriber;
|
||||
}
|
||||
void subscriber(const dds::sub::Subscriber &subscriber)
|
||||
{
|
||||
m_subscriber = subscriber;
|
||||
}
|
||||
|
||||
const dds::pub::qos::DataWriterQos &datawriter_qos() const
|
||||
{
|
||||
return m_datawriter_qos;
|
||||
}
|
||||
void datawriter_qos(const dds::pub::qos::DataWriterQos &datawriter_qos)
|
||||
{
|
||||
m_datawriter_qos = datawriter_qos;
|
||||
}
|
||||
|
||||
const dds::sub::qos::DataReaderQos &datareader_qos() const
|
||||
{
|
||||
return m_datareader_qos;
|
||||
}
|
||||
void datareader_qos(const dds::sub::qos::DataReaderQos &datareader_qos)
|
||||
{
|
||||
m_datareader_qos = datareader_qos;
|
||||
}
|
||||
|
||||
protected:
|
||||
dds::domain::DomainParticipant m_participant;
|
||||
dds::pub::Publisher m_publisher;
|
||||
dds::sub::Subscriber m_subscriber;
|
||||
dds::pub::qos::DataWriterQos m_datawriter_qos;
|
||||
dds::sub::qos::DataReaderQos m_datareader_qos;
|
||||
std::string m_request_topic_name;
|
||||
std::string m_reply_topic_name;
|
||||
};
|
||||
|
||||
template <typename REQUEST, typename REPLY>
|
||||
class ReplierParams : public RequesterParams
|
||||
{
|
||||
public:
|
||||
using RequesterParams::RequesterParams;
|
||||
};
|
||||
|
||||
template<typename REQUEST, typename REPLY>
|
||||
class ReplyRequestBase
|
||||
{
|
||||
protected:
|
||||
explicit ReplyRequestBase(const connext::RequesterParams &requesterParams)
|
||||
: m_requestTopic(getTopic<REQUEST>(requesterParams.domainParticipant(),
|
||||
requesterParams.request_topic_name()))
|
||||
, m_replyTopic(getTopic<REPLY>(requesterParams.domainParticipant(),
|
||||
requesterParams.reply_topic_name()))
|
||||
{}
|
||||
virtual ~ReplyRequestBase() = default;
|
||||
|
||||
protected:
|
||||
dds::topic::Topic<REQUEST> m_requestTopic;
|
||||
dds::topic::Topic<REPLY> m_replyTopic;
|
||||
};
|
||||
|
||||
template<typename REQUEST, typename REPLY>
|
||||
class Requester final : public ReplyRequestBase<REQUEST, REPLY>
|
||||
{
|
||||
public:
|
||||
explicit Requester(const connext::RequesterParams &requesterParams)
|
||||
: ReplyRequestBase<REQUEST, REPLY>(requesterParams)
|
||||
, m_requesterParams(requesterParams)
|
||||
, m_dataWriter(nullptr)
|
||||
, m_dataReader(nullptr)
|
||||
, m_contentFilteredTopic(nullptr)
|
||||
, m_curInstanceHandle(dds::core::InstanceHandle::nil())
|
||||
, m_logger(NGL::NGLLoggerBootstrap::getNGLRootLogger().getNGLLogger("connext.requester"))
|
||||
{}
|
||||
|
||||
void create_contentfilter(bool bNewDataWriter)
|
||||
{
|
||||
|
||||
if (!bNewDataWriter && m_contentFilteredTopic != nullptr)
|
||||
return;
|
||||
|
||||
rti::core::policy::DataWriterProtocol dwp;
|
||||
m_dataWriter.qos() >> dwp;
|
||||
const rti::core::Guid dwGuid = dwp.virtual_guid();
|
||||
const QByteArray writerGuid = DebugInfo::toReadable(dwGuid);
|
||||
const QByteArray filterContent = "@related_sample_identity.writer_guid.value = &hex(" + writerGuid + ")";
|
||||
if (m_contentFilteredTopic == nullptr)
|
||||
{
|
||||
|
||||
const std::string rtName = ReplyRequestBase<REQUEST, REPLY>::m_replyTopic.name();
|
||||
const QByteArray filterName(QByteArray::fromRawData(rtName.c_str(), static_cast<int>(rtName.size())) +
|
||||
QByteArray(" ") + writerGuid);
|
||||
m_contentFilteredTopic = dds::topic::ContentFilteredTopic<REPLY>(ReplyRequestBase<REQUEST, REPLY>::m_replyTopic,
|
||||
filterName.data(),
|
||||
dds::topic::Filter(filterContent.data()));
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
m_contentFilteredTopic->filter(dds::topic::Filter(filterContent.data()));
|
||||
}
|
||||
DEBUG_LOG(m_logger) << "content filter for topic "
|
||||
<< ReplyRequestBase<REQUEST, REPLY>::m_requestTopic.name()
|
||||
<< " - related_sample_identity should be: "
|
||||
<< writerGuid;
|
||||
}
|
||||
|
||||
void send_request(connext::WriteSample<REQUEST> &request)
|
||||
{
|
||||
bool bNewDataWriter = false;
|
||||
if (m_dataWriter == nullptr)
|
||||
{
|
||||
m_dataWriter = dds::pub::DataWriter<REQUEST>(m_requesterParams.publisher(),
|
||||
ReplyRequestBase<REQUEST, REPLY>::m_requestTopic,
|
||||
m_requesterParams.datawriter_qos());
|
||||
bNewDataWriter = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
create_contentfilter(bNewDataWriter);
|
||||
|
||||
if (m_dataReader == nullptr)
|
||||
{
|
||||
|
||||
m_dataReader = dds::sub::DataReader<REPLY>(m_requesterParams.subscriber(),
|
||||
m_contentFilteredTopic,
|
||||
m_requesterParams.datareader_qos());
|
||||
}
|
||||
|
||||
m_curInstanceHandle = m_dataWriter.register_instance(request.data());
|
||||
request.info().replace_automatic_values(true);
|
||||
request.info().identity(rti::core::SampleIdentity::automatic());
|
||||
m_dataWriter->write(request.data(), request.info());
|
||||
|
||||
const rti::core::SampleIdentity currentSampleIdentity = request.info().identity();
|
||||
DEBUG_LOG(m_logger) << "send_request for topic "
|
||||
<< ReplyRequestBase<REQUEST, REPLY>::m_requestTopic.name()
|
||||
<< ", m_currentSampleIdentity: "
|
||||
<< DebugInfo::toReadable(currentSampleIdentity);
|
||||
}
|
||||
|
||||
bool receive_reply(connext::Sample<REPLY> &reply,
|
||||
const dds::core::Duration &duration)
|
||||
{
|
||||
auto condition(dds::sub::cond::ReadCondition(m_dataReader, dds::sub::status::SampleState::not_read()));
|
||||
|
||||
dds::core::cond::WaitSet waitset;
|
||||
waitset.attach_condition(condition);
|
||||
|
||||
bool bRet = false;
|
||||
while (true)
|
||||
{
|
||||
|
||||
if (m_dataReader->take(reply.data(), reply.info()) && reply.info().valid())
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "receive_reply for topic "
|
||||
<< ReplyRequestBase<REQUEST, REPLY>::m_requestTopic.name()
|
||||
<< ", SampleIdentity - original publication virtual: "
|
||||
<< DebugInfo::toReadable(reply.info()->original_publication_virtual_sample_identity());
|
||||
|
||||
DEBUG_LOG(m_logger) << "receive_reply for topic "
|
||||
<< ReplyRequestBase<REQUEST, REPLY>::m_requestTopic.name()
|
||||
<< ", SampleIdentity - related original: "
|
||||
<< DebugInfo::toReadable(reply.info()->related_original_publication_virtual_sample_identity());
|
||||
|
||||
|
||||
bRet = true;
|
||||
break;
|
||||
}
|
||||
|
||||
dds::core::cond::WaitSet::ConditionSeq activeConditionsSeq = waitset.wait(duration);
|
||||
if (activeConditionsSeq.empty())
|
||||
{
|
||||
ERROR_LOG(m_logger) << "Bailing out!";
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
static const rti::core::SampleFlag irs(rti::core::SampleFlag::intermediate_reply_sequence());
|
||||
if ((reply.info()->flag() & irs) == 0)
|
||||
{
|
||||
if (!m_curInstanceHandle.is_nil())
|
||||
{
|
||||
|
||||
DEBUG_LOG(m_logger) << "Keyed topic - disposing instance: "
|
||||
<< ReplyRequestBase<REQUEST, REPLY>::m_requestTopic.name();
|
||||
m_dataWriter.dispose_instance(m_curInstanceHandle);
|
||||
}
|
||||
}
|
||||
|
||||
return bRet;
|
||||
}
|
||||
|
||||
dds::pub::DataWriter<REQUEST> get_request_datawriter() const { return m_dataWriter; }
|
||||
dds::sub::DataReader<REPLY> get_request_datareader() const { return m_dataReader; }
|
||||
|
||||
private:
|
||||
connext::RequesterParams m_requesterParams;
|
||||
dds::pub::DataWriter<REQUEST> m_dataWriter;
|
||||
dds::sub::DataReader<REPLY> m_dataReader;
|
||||
dds::topic::ContentFilteredTopic<REPLY> m_contentFilteredTopic;
|
||||
dds::core::InstanceHandle m_curInstanceHandle;
|
||||
NGL::INGLLogger &m_logger;
|
||||
};
|
||||
|
||||
template<typename REQUEST, typename REPLY>
|
||||
class Replier final : public ReplyRequestBase<REQUEST, REPLY>
|
||||
{
|
||||
public:
|
||||
explicit Replier(const connext::ReplierParams<REQUEST, REPLY> &replierParams)
|
||||
: ReplyRequestBase<REQUEST, REPLY>(replierParams)
|
||||
, m_replierParams(replierParams)
|
||||
, m_dataWriter(dds::pub::DataWriter<REPLY>(replierParams.publisher(),
|
||||
ReplyRequestBase<REQUEST, REPLY>::m_replyTopic,
|
||||
replierParams.datawriter_qos()))
|
||||
, m_dataReader(dds::sub::DataReader<REQUEST>(replierParams.subscriber(),
|
||||
ReplyRequestBase<REQUEST, REPLY>::m_requestTopic,
|
||||
replierParams.datareader_qos()))
|
||||
, m_logger(NGL::NGLLoggerBootstrap::getNGLRootLogger().getNGLLogger("connext.replier"))
|
||||
{}
|
||||
|
||||
void send_reply(connext::WriteSample<REPLY> &reply,
|
||||
const rti::core::SampleIdentity &identity,
|
||||
bool bLastSample = true)
|
||||
{
|
||||
connext::WriteSampleRef<REPLY> replyRef;
|
||||
replyRef.set_data(&reply.data());
|
||||
replyRef.set_info(&reply.info());
|
||||
return send_reply(replyRef, identity, bLastSample);
|
||||
}
|
||||
|
||||
void send_reply(connext::WriteSampleRef<REPLY> &reply,
|
||||
const rti::core::SampleIdentity &identity,
|
||||
bool bLastSample = true)
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "send_reply for topic "
|
||||
<< ReplyRequestBase<REQUEST, REPLY>::m_requestTopic.name()
|
||||
<< ", sampleIdentity: " << DebugInfo::toReadable(identity);
|
||||
|
||||
rti::core::SampleFlag sf = reply.info().flag();
|
||||
if (bLastSample)
|
||||
sf &= ~rti::core::SampleFlag::intermediate_reply_sequence();
|
||||
else
|
||||
sf |= rti::core::SampleFlag::intermediate_reply_sequence();
|
||||
reply.info().flag(sf);
|
||||
reply.info().related_sample_identity(identity);
|
||||
DEBUG_LOG(m_logger) << "send_reply for topic "
|
||||
<< ReplyRequestBase<REQUEST, REPLY>::m_requestTopic.name()
|
||||
<< ", related sampleIdentity: " << DebugInfo::toReadable(reply.info().related_sample_identity());
|
||||
|
||||
m_dataWriter->write(reply.data(), reply.info());
|
||||
}
|
||||
|
||||
bool receive_request(connext::Sample<REQUEST> &request)
|
||||
{
|
||||
if (!m_dataReader->take(request.data(), request.info()))
|
||||
return false;
|
||||
|
||||
if (request.info().valid())
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "receive_request for topic "
|
||||
<< ReplyRequestBase<REQUEST, REPLY>::m_requestTopic.name()
|
||||
<< ", sampleIdentity publication_virtual_sample_identity: "
|
||||
<< DebugInfo::toReadable(request.info()->publication_virtual_sample_identity())
|
||||
<< ", original_publication_virtual_sample_identity: "
|
||||
<< DebugInfo::toReadable(request.info()->original_publication_virtual_sample_identity())
|
||||
<< ", related_original_publication_virtual_sample_identity: "
|
||||
<< DebugInfo::toReadable(request.info()->related_original_publication_virtual_sample_identity())
|
||||
<< " with timestamp_ " << request.info().source_timestamp().to_millisecs();
|
||||
request.set_identity(request.info()->original_publication_virtual_sample_identity());
|
||||
}
|
||||
|
||||
return request.info().valid();
|
||||
}
|
||||
|
||||
dds::pub::DataWriter<REPLY> get_request_datawriter() const { return m_dataWriter; }
|
||||
dds::sub::DataReader<REQUEST> get_request_datareader() const { return m_dataReader; }
|
||||
|
||||
private:
|
||||
connext::ReplierParams<REQUEST, REPLY> m_replierParams;
|
||||
dds::pub::DataWriter<REPLY> m_dataWriter;
|
||||
dds::sub::DataReader<REQUEST> m_dataReader;
|
||||
NGL::INGLLogger &m_logger;
|
||||
};
|
||||
|
||||
}
|
||||
Executable
+26
@@ -0,0 +1,26 @@
|
||||
|
||||
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <rti/core/SampleIdentity.hpp>
|
||||
#include <QVector>
|
||||
|
||||
|
||||
|
||||
|
||||
namespace FTS
|
||||
{
|
||||
|
||||
template <typename TOPICTYPE_REQUEST, typename TOPICTYPE_REPLY>
|
||||
class GenericFts
|
||||
{
|
||||
public:
|
||||
GenericFts() = default;
|
||||
virtual ~GenericFts() = default;
|
||||
|
||||
virtual bool sendReply(const rti::core::SampleIdentity &requestIdentity, const TOPICTYPE_REPLY &replyData) = 0;
|
||||
virtual bool sendReply(const rti::core::SampleIdentity &requestIdentity, const QVector<TOPICTYPE_REPLY> &replyData) = 0;
|
||||
};
|
||||
|
||||
}
|
||||
Executable
+287
@@ -0,0 +1,287 @@
|
||||
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <DDS/Core/core.hpp>
|
||||
|
||||
#include "generic_fts_internal.h"
|
||||
#include "connext.h"
|
||||
#include <DDS/Generic/wait_set_handler.h>
|
||||
#include <DDS/Generic/generic_base.h>
|
||||
#include <DDS/Generic/generic_logger.h>
|
||||
|
||||
#include <queue>
|
||||
#include <type_traits>
|
||||
#include <typeinfo>
|
||||
#include <sstream>
|
||||
|
||||
#ifndef tr_lts
|
||||
#define tr_lts(ctx, t) (t)
|
||||
#endif
|
||||
|
||||
namespace FTS
|
||||
{
|
||||
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<typename GENERIC_FTS>
|
||||
struct TopicNames;
|
||||
|
||||
template<typename TOPICTYPE_REQUEST, typename TOPICTYPE_REPLY, typename DUMMY>
|
||||
class GenericFtsImpl final : public GenericFts<TOPICTYPE_REQUEST,TOPICTYPE_REPLY>
|
||||
, public GenericFtsInternal
|
||||
{
|
||||
public:
|
||||
using RequestTopic = TOPICTYPE_REQUEST;
|
||||
using ReplyTopic = TOPICTYPE_REPLY;
|
||||
using TopicNameHelper = TopicNames<GenericFtsImpl<RequestTopic, ReplyTopic, DUMMY>>;
|
||||
private:
|
||||
using SubscriberCallback = std::function<void(const rti::core::SampleIdentity&, const RequestTopic&)>;
|
||||
|
||||
template <typename T = ReplyTopic, typename std::enable_if<HasStatusFunction<T>::value, int>::type = 0>
|
||||
void sendReplyState(Core::FtReplyState replyState, const rti::core::SampleIdentity &requestSampleIdentity)
|
||||
{
|
||||
connext::WriteSample<ReplyTopic> reply;
|
||||
reply.data().replyState(replyState);
|
||||
|
||||
bool bLastTelegram = ((replyState != Core::FtReplyState_def::InProgress) &&
|
||||
(replyState != Core::FtReplyState_def::NotStarted));
|
||||
m_replier->send_reply(reply, requestSampleIdentity, bLastTelegram);
|
||||
}
|
||||
|
||||
template <typename T = ReplyTopic, typename std::enable_if<!HasStatusFunction<T>::value, int>::type = 0>
|
||||
void sendReplyState(Core::FtReplyState, const rti::core::SampleIdentity &)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
template <typename T, typename std::enable_if<HasStatusFunction<T>::value, int>::type = 0>
|
||||
void setReply(T &data, Core::FtReplyState status)
|
||||
{
|
||||
data.replyState(status);
|
||||
}
|
||||
|
||||
template <typename T, typename std::enable_if<!HasStatusFunction<T>::value, int>::type = 0>
|
||||
void setReply(T &, Core::FtReplyState)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
public:
|
||||
GenericFtsImpl(const dds::domain::DomainParticipant &domainParticipant,
|
||||
const dds::pub::Publisher &ddsPublisher,
|
||||
const dds::sub::Subscriber &ddsSubscriber,
|
||||
DDSGeneric::WaitSetHandler *wsHandler)
|
||||
: GenericFts<RequestTopic,ReplyTopic>()
|
||||
, GenericFtsInternal()
|
||||
, m_wsHandler(wsHandler)
|
||||
, m_subscriberCallback(nullptr)
|
||||
, m_logger(DDS::getLogger(TopicNameHelper::functionTopicName(), DDS::DataType::FUNCTION_TOPIC_SERVER))
|
||||
{
|
||||
Q_ASSERT(domainParticipant != nullptr);
|
||||
Q_ASSERT(ddsPublisher != nullptr);
|
||||
Q_ASSERT(ddsSubscriber != nullptr);
|
||||
Q_ASSERT(wsHandler != nullptr);
|
||||
|
||||
qRegisterMetaType<RequestTopic> (TopicNameHelper::requestTopicTypeName());
|
||||
|
||||
connext::ReplierParams<RequestTopic,ReplyTopic> replierParams(domainParticipant);
|
||||
|
||||
|
||||
|
||||
|
||||
replierParams.request_topic_name(TopicNameHelper::requestTopicName());
|
||||
replierParams.reply_topic_name(TopicNameHelper::replyTopicName());
|
||||
replierParams.publisher(ddsPublisher);
|
||||
replierParams.subscriber(ddsSubscriber);
|
||||
replierParams.datareader_qos(readerQos(m_logger));
|
||||
replierParams.datawriter_qos(writerQos(m_logger));
|
||||
|
||||
|
||||
|
||||
|
||||
m_replier = new connext::Replier<RequestTopic, ReplyTopic>(replierParams);
|
||||
m_readCondition = new dds::sub::cond::ReadCondition(m_replier->get_request_datareader(), dds::sub::status::SampleState::not_read());
|
||||
|
||||
m_wsHandler->attachWaitSet(*m_readCondition, this);
|
||||
}
|
||||
|
||||
~GenericFtsImpl()
|
||||
{
|
||||
m_wsHandler->detachWaitSet(*m_readCondition, this);
|
||||
delete m_readCondition;
|
||||
delete m_replier;
|
||||
}
|
||||
|
||||
bool setSubscriberCallback(const SubscriberCallback &subscriberCallback)
|
||||
{
|
||||
if (m_subscriberCallback)
|
||||
{
|
||||
WARNING_LOG(m_logger) << "Can not set subscriber callback - already set ...";
|
||||
return false;
|
||||
}
|
||||
|
||||
m_subscriberCallback = subscriberCallback;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
bool sendReply(const rti::core::SampleIdentity &requestSampleIdentity, const ReplyTopic &replyData) override final
|
||||
{
|
||||
connext::WriteSampleRef<ReplyTopic> reply;
|
||||
rti::pub::WriteParams replyParams;
|
||||
|
||||
reply.set_data(const_cast<ReplyTopic*>(&replyData));
|
||||
reply.set_info(&replyParams);
|
||||
|
||||
m_replier->send_reply(reply, requestSampleIdentity);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
bool sendReply(const rti::core::SampleIdentity &requestIdentity, const QVector<ReplyTopic> &replyData) override final
|
||||
{
|
||||
DEBUG_LOG(m_logger) << TopicNameHelper::requestTopicName() << " - IN sendReply";
|
||||
|
||||
for(int i = 0; i < replyData.size(); ++i)
|
||||
{
|
||||
if (HasStatusFunction<ReplyTopic>::value)
|
||||
{
|
||||
ReplyTopic rt = replyData.at(i);
|
||||
setReply(rt, i == replyData.size() - 1 ? Core::FtReplyState::Completed : Core::FtReplyState::InProgress);
|
||||
if (!sendReply(requestIdentity, rt))
|
||||
return false;
|
||||
} else {
|
||||
if (!sendReply(requestIdentity, replyData.at(i)))
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
protected:
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void processRequest(const connext::Sample<RequestTopic> &request)
|
||||
{
|
||||
DEBUG_LOG(m_logger) << TopicNameHelper::requestTopicName() << " - processRequest: "
|
||||
<< connext::DebugInfo::toReadable(request.identity());
|
||||
|
||||
qint64 age = qAbs(qint64(request.info().source_timestamp().to_secs()) - time(nullptr));
|
||||
|
||||
if (age > 10)
|
||||
{
|
||||
std::ostringstream ss;
|
||||
ss << request.data();
|
||||
std::string s = ss.str();
|
||||
int slen = s.length();
|
||||
|
||||
static int max_slen = 1024;
|
||||
if (slen > max_slen)
|
||||
{
|
||||
s.resize(max_slen);
|
||||
s.append("...");
|
||||
}
|
||||
|
||||
DEBUG_LOG(m_logger) << "received a request for topic "
|
||||
<< TopicNameHelper::requestTopicName()
|
||||
<< " but it was outdated - ignoring. "
|
||||
<< " local time: " << (time(nullptr) - 10) << "[s], source timestamp: "
|
||||
<< qint64(request.info().source_timestamp().to_secs())
|
||||
<< "[s], -> age: " << age
|
||||
<< "[s], data: " << s;
|
||||
ReplyTopic rt;
|
||||
rt.errorString(tr_lts("dds", "Topic %1 was outdated and therefore ignored. Does the client time match the server time?"));
|
||||
rt.errorParams({TopicNameHelper::requestTopicName()});
|
||||
sendReply(request.identity(), rt);
|
||||
return;
|
||||
}
|
||||
|
||||
sendReplyState(Core::FtReplyState::InProgress, request.identity());
|
||||
|
||||
m_subscriberCallback(request.identity(), request.data());
|
||||
}
|
||||
|
||||
|
||||
|
||||
void processRequests() override
|
||||
{
|
||||
DEBUG_LOG(m_logger) << TopicNameHelper::requestTopicName() << " - processRequests";
|
||||
|
||||
while (true)
|
||||
{
|
||||
connext::Sample<RequestTopic> request;
|
||||
{
|
||||
std::lock_guard<std::mutex> l(m_requestMutex);
|
||||
if (m_requests.empty())
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "No more requests";
|
||||
return;
|
||||
}
|
||||
DEBUG_LOG(m_logger) << TopicNameHelper::requestTopicName() << " - read one request for ";
|
||||
request = m_requests.front();
|
||||
m_requests.pop();
|
||||
}
|
||||
if (!request.info().valid())
|
||||
{
|
||||
WARNING_LOG(m_logger) << TopicNameHelper::requestTopicName() << " - No valid data";
|
||||
continue;
|
||||
}
|
||||
processRequest(request);
|
||||
}
|
||||
DEBUG_LOG(m_logger) << TopicNameHelper::requestTopicName() << " - processRequests finished";
|
||||
}
|
||||
|
||||
bool enqueueRequests() override
|
||||
{
|
||||
bool bValidDataRead = false;
|
||||
connext::Sample<RequestTopic> request;
|
||||
while (m_replier->receive_request(request))
|
||||
{
|
||||
if (!request.info().valid())
|
||||
continue;
|
||||
bValidDataRead = true;
|
||||
std::lock_guard<std::mutex> l(m_requestMutex);
|
||||
m_requests.push(request);
|
||||
}
|
||||
return bValidDataRead;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
connext::Replier<RequestTopic, ReplyTopic> *m_replier;
|
||||
dds::sub::cond::ReadCondition *m_readCondition;
|
||||
|
||||
|
||||
DDSGeneric::WaitSetHandler *m_wsHandler;
|
||||
|
||||
std::queue<connext::Sample<RequestTopic>> m_requests;
|
||||
std::mutex m_requestMutex;
|
||||
|
||||
|
||||
SubscriberCallback m_subscriberCallback;
|
||||
|
||||
NGL::INGLLogger &m_logger;
|
||||
};
|
||||
|
||||
}
|
||||
Executable
+48
@@ -0,0 +1,48 @@
|
||||
|
||||
|
||||
|
||||
#include "generic_fts_internal.h"
|
||||
#include <DDS/QosProfile/DDS_DefaultQos.h>
|
||||
#include <QMetaMethod>
|
||||
#include <NGLLogger/INGLLogger.h>
|
||||
#include <qglobal.h>
|
||||
|
||||
namespace FTS
|
||||
{
|
||||
|
||||
void GenericFtsInternal::printDefaultQos(NGL::INGLLogger &logger)
|
||||
{
|
||||
if (logger.isDebugEnabled())
|
||||
DDS::QosProfile::print(dds::core::QosProvider::Default(), logger);
|
||||
}
|
||||
|
||||
dds::sub::qos::DataReaderQos GenericFtsInternal::readerQos(NGL::INGLLogger &logger)
|
||||
{
|
||||
dds::sub::qos::DataReaderQos qos = DDS::QosProfile::dataReaderDefault(DDS::QosProfile::TopicType::FunctionTopic);
|
||||
if (logger.isDebugEnabled())
|
||||
DDS::QosProfile::print(qos, logger);
|
||||
return qos;
|
||||
}
|
||||
|
||||
dds::pub::qos::DataWriterQos GenericFtsInternal::writerQos(NGL::INGLLogger &logger)
|
||||
{
|
||||
dds::pub::qos::DataWriterQos qos = DDS::QosProfile::dataWriterDefault(DDS::QosProfile::TopicType::FunctionTopic);
|
||||
if (logger.isDebugEnabled())
|
||||
DDS::QosProfile::print(qos, logger);
|
||||
return qos;
|
||||
}
|
||||
|
||||
void GenericFtsInternal::dataAvailable()
|
||||
{
|
||||
if (!enqueueRequests())
|
||||
return;
|
||||
#if QT_VERSION < QT_VERSION_CHECK(5,10,0)
|
||||
if (!QMetaObject::invokeMethod(this, "processRequests", Qt::QueuedConnection))
|
||||
abort();
|
||||
#else
|
||||
QMetaObject::invokeMethod(this, &GenericFtsInternal::processRequests, Qt::QueuedConnection);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
Executable
+34
@@ -0,0 +1,34 @@
|
||||
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "generic_fts.h"
|
||||
#include <DDS/FTS/ddsfts_export.h>
|
||||
#include <DDS/Generic/wait_set_handler.h>
|
||||
#include <dds/sub/qos/DataReaderQos.hpp>
|
||||
#include <dds/pub/qos/DataWriterQos.hpp>
|
||||
|
||||
#include <QObject>
|
||||
|
||||
namespace FTS
|
||||
{
|
||||
class DDSFTS_EXPORT GenericFtsInternal : public QObject
|
||||
, protected DDSGeneric::WaitSetHandler::CallbackInterface
|
||||
{
|
||||
Q_OBJECT
|
||||
protected:
|
||||
GenericFtsInternal() = default;
|
||||
|
||||
static void printDefaultQos(NGL::INGLLogger &logger);
|
||||
static dds::sub::qos::DataReaderQos readerQos(NGL::INGLLogger &logger);
|
||||
static dds::pub::qos::DataWriterQos writerQos(NGL::INGLLogger &logger);
|
||||
|
||||
protected:
|
||||
|
||||
void dataAvailable() override;
|
||||
virtual bool enqueueRequests() = 0;
|
||||
protected Q_SLOTS:
|
||||
virtual void processRequests() = 0;
|
||||
};
|
||||
|
||||
}
|
||||
Reference in new issue
Block a user