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

#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 <functional>

namespace DDSGeneric
{

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 &)>;

  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)
    {
      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 (m_dataReader == 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;
    }

    
    
    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);
  }

  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:
#if RTI_DDS_VERSION_MAJOR < 6
  inline bool checkNewSamples(dds::sub::LoanedSamples<TopicTypeRead> &samples)
#else
  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;
  }

  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<TopicTypeRead> 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 "
                        << DDS::topic_type_name<TopicType>::value();

    for (const auto &sample : samples)
    {
      if (sample.info().valid())
        readData(sample.data(), sample.info(), callbackFunc);
    }

    return true;
  }

  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;
    

    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;
  NGL::INGLLogger &m_logger;
};

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::BaseClass;

  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
    {
      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;
  }

  dds::topic::Filter &filter() { 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);
  }

private:
  dds::topic::ContentFilteredTopic<TopicType> m_filteredTopic = dds::topic::ContentFilteredTopic<TopicType>(nullptr);
  dds::topic::Filter m_filter = dds::topic::Filter(std::string());
};

}