Feature/multicast-fastdds-liveliness (#4)

* restore compatibility with Fast-DDS 2.3.1
* add multicast support
* add liveliness support (remove dead remote participants)
* add ref to embeddedRTPS-Linux

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

Author: 3 people

.gitignore

@@ -5,3 +5,4 @@ cmake-build-debug
 cmake-build-release
 thirdparty/Micro-CDR/CMakeFiles
 include/rtps/config.h
+CMakeFiles

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "thirdparty/googletest"]
+	path = thirdparty/googletest
+	url = https://github.com/google/googletest.git

Readme.md

@@ -10,11 +10,11 @@ More information is provided in our ITSC'2019 publication [1], which we kindly a
 
 **Discovery** Simple Participant Discovery Protocol (SPDP) as well as Simple Endpoint Discovery Protocol (SEDP) are implemented.
 
-**Interoperability** We have successfully tested interoperability with eProsima FastRTPS v1.8.0.
+**Interoperability** We have sucessfully tested interoperability with **FastDDS 2.3.1**
 
 **QoS Policies** Both reliable as well as best-effort endpoints are implemented.
 
-**UDP Multicast** Our implementation currently does not make use of UDP multicast communication. This can lead to sub-optimal performance.
+**UDP Multicast** Our implementation supports multicast locators.
 
 **Message Size** Message size is currently limited by lwIP buffer size and are not split up among multiple buffers
 
@@ -34,6 +34,12 @@ The following repository provides code examples for running embeddedRTPS on the
 
 - [STM32F767ZI](https://github.com/embedded-software-laboratory/embeddedRTPS-STM32)
 
+#### Runnig on Linux & Unittests
+
+The following repository allow to compile and run embeddedRTPS on Linux for development purposes. This repository also contains unittests for embeddedRTPS.
+
+- [embeddedRTPS-linux](https://github.com/embedded-software-laboratory/embeddedRTPS-Linux)
+
 ### Third Party Libraries
 
 embeddedRTPS makes use of the following third party libraries:
@@ -47,7 +53,7 @@ Round-trip-times (RTT) for different platforms and packet sizes are depicted in
 
 **Table 1** 2x Infineon Aurix TC277 running embeddedRTPS. 
 
-**Table 2** STM32F7 running embeddedRTPS connected to an Intel NUC running eProsima FastRTPS.
+**Table 2** STM32F7 running embeddedRTPS connected to an Intel NUC running eProsima FastRTPS 1.8.0.
 
 <img src="https://raw.githubusercontent.com/embedded-software-laboratory/embeddedRTPS/master/media/performance_rtt.png" width="60%">
 

include/rtps/common/Locator.h

@@ -87,6 +87,13 @@ struct Locator {
                              address[15]);
   }
 
+  bool isSameAddress(ip4_addr_t *address) {
+    ip4_addr_t ownaddress = getIp4Address();
+    return ip4_addr_cmp(&ownaddress, address);
+  }
+
+  inline uint32_t getLocatorPort() { return static_cast<Ip4Port_t>(port); }
+
   inline bool isSameSubnet() const {
     return UdpDriver::isSameSubnet(getIp4Address());
   }
@@ -109,7 +116,8 @@ inline Locator getUserUnicastLocator(ParticipantId_t participantId) {
       Config::IP_ADDRESS[3], getUserUnicastPort(participantId));
 }
 
