qt6-migration-pipeline/Lts/DDS/Generic/dynamic_data_reader.h

#pragma once

#include <dds/core/xtypes/StructType.hpp>
#include <dds/sub/DataReader.hpp>
#include <dds/sub/LoanedSamples.hpp>
#include <dds/sub/SampleInfo.hpp>
#include <dds/topic/find.hpp>

#include <DDS/Generic/generic_base.h>
#include <DDS/Generic/generic_logger.h>
#include <LtsUtilsCore/data_modification_enum.h>
#include <functional>

namespace DDSGeneric
{
using StructType  = dds::core::xtypes::StructType;
using DynamicData = dds::core::xtypes::DynamicData;

class DynamicDataReader final
{
public:

  using TopicType  = DynamicData;     
  using DataReader = dds::sub::DataReader<DynamicData>;
  using DataTopic  = dds::topic::Topic<TopicType>;

  using SubscriberCallback = std::function<void(const DynamicData &, const std::string &, DataModification)>;
  using SubscriberCallbackWithSampleInfo = std::function<void(const DynamicData &, const std::string &, const dds::sub::SampleInfo &)>;

  DynamicDataReader()
    : m_dataReader(nullptr)
    , m_structType(nullptr)
    , m_ddsTopic(nullptr)
    , m_bHaveSubscriberCallback(false)
    , m_logger(DDS::getLogger("DynamicDataReader", DDS::DataType::DYNAMIC_DATA_READER))
  {}

  ~DynamicDataReader()
  {
    m_ddsTopic = nullptr;
    m_dataReader = nullptr;
    delete m_structType;
  }

  const DataReader &getDataReader() const { return m_dataReader; }
  const StructType &getStructType() const { return *m_structType; }

