diff --git a/userland/CMakeLists.txt b/userland/CMakeLists.txt
index 7bec3f43..b93dd42d 100644
--- a/userland/CMakeLists.txt
+++ b/userland/CMakeLists.txt
@@ -122,3 +122,7 @@ build_executable(two_nodes src/explicit/two_nodes.cpp)
# Build services examples
build_executable(add_two_ints_client src/add_two_ints_client.cpp)
build_executable(add_two_ints_server src/add_two_ints_server.cpp)
+
+if(AMENT_ENABLE_TESTING)
+ add_subdirectory(test)
+endif()
diff --git a/userland/package.xml b/userland/package.xml
index 8c70ad0f..bb4226bb 100644
--- a/userland/package.xml
+++ b/userland/package.xml
@@ -21,4 +21,7 @@
userland_msgs
rosidl_default_runtime
+
+ ament_cmake_nose
+ launch
diff --git a/userland/test/CMakeLists.txt b/userland/test/CMakeLists.txt
new file mode 100644
index 00000000..d5dd1c37
--- /dev/null
+++ b/userland/test/CMakeLists.txt
@@ -0,0 +1,35 @@
+find_package(ament_cmake_nose REQUIRED)
+
+foreach(middleware_impl_tmp ${middleware_implementations})
+
+ add_executable(test_publisher__${middleware_impl_tmp} "test_publisher.cpp")
+ ament_target_dependencies(test_publisher__${middleware_impl_tmp}
+ "rclcpp"
+ "${middleware_impl_tmp}"
+ "simple_msgs")
+
+ add_executable(test_subscriber__${middleware_impl_tmp} "test_subscriber.cpp")
+ ament_target_dependencies(test_subscriber__${middleware_impl_tmp}
+ "rclcpp"
+ "${middleware_impl_tmp}"
+ "simple_msgs")
+
+ if(NOT WIN32)
+ set(test_executable_extension "")
+ else()
+ set(test_executable_extension ".exe")
+ endif()
+ set(TEST_PUBLISHER_EXECUTABLE "${CMAKE_CURRENT_BINARY_DIR}/test_publisher__${middleware_impl_tmp}${test_executable_extension}")
+ set(TEST_SUBSCRIBER_EXECUTABLE "${CMAKE_CURRENT_BINARY_DIR}/test_subscriber__${middleware_impl_tmp}${test_executable_extension}")
+ configure_file(
+ test_publish_subscribe.py.in
+ test_publish_subscribe__${middleware_impl_tmp}.py
+ @ONLY
+ )
+
+ ament_add_nose_test(publish_subscribe__${middleware_impl_tmp} "${CMAKE_CURRENT_BINARY_DIR}/test_publish_subscribe__${middleware_impl_tmp}.py" TIMEOUT 60)
+ set_tests_properties(
+ publish_subscribe__${middleware_impl_tmp}
+ PROPERTIES DEPENDS "test_publisher__${middleware_impl_tmp};test_subscriber__${middleware_impl_tmp}"
+ )
+endforeach()
diff --git a/userland/test/test_publish_subscribe.py.in b/userland/test/test_publish_subscribe.py.in
new file mode 100644
index 00000000..0dba7296
--- /dev/null
+++ b/userland/test/test_publish_subscribe.py.in
@@ -0,0 +1,28 @@
+from launch import LaunchDescriptor
+from launch.exit_handler import primary_exit_handler
+from launch.launcher import DefaultLauncher
+
+
+def test_publish_subscribe():
+ ld = LaunchDescriptor()
+
+ ld.add_process(
+ cmd=['@TEST_PUBLISHER_EXECUTABLE@'],
+ name='test_publisher',
+ )
+
+ ld.add_process(
+ cmd=['@TEST_SUBSCRIBER_EXECUTABLE@'],
+ name='test_subscriber',
+ exit_handler=primary_exit_handler,
+ )
+
+ launcher = DefaultLauncher()
+ launcher.add_launch_descriptor(ld)
+ rc = launcher.launch()
+
+ assert rc == 0, 'The subsciber did not receive any messages'
+
+
+if __name__ == '__main__':
+ test_publish_subscribe()
diff --git a/userland/test/test_publisher.cpp b/userland/test/test_publisher.cpp
new file mode 100644
index 00000000..bfd30988
--- /dev/null
+++ b/userland/test/test_publisher.cpp
@@ -0,0 +1,48 @@
+#include
+#include
+
+#include
+
+#include
+
+
+template
+int publish(
+ rclcpp::Node::SharedPtr node,
+ void (*set_data_func)(typename T::Ptr&, size_t),
+ size_t number_of_messages)
+{
+ auto p = node->create_publisher("topic_name", 10);
+ typename T::Ptr ros_msg(new T());
+
+ auto start = std::chrono::steady_clock::now();
+ rclcpp::WallRate rate(1);
+ size_t i = 1;
+ while (rclcpp::ok() and i <= number_of_messages) {
+ set_data_func(ros_msg, i);
+ p->publish(ros_msg);
+ std::cout << "published ros msg #" << i << std::endl;
+ ++i;
+ rate.sleep();
+ }
+ auto end = std::chrono::steady_clock::now();
+
+ std::chrono::duration diff = (end - start);
+ std::cout << "Runtime: " << diff.count() << " seconds" << std::endl;
+
+ return 0;
+}
+
+void set_counter_data(simple_msgs::Uint32::Ptr &ros_msg, size_t i)
+{
+ ros_msg->data = i;
+}
+
+int main(int argc, char** argv)
+{
+ rclcpp::init(argc, argv);
+
+ auto node = rclcpp::Node::make_shared("test_publisher");
+
+ return publish(node, &set_counter_data, 10);
+}
diff --git a/userland/test/test_subscriber.cpp b/userland/test/test_subscriber.cpp
new file mode 100644
index 00000000..c353c60d
--- /dev/null
+++ b/userland/test/test_subscriber.cpp
@@ -0,0 +1,31 @@
+#include
+
+#include
+
+
+template
+rclcpp::subscription::SubscriptionBase::SharedPtr subscribe(rclcpp::Node::SharedPtr node, typename rclcpp::subscription::Subscription::CallbackType callback)
+{
+ auto sub = node->create_subscription("topic_name", 10, callback);
+ return sub;
+}
+
+void print_counter_data(const simple_msgs::Uint32::ConstPtr &msg)
+{
+ std::cout << "Got message #" << msg->data << std::endl;
+ rclcpp::shutdown();
+}
+
+int main(int argc, char** argv)
+{
+ rclcpp::init(argc, argv);
+
+ auto node = rclcpp::Node::make_shared("test_subscriber");
+
+ rclcpp::subscription::SubscriptionBase::SharedPtr sub;
+ sub = subscribe(node, print_counter_data);
+
+ rclcpp::spin(node);
+
+ return 0;
+}