-inline Locator getUserMulticastLocator() {
+inline Locator getUserMulticastLocator() { // this would be a unicastaddress, as
+                                           // defined in config
   return Locator::createUDPv4Locator(
       Config::IP_ADDRESS[0], Config::IP_ADDRESS[1], Config::IP_ADDRESS[2],
       Config::IP_ADDRESS[3], getUserMulticastPort());

include/rtps/common/types.h

@@ -77,6 +77,13 @@ enum class ReliabilityKind_t : uint32_t {
   RELIABLE = 2 // Specification says 3 but eprosima sends 2
 };
 
+enum class DurabilityKind_t : uint32_t {
+  VOLATILE = 0,
+  TRANSIENT_LOCAL = 1,
+  TRANSIENT = 2,
+  PERSISTENT = 3
+};
+
 struct GuidPrefix_t {
   std::array<uint8_t, 12> id;
 
@@ -97,13 +104,24 @@ struct EntityId_t {
   bool operator!=(const EntityId_t &other) const { return !(*this == other); }
 };
 
-struct Guid {
+struct Guid_t {
   GuidPrefix_t prefix;
   EntityId_t entityId;
 
-  bool operator==(const Guid &other) const {
+  bool operator==(const Guid_t &other) const {
     return this->prefix == other.prefix && this->entityId == other.entityId;
   }
+
+  static uint32_t sum(const Guid_t &other) {
+    uint32_t ret = 0;
+    for (const auto &i : other.prefix.id) {
+      ret += i;
+    }
+    for (const auto &i : other.entityId.entityKey) {
+      ret += i;
+    }
+    return ret;
+  }
 };
 
 // Described as long but there wasn't any definition. Other than 32 bit does not
@@ -240,6 +258,7 @@ const EntityId_t ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER = {
     {00, 02, 00}, EntityKind_t::BUILD_IN_READER_WITH_KEY};
 
 const GuidPrefix_t GUIDPREFIX_UNKNOWN{};
+const Guid_t GUID_UNKNOWN{};
 
 const ParticipantId_t PARTICIPANT_ID_INVALID = -1;
 

include/rtps/config_aurix.h

@@ -31,6 +31,9 @@ namespace rtps {
 
 #define IS_LITTLE_ENDIAN 1
 
+// define only if using FreeRTOS
+#define OS_IS_FREERTOS
+
 namespace Config {
 const VendorId_t VENDOR_ID = {13, 37};
 const std::array<uint8_t, 4> IP_ADDRESS = {192, 168, 0, 42};
@@ -59,6 +62,8 @@ const uint16_t SPDP_WRITER_STACKSIZE = 550;    // byte
 
 const uint16_t SF_WRITER_HB_PERIOD_MS = 4000;
 const uint16_t SPDP_RESEND_PERIOD_MS = 10000;
+const uint8_t SPDP_CYCLECOUNT_HEARTBEAT =
+    2; // skip x SPDP rounds before checking liveliness
 const uint8_t SPDP_WRITER_PRIO = 3;
 const uint8_t SPDP_MAX_NUMBER_FOUND_PARTICIPANTS = 5;
 const uint8_t SPDP_MAX_NUM_LOCATORS = 5;

include/rtps/config_desktop.h

@@ -34,7 +34,7 @@ namespace rtps {
 namespace Config {
 const VendorId_t VENDOR_ID = {13, 37};
 const std::array<uint8_t, 4> IP_ADDRESS = {
-    192, 168, 0, 66}; // Needs to be set in lwipcfg.h too.
+   192, 168, 1, 2}; // Needs to be set in lwipcfg.h too.
 const GuidPrefix_t BASE_GUID_PREFIX{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 9};
 
 const uint8_t DOMAIN_ID = 0; // 230 possible with UDP
@@ -64,12 +64,19 @@ const int THREAD_POOL_WRITER_STACKSIZE = 1100; // byte
 const int THREAD_POOL_READER_STACKSIZE = 1600; // byte
 const uint16_t SPDP_WRITER_STACKSIZE = 550;    // byte
 
-const uint16_t SF_WRITER_HB_PERIOD_MS = 4000;
+const uint16_t SF_WRITER_HB_PERIOD_MS = 500;
 const uint16_t SPDP_RESEND_PERIOD_MS = 10000;
+const uint8_t SPDP_CYCLECOUNT_HEARTBEAT =
+    2; // skip x SPDP rounds before checking liveliness
 const uint8_t SPDP_WRITER_PRIO = 3;
 const uint8_t SPDP_MAX_NUMBER_FOUND_PARTICIPANTS = 5;
 const uint8_t SPDP_MAX_NUM_LOCATORS = 5;
-const Duration_t SPDP_LEASE_DURATION = {100, 0};
+const Duration_t SPDP_DEFAULT_REMOTE_LEASE_DURATION = {
+    100, 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
 
 const int MAX_NUM_UDP_CONNECTIONS = 10;
 

include/rtps/config_r5.h

@@ -0,0 +1,74 @@
+/*
+ *
+ * Author: Andreas Wüstenberg (andreas.wuestenberg@rwth-aachen.de)
+ */
+
+#ifndef RTPS_CONFIG_H
+#define RTPS_CONFIG_H
+
+#include "rtps/common/types.h"
+
+namespace rtps {
+
+#define IS_LITTLE_ENDIAN 1
+
+namespace Config {
+const VendorId_t VENDOR_ID = {13, 37};
+const std::array<uint8_t, 4> IP_ADDRESS = {
+    192, 168, 1, 10}; // Needs to be set in lwipcfg.h too.
+const GuidPrefix_t BASE_GUID_PREFIX{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 12};
+
+const uint8_t DOMAIN_ID = 0; // 230 possible with UDP
+const uint8_t NUM_STATELESS_WRITERS = 64;
+const uint8_t NUM_STATELESS_READERS = 64;
+const uint8_t NUM_STATEFUL_READERS = 4;
+const uint8_t NUM_STATEFUL_WRITERS = 4;
+const uint8_t MAX_NUM_PARTICIPANTS = 1;
+const uint8_t NUM_WRITERS_PER_PARTICIPANT =
+    64; // 3 will be reserved for SPDP & SEDP
+const uint8_t NUM_READERS_PER_PARTICIPANT =
+    64; // 3 will be reserved for SPDP & SEDP
+const uint8_t NUM_WRITER_PROXIES_PER_READER = 30;
+const uint8_t NUM_READER_PROXIES_PER_WRITER = 30;
+
+const uint8_t HISTORY_SIZE = 64;
+
+const uint8_t MAX_TYPENAME_LENGTH = 64;
+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 uint16_t SPDP_WRITER_STACKSIZE = 550;    // byte
+
+const uint16_t SF_WRITER_HB_PERIOD_MS = 4000;
+const uint16_t SPDP_RESEND_PERIOD_MS = 1000;
+const uint8_t SPDP_WRITER_PRIO = 3;
+const uint8_t SPDP_CYCLECOUNT_HEARTBEAT =
+    2; // Every X*SPDP_RESEND_PERIOD_MS, check for missing heartbeats
+const uint8_t SPDP_MAX_NUMBER_FOUND_PARTICIPANTS = 50;
+const uint8_t SPDP_MAX_NUM_LOCATORS = 5;
+const Duration_t SPDP_DEFAULT_REMOTE_LEASE_DURATION = {
+    100, 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
+
+const int MAX_NUM_UDP_CONNECTIONS = 10;
+
+const int THREAD_POOL_NUM_WRITERS = 1;
+const int THREAD_POOL_NUM_READERS = 1;
+const int THREAD_POOL_WRITER_PRIO = 3;
+const int THREAD_POOL_READER_PRIO = 3;
+const int THREAD_POOL_WORKLOAD_QUEUE_LENGTH = 10;
+
+constexpr int OVERALL_HEAP_SIZE =
+    THREAD_POOL_NUM_WRITERS * THREAD_POOL_WRITER_STACKSIZE +
+    THREAD_POOL_NUM_READERS * THREAD_POOL_READER_STACKSIZE +
+    MAX_NUM_PARTICIPANTS * SPDP_WRITER_STACKSIZE +
+    NUM_STATEFUL_WRITERS * HEARTBEAT_STACKSIZE;
+} // namespace Config
+} // namespace rtps
+
+#endif // RTPS_CONFIG_H

include/rtps/config_stm.h

@@ -31,10 +31,13 @@ namespace rtps {
 
 #define IS_LITTLE_ENDIAN 1
 
+// define only if using FreeRTOS
+#define OS_IS_FREERTOS
+
 namespace Config {
 const VendorId_t VENDOR_ID = {13, 37};
 const std::array<uint8_t, 4> IP_ADDRESS = {
-    192, 168, 0, 47}; // Needs to be set in lwipcfg.h too.
+    192, 168, 0, 66}; // Needs to be set in lwipcfg.h too.
 const GuidPrefix_t BASE_GUID_PREFIX{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 12};
 
 const uint8_t DOMAIN_ID = 0; // 230 possible with UDP
@@ -60,6 +63,8 @@ const uint16_t SPDP_WRITER_STACKSIZE = 550;    // byte
 
 const uint16_t SF_WRITER_HB_PERIOD_MS = 4000;
 const uint16_t SPDP_RESEND_PERIOD_MS = 10000;
+const uint8_t SPDP_CYCLECOUNT_HEARTBEAT =
+    2; // skip x SPDP rounds before checking liveliness
 const uint8_t SPDP_WRITER_PRIO = 3;
 const uint8_t SPDP_MAX_NUMBER_FOUND_PARTICIPANTS = 5;
 const uint8_t SPDP_MAX_NUM_LOCATORS = 5;

include/rtps/discovery/ParticipantProxyData.h

@@ -29,7 +29,9 @@ Author: i11 - Embedded Software, RWTH Aachen University
 #include "rtps/config.h"
 #include "rtps/messages/MessageTypes.h"
 #include "ucdr/microcdr.h"
-
+#if defined(unix) || defined(__unix__)
+#include <chrono>
+#endif
 #include <array>
 
 namespace rtps {
@@ -40,11 +42,11 @@ typedef uint32_t BuiltinEndpointSet_t;
 
 class ParticipantProxyData {
 public:
-  ParticipantProxyData() = default;
-  ParticipantProxyData(Guid guid);
+  ParticipantProxyData() { onAliveSignal(); }
+  ParticipantProxyData(Guid_t guid);
 
   ProtocolVersion_t m_protocolVersion = PROTOCOLVERSION;
-  Guid m_guid = Guid{GUIDPREFIX_UNKNOWN, ENTITYID_UNKNOWN};
+  Guid_t m_guid = Guid_t{GUIDPREFIX_UNKNOWN, ENTITYID_UNKNOWN};
   VendorId_t m_vendorId = VENDOR_UNKNOWN;
   bool m_expectsInlineQos = false;
   BuiltinEndpointSet_t m_availableBuiltInEndpoints;
@@ -57,8 +59,13 @@ class ParticipantProxyData {
   std::array<Locator, Config::SPDP_MAX_NUM_LOCATORS>
       m_defaultMulticastLocatorList;
   Count_t m_manualLivelinessCount{1};
-  Duration_t m_leaseDuration = Config::SPDP_LEASE_DURATION;
-
+  Duration_t m_leaseDuration = Config::SPDP_DEFAULT_REMOTE_LEASE_DURATION;
+#if defined(unix) || defined(__unix__)
+  std::chrono::time_point<std::chrono::high_resolution_clock>
+      m_lastLivelinessReceivedTimestamp;
+#else
+  TickType_t m_lastLivelinessReceivedTickCount = 0;
+#endif
   void reset();
 
   bool readFromUcdrBuffer(ucdrBuffer &buffer);
@@ -70,6 +77,10 @@ class ParticipantProxyData {
   inline bool hasSubscriptionWriter();
   inline bool hasSubscriptionReader();
 
+  inline void onAliveSignal();
+  inline bool isAlive();
+  inline uint32_t getAliveSignalAgeInMilliseconds();
+
 private:
   bool
   readLocatorIntoList(ucdrBuffer &buffer,
@@ -131,5 +142,45 @@ bool ParticipantProxyData::hasSubscriptionReader() {
   return (m_availableBuiltInEndpoints &
           DISC_BUILTIN_ENDPOINT_SUBSCRIPTION_DETECTOR) != 0;
 }
+
+void ParticipantProxyData::onAliveSignal() {
+#if defined(unix) || defined(__unix__)
+  m_lastLivelinessReceivedTimestamp = std::chrono::high_resolution_clock::now();
+#else
+  m_lastLivelinessReceivedTickCount = xTaskGetTickCount();
+#endif
+}
+
+uint32_t ParticipantProxyData::getAliveSignalAgeInMilliseconds() {
+#if defined(unix) || defined(__unix__)
+  auto now = std::chrono::high_resolution_clock::now();
+  std::chrono::duration<double, std::milli> duration =
+      now - m_lastLivelinessReceivedTimestamp;
+  return duration.count();
+#else
+  return (xTaskGetTickCount() - m_lastLivelinessReceivedTickCount) *
+         (1000 / configTICK_RATE_HZ);
+#endif
+}
+
+/*
+ *  Returns true if last heartbeat within lease duration, else false
+ */
+bool ParticipantProxyData::isAlive() {
+  uint32_t lease_in_ms =
+      m_leaseDuration.seconds * 1000 + m_leaseDuration.fraction * 1e-6;
+
+  uint32_t max_lease_in_ms =
+      Config::SPDP_MAX_REMOTE_LEASE_DURATION.seconds * 1000 +
+      Config::SPDP_MAX_REMOTE_LEASE_DURATION.fraction * 1e-6;
+
+  auto heatbeat_age_in_ms = getAliveSignalAgeInMilliseconds();
+
+  if (heatbeat_age_in_ms > std::min(lease_in_ms, max_lease_in_ms)) {
+    return false;
+  }
+  return true;
+}
+
 } // namespace rtps
 #endif // RTPS_PARTICIPANTPROXYDATA_H