diff --git a/rcl/package.xml b/rcl/package.xml index e6e2c48..6ff283f 100644 --- a/rcl/package.xml +++ b/rcl/package.xml @@ -24,6 +24,7 @@ rmw_implementation ament_cmake_gtest + ament_cmake_nose ament_lint_auto ament_lint_common rmw diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c index 3cb4621..09ec8cd 100644 --- a/rcl/src/rcl/client.c +++ b/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; } diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index 531d99e..be959e1 100644 --- a/rcl/src/rcl/service.c +++ b/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; } diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index b8b630f..7d7d403 100644 --- a/rcl/test/CMakeLists.txt +++ b/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) diff --git a/rcl/test/rcl/client_fixture.cpp b/rcl/test/rcl/client_fixture.cpp new file mode 100644 index 0000000..d4f6211 --- /dev/null +++ b/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 +#include +#include + +#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; +} diff --git a/rcl/test/rcl/service_fixture.cpp b/rcl/test/rcl/service_fixture.cpp new file mode 100644 index 0000000..665b800 --- /dev/null +++ b/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 +#include +#include + +#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; +} diff --git a/rcl/test/rcl/test_two_executables.py.in b/rcl/test/rcl/test_two_executables.py.in new file mode 100644 index 0000000..1e4bf86 --- /dev/null +++ b/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@() diff --git a/rcl/test/rcl_add_custom_executable.cmake b/rcl/test/rcl_add_custom_executable.cmake new file mode 100644 index 0000000..647be13 --- /dev/null +++ b/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() diff --git a/rcl/test/rcl_add_custom_launch_test.cmake b/rcl/test/rcl_add_custom_launch_test.cmake new file mode 100644 index 0000000..c7bad91 --- /dev/null +++ b/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 "$") + set(TEST_EXECUTABLE1_NAME "${executable1}") + set(TEST_EXECUTABLE2 "$") + 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}_$.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}_$.py" ${ARGN}) + set_tests_properties(${test_name}${target_suffix} PROPERTIES DEPENDS "${executable1}${target_suffix} ${executable2}${target_suffix}") +endmacro()