Add test for inter-process services (#44)

* Add remote (inter-process) services test for rcl and change default QoS profile for services.

rcl/package.xml

@@ -24,6 +24,7 @@
   <depend>rmw_implementation</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_cmake_nose</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>rmw</test_depend>

rcl/src/rcl/client.c

@@ -79,7 +79,7 @@ rcl_client_init(
     rcl_node_get_rmw_handle(node),
     type_support,
     service_name,
-    &rmw_qos_profile_default);
+    &options->qos);
   if (!client->impl->rmw_handle) {
     RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
     goto fail;
@@ -119,7 +119,7 @@ rcl_client_get_default_options()
 {
   static rcl_client_options_t default_options;
   // Must set the allocator and qos after because they are not a compile time constant.
-  default_options.qos = rmw_qos_profile_default;
+  default_options.qos = rmw_qos_profile_services_default;
   default_options.allocator = rcl_get_default_allocator();
   return default_options;
 }

rcl/src/rcl/service.c

@@ -76,7 +76,7 @@ rcl_service_init(
     rcl_node_get_rmw_handle(node),
     type_support,
     service_name,
-    &rmw_qos_profile_default);
+    &options->qos);
   if (!service->impl->rmw_handle) {
     RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
     goto fail;
@@ -115,7 +115,7 @@ rcl_service_get_default_options()
 {
   static rcl_service_options_t default_options;
   // Must set the allocator and qos after because they are not a compile time constant.
-  default_options.qos = rmw_qos_profile_default;
+  default_options.qos = rmw_qos_profile_services_default;
   default_options.allocator = rcl_get_default_allocator();
   return default_options;
 }

rcl/test/CMakeLists.txt

@@ -1,8 +1,12 @@
 find_package(ament_cmake_gtest REQUIRED)
+find_package(ament_cmake_nose REQUIRED)
+
 find_package(example_interfaces REQUIRED)
 find_package(std_msgs REQUIRED)
 
+include(rcl_add_custom_executable.cmake)
 include(rcl_add_custom_gtest.cmake)
+include(rcl_add_custom_launch_test.cmake)
 
 set(extra_test_libraries)
 set(extra_test_env)
@@ -26,6 +30,8 @@ function(test_target_function)
 
   message(STATUS "Creating tests for '${rmw_implementation}'")
 
+  # Gtests
+
   rcl_add_custom_gtest(test_allocator${target_suffix}
     SRCS rcl/test_allocator.cpp
     ENV ${extra_test_env}
@@ -108,6 +114,32 @@ function(test_target_function)
     LIBRARIES ${PROJECT_NAME}${target_suffix} ${extra_test_libraries}
     AMENT_DEPENDENCIES ${rmw_implementation}
   )
+
+  # Launch tests
+
+  rcl_add_custom_executable(service_fixture${target_suffix}
+    SRCS rcl/service_fixture.cpp
+    ENV ${extra_test_env}
+    APPEND_LIBRARY_DIRS ${extra_lib_dirs}
+    LIBRARIES ${PROJECT_NAME}${target_suffix} ${extra_test_libraries}
+    AMENT_DEPENDENCIES ${rmw_implementation} "example_interfaces"
+  )
+
+  rcl_add_custom_executable(client_fixture${target_suffix}
+    SRCS rcl/client_fixture.cpp
+    ENV ${extra_test_env}
+    APPEND_LIBRARY_DIRS ${extra_lib_dirs}
+    LIBRARIES ${PROJECT_NAME}${target_suffix} ${extra_test_libraries}
+    AMENT_DEPENDENCIES ${rmw_implementation} "example_interfaces"
+  )
+
+  rcl_add_custom_launch_test(test_services
+    service_fixture
+    client_fixture
+    ENV ${extra_test_env}
+    TIMEOUT 15
+    #WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  )
 endfunction()
 
 call_for_each_rmw_implementation(test_target)

rcl/test/rcl/client_fixture.cpp

@@ -0,0 +1,152 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// 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.
+
+#include <chrono>
+#include <string>
+#include <thread>
+
+#include "rcl/client.h"
+#include "rcl/rcl.h"
+
+#include "example_interfaces/srv/add_two_ints.h"
+
+#include "../memory_tools/memory_tools.hpp"
+#include "../scope_exit.hpp"
+#include "rcl/error_handling.h"
+
+bool
+wait_for_client_to_be_ready(
+  rcl_client_t * client,
+  size_t max_tries,
+  int64_t period_ms)
+{
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_ret_t ret = rcl_wait_set_init(&wait_set, 0, 0, 0, 1, 0, rcl_get_default_allocator());
+  if (ret != RCL_RET_OK) {
+    fprintf(stderr, "Error in wait set init: %s\n", rcl_get_error_string_safe());
+    return false;
+  }
+  auto wait_set_exit = make_scope_exit([&wait_set]() {
+    if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) {
+      fprintf(stderr, "Error in wait set fini: %s\n", rcl_get_error_string_safe());
+      throw std::runtime_error("error while waiting for client");
+    }
+  });
+  size_t iteration = 0;
+  do {
+    ++iteration;
+    if (rcl_wait_set_clear_clients(&wait_set) != RCL_RET_OK) {
+      fprintf(stderr, "Error in wait_set_clear_clients: %s\n", rcl_get_error_string_safe());
+      return false;
+    }
+    if (rcl_wait_set_add_client(&wait_set, client) != RCL_RET_OK) {
+      fprintf(stderr, "Error in wait_set_add_client: %s\n", rcl_get_error_string_safe());
+      return false;
+    }
+    ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms));
+    if (ret == RCL_RET_TIMEOUT) {
+      continue;
+    }
+    if (ret != RCL_RET_OK) {
+      fprintf(stderr, "Error in wait: %s\n", rcl_get_error_string_safe());
+      return false;
+    }
+    for (size_t i = 0; i < wait_set.size_of_clients; ++i) {
+      if (wait_set.clients[i] && wait_set.clients[i] == client) {
+        return true;
+      }
+    }
+  } while (iteration < max_tries);
+  return false;
+}
+
+int main(int argc, char ** argv)
+{
+  int main_ret = 0;
+  {
+    if (rcl_init(argc, argv, rcl_get_default_allocator()) != RCL_RET_OK) {
+      fprintf(stderr, "Error in rcl init: %s\n", rcl_get_error_string_safe());
+      return -1;
+    }
+    rcl_node_t node = rcl_get_zero_initialized_node();
+    const char * name = "node_name";
+    rcl_node_options_t node_options = rcl_node_get_default_options();
+    if (rcl_node_init(&node, name, &node_options) != RCL_RET_OK) {
+      fprintf(stderr, "Error in node init: %s\n", rcl_get_error_string_safe());
+      return -1;
+    }
+    auto node_exit = make_scope_exit([&main_ret, &node]() {
+      if (rcl_node_fini(&node) != RCL_RET_OK) {
+        fprintf(stderr, "Error in node fini: %s\n", rcl_get_error_string_safe());
+        main_ret = -1;
+      }
+    });
+
+    const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT(
+      example_interfaces, srv, AddTwoInts);
+    const char * topic = "add_two_ints";
+
+    rcl_client_t client = rcl_get_zero_initialized_client();
+    rcl_client_options_t client_options = rcl_client_get_default_options();
+    rcl_ret_t ret = rcl_client_init(&client, &node, ts, topic, &client_options);
+    if (ret != RCL_RET_OK) {
+      fprintf(stderr, "Error in client init: %s\n", rcl_get_error_string_safe());
+      return -1;
+    }
+
+    auto client_exit = make_scope_exit([&client, &main_ret, &node]() {
+      if (rcl_client_fini(&client, &node)) {
+        fprintf(stderr, "Error in client fini: %s\n", rcl_get_error_string_safe());
+        main_ret = -1;
+      }
+    });
+
+    // Initialize a request.
+    example_interfaces__srv__AddTwoInts_Request client_request;
+    example_interfaces__srv__AddTwoInts_Request__init(&client_request);
+    client_request.a = 1;
+    client_request.b = 2;
+    int64_t sequence_number;
+
+    if (rcl_send_request(&client, &client_request, &sequence_number)) {
+      fprintf(stderr, "Error in send request: %s\n", rcl_get_error_string_safe());
+      return -1;
+    }
+
+    if (sequence_number != 1) {
+      fprintf(stderr, "Got invalid sequence number\n");
+      return -1;
+    }
+
+    example_interfaces__srv__AddTwoInts_Request__fini(&client_request);
+
+    // Initialize the response owned by the client and take the response.
+    example_interfaces__srv__AddTwoInts_Response client_response;
+    example_interfaces__srv__AddTwoInts_Response__init(&client_response);
+
+    if (!wait_for_client_to_be_ready(&client, 1000, 100)) {
+      fprintf(stderr, "Client never became ready\n");
+      return -1;
+    }
+    rmw_request_id_t header;
+    if (rcl_take_response(&client, &header, &client_response) != RCL_RET_OK) {
+      fprintf(stderr, "Error in send response: %s\n", rcl_get_error_string_safe());
+      return -1;
+    }
+
+    example_interfaces__srv__AddTwoInts_Response__fini(&client_response);
+  }
+
+  return main_ret;
+}

