From e4c214578bf980d7f2acae9bb8b4824977231732 Mon Sep 17 00:00:00 2001
From: Erik Hofman <erik@ehofman.com>
Date: Mon, 5 Apr 2021 12:38:13 +0200
Subject: [PATCH] Allow parsing a DDS configuration file. Add a guard condition
 to wake up the waitset at exit.

---
 simgear/io/SGDataDistributionService.cxx | 42 ++++++++++++++++++------
 simgear/io/SGDataDistributionService.hxx | 19 +++++++++--
 2 files changed, 48 insertions(+), 13 deletions(-)

diff --git a/simgear/io/SGDataDistributionService.cxx b/simgear/io/SGDataDistributionService.cxx
index c0af7f65..11a3afe9 100644
--- a/simgear/io/SGDataDistributionService.cxx
+++ b/simgear/io/SGDataDistributionService.cxx
@@ -101,7 +101,7 @@ SG_DDS_Topic::open(dds_entity_t p, SGProtocolDir direction)
     {
         dds_qos_t *qos = dds_create_qos();
         dds_qset_reliability(qos, DDS_RELIABILITY_RELIABLE, DDS_MSECS(1));
-        dds_qset_history(qos, DDS_HISTORY_KEEP_LAST, 3);
+        dds_qset_history(qos, DDS_HISTORY_KEEP_LAST, 1);
     
         entry = dds_create_reader(participant, topic, qos, NULL);
         if (entry < 0) {
@@ -222,12 +222,21 @@ SG_DDS_Topic::close()
 
 
 // participant
-SG_DDS::SG_DDS(dds_domainid_t d) :
-    domain(d)
+SG_DDS::SG_DDS(dds_domainid_t domain_id, const char *config)
 {
+    if (domain < 0 && domain_id != DDS_DOMAIN_DEFAULT)
+    {
+        domain = dds_create_domain(domain_id, config);
+        if (domain < 0) {
+            SG_LOG(SG_IO, SG_ALERT, "dds_create_domain: "
+                                     << dds_strretcode(-domain));
+            return;
+        }
+    }
+
     if (participant < 0)
     {
-        participant = dds_create_participant(domain, NULL, NULL);
+        participant = dds_create_participant(domain_id, NULL, NULL);
 
         if (participant < 0) {
             SG_LOG(SG_IO, SG_ALERT, "dds_create_participant: "
@@ -237,6 +246,12 @@ SG_DDS::SG_DDS(dds_domainid_t d) :
     }
 
     waitset = dds_create_waitset(participant);
+
+    // a guard condition is able to wakeup the waitset at destruction.
+    guard = dds_create_guardcondition(participant);
+    if (guard < 0)
+        SG_LOG(SG_IO, SG_ALERT, "dds_create_guardcondition: "
+                                 << dds_strretcode(-guard));
 }
 
 SG_DDS::~SG_DDS()
@@ -273,6 +288,10 @@ SG_DDS::add(SG_DDS_Topic *topic, const SGProtocolDir d)
 bool
 SG_DDS::close()
 {
+    // wakeup the waitset.
+    if (guard >= 0)
+        dds_set_guardcondition(guard, true);
+
     for (auto it : readers)
         delete it;
 
@@ -291,13 +310,16 @@ SG_DDS::wait(float dt)
     size_t num = readers.size();
     if (!num) return false;
 
-    dds_duration_t timeout = DDS_INFINITY;
+    dds_duration_t timeout = dt * DDS_NSECS_IN_SEC;
+    if (dt == std::numeric_limits<float>::max())
+        timeout = DDS_INFINITY;
+
+    bool triggered;
+    int status = dds_read_guardcondition(guard, &triggered);
+    if (triggered) return true;
+
     dds_attach_t results[num];
-
-    if (dt > 0.0)
-        timeout = dt * DDS_NSECS_IN_SEC;
-
-    int status = dds_waitset_wait(waitset, results, num, timeout);
+    status = dds_waitset_wait(waitset, results, num, timeout);
     if (status < 0) {
         SG_LOG(SG_IO, SG_ALERT, "dds_waitset_wait: "
                                  << dds_strretcode(-status));
diff --git a/simgear/io/SGDataDistributionService.hxx b/simgear/io/SGDataDistributionService.hxx
index 7e38471d..0b05cab9 100644
--- a/simgear/io/SGDataDistributionService.hxx
+++ b/simgear/io/SGDataDistributionService.hxx
@@ -27,6 +27,7 @@
 #ifndef _SG_DDS_Topic_HXX
 #define _SG_DDS_Topic_HXX
 
+#include <limits>
 #include <string>
 #include <vector>
 
@@ -45,6 +46,8 @@ private:
 
     std::string topic_name;
     const dds_topic_descriptor_t *descriptor = nullptr;
+
+    char *buffer = nullptr;
     size_t packet_size = 0;
 
     uint32_t status = 0;
@@ -63,6 +66,7 @@ public:
 
     template<typename T>
     SG_DDS_Topic(T& type, const dds_topic_descriptor_t *desc) : SG_DDS_Topic() {
+        buffer = (char*)(&type);
         setup<T>(desc);
     }
 
@@ -90,6 +94,10 @@ public:
     // read data from the topic.
     int read(char *buf, int length);
 
+    int read() {
+        return buffer ? read(buffer, packet_size) : 0;
+    }
+
     template<typename T>
     bool read(T& sample) {
         return (read((char*)&sample, sizeof(T)) == sizeof(T)) ? true : false;
@@ -112,16 +120,21 @@ public:
 // a class to manage multiple DDS topics
 class SG_DDS {
 private:
+    dds_entity_t domain = -1;
     dds_entity_t participant = -1;
-    dds_domainid_t domain = FG_DDS_DOMAIN;
+    dds_domainid_t domain_id = FG_DDS_DOMAIN;
 
+    dds_entity_t guard = -1;
     dds_entity_t waitset = -1;
 
     std::vector<SG_DDS_Topic*> readers;
     std::vector<SG_DDS_Topic*> writers;
 
 public:
-    SG_DDS(dds_domainid_t d = FG_DDS_DOMAIN);
+    SG_DDS(dds_domainid_t d = FG_DDS_DOMAIN, const char *c = "");
+
+    SG_DDS(dds_domainid_t d, std::string& c) : SG_DDS(d, c.c_str()) {};
+
     ~SG_DDS();
 
     bool add(SG_DDS_Topic *topic, const SGProtocolDir d);
@@ -130,7 +143,7 @@ public:
     const std::vector<SG_DDS_Topic*>& get_readers() { return readers; }
     const std::vector<SG_DDS_Topic*>& get_writers() { return writers; }
 
-    bool wait(float dt = 0.0f);
+    bool wait(float dt = std::numeric_limits<float>::max());
 };