qt6-migration-pipeline/Lts/DDS/Routing/routing_plugin.h

#include <dds/pub/DataWriter.hpp>
#include <dds/pub/discovery.hpp>
#include <dds/sub/DataReader.hpp>
#include <dds/sub/DataReaderListener.hpp>
#include <dds/topic/find.hpp>
#include <dds/topic/Topic.hpp>

#include <DDS/Generic/generic_base.h>
#include <DDS/QosProfile/DDS_DefaultQos.h>
#include <NGLLogger/INGLLogger.h>





namespace DebugInfo
{
static inline QByteArray toReadable(const rti::core::Guid &guid)
{
  return QByteArray(reinterpret_cast<const char *>(&guid[0]), 16).toHex();
}
static inline QByteArray toReadable(const rti::core::SequenceNumber &seqNo,
                                    const rti::core::Guid &guid)
{
  return "SeqNo: " + QByteArray::number(seqNo.value()) + ", Guid-DW: " + toReadable(guid);
}
static inline QByteArray toReadable(const rti::core::SampleIdentity &identiy)
{
  return toReadable(identiy.sequence_number(), identiy.writer_guid());
}
}

namespace FTS
{
template<typename GENERIC_FTS>
struct TopicNames;
}
namespace FTC
{
template<typename GENERIC_FTC>
struct TopicNames;
}

namespace RoutingService
{


template<typename T>
static dds::topic::Topic<T> getTopic(const dds::domain::DomainParticipant &participant,
                                     const std::string &topicName)
{
  using DataTopic = dds::topic::Topic<T>;
  auto ret = dds::topic::find<DataTopic>(participant, topicName);
  if (ret != nullptr)
    return ret;
  return DataTopic(participant, topicName);
}




class SampleWriterInterface
{
public:
  SampleWriterInterface() = default;
  virtual ~SampleWriterInterface() = default;

  virtual void writeSamples() = 0;

  virtual NGL::INGLLogger &logger() = 0;
};

class DataWriterTaskInterface
{
public:
  DataWriterTaskInterface() = default;
  virtual ~DataWriterTaskInterface() = default;

  virtual void enqueue(SampleWriterInterface *swi) = 0;
};

class TopicDispatcherInterface
{
public:
  TopicDispatcherInterface() = default;
  virtual ~TopicDispatcherInterface() = default;

  virtual bool init(DataWriterTaskInterface *dwt,
                    const dds::domain::DomainParticipant &dpIn,
                    const dds::sub::Subscriber &subIn,
                    const dds::domain::DomainParticipant &dpOut,
                    const dds::pub::Publisher &pubOut) = 0;
  virtual void shutdown() = 0;
};




template<typename TOPIC>
class SampleWriter final : public SampleWriterInterface
{
public:
  SampleWriter(dds::sub::LoanedSamples<TOPIC> &&samples,
               const dds::pub::DataWriter<TOPIC> &dataWriter,
               bool bWithOriginalIdentity,
               NGL::INGLLogger &logger)
    : m_loanedSamples(std::move(samples))
    , m_dataWriter(dataWriter)
    , m_bWithOriginalIdentity(bWithOriginalIdentity)
    , m_logger(logger)
  {}

  rti::core::SampleIdentity getIdentity(const rti::core::SampleIdentity &identity) const
  {
    
    static const auto s_unknownSequenceNumber(rti::core::SequenceNumber::unknown());
    static const auto s_unknownGui(rti::core::Guid::unknown());
    static const auto s_unknownSampleIdentity(rti::core::SampleIdentity::unknown());

    if (!m_bWithOriginalIdentity)
      return s_unknownSampleIdentity;

    if (identity.sequence_number() == s_unknownSequenceNumber)
      return s_unknownSampleIdentity;
    if (identity.writer_guid() == s_unknownGui)
      return s_unknownSampleIdentity;
    return identity;
  }

