diff --git a/.gitignore b/.gitignore
index b46542ad..b2745773 100644
--- a/.gitignore
+++ b/.gitignore
@@ -4,4 +4,4 @@ build
 cmake-build-debug
 cmake-build-release
 thirdparty/Micro-CDR/CMakeFiles
-include/rtps/config.h
+#include/rtps/config.h
diff --git a/Readme.md b/Readme.md
index 78a1dfba..88d7376a 100644
--- a/Readme.md
+++ b/Readme.md
@@ -1,3 +1,5 @@
+**NOTE:** We maintain this repository as a component of [mros2](https://github.com/mROS-base/mros2), by adding some modifications from [e372abd](https://github.com/mROS-base/embeddedRTPS/tree/e372abdb6c75059d0eece851d77ae20650522f97) of the original repository. So the base branch of this repository has been changed to `mros2`.
+
 # embeddedRTPS
 
 This repository contains source code for embeddedRTPS, a portable and open-source C++ implementation of the Real-Time Publish-Subscribe Protocol (RTPS) for embedded system.  RTPS is based on the publish-subscribe mechanism and is at the core of the Data Distribution Service (DDS). DDS is used, among many other applications, in Robot Operating System 2 (ROS2) and is also part of the AUTOSAR Adaptive platform. embeddedRTPS allows to integrate Ethernet-capable microcontrollers into DDS-based systems as first-class participants.
diff --git a/include/rtps/ThreadPool.h b/include/rtps/ThreadPool.h
index aafc59fa..2012a31f 100644
--- a/include/rtps/ThreadPool.h
+++ b/include/rtps/ThreadPool.h
@@ -25,6 +25,15 @@ Author: i11 - Embedded Software, RWTH Aachen University
 #ifndef RTPS_THREADPOOL_H
 #define RTPS_THREADPOOL_H
 
+#ifdef __MBED__
+#ifdef __cplusplus
+extern "C" {
+#endif
+void sys_msleep(unsigned int ms);
+#ifdef __cplusplus
+}
+#endif
+#endif /* __MBED__ */
 #include "lwip/sys.h"
 #include "rtps/communication/PacketInfo.h"
 #include "rtps/communication/UdpDriver.h"
@@ -55,6 +64,8 @@ class ThreadPool {
 
   static void readCallback(void *arg, udp_pcb *pcb, pbuf *p,
                            const ip_addr_t *addr, Ip4Port_t port);
+  static void writerThreadFunction(void *arg);
+  static void readerThreadFunction(void *arg);
 
 private:
   receiveJumppad_fp m_receiveJumppad;
@@ -71,12 +82,16 @@ class ThreadPool {
   ThreadSafeCircularBuffer<PacketInfo,
                            Config::THREAD_POOL_WORKLOAD_QUEUE_LENGTH>
       m_queueIncoming;
-
-  static void writerThreadFunction(void *arg);
-  static void readerThreadFunction(void *arg);
   void doWriterWork();
   void doReaderWork();
 };
 } // namespace rtps
-
+#ifdef __cplusplus
+extern "C" {
+#endif
+void callWriterThreadFunction(void *arg);
+void callReaderThreadFunction(void *arg);
+#ifdef __cplusplus
+}
+#endif
 #endif // RTPS_THREADPOOL_H
diff --git a/include/rtps/common/Locator.h b/include/rtps/common/Locator.h
index 06150887..0812d226 100644
--- a/include/rtps/common/Locator.h
+++ b/include/rtps/common/Locator.h
@@ -80,6 +80,7 @@ struct Locator {
       ucdr_serialize_array_uint8_t(&buffer, reinterpret_cast<uint8_t *>(this),
                                    sizeof(Locator));
     }
+    return true;
   }
 
   ip4_addr_t getIp4Address() const {
diff --git a/include/rtps/discovery/SEDPAgent.h b/include/rtps/discovery/SEDPAgent.h
index 565de0e2..0f2b9697 100644
--- a/include/rtps/discovery/SEDPAgent.h
+++ b/include/rtps/discovery/SEDPAgent.h
@@ -52,7 +52,8 @@ class SEDPAgent {
 private:
   Participant *m_part;
   sys_mutex_t m_mutex;
-  uint8_t m_buffer[300]; // TODO check size
+  uint8_t m_buffer[600]; // TODO check size, currently changed from 300 to 600
+                         // (FastDDS gives too much options)
   BuiltInEndpoints m_endpoints;
   void (*mfp_onNewPublisherCallback)(void *arg) = nullptr;
   void *m_onNewPublisherArgs = nullptr;
diff --git a/include/rtps/discovery/SPDPAgent.h b/include/rtps/discovery/SPDPAgent.h
index 8cbaf5fd..de64e52a 100644
--- a/include/rtps/discovery/SPDPAgent.h
+++ b/include/rtps/discovery/SPDPAgent.h
@@ -44,6 +44,7 @@ class SPDPAgent {
   void init(Participant &participant, BuiltInEndpoints &endpoints);
   void start();
   void stop();
+  static void runBroadcast(void *args);
 
 private:
   Participant *mp_participant = nullptr;
@@ -66,9 +67,13 @@ class SPDPAgent {
   void addInlineQos();
   void addParticipantParameters();
   void endCurrentList();
-
-  static void runBroadcast(void *args);
 };
 } // namespace rtps
-
+#ifdef __cplusplus
+extern "C" {
+#endif
+void callRunBroadcast(void *args);
+#ifdef __cplusplus
+}
+#endif
 #endif // RTPS_SPDP_H
diff --git a/include/rtps/entities/Reader.h b/include/rtps/entities/Reader.h
index 87813bcc..d1280d05 100644
--- a/include/rtps/entities/Reader.h
+++ b/include/rtps/entities/Reader.h
@@ -38,13 +38,14 @@ struct SubmessageHeartbeat;
 
 class ReaderCacheChange {
 private:
-  const uint8_t *data;
+
 
 public:
   const ChangeKind_t kind;
   const DataSize_t size;
   const Guid writerGuid;
   const SequenceNumber_t sn;
+  const uint8_t *data;
 
   ReaderCacheChange(ChangeKind_t kind, Guid &writerGuid, SequenceNumber_t sn,
                     const uint8_t *data, DataSize_t size)
diff --git a/include/rtps/entities/StatefulReader.tpp b/include/rtps/entities/StatefulReader.tpp
index 65c46963..76c1f081 100644
--- a/include/rtps/entities/StatefulReader.tpp
+++ b/include/rtps/entities/StatefulReader.tpp
@@ -61,11 +61,11 @@ void StatefulReaderT<NetworkDriver>::newChange(
   Lock lock{m_mutex};
   for (auto &proxy : m_proxies) {
     if (proxy.remoteWriterGuid == cacheChange.writerGuid) {
-      if (proxy.expectedSN == cacheChange.sn) {
+      //if (proxy.expectedSN == cacheChange.sn) {
         m_callback(m_callee, cacheChange);
         ++proxy.expectedSN;
         return;
-      }
+      //}
     }
   }
 }
@@ -148,6 +148,7 @@ bool StatefulReaderT<NetworkDriver>::onNewHeartbeat(
   info.destPort = writer->remoteLocator.port;
   rtps::MessageFactory::addHeader(info.buffer,
                                   m_attributes.endpointGuid.prefix);
+  rtps::MessageFactory::addSubMessageDestination(info.buffer);
   rtps::MessageFactory::addAckNack(info.buffer, msg.writerId, msg.readerId,
                                    writer->getMissing(msg.firstSN, msg.lastSN),
                                    writer->getNextAckNackCount());
diff --git a/include/rtps/entities/StatefulWriter.h b/include/rtps/entities/StatefulWriter.h
index b427f1b5..0219cfc2 100644
--- a/include/rtps/entities/StatefulWriter.h
+++ b/include/rtps/entities/StatefulWriter.h
@@ -30,6 +30,8 @@ Author: i11 - Embedded Software, RWTH Aachen University
 #include "rtps/storages/MemoryPool.h"
 #include "rtps/storages/SimpleHistoryCache.h"
 
+
+
 namespace rtps {
 
 template <class NetworkDriver> class StatefulWriterT final : public Writer {
@@ -67,15 +69,20 @@ template <class NetworkDriver> class StatefulWriterT final : public Writer {
   MemoryPool<ReaderProxy, Config::NUM_READER_PROXIES_PER_WRITER> m_proxies;
 
   bool sendData(const ReaderProxy &reader, const SequenceNumber_t &sn);
-  static void hbFunctionJumppad(void *thisPointer);
   void sendHeartBeatLoop();
   void sendHeartBeat();
   bool isIrrelevant(ChangeKind_t kind) const;
+  static void hbFunctionJumppad(void *thisPointer);
 };
 
 using StatefulWriter = StatefulWriterT<UdpDriver>;
 } // namespace rtps
-
+extern void *networkSubDriverPtr;
+extern void *networkPubDriverPtr;
+extern void (*hbPubFuncPtr)(void *);
+extern void (*hbSubFuncPtr)(void *);
+extern "C" void callHbPubFunc(void *arg);
+extern "C" void callHbSubFunc(void *arg);
 #include "StatefulWriter.tpp"
 
 #endif // RTPS_STATEFULWRITER_H
diff --git a/include/rtps/entities/StatefulWriter.tpp b/include/rtps/entities/StatefulWriter.tpp
index fdb724f6..095b938c 100644
--- a/include/rtps/entities/StatefulWriter.tpp
+++ b/include/rtps/entities/StatefulWriter.tpp
@@ -22,8 +22,6 @@ This file is part of embeddedRTPS.
 Author: i11 - Embedded Software, RWTH Aachen University
 */
 
-#include "rtps/entities/StatefulWriter.h"
-
 #include "rtps/messages/MessageFactory.h"
 #include <cstring>
 #include <stdio.h>
@@ -74,20 +72,40 @@ bool StatefulWriterT<NetworkDriver>::init(TopicData attributes,
   m_packetInfo.srcPort = attributes.unicastLocator.port;
   if (m_attributes.endpointGuid.entityId ==
       ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER) {
+    #ifdef MROS2_USE_EMBEDDEDRTPS
+        m_heartbeatThread = sys_thread_new("HBThreadPub", callHbPubFunc, this,
+                                       Config::HEARTBEAT_STACKSIZE,
+                                       Config::THREAD_POOL_WRITER_PRIO);
+    networkPubDriverPtr = this;
+    hbPubFuncPtr = hbFunctionJumppad;
+    #else
     m_heartbeatThread = sys_thread_new("HBThreadPub", hbFunctionJumppad, this,
                                        Config::HEARTBEAT_STACKSIZE,
                                        Config::THREAD_POOL_WRITER_PRIO);
+    #endif
   } else if (m_attributes.endpointGuid.entityId ==
              ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_WRITER) {
+    #ifdef MROS2_USE_EMBEDDEDRTPS
+    m_heartbeatThread = sys_thread_new("HBThreadSub", callHbSubFunc, this,
+                                       Config::HEARTBEAT_STACKSIZE,
+                                       Config::THREAD_POOL_WRITER_PRIO);
+    networkSubDriverPtr = this;
+    hbSubFuncPtr = hbFunctionJumppad;
+    #else
     m_heartbeatThread = sys_thread_new("HBThreadSub", hbFunctionJumppad, this,
                                        Config::HEARTBEAT_STACKSIZE,
                                        Config::THREAD_POOL_WRITER_PRIO);
+    #endif
   } else {
     m_heartbeatThread = sys_thread_new("HBThread", hbFunctionJumppad, this,
                                        Config::HEARTBEAT_STACKSIZE,
                                        Config::THREAD_POOL_WRITER_PRIO);
   }
-
+  /*
+  if(hbFuncPointer == NULL)
+  {
+    hbFuncPointer = hbFunctionJumppad;
+  }*/
   m_is_initialized_ = true;
   return true;
 }
@@ -265,6 +283,7 @@ bool StatefulWriterT<NetworkDriver>::sendData(
   info.srcPort = m_packetInfo.srcPort;
 
   MessageFactory::addHeader(info.buffer, m_attributes.endpointGuid.prefix);
+  MessageFactory::addSubMessageDestination(info.buffer);
   MessageFactory::addSubMessageTimeStamp(info.buffer);
 
   // Just usable for IPv4
@@ -338,7 +357,7 @@ void StatefulWriterT<NetworkDriver>::sendHeartBeat() {
 #endif
       return;
     }
-
+    MessageFactory::addSubMessageDestination(info.buffer, proxy.remoteReaderGuid.prefix.id.data());
     MessageFactory::addHeartbeat(
         info.buffer, m_attributes.endpointGuid.entityId,
         proxy.remoteReaderGuid.entityId, firstSN, lastSN, m_hbCount);
@@ -351,4 +370,5 @@ void StatefulWriterT<NetworkDriver>::sendHeartBeat() {
   m_hbCount.value++;
 }
 
+
 #undef SFW_VERBOSE
diff --git a/include/rtps/entities/StatelessWriter.tpp b/include/rtps/entities/StatelessWriter.tpp
index 1698b83c..840bbe49 100644
--- a/include/rtps/entities/StatelessWriter.tpp
+++ b/include/rtps/entities/StatelessWriter.tpp
@@ -164,7 +164,7 @@ void StatelessWriterT<NetworkDriver>::progress() {
     info.srcPort = m_packetInfo.srcPort;
 
     MessageFactory::addHeader(info.buffer, m_attributes.endpointGuid.prefix);
-    MessageFactory::addSubMessageTimeStamp(info.buffer);
+    //MessageFactory::addSubMessageTimeStamp(info.buffer);
 
     {
       Lock lock(m_mutex);
@@ -185,6 +185,15 @@ void StatelessWriterT<NetworkDriver>::progress() {
                m_nextSequenceNumberToSend.low);
 #endif
       }
+      //TODO: these should be called only when the message data is published.
+      char hoge[2];
+      pbuf_copy_partial(next->data.firstElement, &hoge, 2,0);
+      if(hoge[0] != 0 || hoge[1] != 3) {
+        MessageFactory::addSubMessageDestination(info.buffer);
+        //next->data = next->data[1];
+        //next->size - next->size - 1;
+      }
+      MessageFactory::addSubMessageTimeStamp(info.buffer);
       MessageFactory::addSubMessageData(
           info.buffer, next->data, false, next->sequenceNumber,
           m_attributes.endpointGuid.entityId,
diff --git a/include/rtps/ethernetif_init.h b/include/rtps/ethernetif_init.h
new file mode 100644
index 00000000..2e2c645d
--- /dev/null
+++ b/include/rtps/ethernetif_init.h
@@ -0,0 +1,16 @@
+#ifdef _RTPS_CALLBACK_H_
+#define _RTPS_CALLBACK_H_
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+#include "lwip.h"
+
+extern err_t rtps_ethernetif_init(struct netif *netif);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _RTPS_CALLBACK_H_ */
diff --git a/include/rtps/messages/MessageFactory.h b/include/rtps/messages/MessageFactory.h
index 05b656e8..d66b1334 100644
--- a/include/rtps/messages/MessageFactory.h
+++ b/include/rtps/messages/MessageFactory.h
@@ -74,13 +74,30 @@ void addSubMessageTimeStamp(Buffer &buffer, bool setInvalid = false) {
   if (!setInvalid) {
     buffer.reserve(header.submessageLength);
     Time_t now = getCurrentTimeStamp();
-    buffer.append(reinterpret_cast<uint8_t *>(&now.seconds),
-                  sizeof(Time_t::seconds));
-    buffer.append(reinterpret_cast<uint8_t *>(&now.fraction),
-                  sizeof(Time_t::fraction));
+    // buffer.append(reinterpret_cast<uint8_t *>(&now.seconds),
+    //               sizeof(Time_t::seconds));
+    // buffer.append(reinterpret_cast<uint8_t *>(&now.fraction),
+    //               sizeof(Time_t::fraction));
+     uint8_t time_arr[8] = {0xff, 0x3f, 0x04, 0x60, 0x00, 0xd4, 0x92, 0xa6};
+    buffer.append(time_arr, 8);
   }
 }
 
+template <class Buffer>
+void addSubMessageDestination(Buffer &buffer, std::array<unsigned char, 12>::pointer id_ptr) {
+  buffer.reserve(16);
+  uint8_t info_dst[16] = {0x0e, 0x01, 0x0c, 0x00, 0x01, 0x0f, 0xf2, 0x05, 0xa8, 0x03, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00};
+  memcpy(&info_dst[4], id_ptr, 12);
+  buffer.append(info_dst, 16);
+}
+
+template <class Buffer>
+void addSubMessageDestination(Buffer &buffer) {
+  buffer.reserve(16);
+  uint8_t info_dst[16] = {0x0e, 0x01, 0x0c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+  buffer.append(info_dst, 16);
+}
+
 template <class Buffer>
 void addSubMessageData(Buffer &buffer, const Buffer &filledPayload,
                        bool containsInlineQos, const SequenceNumber_t &SN,
diff --git a/src/ThreadPool.cpp b/src/ThreadPool.cpp
index 7de1cfe1..31f4383a 100644
--- a/src/ThreadPool.cpp
+++ b/src/ThreadPool.cpp
@@ -73,16 +73,28 @@ bool ThreadPool::startThreads() {
   m_running = true;
   for (auto &thread : m_writers) {
     // TODO ID, err check, waitOnStop
+    #ifdef MROS2_USE_EMBEDDEDRTPS
+    thread = sys_thread_new("WriterThread", callWriterThreadFunction, this,
+                            Config::THREAD_POOL_WRITER_STACKSIZE,
+                            Config::THREAD_POOL_WRITER_PRIO);
+    #else
     thread = sys_thread_new("WriterThread", writerThreadFunction, this,
                             Config::THREAD_POOL_WRITER_STACKSIZE,
                             Config::THREAD_POOL_WRITER_PRIO);
+    #endif
   }
 
   for (auto &thread : m_readers) {
     // TODO ID, err check, waitOnStop
-    thread = sys_thread_new("ReaderThread", readerThreadFunction, this,
+    #ifdef MROS2_USE_EMBEDDEDRTPS
+    thread = sys_thread_new("ReaderThread", callReaderThreadFunction, this,
+                            Config::THREAD_POOL_READER_STACKSIZE,
+                            Config::THREAD_POOL_READER_PRIO);
+    # else 
+     thread = sys_thread_new("ReaderThread", readerThreadFunction, this,
                             Config::THREAD_POOL_READER_STACKSIZE,
                             Config::THREAD_POOL_READER_PRIO);
+    #endif
   }
   return true;
 }
@@ -182,4 +194,12 @@ void ThreadPool::doReaderWork() {
   }
 }
 
+void callWriterThreadFunction(void *arg){
+	ThreadPool::writerThreadFunction(arg);
+}
+
+void callReaderThreadFunction(void *arg){
+	ThreadPool::readerThreadFunction(arg);
+}
+
 #undef THREAD_POOL_VERBOSE
diff --git a/src/discovery/SPDPAgent.cpp b/src/discovery/SPDPAgent.cpp
index 4a48f2e1..8f1617c5 100644
--- a/src/discovery/SPDPAgent.cpp
+++ b/src/discovery/SPDPAgent.cpp
@@ -70,8 +70,13 @@ void SPDPAgent::start() {
     return;
   }
   m_running = true;
-  sys_thread_new("SPDPThread", runBroadcast, this,
+#ifdef MROS2_USE_EMBEDDEDRTPS
+  sys_thread_new("SPDPThread", callRunBroadcast, this,
                  Config::SPDP_WRITER_STACKSIZE, Config::SPDP_WRITER_PRIO);
+#else
+  sys_thread_new("SPDPThread", runBroadcast, this,
+                   Config::SPDP_WRITER_STACKSIZE, Config::SPDP_WRITER_PRIO);
+#endif
 }
 
 void SPDPAgent::stop() { m_running = false; }
@@ -243,6 +248,45 @@ void SPDPAgent::addInlineQos() {
 }
 
 void SPDPAgent::endCurrentList() {
+  //adding node name and node namespace
+/*
+  ucdr_serialize_uint16_t(&m_microbuffer, ParameterId::PID_ENTITY_NAME); //TODO: clean this
+  ucdr_serialize_uint16_t(&m_microbuffer, 8);
+  ucdr_serialize_uint16_t(&m_microbuffer, 4);
+  ucdr_serialize_uint16_t(&m_microbuffer, 0);
+  ucdr_serialize_uint8_t(&m_microbuffer, 's');
+  ucdr_serialize_uint8_t(&m_microbuffer, 't');
+  ucdr_serialize_uint8_t(&m_microbuffer, 'm');
+  ucdr_serialize_uint8_t(&m_microbuffer, 0);
+
+  ucdr_serialize_uint16_t(&m_microbuffer, ParameterId::PID_USER_DATA); //TODO: clean this
+    ucdr_serialize_uint16_t(&m_microbuffer, 28);
+  ucdr_serialize_uint16_t(&m_microbuffer, 22);
+  ucdr_serialize_uint16_t(&m_microbuffer, 0);
+  ucdr_serialize_uint8_t(&m_microbuffer, 'n');
+  ucdr_serialize_uint8_t(&m_microbuffer, 'a');
+  ucdr_serialize_uint8_t(&m_microbuffer, 'm');
+  ucdr_serialize_uint8_t(&m_microbuffer, 'e');
+  ucdr_serialize_uint8_t(&m_microbuffer, '=');
+  ucdr_serialize_uint8_t(&m_microbuffer, 's');
+  ucdr_serialize_uint8_t(&m_microbuffer, 't');
+  ucdr_serialize_uint8_t(&m_microbuffer, 'm');
+  ucdr_serialize_uint8_t(&m_microbuffer, ';');
+  ucdr_serialize_uint8_t(&m_microbuffer, 'n');
+  ucdr_serialize_uint8_t(&m_microbuffer, 'a');
+  ucdr_serialize_uint8_t(&m_microbuffer, 'm');
+  ucdr_serialize_uint8_t(&m_microbuffer, 'e');
+  ucdr_serialize_uint8_t(&m_microbuffer, 's');
+  ucdr_serialize_uint8_t(&m_microbuffer, 'p');
+  ucdr_serialize_uint8_t(&m_microbuffer, 'a');
+  ucdr_serialize_uint8_t(&m_microbuffer, 'c');
+  ucdr_serialize_uint8_t(&m_microbuffer, 'e');
+  ucdr_serialize_uint8_t(&m_microbuffer, '=');
+  ucdr_serialize_uint8_t(&m_microbuffer, '/');
+  ucdr_serialize_uint8_t(&m_microbuffer, ';');
+  ucdr_serialize_uint8_t(&m_microbuffer, 0);
+  ucdr_serialize_uint16_t(&m_microbuffer, 0);
+*/
   ucdr_serialize_uint16_t(&m_microbuffer, ParameterId::PID_SENTINEL);
   ucdr_serialize_uint16_t(&m_microbuffer, 0);
 }
@@ -338,4 +382,8 @@ void SPDPAgent::addParticipantParameters() {
   endCurrentList();
 }
 
+void callRunBroadcast(void *arg){
+	SPDPAgent::runBroadcast(arg);
+}
+
 #undef SPDP_VERBOSE
diff --git a/src/discovery/TopicData.cpp b/src/discovery/TopicData.cpp
index ddbabf58..b2aadc5a 100644
--- a/src/discovery/TopicData.cpp
+++ b/src/discovery/TopicData.cpp
@@ -38,6 +38,7 @@ bool TopicData::readFromUcdrBuffer(ucdrBuffer &buffer) {
   while (ucdr_buffer_remaining(&buffer) >= 4) {
     ParameterId pid;
     uint16_t length;
+    Locator uLoc;
     ucdr_deserialize_uint16_t(&buffer, reinterpret_cast<uint16_t *>(&pid));
     ucdr_deserialize_uint16_t(&buffer, &length);
 
@@ -74,7 +75,11 @@ bool TopicData::readFromUcdrBuffer(ucdrBuffer &buffer) {
       ucdr_deserialize_array_char(&buffer, typeName, typeNameLength);
       break;
     case ParameterId::PID_UNICAST_LOCATOR:
-      unicastLocator.readFromUcdrBuffer(buffer);
+      uLoc.readFromUcdrBuffer(buffer);
+      if (uLoc.kind == LocatorKind_t::LOCATOR_KIND_UDPv4 &&
+          uLoc.isSameSubnet()) {
+        unicastLocator = uLoc;
+      }
       break;
     default:
       buffer.iterator += length;
@@ -137,6 +142,11 @@ bool TopicData::serializeIntoUcdrBuffer(ucdrBuffer &buffer) const {
   ucdr_serialize_uint8_t(
       &buffer, static_cast<uint8_t>(endpointGuid.entityId.entityKind));
 
+  // //add: qos expects inline qos
+  // ucdr_serialize_uint16_t(&buffer, ParameterId::PID_EXPECTS_INLINE_QOS);
+  // ucdr_serialize_uint16_t(&buffer, 4);
+  // ucdr_serialize_uint32_t(&buffer, 0);
+
   ucdr_serialize_uint16_t(&buffer, ParameterId::PID_ENDPOINT_GUID);
   ucdr_serialize_uint16_t(&buffer, guidSize);
   ucdr_serialize_array_uint8_t(&buffer, endpointGuid.prefix.id.data(),
@@ -145,7 +155,53 @@ bool TopicData::serializeIntoUcdrBuffer(ucdrBuffer &buffer) const {
                                endpointGuid.entityId.entityKey.size());
   ucdr_serialize_uint8_t(
       &buffer, static_cast<uint8_t>(endpointGuid.entityId.entityKind));
-
+/*
+  //TODO: check the max length
+  //added: qos maxlength param
+  ucdr_serialize_uint16_t(&buffer, ParameterId::PID_TYPE_MAX_SIZE_SERIALIZED);
+  ucdr_serialize_uint16_t(&buffer, 4);
+  ucdr_serialize_uint32_t(&buffer, 84);
+
+  //added: qos durability param
+  ucdr_serialize_uint16_t(&buffer, ParameterId::PID_DURABILITY);
+  ucdr_serialize_uint16_t(&buffer, 4);
+  ucdr_serialize_uint32_t(&buffer, 0);
+
+  //added: qos deadline param
+  ucdr_serialize_uint16_t(&buffer, ParameterId::PID_DEADLINE);
+  ucdr_serialize_uint16_t(&buffer, 8);
+  ucdr_serialize_uint32_t(&buffer, 0x7fffffff);
+  ucdr_serialize_uint32_t(&buffer, 0xffffffff);
+
+  //added: qos latency_budget
+  ucdr_serialize_uint16_t(&buffer, ParameterId::PID_LATENCY_BUDGET);
+  ucdr_serialize_uint16_t(&buffer, 8);
+  ucdr_serialize_uint32_t(&buffer, 0);
+  ucdr_serialize_uint32_t(&buffer, 0);
+
+  //added: qos liveliness param
+  ucdr_serialize_uint16_t(&buffer, ParameterId::PID_LIVELINESS);
+  ucdr_serialize_uint16_t(&buffer, 12);
+  ucdr_serialize_uint32_t(&buffer, 0);
+  ucdr_serialize_uint32_t(&buffer, 0x7fffffff);
+  ucdr_serialize_uint32_t(&buffer, 0xffffffff);
+
+  //added: qos lifespan param
+  ucdr_serialize_uint16_t(&buffer, ParameterId::PID_LIFESPAN);
+  ucdr_serialize_uint16_t(&buffer, 8);
+  ucdr_serialize_uint32_t(&buffer, 0x7fffffff);
+  ucdr_serialize_uint32_t(&buffer, 0xffffffff);
+
+    //added: qos ownership param
+  ucdr_serialize_uint16_t(&buffer, ParameterId::PID_OWNERSHIP);
+  ucdr_serialize_uint16_t(&buffer, 4);
+  ucdr_serialize_uint32_t(&buffer, 0);
+
+    //added: qos destination order
+  ucdr_serialize_uint16_t(&buffer, ParameterId::PID_DESTINATION_ORDER);
+  ucdr_serialize_uint16_t(&buffer, 4);
+  ucdr_serialize_uint32_t(&buffer, 0);
+*/
   const uint8_t unidentifiedOffset = 8;
   ucdr_serialize_uint16_t(&buffer, ParameterId::PID_RELIABILITY);
   ucdr_serialize_uint16_t(&buffer,
diff --git a/src/entities/Participant.cpp b/src/entities/Participant.cpp
index bb28d3fb..476fa8c1 100644
--- a/src/entities/Participant.cpp
+++ b/src/entities/Participant.cpp
@@ -146,9 +146,9 @@ rtps::Reader *
 Participant::getMatchingReader(const TopicData &writerTopicData) const {
   for (uint8_t i = 0; i < m_numReaders; ++i) {
     if (m_readers[i]->m_attributes.matchesTopicOf(writerTopicData) &&
-        (writerTopicData.reliabilityKind == ReliabilityKind_t::RELIABLE ||
-         m_readers[i]->m_attributes.reliabilityKind ==
-             ReliabilityKind_t::BEST_EFFORT)) {
+            (writerTopicData.reliabilityKind == ReliabilityKind_t::RELIABLE ||
+              m_readers[i]->m_attributes.reliabilityKind ==
+                  ReliabilityKind_t::BEST_EFFORT)) {
       return m_readers[i];
     }
   }
diff --git a/src/rtps.cpp b/src/rtps.cpp
index 44bbbe8b..14a44e9e 100644
--- a/src/rtps.cpp
+++ b/src/rtps.cpp
@@ -44,7 +44,7 @@ Author: i11 - Embedded Software, RWTH Aachen University
 #include "default_netif.h"
 #include "lwipcfg.h"
 #else
-#include "ethernetif.h"
+#include "ethernetif_init.h"
 #endif
 
 #define INIT_VERBOSE 0
@@ -78,8 +78,7 @@ static void init(void *arg) {
     defined(__WIN32) && !defined(__CYGWIN__)
   netif_add(&netif, &ipaddr, &netmask, &gw, nullptr, pcapif_init, tcpip_input);
 #else
-  netif_add(&netif, &ipaddr, &netmask, &gw, nullptr, ethernetif_init,
-            tcpip_input);
+  netif_add(&netif, &ipaddr, &netmask, &gw, nullptr, rtps_ethernetif_init, tcpip_input);
 #endif
 
   netif_set_default(&netif);
diff --git a/thirdparty/Micro-CDR/include/ucdr/microcdr.h b/thirdparty/Micro-CDR/include/ucdr/microcdr.h
index e79f60f1..24075806 100644
--- a/thirdparty/Micro-CDR/include/ucdr/microcdr.h
+++ b/thirdparty/Micro-CDR/include/ucdr/microcdr.h
@@ -21,7 +21,7 @@ extern "C" {
 
 #include <ucdr/common.h>
 #include <ucdr/types/basic.h>
-#include <ucdr/types/string.h>
+#include <ucdr/types/cdr_string.h>
 #include <ucdr/types/array.h>
 #include <ucdr/types/sequence.h>
 
diff --git a/thirdparty/Micro-CDR/include/ucdr/types/string.h b/thirdparty/Micro-CDR/include/ucdr/types/cdr_string.h
similarity index 100%
rename from thirdparty/Micro-CDR/include/ucdr/types/string.h
rename to thirdparty/Micro-CDR/include/ucdr/types/cdr_string.h
diff --git a/thirdparty/Micro-CDR/src/c/types/string.c b/thirdparty/Micro-CDR/src/c/types/string.c
index de0760fe..59d6ed2c 100644
--- a/thirdparty/Micro-CDR/src/c/types/string.c
+++ b/thirdparty/Micro-CDR/src/c/types/string.c
@@ -13,7 +13,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <ucdr/types/string.h>
+#include <ucdr/types/cdr_string.h>
 #include <ucdr/types/sequence.h>
 
 #include <string.h>