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
+363
@@ -0,0 +1,363 @@
|
||||
|
||||
|
||||
|
||||
#include <dds/pub/DataWriter.hpp>
|
||||
#include <dds/pub/discovery.hpp>
|
||||
#include <dds/sub/DataReader.hpp>
|
||||
#include <dds/sub/DataReaderListener.hpp>
|
||||
#include <dds/topic/find.hpp>
|
||||
#include <dds/topic/Topic.hpp>
|
||||
|
||||
#include <DDS/Generic/generic_base.h>
|
||||
#include <DDS/QosProfile/DDS_DefaultQos.h>
|
||||
#include <NGLLogger/INGLLogger.h>
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
namespace DebugInfo
|
||||
{
|
||||
static inline QByteArray toReadable(const rti::core::Guid &guid)
|
||||
{
|
||||
return QByteArray(reinterpret_cast<const char *>(&guid[0]), 16).toHex();
|
||||
}
|
||||
static inline QByteArray toReadable(const rti::core::SequenceNumber &seqNo,
|
||||
const rti::core::Guid &guid)
|
||||
{
|
||||
return "SeqNo: " + QByteArray::number(seqNo.value()) + ", Guid-DW: " + toReadable(guid);
|
||||
}
|
||||
static inline QByteArray toReadable(const rti::core::SampleIdentity &identiy)
|
||||
{
|
||||
return toReadable(identiy.sequence_number(), identiy.writer_guid());
|
||||
}
|
||||
}
|
||||
|
||||
namespace FTS
|
||||
{
|
||||
template<typename GENERIC_FTS>
|
||||
struct TopicNames;
|
||||
}
|
||||
namespace FTC
|
||||
{
|
||||
template<typename GENERIC_FTC>
|
||||
struct TopicNames;
|
||||
}
|
||||
|
||||
namespace RoutingService
|
||||
{
|
||||
|
||||
|
||||
template<typename T>
|
||||
static 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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
class SampleWriterInterface
|
||||
{
|
||||
public:
|
||||
SampleWriterInterface() = default;
|
||||
virtual ~SampleWriterInterface() = default;
|
||||
|
||||
virtual void writeSamples() = 0;
|
||||
|
||||
virtual NGL::INGLLogger &logger() = 0;
|
||||
};
|
||||
|
||||
class DataWriterTaskInterface
|
||||
{
|
||||
public:
|
||||
DataWriterTaskInterface() = default;
|
||||
virtual ~DataWriterTaskInterface() = default;
|
||||
|
||||
virtual void enqueue(SampleWriterInterface *swi) = 0;
|
||||
};
|
||||
|
||||
class TopicDispatcherInterface
|
||||
{
|
||||
public:
|
||||
TopicDispatcherInterface() = default;
|
||||
virtual ~TopicDispatcherInterface() = default;
|
||||
|
||||
virtual bool init(DataWriterTaskInterface *dwt,
|
||||
const dds::domain::DomainParticipant &dpIn,
|
||||
const dds::sub::Subscriber &subIn,
|
||||
const dds::domain::DomainParticipant &dpOut,
|
||||
const dds::pub::Publisher &pubOut) = 0;
|
||||
virtual void shutdown() = 0;
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
template<typename TOPIC>
|
||||
class SampleWriter final : public SampleWriterInterface
|
||||
{
|
||||
public:
|
||||
SampleWriter(dds::sub::LoanedSamples<TOPIC> &&samples,
|
||||
const dds::pub::DataWriter<TOPIC> &dataWriter,
|
||||
bool bWithOriginalIdentity,
|
||||
NGL::INGLLogger &logger)
|
||||
: m_loanedSamples(std::move(samples))
|
||||
, m_dataWriter(dataWriter)
|
||||
, m_bWithOriginalIdentity(bWithOriginalIdentity)
|
||||
, m_logger(logger)
|
||||
{}
|
||||
|
||||
rti::core::SampleIdentity getIdentity(const rti::core::SampleIdentity &identity) const
|
||||
{
|
||||
|
||||
static const auto s_unknownSequenceNumber(rti::core::SequenceNumber::unknown());
|
||||
static const auto s_unknownGui(rti::core::Guid::unknown());
|
||||
static const auto s_unknownSampleIdentity(rti::core::SampleIdentity::unknown());
|
||||
|
||||
if (!m_bWithOriginalIdentity)
|
||||
return s_unknownSampleIdentity;
|
||||
|
||||
if (identity.sequence_number() == s_unknownSequenceNumber)
|
||||
return s_unknownSampleIdentity;
|
||||
if (identity.writer_guid() == s_unknownGui)
|
||||
return s_unknownSampleIdentity;
|
||||
return identity;
|
||||
}
|
||||
|
||||
void writeSamples() override
|
||||
{
|
||||
TRACE_LOG(m_logger) << "Writing samples for " << DDS::topic_type_name<TOPIC>::value()
|
||||
<< ", sample count: " << m_loanedSamples.length();
|
||||
rti::pub::WriteParams writeParams;
|
||||
for (const auto &sample : m_loanedSamples)
|
||||
{
|
||||
try
|
||||
{
|
||||
writeParams.identity(getIdentity(sample.info()->original_publication_virtual_sample_identity()));
|
||||
writeParams.related_sample_identity(getIdentity(sample.info()->related_original_publication_virtual_sample_identity()));
|
||||
|
||||
|
||||
writeParams.handle(sample.info().instance_handle());
|
||||
writeParams.flag(sample.info()->flag());
|
||||
|
||||
|
||||
|
||||
TRACE_LOG(m_logger) << "identity: " << DebugInfo::toReadable(writeParams.identity())
|
||||
<< ", related_sample_identity: " << DebugInfo::toReadable(writeParams.related_sample_identity());
|
||||
|
||||
const auto &state = sample.info().state();
|
||||
const dds::sub::status::InstanceState &instanceState = state.instance_state();
|
||||
if (instanceState == dds::sub::status::InstanceState::alive())
|
||||
{
|
||||
m_dataWriter->write(sample.data(), writeParams);
|
||||
}
|
||||
else if (instanceState == dds::sub::status::InstanceState::not_alive_disposed())
|
||||
{
|
||||
TRACE_LOG(m_logger) << " - is disposed";
|
||||
m_dataWriter->dispose_instance(writeParams);
|
||||
}
|
||||
else if (instanceState == dds::sub::status::InstanceState::not_alive_no_writers())
|
||||
{
|
||||
TRACE_LOG(m_logger) << " - is unregistered";
|
||||
m_dataWriter->unregister_instance(writeParams);
|
||||
}
|
||||
else
|
||||
WARNING_LOG(m_logger) << "Unknown instance state: " << instanceState;
|
||||
}
|
||||
catch (const dds::core::Exception &ex)
|
||||
{
|
||||
ERROR_LOG(m_logger) << "writeSamples exception: " << ex.what();
|
||||
}
|
||||
}
|
||||
m_dataWriter->flush();
|
||||
}
|
||||
NGL::INGLLogger &logger() override { return m_logger; }
|
||||
private:
|
||||
dds::sub::LoanedSamples<TOPIC> m_loanedSamples;
|
||||
dds::pub::DataWriter<TOPIC> m_dataWriter;
|
||||
bool m_bWithOriginalIdentity;
|
||||
NGL::INGLLogger &m_logger;
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
class DataTopicListener final : public dds::sub::DataReaderListener<T>
|
||||
{
|
||||
public:
|
||||
using DataReader = dds::sub::DataReader<T>;
|
||||
|
||||
DataTopicListener(DataWriterTaskInterface *dwt,
|
||||
const std::string &topicName,
|
||||
DDS::QosProfile::TopicType topicType,
|
||||
NGL::INGLLogger &logger)
|
||||
: m_dwt(dwt)
|
||||
, m_dataWriter(nullptr)
|
||||
, m_topicName(topicName)
|
||||
, m_topicType(topicType)
|
||||
, m_logger(logger)
|
||||
{}
|
||||
|
||||
bool init(const dds::domain::DomainParticipant &dpOut,
|
||||
const dds::pub::Publisher &pubOut)
|
||||
{
|
||||
if (!m_dwt)
|
||||
return false;
|
||||
DEBUG_LOG(m_logger) << "Creating DataWriter for domain " << dpOut.domain_id();
|
||||
m_dataWriter = dds::pub::DataWriter<T>(pubOut,
|
||||
getTopic<T>(dpOut, m_topicName),
|
||||
DDS::QosProfile::dataWriterDefault(m_topicType));
|
||||
|
||||
|
||||
dds::domain::DomainParticipant &dpOutNonConst = const_cast<dds::domain::DomainParticipant &>(dpOut);
|
||||
dds::pub::ignore(dpOutNonConst, m_dataWriter.instance_handle());
|
||||
return true;
|
||||
}
|
||||
|
||||
void on_data_available(dds::sub::DataReader<T> &reader) override
|
||||
{
|
||||
|
||||
|
||||
TRACE_LOG(m_logger) << "Data available for " << DDS::topic_type_name<T>::value();
|
||||
auto ws = new SampleWriter<T>(reader.take(), m_dataWriter, m_topicType == DDS::QosProfile::TopicType::FunctionTopic, m_logger);
|
||||
m_dwt->enqueue(ws);
|
||||
}
|
||||
|
||||
void on_requested_deadline_missed(dds::sub::DataReader<T> &, const dds::core::status::RequestedDeadlineMissedStatus &) override {}
|
||||
void on_requested_incompatible_qos(dds::sub::DataReader<T> &,
|
||||
const dds::core::status::RequestedIncompatibleQosStatus &) override {}
|
||||
void on_sample_rejected(dds::sub::DataReader<T> &,
|
||||
const dds::core::status::SampleRejectedStatus &) override {}
|
||||
void on_liveliness_changed(dds::sub::DataReader<T> &,
|
||||
const dds::core::status::LivelinessChangedStatus &) override {}
|
||||
void on_subscription_matched(dds::sub::DataReader<T> &,
|
||||
const dds::core::status::SubscriptionMatchedStatus &) override {}
|
||||
void on_sample_lost(dds::sub::DataReader<T> &,
|
||||
const dds::core::status::SampleLostStatus &) override {}
|
||||
|
||||
static dds::core::status::StatusMask mask()
|
||||
{
|
||||
return (dds::core::status::StatusMask::data_available());
|
||||
}
|
||||
|
||||
protected:
|
||||
DataWriterTaskInterface *m_dwt;
|
||||
dds::pub::DataWriter<T> m_dataWriter;
|
||||
std::string m_topicName;
|
||||
DDS::QosProfile::TopicType m_topicType;
|
||||
|
||||
NGL::INGLLogger &m_logger;
|
||||
};
|
||||
|
||||
template<typename TOPIC>
|
||||
class TopicDispatcher : public TopicDispatcherInterface
|
||||
{
|
||||
protected:
|
||||
explicit TopicDispatcher(NGL::INGLLogger &logger, const std::string &topicName, DDS::QosProfile::TopicType topicType)
|
||||
: m_dataReader(nullptr)
|
||||
, m_topicName(topicName)
|
||||
, m_topicType(topicType)
|
||||
, m_logger(logger)
|
||||
{
|
||||
}
|
||||
|
||||
~TopicDispatcher()
|
||||
{
|
||||
shutdown();
|
||||
}
|
||||
|
||||
bool init(DataWriterTaskInterface *dwt,
|
||||
const dds::domain::DomainParticipant &dpIn,
|
||||
const dds::sub::Subscriber &subIn,
|
||||
const dds::domain::DomainParticipant &dpOut,
|
||||
const dds::pub::Publisher &pubOut) override
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "Initializing TopicDispatcher " << "(" << this << ")";
|
||||
if (m_dataReader != nullptr)
|
||||
return false;
|
||||
|
||||
m_dataReader = dds::sub::DataReader<TOPIC>(subIn,
|
||||
getTopic<TOPIC>(dpIn, m_topicName),
|
||||
DDS::QosProfile::dataReaderDefault(m_topicType));
|
||||
DEBUG_LOG(m_logger) << "Creating DataReader for domain " << dpIn.domain_id() << ", ptr: " << m_dataReader.delegate().get();
|
||||
|
||||
m_dtl.reset(new DataTopicListener<TOPIC>(dwt, m_topicName, m_topicType, m_logger));
|
||||
|
||||
if (!m_dtl->init(dpOut, pubOut))
|
||||
return false;
|
||||
m_dataReader.listener(m_dtl.get(), DataTopicListener<TOPIC>::mask());
|
||||
return true;
|
||||
}
|
||||
|
||||
void shutdown() override
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "Cleaning up TopicDispatcher " << "(" << this << ")";
|
||||
if (m_dataReader != nullptr)
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "shutting down DataReader: " << m_dataReader.delegate().get();
|
||||
m_dataReader.listener(nullptr, dds::core::status::StatusMask::none());
|
||||
try
|
||||
{
|
||||
m_dataReader.close();
|
||||
}
|
||||
catch (const dds::core::Exception &ex)
|
||||
{
|
||||
ERROR_LOG(m_logger) << "topic close ex: " << ex.what();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
m_dtl = nullptr;
|
||||
|
||||
m_dataReader = nullptr;
|
||||
}
|
||||
|
||||
private:
|
||||
dds::sub::DataReader<TOPIC> m_dataReader;
|
||||
std::unique_ptr<DataTopicListener<TOPIC>> m_dtl;
|
||||
std::string m_topicName;
|
||||
DDS::QosProfile::TopicType m_topicType;
|
||||
NGL::INGLLogger &m_logger;
|
||||
};
|
||||
|
||||
|
||||
template<typename TOPIC>
|
||||
class DataTopicDispatcher final : public TopicDispatcher<TOPIC>
|
||||
{
|
||||
public:
|
||||
explicit DataTopicDispatcher(NGL::INGLLogger &logger)
|
||||
: TopicDispatcher<TOPIC>(logger,
|
||||
DDS::Helper::getShortTopicName(dds::topic::topic_type_name<TOPIC>::value()),
|
||||
DDS::QosProfile::TopicType::DataTopic)
|
||||
{}
|
||||
};
|
||||
|
||||
|
||||
template<typename FTS_TOPIC>
|
||||
class FunctionTopicReplyDispatcher final : public TopicDispatcher<typename FTS_TOPIC::ReplyTopic>
|
||||
{
|
||||
public:
|
||||
explicit FunctionTopicReplyDispatcher(NGL::INGLLogger &logger)
|
||||
: TopicDispatcher<typename FTS_TOPIC::ReplyTopic>(logger,
|
||||
FTS::TopicNames<FTS_TOPIC>::replyTopicName(),
|
||||
DDS::QosProfile::TopicType::FunctionTopic)
|
||||
{}
|
||||
};
|
||||
|
||||
|
||||
template<typename FTC_TOPIC>
|
||||
class FunctionTopicRequestDispatcher final : public TopicDispatcher<typename FTC_TOPIC::RequestTopic>
|
||||
{
|
||||
public:
|
||||
explicit FunctionTopicRequestDispatcher(NGL::INGLLogger &logger)
|
||||
: TopicDispatcher<typename FTC_TOPIC::RequestTopic>(logger,
|
||||
FTC::TopicNames<FTC_TOPIC>::requestTopicName(),
|
||||
DDS::QosProfile::TopicType::FunctionTopic)
|
||||
{}
|
||||
};
|
||||
|
||||
}
|
||||
Reference in new issue
Block a user