  bool create(const dds::domain::DomainParticipant &domainParticipant,
              const dds::sub::Subscriber &ddsSubscriber,
              const std::string &specialTopicName,
              const StructType &structType)
  {
    DEBUG_LOG(m_logger) << "IN DynamicDataReader::create ("
                        << m_structType->name()
                        << ")";

    if (domainParticipant == nullptr)
    {
      ERROR_LOG(m_logger) << "DomainParticipant does not exist ";
      return false;
    }

    if (m_ddsTopic != nullptr)
    {
      ERROR_LOG(m_logger) << "Type '"
                          << m_structType->name()
                          << "' already created";
      return false;
    }

    if (structType.has_parent())
      m_structType = new dds::core::xtypes::StructType(structType.name(), structType.parent(), structType.members());
    else
      m_structType = new dds::core::xtypes::StructType(structType.name(), structType.members());

    try
    {
      
      
      const std::string topicName = specialTopicName.empty() ?
                                    DDS::Helper::getShortTopicName(m_structType->name()) :
                                    specialTopicName;

      DEBUG_LOG(m_logger) << "Creating topic " << topicName << " with type: "
                          << m_structType->name();

      m_ddsTopic = dds::topic::find<dds::topic::Topic<DynamicData>>(domainParticipant, topicName);
      if (m_ddsTopic == nullptr)
      {
        rti::core::xtypes::DynamicDataTypeSerializationProperty serialization_property;
        serialization_property.min_size_serialized(0);
        dds::domain::DomainParticipant dp = domainParticipant;
        rti::domain::register_type(dp, structType.name(), structType, serialization_property);
        m_ddsTopic = dds::topic::Topic<DynamicData>(domainParticipant, topicName, structType);
      }

      DEBUG_LOG(m_logger) << "m_ddsTopic created: " << m_ddsTopic.name();

      if (m_logger.isDebugEnabled())
      {
        dds::sub::qos::SubscriberQos subscriberQoS = ddsSubscriber.qos();
        dds::core::policy::Partition partition = subscriberQoS.policy<dds::core::policy::Partition>();
        dds::core::StringSeq parititionNames = partition.name();
        DEBUG_LOG(m_logger) << "Subscribing partitions: " << parititionNames.size();
        for (const std::string &name : parititionNames)
          DEBUG_LOG(m_logger) << "SubscriberQoS: partition name = " << name;
      }

      dds::sub::qos::DataReaderQos dataReaderQos = ddsSubscriber.default_datareader_qos();

      QString typeName = QString::fromStdString(m_structType->name());
      if (typeName.endsWith(QLatin1String("Request")) || typeName.endsWith(QLatin1String("Reply")))
      {
        
        dds::core::policy::Durability readerDurability;
        dataReaderQos >> readerDurability;
        readerDurability.kind(dds::core::policy::DurabilityKind_def::VOLATILE);  
        dataReaderQos << readerDurability;
      }

      m_dataReader = DataReader(ddsSubscriber,
                                m_ddsTopic,
                                dataReaderQos,
                                nullptr ,
                                dds::core::status::StatusMask::all());

      if (m_dataReader == nullptr)
      {
        ERROR_LOG(m_logger) << "Creating dataReader for '" << topicName << "' with type: '"
                            << m_structType->name()
                            << "' failed";
        return false;
      }

    }
    catch (const dds::core::Exception &ex)
    {
      ERROR_LOG(m_logger) << ex.what();
      return false;
    }

    DEBUG_LOG(m_logger) << "dataReader created ("
                        << m_structType->name()
                        << ")";

    return true;
  }

  
  bool hasSamples() const
  {
    qint64 sampleSize = m_dataReader->datareader_cache_status().sample_count();
    DEBUG_LOG(m_logger) << "sampleSize = " << sampleSize;
    return sampleSize > 0;
  }

  
  dds::sub::LoanedSamples<DynamicData> readAlive(bool &bSeenNewSamples)
  {
    dds::sub::LoanedSamples<DynamicData> samples =
      m_dataReader.select().state(dds::sub::status::InstanceState::alive()).read();
    TRACE_LOG(m_logger) << "readAlive for " << DDS::topic_type_name<TopicType>::value()
                        << ", count: " << samples.length()
                        << ", this: " << this << ", instance handle " << m_dataReader.instance_handle();

    bSeenNewSamples = checkNewSamples(samples);
    return samples;
  }

  
  dds::sub::LoanedSamples<DynamicData> takeAlive(bool &bSeenNewSamples)
  {
    dds::sub::LoanedSamples<DynamicData> samples =
      m_dataReader.select().state(dds::sub::status::InstanceState::alive()).take();
    TRACE_LOG(m_logger) << "takeAlive for " << DDS::topic_type_name<TopicType>::value()
                        << ", count: " << samples.length()
                        << ", this: " << this << ", instance handle " << m_dataReader.instance_handle();

    bSeenNewSamples = checkNewSamples(samples);
    return samples;
  }

  dds::sub::LoanedSamples<DynamicData> takeDisposed()
  {
    dds::sub::LoanedSamples<DynamicData> samples =
      m_dataReader.select().state(dds::sub::status::InstanceState::not_alive_disposed()).take();
    TRACE_LOG(m_logger) << "takeDisposed for " << DDS::topic_type_name<TopicType>::value()
                        << ", count: " << samples.length()
                        << ", this: " << this << ", instance handle " << m_dataReader.instance_handle();
    return samples;
  }

  template<typename C>
  bool read(C callbackFunc)
  {
    DEBUG_LOG(m_logger) << "IN read ("
                        << DDS::topic_type_name<TopicType>::value()
                        << "), this: " << this << ", instance handle " << m_dataReader.instance_handle();

    if (m_dataReader == nullptr || m_ddsTopic == nullptr)
    {
      ERROR_LOG(m_logger) << "No dataReader initialised ";
      return false;
    }
    if (!isValid(callbackFunc))
    {
      ERROR_LOG(m_logger) << "No subscriber set for "
                          << DDS::topic_type_name<TopicType>::value();
      return false;
    }

    
    
    dds::sub::LoanedSamples<DynamicData> samples = m_dataReader.select().state(dds::sub::status::DataState::new_data()).read();

    DEBUG_LOG(m_logger) << "got " << samples.length() + m_newSamplesDuringRead.size() << " samples from reader for "
                        << DDS::topic_type_name<TopicType>::value();

    for (const auto &sample : samples)
    {
      if (sample.info().valid())
        readData(sample, m_dataReader->topic_name(), callbackFunc);
    }
    
    auto copy = m_newSamplesDuringRead; 
                                        
    m_newSamplesDuringRead.clear();
    for (auto &sample : copy)
    {
      
      readData(rti::sub::LoanedSample<DynamicData>(sample.sample, &sample.info), m_dataReader->topic_name(), callbackFunc);
      delete sample.sample;
    }

    return checkDisposedInstances(callbackFunc);
  }

