diff --git a/test_communication/CMakeLists.txt b/test_communication/CMakeLists.txt index a6d52d4a..6d3cfee7 100644 --- a/test_communication/CMakeLists.txt +++ b/test_communication/CMakeLists.txt @@ -28,22 +28,25 @@ if(BUILD_TESTING) set(message_files "") set(service_files "") + set(action_files "") foreach(interface_file ${interface_files}) get_filename_component(interface_ns "${interface_file}" DIRECTORY) get_filename_component(interface_ns "${interface_ns}" NAME) - if(interface_ns STREQUAL "action") - continue() - endif() string_ends_with("${interface_file}" ".msg" is_message) if(is_message AND interface_ns STREQUAL "msg") list(APPEND message_files "${interface_file}") continue() endif() string_ends_with("${interface_file}" ".srv" is_service) - if(is_service) + if(is_service AND interface_ns STREQUAL "srv") list(APPEND service_files "${interface_file}") continue() endif() + string_ends_with("${interface_file}" ".idl" is_action) + if(is_action AND interface_ns STREQUAL "action") + list(APPEND action_files "${interface_file}") + continue() + endif() endforeach() set(other_message_files @@ -113,7 +116,9 @@ if(BUILD_TESTING) rosidl_target_interfaces(${target} ${PROJECT_NAME} "rosidl_typesupport_cpp") ament_target_dependencies(${target} - "rclcpp") + "rclcpp" + "rclcpp_action" + ) endfunction() function(custom_test_c target) @@ -226,6 +231,45 @@ if(BUILD_TESTING) ) endif() endforeach() + + # test action client / server + # Note: taking same exclusions as services since actions use services too + # set(SKIP_TEST "") + + # TODO(wjwwood): actions do not support python + if(client_library1 STREQUAL "rclpy" OR client_library2 STREQUAL "rclpy") + set(SKIP_TEST "SKIP_TEST") + endif() + + set(ACTION_CLIENT_RMW ${rmw_implementation1}) + set(ACTION_SERVER_RMW ${rmw_implementation2}) + foreach(action_file ${action_files}) + get_filename_component(TEST_ACTION_TYPE "${action_file}" NAME_WE) + set(test_suffix "__${TEST_ACTION_TYPE}${suffix}") + configure_file( + test/test_action_client_server.py.in + test_action_client_server${test_suffix}.py.configured + @ONLY + ) + file(GENERATE + OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/test_action_client_server${test_suffix}_$.py" + INPUT "${CMAKE_CURRENT_BINARY_DIR}/test_action_client_server${test_suffix}.py.configured" + ) + + ament_add_pytest_test(test_action_client_server${test_suffix} + "${CMAKE_CURRENT_BINARY_DIR}/test_action_client_server${test_suffix}_$.py" + PYTHON_EXECUTABLE "${_PYTHON_EXECUTABLE}" + APPEND_LIBRARY_DIRS "${append_library_dirs}" + TIMEOUT 30 + ${SKIP_TEST}) + if(TEST test_action_client_server${test_suffix}) + set_tests_properties( + test_action_client_server${test_suffix} + PROPERTIES DEPENDS + "test_action_client_cpp__${rmw_implementation1};test_action_server_cpp__${rmw_implementation2}" + ) + endif() + endforeach() endmacro() macro(configure_template _client_library1 _client_library2) @@ -236,6 +280,8 @@ if(BUILD_TESTING) set(TEST_SUBSCRIBER_RCL "${_client_library2}") set(TEST_REQUESTER_RCL "${_client_library1}") set(TEST_REPLIER_RCL "${_client_library2}") + set(TEST_ACTION_CLIENT_RCL "${_client_library1}") + set(TEST_ACTION_SERVER_RCL "${_client_library2}") if(_client_library1 STREQUAL _client_library2) set(suffix "__${_client_library1}") @@ -254,17 +300,21 @@ if(BUILD_TESTING) if(_client_library1 STREQUAL "rclpy") set(TEST_PUBLISHER_EXECUTABLE "${CMAKE_CURRENT_SOURCE_DIR}/test/publisher_py.py") set(TEST_REQUESTER_EXECUTABLE "${CMAKE_CURRENT_SOURCE_DIR}/test/requester_py.py") + # TODO(wjwwood): actions do not support python elseif(_client_library1 STREQUAL "rclcpp") set(TEST_PUBLISHER_EXECUTABLE "$") set(TEST_REQUESTER_EXECUTABLE "$") + set(TEST_ACTION_CLIENT_EXECUTABLE "$") endif() if(_client_library2 STREQUAL "rclpy") set(TEST_SUBSCRIBER_EXECUTABLE "${CMAKE_CURRENT_SOURCE_DIR}/test/subscriber_py.py") set(TEST_REPLIER_EXECUTABLE "${CMAKE_CURRENT_SOURCE_DIR}/test/replier_py.py") + # TODO(wjwwood): actions do not support python elseif(_client_library2 STREQUAL "rclcpp") set(TEST_SUBSCRIBER_EXECUTABLE "$") set(TEST_REPLIER_EXECUTABLE "$") + set(TEST_ACTION_SERVER_EXECUTABLE "$") endif() endmacro() @@ -298,6 +348,11 @@ if(BUILD_TESTING) "test/test_requester.cpp") custom_executable(test_replier_cpp "test/test_replier.cpp") + # executables action client / server + custom_executable(test_action_client_cpp + "test/test_action_client.cpp") + custom_executable(test_action_server_cpp + "test/test_action_server.cpp") macro(targets) custom_test_c(test_messages_c @@ -380,6 +435,7 @@ if(BUILD_TESTING) add_dependencies(${target_name} ${PROJECT_NAME}) ament_target_dependencies(${target_name} "rclcpp" + "rclcpp_action" "rmw" "rosidl_typesupport_c" "rosidl_typesupport_cpp" diff --git a/test_communication/package.xml b/test_communication/package.xml index 0aef6be2..dd3dbc7f 100644 --- a/test_communication/package.xml +++ b/test_communication/package.xml @@ -27,6 +27,7 @@ osrf_testing_tools_cpp rcl rclcpp + rclcpp_action rclpy rmw_implementation rmw_implementation_cmake diff --git a/test_communication/test/test_action_client.cpp b/test_communication/test/test_action_client.cpp new file mode 100644 index 00000000..df08d0cd --- /dev/null +++ b/test_communication/test/test_action_client.cpp @@ -0,0 +1,193 @@ +// Copyright 2018 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 + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/scope_exit.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +#include "test_msgs/action/fibonacci.hpp" + +using namespace std::chrono_literals; + +template +struct ActionClientTest +{ + typename ActionT::Goal goal; + std::function result_is_valid; + std::function feedback_is_valid; +}; + +template +int +send_goals( + rclcpp::Node::SharedPtr node, + const std::string & action_type_name, + const std::vector> & goal_tests) +{ + auto action_client = + rclcpp_action::create_client(node, "test/action/" + action_type_name); + auto logger = node->get_logger(); + + if (!action_client->wait_for_action_server(20s)) { + RCLCPP_ERROR(logger, "requester service not available after waiting"); + return 1; + } + + size_t test_index = 0; + bool invalid_feedback = false; + auto start = std::chrono::steady_clock::now(); + RCLCPP_SCOPE_EXIT({ + auto end = std::chrono::steady_clock::now(); + std::chrono::duration diff = (end - start); + RCLCPP_INFO(logger, "sent goals for %f seconds\n", diff.count()); + }); + + while (rclcpp::ok() && test_index < goal_tests.size()) { + RCLCPP_INFO(logger, "sending goal #%zu", test_index + 1); + + // on feedback, check the feedback is valid + auto feedback_callback = + [&](auto, const auto & feedback) { + RCLCPP_INFO(logger, "received feedback"); + if (!goal_tests[test_index].feedback_is_valid(feedback)) { + RCLCPP_ERROR(logger, "invalid feedback"); + invalid_feedback = true; + } + }; + + // send the request + auto goal_handle_future = + action_client->async_send_goal(goal_tests[test_index].goal, feedback_callback); + + using rclcpp::executor::FutureReturnCode; + // wait for the sent goal to be accepted + auto status = rclcpp::spin_until_future_complete(node, goal_handle_future, 1000s); + if (status != FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(logger, "send goal call failed"); + return 1; + } + + // wait for the result (feedback may be received in the meantime) + auto result_future = goal_handle_future.get()->async_result(); + status = rclcpp::spin_until_future_complete(node, result_future, 1000s); + if (status != FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(logger, "failed to receive a goal result in time"); + return 1; + } + + if (!goal_tests[test_index].result_is_valid(result_future.get().response)) { + RCLCPP_ERROR(logger, "invalid goal result"); + return 1; + } + RCLCPP_INFO(logger, "received goal #%zu of %zu", test_index + 1, goal_tests.size()); + test_index++; + } + + if (test_index != goal_tests.size()) { + return 1; + } + + if (invalid_feedback) { + return 1; + } + + return 0; +} + +std::vector> +generate_fibonacci_goal_tests() +{ + std::vector> result; + + constexpr size_t order = 10; + static_assert(order > 0, "order needs to be non-zero"); + + std::vector valid_fibo_seq; + valid_fibo_seq.push_back(0); + valid_fibo_seq.push_back(1); + for (size_t i = 1; i < order; ++i) { + valid_fibo_seq.push_back(valid_fibo_seq[i] + valid_fibo_seq[i - 1]); + } + + { + ActionClientTest test; + size_t order = 10; + test.goal.order = static_cast(order); + test.result_is_valid = + [order, valid_fibo_seq](auto result) -> bool { + if (result->sequence.size() != (order + 1)) { + fprintf(stderr, "result sequence not equal to goal order\n"); + return false; + } + for (size_t i = 0; i < order; ++i) { + if (valid_fibo_seq[i] != result->sequence[i]) { + fprintf( + stderr, + "result sequence not correct, expected %d but got %d for order %zu\n", + valid_fibo_seq[i], result->sequence[i], i); + return false; + } + } + return true; + }; + test.feedback_is_valid = + [order, valid_fibo_seq](auto feedback) -> bool { + if (feedback->sequence.size() > (order + 1)) { + fprintf(stderr, "feedback sequence greater than the goal order\n"); + return false; + } + for (size_t i = 0; i < feedback->sequence.size(); ++i) { + if (valid_fibo_seq[i] != feedback->sequence[i]) { + fprintf( + stderr, + "feedback sequence not correct, expected %d but got %d for order %zu\n", + valid_fibo_seq[i], feedback->sequence[i], i); + return false; + } + } + return true; + }; + result.push_back(test); + } + + return result; +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + if (argc != 3) { + fprintf(stderr, "Wrong number of arguments, pass an action type and a namespace\n"); + return 1; + } + + std::string action = argv[1]; + std::string namespace_ = argv[2]; + auto node = rclcpp::Node::make_shared("test_action_client_" + action, namespace_); + + int rc; + if (action == "Fibonacci") { + rc = send_goals(node, action, generate_fibonacci_goal_tests()); + } else { + fprintf(stderr, "Unknown action type '%s'\n", action.c_str()); + return 1; + } + return rc; +} diff --git a/test_communication/test/test_action_client_server.py.in b/test_communication/test/test_action_client_server.py.in new file mode 100644 index 00000000..daf51136 --- /dev/null +++ b/test_communication/test/test_action_client_server.py.in @@ -0,0 +1,56 @@ +# generated from test_communication/test/test_action_client_server.py.in + +import os +import sys +import time + +from launch.legacy import LaunchDescriptor +from launch.legacy.exit_handler import primary_exit_handler +from launch.legacy.launcher import DefaultLauncher + + +def test_action_client_server(): + namespace = '/test_time_%s' % time.strftime('%H_%M_%S', time.gmtime()) + + ld = LaunchDescriptor() + action_client_cmd = ['@TEST_ACTION_CLIENT_EXECUTABLE@', '@TEST_ACTION_TYPE@', namespace] + action_server_cmd = ['@TEST_ACTION_SERVER_EXECUTABLE@', '@TEST_ACTION_TYPE@', namespace] + + action_server_env = dict(os.environ) + action_client_env = dict(os.environ) + + if '@TEST_ACTION_SERVER_RCL@' == 'rclpy': + action_server_cmd.insert(0, sys.executable) + action_server_env['PYTHONUNBUFFERED'] = '1' + if '@TEST_ACTION_CLIENT_RCL@' == 'rclpy': + action_client_cmd.insert(0, sys.executable) + action_client_env['PYTHONUNBUFFERED'] = '1' + + action_server_env['RCL_ASSERT_RMW_ID_MATCHES'] = '@ACTION_SERVER_RMW@' + action_server_env['RMW_IMPLEMENTATION'] = '@ACTION_SERVER_RMW@' + ld.add_process( + cmd=action_server_cmd, + name='test_action_server', + env=action_server_env, + ) + + action_client_env['RCL_ASSERT_RMW_ID_MATCHES'] = '@ACTION_CLIENT_RMW@' + action_client_env['RMW_IMPLEMENTATION'] = '@ACTION_CLIENT_RMW@' + ld.add_process( + cmd=action_client_cmd, + name='test_action_client', + env=action_client_env, + exit_handler=primary_exit_handler, + ) + + launcher = DefaultLauncher() + launcher.add_launch_descriptor(ld) + rc = launcher.launch() + + assert rc == 0, \ + "The launch file failed with exit code '" + str(rc) + "'. " \ + 'May be the action client did not receive any completed goals from the action server?' + + +if __name__ == '__main__': + test_action_client_server() diff --git a/test_communication/test/test_action_server.cpp b/test_communication/test/test_action_server.cpp new file mode 100644 index 00000000..03bfd36b --- /dev/null +++ b/test_communication/test/test_action_server.cpp @@ -0,0 +1,198 @@ +// Copyright 2018 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 +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +#include "test_msgs/action/fibonacci.hpp" + +template +struct ExpectedGoalRequest +{ + std::function)> goal_is_expected; + std::function)> goal_is_valid; + std::function>)> execute_goal; +}; + +bool should_fail = false; + +template +typename rclcpp_action::Server::SharedPtr +receive_goals( + rclcpp::Node::SharedPtr node, + const std::string & action_type_name, + const std::vector> & expected_goal_requests +) +{ + auto logger = node->get_logger(); + + auto goal_callback = + [expected_goal_requests, logger](auto /* goal_id */, auto goal) { + // make sure this goal was expected + for (const auto & expected_goal_request : expected_goal_requests) { + if (expected_goal_request.goal_is_expected(goal)) { + if (!expected_goal_request.goal_is_valid(goal)) { + RCLCPP_ERROR(logger, "goal request is invalid"); + should_fail = true; + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + } + RCLCPP_ERROR(logger, "unexpected goal request"); + should_fail = true; + return rclcpp_action::GoalResponse::REJECT; + }; + + auto cancel_callback = + [logger](auto /* goal_handle */) { + RCLCPP_ERROR(logger, "received unexpected request to cancel goal"); + return rclcpp_action::CancelResponse::ACCEPT; + }; + + auto execute_callback = + [expected_goal_requests, logger](auto goal_handle) { + auto goal = goal_handle->get_goal(); + bool known_goal = false; + size_t index = 1; + for (const auto & expected_goal_request : expected_goal_requests) { + if (expected_goal_request.goal_is_expected(goal)) { + RCLCPP_INFO(logger, "executing goal #%zu of %zu", index, expected_goal_requests.size()); + known_goal = true; + expected_goal_request.execute_goal(goal_handle); + break; + } + index++; + } + if (!known_goal) { + RCLCPP_ERROR(logger, "unexpected goal which was accepted (should not happen)"); + should_fail = true; + } + }; + + return rclcpp_action::create_server( + node, + std::string("test/action/") + action_type_name, + goal_callback, + cancel_callback, + execute_callback); +} + +std::vector> +generate_expected_fibonacci_goals(rclcpp::Logger logger) +{ + std::vector> result; + + { + ExpectedGoalRequest expected_goal; + + expected_goal.goal_is_expected = + [](auto goal) { + if (goal && goal->order == 10) { + return true; + } + return false; + }; + + expected_goal.goal_is_valid = + [](auto) { + return true; + }; + + expected_goal.execute_goal = + [logger](auto goal_handle) { + const auto goal = goal_handle->get_goal(); + rclcpp::Rate loop_rate(10); + + auto feedback = std::make_shared(); + feedback->sequence.push_back(0); + feedback->sequence.push_back(1); + + auto result = std::make_shared(); + + if (goal->order <= 0) { + RCLCPP_ERROR(logger, "expected a goal > 0, got %d", goal->order); + return; + } + size_t order_size = goal->order; + + for (size_t i = 1; i < order_size; ++i) { + if (!rclcpp::ok()) { + return; + } + // Check if the goal was canceled. + if (goal_handle->is_canceling()) { + result->sequence = feedback->sequence; + goal_handle->set_canceled(result); + RCLCPP_INFO(logger, "goal was canceled"); + return; + } + // Update the sequence. + feedback->sequence.push_back(feedback->sequence[i] + feedback->sequence[i - 1]); + // Publish the current state as feedback. + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(logger, "publishing feedback for goal"); + + loop_rate.sleep(); + } + + result->sequence = feedback->sequence; + goal_handle->set_succeeded(result); + RCLCPP_INFO(logger, "goal succeeded"); + }; + + result.push_back(expected_goal); + } + + return result; +} + +int main(int argc, char ** argv) +{ + if (argc != 3) { + fprintf(stderr, "Wrong number of arguments, pass one action type and a namespace\n"); + return 1; + } + rclcpp::init(argc, argv); + + auto start = std::chrono::steady_clock::now(); + + std::string action = argv[1]; + std::string namespace_ = argv[2]; + auto node = rclcpp::Node::make_shared(std::string("test_action_server_") + action, namespace_); + + rclcpp_action::ServerBase::SharedPtr server; + + if (action == "Fibonacci") { + server = receive_goals( + node, action, generate_expected_fibonacci_goals(node->get_logger())); + } else { + fprintf(stderr, "Unknown action type '%s'\n", action.c_str()); + return 1; + } + rclcpp::spin(node); + + auto end = std::chrono::steady_clock::now(); + std::chrono::duration diff = (end - start); + printf("replied for %f seconds\n", diff.count()); + + return should_fail ? 1 : 0; +} diff --git a/test_communication/test/test_requester_replier.py.in b/test_communication/test/test_requester_replier.py.in index 44ef059d..3784b77d 100644 --- a/test_communication/test/test_requester_replier.py.in +++ b/test_communication/test/test_requester_replier.py.in @@ -19,11 +19,11 @@ def test_requester_replier(): replier_env = dict(os.environ) requester_env = dict(os.environ) - if '@TEST_REQUESTER_RCL@' == 'rclpy': - requester_cmd.insert(0, sys.executable) - replier_env['PYTHONUNBUFFERED'] = '1' if '@TEST_REPLIER_RCL@' == 'rclpy': replier_cmd.insert(0, sys.executable) + replier_env['PYTHONUNBUFFERED'] = '1' + if '@TEST_REQUESTER_RCL@' == 'rclpy': + requester_cmd.insert(0, sys.executable) requester_env['PYTHONUNBUFFERED'] = '1' replier_env['RCL_ASSERT_RMW_ID_MATCHES'] = '@REPLIER_RMW@'