Feature/endpoint deletion (#17)

* squashed commits from endpoint deletion, multiple callback features

* updating r5 config

Co-authored-by: Alexandru Kampmann <kampmann@embedded.rwth-aachen.de>

Author: akampmann

include/rtps/common/types.h

@@ -158,6 +158,10 @@ struct SequenceNumber_t {
     return high < other.high || (high == other.high && low < other.low);
   }
 
+  bool operator>(const SequenceNumber_t &other) const {
+    return high > other.high || (high == other.high && low > other.low);
+  }
+
   bool operator<=(const SequenceNumber_t &other) const {
     return *this == other || *this < other;
   }
@@ -177,7 +181,11 @@ struct SequenceNumber_t {
   }
 };
 
-const uint32_t SNS_NUM_BITS = 32;
+#define SNS_MAX_NUM_BITS 32
+#define SNS_NUM_BYTES (SNS_MAX_NUM_BITS / 8)
+static_assert(!(SNS_MAX_NUM_BITS % 32) && SNS_MAX_NUM_BITS != 0,
+              "SNS_MAX_NUM_BITS must be multiple of 32");
+
 struct SequenceNumberSet {
 
   SequenceNumberSet() = default;
@@ -186,12 +194,12 @@ struct SequenceNumberSet {
 
   SequenceNumber_t base = {0, 0};
   // Cannot be static because of packed
-  uint32_t numBits = SNS_NUM_BITS;
-  std::array<uint32_t, 1> bitMap{};
+  uint32_t numBits = 0;
+  std::array<uint32_t, (SNS_MAX_NUM_BITS / 32)> bitMap{};
 
   // We only need 1 byte because atm we don't store packets.
   bool isSet(uint32_t bit) const {
-    if (bit >= SNS_NUM_BITS) {
+    if (bit >= SNS_MAX_NUM_BITS) {
       return true;
     }
     const auto bucket = static_cast<uint8_t>(bit / 32);

include/rtps/config_r5.h

@@ -29,6 +29,8 @@ const uint8_t NUM_READER_PROXIES_PER_WRITER = 60;
 const uint8_t MAX_NUM_UNMATCHED_REMOTE_WRITERS = 150;
 const uint8_t MAX_NUM_UNMATCHED_REMOTE_READERS = 150;
 
+const uint8_t MAX_NUM_READER_CALLBACKS = 5;
+
 const uint8_t HISTORY_SIZE_STATELESS = 64;
 const uint8_t HISTORY_SIZE_STATEFUL = 64;
 
@@ -37,7 +39,7 @@ const uint8_t MAX_TOPICNAME_LENGTH = 64;
 
 const int HEARTBEAT_STACKSIZE = 1200;          // byte
 const int THREAD_POOL_WRITER_STACKSIZE = 1100; // byte
-const int THREAD_POOL_READER_STACKSIZE = 1600; // byte
+const int THREAD_POOL_READER_STACKSIZE = 3600; // byte
 const uint16_t SPDP_WRITER_STACKSIZE = 550;    // byte
 
 const uint16_t SF_WRITER_HB_PERIOD_MS = 4000;
@@ -48,8 +50,8 @@ const uint8_t SPDP_CYCLECOUNT_HEARTBEAT =
 const uint8_t SPDP_MAX_NUMBER_FOUND_PARTICIPANTS = 100;
 const uint8_t SPDP_MAX_NUM_LOCATORS = 1;
 const Duration_t SPDP_DEFAULT_REMOTE_LEASE_DURATION = {
-    100, 0}; // Default lease duration for remote participants, usually
-             // overwritten by remote info
+    5, 0}; // Default lease duration for remote participants, usually
+           // overwritten by remote info
 const Duration_t SPDP_MAX_REMOTE_LEASE_DURATION = {
     180,
     0}; // Absolute maximum lease duration, ignoring remote participant info

include/rtps/discovery/SEDPAgent.h

@@ -40,17 +40,22 @@ class SEDPAgent {
   void init(Participant &part, const BuiltInEndpoints &endpoints);
   void addWriter(Writer &writer);
   void addReader(Reader &reader);
+  bool deleteReader(Reader *reader);
+  bool deleteWriter(Writer *reader);
+
   void registerOnNewPublisherMatchedCallback(void (*callback)(void *arg),
                                              void *args);
   void registerOnNewSubscriberMatchedCallback(void (*callback)(void *arg),
                                               void *args);
   void removeUnmatchedEntitiesOfParticipant(const GuidPrefix_t &guidPrefix);
+  void removeUnmatchedEntity(const Guid_t &guid);
+
   uint32_t getNumRemoteUnmatchedReaders();
   uint32_t getNumRemoteUnmatchedWriters();
 
 protected: // For testing purposes
-  void onNewPublisher(const TopicData &writerData);
-  void onNewSubscriber(const TopicData &writerData);
+  void handlePublisherReaderMessage(const TopicData &writerData);
+  void handleSubscriptionReaderMessage(const TopicData &writerData);
 
 private:
   Participant *m_part;
@@ -74,18 +79,38 @@ class SEDPAgent {
   void tryMatchUnmatchedEndpoints();
   void addUnmatchedRemoteWriter(const TopicData &writerData);
   void addUnmatchedRemoteReader(const TopicData &readerData);
+  void addUnmatchedRemoteWriter(const TopicDataCompressed &writerData);
+  void addUnmatchedRemoteReader(const TopicDataCompressed &readerData);
+
+  void handleRemoteEndpointDeletion(const TopicData &topic);
 
   void (*mfp_onNewPublisherCallback)(void *arg) = nullptr;
   void *m_onNewPublisherArgs = nullptr;
   void (*mfp_onNewSubscriberCallback)(void *arg) = nullptr;
   void *m_onNewSubscriberArgs = nullptr;
 
-  static void receiveCallbackPublisher(void *callee,
-                                       const ReaderCacheChange &cacheChange);
-  static void receiveCallbackSubscriber(void *callee,
+  static void jumppadPublisherReader(void *callee,
+                                     const ReaderCacheChange &cacheChange);
+  static void jumppadSubscriptionReader(void *callee,
                                         const ReaderCacheChange &cacheChange);
-  void onNewPublisher(const ReaderCacheChange &change);
-  void onNewSubscriber(const ReaderCacheChange &change);
+
+  static void jumppadTakeProxyOfDisposedReader(const Reader *reader,
+                                               const WriterProxy &proxy,
+                                               void *arg);
+  static void jumppadTakeProxyOfDisposedWriter(const Writer *writer,
+                                               const ReaderProxy &proxy,
+                                               void *arg);
+
+  void handlePublisherReaderMessage(const ReaderCacheChange &change);
+  void handleSubscriptionReaderMessage(const ReaderCacheChange &change);
+
+  template <typename A> bool deleteEndpoint(A *endpoint, Writer *sedp_endpoint);
+
+  template <typename A>
+  bool announceEndpointDeletion(A *local_endpoint, Writer *sedp_endpoint);
+
+  template <typename A>
+  bool disposeEndpointInSEDPHistory(A *local_endpoint, Writer *sedp_writer);
 };
 } // namespace rtps
 

include/rtps/discovery/TopicData.h

@@ -48,6 +48,13 @@ struct TopicData {
   FullLengthLocator unicastLocator;
   FullLengthLocator multicastLocator;
 
+  uint8_t statusInfo;
+  bool statusInfoValid;
+  // Use Case: Remotes communicates id of deleted endpoint through key_hash
+  // parameter
+  EntityId_t entityIdFromKeyHash;
+  bool entityIdFromKeyHashValid;
+
   TopicData()
       : endpointGuid(GUID_UNKNOWN), typeName{'\0'}, topicName{'\0'},
         reliabilityKind(ReliabilityKind_t::BEST_EFFORT),
@@ -69,14 +76,16 @@ struct TopicData {
 
   bool readFromUcdrBuffer(ucdrBuffer &buffer);
   bool serializeIntoUcdrBuffer(ucdrBuffer &buffer) const;
+
+  bool isDisposedFlagSet() const;
+  bool isUnregisteredFlagSet() const;
 };
 
 struct TopicDataCompressed {
   Guid_t endpointGuid;
   std::size_t topicHash;
   std::size_t typeHash;
-  ReliabilityKind_t reliabilityKind;
-  DurabilityKind_t durabilityKind;
+  bool is_reliable;
   LocatorIPv4 unicastLocator;
   LocatorIPv4 multicastLocator;
 
@@ -86,8 +95,9 @@ struct TopicDataCompressed {
     topicHash =
         hashCharArray(topic_data.topicName, Config::MAX_TOPICNAME_LENGTH);
     typeHash = hashCharArray(topic_data.typeName, Config::MAX_TYPENAME_LENGTH);
-    reliabilityKind = topic_data.reliabilityKind;
-    durabilityKind = topic_data.durabilityKind;
+    is_reliable = (topic_data.reliabilityKind == ReliabilityKind_t::RELIABLE)
+                      ? true
+                      : false;
     unicastLocator = topic_data.unicastLocator;
     multicastLocator = topic_data.multicastLocator;
   }

include/rtps/entities/Domain.h

@@ -57,6 +57,11 @@ class Domain {
   Reader *readerExists(Participant &part, const char *topicName,
                        const char *typeName, bool reliable);
 
+  bool deleteWriter(Participant &part, Writer *writer);
+  bool deleteReader(Participant &part, Reader *reader);
+
+  void printInfo();
+
 private:
   friend class SizeInspector;
   ThreadPool m_threadPool;
@@ -67,14 +72,19 @@ class Domain {
 
   std::array<StatelessWriter, Config::NUM_STATELESS_WRITERS> m_statelessWriters;
   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;
   std::array<StatefulWriter, Config::NUM_STATEFUL_WRITERS> m_statefulWriters;
-  uint8_t m_numStatefulWriters = 0;
+  template <typename A, typename B> B *getNextUnusedEndpoint(A &a) {
+    for (unsigned int i = 0; i < a.size(); i++) {
+      if (!a[i].isInitialized()) {
+        return &(a[i]);
+      }
+    }
+    return nullptr;
+  }
 
   bool m_initComplete = false;
+  sys_mutex_t m_mutex;
 
   void receiveCallback(const PacketInfo &packet);
   GuidPrefix_t generateGuidPrefix(ParticipantId_t id) const;

include/rtps/entities/Participant.h

@@ -68,30 +68,33 @@ class Participant {
   //! Not-thread-safe function to add a writer
   Writer *addWriter(Writer *writer);
   bool isWritersFull();
+  bool deleteWriter(Writer *writer);
 
   //! Not-thread-safe function to add a reader
   Reader *addReader(Reader *reader);
   bool isReadersFull();
+  bool deleteReader(Reader *reader);
 
   //! (Probably) Thread safe if writers cannot be removed
-  Writer *getWriter(EntityId_t id) const;
-  Writer *getMatchingWriter(const TopicData &topicData) const;
-  Writer *getMatchingWriter(const TopicDataCompressed &topicData) const;
+  Writer *getWriter(EntityId_t id);
+  Writer *getMatchingWriter(const TopicData &topicData);
+  Writer *getMatchingWriter(const TopicDataCompressed &topicData);
 
   //! (Probably) Thread safe if readers cannot be removed
-  Reader *getReader(EntityId_t id) const;
-  Reader *getReaderByWriterId(const Guid_t &guid) const;
-  Reader *getMatchingReader(const TopicData &topicData) const;
-  Reader *getMatchingReader(const TopicDataCompressed &topicData) const;
+  Reader *getReader(EntityId_t id);
+  Reader *getReaderByWriterId(const Guid_t &guid);
+  Reader *getMatchingReader(const TopicData &topicData);
+  Reader *getMatchingReader(const TopicDataCompressed &topicData);
 
   bool addNewRemoteParticipant(const ParticipantProxyData &remotePart);
   bool removeRemoteParticipant(const GuidPrefix_t &prefix);
-  void removeAllEntitiesOfParticipant(const GuidPrefix_t &prefix);
+  void removeAllProxiesOfParticipant(const GuidPrefix_t &prefix);
+  void removeProxyFromAllEndpoints(const Guid_t &guid);
+
   const ParticipantProxyData *findRemoteParticipant(const GuidPrefix_t &prefix);
   void refreshRemoteParticipantLiveliness(const GuidPrefix_t &prefix);
   uint32_t getRemoteParticipantCount();
   MessageReceiver *getMessageReceiver();
-  void addHeartbeat(GuidPrefix_t sourceGuidPrefix);
   bool checkAndResetHeartbeats();
 
   bool hasReaderWithMulticastLocator(ip4_addr_t address);
@@ -100,16 +103,17 @@ class Participant {
   void newMessage(const uint8_t *data, DataSize_t size);
 
   SPDPAgent &getSPDPAgent();
+  void printInfo();
 
 private:
   friend class SizeInspector;
   MessageReceiver m_receiver;
   bool m_hasBuilInEndpoints = false;
   std::array<uint8_t, 3> m_nextUserEntityId{{0, 0, 1}};
-  std::array<Writer *, Config::NUM_WRITERS_PER_PARTICIPANT> m_writers{};
-  uint8_t m_numWriters = 0;
-  std::array<Reader *, Config::NUM_READERS_PER_PARTICIPANT> m_readers{};
-  uint8_t m_numReaders = 0;
+  std::array<Writer *, Config::NUM_WRITERS_PER_PARTICIPANT> m_writers = {
+      nullptr};
+  std::array<Reader *, Config::NUM_READERS_PER_PARTICIPANT> m_readers = {
+      nullptr};
 
   sys_mutex_t m_mutex;
   MemoryPool<ParticipantProxyData, Config::SPDP_MAX_NUMBER_FOUND_PARTICIPANTS>

include/rtps/entities/Reader.h

@@ -36,6 +36,7 @@ Author: i11 - Embedded Software, RWTH Aachen University
 namespace rtps {
 
 struct SubmessageHeartbeat;
+struct SubmessageGap;
 
 class ReaderCacheChange {
 private:
@@ -79,31 +80,65 @@ typedef void (*ddsReaderCallback_fp)(void *callee,
 
 class Reader {
 public:
+  using callbackFunction_t = void (*)(void *, const ReaderCacheChange &);
+  using callbackIdentifier_t = uint32_t;
+
   TopicData m_attributes;
   virtual void newChange(const ReaderCacheChange &cacheChange) = 0;
-  virtual void registerCallback(ddsReaderCallback_fp cb, void *callee) = 0;
+  virtual callbackIdentifier_t registerCallback(callbackFunction_t cb,
+                                                void *arg);
+  virtual bool removeCallback(callbackIdentifier_t identifier);
+  uint8_t getNumCallbacks();
+
   virtual bool onNewHeartbeat(const SubmessageHeartbeat &msg,
                               const GuidPrefix_t &remotePrefix) = 0;
+  virtual bool onNewGapMessage(const SubmessageGap &msg,
+                               const GuidPrefix_t &remotePrefix) = 0;
   virtual bool addNewMatchedWriter(const WriterProxy &newProxy) = 0;
-  virtual void removeWriter(const Guid_t &guid) = 0;
-  virtual void removeWriterOfParticipant(const GuidPrefix_t &guidPrefix) = 0;
+  virtual bool removeProxy(const Guid_t &guid);
+  virtual void removeAllProxiesOfParticipant(const GuidPrefix_t &guidPrefix);
   bool isInitialized() { return m_is_initialized_; }
+  virtual void reset();
+  bool isProxy(const Guid_t &guid);
+  WriterProxy *getProxy(Guid_t guid);
+  uint32_t getProxiesCount();
 
-  bool knowWriterId(const Guid_t &guid) {
-    for (const auto &proxy : m_proxies) {
-      if (proxy.remoteWriterGuid.operator==(guid)) {
-        return true;
-      }
-    }
-    return false;
-  }
+  void setSEDPSequenceNumber(const SequenceNumber_t &sn);
+  const SequenceNumber_t &getSEDPSequenceNumber();
+
+  using dumpProxyCallback = void (*)(const Reader *reader, const WriterProxy &,
+                                     void *arg);
 
-  uint32_t getNumMatchedWriters() { return m_proxies.getSize(); }
+  //! Dangerous, only
+  int dumpAllProxies(dumpProxyCallback target, void *arg);
 
 protected:
+  void executeCallbacks(const ReaderCacheChange &cacheChange);
+  bool initMutex();
+
+  SequenceNumber_t m_sedp_sequence_number;
+
   bool m_is_initialized_ = false;
   virtual ~Reader() = default;
   MemoryPool<WriterProxy, Config::NUM_WRITER_PROXIES_PER_READER> m_proxies;
+
+  callbackIdentifier_t m_callback_identifier = 1;
+
+  uint8_t m_callback_count = 0;
+  using callbackElement_t = struct {
+    callbackFunction_t function = nullptr;
+    void *arg = nullptr;
+    callbackIdentifier_t identifier;
+  };
+
+  std::array<callbackElement_t, Config::MAX_NUM_READER_CALLBACKS> m_callbacks =
+      {nullptr};
+
+  // Guards manipulation of the proxies array
+  sys_mutex_t m_proxies_mutex = nullptr;
+
+  // Guards manipulation of callback array
+  sys_mutex_t m_callback_mutex = nullptr;
 };
 } // namespace rtps