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
+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;
|
||||
};
|
||||
|
||||
}
|
||||
Reference in new issue
Block a user