qt6-migration-pipeline/Lts/DDS/DDSConnector/dds_connector.cpp

#include "dds_connector.h"
#include "publisher.h"
#include "subscriber.h"

#include <NGLLogger/NGLLoggerBootstrap.h>
#include <DDS/QosProfile/DDS_DefaultQos.h>
#include <DDS/Generic/wait_set_handler.h>
#include <QCoreApplication>

namespace DDSConnector
{

DdsConnector::DdsConnector(int domainId,
                           const dds::domain::DomainParticipant &domainParticipant,
                           IDL::PartitionType defaultPartitionType,
                           int partitionId,
                           int exerciseId,
                           DDS::QosProfile::ConnectionType connectionType,
                           const QUrl &tcpConnectionUrl)
  : m_domainId(domainId)
  , m_defaultPartitionType(defaultPartitionType)
  , m_partitionId(partitionId)
  , m_exerciseId(exerciseId)
  , m_tcpConnectionUrl(tcpConnectionUrl)
  , m_connectionType(tcpConnectionUrl.isEmpty() ? connectionType : DDS::QosProfile::ConnectionType::Tcp)
  , m_domainParticipant(domainParticipant)
  , m_publisher(nullptr)
  , m_subscriber(nullptr)
  , m_wsHandler(nullptr)
  , m_logger(NGL::NGLLoggerBootstrap::getNGLRootLogger().getNGLLogger("Common.DDSConnector"))
{
  initInternal();
  DEBUG_LOG(m_logger) << "DdsConnector ctor: " << m_domainId << "," << m_partitionId << ","<< m_exerciseId << "," << static_cast<int>(m_defaultPartitionType);
}

DdsConnector::DdsConnector(int domainId,
                           IDL::PartitionType defaultPartitionType,
                           int partitionId, int exerciseId,
                           const QUrl &tcpConnectionUrl)
  : DdsConnector(domainId,
                 nullptr,
                 defaultPartitionType,
                 partitionId,
                 exerciseId,
                 DDS::QosProfile::ConnectionType::Server,
                 tcpConnectionUrl)
{}

DdsConnector::DdsConnector(const dds::domain::DomainParticipant &domainParticipant,
                           IDL::PartitionType defaultPartitionType,
                           int partitionId, int exerciseId,
                           const QUrl &tcpConnectionUrl)
  : DdsConnector(domainParticipant.domain_id(),
                 domainParticipant,
                 defaultPartitionType,
                 partitionId,
                 exerciseId,
                 DDS::QosProfile::ConnectionType::Server,
                 tcpConnectionUrl)
{}


DdsConnector::DdsConnector(const dds::domain::DomainParticipant &domainParticipant,
                           IDL::PartitionType defaultPartitionType,
                           DDS::QosProfile::ConnectionType connectionType,
                           int partitionId,
                           int exerciseId
                           )
  : DdsConnector(domainParticipant.domain_id(),
                 domainParticipant,
                 defaultPartitionType,
                 partitionId,
                 exerciseId,
                 connectionType,
                 QUrl())
{}

DdsConnector::DdsConnector(int domainId,
                           IDL::PartitionType defaultPartitionType,
                           int partitionId, int exerciseId,
                           DDS::QosProfile::ConnectionType connectionType)
  : DdsConnector(domainId,
                 nullptr,
                 defaultPartitionType,
                 partitionId,
                 exerciseId,
                 connectionType,
                 QUrl())
{
  Q_ASSERT(connectionType != DDS::QosProfile::ConnectionType::Tcp);
}


DdsConnector::~DdsConnector()
{
  delete m_subscriber;
  delete m_publisher;
  delete m_wsHandler;
}

bool DdsConnector::init(const QStringList &qosFileList)
{
  return DDS::QosProfile::setDefault(qosFileList);
}

QUrl DdsConnector::getTcpConnectionUrl() const
{
  DEBUG_LOG(m_logger) << this << " gettcpConnectionUrl: " << m_tcpConnectionUrl.toString();
  return m_tcpConnectionUrl;
}

dds::pub::Publisher DdsConnector::getDdsPublisher() const
{
  return m_publisher ? m_publisher->getDdsPublisher() : nullptr;
}

dds::sub::Subscriber DdsConnector::getDdsSubscriber() const
{
  return m_subscriber ? m_subscriber->getDdsSubscriber() : nullptr;
}

DDSGeneric::SubscriberListener *DdsConnector::getSubscriberListener()
{
  return m_subscriber ? m_subscriber->getSubscriberListener() : nullptr;
}

void DdsConnector::initPublisherSubscriber()
{
  initSubscriber();
  initPublisher();
}

void DdsConnector::initPublisher()
{
  if (m_publisher)
    return;
  initDomainParticipant();
  
  setPublisher(new DDSConnector::Publisher(m_domainParticipant, getDefaultPartition()));
}

void DdsConnector::initSubscriber()
{
  if (m_subscriber)
    return;
  initDomainParticipant();
  
  setSubscriber(new DDSConnector::Subscriber(m_domainParticipant, getDefaultPartition()));
}

void DdsConnector::initDomainParticipant()
{
  if (m_domainParticipant == nullptr)
    setDomainParticipant(dds::domain::DomainParticipant(m_domainId,
                                                        DDS::QosProfile::domainParticipantDefault(m_connectionType, m_tcpConnectionUrl)));
}


std::string DdsConnector::getDefaultPartition() const
{
  return getPartitionString(m_defaultPartitionType, m_partitionId);
}

std::string DdsConnector::getPartitionString(IDL::PartitionType pt, int partitionId)
{
  switch (pt)
  {
  case IDL::PartitionType::Any:
    return "*";
  case IDL::PartitionType::Global:
    return IDL::exercisePartition(0);
  case IDL::PartitionType::Exercise:
  case IDL::PartitionType::AllExercises:
    return IDL::exercisePartition(partitionId);
  case IDL::PartitionType::Replay:
    return IDL::replayPartition(partitionId);
  }

  NGL::INGLLogger &m_logger = NGL::NGLLoggerBootstrap::getNGLRootLogger().getNGLLogger("Common.DDSConnector");
  ERROR_LOG(m_logger) << "Invalid partition type: " << static_cast<int>(pt);
  return std::string();
}

void DdsConnector::setDomainParticipant(const dds::domain::DomainParticipant &participant)
{
  m_domainParticipant = participant;
  if (m_domainParticipant == nullptr)
    return;

  try
  {
    
    dds::domain::qos::DomainParticipantQos dpQos(m_domainParticipant.qos());
    DDS::QosProfile::print(dpQos, m_logger);
    rti::core::policy::EntityName entityName;
    entityName.name(qAppName().toStdString() + (" - DP ") + std::to_string(m_domainId));
    dpQos << entityName;
    m_domainParticipant.qos(dpQos);
    m_domainParticipant->default_datareader_qos(DDS::QosProfile::dataReaderDefault(m_connectionType)); 
    m_domainParticipant.enable();
  }
  catch (const dds::core::Exception &ex)
  {
    WARNING_LOG(m_logger) << "Could not enable DomainParticipant: " << ex.what();
    exit(1);
  }
}

void DdsConnector::setPublisher(Publisher *publisher)
{
  if (m_publisher && publisher != m_publisher)
  {
    WARNING_LOG(m_logger) << "Trying to set publisher but there is already a publisher set - overwriting!";
    delete m_publisher;
  }
  m_publisher = publisher;
  if (m_publisher)
    DDS::QosProfile::print(m_publisher->getDdsPublisher().default_datawriter_qos(), m_logger);
}

void DdsConnector::enableBatchedWriter(bool bEnable)
{
  if (m_publisher)
    m_publisher->enableBatchedWriter(bEnable);
}


void DdsConnector::setSubscriber(Subscriber *subscriber)
{
  if (m_subscriber && subscriber != m_subscriber)
  {
    WARNING_LOG(m_logger) << "Trying to set subscriber but there is already a subscriber set - overwriting!";
    delete m_subscriber;
  }
  m_subscriber = subscriber;
  if (m_subscriber)
    DDS::QosProfile::print(m_subscriber->getDdsSubscriber().default_datareader_qos(), m_logger);
}

void DdsConnector::initInternal()
{
  if (m_tcpConnectionUrl.isValid())
  {
    m_connectionType = DDS::QosProfile::ConnectionType::Tcp;
  }
  else
  {
    m_tcpConnectionUrl.clear();
    if (m_connectionType == DDS::QosProfile::ConnectionType::Tcp)
      m_connectionType = DDS::QosProfile::ConnectionType::ShmUdp;
  }

  if (m_defaultPartitionType == IDL::PartitionType::Any ||
      m_defaultPartitionType == IDL::PartitionType::AllExercises)
    m_partitionId = -1;
  if (m_defaultPartitionType == IDL::PartitionType::Exercise && m_partitionId < 0)
    WARNING_LOG(m_logger) << "Want to subscribe to an exercise, but no exercise Id is given!";
  if (m_defaultPartitionType == IDL::PartitionType::Global)
  {
    m_partitionId = 0;
    m_exerciseId = 0;
  }
  INFO_LOG(m_logger) << "DDSConnector(" << m_domainId << ") for partition: " << m_partitionId
                     << " with partition type " << static_cast<int>(m_defaultPartitionType);

  
  dds::domain::qos::DomainParticipantFactoryQos domainParticipantFactoryQos = dds::domain::DomainParticipant::participant_factory_qos();
  
  domainParticipantFactoryQos << dds::core::policy::EntityFactory::ManuallyEnable();
  dds::domain::DomainParticipant::participant_factory_qos(domainParticipantFactoryQos);
}


void DdsConnector::createWaitSetHandler()
{
  if (!m_wsHandler)
    m_wsHandler = new DDSGeneric::WaitSetHandler;
}

}