rcl/test/rcl/service_fixture.cpp

@@ -0,0 +1,154 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// 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.
+
+#include <chrono>
+#include <string>
+#include <thread>
+
+#include "rcl/service.h"
+
+#include "rcl/rcl.h"
+
+#include "example_interfaces/srv/add_two_ints.h"
+
+#include "../memory_tools/memory_tools.hpp"
+#include "../scope_exit.hpp"
+#include "rcl/error_handling.h"
+
+bool
+wait_for_service_to_be_ready(
+  rcl_service_t * service,
+  size_t max_tries,
+  int64_t period_ms)
+{
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_ret_t ret = rcl_wait_set_init(&wait_set, 0, 0, 0, 0, 1, rcl_get_default_allocator());
+  if (ret != RCL_RET_OK) {
+    fprintf(stderr, "Error in wait set init: %s\n", rcl_get_error_string_safe());
+    return false;
+  }
+  auto wait_set_exit = make_scope_exit([&wait_set]() {
+    if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) {
+      fprintf(stderr, "Error in wait set fini: %s\n", rcl_get_error_string_safe());
+      throw std::runtime_error("error waiting for service to be ready");
+    }
+  });
+  size_t iteration = 0;
+  do {
+    ++iteration;
+    if (rcl_wait_set_clear_services(&wait_set) != RCL_RET_OK) {
+      fprintf(stderr, "Error in wait_set_clear_services: %s\n", rcl_get_error_string_safe());
+      return false;
+    }
+    if (rcl_wait_set_add_service(&wait_set, service) != RCL_RET_OK) {
+      fprintf(stderr, "Error in wait_set_add_service: %s\n", rcl_get_error_string_safe());
+      return false;
+    }
+    ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms));
+    if (ret == RCL_RET_TIMEOUT) {
+      continue;
+    }
+    if (ret != RCL_RET_OK) {
+      fprintf(stderr, "Error in wait: %s\n", rcl_get_error_string_safe());
+      return false;
+    }
+    for (size_t i = 0; i < wait_set.size_of_services; ++i) {
+      if (wait_set.services[i] && wait_set.services[i] == service) {
+        return true;
+      }
+    }
+  } while (iteration < max_tries);
+  return false;
+}
+
+int main(int argc, char ** argv)
+{
+  int main_ret = 0;
+  {
+    if (rcl_init(argc, argv, rcl_get_default_allocator()) != RCL_RET_OK) {
+      fprintf(stderr, "Error in rcl init: %s\n", rcl_get_error_string_safe());
+      return -1;
+    }
+    rcl_node_t node = rcl_get_zero_initialized_node();
+    const char * name = "node_name";
+    rcl_node_options_t node_options = rcl_node_get_default_options();
+    if (rcl_node_init(&node, name, &node_options) != RCL_RET_OK) {
+      fprintf(stderr, "Error in node init: %s\n", rcl_get_error_string_safe());
+      return -1;
+    }
+    auto node_exit = make_scope_exit([&main_ret, &node]() {
+      if (rcl_node_fini(&node) != RCL_RET_OK) {
+        fprintf(stderr, "Error in node fini: %s\n", rcl_get_error_string_safe());
+        main_ret = -1;
+      }
+    });
+
+    const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT(
+      example_interfaces, srv, AddTwoInts);
+    const char * topic = "add_two_ints";
+
+    rcl_service_t service = rcl_get_zero_initialized_service();
+    rcl_service_options_t service_options = rcl_service_get_default_options();
+    rcl_ret_t ret = rcl_service_init(&service, &node, ts, topic, &service_options);
+    if (ret != RCL_RET_OK) {
+      fprintf(stderr, "Error in service init: %s\n", rcl_get_error_string_safe());
+      return -1;
+    }
+
+    auto service_exit = make_scope_exit([&main_ret, &service, &node]() {
+      if (rcl_service_fini(&service, &node)) {
+        fprintf(stderr, "Error in service fini: %s\n", rcl_get_error_string_safe());
+        main_ret = -1;
+      }
+    });
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+
+    // Initialize a response.
+    example_interfaces__srv__AddTwoInts_Response service_response;
+    example_interfaces__srv__AddTwoInts_Response__init(&service_response);
+    auto response_exit = make_scope_exit([&service_response]() {
+      example_interfaces__srv__AddTwoInts_Response__fini(&service_response);
+    });
+
+    // Block until a client request comes in.
+
+    if (!wait_for_service_to_be_ready(&service, 1000, 100)) {
+      fprintf(stderr, "Service never became ready\n");
+      return -1;
+    }
+
+    // Take the pending request.
+    example_interfaces__srv__AddTwoInts_Request service_request;
+    example_interfaces__srv__AddTwoInts_Request__init(&service_request);
+    auto request_exit = make_scope_exit([&service_request]() {
+      example_interfaces__srv__AddTwoInts_Request__fini(&service_request);
+    });
+    rmw_request_id_t header;
+    // TODO(jacquelinekay) May have to check for timeout error codes
+    if (rcl_take_request(&service, &header, &service_request) != RCL_RET_OK) {
+      fprintf(stderr, "Error in take_request: %s\n", rcl_get_error_string_safe());
+      return -1;
+    }
+
+    // Sum the request and send the response.
+    service_response.sum = service_request.a + service_request.b;
+    if (rcl_send_response(&service, &header, &service_response) != RCL_RET_OK) {
+      fprintf(stderr, "Error in send_response: %s\n", rcl_get_error_string_safe());
+      return -1;
+    }
+    // Our scope exits should take care of fini for everything
+  }
+  return main_ret;
+}

