qt6-migration-pipeline/Lts/DDS/FTS/connext.h

#pragma once

#include <dds/core/cond/GuardCondition.hpp>
#include <dds/core/cond/StatusCondition.hpp>
#include <dds/core/cond/WaitSet.hpp>
#include <dds/domain/DomainParticipant.hpp>
#include <dds/pub/DataWriter.hpp>
#include <dds/pub/Publisher.hpp>
#include <dds/sub/DataReader.hpp>
#include <dds/sub/SampleInfo.hpp>
#include <dds/sub/Subscriber.hpp>
#include <dds/topic/ContentFilteredTopic.hpp>
#include <dds/topic/find.hpp>
#include <rti/pub/WriteParams.hpp>


#include <NGLLogger/NGLLoggerBootstrap.h>

namespace connext
{

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


template<typename T>
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);
}


template<typename T>
class SampleBase
{
protected:
  SampleBase() = default;
  virtual ~SampleBase() = default;

public:
  void set_data(const T &data)
  {
    m_data = data;
  }
  T &data()
  {
    return m_data;
  }
  const T &data() const
  {
    return m_data;
  }
protected:
  T m_data;
};

template<typename T>
class WriteSample : public SampleBase<T>
{
public:
  WriteSample() = default;

  void set_info(const rti::pub::WriteParams &info)
  {
    m_info = info;
  }
  rti::pub::WriteParams &info()
  {
    return m_info;
  }
  const rti::pub::WriteParams &info() const
  {
    return m_info;
  }

private:
  rti::pub::WriteParams m_info;
};

template<typename T>
class WriteSampleRef
{
public:
  WriteSampleRef()
    : m_data_ptr(nullptr)
    , m_info_ptr(nullptr)
  {}

  void set_data(T *data_ptr)
  {
    m_data_ptr = data_ptr;
  }

  T &data()
  {
    return *m_data_ptr;
  }

  void set_info(rti::pub::WriteParams *info_ptr)
  {
    m_info_ptr = info_ptr;
  }

  rti::pub::WriteParams &info()
  {
    return *m_info_ptr;
  }

private:
  T *m_data_ptr;
  rti::pub::WriteParams *m_info_ptr;
};

template<typename T>
class Sample : public SampleBase<T>
{
public:
  Sample() = default;

  void set_info(const dds::sub::SampleInfo &info)
  {
    m_info = info;
  }
  dds::sub::SampleInfo &info()
  {
    return m_info;
  }
  const dds::sub::SampleInfo &info() const
  {
    return m_info;
  }

  void set_identity(const rti::core::SampleIdentity &identity)
  {
    m_identity = identity;
  }

  const rti::core::SampleIdentity &identity() const
  {
    return m_identity;
  }

private:
  dds::sub::SampleInfo m_info;
  rti::core::SampleIdentity m_identity;
};

class RequesterParams
{
public:
  explicit RequesterParams(const dds::domain::DomainParticipant &participant)
    : m_participant(participant)
    , m_publisher(nullptr)
    , m_subscriber(nullptr)
  {}
  virtual ~RequesterParams() = default;

  const std::string &request_topic_name() const
  {
    return m_request_topic_name;
  }
  void request_topic_name(const std::string &request_topic_name)
  {
    m_request_topic_name = request_topic_name;
  }

  const std::string &reply_topic_name() const
  {
    return m_reply_topic_name;
  }
  void reply_topic_name(const std::string &reply_topic_name)
  {
    m_reply_topic_name = reply_topic_name;
  }

  const dds::domain::DomainParticipant &domainParticipant() const
  {
    return m_participant;
  }

  const dds::pub::Publisher &publisher() const
  {
    return m_publisher;
  }
  void publisher(const dds::pub::Publisher &publisher)
  {
    m_publisher = publisher;
  }

