Initial commit

Lts/Tools/TopicDiscover/topic_discoverer.cpp

@@ -0,0 +1,194 @@
+
+#include "topic_discoverer.h"
+
+#include <NGLLogger/NGLLoggerBootstrap.h>
+#include <QCoreApplication>
+#include <DDS/QosProfile/DDS_DefaultQos.h>
+
+BuiltinAnyListener::BuiltinAnyListener(QObject *parent)
+  : m_logger(NGL::NGLLoggerBootstrap::getNGLRootLogger().getNGLLogger("TopicDiscoverer.BuiltinAnyListener"))
+{
+  Q_UNUSED(parent)
+}
+
+dds::core::xtypes::StructType BuiltinAnyListener::structType(const std::string &typeName)
+{
+  QReadLocker l(&m_safeTypeLock);
+  return m_structTypes.value(typeName, dds::core::xtypes::StructType("NULL"));
+}
+
+// This gets called when a participant has been discovered
+template <typename S>
+void BuiltinAnyListener::on_data_available(dds::sub::DataReader<S> &reader)
+{
+  // We only process newly seen participants
+  dds::sub::LoanedSamples<S> samples
+      = reader.select().state(dds::sub::status::DataState::new_instance()).take();
+
+  for (typename dds::sub::LoanedSamples<S>::iterator sampleIt = samples.begin();
+       sampleIt != samples.end();
+       ++sampleIt)
+  {
+    if (!sampleIt->info().valid())
+      continue;
+
+    const std::string &topicName = sampleIt->data().topic_name();
+    const std::string &typeName  = sampleIt->data().type_name();
+
+    if (QString::fromStdString(typeName).contains(QLatin1String("rti::"), Qt::CaseInsensitive))
+      continue;
+
+    dds::core::optional<dds::core::xtypes::DynamicType> dynamicType = sampleIt->data()->type();
+    if (!dynamicType.is_set() || dynamicType.get().kind() != dds::core::xtypes::TypeKind::STRUCTURE_TYPE)
+      continue;
+
+    const dds::core::xtypes::StructType &structType = static_cast<dds::core::xtypes::StructType&>(dynamicType.get());
+
+    DEBUG_LOG(m_logger) << "Built-in Reader: found subscriber: "
+                        << " topicName: " << topicName
+                        << " topicType: " <<  typeName
+                       ;
+
+    {
+      QWriteLocker l(&m_safeTypeLock);
+      m_structTypes.insert(typeName, structType);
+    }
+    emit topicDescription(topicName, typeName);
+  }
+}
+
+/***********************************************************************************************/
+
+BuiltinPublicationListener::BuiltinPublicationListener(QObject *parent)
+  : BuiltinAnyListener(parent)
+{
+
+}
+
+void BuiltinPublicationListener::on_data_available(dds::sub::DataReader<dds::topic::PublicationBuiltinTopicData> &reader)
+{
+  BuiltinAnyListener::on_data_available(reader);
+}
+
+/***********************************************************************************************/
+
+BuiltinSubscriberListener::BuiltinSubscriberListener(QObject *parent)
+  : BuiltinAnyListener(parent)
+{
+
+}
+
+void BuiltinSubscriberListener::on_data_available(dds::sub::DataReader<dds::topic::SubscriptionBuiltinTopicData> &reader)
+{
+  BuiltinAnyListener::on_data_available(reader);
+}
+
+/***********************************************************************************************/
+
+TopicDiscoverer::TopicDiscoverer(int domainId, QObject *parent)
+  : QObject(parent)
+  , m_domainId(domainId)
+  , m_domainParticipant(nullptr)
+  , m_publisherListener(nullptr)
+  , m_subscriberListener(nullptr)
+  , m_logger(NGL::NGLLoggerBootstrap::getNGLRootLogger().getNGLLogger("TopicDiscoverer"))
+{
+  qRegisterMetaType<std::string>("std::string");
+}
+
+void TopicDiscoverer::init(DDS::QosProfile::ConnectionType ct, const QUrl &tcpDiscoveryUrl)
+{
+  // make sure to get the same qos as a normal dp/publisher/subscriber
+  DDS::QosProfile::setDefault();
+
+  // By default, the participant is enabled upon construction.
+  // At that time our listeners for the builtin topics have not
+  // been installed, so we disable the participant until we
+  // set up the listeners.
+
+  dds::domain::qos::DomainParticipantFactoryQos factoryQos =
+      dds::domain::DomainParticipant::participant_factory_qos();
+
+  factoryQos << dds::core::policy::EntityFactory::ManuallyEnable();
+  dds::domain::DomainParticipant::participant_factory_qos(factoryQos);
+
+  // Create a DomainParticipant with given Qos.
+  m_domainParticipant = dds::domain::DomainParticipant(m_domainId, DDS::QosProfile::domainParticipantDefault(ct, tcpDiscoveryUrl));
+  dds::domain::qos::DomainParticipantQos dpQos = m_domainParticipant.qos();
+  rti::core::policy::EntityName entityName;
+  entityName.name(QString(qAppName() +
+                          QStringLiteral(" - DP ") +
+                          QString::number(m_domainId)
+                          ).toStdString());
+  dpQos << entityName;
+  m_domainParticipant.qos(dpQos);
+
+  // Installing listeners for the builtin topics requires several steps
+  // First get the builtin subscriber.
+  dds::sub::Subscriber builtin_subscriber = dds::sub::builtin_subscriber(m_domainParticipant);
+
+  // Get builtin subscriber's datareader for publishers.
+  std::vector< dds::sub::DataReader<dds::topic::PublicationBuiltinTopicData> > publication_reader;
+  dds::sub::find< dds::sub::DataReader<dds::topic::PublicationBuiltinTopicData> >
+      (
+        builtin_subscriber,
+        dds::topic::publication_topic_name(),
+        std::back_inserter(publication_reader)
+        );
+
+  BuiltinPublicationListener *bipl = new BuiltinPublicationListener(this);
+
+  // Install our listener using ListenerBinder.
+  m_publisherListener = rti::core::bind_and_manage_listener(publication_reader[0],
+      bipl,
+      dds::core::status::StatusMask::data_available()
+      );
+
+  connect(bipl, &BuiltinPublicationListener::topicDescription,
+          this, &TopicDiscoverer::onTopicDescription);
+  /*------------------------------*/
+
+  // Get builtin subscriber's datareader for subscribers.
+  std::vector< dds::sub::DataReader<dds::topic::SubscriptionBuiltinTopicData> > subscription_reader;
+  dds::sub::find< dds::sub::DataReader<dds::topic::SubscriptionBuiltinTopicData> >
+      (
+        builtin_subscriber,
+        dds::topic::subscription_topic_name(),
+        std::back_inserter(subscription_reader)
+        );
+
+  BuiltinSubscriberListener *bisl = new BuiltinSubscriberListener(this);
+
+  // Install our listener using ListenerBinder.
+  m_subscriberListener = rti::core::bind_and_manage_listener(subscription_reader[0],
+      bisl,
+      dds::core::status::StatusMask::data_available()
+      );
+
+  connect(bisl, &BuiltinSubscriberListener::topicDescription,
+          this, &TopicDiscoverer::onTopicDescription);
+  /*------------------------------*/
+
+  // Done! All the listeners are installed, so we can enable the
+  // participant now.
+  m_domainParticipant.enable();
+}
+
+void TopicDiscoverer::onTopicDescription(const std::string &topicName,
+                                         const std::string &typeName)
+{
+  INFO_LOG(m_logger) << "Built-in Reader: found Topic: "
+                     << " topicName: " << topicName
+                     << " typeName: " << typeName
+                    ;
+
+  BuiltinAnyListener* bal = qobject_cast<BuiltinAnyListener*>(sender());
+
+  if (!bal)
+    return;
+
+  dds::core::xtypes::StructType type = bal->structType(typeName);
+
+  if (type.name() != "NULL")
+    emit topicDescription(topicName, typeName, type);
+}