434 lines
14 KiB
C++
434 lines
14 KiB
C++
|
|
|
|
#include <DDS/Generic/generic_base.h>
|
|
#include <DDS/QosProfile/DDS_DefaultQos.h>
|
|
#include <NGLLogger/INGLLogger.h>
|
|
|
|
#if BUILD_RTI
|
|
#include <dds/pub/DataWriter.hpp>
|
|
#include <dds/pub/discovery.hpp>
|
|
#include <dds/sub/DataReader.hpp>
|
|
#include <dds/sub/DataReaderListener.hpp>
|
|
#include <dds/topic/Topic.hpp>
|
|
#include <dds/topic/find.hpp>
|
|
|
|
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: " + QString::number(seqNo.value()).toLatin1() + ", Guid-DW: " + toReadable(guid);
|
|
}
|
|
|
|
static inline QByteArray toReadable(const rti::core::SampleIdentity& identiy)
|
|
{
|
|
return toReadable(identiy.sequence_number(), identiy.writer_guid());
|
|
}
|
|
} // namespace DebugInfo
|
|
|
|
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)
|
|
{
|
|
}
|
|
};
|
|
|
|
} // namespace RoutingService
|
|
|
|
#else // BUILD_RTI=OFF - Stub declarations
|
|
|
|
namespace RoutingService
|
|
{
|
|
// Stub interfaces when BUILD_RTI=OFF
|
|
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*/, void* /*dpIn*/, void* /*subIn*/, void* /*dpOut*/,
|
|
void* /*pubOut*/) = 0;
|
|
virtual void shutdown() = 0;
|
|
};
|
|
|
|
// Stub template classes when BUILD_RTI=OFF
|
|
template <typename TOPIC> class SampleWriter final : public SampleWriterInterface
|
|
{
|
|
public:
|
|
SampleWriter(void* /*samples*/, void* /*dataWriter*/, bool /*bWithOriginalIdentity*/, NGL::INGLLogger& /*logger*/)
|
|
{
|
|
}
|
|
void writeSamples() override {}
|
|
NGL::INGLLogger& logger() override
|
|
{
|
|
static NGL::INGLLogger dummyLogger;
|
|
return dummyLogger;
|
|
}
|
|
|
|
private:
|
|
NGL::INGLLogger& m_logger;
|
|
};
|
|
|
|
template <typename TOPIC> class TopicDispatcher : public TopicDispatcherInterface
|
|
{
|
|
protected:
|
|
explicit TopicDispatcher(NGL::INGLLogger& /*logger*/, const std::string& /*topicName*/, int /*topicType*/) {}
|
|
~TopicDispatcher() { shutdown(); }
|
|
bool init(DataWriterTaskInterface* /*dwt*/, void* /*dpIn*/, void* /*subIn*/, void* /*dpOut*/,
|
|
void* /*pubOut*/) override
|
|
{
|
|
return false;
|
|
}
|
|
void shutdown() override {}
|
|
|
|
private:
|
|
NGL::INGLLogger& m_logger;
|
|
};
|
|
|
|
template <typename TOPIC> class DataTopicDispatcher final : public TopicDispatcher<TOPIC>
|
|
{
|
|
public:
|
|
explicit DataTopicDispatcher(NGL::INGLLogger& logger) : TopicDispatcher<TOPIC>(logger, "", 0) {}
|
|
};
|
|
|
|
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, "", 0)
|
|
{
|
|
}
|
|
};
|
|
|
|
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, "", 0)
|
|
{
|
|
}
|
|
};
|
|
|
|
} // namespace RoutingService
|
|
|
|
#endif // BUILD_RTI
|