  void setHaveSubscriberCallback(bool bHave)
  {
    m_bHaveSubscriberCallback = bHave;
  }
  std::string topicName() const
  {
    return m_ddsTopic.name();
  }

  void recreateReader()
  {
    if (m_dataReader == nullptr)
      return;
    m_dataReader = DataReader(m_dataReader.subscriber(), m_ddsTopic);
  }

private:
#if RTI_DDS_VERSION_MAJOR < 6
  inline bool checkNewSamples(dds::sub::LoanedSamples<DynamicData> &samples)
#else
  inline bool checkNewSamples(const dds::sub::LoanedSamples<DynamicData> &samples)
#endif
  {
    
    if (!m_bHaveSubscriberCallback)
      return false;

    bool bSeenNewSamples = false;
    for (const auto &sample : samples)
    {
      const dds::sub::status::DataState ds = sample.info()->state();
      if (ds.view_state() == dds::sub::status::ViewState::new_view() ||
          ds.sample_state() == dds::sub::status::SampleState::not_read())
      {
        SampleStruct ss;
        ss.sample = new DynamicData(sample.data());
        ss.info = sample.info();
        
        m_newSamplesDuringRead.emplace_back(ss);

        bSeenNewSamples = true;
      }
    }
    return bSeenNewSamples;
  }

  template<typename C>
  bool checkDisposedInstances(C callbackFunc)
  {
    const dds::sub::status::DataState ds(dds::sub::status::SampleState::any(),
                                         dds::sub::status::ViewState::any(),
                                         dds::sub::status::InstanceState::not_alive_mask());

    
    dds::sub::LoanedSamples<DynamicData> samples = m_dataReader.select().state(ds).take();

    if (samples.length() == 0)
      return false;

    DEBUG_LOG(m_logger) << "got " << samples.length() << " disposed instances from reader for "
                        << m_ddsTopic.name();

    for (const auto &sample : samples)
    {
      if (sample.info().valid())
        readData(sample, m_dataReader->topic_name(), callbackFunc);
    }

    return true;
  }

  static void readData(const rti::sub::LoanedSample<DynamicData> &sample, const std::string &topicName, const SubscriberCallback &callbackFunc)
  {
    DataModification modType = DataModification::eModified;
    const dds::sub::status::DataState ds = sample.info()->state();
    if (ds.view_state() == dds::sub::status::ViewState::new_view())
      modType = DataModification::eAdded;
    if (ds.instance_state() != dds::sub::status::InstanceState::alive())
      modType = DataModification::eRemoved;
    

    callbackFunc(sample.data(), topicName, modType);
  }

  static void readData(const rti::sub::LoanedSample<DynamicData> &sample, const std::string &topicName, const SubscriberCallbackWithSampleInfo &callbackFunc)
  {
    callbackFunc(sample.data(), topicName, sample.info());
  }

  static bool isValid(const SubscriberCallback &callbackFunc)
  {
    return callbackFunc != nullptr;
  }
  static bool isValid(const SubscriberCallbackWithSampleInfo &callbackFunc)
  {
    return callbackFunc != nullptr;
  }

private:
  DataReader                m_dataReader;
  StructType               *m_structType;
  DataTopic                 m_ddsTopic;

  struct SampleStruct
  {
    DynamicData *sample = nullptr;
    dds::sub::SampleInfo info;
  };
  std::vector<SampleStruct> m_newSamplesDuringRead;
  bool                      m_bHaveSubscriberCallback;
  NGL::INGLLogger          &m_logger;
};

}