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
+354
@@ -0,0 +1,354 @@
|
||||
|
||||
|
||||
|
||||
#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;
|
||||
};
|
||||
|
||||
}
|
||||
Reference in new issue
Block a user