rcl/test/rcl/test_two_executables.py.in

@@ -0,0 +1,29 @@
+# generated from rcl/test/test_two_executables.py.in
+
+from launch import LaunchDescriptor
+from launch.exit_handler import primary_exit_handler
+from launch.launcher import DefaultLauncher
+
+
+def @TEST_NAME@():
+    ld = LaunchDescriptor()
+
+    ld.add_process(
+        cmd=['@TEST_EXECUTABLE1@'],
+        name='@TEST_EXECUTABLE1_NAME@',
+    )
+
+    ld.add_process(
+        cmd=['@TEST_EXECUTABLE2@', '@TEST_EXECUTABLE1_NAME@'],
+        name='@TEST_EXECUTABLE2_NAME@',
+    )
+
+    launcher = DefaultLauncher()
+    launcher.add_launch_descriptor(ld)
+    rc = launcher.launch()
+
+    assert rc == 0, "The launch file failed with exit code '" + str(rc) + "'. "
+
+
+if __name__ == '__main__':
+    @TEST_NAME@()

rcl/test/rcl_add_custom_executable.cmake

@@ -0,0 +1,71 @@
+# Copyright 2016 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+if(rcl_add_custom_executable_INCLUDED)
+  return()
+endif()
+set(rcl_add_custom_executable_INCLUDED TRUE)
+
+macro(rcl_add_custom_executable target)
+  cmake_parse_arguments(_ARG
+    "TRACE"
+    ""
+    "SRCS;ENV;APPEND_ENV;APPEND_LIBRARY_DIRS;INCLUDE_DIRS;LIBRARIES;AMENT_DEPENDENCIES"
+    ${ARGN})
+  if(_ARG_UNPARSED_ARGUMENTS)
+    message(FATAL_ERROR
+      "rcl_add_custom_executable() called with unused arguments:"
+      "${_ARG_UNPARSED_ARGUMENTS}")
+  endif()
+  if(_ARG_ENV)
+    set(_ARG_ENV "ENV" ${_ARG_ENV})
+  endif()
+  if(_ARG_APPEND_ENV)
+    set(_ARG_APPEND_ENV "APPEND_ENV" ${_ARG_APPEND_ENV})
+  endif()
+  if(_ARG_APPEND_LIBRARY_DIRS)
+    set(_ARG_APPEND_LIBRARY_DIRS "APPEND_LIBRARY_DIRS" ${_ARG_APPEND_LIBRARY_DIRS})
+  endif()
+
+  add_executable(${target} ${_ARG_SRCS})
+
+  if(TARGET ${target})
+    if(_ARG_TRACE)
+      message(STATUS "rcl_add_custom_executable() Target '${target}':")
+    endif()
+    # Add extra include directories, if any.
+    if(_ARG_INCLUDE_DIRS)
+      if(_ARG_TRACE)
+        message(STATUS "  rcl_add_custom_executable() INCLUDE_DIRS: ${_ARG_INCLUDE_DIRS}")
+      endif()
+      target_include_directories(${target} PUBLIC ${_ARG_INCLUDE_DIRS})
+    endif()
+    # Add extra link libraries, if any.
+    if(_ARG_LIBRARIES)
+      if(_ARG_TRACE)
+        message(STATUS "  rcl_add_custom_executable() LIBRARIES: ${_ARG_LIBRARIES}")
+      endif()
+      target_link_libraries(${target} ${_ARG_LIBRARIES})
+    endif()
+    # Add extra ament dependencies, if any.
+    if(_ARG_AMENT_DEPENDENCIES)
+      if(_ARG_TRACE)
+        message(STATUS "  rcl_add_custom_executable() AMENT_DEPENDENCIES: ${_ARG_AMENT_DEPENDENCIES}")
+      endif()
+      ament_target_dependencies(${target} ${_ARG_AMENT_DEPENDENCIES})
+    endif()
+    target_compile_definitions(${target}
+      PUBLIC "RMW_IMPLEMENTATION=${rmw_implementation}")
+  endif()
+endmacro()

