Apply clang-format to Lts/Common and DTC/enum headers
- Formatted all CodeGen module files (9 files) - Formatted LtsAppDescriptor module files - Formatted LtsAppManager/AppChooser files - Applied clang-format to all DTC and enum headers across modules - Removed coordTransTests from CommonUnitTestsPlaceholder (moved to tests/) Cosmetic changes only, no functional modifications. Co-authored-by: Copilot <223556219+Copilot@users.noreply.github.com>
This commit is contained in:
848 files changed
+89305
-100020
No files matched your search
Executable → Regular
+281
-298
@@ -1,353 +1,336 @@
|
||||
|
||||
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <DDS/Generic/generic_base.h>
|
||||
#include <DDS/Generic/generic_logger.h>
|
||||
#include <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 <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 StructType = dds::core::xtypes::StructType;
|
||||
using DynamicData = dds::core::xtypes::DynamicData;
|
||||
|
||||
class DynamicDataReader final
|
||||
{
|
||||
public:
|
||||
public:
|
||||
using TopicType = DynamicData;
|
||||
using DataReader = dds::sub::DataReader<DynamicData>;
|
||||
using DataTopic = dds::topic::Topic<TopicType>;
|
||||
|
||||
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&)>;
|
||||
|
||||
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)
|
||||
DynamicDataReader()
|
||||
: m_dataReader(nullptr), m_structType(nullptr), m_ddsTopic(nullptr), m_bHaveSubscriberCallback(false),
|
||||
m_logger(DDS::getLogger("DynamicDataReader", DDS::DataType::DYNAMIC_DATA_READER))
|
||||
{
|
||||
ERROR_LOG(m_logger) << "DomainParticipant does not exist ";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (m_ddsTopic != nullptr)
|
||||
~DynamicDataReader()
|
||||
{
|
||||
ERROR_LOG(m_logger) << "Type '"
|
||||
<< m_structType->name()
|
||||
<< "' already created";
|
||||
return false;
|
||||
m_ddsTopic = nullptr;
|
||||
m_dataReader = nullptr;
|
||||
delete m_structType;
|
||||
}
|
||||
|
||||
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 DataReader& getDataReader() const
|
||||
{
|
||||
|
||||
|
||||
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;
|
||||
return m_dataReader;
|
||||
}
|
||||
|
||||
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)
|
||||
const StructType& getStructType() const
|
||||
{
|
||||
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;
|
||||
return *m_structType;
|
||||
}
|
||||
|
||||
|
||||
|
||||
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)
|
||||
bool create(const dds::domain::DomainParticipant& domainParticipant, const dds::sub::Subscriber& ddsSubscriber,
|
||||
const std::string& specialTopicName, const StructType& structType)
|
||||
{
|
||||
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;
|
||||
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;
|
||||
}
|
||||
|
||||
return checkDisposedInstances(callbackFunc);
|
||||
}
|
||||
bool hasSamples() const
|
||||
{
|
||||
qint64 sampleSize = m_dataReader->datareader_cache_status().sample_count();
|
||||
DEBUG_LOG(m_logger) << "sampleSize = " << sampleSize;
|
||||
return sampleSize > 0;
|
||||
}
|
||||
|
||||
void setHaveSubscriberCallback(bool bHave)
|
||||
{
|
||||
m_bHaveSubscriberCallback = bHave;
|
||||
}
|
||||
std::string topicName() const
|
||||
{
|
||||
return m_ddsTopic.name();
|
||||
}
|
||||
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();
|
||||
|
||||
void recreateReader()
|
||||
{
|
||||
if (m_dataReader == nullptr)
|
||||
return;
|
||||
m_dataReader = DataReader(m_dataReader.subscriber(), m_ddsTopic);
|
||||
}
|
||||
bSeenNewSamples = checkNewSamples(samples);
|
||||
return samples;
|
||||
}
|
||||
|
||||
private:
|
||||
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)
|
||||
inline bool checkNewSamples(dds::sub::LoanedSamples<DynamicData>& samples)
|
||||
#else
|
||||
inline bool checkNewSamples(const dds::sub::LoanedSamples<DynamicData> &samples)
|
||||
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;
|
||||
}
|
||||
if (!m_bHaveSubscriberCallback)
|
||||
return false;
|
||||
|
||||
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());
|
||||
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();
|
||||
|
||||
|
||||
dds::sub::LoanedSamples<DynamicData> samples = m_dataReader.select().state(ds).take();
|
||||
m_newSamplesDuringRead.emplace_back(ss);
|
||||
|
||||
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);
|
||||
bSeenNewSamples = true;
|
||||
}
|
||||
}
|
||||
return bSeenNewSamples;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
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());
|
||||
|
||||
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;
|
||||
|
||||
dds::sub::LoanedSamples<DynamicData> samples = m_dataReader.select().state(ds).take();
|
||||
|
||||
callbackFunc(sample.data(), topicName, modType);
|
||||
}
|
||||
if (samples.length() == 0)
|
||||
return false;
|
||||
|
||||
static void readData(const rti::sub::LoanedSample<DynamicData> &sample, const std::string &topicName, const SubscriberCallbackWithSampleInfo &callbackFunc)
|
||||
{
|
||||
callbackFunc(sample.data(), topicName, sample.info());
|
||||
}
|
||||
DEBUG_LOG(m_logger) << "got " << samples.length() << " disposed instances from reader for "
|
||||
<< m_ddsTopic.name();
|
||||
|
||||
static bool isValid(const SubscriberCallback &callbackFunc)
|
||||
{
|
||||
return callbackFunc != nullptr;
|
||||
}
|
||||
static bool isValid(const SubscriberCallbackWithSampleInfo &callbackFunc)
|
||||
{
|
||||
return callbackFunc != nullptr;
|
||||
}
|
||||
for (const auto& sample : samples)
|
||||
{
|
||||
if (sample.info().valid())
|
||||
readData(sample, m_dataReader->topic_name(), callbackFunc);
|
||||
}
|
||||
|
||||
private:
|
||||
DataReader m_dataReader;
|
||||
StructType *m_structType;
|
||||
DataTopic m_ddsTopic;
|
||||
return true;
|
||||
}
|
||||
|
||||
struct SampleStruct
|
||||
{
|
||||
DynamicData *sample = nullptr;
|
||||
dds::sub::SampleInfo info;
|
||||
};
|
||||
std::vector<SampleStruct> m_newSamplesDuringRead;
|
||||
bool m_bHaveSubscriberCallback;
|
||||
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;
|
||||
};
|
||||
|
||||
}
|
||||
} // namespace DDSGeneric
|
||||
Executable → Regular
+299
-311
@@ -1,381 +1,369 @@
|
||||
|
||||
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <dds/sub/DataReader.hpp>
|
||||
#include <dds/topic/find.hpp>
|
||||
#include <dds/topic/Topic.hpp>
|
||||
|
||||
#include <DDS/Generic/generic_base.h>
|
||||
#include <DDS/Generic/generic_logger.h>
|
||||
#include <LtsUtilsCore/data_modification_enum.h>
|
||||
#include <dds/sub/DataReader.hpp>
|
||||
#include <dds/topic/Topic.hpp>
|
||||
#include <dds/topic/find.hpp>
|
||||
#include <functional>
|
||||
|
||||
namespace DDSGeneric
|
||||
{
|
||||
|
||||
template <typename TOPICTYPE>
|
||||
class GenericDataReader
|
||||
template <typename TOPICTYPE> class GenericDataReader
|
||||
{
|
||||
public:
|
||||
using TopicType = TOPICTYPE;
|
||||
using TopicTypeRead = TOPICTYPE;
|
||||
using DataReader = dds::sub::DataReader<TopicTypeRead>;
|
||||
using DataTopic = dds::topic::Topic<TopicTypeRead>;
|
||||
using DataReaderQos = dds::sub::qos::DataReaderQos;
|
||||
using SubscriberCallback = std::function<void(const TopicTypeRead &, DataModification)>;
|
||||
using SubscriberCallbackWithSampleInfo = std::function<void(const TopicTypeRead &, const dds::sub::SampleInfo &)>;
|
||||
public:
|
||||
using TopicType = TOPICTYPE;
|
||||
using TopicTypeRead = TOPICTYPE;
|
||||
using DataReader = dds::sub::DataReader<TopicTypeRead>;
|
||||
using DataTopic = dds::topic::Topic<TopicTypeRead>;
|
||||
using DataReaderQos = dds::sub::qos::DataReaderQos;
|
||||
using SubscriberCallback = std::function<void(const TopicTypeRead&, DataModification)>;
|
||||
using SubscriberCallbackWithSampleInfo = std::function<void(const TopicTypeRead&, const dds::sub::SampleInfo&)>;
|
||||
|
||||
GenericDataReader()
|
||||
: m_dataReader(nullptr)
|
||||
, m_ddsTopic(nullptr)
|
||||
, m_bHaveSubscriberCallback(false)
|
||||
, m_logger(DDS::getLogger(DDS::topic_type_name<TopicType>::value(), DDS::DataType::DATA_READER))
|
||||
{}
|
||||
|
||||
virtual ~GenericDataReader()
|
||||
{
|
||||
m_ddsTopic = nullptr;
|
||||
m_dataReader = nullptr;
|
||||
}
|
||||
|
||||
DataReader getDataReader() const { return m_dataReader; }
|
||||
|
||||
bool create(const dds::domain::DomainParticipant &domainParticipant,
|
||||
const dds::sub::Subscriber &ddsSubscriber,
|
||||
const std::string &topicName = DDS::Helper::getShortTopicName(DDS::topic_type_name<TopicType>::value()),
|
||||
bool bCreateDataReader = true)
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "IN GenericDataReader::create ("
|
||||
<< DDS::topic_type_name<TopicType>::value()
|
||||
<< "), this: " << this;
|
||||
|
||||
if (domainParticipant == nullptr)
|
||||
GenericDataReader()
|
||||
: m_dataReader(nullptr), m_ddsTopic(nullptr), m_bHaveSubscriberCallback(false),
|
||||
m_logger(DDS::getLogger(DDS::topic_type_name<TopicType>::value(), DDS::DataType::DATA_READER))
|
||||
{
|
||||
ERROR_LOG(m_logger) << "DomainParticipant does not exist ";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (m_ddsTopic != nullptr)
|
||||
virtual ~GenericDataReader()
|
||||
{
|
||||
ERROR_LOG(m_logger) << "Type '"
|
||||
<< DDS::topic_type_name<TopicType>::value()
|
||||
<< "' already created";
|
||||
return false;
|
||||
m_ddsTopic = nullptr;
|
||||
m_dataReader = nullptr;
|
||||
}
|
||||
|
||||
try
|
||||
DataReader getDataReader() const
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "Creating read topic " << topicName << " with type: "
|
||||
<< DDS::topic_type_name<TopicType>::value();
|
||||
return m_dataReader;
|
||||
}
|
||||
|
||||
m_ddsTopic = dds::topic::find<DataTopic>(domainParticipant, topicName);
|
||||
if (m_ddsTopic == nullptr)
|
||||
m_ddsTopic = DataTopic(domainParticipant, topicName);
|
||||
bool create(const dds::domain::DomainParticipant& domainParticipant, const dds::sub::Subscriber& ddsSubscriber,
|
||||
const std::string& topicName = DDS::Helper::getShortTopicName(DDS::topic_type_name<TopicType>::value()),
|
||||
bool bCreateDataReader = true)
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "IN GenericDataReader::create (" << DDS::topic_type_name<TopicType>::value()
|
||||
<< "), this: " << this;
|
||||
|
||||
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) << " partition name = " << name;
|
||||
}
|
||||
if (domainParticipant == nullptr)
|
||||
{
|
||||
ERROR_LOG(m_logger) << "DomainParticipant does not exist ";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (m_ddsTopic != nullptr)
|
||||
{
|
||||
ERROR_LOG(m_logger) << "Type '" << DDS::topic_type_name<TopicType>::value() << "' already created";
|
||||
return false;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "Creating read topic " << topicName
|
||||
<< " with type: " << DDS::topic_type_name<TopicType>::value();
|
||||
|
||||
m_ddsTopic = dds::topic::find<DataTopic>(domainParticipant, topicName);
|
||||
if (m_ddsTopic == nullptr)
|
||||
m_ddsTopic = DataTopic(domainParticipant, topicName);
|
||||
|
||||
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) << " partition name = " << name;
|
||||
}
|
||||
|
||||
if (bCreateDataReader)
|
||||
{
|
||||
dds::sub::qos::DataReaderQos drQos = domainParticipant->default_datareader_qos();
|
||||
m_dataReader = DataReader(ddsSubscriber, m_ddsTopic, drQos);
|
||||
if (m_dataReader == nullptr)
|
||||
{
|
||||
ERROR_LOG(m_logger) << "Creating dataReader for '" << topicName << "' with type: '"
|
||||
<< DDS::topic_type_name<TopicType>::value() << "' failed";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
catch (const dds::core::Exception& ex)
|
||||
{
|
||||
ERROR_LOG(m_logger) << ex.what();
|
||||
return false;
|
||||
}
|
||||
|
||||
DEBUG_LOG(m_logger) << "dataReader created (" << DDS::topic_type_name<TopicType>::value() << ")";
|
||||
|
||||
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<TopicTypeRead> readAlive(bool& bSeenNewSamples)
|
||||
{
|
||||
dds::sub::LoanedSamples<TopicTypeRead> 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<TopicTypeRead> takeAlive(bool& bSeenNewSamples)
|
||||
{
|
||||
dds::sub::LoanedSamples<TopicTypeRead> 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<TopicTypeRead> takeDisposed()
|
||||
{
|
||||
dds::sub::LoanedSamples<TopicTypeRead> 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 (bCreateDataReader)
|
||||
{
|
||||
dds::sub::qos::DataReaderQos drQos = domainParticipant->default_datareader_qos();
|
||||
m_dataReader = DataReader(ddsSubscriber, m_ddsTopic, drQos);
|
||||
if (m_dataReader == nullptr)
|
||||
{
|
||||
ERROR_LOG(m_logger) << "Creating dataReader for '" << topicName << "' with type: '"
|
||||
<< DDS::topic_type_name<TopicType>::value()
|
||||
<< "' failed";
|
||||
return false;
|
||||
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;
|
||||
}
|
||||
|
||||
std::vector<SampleStruct> samples;
|
||||
{
|
||||
dds::sub::LoanedSamples<TopicTypeRead> loanedSamples =
|
||||
m_dataReader.select().state(dds::sub::status::DataState::new_data()).read();
|
||||
samples.reserve(loanedSamples.length());
|
||||
for (const auto& sample : loanedSamples)
|
||||
{
|
||||
if (!sample.info().valid())
|
||||
continue;
|
||||
samples.emplace_back(sample.data(), sample.info());
|
||||
}
|
||||
}
|
||||
|
||||
DEBUG_LOG(m_logger) << "got " << samples.size() + m_newSamplesDuringRead.size() << " samples from reader for "
|
||||
<< DDS::topic_type_name<TopicType>::value();
|
||||
|
||||
for (const SampleStruct& ss : samples)
|
||||
readData(ss.sample, ss.info, callbackFunc);
|
||||
|
||||
if (!m_newSamplesDuringRead.empty())
|
||||
{
|
||||
std::vector<SampleStruct> copy;
|
||||
std::swap(copy, m_newSamplesDuringRead);
|
||||
|
||||
for (const SampleStruct& ss : copy)
|
||||
readData(ss.sample, ss.info, callbackFunc);
|
||||
}
|
||||
|
||||
return checkDisposedInstances(callbackFunc);
|
||||
}
|
||||
catch (const dds::core::Exception &ex)
|
||||
|
||||
void setHaveSubscriberCallback(bool bHave)
|
||||
{
|
||||
ERROR_LOG(m_logger) << ex.what();
|
||||
return false;
|
||||
m_bHaveSubscriberCallback = bHave;
|
||||
}
|
||||
|
||||
DEBUG_LOG(m_logger) << "dataReader created ("
|
||||
<< DDS::topic_type_name<TopicType>::value()
|
||||
<< ")";
|
||||
|
||||
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<TopicTypeRead> readAlive(bool &bSeenNewSamples)
|
||||
{
|
||||
dds::sub::LoanedSamples<TopicTypeRead> 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<TopicTypeRead> takeAlive(bool &bSeenNewSamples)
|
||||
{
|
||||
dds::sub::LoanedSamples<TopicTypeRead> 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<TopicTypeRead> takeDisposed()
|
||||
{
|
||||
dds::sub::LoanedSamples<TopicTypeRead> 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)
|
||||
std::string topicName() const
|
||||
{
|
||||
ERROR_LOG(m_logger) << "No dataReader initialised ";
|
||||
return false;
|
||||
return m_ddsTopic.name();
|
||||
}
|
||||
if (!isValid(callbackFunc))
|
||||
|
||||
DataReader dataReader() const
|
||||
{
|
||||
ERROR_LOG(m_logger) << "No subscriber set for "
|
||||
<< DDS::topic_type_name<TopicType>::value();
|
||||
return false;
|
||||
return m_dataReader;
|
||||
}
|
||||
|
||||
|
||||
|
||||
std::vector<SampleStruct> samples;
|
||||
virtual void recreateReader()
|
||||
{
|
||||
dds::sub::LoanedSamples<TopicTypeRead> loanedSamples = m_dataReader.select().state(dds::sub::status::DataState::new_data()).read();
|
||||
samples.reserve(loanedSamples.length());
|
||||
for (const auto &sample : loanedSamples)
|
||||
{
|
||||
if (!sample.info().valid())
|
||||
continue;
|
||||
samples.emplace_back(sample.data(), sample.info());
|
||||
}
|
||||
if (m_dataReader == nullptr)
|
||||
return;
|
||||
m_dataReader = DataReader(m_dataReader.subscriber(), m_ddsTopic);
|
||||
}
|
||||
|
||||
DEBUG_LOG(m_logger) << "got " << samples.size() + m_newSamplesDuringRead.size() << " samples from reader for "
|
||||
<< DDS::topic_type_name<TopicType>::value();
|
||||
|
||||
for (const SampleStruct &ss : samples)
|
||||
readData(ss.sample, ss.info, callbackFunc);
|
||||
|
||||
if (!m_newSamplesDuringRead.empty())
|
||||
{
|
||||
std::vector<SampleStruct> copy;
|
||||
std::swap(copy, m_newSamplesDuringRead);
|
||||
|
||||
|
||||
for (const SampleStruct &ss : copy)
|
||||
readData(ss.sample, ss.info, callbackFunc);
|
||||
}
|
||||
|
||||
return checkDisposedInstances(callbackFunc);
|
||||
}
|
||||
|
||||
void setHaveSubscriberCallback(bool bHave)
|
||||
{
|
||||
m_bHaveSubscriberCallback = bHave;
|
||||
}
|
||||
std::string topicName() const
|
||||
{
|
||||
return m_ddsTopic.name();
|
||||
}
|
||||
|
||||
DataReader dataReader() const { return m_dataReader; }
|
||||
|
||||
virtual void recreateReader()
|
||||
{
|
||||
if (m_dataReader == nullptr)
|
||||
return;
|
||||
m_dataReader = DataReader(m_dataReader.subscriber(), m_ddsTopic);
|
||||
}
|
||||
private:
|
||||
private:
|
||||
#if RTI_DDS_VERSION_MAJOR < 6
|
||||
inline bool checkNewSamples(dds::sub::LoanedSamples<TopicTypeRead> &samples)
|
||||
inline bool checkNewSamples(dds::sub::LoanedSamples<TopicTypeRead>& samples)
|
||||
#else
|
||||
inline bool checkNewSamples(const dds::sub::LoanedSamples<TopicTypeRead> &samples)
|
||||
inline bool checkNewSamples(const dds::sub::LoanedSamples<TopicTypeRead>& 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())
|
||||
{
|
||||
|
||||
m_newSamplesDuringRead.emplace_back(sample.data(), sample.info());
|
||||
|
||||
bSeenNewSamples = true;
|
||||
}
|
||||
}
|
||||
return bSeenNewSamples;
|
||||
}
|
||||
if (!m_bHaveSubscriberCallback)
|
||||
return false;
|
||||
|
||||
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());
|
||||
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())
|
||||
{
|
||||
|
||||
|
||||
dds::sub::LoanedSamples<TopicTypeRead> samples = m_dataReader.select().state(ds).take();
|
||||
m_newSamplesDuringRead.emplace_back(sample.data(), sample.info());
|
||||
|
||||
if (samples.length() == 0)
|
||||
return false;
|
||||
|
||||
DEBUG_LOG(m_logger) << "got " << samples.length() << " disposed instances from reader for "
|
||||
<< DDS::topic_type_name<TopicType>::value();
|
||||
|
||||
for (const auto &sample : samples)
|
||||
{
|
||||
if (sample.info().valid())
|
||||
readData(sample.data(), sample.info(), callbackFunc);
|
||||
bSeenNewSamples = true;
|
||||
}
|
||||
}
|
||||
return bSeenNewSamples;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
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());
|
||||
|
||||
static void readData(const TopicType &sample, const dds::sub::SampleInfo &info, const SubscriberCallback &callbackFunc)
|
||||
{
|
||||
DataModification modType = DataModification::eModified;
|
||||
const dds::sub::status::DataState ds = 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;
|
||||
|
||||
dds::sub::LoanedSamples<TopicTypeRead> samples = m_dataReader.select().state(ds).take();
|
||||
|
||||
callbackFunc(sample, modType);
|
||||
}
|
||||
if (samples.length() == 0)
|
||||
return false;
|
||||
|
||||
static void readData(const TopicType &sample, const dds::sub::SampleInfo &info, const SubscriberCallbackWithSampleInfo &callbackFunc)
|
||||
{
|
||||
callbackFunc(sample, info);
|
||||
}
|
||||
DEBUG_LOG(m_logger) << "got " << samples.length() << " disposed instances from reader for "
|
||||
<< DDS::topic_type_name<TopicType>::value();
|
||||
|
||||
static bool isValid(const SubscriberCallback &callbackFunc)
|
||||
{
|
||||
return callbackFunc != nullptr;
|
||||
}
|
||||
static bool isValid(const SubscriberCallbackWithSampleInfo &callbackFunc)
|
||||
{
|
||||
return callbackFunc != nullptr;
|
||||
}
|
||||
for (const auto& sample : samples)
|
||||
{
|
||||
if (sample.info().valid())
|
||||
readData(sample.data(), sample.info(), callbackFunc);
|
||||
}
|
||||
|
||||
protected:
|
||||
DataReader m_dataReader;
|
||||
DataReaderQos m_dataReaderQos;
|
||||
return true;
|
||||
}
|
||||
|
||||
DataTopic m_ddsTopic;
|
||||
struct SampleStruct
|
||||
{
|
||||
TopicTypeRead sample;
|
||||
dds::sub::SampleInfo info;
|
||||
static void readData(const TopicType& sample, const dds::sub::SampleInfo& info,
|
||||
const SubscriberCallback& callbackFunc)
|
||||
{
|
||||
DataModification modType = DataModification::eModified;
|
||||
const dds::sub::status::DataState ds = 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;
|
||||
|
||||
SampleStruct() = default;
|
||||
SampleStruct(const TopicTypeRead &s, const dds::sub::SampleInfo &i)
|
||||
: sample(s)
|
||||
, info(i)
|
||||
{}
|
||||
};
|
||||
std::vector<SampleStruct> m_newSamplesDuringRead;
|
||||
bool m_bHaveSubscriberCallback;
|
||||
callbackFunc(sample, modType);
|
||||
}
|
||||
|
||||
static void readData(const TopicType& sample, const dds::sub::SampleInfo& info,
|
||||
const SubscriberCallbackWithSampleInfo& callbackFunc)
|
||||
{
|
||||
callbackFunc(sample, info);
|
||||
}
|
||||
|
||||
static bool isValid(const SubscriberCallback& callbackFunc)
|
||||
{
|
||||
return callbackFunc != nullptr;
|
||||
}
|
||||
|
||||
static bool isValid(const SubscriberCallbackWithSampleInfo& callbackFunc)
|
||||
{
|
||||
return callbackFunc != nullptr;
|
||||
}
|
||||
|
||||
protected:
|
||||
DataReader m_dataReader;
|
||||
DataReaderQos m_dataReaderQos;
|
||||
|
||||
DataTopic m_ddsTopic;
|
||||
|
||||
struct SampleStruct
|
||||
{
|
||||
TopicTypeRead sample;
|
||||
dds::sub::SampleInfo info;
|
||||
|
||||
SampleStruct() = default;
|
||||
|
||||
SampleStruct(const TopicTypeRead& s, const dds::sub::SampleInfo& i) : sample(s), info(i) {}
|
||||
};
|
||||
|
||||
std::vector<SampleStruct> m_newSamplesDuringRead;
|
||||
bool m_bHaveSubscriberCallback;
|
||||
};
|
||||
|
||||
template<typename TOPICTYPE>
|
||||
class GenericDataReaderContentFilter final
|
||||
: public GenericDataReader<TOPICTYPE>
|
||||
template <typename TOPICTYPE> class GenericDataReaderContentFilter final : public GenericDataReader<TOPICTYPE>
|
||||
{
|
||||
using BaseClass = GenericDataReader<TOPICTYPE>;
|
||||
public:
|
||||
using TopicType = TOPICTYPE;
|
||||
using DataReader = dds::sub::DataReader<TopicType>;
|
||||
using BaseClass = GenericDataReader<TOPICTYPE>;
|
||||
|
||||
using BaseClass::BaseClass;
|
||||
public:
|
||||
using TopicType = TOPICTYPE;
|
||||
using DataReader = dds::sub::DataReader<TopicType>;
|
||||
|
||||
bool create(const dds::domain::DomainParticipant &domainParticipant,
|
||||
const dds::sub::Subscriber &ddsSubscriber,
|
||||
const std::string &specialTopicName,
|
||||
const dds::topic::Filter &filter)
|
||||
{
|
||||
if (!BaseClass::create(domainParticipant, ddsSubscriber, specialTopicName, false))
|
||||
return false;
|
||||
m_filter = filter;
|
||||
try
|
||||
using BaseClass::BaseClass;
|
||||
|
||||
bool create(const dds::domain::DomainParticipant& domainParticipant, const dds::sub::Subscriber& ddsSubscriber,
|
||||
const std::string& specialTopicName, const dds::topic::Filter& filter)
|
||||
{
|
||||
qint64 ptr = reinterpret_cast<qint64>(this);
|
||||
const QByteArray name = QByteArray(DDS::topic_type_name<TopicType>::value().c_str())
|
||||
+ "- content filter 0x"
|
||||
+ QByteArray::number(ptr, 16);
|
||||
m_filteredTopic = dds::topic::ContentFilteredTopic<TopicType>(BaseClass::m_ddsTopic, name.data(), m_filter);
|
||||
BaseClass::m_dataReader = DataReader(ddsSubscriber, m_filteredTopic);
|
||||
if (!BaseClass::create(domainParticipant, ddsSubscriber, specialTopicName, false))
|
||||
return false;
|
||||
m_filter = filter;
|
||||
try
|
||||
{
|
||||
qint64 ptr = reinterpret_cast<qint64>(this);
|
||||
const QByteArray name = QByteArray(DDS::topic_type_name<TopicType>::value().c_str()) +
|
||||
"- content filter 0x" + QByteArray::number(ptr, 16);
|
||||
m_filteredTopic = dds::topic::ContentFilteredTopic<TopicType>(BaseClass::m_ddsTopic, name.data(), m_filter);
|
||||
BaseClass::m_dataReader = DataReader(ddsSubscriber, m_filteredTopic);
|
||||
}
|
||||
catch (const dds::core::Exception& ex)
|
||||
{
|
||||
ERROR_LOG(BaseClass::m_logger) << ex.what();
|
||||
return false;
|
||||
}
|
||||
DEBUG_LOG(BaseClass::m_logger) << "dataReader with filter created (" << DDS::topic_type_name<TopicType>::value()
|
||||
<< ")";
|
||||
return true;
|
||||
}
|
||||
catch (const dds::core::Exception &ex)
|
||||
|
||||
dds::topic::Filter& filter()
|
||||
{
|
||||
ERROR_LOG(BaseClass::m_logger) << ex.what();
|
||||
return false;
|
||||
return m_filter;
|
||||
}
|
||||
DEBUG_LOG(BaseClass::m_logger) << "dataReader with filter created ("
|
||||
<< DDS::topic_type_name<TopicType>::value()
|
||||
<< ")";
|
||||
return true;
|
||||
}
|
||||
|
||||
dds::topic::Filter &filter() { return m_filter; }
|
||||
const dds::topic::Filter &filter() const { return m_filter; }
|
||||
const dds::topic::Filter& filter() const
|
||||
{
|
||||
return m_filter;
|
||||
}
|
||||
|
||||
virtual void recreateReader()
|
||||
{
|
||||
if (BaseClass::m_dataReader == nullptr)
|
||||
return;
|
||||
BaseClass::m_dataReader = DataReader(BaseClass::m_dataReader.subscriber(), m_filteredTopic);
|
||||
}
|
||||
virtual void recreateReader()
|
||||
{
|
||||
if (BaseClass::m_dataReader == nullptr)
|
||||
return;
|
||||
BaseClass::m_dataReader = DataReader(BaseClass::m_dataReader.subscriber(), m_filteredTopic);
|
||||
}
|
||||
|
||||
private:
|
||||
dds::topic::ContentFilteredTopic<TopicType> m_filteredTopic = dds::topic::ContentFilteredTopic<TopicType>(nullptr);
|
||||
dds::topic::Filter m_filter = dds::topic::Filter(std::string());
|
||||
private:
|
||||
dds::topic::ContentFilteredTopic<TopicType> m_filteredTopic = dds::topic::ContentFilteredTopic<TopicType>(nullptr);
|
||||
dds::topic::Filter m_filter = dds::topic::Filter(std::string());
|
||||
};
|
||||
|
||||
}
|
||||
} // namespace DDSGeneric
|
||||
Executable → Regular
+101
-112
@@ -1,142 +1,131 @@
|
||||
|
||||
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <DDS/Generic/generic_base.h>
|
||||
#include <DDS/Generic/generic_logger.h>
|
||||
#include <dds/pub/DataWriter.hpp>
|
||||
#include <dds/sub/SampleInfo.hpp>
|
||||
#include <dds/topic/find.hpp>
|
||||
#include <dds/topic/Topic.hpp>
|
||||
|
||||
#include <DDS/Generic/generic_logger.h>
|
||||
#include <DDS/Generic/generic_base.h>
|
||||
#include <dds/topic/find.hpp>
|
||||
|
||||
namespace DDSGeneric
|
||||
{
|
||||
|
||||
template <typename TOPICTYPE>
|
||||
class GenericDataWriter final
|
||||
template <typename TOPICTYPE> class GenericDataWriter final
|
||||
{
|
||||
public:
|
||||
using TopicType = TOPICTYPE;
|
||||
using DataWriter = dds::pub::DataWriter<TopicType>;
|
||||
using DataTopic = dds::topic::Topic<TopicType>;
|
||||
public:
|
||||
using TopicType = TOPICTYPE;
|
||||
using DataWriter = dds::pub::DataWriter<TopicType>;
|
||||
using DataTopic = dds::topic::Topic<TopicType>;
|
||||
|
||||
GenericDataWriter()
|
||||
: m_dataWriter(nullptr)
|
||||
, m_ddsTopic(nullptr)
|
||||
, m_logger(DDS::getLogger(DDS::topic_type_name<TopicType>::value(), DDS::DataType::DATA_WRITER))
|
||||
{}
|
||||
|
||||
bool create(const dds::domain::DomainParticipant &participant,
|
||||
const dds::pub::Publisher &ddsPublisher,
|
||||
const std::string &topicName = DDS::Helper::getShortTopicName(DDS::topic_type_name<TopicType>::value()))
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "IN GenericDataWriter::create ("
|
||||
<< DDS::topic_type_name<TopicType>::value()
|
||||
<< ")";
|
||||
|
||||
if (participant == nullptr)
|
||||
GenericDataWriter()
|
||||
: m_dataWriter(nullptr), m_ddsTopic(nullptr),
|
||||
m_logger(DDS::getLogger(DDS::topic_type_name<TopicType>::value(), DDS::DataType::DATA_WRITER))
|
||||
{
|
||||
ERROR_LOG(m_logger) << "Participant not available, aborting. ";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (m_ddsTopic != nullptr)
|
||||
bool create(const dds::domain::DomainParticipant& participant, const dds::pub::Publisher& ddsPublisher,
|
||||
const std::string& topicName = DDS::Helper::getShortTopicName(DDS::topic_type_name<TopicType>::value()))
|
||||
{
|
||||
ERROR_LOG(m_logger) << "Type '"
|
||||
<< DDS::topic_type_name<TopicType>::value()
|
||||
<< "' already created";
|
||||
return false;
|
||||
DEBUG_LOG(m_logger) << "IN GenericDataWriter::create (" << DDS::topic_type_name<TopicType>::value() << ")";
|
||||
|
||||
if (participant == nullptr)
|
||||
{
|
||||
ERROR_LOG(m_logger) << "Participant not available, aborting. ";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (m_ddsTopic != nullptr)
|
||||
{
|
||||
ERROR_LOG(m_logger) << "Type '" << DDS::topic_type_name<TopicType>::value() << "' already created";
|
||||
return false;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
|
||||
DEBUG_LOG(m_logger) << "Creating write topic " << topicName
|
||||
<< " with type: " << DDS::topic_type_name<TopicType>::value();
|
||||
|
||||
m_ddsTopic = dds::topic::find<DataTopic>(participant, topicName);
|
||||
if (m_ddsTopic == nullptr)
|
||||
m_ddsTopic = DataTopic(participant, topicName);
|
||||
|
||||
m_dataWriter = DataWriter(ddsPublisher, m_ddsTopic, ddsPublisher.default_datawriter_qos());
|
||||
|
||||
if (m_dataWriter == nullptr)
|
||||
{
|
||||
ERROR_LOG(m_logger) << "Initialisation of " << DDS::topic_type_name<TopicType>::value() << " failed ";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
catch (const dds::core::Exception& ex)
|
||||
{
|
||||
ERROR_LOG(m_logger) << ex.what();
|
||||
return false;
|
||||
}
|
||||
|
||||
DEBUG_LOG(m_logger) << "dataWriter created (" << DDS::topic_type_name<TopicType>::value() << ")";
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
try {
|
||||
|
||||
DEBUG_LOG(m_logger) << "Creating write topic " << topicName << " with type: "
|
||||
<< DDS::topic_type_name<TopicType>::value();
|
||||
|
||||
m_ddsTopic = dds::topic::find<DataTopic>(participant, topicName);
|
||||
if (m_ddsTopic == nullptr)
|
||||
m_ddsTopic = DataTopic(participant, topicName);
|
||||
|
||||
m_dataWriter = DataWriter(ddsPublisher,
|
||||
m_ddsTopic,
|
||||
ddsPublisher.default_datawriter_qos());
|
||||
|
||||
if (m_dataWriter == nullptr)
|
||||
{
|
||||
ERROR_LOG(m_logger) << "Initialisation of "
|
||||
<< DDS::topic_type_name<TopicType>::value()
|
||||
<< " failed ";
|
||||
return false;
|
||||
}
|
||||
|
||||
}
|
||||
catch ( const dds::core::Exception &ex )
|
||||
bool write(const TopicType& data,
|
||||
const dds::core::optional<dds::sub::SampleInfo>& si = dds::core::optional<dds::sub::SampleInfo>())
|
||||
{
|
||||
ERROR_LOG(m_logger) << ex.what();
|
||||
return false;
|
||||
DEBUG_LOG(m_logger) << "IN write (" << DDS::topic_type_name<TopicType>::value() << ") ";
|
||||
|
||||
if (m_dataWriter == nullptr)
|
||||
{
|
||||
ERROR_LOG(m_logger) << "No dataWriter initialised ";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (si.is_set())
|
||||
{
|
||||
rti::pub::WriteParams writeParams;
|
||||
rti::core::SampleIdentity sampleIdentity(si.get()->source_guid(), si.get()->publication_sequence_number());
|
||||
writeParams.related_sample_identity(sampleIdentity);
|
||||
|
||||
m_dataWriter->write(data, writeParams);
|
||||
}
|
||||
else
|
||||
{
|
||||
m_dataWriter.write(data);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
DEBUG_LOG(m_logger) << "dataWriter created ("
|
||||
<< DDS::topic_type_name<TopicType>::value()
|
||||
<< ")";
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool write(const TopicType &data,
|
||||
const dds::core::optional<dds::sub::SampleInfo> &si = dds::core::optional<dds::sub::SampleInfo>())
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "IN write ("
|
||||
<< DDS::topic_type_name<TopicType>::value()
|
||||
<< ") ";
|
||||
|
||||
if (m_dataWriter == nullptr) {
|
||||
ERROR_LOG(m_logger) << "No dataWriter initialised ";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (si.is_set())
|
||||
bool remove(const TopicType& data, bool errorMsgIfNotExists = true)
|
||||
{
|
||||
rti::pub::WriteParams writeParams;
|
||||
rti::core::SampleIdentity sampleIdentity(si.get()->source_guid(), si.get()->publication_sequence_number());
|
||||
writeParams.related_sample_identity(sampleIdentity);
|
||||
DEBUG_LOG(m_logger) << "IN remove (" << DDS::topic_type_name<TopicType>::value() << ") ";
|
||||
|
||||
m_dataWriter->write(data, writeParams);
|
||||
} else {
|
||||
m_dataWriter.write(data);
|
||||
if (m_dataWriter == nullptr)
|
||||
{
|
||||
ERROR_LOG(m_logger) << "No dataWriter initialised ";
|
||||
return false;
|
||||
}
|
||||
const dds::core::InstanceHandle ih = m_dataWriter.lookup_instance(data);
|
||||
if (ih.is_nil())
|
||||
{
|
||||
if (errorMsgIfNotExists)
|
||||
ERROR_LOG(m_logger) << "Could not find instance for topic " << DDS::topic_type_name<TopicType>::value()
|
||||
<< " for unregistering!";
|
||||
return false;
|
||||
}
|
||||
m_dataWriter.dispose_instance(ih);
|
||||
return true;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool remove(const TopicType &data, bool errorMsgIfNotExists = true)
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "IN remove ("
|
||||
<< DDS::topic_type_name<TopicType>::value()
|
||||
<< ") ";
|
||||
|
||||
if (m_dataWriter == nullptr) {
|
||||
ERROR_LOG(m_logger) << "No dataWriter initialised ";
|
||||
return false;
|
||||
DataWriter dataWriter() const
|
||||
{
|
||||
return m_dataWriter;
|
||||
}
|
||||
const dds::core::InstanceHandle ih = m_dataWriter.lookup_instance(data);
|
||||
if (ih.is_nil()) {
|
||||
if (errorMsgIfNotExists)
|
||||
ERROR_LOG(m_logger) << "Could not find instance for topic "
|
||||
<< DDS::topic_type_name<TopicType>::value()
|
||||
<< " for unregistering!";
|
||||
return false;
|
||||
}
|
||||
m_dataWriter.dispose_instance(ih);
|
||||
return true;
|
||||
}
|
||||
|
||||
DataWriter dataWriter() const { return m_dataWriter; }
|
||||
|
||||
private:
|
||||
DataWriter m_dataWriter;
|
||||
DataTopic m_ddsTopic;
|
||||
private:
|
||||
DataWriter m_dataWriter;
|
||||
DataTopic m_ddsTopic;
|
||||
};
|
||||
|
||||
}
|
||||
} // namespace DDSGeneric
|
||||
Executable → Regular
+128
-147
@@ -1,7 +1,7 @@
|
||||
|
||||
|
||||
|
||||
#include "subscriber_listener.h"
|
||||
|
||||
#include "generic_base.h"
|
||||
#include "generic_logger.h"
|
||||
|
||||
@@ -9,183 +9,164 @@ namespace DDSGeneric
|
||||
{
|
||||
|
||||
SubscriberListener::SubscriberListener()
|
||||
: m_bPrepareForShutdown(false)
|
||||
, m_logger(DDS::getLogger("SubscriberListener", DDS::DataType::DATA_TOPIC_CLIENT))
|
||||
: m_bPrepareForShutdown(false), m_logger(DDS::getLogger("SubscriberListener", DDS::DataType::DATA_TOPIC_CLIENT))
|
||||
{
|
||||
qRegisterMetaType<std::string>("std::string");
|
||||
DEBUG_LOG(m_logger) << "SubscriberListener initialized, this " << this;
|
||||
qRegisterMetaType<std::string>("std::string");
|
||||
DEBUG_LOG(m_logger) << "SubscriberListener initialized, this " << this;
|
||||
}
|
||||
|
||||
void SubscriberListener::addCallback(const std::string &topicName,
|
||||
const std::string &topicTypeName,
|
||||
const dds::sub::AnyDataReader &dataReader,
|
||||
SubscriberListenerCallback *cb)
|
||||
void SubscriberListener::addCallback(const std::string& topicName, const std::string& topicTypeName,
|
||||
const dds::sub::AnyDataReader& dataReader, SubscriberListenerCallback* cb)
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "addSubscriberCallback for " << topicName << ", type " << topicTypeName
|
||||
<< ", cb " << cb << ", this " << this;
|
||||
QWriteLocker l(&m_accessLock);
|
||||
if (m_bPrepareForShutdown)
|
||||
return;
|
||||
m_droCallbacks.insertMulti(std::make_pair(topicName, topicTypeName), fillData(dataReader, cb));
|
||||
}
|
||||
|
||||
void SubscriberListener::removeCallback(const std::string &topicName,
|
||||
const std::string &topicTypeName,
|
||||
const dds::sub::AnyDataReader &dataReader,
|
||||
SubscriberListenerCallback *cb)
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "IN removeSubscriberCallback for " << topicName << ", type " << topicTypeName
|
||||
<< ", cb " << cb << ", this " << this;
|
||||
QWriteLocker l(&m_accessLock);
|
||||
m_droCallbacks.remove(std::make_pair(topicName, topicTypeName), fillData(dataReader, cb));
|
||||
}
|
||||
|
||||
void SubscriberListener::enaDisaCallback(const std::string &topicName,
|
||||
const std::string &topicTypeName,
|
||||
const dds::sub::AnyDataReader &dataReader,
|
||||
SubscriberListenerCallback *cb,
|
||||
bool bEnable)
|
||||
{
|
||||
const std::pair<std::string, std::string> key(std::make_pair(topicName, topicTypeName));
|
||||
const cbData cbDataSearch(fillData(dataReader, cb));
|
||||
bool bFound = false;
|
||||
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "addSubscriberCallback for " << topicName << ", type " << topicTypeName << ", cb " << cb
|
||||
<< ", this " << this;
|
||||
QWriteLocker l(&m_accessLock);
|
||||
if (m_bPrepareForShutdown)
|
||||
return;
|
||||
return;
|
||||
m_droCallbacks.insertMulti(std::make_pair(topicName, topicTypeName), fillData(dataReader, cb));
|
||||
}
|
||||
|
||||
void SubscriberListener::removeCallback(const std::string& topicName, const std::string& topicTypeName,
|
||||
const dds::sub::AnyDataReader& dataReader, SubscriberListenerCallback* cb)
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "IN removeSubscriberCallback for " << topicName << ", type " << topicTypeName << ", cb "
|
||||
<< cb << ", this " << this;
|
||||
QWriteLocker l(&m_accessLock);
|
||||
m_droCallbacks.remove(std::make_pair(topicName, topicTypeName), fillData(dataReader, cb));
|
||||
}
|
||||
|
||||
void SubscriberListener::enaDisaCallback(const std::string& topicName, const std::string& topicTypeName,
|
||||
const dds::sub::AnyDataReader& dataReader, SubscriberListenerCallback* cb,
|
||||
bool bEnable)
|
||||
{
|
||||
const std::pair<std::string, std::string> key(std::make_pair(topicName, topicTypeName));
|
||||
const cbData cbDataSearch(fillData(dataReader, cb));
|
||||
bool bFound = false;
|
||||
|
||||
QMultiMap<std::pair<std::string, std::string>, cbData>::iterator cbIt = m_droCallbacks.find(key);
|
||||
for( ; cbIt != m_droCallbacks.end() && cbIt.key() == key; ++cbIt)
|
||||
{
|
||||
const cbData &data = cbIt.value();
|
||||
if (data != cbDataSearch)
|
||||
continue;
|
||||
bFound = true;
|
||||
cbIt.value().enabled = bEnable;
|
||||
break;
|
||||
}
|
||||
}
|
||||
QWriteLocker l(&m_accessLock);
|
||||
if (m_bPrepareForShutdown)
|
||||
return;
|
||||
|
||||
if (!bFound)
|
||||
WARNING_LOG(m_logger) << "Can not " << (bEnable ? "enable" : "disable")
|
||||
<< " callback for " << topicName << ", type " << topicTypeName
|
||||
<< ", cb " << cb << ", this " << this << " - not found!";
|
||||
QMultiMap<std::pair<std::string, std::string>, cbData>::iterator cbIt = m_droCallbacks.find(key);
|
||||
for (; cbIt != m_droCallbacks.end() && cbIt.key() == key; ++cbIt)
|
||||
{
|
||||
const cbData& data = cbIt.value();
|
||||
if (data != cbDataSearch)
|
||||
continue;
|
||||
bFound = true;
|
||||
cbIt.value().enabled = bEnable;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!bFound)
|
||||
WARNING_LOG(m_logger) << "Can not " << (bEnable ? "enable" : "disable") << " callback for " << topicName
|
||||
<< ", type " << topicTypeName << ", cb " << cb << ", this " << this << " - not found!";
|
||||
}
|
||||
|
||||
void SubscriberListener::prepareShutdown()
|
||||
{
|
||||
QWriteLocker l(&m_accessLock);
|
||||
m_droCallbacks.clear();
|
||||
m_bPrepareForShutdown = true;
|
||||
QWriteLocker l(&m_accessLock);
|
||||
m_droCallbacks.clear();
|
||||
m_bPrepareForShutdown = true;
|
||||
}
|
||||
|
||||
|
||||
void SubscriberListener::on_data_available(dds::sub::AnyDataReader &reader)
|
||||
void SubscriberListener::on_data_available(dds::sub::AnyDataReader& reader)
|
||||
{
|
||||
|
||||
const std::string &topicName = reader.topic_name();
|
||||
const std::string &topicTypeName = reader.type_name();
|
||||
const std::pair<std::string, std::string> key(std::make_pair(topicName, topicTypeName));
|
||||
|
||||
QReadLocker l(&m_accessLock);
|
||||
if (m_droCallbacks.isEmpty() || m_bPrepareForShutdown)
|
||||
return;
|
||||
DEBUG_LOG(m_logger) << "Data available for " << topicName << ", type " << topicTypeName << ", this " << this;
|
||||
QMultiMap<std::pair<std::string, std::string>, cbData>::iterator cbIt = m_droCallbacks.find(key);
|
||||
const QMultiMap<std::pair<std::string, std::string>, cbData>::iterator cbEnd = m_droCallbacks.end();
|
||||
for( ; cbIt != cbEnd && cbIt.key() == key; ++cbIt)
|
||||
{
|
||||
const cbData &data = cbIt.value();
|
||||
if (data.dataReader != reader)
|
||||
continue;
|
||||
const std::string& topicName = reader.topic_name();
|
||||
const std::string& topicTypeName = reader.type_name();
|
||||
const std::pair<std::string, std::string> key(std::make_pair(topicName, topicTypeName));
|
||||
|
||||
if (!data.enabled)
|
||||
continue;
|
||||
|
||||
if (data.methodToCall.isValid())
|
||||
QReadLocker l(&m_accessLock);
|
||||
if (m_droCallbacks.isEmpty() || m_bPrepareForShutdown)
|
||||
return;
|
||||
DEBUG_LOG(m_logger) << "Data available for " << topicName << ", type " << topicTypeName << ", this " << this;
|
||||
QMultiMap<std::pair<std::string, std::string>, cbData>::iterator cbIt = m_droCallbacks.find(key);
|
||||
const QMultiMap<std::pair<std::string, std::string>, cbData>::iterator cbEnd = m_droCallbacks.end();
|
||||
for (; cbIt != cbEnd && cbIt.key() == key; ++cbIt)
|
||||
{
|
||||
data.methodToCall.invoke(data.cb, Qt::QueuedConnection,
|
||||
Q_ARG(std::string, topicTypeName),
|
||||
Q_ARG(std::string, topicName));
|
||||
cbIt.value().enabled = false;
|
||||
const cbData& data = cbIt.value();
|
||||
if (data.dataReader != reader)
|
||||
continue;
|
||||
|
||||
if (!data.enabled)
|
||||
continue;
|
||||
|
||||
if (data.methodToCall.isValid())
|
||||
{
|
||||
data.methodToCall.invoke(data.cb, Qt::QueuedConnection, Q_ARG(std::string, topicTypeName),
|
||||
Q_ARG(std::string, topicName));
|
||||
cbIt.value().enabled = false;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void SubscriberListener::on_liveliness_changed(dds::sub::AnyDataReader &reader,
|
||||
const dds::core::status::LivelinessChangedStatus &livelinessStatus)
|
||||
void SubscriberListener::on_liveliness_changed(dds::sub::AnyDataReader& reader,
|
||||
const dds::core::status::LivelinessChangedStatus& livelinessStatus)
|
||||
{
|
||||
const std::string &topicName = reader.topic_name();
|
||||
const std::string &topicTypeName = reader.type_name();
|
||||
|
||||
DEBUG_LOG(m_logger) << "on_liveliness_changed: " << livelinessStatus.alive_count()
|
||||
<< ", " << livelinessStatus.not_alive_count()
|
||||
<< ", " << livelinessStatus.alive_count_change()
|
||||
<< ", " << livelinessStatus.not_alive_count_change()
|
||||
<< " for type " << topicTypeName;
|
||||
const std::string& topicName = reader.topic_name();
|
||||
const std::string& topicTypeName = reader.type_name();
|
||||
|
||||
bool bNewWriters = false;
|
||||
bool bNoWritersAnymore = false;
|
||||
if (livelinessStatus.alive_count() == 0 &&
|
||||
livelinessStatus.alive_count_change() < 0)
|
||||
bNoWritersAnymore = true;
|
||||
if (livelinessStatus.alive_count() != 0 &&
|
||||
livelinessStatus.alive_count_change() == livelinessStatus.alive_count())
|
||||
bNewWriters = true;
|
||||
if (!bNewWriters && !bNoWritersAnymore)
|
||||
return;
|
||||
DEBUG_LOG(m_logger) << "on_liveliness_changed: " << livelinessStatus.alive_count() << ", "
|
||||
<< livelinessStatus.not_alive_count() << ", " << livelinessStatus.alive_count_change() << ", "
|
||||
<< livelinessStatus.not_alive_count_change() << " for type " << topicTypeName;
|
||||
|
||||
QReadLocker l(&m_accessLock);
|
||||
if (m_droCallbacks.isEmpty())
|
||||
return;
|
||||
bool bNewWriters = false;
|
||||
bool bNoWritersAnymore = false;
|
||||
if (livelinessStatus.alive_count() == 0 && livelinessStatus.alive_count_change() < 0)
|
||||
bNoWritersAnymore = true;
|
||||
if (livelinessStatus.alive_count() != 0 && livelinessStatus.alive_count_change() == livelinessStatus.alive_count())
|
||||
bNewWriters = true;
|
||||
if (!bNewWriters && !bNoWritersAnymore)
|
||||
return;
|
||||
|
||||
const std::pair<std::string, std::string> key(std::make_pair(topicName, topicTypeName));
|
||||
QMultiMap<std::pair<std::string, std::string>, cbData>::iterator cbIt = m_droCallbacks.find(key);
|
||||
const QMultiMap<std::pair<std::string, std::string>, cbData>::iterator cbEnd = m_droCallbacks.end();
|
||||
for( ; cbIt != cbEnd && cbIt.key() == key; ++cbIt)
|
||||
{
|
||||
const cbData &data = cbIt.value();
|
||||
if (data.dataReader != reader)
|
||||
continue;
|
||||
QReadLocker l(&m_accessLock);
|
||||
if (m_droCallbacks.isEmpty())
|
||||
return;
|
||||
|
||||
if (bNoWritersAnymore)
|
||||
const std::pair<std::string, std::string> key(std::make_pair(topicName, topicTypeName));
|
||||
QMultiMap<std::pair<std::string, std::string>, cbData>::iterator cbIt = m_droCallbacks.find(key);
|
||||
const QMultiMap<std::pair<std::string, std::string>, cbData>::iterator cbEnd = m_droCallbacks.end();
|
||||
for (; cbIt != cbEnd && cbIt.key() == key; ++cbIt)
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "bNoWritersAnymore for " << topicName << ", type " << topicTypeName;
|
||||
data.cb->noWriterAnymore(topicTypeName, topicName);
|
||||
const cbData& data = cbIt.value();
|
||||
if (data.dataReader != reader)
|
||||
continue;
|
||||
|
||||
if (bNoWritersAnymore)
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "bNoWritersAnymore for " << topicName << ", type " << topicTypeName;
|
||||
data.cb->noWriterAnymore(topicTypeName, topicName);
|
||||
}
|
||||
else if (bNewWriters)
|
||||
{
|
||||
data.cb->writerAvailable(topicTypeName, topicName);
|
||||
if (!data.enabled)
|
||||
continue;
|
||||
if (data.methodToCall.isValid())
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "methodcall is valid for " << topicName << ", type " << topicTypeName;
|
||||
data.methodToCall.invoke(data.cb, Qt::QueuedConnection, Q_ARG(std::string, topicTypeName),
|
||||
Q_ARG(std::string, topicName));
|
||||
cbIt.value().enabled = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (bNewWriters)
|
||||
{
|
||||
data.cb->writerAvailable(topicTypeName, topicName);
|
||||
if (!data.enabled)
|
||||
continue;
|
||||
if (data.methodToCall.isValid())
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "methodcall is valid for " << topicName << ", type " << topicTypeName;
|
||||
data.methodToCall.invoke(data.cb, Qt::QueuedConnection,
|
||||
Q_ARG(std::string, topicTypeName),
|
||||
Q_ARG(std::string, topicName));
|
||||
cbIt.value().enabled = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
SubscriberListener::cbData SubscriberListener::fillData(const dds::sub::AnyDataReader &dataReader, SubscriberListenerCallback *cb) const
|
||||
SubscriberListener::cbData SubscriberListener::fillData(const dds::sub::AnyDataReader& dataReader,
|
||||
SubscriberListenerCallback* cb) const
|
||||
{
|
||||
|
||||
const char *signal = SIGNAL(droDataAvailable(std::string,std::string));
|
||||
const char *slot = SLOT(droDataAvailable(std::string,std::string));
|
||||
QMetaMethod method = DDS::Helper::getMethod(m_logger,
|
||||
cb,
|
||||
slot,
|
||||
signal);
|
||||
|
||||
|
||||
return cbData(cb, dataReader, method);
|
||||
|
||||
const char* signal = SIGNAL(droDataAvailable(std::string, std::string));
|
||||
const char* slot = SLOT(droDataAvailable(std::string, std::string));
|
||||
QMetaMethod method = DDS::Helper::getMethod(m_logger, cb, slot, signal);
|
||||
|
||||
return cbData(cb, dataReader, method);
|
||||
}
|
||||
|
||||
}
|
||||
} // namespace DDSGeneric
|
||||
Executable → Regular
+113
-115
@@ -1,140 +1,138 @@
|
||||
|
||||
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <ddsgen_export.h>
|
||||
#include <dds/sub/SubscriberListener.hpp>
|
||||
#include <dds/sub/AnyDataReader.hpp>
|
||||
#include <dds/topic/Topic.hpp>
|
||||
#include <QMetaMethod>
|
||||
#include <QReadWriteLock>
|
||||
#include <QObject>
|
||||
#include <QReadWriteLock>
|
||||
#include <dds/sub/AnyDataReader.hpp>
|
||||
#include <dds/sub/SubscriberListener.hpp>
|
||||
#include <dds/topic/Topic.hpp>
|
||||
#include <ddsgen_export.h>
|
||||
|
||||
namespace NGL {
|
||||
namespace NGL
|
||||
{
|
||||
class INGLLogger;
|
||||
}
|
||||
|
||||
|
||||
|
||||
namespace DDSGeneric
|
||||
{
|
||||
class DDSGEN_EXPORT SubscriberListenerCallback : public QObject
|
||||
{
|
||||
Q_OBJECT
|
||||
public:
|
||||
virtual ~SubscriberListenerCallback() = default;
|
||||
public Q_SLOTS:
|
||||
|
||||
virtual void droDataAvailable(const std::string &topicType, const std::string &topicName) = 0;
|
||||
|
||||
virtual void noWriterAnymore(const std::string &topicType, const std::string &topicName) = 0;
|
||||
virtual void writerAvailable(const std::string &topicType, const std::string &topicName) = 0;
|
||||
Q_OBJECT
|
||||
public:
|
||||
virtual ~SubscriberListenerCallback() = default;
|
||||
public Q_SLOTS:
|
||||
|
||||
virtual void droDataAvailable(const std::string& topicType, const std::string& topicName) = 0;
|
||||
|
||||
virtual void noWriterAnymore(const std::string& topicType, const std::string& topicName) = 0;
|
||||
virtual void writerAvailable(const std::string& topicType, const std::string& topicName) = 0;
|
||||
};
|
||||
|
||||
class DDSGEN_EXPORT SubscriberListener final : public dds::sub::SubscriberListener
|
||||
{
|
||||
public:
|
||||
SubscriberListener();
|
||||
public:
|
||||
SubscriberListener();
|
||||
|
||||
template<typename TopicType>
|
||||
void addCallback(const dds::sub::DataReader<TopicType> &dataReader,
|
||||
SubscriberListenerCallback *cb)
|
||||
{
|
||||
addCallback(dataReader->topic_name(), dataReader->type_name(), dds::sub::AnyDataReader(dataReader), cb);
|
||||
}
|
||||
template<typename TopicType>
|
||||
void removeCallback(const dds::sub::DataReader<TopicType> &dataReader,
|
||||
SubscriberListenerCallback *cb)
|
||||
{
|
||||
removeCallback(dataReader->topic_name(), dataReader->type_name(), dds::sub::AnyDataReader(dataReader), cb);
|
||||
}
|
||||
|
||||
template<typename TopicType>
|
||||
void enableCallback(const dds::sub::DataReader<TopicType> &dataReader,
|
||||
SubscriberListenerCallback *cb)
|
||||
{
|
||||
enaDisaCallback(dataReader->topic_name(), dataReader->type_name(), dds::sub::AnyDataReader(dataReader), cb, true);
|
||||
}
|
||||
template<typename TopicType>
|
||||
void disableCallback(const dds::sub::DataReader<TopicType> &dataReader,
|
||||
SubscriberListenerCallback *cb)
|
||||
{
|
||||
enaDisaCallback(dataReader->topic_name(), dataReader->type_name(), dds::sub::AnyDataReader(dataReader), cb, false);
|
||||
}
|
||||
|
||||
static dds::core::status::StatusMask mask()
|
||||
{
|
||||
return (dds::core::status::StatusMask::liveliness_changed() | dds::core::status::StatusMask::data_available());
|
||||
}
|
||||
|
||||
void prepareShutdown();
|
||||
|
||||
protected:
|
||||
NGL::INGLLogger &m_logger;
|
||||
void addCallback(const std::string &topicName,
|
||||
const std::string &topicTypeName,
|
||||
const dds::sub::AnyDataReader &dataReader,
|
||||
SubscriberListenerCallback *cb);
|
||||
void removeCallback(const std::string &topicName,
|
||||
const std::string &topicTypeName,
|
||||
const dds::sub::AnyDataReader &dataReader,
|
||||
SubscriberListenerCallback *cb);
|
||||
void enaDisaCallback(const std::string &topicName,
|
||||
const std::string &topicTypeName,
|
||||
const dds::sub::AnyDataReader &dataReader,
|
||||
SubscriberListenerCallback *cb,
|
||||
bool bEnable);
|
||||
|
||||
void on_data_on_readers(dds::sub::Subscriber &) override {}
|
||||
|
||||
void on_liveliness_changed(dds::sub::AnyDataReader &reader,
|
||||
const dds::core::status::LivelinessChangedStatus &livelinessStatus) override;
|
||||
|
||||
void on_requested_deadline_missed(dds::sub::AnyDataReader&,
|
||||
const dds::core::status::RequestedDeadlineMissedStatus&) override {}
|
||||
void on_requested_incompatible_qos(dds::sub::AnyDataReader&,
|
||||
const dds::core::status::RequestedIncompatibleQosStatus&) override {}
|
||||
void on_sample_rejected(dds::sub::AnyDataReader&,
|
||||
const dds::core::status::SampleRejectedStatus&) override {}
|
||||
void on_data_available(dds::sub::AnyDataReader &reader) override;
|
||||
void on_subscription_matched(dds::sub::AnyDataReader&,
|
||||
const dds::core::status::SubscriptionMatchedStatus&) override {}
|
||||
void on_sample_lost(dds::sub::AnyDataReader&,
|
||||
const dds::core::status::SampleLostStatus&) override {}
|
||||
|
||||
private:
|
||||
struct cbData
|
||||
{
|
||||
SubscriberListenerCallback *cb = nullptr;
|
||||
bool enabled = true;
|
||||
dds::sub::AnyDataReader dataReader;
|
||||
QMetaMethod methodToCall;
|
||||
|
||||
bool operator ==(const cbData &o) const
|
||||
template <typename TopicType>
|
||||
void addCallback(const dds::sub::DataReader<TopicType>& dataReader, SubscriberListenerCallback* cb)
|
||||
{
|
||||
|
||||
return cb == o.cb && methodToCall == o.methodToCall && dataReader == o.dataReader;
|
||||
}
|
||||
bool operator !=(const cbData &o) const
|
||||
{
|
||||
return !operator ==(o);
|
||||
addCallback(dataReader->topic_name(), dataReader->type_name(), dds::sub::AnyDataReader(dataReader), cb);
|
||||
}
|
||||
|
||||
cbData(SubscriberListenerCallback *cb_,
|
||||
const dds::sub::AnyDataReader &dataReader_,
|
||||
const QMetaMethod &methodToCall_)
|
||||
: cb(cb_)
|
||||
, enabled(true)
|
||||
, dataReader(dataReader_)
|
||||
, methodToCall(methodToCall_)
|
||||
{}
|
||||
};
|
||||
cbData fillData(const dds::sub::AnyDataReader &dataReader, SubscriberListenerCallback *cb) const;
|
||||
template <typename TopicType>
|
||||
void removeCallback(const dds::sub::DataReader<TopicType>& dataReader, SubscriberListenerCallback* cb)
|
||||
{
|
||||
removeCallback(dataReader->topic_name(), dataReader->type_name(), dds::sub::AnyDataReader(dataReader), cb);
|
||||
}
|
||||
|
||||
QMultiMap<std::pair<std::string, std::string>, cbData> m_droCallbacks;
|
||||
QReadWriteLock m_accessLock;
|
||||
bool m_bPrepareForShutdown;
|
||||
template <typename TopicType>
|
||||
void enableCallback(const dds::sub::DataReader<TopicType>& dataReader, SubscriberListenerCallback* cb)
|
||||
{
|
||||
enaDisaCallback(dataReader->topic_name(), dataReader->type_name(), dds::sub::AnyDataReader(dataReader), cb,
|
||||
true);
|
||||
}
|
||||
|
||||
template <typename TopicType>
|
||||
void disableCallback(const dds::sub::DataReader<TopicType>& dataReader, SubscriberListenerCallback* cb)
|
||||
{
|
||||
enaDisaCallback(dataReader->topic_name(), dataReader->type_name(), dds::sub::AnyDataReader(dataReader), cb,
|
||||
false);
|
||||
}
|
||||
|
||||
static dds::core::status::StatusMask mask()
|
||||
{
|
||||
return (dds::core::status::StatusMask::liveliness_changed() | dds::core::status::StatusMask::data_available());
|
||||
}
|
||||
|
||||
void prepareShutdown();
|
||||
|
||||
protected:
|
||||
NGL::INGLLogger& m_logger;
|
||||
void addCallback(const std::string& topicName, const std::string& topicTypeName,
|
||||
const dds::sub::AnyDataReader& dataReader, SubscriberListenerCallback* cb);
|
||||
void removeCallback(const std::string& topicName, const std::string& topicTypeName,
|
||||
const dds::sub::AnyDataReader& dataReader, SubscriberListenerCallback* cb);
|
||||
void enaDisaCallback(const std::string& topicName, const std::string& topicTypeName,
|
||||
const dds::sub::AnyDataReader& dataReader, SubscriberListenerCallback* cb, bool bEnable);
|
||||
|
||||
void on_data_on_readers(dds::sub::Subscriber&) override {}
|
||||
|
||||
void on_liveliness_changed(dds::sub::AnyDataReader& reader,
|
||||
const dds::core::status::LivelinessChangedStatus& livelinessStatus) override;
|
||||
|
||||
void on_requested_deadline_missed(dds::sub::AnyDataReader&,
|
||||
const dds::core::status::RequestedDeadlineMissedStatus&) override
|
||||
{
|
||||
}
|
||||
|
||||
void on_requested_incompatible_qos(dds::sub::AnyDataReader&,
|
||||
const dds::core::status::RequestedIncompatibleQosStatus&) override
|
||||
{
|
||||
}
|
||||
|
||||
void on_sample_rejected(dds::sub::AnyDataReader&, const dds::core::status::SampleRejectedStatus&) override {}
|
||||
|
||||
void on_data_available(dds::sub::AnyDataReader& reader) override;
|
||||
|
||||
void on_subscription_matched(dds::sub::AnyDataReader&, const dds::core::status::SubscriptionMatchedStatus&) override
|
||||
{
|
||||
}
|
||||
|
||||
void on_sample_lost(dds::sub::AnyDataReader&, const dds::core::status::SampleLostStatus&) override {}
|
||||
|
||||
private:
|
||||
struct cbData
|
||||
{
|
||||
SubscriberListenerCallback* cb = nullptr;
|
||||
bool enabled = true;
|
||||
dds::sub::AnyDataReader dataReader;
|
||||
QMetaMethod methodToCall;
|
||||
|
||||
bool operator==(const cbData& o) const
|
||||
{
|
||||
|
||||
return cb == o.cb && methodToCall == o.methodToCall && dataReader == o.dataReader;
|
||||
}
|
||||
|
||||
bool operator!=(const cbData& o) const
|
||||
{
|
||||
return !operator==(o);
|
||||
}
|
||||
|
||||
cbData(SubscriberListenerCallback* cb_, const dds::sub::AnyDataReader& dataReader_,
|
||||
const QMetaMethod& methodToCall_)
|
||||
: cb(cb_), enabled(true), dataReader(dataReader_), methodToCall(methodToCall_)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
cbData fillData(const dds::sub::AnyDataReader& dataReader, SubscriberListenerCallback* cb) const;
|
||||
|
||||
QMultiMap<std::pair<std::string, std::string>, cbData> m_droCallbacks;
|
||||
QReadWriteLock m_accessLock;
|
||||
bool m_bPrepareForShutdown;
|
||||
};
|
||||
|
||||
}
|
||||
} // namespace DDSGeneric
|
||||
Executable → Regular
+103
-106
@@ -1,147 +1,144 @@
|
||||
|
||||
|
||||
|
||||
|
||||
#include "wait_set_handler.h"
|
||||
#include "generic_logger.h"
|
||||
|
||||
#include "generic_base.h"
|
||||
#include "generic_logger.h"
|
||||
|
||||
namespace DDSGeneric
|
||||
{
|
||||
|
||||
WaitSetHandler::WaitSetHandler()
|
||||
: m_logger(DDS::getLogger("WaitSetHandler", DDS::DataType::FUNCTION_TOPIC_SERVER))
|
||||
WaitSetHandler::WaitSetHandler() : m_logger(DDS::getLogger("WaitSetHandler", DDS::DataType::FUNCTION_TOPIC_SERVER))
|
||||
{
|
||||
m_waitSet.attach_condition(m_abortCondition);
|
||||
m_task = std::async(std::launch::async, [this]() { this->waitSetTask();});
|
||||
m_waitSet.attach_condition(m_abortCondition);
|
||||
m_task = std::async(std::launch::async, [this]() { this->waitSetTask(); });
|
||||
}
|
||||
|
||||
WaitSetHandler::~WaitSetHandler()
|
||||
{
|
||||
m_abortCondition.trigger_value(true);
|
||||
m_task.wait();
|
||||
m_abortCondition.trigger_value(true);
|
||||
m_task.wait();
|
||||
}
|
||||
|
||||
bool WaitSetHandler::attachWaitSet(const dds::core::cond::Condition &cond, CallbackInterface *cb)
|
||||
bool WaitSetHandler::attachWaitSet(const dds::core::cond::Condition& cond, CallbackInterface* cb)
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "AttachWaitSet with callback " << cb;
|
||||
DEBUG_LOG(m_logger) << "AttachWaitSet with callback " << cb;
|
||||
|
||||
WaitSetData wsd(cond, cb);
|
||||
WaitSetData wsd(cond, cb);
|
||||
|
||||
std::lock_guard<std::mutex> l(m_accessMutex);
|
||||
|
||||
auto it = m_waitSetData.find(wsd.condition->native_condition());
|
||||
if (it != m_waitSetData.end())
|
||||
{
|
||||
WARNING_LOG(m_logger) << "Waitset " << cb << " already registered";
|
||||
return false;
|
||||
}
|
||||
std::lock_guard<std::mutex> l(m_accessMutex);
|
||||
|
||||
m_waitSet.attach_condition(wsd.condition);
|
||||
wsd.enabled = true;
|
||||
wsd.needWaitSetAttach = false;
|
||||
auto it = m_waitSetData.find(wsd.condition->native_condition());
|
||||
if (it != m_waitSetData.end())
|
||||
{
|
||||
WARNING_LOG(m_logger) << "Waitset " << cb << " already registered";
|
||||
return false;
|
||||
}
|
||||
|
||||
m_waitSetData.insert(std::make_pair(wsd.condition->native_condition(), wsd));
|
||||
return true;
|
||||
}
|
||||
|
||||
bool WaitSetHandler::detachWaitSet(const dds::core::cond::Condition &cond, CallbackInterface *cb)
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "DetachWaitSet with callback " << cb;
|
||||
|
||||
WaitSetData wsd(cond, cb);
|
||||
|
||||
std::lock_guard<std::mutex> l(m_accessMutex);
|
||||
|
||||
auto it = m_waitSetData.find(wsd.condition->native_condition());
|
||||
if (it == m_waitSetData.end())
|
||||
{
|
||||
WARNING_LOG(m_logger) << "Waitset for " << cb << " not registered";
|
||||
return false;
|
||||
}
|
||||
|
||||
m_waitSet.detach_condition(wsd.condition);
|
||||
m_waitSetData.erase(it);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool WaitSetHandler::enableWaitSet(const dds::core::cond::Condition &cond, CallbackInterface *cb)
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "EnableWaitSet with callback " << cb;
|
||||
|
||||
WaitSetData wsd(cond, cb);
|
||||
|
||||
std::lock_guard<std::mutex> l(m_accessMutex);
|
||||
auto it = m_waitSetData.find(wsd.condition->native_condition());
|
||||
if (it == m_waitSetData.end())
|
||||
{
|
||||
ERROR_LOG(m_logger) << "Could not find WaitSetData - cannot enable waitSet for " << cb;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (it->second.needWaitSetAttach)
|
||||
m_waitSet.attach_condition(wsd.condition);
|
||||
it->second.enabled = true;
|
||||
it->second.needWaitSetAttach = false;
|
||||
return true;
|
||||
wsd.enabled = true;
|
||||
wsd.needWaitSetAttach = false;
|
||||
|
||||
m_waitSetData.insert(std::make_pair(wsd.condition->native_condition(), wsd));
|
||||
return true;
|
||||
}
|
||||
|
||||
bool WaitSetHandler::disableWaitSet(const dds::core::cond::Condition &cond, CallbackInterface *cb)
|
||||
bool WaitSetHandler::detachWaitSet(const dds::core::cond::Condition& cond, CallbackInterface* cb)
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "DisableWaitSet with callback " << cb;
|
||||
DEBUG_LOG(m_logger) << "DetachWaitSet with callback " << cb;
|
||||
|
||||
WaitSetData wsd(cond, cb);
|
||||
WaitSetData wsd(cond, cb);
|
||||
|
||||
std::lock_guard<std::mutex> l(m_accessMutex);
|
||||
auto it = m_waitSetData.find(wsd.condition->native_condition());
|
||||
if (it == m_waitSetData.end())
|
||||
{
|
||||
ERROR_LOG(m_logger) << "Could not find WaitSetData - cannot disable waitSet for " << cb;
|
||||
return false;
|
||||
}
|
||||
std::lock_guard<std::mutex> l(m_accessMutex);
|
||||
|
||||
m_waitSet.detach_condition(wsd.condition);
|
||||
it->second.enabled = false;
|
||||
it->second.needWaitSetAttach = true;
|
||||
return true;
|
||||
auto it = m_waitSetData.find(wsd.condition->native_condition());
|
||||
if (it == m_waitSetData.end())
|
||||
{
|
||||
WARNING_LOG(m_logger) << "Waitset for " << cb << " not registered";
|
||||
return false;
|
||||
}
|
||||
|
||||
m_waitSet.detach_condition(wsd.condition);
|
||||
m_waitSetData.erase(it);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool WaitSetHandler::enableWaitSet(const dds::core::cond::Condition& cond, CallbackInterface* cb)
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "EnableWaitSet with callback " << cb;
|
||||
|
||||
WaitSetData wsd(cond, cb);
|
||||
|
||||
std::lock_guard<std::mutex> l(m_accessMutex);
|
||||
auto it = m_waitSetData.find(wsd.condition->native_condition());
|
||||
if (it == m_waitSetData.end())
|
||||
{
|
||||
ERROR_LOG(m_logger) << "Could not find WaitSetData - cannot enable waitSet for " << cb;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (it->second.needWaitSetAttach)
|
||||
m_waitSet.attach_condition(wsd.condition);
|
||||
it->second.enabled = true;
|
||||
it->second.needWaitSetAttach = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool WaitSetHandler::disableWaitSet(const dds::core::cond::Condition& cond, CallbackInterface* cb)
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "DisableWaitSet with callback " << cb;
|
||||
|
||||
WaitSetData wsd(cond, cb);
|
||||
|
||||
std::lock_guard<std::mutex> l(m_accessMutex);
|
||||
auto it = m_waitSetData.find(wsd.condition->native_condition());
|
||||
if (it == m_waitSetData.end())
|
||||
{
|
||||
ERROR_LOG(m_logger) << "Could not find WaitSetData - cannot disable waitSet for " << cb;
|
||||
return false;
|
||||
}
|
||||
|
||||
m_waitSet.detach_condition(wsd.condition);
|
||||
it->second.enabled = false;
|
||||
it->second.needWaitSetAttach = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
int WaitSetHandler::attachedConditionCount() const
|
||||
{
|
||||
return m_waitSet.conditions().size();
|
||||
return m_waitSet.conditions().size();
|
||||
}
|
||||
|
||||
|
||||
void WaitSetHandler::waitSetTask()
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
dds::core::cond::WaitSet::ConditionSeq activeConditionsSeq = m_waitSet.wait();
|
||||
for (const dds::core::cond::Condition &cond : activeConditionsSeq)
|
||||
while (true)
|
||||
{
|
||||
if (cond == m_abortCondition)
|
||||
{
|
||||
m_waitSet->detach_all();
|
||||
return;
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> l(m_accessMutex);
|
||||
const DDS_Condition *ddsCond = cond->native_condition();
|
||||
auto it = m_waitSetData.lower_bound(ddsCond);
|
||||
while (it != m_waitSetData.end())
|
||||
{
|
||||
WaitSetData &wsd = it->second;
|
||||
if (wsd.enabled)
|
||||
dds::core::cond::WaitSet::ConditionSeq activeConditionsSeq = m_waitSet.wait();
|
||||
for (const dds::core::cond::Condition& cond : activeConditionsSeq)
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "Condition triggered for " << wsd.callback;
|
||||
wsd.callback->dataAvailable();
|
||||
if (cond == m_abortCondition)
|
||||
{
|
||||
m_waitSet->detach_all();
|
||||
return;
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> l(m_accessMutex);
|
||||
const DDS_Condition* ddsCond = cond->native_condition();
|
||||
auto it = m_waitSetData.lower_bound(ddsCond);
|
||||
while (it != m_waitSetData.end())
|
||||
{
|
||||
WaitSetData& wsd = it->second;
|
||||
if (wsd.enabled)
|
||||
{
|
||||
DEBUG_LOG(m_logger) << "Condition triggered for " << wsd.callback;
|
||||
wsd.callback->dataAvailable();
|
||||
}
|
||||
++it;
|
||||
if (it == m_waitSetData.end() || it->first != ddsCond)
|
||||
break;
|
||||
}
|
||||
}
|
||||
++it;
|
||||
if (it == m_waitSetData.end() || it->first != ddsCond)
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
} // namespace DDSGeneric
|
||||
Executable → Regular
+37
-40
@@ -1,15 +1,13 @@
|
||||
|
||||
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <dds/core/cond/Condition.hpp>
|
||||
#include <dds/core/cond/GuardCondition.hpp>
|
||||
#include <dds/core/cond/WaitSet.hpp>
|
||||
|
||||
#include <ddsgen_export.h>
|
||||
#include <future>
|
||||
#include <map>
|
||||
#include <ddsgen_export.h>
|
||||
|
||||
namespace NGL
|
||||
{
|
||||
@@ -20,48 +18,47 @@ namespace DDSGeneric
|
||||
{
|
||||
class DDSGEN_EXPORT WaitSetHandler final
|
||||
{
|
||||
public:
|
||||
class CallbackInterface
|
||||
{
|
||||
public:
|
||||
virtual ~CallbackInterface() = default;
|
||||
virtual void dataAvailable() = 0;
|
||||
};
|
||||
class CallbackInterface
|
||||
{
|
||||
public:
|
||||
virtual ~CallbackInterface() = default;
|
||||
virtual void dataAvailable() = 0;
|
||||
};
|
||||
|
||||
WaitSetHandler();
|
||||
~WaitSetHandler();
|
||||
WaitSetHandler();
|
||||
~WaitSetHandler();
|
||||
|
||||
bool attachWaitSet(const dds::core::cond::Condition &cond, CallbackInterface *cb);
|
||||
bool detachWaitSet(const dds::core::cond::Condition &cond, CallbackInterface *cb);
|
||||
bool enableWaitSet(const dds::core::cond::Condition &cond, CallbackInterface *cb);
|
||||
bool disableWaitSet(const dds::core::cond::Condition &cond, CallbackInterface *cb);
|
||||
bool attachWaitSet(const dds::core::cond::Condition& cond, CallbackInterface* cb);
|
||||
bool detachWaitSet(const dds::core::cond::Condition& cond, CallbackInterface* cb);
|
||||
bool enableWaitSet(const dds::core::cond::Condition& cond, CallbackInterface* cb);
|
||||
bool disableWaitSet(const dds::core::cond::Condition& cond, CallbackInterface* cb);
|
||||
|
||||
int attachedConditionCount() const;
|
||||
int attachedConditionCount() const;
|
||||
|
||||
private:
|
||||
NGL::INGLLogger &m_logger;
|
||||
struct WaitSetData final
|
||||
{
|
||||
dds::core::cond::Condition condition;
|
||||
CallbackInterface *callback = nullptr;
|
||||
bool enabled = true;
|
||||
bool needWaitSetAttach = false;
|
||||
private:
|
||||
NGL::INGLLogger& m_logger;
|
||||
|
||||
WaitSetData(const dds::core::cond::Condition &cond, CallbackInterface *cb)
|
||||
: condition(cond)
|
||||
, callback(cb)
|
||||
{}
|
||||
};
|
||||
private:
|
||||
NGL::INGLLogger &m_logger;
|
||||
void waitSetTask();
|
||||
private:
|
||||
struct WaitSetData final
|
||||
{
|
||||
dds::core::cond::Condition condition;
|
||||
CallbackInterface* callback = nullptr;
|
||||
bool enabled = true;
|
||||
bool needWaitSetAttach = false;
|
||||
|
||||
NGL::INGLLogger &m_logger;
|
||||
dds::core::cond::WaitSet m_waitSet;
|
||||
dds::core::cond::GuardCondition m_abortCondition;
|
||||
std::future<void> m_task;
|
||||
std::mutex m_accessMutex;
|
||||
std::map<const DDS_Condition *, WaitSetData> m_waitSetData;
|
||||
WaitSetData(const dds::core::cond::Condition& cond, CallbackInterface* cb) : condition(cond), callback(cb) {}
|
||||
};
|
||||
|
||||
private:
|
||||
NGL::INGLLogger& m_logger;
|
||||
void waitSetTask();
|
||||
|
||||
private:
|
||||
NGL::INGLLogger& m_logger;
|
||||
dds::core::cond::WaitSet m_waitSet;
|
||||
dds::core::cond::GuardCondition m_abortCondition;
|
||||
std::future<void> m_task;
|
||||
std::mutex m_accessMutex;
|
||||
std::map<const DDS_Condition*, WaitSetData> m_waitSetData;
|
||||
};
|
||||
}
|
||||
} // namespace DDSGeneric
|
||||
Reference in new issue
Block a user