  void writeSamples() override
  {
    TRACE_LOG(m_logger) << "Writing samples for " << DDS::topic_type_name<TOPIC>::value()
                      << ", sample count: " << m_loanedSamples.length();
    rti::pub::WriteParams writeParams;
    for (const auto &sample : m_loanedSamples)
    {
      try
      {
        writeParams.identity(getIdentity(sample.info()->original_publication_virtual_sample_identity()));
        writeParams.related_sample_identity(getIdentity(sample.info()->related_original_publication_virtual_sample_identity()));

        
        writeParams.handle(sample.info().instance_handle());
        writeParams.flag(sample.info()->flag());

        

        TRACE_LOG(m_logger) << "identity: " << DebugInfo::toReadable(writeParams.identity())
                            << ", related_sample_identity: " << DebugInfo::toReadable(writeParams.related_sample_identity());

        const auto &state = sample.info().state();  
        const dds::sub::status::InstanceState &instanceState = state.instance_state();
        if (instanceState == dds::sub::status::InstanceState::alive())
        {
          m_dataWriter->write(sample.data(), writeParams);
        }
        else if (instanceState == dds::sub::status::InstanceState::not_alive_disposed())
        {
          TRACE_LOG(m_logger) << " - is disposed";
          m_dataWriter->dispose_instance(writeParams);
        }
        else if (instanceState == dds::sub::status::InstanceState::not_alive_no_writers())
        {
          TRACE_LOG(m_logger) << " - is unregistered";
          m_dataWriter->unregister_instance(writeParams);
        }
        else
          WARNING_LOG(m_logger) << "Unknown instance state: " << instanceState;
      }
      catch (const dds::core::Exception &ex)
      {
        ERROR_LOG(m_logger) << "writeSamples exception: " << ex.what();
      }
    }
    m_dataWriter->flush();
  }
  NGL::INGLLogger &logger() override { return m_logger; }
private:
  dds::sub::LoanedSamples<TOPIC> m_loanedSamples;
  dds::pub::DataWriter<TOPIC> m_dataWriter;
  bool m_bWithOriginalIdentity;
  NGL::INGLLogger &m_logger;
};

template<typename T>
class DataTopicListener final : public dds::sub::DataReaderListener<T>
{
public:
  using DataReader = dds::sub::DataReader<T>;

  DataTopicListener(DataWriterTaskInterface *dwt,
                    const std::string &topicName,
                    DDS::QosProfile::TopicType topicType,
                    NGL::INGLLogger &logger)
    : m_dwt(dwt)
    , m_dataWriter(nullptr)
    , m_topicName(topicName)
    , m_topicType(topicType)
    , m_logger(logger)
  {}

  bool init(const dds::domain::DomainParticipant &dpOut,
            const dds::pub::Publisher &pubOut)
  {
    if (!m_dwt)
      return false;
    DEBUG_LOG(m_logger) << "Creating DataWriter for domain " << dpOut.domain_id();
    m_dataWriter = dds::pub::DataWriter<T>(pubOut,
                                           getTopic<T>(dpOut, m_topicName),
                                           DDS::QosProfile::dataWriterDefault(m_topicType));

    
    dds::domain::DomainParticipant &dpOutNonConst = const_cast<dds::domain::DomainParticipant &>(dpOut);
    dds::pub::ignore(dpOutNonConst, m_dataWriter.instance_handle());
    return true;
  }

  void on_data_available(dds::sub::DataReader<T> &reader) override
  {
    

    TRACE_LOG(m_logger) << "Data available for " << DDS::topic_type_name<T>::value();
    auto ws = new SampleWriter<T>(reader.take(), m_dataWriter, m_topicType == DDS::QosProfile::TopicType::FunctionTopic, m_logger);
    m_dwt->enqueue(ws);
  }

  void on_requested_deadline_missed(dds::sub::DataReader<T> &, const dds::core::status::RequestedDeadlineMissedStatus &) override {}
  void on_requested_incompatible_qos(dds::sub::DataReader<T> &,
                                     const dds::core::status::RequestedIncompatibleQosStatus &) override {}
  void on_sample_rejected(dds::sub::DataReader<T> &,
                          const dds::core::status::SampleRejectedStatus &) override {}
  void on_liveliness_changed(dds::sub::DataReader<T> &,
                             const dds::core::status::LivelinessChangedStatus &) override {}
  void on_subscription_matched(dds::sub::DataReader<T> &,
                               const dds::core::status::SubscriptionMatchedStatus &) override {}
  void on_sample_lost(dds::sub::DataReader<T> &,
                      const dds::core::status::SampleLostStatus &) override {}

