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

#pragma once

#include <DDS/Core/core.hpp>

#include "generic_fts_internal.h"
#include "connext.h"
#include <DDS/Generic/wait_set_handler.h>
#include <DDS/Generic/generic_base.h>
#include <DDS/Generic/generic_logger.h>

#include <queue>
#include <type_traits>
#include <typeinfo>
#include <sstream>

#ifndef tr_lts
#define tr_lts(ctx, t) (t)
#endif

namespace FTS
{
template <class C>
class HasStatusFunction
{
  template <class T>
  static std::true_type testSignature(void (T::*)(const Core::FtReplyState&));

  template <class T>
  static decltype(testSignature(&T::replyState)) test(std::nullptr_t);

  template <class T>
  static std::false_type test(...);

public:
  using type = decltype(test<C>(nullptr));
  static const bool value = type::value;
};

template<typename GENERIC_FTS>
struct TopicNames;

template<typename TOPICTYPE_REQUEST, typename TOPICTYPE_REPLY, typename DUMMY>
class GenericFtsImpl final : public GenericFts<TOPICTYPE_REQUEST,TOPICTYPE_REPLY>
    , public GenericFtsInternal
{
public:
  using RequestTopic =    TOPICTYPE_REQUEST;
  using ReplyTopic =      TOPICTYPE_REPLY;
  using TopicNameHelper = TopicNames<GenericFtsImpl<RequestTopic, ReplyTopic, DUMMY>>;
private:
  using SubscriberCallback = std::function<void(const rti::core::SampleIdentity&, const RequestTopic&)>;

  template <typename T = ReplyTopic, typename std::enable_if<HasStatusFunction<T>::value, int>::type = 0>
  void sendReplyState(Core::FtReplyState replyState, const rti::core::SampleIdentity &requestSampleIdentity)
  {
    connext::WriteSample<ReplyTopic> reply;
    reply.data().replyState(replyState);

    bool bLastTelegram = ((replyState != Core::FtReplyState_def::InProgress) &&
                          (replyState != Core::FtReplyState_def::NotStarted));
    m_replier->send_reply(reply, requestSampleIdentity, bLastTelegram);
  }

  template <typename T = ReplyTopic, typename std::enable_if<!HasStatusFunction<T>::value, int>::type = 0>
  void sendReplyState(Core::FtReplyState, const rti::core::SampleIdentity &)
  {
    
  }

  template <typename T, typename std::enable_if<HasStatusFunction<T>::value, int>::type = 0>
  void setReply(T &data, Core::FtReplyState status)
  {
    data.replyState(status);
  }

  template <typename T, typename std::enable_if<!HasStatusFunction<T>::value, int>::type = 0>
  void setReply(T &, Core::FtReplyState)
  {
    
  }

public:
  GenericFtsImpl(const dds::domain::DomainParticipant &domainParticipant,
                 const dds::pub::Publisher &ddsPublisher,
                 const dds::sub::Subscriber &ddsSubscriber,
                 DDSGeneric::WaitSetHandler *wsHandler)
    : GenericFts<RequestTopic,ReplyTopic>()
    , GenericFtsInternal()
    , m_wsHandler(wsHandler)
    , m_subscriberCallback(nullptr)
    , m_logger(DDS::getLogger(TopicNameHelper::functionTopicName(), DDS::DataType::FUNCTION_TOPIC_SERVER))
  {
    Q_ASSERT(domainParticipant != nullptr);
    Q_ASSERT(ddsPublisher != nullptr);
    Q_ASSERT(ddsSubscriber != nullptr);
    Q_ASSERT(wsHandler != nullptr);

    qRegisterMetaType<RequestTopic> (TopicNameHelper::requestTopicTypeName());

    connext::ReplierParams<RequestTopic,ReplyTopic> replierParams(domainParticipant);

    

    
    replierParams.request_topic_name(TopicNameHelper::requestTopicName());
    replierParams.reply_topic_name(TopicNameHelper::replyTopicName());
    replierParams.publisher(ddsPublisher);
    replierParams.subscriber(ddsSubscriber);
    replierParams.datareader_qos(readerQos(m_logger));
    replierParams.datawriter_qos(writerQos(m_logger));

    
    

    m_replier = new connext::Replier<RequestTopic, ReplyTopic>(replierParams);
    m_readCondition = new dds::sub::cond::ReadCondition(m_replier->get_request_datareader(), dds::sub::status::SampleState::not_read());

    m_wsHandler->attachWaitSet(*m_readCondition, this);
  }

  ~GenericFtsImpl()
  {
    m_wsHandler->detachWaitSet(*m_readCondition, this);
    delete m_readCondition;
    delete m_replier;
  }

  bool setSubscriberCallback(const SubscriberCallback &subscriberCallback)
  {
    if (m_subscriberCallback)
    {
      WARNING_LOG(m_logger) << "Can not set subscriber callback - already set ...";
      return false;
    }

    m_subscriberCallback = subscriberCallback;
    return true;
  }

  

  bool sendReply(const rti::core::SampleIdentity &requestSampleIdentity, const ReplyTopic &replyData) override final
  {
    connext::WriteSampleRef<ReplyTopic> reply;
    rti::pub::WriteParams replyParams;

    reply.set_data(const_cast<ReplyTopic*>(&replyData));
    reply.set_info(&replyParams);

    m_replier->send_reply(reply, requestSampleIdentity);
    return true;
  }

  

  bool sendReply(const rti::core::SampleIdentity &requestIdentity, const QVector<ReplyTopic> &replyData) override final
  {
    DEBUG_LOG(m_logger) << TopicNameHelper::requestTopicName() << " - IN sendReply";

    for(int i = 0; i < replyData.size(); ++i)
    {
      if (HasStatusFunction<ReplyTopic>::value)
      {
        ReplyTopic rt = replyData.at(i);
        setReply(rt, i == replyData.size() - 1 ? Core::FtReplyState::Completed : Core::FtReplyState::InProgress);
        if (!sendReply(requestIdentity, rt))
          return false;
      } else {
        if (!sendReply(requestIdentity, replyData.at(i)))
          return false;
      }
    }

    return true;
  }

protected:
  
  
  
  

  void processRequest(const connext::Sample<RequestTopic> &request)
  {
    DEBUG_LOG(m_logger) << TopicNameHelper::requestTopicName() << " - processRequest: "
                        << connext::DebugInfo::toReadable(request.identity());

    qint64 age = qAbs(qint64(request.info().source_timestamp().to_secs()) - time(nullptr));

    if (age > 10) 
    {
      std::ostringstream ss;
      ss << request.data();
      std::string s = ss.str();
      int slen = s.length();

      static int max_slen = 1024;
      if (slen > max_slen)
      {
        s.resize(max_slen);
        s.append("...");
      }

      DEBUG_LOG(m_logger) << "received a request for topic "
                           << TopicNameHelper::requestTopicName()
                           << " but it was outdated - ignoring. "
                           << " local time: " << (time(nullptr) - 10) << "[s], source timestamp: "
                           << qint64(request.info().source_timestamp().to_secs())
                           << "[s],  -> age: " << age
                           << "[s], data: " << s;
      ReplyTopic rt;
      rt.errorString(tr_lts("dds", "Topic %1 was outdated and therefore ignored. Does the client time match the server time?"));
      rt.errorParams({TopicNameHelper::requestTopicName()});
      sendReply(request.identity(), rt);
      return;
    }

    sendReplyState(Core::FtReplyState::InProgress, request.identity());

    m_subscriberCallback(request.identity(), request.data());
  }

  

  void processRequests() override
  {
    DEBUG_LOG(m_logger) << TopicNameHelper::requestTopicName() << " - processRequests";

    while (true)
    {
      connext::Sample<RequestTopic> request;
      {
        std::lock_guard<std::mutex> l(m_requestMutex);
        if (m_requests.empty())
        {
          DEBUG_LOG(m_logger) << "No more requests";
          return;
        }
        DEBUG_LOG(m_logger) << TopicNameHelper::requestTopicName() << " - read one request for ";
        request = m_requests.front();
        m_requests.pop();
      }
      if (!request.info().valid())
      {
        WARNING_LOG(m_logger) << TopicNameHelper::requestTopicName() << " - No valid data";
        continue;
      }
      processRequest(request);
    }
    DEBUG_LOG(m_logger) << TopicNameHelper::requestTopicName() << " - processRequests finished";
  }

  bool enqueueRequests() override
  {
    bool bValidDataRead = false;
    connext::Sample<RequestTopic> request;
    while (m_replier->receive_request(request))
    {
      if (!request.info().valid())
        continue;
      bValidDataRead = true;
      std::lock_guard<std::mutex> l(m_requestMutex);
      m_requests.push(request);
    }
    return bValidDataRead;
  }

private:
  
  connext::Replier<RequestTopic, ReplyTopic> *m_replier;
  dds::sub::cond::ReadCondition *m_readCondition;

  
  DDSGeneric::WaitSetHandler    *m_wsHandler;

  std::queue<connext::Sample<RequestTopic>> m_requests;
  std::mutex m_requestMutex;

  
  SubscriberCallback m_subscriberCallback;

  NGL::INGLLogger               &m_logger;
};

}