* adding flag to indicate that writer/reader has been initialized

* adding API to check if a reader or writer for specific topic/typename already exists

Author: Alexandru Kampmann

include/rtps/entities/Domain.h

@@ -47,6 +47,9 @@ namespace rtps{
         Writer* createWriter(Participant& part, const char* topicName, const char* typeName, bool reliable);
         Reader* createReader(Participant& part, const char* topicName, const char* typeName, bool reliable);
 
+        Writer* writerExists(Participant& part, const char* topicName, const char* typeName, bool reliable);
+        Reader* readerExists(Participant& part, const char* topicName, const char* typeName, bool reliable);
+
     private:
         ThreadPool m_threadPool;
         UdpDriver m_transport;
@@ -55,8 +58,8 @@ namespace rtps{
         ParticipantId_t m_nextParticipantId = PARTICIPANT_START_ID;
 
         std::array<StatelessWriter, Config::NUM_STATELESS_WRITERS> m_statelessWriters;
-        uint8_t m_numStatelessWriters = 0;
         std::array<StatelessReader, Config::NUM_STATELESS_READERS> m_statelessReaders;
+        uint8_t m_numStatelessWriters = 0;
         uint8_t m_numStatelessReaders = 0;
         std::array<StatefulReader, Config::NUM_STATEFUL_READERS> m_statefulReaders;
         uint8_t m_numStatefulReaders = 0;

include/rtps/entities/Reader.h

@@ -86,7 +86,11 @@ namespace rtps{
         virtual bool onNewHeartbeat(const SubmessageHeartbeat& msg, const GuidPrefix_t& remotePrefix) = 0;
         virtual bool addNewMatchedWriter(const WriterProxy& newProxy) = 0;
         virtual void removeWriter(const Guid& guid) = 0;
+        bool isInitialized(){
+        	return m_is_initialized_;
+        }
     protected:
+        bool m_is_initialized_ = false;
         virtual ~Reader() = default;
     };
 }

include/rtps/entities/StatefulReader.tpp

@@ -48,6 +48,7 @@ void StatefulReaderT<NetworkDriver>::init(const TopicData& attributes, NetworkDr
     m_transport = &driver;
     m_packetInfo.srcPort = attributes.unicastLocator.port;
     sys_mutex_new(&m_mutex);
+    m_is_initialized_ = true;
 }
 
 template <class NetworkDriver>

include/rtps/entities/StatefulWriter.tpp

@@ -78,6 +78,7 @@ bool StatefulWriterT<NetworkDriver>::init(TopicData attributes, TopicKind_t topi
     	 m_heartbeatThread = sys_thread_new("HBThread", hbFunctionJumppad, this, Config::HEARTBEAT_STACKSIZE, Config::THREAD_POOL_WRITER_PRIO);
     }
 
+    m_is_initialized_ = true;
     return true;
 }
 

include/rtps/entities/StatelessWriter.tpp

@@ -64,6 +64,7 @@ bool StatelessWriterT<NetworkDriver>::init(TopicData attributes, TopicKind_t top
     mp_threadPool = threadPool;
     m_transport = &driver;
 
+    m_is_initialized_ = true;
     return true;
 }
 

include/rtps/entities/Writer.h

@@ -45,7 +45,12 @@ namespace rtps{
         virtual void setAllChangesToUnsent() = 0;
         virtual void onNewAckNack(const SubmessageAckNack& msg, const GuidPrefix_t& sourceGuidPrefix) = 0;
 
+        bool isInitialized(){
+        	return m_is_initialized_;
+        }
+
     protected:
+        bool m_is_initialized_ = false;
         virtual ~Writer() = default;
     };
 }

src/discovery/SPDPAgent.cpp

@@ -188,6 +188,10 @@ bool SPDPAgent::addProxiesForBuiltInEndpoints(){
 		return false;
 	}
 
