From 55e0d4760f9fa982697e937781fcea6bbabe4a80 Mon Sep 17 00:00:00 2001 From: Erik Hofman Date: Fri, 19 Mar 2021 13:51:58 +0100 Subject: [PATCH] Rename the SG_DDS class to SG_DDS_Topic and create a new SG_DDS class which can handle multiple SG_DDS_Topic classes with a single waitset. Add a unit test for DDS --- simgear/io/CMakeLists.txt | 1 + simgear/io/SGDataDistributionService.cxx | 161 ++++++++++++++++++----- simgear/io/SGDataDistributionService.hxx | 64 ++++++--- simgear/io/test_dds.cxx | 95 +++++++++++++ 4 files changed, 268 insertions(+), 53 deletions(-) create mode 100644 simgear/io/test_dds.cxx diff --git a/simgear/io/CMakeLists.txt b/simgear/io/CMakeLists.txt index a3ce2094..c0b9c54a 100644 --- a/simgear/io/CMakeLists.txt +++ b/simgear/io/CMakeLists.txt @@ -55,6 +55,7 @@ if (CycloneDDS_FOUND) set(SOURCES ${SOURCES} SGDataDistributionService.cxx ) + add_simgear_test(test_dds test_dds.cxx) endif (CycloneDDS_FOUND) simgear_component(io io "${SOURCES}" "${HEADERS}") diff --git a/simgear/io/SGDataDistributionService.cxx b/simgear/io/SGDataDistributionService.cxx index bc79ab41..90d20a4b 100644 --- a/simgear/io/SGDataDistributionService.cxx +++ b/simgear/io/SGDataDistributionService.cxx @@ -24,6 +24,8 @@ # include #endif +#include + #include #include @@ -31,40 +33,37 @@ #include "SGDataDistributionService.hxx" -#define FG_DDS_DOMAIN DDS_DOMAIN_DEFAULT - -SG_DDS::SG_DDS() +SG_DDS_Topic::SG_DDS_Topic() { - set_type( sgDDSType ); + set_type(sgDDSType); } - -SG_DDS::~SG_DDS() +SG_DDS_Topic::~SG_DDS_Topic() { this->close(); } + +SG_DDS_Topic::SG_DDS_Topic(const char *topic, const dds_topic_descriptor_t *desc, size_t size) +{ + set_type(sgDDSType); + setup(topic, desc, size); +} + // Set the paramaters which weren't available at creation time. void -SG_DDS::setup( const char *topic, const dds_topic_descriptor_t *desc, const size_t size) +SG_DDS_Topic::setup(const char *topic, const dds_topic_descriptor_t *desc, size_t size) { topic_name = topic; descriptor = desc; packet_size = size; } -void -SG_DDS::setup(const dds_topic_descriptor_t *desc, const size_t size) -{ - topic_name = desc->m_typename; - descriptor = desc; - packet_size = size; -} // If specified as a server (in direction) create a subscriber. // If specified as a client (out direction), create a publisher. bool -SG_DDS::open( SGProtocolDir direction ) +SG_DDS_Topic::open(SGProtocolDir direction) { set_dir(direction); @@ -77,8 +76,15 @@ SG_DDS::open( SGProtocolDir direction ) << dds_strretcode(-participant)); return false; } + shared_participant = false; } + return open(participant, direction); +} +bool +SG_DDS_Topic::open(dds_entity_t p, SGProtocolDir direction) +{ + participant = p; if (topic < 0) { topic = dds_create_topic(participant, descriptor, @@ -97,10 +103,10 @@ SG_DDS::open( SGProtocolDir direction ) dds_qset_reliability(qos, DDS_RELIABILITY_RELIABLE, DDS_MSECS(1)); dds_qset_history(qos, DDS_HISTORY_KEEP_LAST, 3); - reader = dds_create_reader(participant, topic, qos, NULL); - if (reader < 0) { + entry = dds_create_reader(participant, topic, qos, NULL); + if (entry < 0) { SG_LOG(SG_IO, SG_ALERT, "dds_create_reader: " - << dds_strretcode(-reader)); + << dds_strretcode(-entry)); return false; } @@ -111,14 +117,14 @@ SG_DDS::open( SGProtocolDir direction ) { dds_return_t rc; - writer = dds_create_writer(participant, topic, NULL, NULL); - if (writer < 0) { + entry = dds_create_writer(participant, topic, NULL, NULL); + if (entry < 0) { SG_LOG(SG_IO, SG_ALERT, "dds_create_writer: " - << dds_strretcode(-writer)); + << dds_strretcode(-entry)); return false; } - rc = dds_set_status_mask(writer, DDS_PUBLICATION_MATCHED_STATUS); + rc = dds_set_status_mask(entry, DDS_PUBLICATION_MATCHED_STATUS); if (rc != DDS_RETCODE_OK) { SG_LOG(SG_IO, SG_ALERT, "dds_set_status_mask: " << dds_strretcode(-rc)); @@ -133,16 +139,16 @@ SG_DDS::open( SGProtocolDir direction ) // read data from topic (server) // read a block of data of specified size int -SG_DDS::read( char *buf, int length ) +SG_DDS_Topic::read(char *buf, int length) { int result; - if (reader < 0 || length < (int)packet_size) { + if (entry < 0 || length < (int)packet_size) { return 0; } dds_sample_info_t info[1]; - result = dds_take(reader, (void**)&buf, info, 1, 1); + result = dds_take(entry, (void**)&buf, info, 1, 1); if(result < 0 || !info[0].valid_data) { errno = -result; @@ -157,17 +163,17 @@ SG_DDS::read( char *buf, int length ) // write data to topic (client) int -SG_DDS::write( const char *buf, const int length ) +SG_DDS_Topic::write(const char *buf, const int length) { int result; - if (writer < 0 || length < (int)packet_size) { + if (entry < 0 || length < (int)packet_size) { return 0; } if (!status) { - dds_return_t rc = dds_get_status_changes(writer, &status); + dds_return_t rc = dds_get_status_changes(entry, &status); if (rc != DDS_RETCODE_OK) { SG_LOG(SG_IO, SG_ALERT, "dds_get_status_changes: " << dds_strretcode(-rc)); @@ -180,7 +186,7 @@ SG_DDS::write( const char *buf, const int length ) } } - result = dds_write(writer, buf); + result = dds_write(entry, buf); if(result != DDS_RETCODE_OK) { errno = -result; @@ -198,15 +204,102 @@ SG_DDS::write( const char *buf, const int length ) // close the port bool -SG_DDS::close() +SG_DDS_Topic::close() { - dds_delete(participant); + if (!shared_participant) + dds_delete(participant); - reader = -1; - writer = -1; - topic = -1; + shared_participant = true; participant = -1; + topic = -1; + entry = -1; return true; } + +// participant +SG_DDS::SG_DDS(dds_domainid_t d) : + domain(d) +{ + if (participant < 0) + { + participant = dds_create_participant(domain, NULL, NULL); + + if (participant < 0) { + SG_LOG(SG_IO, SG_ALERT, "dds_create_participant: " + << dds_strretcode(-participant)); + return; + } + } + + waitset = dds_create_waitset(participant); +} + +SG_DDS::~SG_DDS() +{ + this->close(); + dds_delete(participant); +} + +bool +SG_DDS::add(SG_DDS_Topic *topic, const SGProtocolDir d) +{ + bool result = topic->open(participant, d); + if (result) + { + if (d == SG_IO_OUT || d == SG_IO_BI) + writers.push_back(topic); + else if (d == SG_IO_IN || d == SG_IO_BI) + { + readers.push_back(topic); + + dds_entity_t entry = topic->get(); + dds_entity_t rdcond = dds_create_readcondition(entry, + DDS_NOT_READ_SAMPLE_STATE); + int status = dds_waitset_attach(waitset, rdcond, entry); + if (status < 0) + SG_LOG(SG_IO, SG_ALERT, "ds_waitset_attach: " + << dds_strretcode(-status)); + } + } + + return result; +} + +bool +SG_DDS::close() +{ + for (auto it : readers) + it->close(); + + for (auto it : writers) + it->close(); + + readers.clear(); + writers.clear(); + + return true; +} + +bool +SG_DDS::wait(float dt) +{ + size_t num = readers.size(); + if (!num) return false; + + dds_duration_t timeout = DDS_INFINITY; + dds_attach_t results[num]; + + if (dt > 0.0) + timeout = dt*1e9f * DDS_NSECS_IN_SEC; + + int status = dds_waitset_wait(waitset, results, num, timeout); + if (status < 0) { + SG_LOG(SG_IO, SG_ALERT, "dds_waitset_wait: " + << dds_strretcode(-status)); + return false; + } + + return true; +} diff --git a/simgear/io/SGDataDistributionService.hxx b/simgear/io/SGDataDistributionService.hxx index 4e2afad8..244c8cb0 100644 --- a/simgear/io/SGDataDistributionService.hxx +++ b/simgear/io/SGDataDistributionService.hxx @@ -24,21 +24,22 @@ // $Id$ -#ifndef _SG_DDS_HXX -#define _SG_DDS_HXX +#ifndef _SG_DDS_Topic_HXX +#define _SG_DDS_Topic_HXX #include +#include + #include #include #include #include +#define FG_DDS_DOMAIN DDS_DOMAIN_DEFAULT -/** - * A socket I/O class based on SGIOChannel. - */ -class SG_DDS : public SGIOChannel { +// Data Distribution Service (DDS) class based on SGIOChannel. +class SG_DDS_Topic : public SGIOChannel { private: std::string topic_name; @@ -47,22 +48,20 @@ private: uint32_t status = 0; + bool shared_participant = true; dds_entity_t participant = -1; dds_entity_t topic = -1; - dds_entity_t writer = -1; - dds_entity_t reader = -1; + dds_entity_t entry = -1; public: - /** Create an instance of SG_DDS. */ - SG_DDS(); + /** Create an instance of SG_DDS_Topic. */ + SG_DDS_Topic(); + + SG_DDS_Topic(const char* topic, const dds_topic_descriptor_t *desc, size_t size); /** Destructor */ - ~SG_DDS(); - - // Set the paramaters which weren't available at creation time and use - // the descrtors typename as topic name. - void setup(const dds_topic_descriptor_t *desc, size_t size); + ~SG_DDS_Topic(); // Set the paramaters which weren't available at creation time and use // a custom topic name. @@ -70,17 +69,44 @@ public: // If specified as a server start a publishing participant. // If specified as a client start a subscribing participant. - bool open( const SGProtocolDir d ); + bool open(const SGProtocolDir d); + + // If specified as a server start publishing to a participant. + // If specified as a client start subscribing to a participant. + bool open(dds_entity_t p, const SGProtocolDir d); // read data from the topic. - int read( char *buf, int length ); + int read(char *buf, int length); // write data to the topic. - int write( const char *buf, const int length ); + int write(const char *buf, const int length); // close the participant. bool close(); + + dds_entity_t get() { return entry; } +}; + +// a class to manage multiple DDS topics +class SG_DDS { +private: + dds_entity_t participant = -1; + dds_domainid_t domain = FG_DDS_DOMAIN; + + dds_entity_t waitset = -1; + + std::vector readers; + std::vector writers; + +public: + SG_DDS(dds_domainid_t d = FG_DDS_DOMAIN); + ~SG_DDS(); + + bool add(SG_DDS_Topic *topic, const SGProtocolDir d); + bool close(); + + bool wait(float dt = 0.0f); }; -#endif // _SG_DDS_HXX +#endif // _SG_DDS_Topic_HXX diff --git a/simgear/io/test_dds.cxx b/simgear/io/test_dds.cxx new file mode 100644 index 00000000..d3696c23 --- /dev/null +++ b/simgear/io/test_dds.cxx @@ -0,0 +1,95 @@ +//////////////////////////////////////////////////////////////////////// +// Test harness. +//////////////////////////////////////////////////////////////////////// + +#include +#include + +#include +#include + +#include "untar.hxx" + +#include +#include +#include +#include + + +using std::cout; +using std::cerr; +using std::endl; + +using namespace simgear; + +typedef struct DDSMessageData_Msg +{ + int32_t userID = -1; + char * message = nullptr; +} DDSMessageData_Msg; + +extern const dds_topic_descriptor_t DDSMessageData_Msg_desc; + +#define DDSMessageData_Msg__alloc() \ +((DDSMessageData_Msg*) dds_alloc (sizeof (DDSMessageData_Msg))); + +#define DDSMessageData_Msg_free(d,o) \ +dds_sample_free ((d), &DDSMessageData_Msg_desc, (o)) + +static const dds_key_descriptor_t DDSMessageData_Msg_keys[1] = +{ + { "userID", 0 } +}; + +static const uint32_t DDSMessageData_Msg_ops [] = +{ + DDS_OP_ADR | DDS_OP_TYPE_4BY | DDS_OP_FLAG_SGN | DDS_OP_FLAG_KEY, offsetof (DDSMessageData_Msg, userID), + DDS_OP_ADR | DDS_OP_TYPE_STR, offsetof (DDSMessageData_Msg, message), + DDS_OP_RTS +}; + +const dds_topic_descriptor_t DDSMessageData_Msg_desc = +{ + sizeof (DDSMessageData_Msg), + sizeof (char *), + DDS_TOPIC_FIXED_KEY | DDS_TOPIC_NO_OPTIMIZE, + 1u, + "DDSMessageData::Msg", + DDSMessageData_Msg_keys, + 3, + DDSMessageData_Msg_ops, + "" +}; + +void testSendReceive() +{ + SG_DDS participant; + + SG_DDS_Topic *writer = new SG_DDS_Topic(); + SG_DDS_Topic *reader = new SG_DDS_Topic(); + + writer->setup("DDS", &DDSMessageData_Msg_desc, sizeof(DDSMessageData_Msg)); + reader->setup("DDS", &DDSMessageData_Msg_desc, sizeof(DDSMessageData_Msg)); + + participant.add(writer, SG_IO_OUT); + participant.add(reader, SG_IO_IN); + + DDSMessageData_Msg out = { 0, (char*)"testSendReceive" }; + DDSMessageData_Msg in; + + writer->write((char*)&out, sizeof(out)); + + participant.wait(); + reader->read((char*)&in, sizeof(in)); + + SG_VERIFY(in.userID == out.userID); + SG_VERIFY(!strcmp(in.message, out.message)); +} + +int main(int ac, char ** av) +{ + testSendReceive(); + + std::cout << "all tests passed" << std::endl; + return 0; +}