  static dds::core::status::StatusMask mask()
  {
    return (dds::core::status::StatusMask::data_available());
  }

protected:
  DataWriterTaskInterface *m_dwt;
  dds::pub::DataWriter<T> m_dataWriter;
  std::string m_topicName;
  DDS::QosProfile::TopicType m_topicType;

  NGL::INGLLogger &m_logger;
};

template<typename TOPIC>
class TopicDispatcher : public TopicDispatcherInterface
{
protected:
  explicit TopicDispatcher(NGL::INGLLogger &logger, const std::string &topicName, DDS::QosProfile::TopicType topicType)
    : m_dataReader(nullptr)
    , m_topicName(topicName)
    , m_topicType(topicType)
    , m_logger(logger)
  {
  }

  ~TopicDispatcher()
  {
    shutdown();
  }

  bool init(DataWriterTaskInterface *dwt,
            const dds::domain::DomainParticipant &dpIn,
            const dds::sub::Subscriber &subIn,
            const dds::domain::DomainParticipant &dpOut,
            const dds::pub::Publisher &pubOut) override
  {
    DEBUG_LOG(m_logger) << "Initializing TopicDispatcher " << "(" << this << ")";
    if (m_dataReader != nullptr)
      return false;

    m_dataReader = dds::sub::DataReader<TOPIC>(subIn,
                                               getTopic<TOPIC>(dpIn, m_topicName),
                                               DDS::QosProfile::dataReaderDefault(m_topicType));
    DEBUG_LOG(m_logger) << "Creating DataReader for domain " << dpIn.domain_id() << ", ptr: " <<  m_dataReader.delegate().get();

    m_dtl.reset(new DataTopicListener<TOPIC>(dwt, m_topicName, m_topicType, m_logger));
    
    if (!m_dtl->init(dpOut, pubOut))
      return false;
    m_dataReader.listener(m_dtl.get(), DataTopicListener<TOPIC>::mask());
    return true;
  }

  void shutdown() override
  {
    DEBUG_LOG(m_logger) << "Cleaning up TopicDispatcher " << "(" << this << ")";
    if (m_dataReader != nullptr)
    {
      DEBUG_LOG(m_logger) << "shutting down DataReader: " <<  m_dataReader.delegate().get();
      m_dataReader.listener(nullptr, dds::core::status::StatusMask::none());
      try
      {
        m_dataReader.close();
      }
      catch (const dds::core::Exception &ex)
      {
        ERROR_LOG(m_logger) << "topic close ex: " << ex.what();
      }
    }
    
    
    m_dtl = nullptr;
    
    m_dataReader = nullptr;
  }

private:
  dds::sub::DataReader<TOPIC> m_dataReader;
  std::unique_ptr<DataTopicListener<TOPIC>> m_dtl;
  std::string m_topicName;
  DDS::QosProfile::TopicType m_topicType;
  NGL::INGLLogger &m_logger;
};


template<typename TOPIC>
class DataTopicDispatcher final : public TopicDispatcher<TOPIC>
{
public:
  explicit DataTopicDispatcher(NGL::INGLLogger &logger)
    : TopicDispatcher<TOPIC>(logger,
                             DDS::Helper::getShortTopicName(dds::topic::topic_type_name<TOPIC>::value()),
                              DDS::QosProfile::TopicType::DataTopic)
  {}
};


template<typename FTS_TOPIC>
class FunctionTopicReplyDispatcher final : public TopicDispatcher<typename FTS_TOPIC::ReplyTopic>
{
public:
  explicit FunctionTopicReplyDispatcher(NGL::INGLLogger &logger)
    : TopicDispatcher<typename FTS_TOPIC::ReplyTopic>(logger,
                                                      FTS::TopicNames<FTS_TOPIC>::replyTopicName(),
                                                      DDS::QosProfile::TopicType::FunctionTopic)
  {}
};


template<typename FTC_TOPIC>
class FunctionTopicRequestDispatcher final : public TopicDispatcher<typename FTC_TOPIC::RequestTopic>
{
public:
  explicit FunctionTopicRequestDispatcher(NGL::INGLLogger &logger)
    : TopicDispatcher<typename FTC_TOPIC::RequestTopic>(logger,
                                                        FTC::TopicNames<FTC_TOPIC>::requestTopicName(),
                                                        DDS::QosProfile::TopicType::FunctionTopic)
  {}
};

}