add launch-based nosetest with simple pub/sub testing for all rmw imp…

…lementations

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()

userland/package.xml

@@ -21,4 +21,7 @@
   <exec_depend>userland_msgs</exec_depend>
 
   <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <test_depend>ament_cmake_nose</test_depend>
+  <test_depend>launch</test_depend>
 </package>

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()

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()

userland/test/test_publisher.cpp

@@ -0,0 +1,48 @@
+#include <chrono>
+#include <iostream>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <simple_msgs/Uint32.h>
+
+
+template<typename T>
+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<T>("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<float> 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<simple_msgs::Uint32>(node, &set_counter_data, 10);
+}

userland/test/test_subscriber.cpp

@@ -0,0 +1,31 @@
+#include <rclcpp/rclcpp.hpp>
+
+#include <simple_msgs/Uint32.h>
+
+
+template<typename T>
+rclcpp::subscription::SubscriptionBase::SharedPtr subscribe(rclcpp::Node::SharedPtr node, typename rclcpp::subscription::Subscription<T>::CallbackType callback)
+{
+  auto sub = node->create_subscription<T>("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<simple_msgs::Uint32>(node, print_counter_data);
+
+  rclcpp::spin(node);
+
+  return 0;
+}