diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..ba0430d
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+__pycache__/
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 60b132e..408fbb1 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,34 +1,36 @@
 # Copyright (c) 2021 smorita_emb
 # SPDX-License-Identifier: Apache-2.0
 
+cmake_minimum_required(VERSION 3.16.3)
+
+project(mros2)
+
 target_include_directories(mros2
   BEFORE INTERFACE
-  embeddedRTPS/thirdparty/Micro-CDR/include
-  embeddedRTPS/include
-  include
-  mros2_msgs
+  ${PROJECT_SOURCE_DIR}/embeddedRTPS/thirdparty/Micro-CDR/include
+  ${PROJECT_SOURCE_DIR}/embeddedRTPS/include
+  ${PROJECT_SOURCE_DIR}/include
+  ${PROJECT_SOURCE_DIR}/mros2_msgs
 )
 
 target_sources(mros2
   INTERFACE
-  src/mros2.cpp
-  embeddedRTPS/src/communication/UdpDriver.cpp
-  embeddedRTPS/src/messages/MessageTypes.cpp
-  embeddedRTPS/src/messages/MessageReceiver.cpp
-  embeddedRTPS/src/discovery/TopicData.cpp
-  embeddedRTPS/src/discovery/ParticipantProxyData.cpp
-  embeddedRTPS/src/discovery/SEDPAgent.cpp
-  embeddedRTPS/src/discovery/SPDPAgent.cpp
-  embeddedRTPS/src/storages/HistoryCache.cpp
-  embeddedRTPS/src/storages/SimpleHistoryCache.cpp
-  embeddedRTPS/src/storages/PBufWrapper.cpp
-  embeddedRTPS/src/ThreadPool.cpp
-  embeddedRTPS/src/entities/Participant.cpp
-  embeddedRTPS/src/entities/Domain.cpp
-  embeddedRTPS/src/entities/StatelessReader.cpp
-  embeddedRTPS/thirdparty/Micro-CDR/src/c/common.c
-  embeddedRTPS/thirdparty/Micro-CDR/src/c/types/basic.c
-  embeddedRTPS/thirdparty/Micro-CDR/src/c/types/string.c
-  embeddedRTPS/thirdparty/Micro-CDR/src/c/types/sequence.c
-  embeddedRTPS/thirdparty/Micro-CDR/src/c/types/array.c
+  ${PROJECT_SOURCE_DIR}/src/mros2.cpp
+  ${PROJECT_SOURCE_DIR}/embeddedRTPS/src/communication/UdpDriver.cpp
+  ${PROJECT_SOURCE_DIR}/embeddedRTPS/src/messages/MessageTypes.cpp
+  ${PROJECT_SOURCE_DIR}/embeddedRTPS/src/messages/MessageReceiver.cpp
+  ${PROJECT_SOURCE_DIR}/embeddedRTPS/src/discovery/TopicData.cpp
+  ${PROJECT_SOURCE_DIR}/embeddedRTPS/src/discovery/ParticipantProxyData.cpp
+  ${PROJECT_SOURCE_DIR}/embeddedRTPS/src/discovery/SEDPAgent.cpp
+  ${PROJECT_SOURCE_DIR}/embeddedRTPS/src/discovery/SPDPAgent.cpp
+  ${PROJECT_SOURCE_DIR}/embeddedRTPS/src/storages/PBufWrapper.cpp
+  ${PROJECT_SOURCE_DIR}/embeddedRTPS/src/ThreadPool.cpp
+  ${PROJECT_SOURCE_DIR}/embeddedRTPS/src/entities/Participant.cpp
+  ${PROJECT_SOURCE_DIR}/embeddedRTPS/src/entities/Domain.cpp
+  ${PROJECT_SOURCE_DIR}/embeddedRTPS/src/entities/StatelessReader.cpp
+  ${PROJECT_SOURCE_DIR}/embeddedRTPS/thirdparty/Micro-CDR/src/c/common.c
+  ${PROJECT_SOURCE_DIR}/embeddedRTPS/thirdparty/Micro-CDR/src/c/types/basic.c
+  ${PROJECT_SOURCE_DIR}/embeddedRTPS/thirdparty/Micro-CDR/src/c/types/string.c
+  ${PROJECT_SOURCE_DIR}/embeddedRTPS/thirdparty/Micro-CDR/src/c/types/sequence.c
+  ${PROJECT_SOURCE_DIR}/embeddedRTPS/thirdparty/Micro-CDR/src/c/types/array.c
 )
