344 lines
12 KiB
C++
344 lines
12 KiB
C++
|
|
|
|
#pragma once
|
|
|
|
// Include ALL headers BEFORE opening DDSGeneric namespace to prevent Qt symbols
|
|
// from being resolved in DDSGeneric namespace when RTI DDS headers include Qt
|
|
#include <QtCore/QString>
|
|
#include <QtCore/QUrl>
|
|
#include <QtCore/QFlag>
|
|
#include <DDS/Generic/generic_logger.h>
|
|
#include <DDS/Generic/generic_base.h>
|
|
#include <LtsUtils/LtsUtilsCore/data_modification_enum.h>
|
|
#include <dds/core/xtypes/StructType.hpp>
|
|
#include <dds/sub/DataReader.hpp>
|
|
#include <dds/sub/LoanedSamples.hpp>
|
|
#include <dds/sub/SampleInfo.hpp>
|
|
#include <dds/topic/find.hpp>
|
|
#include <rti/core/xtypes/DynamicDataImpl.hpp>
|
|
#include <functional>
|
|
|
|
namespace DDSGeneric
|
|
{
|
|
using StructType = dds::core::xtypes::StructType;
|
|
using DynamicData = dds::core::xtypes::DynamicData;
|
|
|
|
class DynamicDataReader final
|
|
{
|
|
public:
|
|
using TopicType = DynamicData;
|
|
using DataReader = dds::sub::DataReader<DynamicData>;
|
|
using DataTopic = dds::topic::Topic<TopicType>;
|
|
|
|
using SubscriberCallback = std::function<void(const DynamicData&, const std::string&, DataModification)>;
|
|
using SubscriberCallbackWithSampleInfo =
|
|
std::function<void(const DynamicData&, const std::string&, const dds::sub::SampleInfo&)>;
|
|
|
|
DynamicDataReader()
|
|
: m_dataReader(nullptr), m_structType(nullptr), m_ddsTopic(nullptr), m_bHaveSubscriberCallback(false),
|
|
m_logger(DDS::getLogger("DynamicDataReader", DDS::DataType::DYNAMIC_DATA_READER))
|
|
{
|
|
}
|
|
|
|
~DynamicDataReader()
|
|
{
|
|
m_ddsTopic = nullptr;
|
|
m_dataReader = nullptr;
|
|
delete m_structType;
|
|
}
|
|
|
|
const DataReader& getDataReader() const
|
|
{
|
|
return m_dataReader;
|
|
}
|
|
|
|
const StructType& getStructType() const
|
|
{
|
|
return *m_structType;
|
|
}
|
|
|
|
bool create(const dds::domain::DomainParticipant& domainParticipant, const dds::sub::Subscriber& ddsSubscriber,
|
|
const std::string& specialTopicName, const StructType& structType)
|
|
{
|
|
DEBUG_LOG(m_logger) << "IN DynamicDataReader::create (" << m_structType->name() << ")";
|
|
|
|
if (domainParticipant == nullptr)
|
|
{
|
|
ERROR_LOG(m_logger) << "DomainParticipant does not exist ";
|
|
return false;
|
|
}
|
|
|
|
if (m_ddsTopic != nullptr)
|
|
{
|
|
ERROR_LOG(m_logger) << "Type '" << m_structType->name() << "' already created";
|
|
return false;
|
|
}
|
|
|
|
if (structType.has_parent())
|
|
m_structType =
|
|
new dds::core::xtypes::StructType(structType.name(), structType.parent(), structType.members());
|
|
else
|
|
m_structType = new dds::core::xtypes::StructType(structType.name(), structType.members());
|
|
|
|
try
|
|
{
|
|
|
|
const std::string topicName =
|
|
specialTopicName.empty() ? DDS::Helper::getShortTopicName(m_structType->name()) : specialTopicName;
|
|
|
|
DEBUG_LOG(m_logger) << "Creating topic " << topicName << " with type: " << m_structType->name();
|
|
|
|
m_ddsTopic = dds::topic::find<dds::topic::Topic<DynamicData>>(domainParticipant, topicName);
|
|
if (m_ddsTopic == nullptr)
|
|
{
|
|
rti::core::xtypes::DynamicDataTypeSerializationProperty serialization_property;
|
|
serialization_property.min_size_serialized(0);
|
|
dds::domain::DomainParticipant dp = domainParticipant;
|
|
rti::domain::register_type(dp, structType.name(), structType, serialization_property);
|
|
m_ddsTopic = dds::topic::Topic<DynamicData>(domainParticipant, topicName, structType);
|
|
}
|
|
|
|
DEBUG_LOG(m_logger) << "m_ddsTopic created: " << m_ddsTopic.name();
|
|
|
|
if (m_logger.isDebugEnabled())
|
|
{
|
|
dds::sub::qos::SubscriberQos subscriberQoS = ddsSubscriber.qos();
|
|
dds::core::policy::Partition partition = subscriberQoS.policy<dds::core::policy::Partition>();
|
|
dds::core::StringSeq parititionNames = partition.name();
|
|
DEBUG_LOG(m_logger) << "Subscribing partitions: " << parititionNames.size();
|
|
for (const std::string& name : parititionNames)
|
|
DEBUG_LOG(m_logger) << "SubscriberQoS: partition name = " << name;
|
|
}
|
|
|
|
dds::sub::qos::DataReaderQos dataReaderQos = ddsSubscriber.default_datareader_qos();
|
|
|
|
QString typeName = QString::fromStdString(m_structType->name());
|
|
if (typeName.endsWith(QLatin1String("Request")) || typeName.endsWith(QLatin1String("Reply")))
|
|
{
|
|
|
|
dds::core::policy::Durability readerDurability;
|
|
dataReaderQos >> readerDurability;
|
|
readerDurability.kind(dds::core::policy::DurabilityKind_def::VOLATILE);
|
|
dataReaderQos << readerDurability;
|
|
}
|
|
|
|
m_dataReader =
|
|
DataReader(ddsSubscriber, m_ddsTopic, dataReaderQos, nullptr, dds::core::status::StatusMask::all());
|
|
|
|
if (m_dataReader == nullptr)
|
|
{
|
|
ERROR_LOG(m_logger) << "Creating dataReader for '" << topicName << "' with type: '"
|
|
<< m_structType->name() << "' failed";
|
|
return false;
|
|
}
|
|
}
|
|
catch (const dds::core::Exception& ex)
|
|
{
|
|
ERROR_LOG(m_logger) << ex.what();
|
|
return false;
|
|
}
|
|
|
|
DEBUG_LOG(m_logger) << "dataReader created (" << m_structType->name() << ")";
|
|
|
|
return true;
|
|
}
|
|
|
|
bool hasSamples() const
|
|
{
|
|
qint64 sampleSize = m_dataReader->datareader_cache_status().sample_count();
|
|
DEBUG_LOG(m_logger) << "sampleSize = " << sampleSize;
|
|
return sampleSize > 0;
|
|
}
|
|
|
|
dds::sub::LoanedSamples<DynamicData> readAlive(bool& bSeenNewSamples)
|
|
{
|
|
dds::sub::LoanedSamples<DynamicData> samples =
|
|
m_dataReader.select().state(dds::sub::status::InstanceState::alive()).read();
|
|
TRACE_LOG(m_logger) << "readAlive for " << DDS::topic_type_name<TopicType>::value()
|
|
<< ", count: " << samples.length() << ", this: " << this << ", instance handle "
|
|
<< m_dataReader.instance_handle();
|
|
|
|
bSeenNewSamples = checkNewSamples(samples);
|
|
return samples;
|
|
}
|
|
|
|
dds::sub::LoanedSamples<DynamicData> takeAlive(bool& bSeenNewSamples)
|
|
{
|
|
dds::sub::LoanedSamples<DynamicData> samples =
|
|
m_dataReader.select().state(dds::sub::status::InstanceState::alive()).take();
|
|
TRACE_LOG(m_logger) << "takeAlive for " << DDS::topic_type_name<TopicType>::value()
|
|
<< ", count: " << samples.length() << ", this: " << this << ", instance handle "
|
|
<< m_dataReader.instance_handle();
|
|
|
|
bSeenNewSamples = checkNewSamples(samples);
|
|
return samples;
|
|
}
|
|
|
|
dds::sub::LoanedSamples<DynamicData> takeDisposed()
|
|
{
|
|
dds::sub::LoanedSamples<DynamicData> samples =
|
|
m_dataReader.select().state(dds::sub::status::InstanceState::not_alive_disposed()).take();
|
|
TRACE_LOG(m_logger) << "takeDisposed for " << DDS::topic_type_name<TopicType>::value()
|
|
<< ", count: " << samples.length() << ", this: " << this << ", instance handle "
|
|
<< m_dataReader.instance_handle();
|
|
return samples;
|
|
}
|
|
|
|
template <typename C> bool read(C callbackFunc)
|
|
{
|
|
DEBUG_LOG(m_logger) << "IN read (" << DDS::topic_type_name<TopicType>::value() << "), this: " << this
|
|
<< ", instance handle " << m_dataReader.instance_handle();
|
|
|
|
if (m_dataReader == nullptr || m_ddsTopic == nullptr)
|
|
{
|
|
ERROR_LOG(m_logger) << "No dataReader initialised ";
|
|
return false;
|
|
}
|
|
if (!isValid(callbackFunc))
|
|
{
|
|
ERROR_LOG(m_logger) << "No subscriber set for " << DDS::topic_type_name<TopicType>::value();
|
|
return false;
|
|
}
|
|
|
|
dds::sub::LoanedSamples<DynamicData> samples =
|
|
m_dataReader.select().state(dds::sub::status::DataState::new_data()).read();
|
|
|
|
DEBUG_LOG(m_logger) << "got " << samples.length() + m_newSamplesDuringRead.size() << " samples from reader for "
|
|
<< DDS::topic_type_name<TopicType>::value();
|
|
|
|
for (const auto& sample : samples)
|
|
{
|
|
if (sample.info().valid())
|
|
readData(sample, m_dataReader->topic_name(), callbackFunc);
|
|
}
|
|
|
|
auto copy = m_newSamplesDuringRead;
|
|
|
|
m_newSamplesDuringRead.clear();
|
|
for (auto& sample : copy)
|
|
{
|
|
|
|
readData(rti::sub::LoanedSample<DynamicData>(sample.sample, &sample.info), m_dataReader->topic_name(),
|
|
callbackFunc);
|
|
delete sample.sample;
|
|
}
|
|
|
|
return checkDisposedInstances(callbackFunc);
|
|
}
|
|
|
|
void setHaveSubscriberCallback(bool bHave)
|
|
{
|
|
m_bHaveSubscriberCallback = bHave;
|
|
}
|
|
|
|
std::string topicName() const
|
|
{
|
|
return m_ddsTopic.name();
|
|
}
|
|
|
|
void recreateReader()
|
|
{
|
|
if (m_dataReader == nullptr)
|
|
return;
|
|
m_dataReader = DataReader(m_dataReader.subscriber(), m_ddsTopic);
|
|
}
|
|
|
|
private:
|
|
#if RTI_DDS_VERSION_MAJOR < 6
|
|
inline bool checkNewSamples(dds::sub::LoanedSamples<DynamicData>& samples)
|
|
#else
|
|
inline bool checkNewSamples(const dds::sub::LoanedSamples<DynamicData>& samples)
|
|
#endif
|
|
{
|
|
|
|
if (!m_bHaveSubscriberCallback)
|
|
return false;
|
|
|
|
bool bSeenNewSamples = false;
|
|
for (const auto& sample : samples)
|
|
{
|
|
const dds::sub::status::DataState ds = sample.info()->state();
|
|
if (ds.view_state() == dds::sub::status::ViewState::new_view() ||
|
|
ds.sample_state() == dds::sub::status::SampleState::not_read())
|
|
{
|
|
SampleStruct ss;
|
|
ss.sample = new DynamicData(sample.data());
|
|
ss.info = sample.info();
|
|
|
|
m_newSamplesDuringRead.emplace_back(ss);
|
|
|
|
bSeenNewSamples = true;
|
|
}
|
|
}
|
|
return bSeenNewSamples;
|
|
}
|
|
|
|
template <typename C> bool checkDisposedInstances(C callbackFunc)
|
|
{
|
|
const dds::sub::status::DataState ds(dds::sub::status::SampleState::any(), dds::sub::status::ViewState::any(),
|
|
dds::sub::status::InstanceState::not_alive_mask());
|
|
|
|
dds::sub::LoanedSamples<DynamicData> samples = m_dataReader.select().state(ds).take();
|
|
|
|
if (samples.length() == 0)
|
|
return false;
|
|
|
|
DEBUG_LOG(m_logger) << "got " << samples.length() << " disposed instances from reader for "
|
|
<< m_ddsTopic.name();
|
|
|
|
for (const auto& sample : samples)
|
|
{
|
|
if (sample.info().valid())
|
|
readData(sample, m_dataReader->topic_name(), callbackFunc);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
static void readData(const rti::sub::LoanedSample<DynamicData>& sample, const std::string& topicName,
|
|
const SubscriberCallback& callbackFunc)
|
|
{
|
|
DataModification modType = DataModification::eModified;
|
|
const dds::sub::status::DataState ds = sample.info()->state();
|
|
if (ds.view_state() == dds::sub::status::ViewState::new_view())
|
|
modType = DataModification::eAdded;
|
|
if (ds.instance_state() != dds::sub::status::InstanceState::alive())
|
|
modType = DataModification::eRemoved;
|
|
|
|
callbackFunc(sample.data(), topicName, modType);
|
|
}
|
|
|
|
static void readData(const rti::sub::LoanedSample<DynamicData>& sample, const std::string& topicName,
|
|
const SubscriberCallbackWithSampleInfo& callbackFunc)
|
|
{
|
|
callbackFunc(sample.data(), topicName, sample.info());
|
|
}
|
|
|
|
static bool isValid(const SubscriberCallback& callbackFunc)
|
|
{
|
|
return callbackFunc != nullptr;
|
|
}
|
|
|
|
static bool isValid(const SubscriberCallbackWithSampleInfo& callbackFunc)
|
|
{
|
|
return callbackFunc != nullptr;
|
|
}
|
|
|
|
private:
|
|
DataReader m_dataReader;
|
|
StructType* m_structType;
|
|
DataTopic m_ddsTopic;
|
|
|
|
struct SampleStruct
|
|
{
|
|
DynamicData* sample = nullptr;
|
|
dds::sub::SampleInfo info;
|
|
};
|
|
|
|
std::vector<SampleStruct> m_newSamplesDuringRead;
|
|
bool m_bHaveSubscriberCallback;
|
|
NGL::INGLLogger& m_logger;
|
|
};
|
|
|
|
} // namespace DDSGeneric
|