+	ip4_addr_t ip4addr = locator->getIp4Address();
+	const char* addr = ip4addr_ntoa(&ip4addr);
+	printf("Adding IPv4 Locator %s", addr);
+
 
     if (m_proxyDataBuffer.hasPublicationWriter()){
         const WriterProxy proxy{{m_proxyDataBuffer.m_guid.prefix, ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER}, *locator};

src/entities/Domain.cpp

@@ -172,7 +172,95 @@ void Domain::registerPort(const Participant& part){
     m_transport.createUdpConnection(getBuiltInUnicastPort(part.m_participantId));
 }
 
+rtps::Reader* Domain::readerExists(Participant& part, const char* topicName, const char* typeName, bool reliable){
+	if(reliable){
+		for(unsigned int i = 0; i < m_numStatefulReaders; i++){
+			if(m_statefulReaders[i].isInitialized()){
+				if(strncmp(m_statefulReaders[i].m_attributes.topicName, topicName, Config::MAX_TYPENAME_LENGTH) != 0){
+					continue;
+				}
+
+				if(strncmp(m_statefulReaders[i].m_attributes.typeName, typeName, Config::MAX_TYPENAME_LENGTH) != 0){
+					continue;
+				}
+
+#if DOMAIN_VERBOSE
+				printf("StatefulReader exists already [%s, %s]\n", topicName, typeName);
+#endif
+
+				return &m_statefulReaders[i];
+			}
+		}
+	}else{
+		for(unsigned int i = 0; i < m_numStatelessReaders; i++){
+			if(m_statelessReaders[i].isInitialized()){
+				if(strncmp(m_statelessReaders[i].m_attributes.topicName, topicName, Config::MAX_TYPENAME_LENGTH) != 0){
+					continue;
+				}
+
+				if(strncmp(m_statelessReaders[i].m_attributes.typeName, typeName, Config::MAX_TYPENAME_LENGTH) != 0){
+					continue;
+				}
+
+#if DOMAIN_VERBOSE
+				printf("StatelessReader exists [%s, %s]\n", topicName, typeName);
+#endif
+				return &m_statelessReaders[i];
+			}
+		}
+	}
+
+	return nullptr;
+}
+
+
+rtps::Writer* Domain::writerExists(Participant& part, const char* topicName, const char* typeName, bool reliable){
+	if(reliable){
+		for(unsigned int i = 0; i < m_numStatefulWriters; i++){
+			if(m_statefulWriters[i].isInitialized()){
+				if(strncmp(m_statefulWriters[i].m_attributes.topicName, topicName, Config::MAX_TYPENAME_LENGTH) != 0){
+					continue;
+				}
+
+				if(strncmp(m_statefulWriters[i].m_attributes.typeName, typeName, Config::MAX_TYPENAME_LENGTH) != 0){
+					continue;
+				}
+
+#if DOMAIN_VERBOSE
+				printf("StatefulWriter exists [%s, %s]\n", topicName, typeName);
+#endif
+
+				return &m_statefulWriters[i];
+			}
+		}
+	}else{
+		for(unsigned int i = 0; i < m_numStatelessWriters; i++){
+			if(m_statelessWriters[i].isInitialized()){
+				if(strncmp(m_statelessWriters[i].m_attributes.topicName, topicName, Config::MAX_TYPENAME_LENGTH) != 0){
+					continue;
+				}
+
+				if(strncmp(m_statelessWriters[i].m_attributes.typeName, typeName, Config::MAX_TYPENAME_LENGTH) != 0){
+					continue;
+				}
+
+#if DOMAIN_VERBOSE
+				printf("StatelessWriter exists [%s, %s]\n", topicName, typeName);
+#endif
+				return &m_statelessWriters[i];
+			}
+		}
+	}
+
+	return nullptr;
+}
+
 rtps::Writer* Domain::createWriter(Participant& part, const char* topicName, const char* typeName, bool reliable){
+#if DOMAIN_VERBOSE
+    printf("Creating writer[%s, %s]\n", topicName, typeName);
+#endif
+
+	// Check if there is enough capacity for more writers
     if((reliable && m_statefulWriters.size() <= m_numStatefulWriters) ||
        (!reliable && m_statelessWriters.size() <= m_numStatelessWriters) ||
 	    part.isWritersFull()){
@@ -215,10 +303,13 @@ rtps::Writer* Domain::createWriter(Participant& part, const char* topicName, con
 
 
 rtps::Reader* Domain::createReader(Participant& part, const char* topicName, const char* typeName, bool reliable){
-	if((reliable && m_statefulReaders.size() <= m_numStatefulReaders) ||
-       (!reliable && m_statelessReaders.size() <= m_numStatelessReaders) ||
-	    part.isReadersFull()){
-        return nullptr;
+#if DOMAIN_VERBOSE
+    printf("Creating reader[%s, %s]\n", topicName, typeName);
+#endif
+    if((reliable && m_statefulReaders.size() <= m_numStatefulReaders) ||
+    (!reliable && m_statelessReaders.size() <= m_numStatelessReaders) ||
+    part.isReadersFull()){
+    	return nullptr;
     }
 
     // TODO Distinguish WithKey and NoKey (Also changes EntityKind)

src/entities/StatelessReader.cpp

@@ -31,6 +31,7 @@ using rtps::StatelessReader;
 
 void StatelessReader::init(const TopicData& attributes){
     m_attributes = attributes;
+    m_is_initialized_ = true;
 }
 
 void StatelessReader::newChange(const ReaderCacheChange& cacheChange){