Add probe for dds

src/lava/magma/runtime/_c_message_infrastructure/csrc/channel/dds/cyclone_dds.cc

@@ -39,6 +39,9 @@ void CycloneDDSPubListener::on_publication_matched(
 
 DDSInitErrorType CycloneDDSPublisher::Init() {
   LAVA_LOG(LOG_DDS, "publisher init\n");
+  LAVA_DEBUG(LOG_DDS,
+             "Init CycloneDDS Publisher Successfully, topic name: %s\n",
+             topic_name_.c_str());
   dds_metadata_ = std::make_shared<ddsmetadata::msg::DDSMetaData>();
   if (dds_transfer_type_ != DDSTransportType::DDSUDPv4) {
     LAVA_LOG_WARN(LOG_DDS, "Unsupport Transfer type and will use UDP\n");
@@ -65,12 +68,12 @@ DDSInitErrorType CycloneDDSPublisher::Init() {
 
 bool CycloneDDSPublisher::Publish(DataPtr data) {
   LAVA_DEBUG(LOG_DDS,
-             "CycloneDDS publisher start publishing, matched:%d\n",
-             listener_->matched_.load());
+          "CycloneDDS publisher start publishing topic name = %s, matched:%d\n",
+          topic_name_.c_str(), listener_->matched_.load());
   LAVA_DEBUG(LOG_DDS,
              "writer_ matched: %d\n",
              writer_.publication_matched_status().current_count());
-  while (listener_->matched_.load() == 0) {
+  while (writer_.publication_matched_status().current_count() == 0) {
     helper::Sleep();
   }
   LAVA_DEBUG(LOG_DDS, "CycloneDDS publisher find matched reader\n");
@@ -95,20 +98,26 @@ bool CycloneDDSPublisher::Publish(DataPtr data) {
 }
 
 void CycloneDDSPublisher::Stop() {
-  LAVA_LOG(LOG_DDS, "Stop CycloneDDS Publisher, waiting unmatched...\n");
+  LAVA_LOG(LOG_DDS, \
+           "Stop CycloneDDS Publisher, topic_name%s, waiting unmatched...\n",
+           topic_name_.c_str());
   if (stop_) {
     return;
   }
-  while (listener_->matched_.load() > 0) {
+  while (listener_ != nullptr && listener_->matched_.load() > 0) {
     helper::Sleep();
   }
-  try {
-    participant_ = dds::core::null;
+  if (writer_ != dds::core::null) {
+    writer_ = dds::core::null;
+  }
+  if (publisher_ != dds::core::null) {
     publisher_ = dds::core::null;
+  }
+  if (topic_ != dds::core::null) {
     topic_ = dds::core::null;
-    writer_ = dds::core::null;
-  } catch (const dds::core::Exception& e) {
-    LAVA_LOG_ERR("DDS Exception: %s\n", e.what());
+  }
+  if (participant_ != dds::core::null) {
+    participant_ = dds::core::null;
   }
   stop_ = true;
 }
@@ -136,6 +145,9 @@ void CycloneDDSSubListener::on_subscription_matched(
 }
 DDSInitErrorType CycloneDDSSubscriber::Init() {
   LAVA_LOG(LOG_DDS, "subscriber init\n");
+  LAVA_DEBUG(LOG_DDS,
+             "Init CycloneDDS Subscriber, topic name: %s\n",
+              topic_name_.c_str());
   if (dds_transfer_type_ != DDSTransportType::DDSUDPv4) {
     LAVA_LOG_WARN(LOG_DDS, "Unsupport Transfer type and will use UDP\n");
   }
@@ -165,7 +177,9 @@ DDSInitErrorType CycloneDDSSubscriber::Init() {
 }
 
 MetaDataPtr CycloneDDSSubscriber::Recv(bool keep) {
-  LAVA_DEBUG(LOG_DDS, "CycloneDDS recving...\n");
+  LAVA_DEBUG(LOG_DDS,
+             "CycloneDDS topic name= %s recving...\n",
+             topic_name_.c_str());
   dds::sub::LoanedSamples<ddsmetadata::msg::DDSMetaData> samples;
   if (keep) {
     while ((samples = selector_->read()).length() <= 0) {
@@ -204,19 +218,22 @@ MetaDataPtr CycloneDDSSubscriber::Recv(bool keep) {
   return nullptr;
 }
 
+bool CycloneDDSSubscriber::Probe() {
+  return (selector_->read()).length() > 0;
+}
 void CycloneDDSSubscriber::Stop() {
   if (stop_)
     return;
-  LAVA_DEBUG(LOG_DDS, "Subscriber Stop and release...\n");
-  try {
+  LAVA_DEBUG(LOG_DDS,
+            "CycloneDDSSubscriber topic name = %s Stop and release...\n",
+             topic_name_.c_str());
+  if (listener_ != nullptr && reader_  != dds::core::null) {
     reader_.~DataReader();
-    participant_ = dds::core::null;
-    subscriber_ = dds::core::null;
-    topic_ = dds::core::null;
     reader_ = dds::core::null;
-  } catch (const dds::core::Exception& e) {
-    LAVA_LOG_ERR("DDS Exception: %s\n", e.what());
   }
+  if (participant_ != dds::core::null) participant_ = dds::core::null;
+  if (subscriber_ != dds::core::null) subscriber_ = dds::core::null;
+  if (topic_ != dds::core::null) topic_ = dds::core::null;
   stop_ = true;
 }
 

src/lava/magma/runtime/_c_message_infrastructure/csrc/channel/dds/cyclone_dds.h

@@ -86,6 +86,7 @@ class CycloneDDSSubscriber final : public DDSSubscriber {
   DDSInitErrorType Init();
   void Stop();
   MetaDataPtr Recv(bool keep);
+  bool Probe();
 
  private:
   CycloneDDSSubListenerPtr listener_ = nullptr;

src/lava/magma/runtime/_c_message_infrastructure/csrc/channel/dds/dds.cc

@@ -19,6 +19,7 @@ DDSPtr DDSManager::AllocDDS(const std::string &topic_name,
                             const DDSBackendType &dds_backend,
                             const size_t &max_samples) {
   std::lock_guard<std::mutex> lg(dds_lock_);
+
   if (dds_topics_.find(topic_name) != dds_topics_.end()) {
     LAVA_LOG_ERR("The topic %s has already been used\n", topic_name.c_str());
     return nullptr;
@@ -45,6 +46,7 @@ void DDS::CreateFastDDSBackend(const std::string &topic_name,
                                const DDSTransportType &dds_transfer_type,
                                const size_t &max_samples) {
 #if defined(FASTDDS_ENABLE)
+  LAVA_DEBUG(LOG_DDS, "DDS::CreateFastDDSBackend\n");
   dds_publisher_ = std::make_shared<FastDDSPublisher>(topic_name,
                                                       dds_transfer_type,
                                                       max_samples);

src/lava/magma/runtime/_c_message_infrastructure/csrc/channel/dds/dds.h

@@ -31,6 +31,7 @@ class DDSSubscriber {
  public:
   virtual DDSInitErrorType Init() = 0;
   virtual MetaDataPtr Recv(bool keep) = 0;
+  virtual bool Probe() = 0;
   virtual void Stop() = 0;
   virtual ~DDSSubscriber() {}
 };

src/lava/magma/runtime/_c_message_infrastructure/csrc/channel/dds/dds_channel.cc

@@ -9,17 +9,22 @@
 
 namespace message_infrastructure {
 
-DDSChannel::DDSChannel(const std::string &topic_name,
+DDSChannel::DDSChannel(const std::string &src_name,
+                       const std::string &dst_name,
+                       const std::string &topic_name,
+                       const size_t &size,
+                       const size_t &nbytes,
                        const DDSTransportType &dds_transfer_type,
-                       const DDSBackendType &dds_backend,
-                       const size_t &size) {
+                       const DDSBackendType &dds_backend) {
   LAVA_DEBUG(LOG_DDS, "Creating DDSChannel...\n");
-  dds_ = GetDDSManagerSingleton().AllocDDS(topic_name,
+
+  dds_ = GetDDSManagerSingleton().AllocDDS(
+                                  topic_name,
                                   dds_transfer_type,
                                   dds_backend,
                                   size);
-  send_port_ = std::make_shared<DDSSendPort>(dds_);
-  recv_port_ = std::make_shared<DDSRecvPort>(dds_);
+  send_port_ = std::make_shared<DDSSendPort>(src_name, size, nbytes, dds_);
+  recv_port_ = std::make_shared<DDSRecvPort>(dst_name, size, nbytes, dds_);
 }
 
 AbstractSendPortPtr DDSChannel::GetSendPort() {
@@ -29,4 +34,23 @@ AbstractSendPortPtr DDSChannel::GetSendPort() {
 AbstractRecvPortPtr DDSChannel::GetRecvPort() {
   return recv_port_;
 }
+
+std::shared_ptr<DDSChannel> GetDefaultDDSChannel(const size_t &nbytes,
+                                                 const size_t &size,
+                                                 const std::string &src_name,
+                                                 const std::string &dst_name) {
+  DDSBackendType BackendType = DDSBackendType::FASTDDSBackend;
+  #if defined(CycloneDDS_ENABLE)
+    BackendType = DDSBackendType::CycloneDDSBackend;
+  #endif
+  return std::make_shared<DDSChannel>(
+                          src_name,
+                          dst_name,
+                          "dds_topic_" + std::to_string(std::rand()),
+                          size,
+                          nbytes,
+                          DDSTransportType::DDSUDPv4,
+                          BackendType);
+}
+
 }  // namespace message_infrastructure

src/lava/magma/runtime/_c_message_infrastructure/csrc/channel/dds/dds_channel.h

@@ -20,10 +20,13 @@ class DDSChannel : public AbstractChannel {
  public:
   DDSChannel() = delete;
   ~DDSChannel() override {}
-  DDSChannel(const std::string &topic_name,
+  DDSChannel(const std::string &src_name,
+            const std::string &dst_name,
+            const std::string &topic_name,
+            const size_t &size,
+            const size_t &nbytes,
             const DDSTransportType &dds_transfer_type,
-            const DDSBackendType &dds_backend,
-            const size_t &size);
+            const DDSBackendType &dds_backend);
   AbstractSendPortPtr GetSendPort();
   AbstractRecvPortPtr GetRecvPort();
 
@@ -32,6 +35,12 @@ class DDSChannel : public AbstractChannel {
   DDSSendPortPtr send_port_ = nullptr;
   DDSRecvPortPtr recv_port_ = nullptr;
 };
+
+std::shared_ptr<DDSChannel> GetDefaultDDSChannel(const size_t &nbytes,
+                                                 const size_t &size,
+                                                 const std::string &src_name,
+                                                 const std::string &dst_name);
+
 }  // namespace message_infrastructure
 
 #endif  // CHANNEL_DDS_DDS_CHANNEL_H_

src/lava/magma/runtime/_c_message_infrastructure/csrc/channel/dds/dds_port.h

@@ -7,13 +7,17 @@
 
 #include <atomic>
 #include <memory>
-
+#include <string>
 namespace message_infrastructure {
 
 class DDSSendPort final : public AbstractSendPort {
  public:
   DDSSendPort() = delete;
-  DDSSendPort(DDSPtr dds) : publisher_(dds->dds_publisher_) {}
+  DDSSendPort(const std::string &name,
+              const size_t &size,
+              const size_t &nbytes,
+              DDSPtr dds) :AbstractSendPort(name, size, nbytes),
+              publisher_(dds->dds_publisher_) {}
   ~DDSSendPort() = default;
   void Start() {
     auto flag = publisher_->Init();
@@ -45,7 +49,11 @@ using DDSSendPortPtr = std::shared_ptr<DDSSendPort>;
 class DDSRecvPort final : public AbstractRecvPort {
  public:
   DDSRecvPort() = delete;
-  DDSRecvPort(DDSPtr dds) : subscriber_(dds->dds_subscriber_) {}
+  DDSRecvPort(const std::string &name,
+              const size_t &size,
+              const size_t &nbytes,
+              DDSPtr dds) :AbstractRecvPort(name, size, nbytes),
+              subscriber_(dds->dds_subscriber_) {}
   ~DDSRecvPort() override {}
   void Start() {
     auto flag = subscriber_->Init();
@@ -64,7 +72,7 @@ class DDSRecvPort final : public AbstractRecvPort {
     return subscriber_->Recv(true);
   }
   bool Probe() {
-    return false;
+    return subscriber_->Probe();
   }
 
  private:

src/lava/magma/runtime/_c_message_infrastructure/csrc/channel/dds/fast_dds.cc

@@ -36,7 +36,6 @@ DDSInitErrorType FastDDSPublisher::Init() {
   InitParticipant();
   if (participant_ == nullptr)
     return DDSInitErrorType::DDSParticipantError;
-
   type_.register_type(participant_);
   publisher_ = participant_->create_publisher(PUBLISHER_QOS_DEFAULT);
   if (publisher_ == nullptr)
@@ -100,6 +99,9 @@ void FastDDSPublisher::InitParticipant() {
 }
 
 bool FastDDSPublisher::Publish(DataPtr data) {
+  LAVA_DEBUG(LOG_DDS,
+             "FastDDS Publish topic name = %s\n",
+             topic_name_.c_str());
   MetaData* metadata = reinterpret_cast<MetaData*>(data.get());
   if (listener_->matched_ > 0) {
     LAVA_DEBUG(LOG_DDS, "FastDDS publisher start publishing...\n");
@@ -129,7 +131,7 @@ bool FastDDSPublisher::Publish(DataPtr data) {
 
 void FastDDSPublisher::Stop() {
   LAVA_LOG(LOG_DDS, "Stop FastDDS Publisher, waiting unmatched...\n");
-  while (listener_->matched_ > 0) {
+  while (listener_ != nullptr && listener_->matched_ > 0) {
     helper::Sleep();
   }
   if (writer_ != nullptr) {
@@ -250,6 +252,7 @@ DDSInitErrorType FastDDSSubscriber::Init() {
 }
 
 MetaDataPtr FastDDSSubscriber::Recv(bool keep) {
+  LAVA_DEBUG(LOG_DDS, "FastDDS Recv topic name = %s\n", topic_name_.c_str());
   FASTDDS_CONST_SEQUENCE(MDataSeq, ddsmetadata::msg::DDSMetaData);
   MDataSeq mdata_seq;
   SampleInfoSeq infos;
@@ -261,6 +264,7 @@ MetaDataPtr FastDDSSubscriber::Recv(bool keep) {
     }
   } else {
     LAVA_DEBUG(LOG_DDS, "Take the data recieved\n");
+
     while (ReturnCode_t::RETCODE_OK !=
            reader_->take(mdata_seq, infos, 1)) {
       helper::Sleep();
@@ -298,32 +302,28 @@ MetaDataPtr FastDDSSubscriber::Recv(bool keep) {
   return nullptr;
 }
 
+bool FastDDSSubscriber::Probe() {
+  FASTDDS_CONST_SEQUENCE(MDataSeq, ddsmetadata::msg::DDSMetaData);
+  MDataSeq mdata_seq;
+  SampleInfoSeq infos;
+  bool res = false;
+  if (ReturnCode_t::RETCODE_OK == reader_->read(mdata_seq, infos, 1)) {
+    reader_->return_loan(mdata_seq, infos);
+    res = true;
+  }
+  return res;
+}
+
 void FastDDSSubscriber::Stop() {
   LAVA_DEBUG(LOG_DDS, "Subscriber Stop and release\n");
-  bool valid = true;
-  if (reader_ != nullptr) {
+  if (reader_ != nullptr)
     subscriber_->delete_datareader(reader_);
-  } else {
-    valid = false;
-  }
-  if (topic_ != nullptr) {
+  if (topic_ != nullptr)
     participant_->delete_topic(topic_);
-  } else {
-    valid = false;
-  }
-  if (subscriber_ != nullptr) {
+  if (subscriber_ != nullptr)
     participant_->delete_subscriber(subscriber_);
-  } else {
-    valid = false;
-  }
-  if (participant_ != nullptr) {
+  if (participant_ != nullptr)
     DomainParticipantFactory::get_instance()->delete_participant(participant_);
-  } else {
-    valid = false;
-  }
-  if (!valid) {
-    LAVA_LOG_ERR("Stop function is not valid\n");
-  }
   stop_ = true;
 }
 

src/lava/magma/runtime/_c_message_infrastructure/csrc/channel/dds/fast_dds.h

@@ -109,6 +109,7 @@ class FastDDSSubscriber final : public DDSSubscriber {
   DDSInitErrorType Init();
   void Stop();
   MetaDataPtr Recv(bool keep);
+  bool Probe();
 
  private:
   void InitParticipant();