rcl/test/rcl_add_custom_launch_test.cmake

@@ -0,0 +1,37 @@
+# Copyright 2016 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+if(rcl_add_custom_launch_test_INCLUDED)
+  return()
+endif()
+set(rcl_add_custom_launch_test_INCLUDED TRUE)
+
+macro(rcl_add_custom_launch_test test_name executable1 executable2)
+  set(TEST_NAME "${test_name}")
+  set(TEST_EXECUTABLE1 "$<TARGET_FILE:${executable1}${target_suffix}>")
+  set(TEST_EXECUTABLE1_NAME "${executable1}")
+  set(TEST_EXECUTABLE2 "$<TARGET_FILE:${executable2}${target_suffix}>")
+  set(TEST_EXECUTABLE2_NAME "${executable2}")
+  configure_file(
+    rcl/test_two_executables.py.in
+    ${CMAKE_CURRENT_BINARY_DIR}/${test_name}${target_suffix}.py.configure
+    @ONLY
+  )
+  file(GENERATE
+    OUTPUT "test/${test_name}${target_suffix}_$<CONFIGURATION>.py"
+    INPUT "${CMAKE_CURRENT_BINARY_DIR}/${test_name}${target_suffix}.py.configure"
+  )
+  ament_add_nose_test(${test_name}${target_suffix} "${CMAKE_CURRENT_BINARY_DIR}/${test_name}${target_suffix}_$<CONFIGURATION>.py" ${ARGN})
+  set_tests_properties(${test_name}${target_suffix} PROPERTIES DEPENDS "${executable1}${target_suffix} ${executable2}${target_suffix}")
+endmacro()