355 lines
11 KiB
C++
Executable File
355 lines
11 KiB
C++
Executable File
|
|
|
|
|
|
#pragma once
|
|
|
|
#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 <DDS/Generic/generic_base.h>
|
|
#include <DDS/Generic/generic_logger.h>
|
|
#include <LtsUtilsCore/data_modification_enum.h>
|
|
#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;
|
|
};
|
|
|
|
}
|