  const dds::sub::Subscriber &subscriber() const
  {
    return m_subscriber;
  }
  void subscriber(const dds::sub::Subscriber &subscriber)
  {
    m_subscriber = subscriber;
  }

  const dds::pub::qos::DataWriterQos &datawriter_qos() const
  {
    return m_datawriter_qos;
  }
  void datawriter_qos(const dds::pub::qos::DataWriterQos &datawriter_qos)
  {
    m_datawriter_qos = datawriter_qos;
  }

  const dds::sub::qos::DataReaderQos &datareader_qos() const
  {
    return m_datareader_qos;
  }
  void datareader_qos(const dds::sub::qos::DataReaderQos &datareader_qos)
  {
    m_datareader_qos = datareader_qos;
  }

protected:
  dds::domain::DomainParticipant m_participant;
  dds::pub::Publisher m_publisher;
  dds::sub::Subscriber m_subscriber;
  dds::pub::qos::DataWriterQos m_datawriter_qos;
  dds::sub::qos::DataReaderQos m_datareader_qos;
  std::string m_request_topic_name;
  std::string m_reply_topic_name;
};

template <typename REQUEST, typename REPLY>
class ReplierParams : public RequesterParams
{
public:
  using RequesterParams::RequesterParams;
};

template<typename REQUEST, typename REPLY>
class ReplyRequestBase
{
protected:
  explicit ReplyRequestBase(const connext::RequesterParams &requesterParams)
    : m_requestTopic(getTopic<REQUEST>(requesterParams.domainParticipant(),
                                       requesterParams.request_topic_name()))
    , m_replyTopic(getTopic<REPLY>(requesterParams.domainParticipant(),
                                   requesterParams.reply_topic_name()))
  {}
  virtual ~ReplyRequestBase() = default;

protected:
  dds::topic::Topic<REQUEST> m_requestTopic;
  dds::topic::Topic<REPLY> m_replyTopic;
};

template<typename REQUEST, typename REPLY>
class Requester final : public ReplyRequestBase<REQUEST, REPLY>
{
public:
  explicit Requester(const connext::RequesterParams &requesterParams)
    : ReplyRequestBase<REQUEST, REPLY>(requesterParams)
    , m_requesterParams(requesterParams)
    , m_dataWriter(nullptr)
    , m_dataReader(nullptr)
    , m_contentFilteredTopic(nullptr)
    , m_curInstanceHandle(dds::core::InstanceHandle::nil())
    , m_logger(NGL::NGLLoggerBootstrap::getNGLRootLogger().getNGLLogger("connext.requester"))
  {}

  void create_contentfilter(bool bNewDataWriter)
  {
    
    if (!bNewDataWriter && m_contentFilteredTopic != nullptr)
      return;
    
    rti::core::policy::DataWriterProtocol dwp;
    m_dataWriter.qos() >> dwp;
    const rti::core::Guid dwGuid = dwp.virtual_guid();
    const QByteArray writerGuid = DebugInfo::toReadable(dwGuid);
    const QByteArray filterContent = "@related_sample_identity.writer_guid.value = &hex(" + writerGuid + ")";
    if (m_contentFilteredTopic == nullptr)
    {
      
      const std::string rtName = ReplyRequestBase<REQUEST, REPLY>::m_replyTopic.name();
      const QByteArray filterName(QByteArray::fromRawData(rtName.c_str(), static_cast<int>(rtName.size())) +
                                  QByteArray(" ") + writerGuid);
      m_contentFilteredTopic = dds::topic::ContentFilteredTopic<REPLY>(ReplyRequestBase<REQUEST, REPLY>::m_replyTopic,
                                                                       filterName.data(),
                                                                       dds::topic::Filter(filterContent.data()));
    }
    else
    {
      
      m_contentFilteredTopic->filter(dds::topic::Filter(filterContent.data()));
    }
    DEBUG_LOG(m_logger) << "content filter for topic "
                        << ReplyRequestBase<REQUEST, REPLY>::m_requestTopic.name()
                        << " - related_sample_identity should be: "
                        << writerGuid;
  }