\ No newline at end of file
diff --git a/README.md b/README.md
index f22a539..2261cc8 100644
--- a/README.md
+++ b/README.md
@@ -10,8 +10,9 @@ This repository maintains the communication layer of mROS 2, that mainly offers
 ## Features
 
 - **Agent-less**: a node on mROS 2 autonomously discovers communication nodes on the host with the feature of [embeddedRTPS](https://github.com/mROS-base/embeddedRTPS). It means no agent/bridge is required for pub/sub communication from the embedded devices.
+  - mros2 currently uses [embeddedRTPS @ 1410a87](https://github.com/mROS-base/embeddedRTPS/tree/1410a8776660244249a84031ffa78c9bdaa45e19) as it is.
 - **Lightweight**: mROS 2 APIs are implemented by C++ to be operated on the embedded devices. All software stacks are also implemented only in C/C++.
-- **Real-time**: mROS 2 employs the real-time kernel (e.g., [TOPPERS kernel](https://www.toppers.jp/en/project.html)) as the runtime platform in order to enhance the real-time capability.
+- **Real-time**: mROS 2 employs the real-time kernel (e.g., [TOPPERS kernel](https://www.toppers.jp/en/project.html), [Mbed OS](https://os.mbed.com/mbed-os/)) as the runtime platform in order to enhance the real-time capability.
 
 ## Functionalities and limitations
 
@@ -21,7 +22,8 @@ Here are functionalities that mROS 2 offers for you, and current limitations (a.
   - [Built-in-types](https://docs.ros.org/en/foxy/Concepts/About-ROS-Interfaces.html#field-types) except for `wstring` (UTF-16)
     - Please check [mros2-asp3-f767zi#exapmle-applications](https://github.com/mROS-base/mros2-asp3-f767zi#example-applications) for more details.
     - `array` types are not supported
-  - Custom message is not supported (but ongoing work)
+  - Some custom message types (e.g., Twist, Pose)
+    - We think variable-length types and types exceeding one packet cannot be handled, probably due to the limitation of lwIP.
 - Service, Actions, and Parameters are not supported
   - Please let us know if you want to use them as soon as possible. We can consider of raising the priority of these supports.
 
@@ -31,14 +33,15 @@ Since this repository itself only maintains the communication layer of mROS 2, w
 Please see each repository to learn how to use it.
 
 
-| name | kernel | board |
-|:---|:---|:---|
-| [mros2-asp3-f767zi](https://github.com/mROS-base/mros2-asp3-f767zi) | [TOPPERS/ASP3](https://www.toppers.jp/en/project.html) | [STM32 NUCLEO-F767ZI](https://www.st.com/en/evaluation-tools/nucleo-f767zi.html) |
-| [mros2-mbed](https://github.com/mROS-base/mros2-mbed) | [Mbed OS 6](https://github.com/ARMmbed/mbed-os) | Mbed enabled boards having an Ethernet port (See [detail](https://github.com/mROS-base/mros2-mbed#supported-environment)) |
+| name | kernel | board | support status |
+|:---|:---|:---|:---|
+| [mros2-asp3-f767zi](https://github.com/mROS-base/mros2-asp3-f767zi) | [TOPPERS/ASP3](https://www.toppers.jp/en/project.html) | [STM32 NUCLEO-F767ZI](https://www.st.com/en/evaluation-tools/nucleo-f767zi.html) | currently supported up to v0.3.2 (see [status](https://github.com/mROS-base/mros2-asp3-f767zi/issues/74)) |
+| [mros2-mbed](https://github.com/mROS-base/mros2-mbed) | [Mbed OS 6](https://github.com/ARMmbed/mbed-os) | Mbed enabled boards having an Ethernet port (See [detail](https://github.com/mROS-base/mros2-mbed#supported-environment)) | well maintained and easy to try |
+| [mros2-posix](https://github.com/mROS-base/mros2-posix) | POSIX (pthread) | any machine that runs Linux | partly maintained |
 
 Please let us know if you have a request for a support of board/kernel, or if you could implement this layer on another platform.
 
 ## License
 
 The source code of this repository itself is published under [Apache License 2.0](https://github.com/mROS-base/mros2/blob/main/LICENSE).  
-Please note that this repository contains [embeddedRTPS and its third party libraries](https://github.com/mROS-base/embeddedRTPS#third-party-libraries) as the submodule, and also check their Licenses.
\ No newline at end of file
+Please note that this repository contains [embeddedRTPS and its third party libraries](https://github.com/mROS-base/embeddedRTPS#third-party-libraries) as the submodule, and also check their Licenses.
diff --git a/embeddedRTPS b/embeddedRTPS
index 501987d..1410a87 160000
--- a/embeddedRTPS
+++ b/embeddedRTPS
@@ -1 +1 @@
-Subproject commit 501987d23540b4aeed7cb9703db36e58d976c10b
+Subproject commit 1410a8776660244249a84031ffa78c9bdaa45e19
diff --git a/include/mros2/freertos_conversion.h b/include/mros2/freertos_conversion.h
new file mode 100644
index 0000000..93bc348
--- /dev/null
+++ b/include/mros2/freertos_conversion.h
@@ -0,0 +1,41 @@
+/* 
+ * Conversion of FreeRTOS's definitions/types/functions in embeddedRTPS
+ * 
+ * Copyright (c) 2022 mROS-base
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef RTPS_CONVERSION_H
+#define RTPS_CONVERSION_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+#include <stdint.h>
+typedef uint32_t TickType_t;
+#define configTICK_RATE_HZ ((TickType_t)1000)
+
+#ifndef pdMS_TO_TICKS
+#define pdMS_TO_TICKS(xTimeInMs) ((TickType_t)(((TickType_t)(xTimeInMs) * (TickType_t)configTICK_RATE_HZ) / (TickType_t)1000))
+#endif
+
+#define xTaskGetTickCount osKernelGetTickCount
+#define vTaskDelay osDelay
+
+void sys_msleep(unsigned int ms);
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* RTPS_CONVERSION_H */
\ No newline at end of file
diff --git a/include/mros2_user_config.h b/include/mros2_config.h
similarity index 75%
rename from include/mros2_user_config.h
rename to include/mros2_config.h
index 114d0c5..ea6cff3 100644
--- a/include/mros2_user_config.h
+++ b/include/mros2_config.h
@@ -5,7 +5,7 @@
 extern "C" {
 #endif
 
-extern int mros2_get_submsg_count(void);
+#define SUB_MSG_COUNT 10
 
 #ifdef __cplusplus
 }
diff --git a/src/mros2.cpp b/src/mros2.cpp
index a2c8014..b81b264 100644
--- a/src/mros2.cpp
+++ b/src/mros2.cpp
@@ -1,11 +1,11 @@
 #include "mros2.h"
-#include "mros2_user_config.h"
 
 #include <rtps/rtps.h>
 
 #ifdef __MBED__
 #include "mbed.h"
 #else  /* __MBED__ */
+#include "mros2_config.h"
 #include "cmsis_os.h"
 #endif /* __MBED__ */
 
@@ -61,13 +61,13 @@ void init(int argc, char * argv[])
   buf[3] = 0;
 
 #ifdef __MBED__
-  mros2_init_thread =  new Thread(osPriorityAboveNormal, 2000, nullptr, "mROS2Thread");
+  mros2_init_thread =  new Thread(osPriorityAboveNormal, 5000, nullptr, "mROS2Thread");
   mros2_init_thread->start(callback(mros2_init, (void *)NULL));
 #else /* __MBED__ */
   osThreadAttr_t attributes;
 
   attributes.name = "mROS2Thread",
-  attributes.stack_size = 1000,
+  attributes.stack_size = 5000,
   attributes.priority = (osPriority_t)24,
 
   osThreadNew(mros2_init, NULL, (const osThreadAttr_t*)&attributes);
@@ -86,13 +86,11 @@ void mros2_init(void *args)
   MROS2_DEBUG("[MROS2LIB] Initilizing lwIP complete");
 #endif /* __MBED__ */
 
-  int sub_msg_count;
   static rtps::Domain domain;
   domain_ptr = &domain;
 
 #ifndef __MBED__
-  sub_msg_count = mros2_get_submsg_count();
-  subscriber_msg_queue_id = osMessageQueueNew(sub_msg_count, SUB_MSG_SIZE, NULL);
+  subscriber_msg_queue_id = osMessageQueueNew(SUB_MSG_COUNT, SUB_MSG_SIZE, NULL);
   if (subscriber_msg_queue_id == NULL) {
     MROS2_ERROR("[MROS2LIB] ERROR: mROS2 init failed");
     return;
@@ -209,7 +207,7 @@ Subscriber Node::create_subscription(std::string topic_name, int qos, void(*fp)(
   /* Register callback to ensure that a subscriber is matched to the reader before receiving messages */
   part_ptr->registerOnNewPublisherMatchedCallback(subMatch, &pubMatched);
 
-  MROS2_DEBUG("[MROS2LIB] create_subscription complete. data memory address=0x%x", data_p);
+  MROS2_DEBUG("[MROS2LIB] create_subscription complete.");
   return sub;
 }
 
@@ -217,7 +215,8 @@ template <class T>
 void Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange)
 {
   T msg;
-  msg.copyFromBuf(&cacheChange.data[4]);
+  const uint8_t *cacheData = cacheChange.getData();
+  msg.copyFromBuf(&cacheData[4]);
 
   SubscribeDataType *sub = (SubscribeDataType *)callee;
   void (*fp)(intptr_t) = sub->cb_fp;