  void send_request(connext::WriteSample<REQUEST> &request)
  {
    bool bNewDataWriter = false;
    if (m_dataWriter == nullptr)
    {
      m_dataWriter = dds::pub::DataWriter<REQUEST>(m_requesterParams.publisher(),
                                                   ReplyRequestBase<REQUEST, REPLY>::m_requestTopic,
                                                   m_requesterParams.datawriter_qos());
      bNewDataWriter = true;
    }
    else
    {
      
    }

    create_contentfilter(bNewDataWriter);
    
    if (m_dataReader == nullptr)
    {
      
      m_dataReader = dds::sub::DataReader<REPLY>(m_requesterParams.subscriber(),
                                                 m_contentFilteredTopic,
                                                 m_requesterParams.datareader_qos());
    }

    m_curInstanceHandle = m_dataWriter.register_instance(request.data());
    request.info().replace_automatic_values(true);
    request.info().identity(rti::core::SampleIdentity::automatic());
    m_dataWriter->write(request.data(), request.info());

    const rti::core::SampleIdentity currentSampleIdentity = request.info().identity();
    DEBUG_LOG(m_logger) << "send_request for topic "
                        << ReplyRequestBase<REQUEST, REPLY>::m_requestTopic.name()
                        << ", m_currentSampleIdentity: "
                        << DebugInfo::toReadable(currentSampleIdentity);
  }

  bool receive_reply(connext::Sample<REPLY> &reply,
                     const dds::core::Duration &duration)
  {
    auto condition(dds::sub::cond::ReadCondition(m_dataReader, dds::sub::status::SampleState::not_read()));

    dds::core::cond::WaitSet waitset;
    waitset.attach_condition(condition);

    bool bRet = false;
    while (true)
    {
      
      if (m_dataReader->take(reply.data(), reply.info()) && reply.info().valid())
      {
        DEBUG_LOG(m_logger) << "receive_reply for topic "
                            << ReplyRequestBase<REQUEST, REPLY>::m_requestTopic.name()
                            << ", SampleIdentity - original publication virtual: "
                            << DebugInfo::toReadable(reply.info()->original_publication_virtual_sample_identity());

        DEBUG_LOG(m_logger) << "receive_reply for topic "
                            << ReplyRequestBase<REQUEST, REPLY>::m_requestTopic.name()
                            << ", SampleIdentity - related original: "
                            << DebugInfo::toReadable(reply.info()->related_original_publication_virtual_sample_identity());

        
        bRet = true;
        break;
      }

      dds::core::cond::WaitSet::ConditionSeq activeConditionsSeq = waitset.wait(duration);
      if (activeConditionsSeq.empty())
      {
        ERROR_LOG(m_logger) << "Bailing out!";
        break;
      }
    }
    
    static const rti::core::SampleFlag irs(rti::core::SampleFlag::intermediate_reply_sequence());
    if ((reply.info()->flag() & irs) == 0)
    {
      if (!m_curInstanceHandle.is_nil())
      {
        
        DEBUG_LOG(m_logger) << "Keyed topic - disposing instance: "
                            << ReplyRequestBase<REQUEST, REPLY>::m_requestTopic.name();
        m_dataWriter.dispose_instance(m_curInstanceHandle);
      }
    }

    return bRet;
  }

  dds::pub::DataWriter<REQUEST> get_request_datawriter() const { return m_dataWriter; }
  dds::sub::DataReader<REPLY>   get_request_datareader() const { return m_dataReader; }

private:
  connext::RequesterParams      m_requesterParams;
  dds::pub::DataWriter<REQUEST> m_dataWriter;
  dds::sub::DataReader<REPLY>   m_dataReader;
  dds::topic::ContentFilteredTopic<REPLY> m_contentFilteredTopic;
  dds::core::InstanceHandle     m_curInstanceHandle;
  NGL::INGLLogger              &m_logger;
};

template<typename REQUEST, typename REPLY>
class Replier final : public ReplyRequestBase<REQUEST, REPLY>
{
public:
  explicit Replier(const connext::ReplierParams<REQUEST, REPLY> &replierParams)
    : ReplyRequestBase<REQUEST, REPLY>(replierParams)
    , m_replierParams(replierParams)
    , m_dataWriter(dds::pub::DataWriter<REPLY>(replierParams.publisher(),
                                               ReplyRequestBase<REQUEST, REPLY>::m_replyTopic,
                                               replierParams.datawriter_qos()))
    , m_dataReader(dds::sub::DataReader<REQUEST>(replierParams.subscriber(),
                                                 ReplyRequestBase<REQUEST, REPLY>::m_requestTopic,
                                                 replierParams.datareader_qos()))
    , m_logger(NGL::NGLLoggerBootstrap::getNGLRootLogger().getNGLLogger("connext.replier"))
  {}

  void send_reply(connext::WriteSample<REPLY> &reply,
                  const rti::core::SampleIdentity &identity,
                  bool bLastSample = true)
  {
    connext::WriteSampleRef<REPLY> replyRef;
    replyRef.set_data(&reply.data());
    replyRef.set_info(&reply.info());
    return send_reply(replyRef, identity, bLastSample);
  }

  void send_reply(connext::WriteSampleRef<REPLY> &reply,
                  const rti::core::SampleIdentity &identity,
                  bool bLastSample = true)
  {
    DEBUG_LOG(m_logger) << "send_reply for topic "
                        << ReplyRequestBase<REQUEST, REPLY>::m_requestTopic.name()
                        << ", sampleIdentity: " << DebugInfo::toReadable(identity);

    rti::core::SampleFlag sf = reply.info().flag();
    if (bLastSample)
      sf &= ~rti::core::SampleFlag::intermediate_reply_sequence();
    else
      sf |= rti::core::SampleFlag::intermediate_reply_sequence();
    reply.info().flag(sf);
    reply.info().related_sample_identity(identity);
    DEBUG_LOG(m_logger) << "send_reply for topic "
                        << ReplyRequestBase<REQUEST, REPLY>::m_requestTopic.name()
                        << ", related sampleIdentity: " << DebugInfo::toReadable(reply.info().related_sample_identity());

    m_dataWriter->write(reply.data(), reply.info());
  }

  bool receive_request(connext::Sample<REQUEST> &request)
  {
    if (!m_dataReader->take(request.data(), request.info()))
      return false;

    if (request.info().valid())
    {
      DEBUG_LOG(m_logger) << "receive_request for topic "
                          << ReplyRequestBase<REQUEST, REPLY>::m_requestTopic.name()
                          << ", sampleIdentity publication_virtual_sample_identity: "
                          << DebugInfo::toReadable(request.info()->publication_virtual_sample_identity())
                          << ", original_publication_virtual_sample_identity: "
                          << DebugInfo::toReadable(request.info()->original_publication_virtual_sample_identity())
                          << ", related_original_publication_virtual_sample_identity: "
                          << DebugInfo::toReadable(request.info()->related_original_publication_virtual_sample_identity())
                          << " with timestamp_ " << request.info().source_timestamp().to_millisecs();
      request.set_identity(request.info()->original_publication_virtual_sample_identity());
    }

    return request.info().valid();
  }

  dds::pub::DataWriter<REPLY>   get_request_datawriter() const { return m_dataWriter; }
  dds::sub::DataReader<REQUEST> get_request_datareader() const { return m_dataReader; }

private:
  connext::ReplierParams<REQUEST, REPLY> m_replierParams;
  dds::pub::DataWriter<REPLY>           m_dataWriter;
  dds::sub::DataReader<REQUEST>         m_dataReader;
  NGL::INGLLogger                      &m_logger;
};

}