diff --git a/rclcpp_action/CMakeLists.txt b/rclcpp_action/CMakeLists.txt index 785ed0e5e1..2398c0d759 100644 --- a/rclcpp_action/CMakeLists.txt +++ b/rclcpp_action/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(ament_cmake_ros REQUIRED) find_package(action_msgs REQUIRED) find_package(rclcpp REQUIRED) find_package(rcl_action REQUIRED) +find_package(test_msgs REQUIRED) find_package(rcpputils REQUIRED) find_package(rosidl_runtime_c REQUIRED) @@ -110,6 +111,17 @@ if(BUILD_TESTING) ) endif() + ament_add_gtest(test_actions test/test_actions.cpp TIMEOUT 180) + if(TARGET test_actions) + ament_target_dependencies(test_actions + "rcpputils" + "test_msgs" + ) + target_link_libraries(test_actions + ${PROJECT_NAME} + ) + endif() + ament_add_gtest(test_server_goal_handle test/test_server_goal_handle.cpp) if(TARGET test_server_goal_handle) ament_target_dependencies(test_server_goal_handle diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 0f598270e9..e1a78c7e47 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -675,8 +675,7 @@ class Server : public ServerBase, public std::enable_shared_from_thisaction_name_); - + this->action_name_.c_str()); return true; } diff --git a/rclcpp_action/test/test_actions.cpp b/rclcpp_action/test/test_actions.cpp new file mode 100644 index 0000000000..0ffbc92afc --- /dev/null +++ b/rclcpp_action/test/test_actions.cpp @@ -0,0 +1,284 @@ +// Copyright 2024 iRobot Corporation. All Rights Reserved. +#include + +#include "test_actions.hpp" + +/* +Todo: Add more action tests: +- First request not ready, second yes +- Cancel 1st - 2nd normal +- Cancel 2nd - 1st normal +- Abort 1st - 2nd normal +- Abort 2nd - 1st normal +- Combine executors +*/ + +struct actions_test_data_t +{ + bool use_events_executor; + bool use_server_ipc; + bool use_client_ipc; +}; + +class ActionsTest +: public testing::Test, public testing::WithParamInterface +{ +public: + void SetUp() override + { + test_info = std::make_shared(); + rclcpp::init(0, nullptr); + auto p = GetParam(); + std::cout << "Test permutation: " + << (p.use_events_executor ? "{ EventsExecutor, " : "{ SingleThreadedExecutor, ") + << (p.use_server_ipc ? "IPC Server, " : "Non-IPC Server, ") + << (p.use_client_ipc ? "IPC Client }" : "Non-IPC Client }") << std::endl; + + executor = test_info->create_executor(p.use_events_executor); + executor_thread = std::thread([&]() { + executor->spin(); + }); + client_node = test_info->create_node("client_node", p.use_client_ipc); + server_node = test_info->create_node("server_node", p.use_server_ipc); + action_client = test_info->create_action_client(client_node); + action_server = test_info->create_action_server(server_node); + send_goal_options = test_info->create_goal_options(); + goal_msg = Fibonacci::Goal(); + } + + void TearDown() override + { + test_info.reset(); + executor->cancel(); + if (executor_thread.joinable()) { + executor_thread.join(); + } + rclcpp::shutdown(); + } + + rclcpp::Executor::UniquePtr executor; + std::thread executor_thread; + rclcpp::Node::SharedPtr client_node; + rclcpp::Node::SharedPtr server_node; + rclcpp_action::Client::SharedPtr action_client; + rclcpp_action::Server::SharedPtr action_server; + rclcpp_action::Client::SendGoalOptions send_goal_options; + Fibonacci::Goal goal_msg; + std::shared_ptr test_info; +}; + +INSTANTIATE_TEST_SUITE_P( + ActionsTest, + ActionsTest, + testing::Values( + /* */ + actions_test_data_t{ false, false, false }, + actions_test_data_t{ false, false, true }, + actions_test_data_t{ false, true, false }, + actions_test_data_t{ false, true, true }, + actions_test_data_t{ true, false, false }, + actions_test_data_t{ true, false, true }, + actions_test_data_t{ true, true, false }, + actions_test_data_t{ true, true, true } + )); + +TEST_P(ActionsTest, SucceedGoal) +{ + executor->add_node(server_node); + executor->add_node(client_node); + + bool server_available = action_client->wait_for_action_server(std::chrono::seconds(1)); + ASSERT_TRUE(server_available); + + auto goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options); + auto accepted_response_wait = goal_handle_future.wait_for(std::chrono::seconds(5)); + ASSERT_TRUE(accepted_response_wait == std::future_status::ready) << "Goal was rejected by server"; + + auto goal_handle = goal_handle_future.get(); + ASSERT_TRUE(goal_handle != nullptr) << "Invalid goal"; + + auto result_future = action_client->async_get_result(goal_handle); + + test_info->succeed_goal(); + + auto result_wait = result_future.wait_for(std::chrono::seconds(5)); + ASSERT_TRUE(result_wait == std::future_status::ready) << "Goal not completed on time"; + + auto wrapped_result = result_future.get(); + EXPECT_EQ(wrapped_result.code, rclcpp_action::ResultCode::SUCCEEDED); + EXPECT_TRUE(test_info->result_is_correct( + wrapped_result.result->sequence, rclcpp_action::ResultCode::SUCCEEDED)); +} + +TEST_P(ActionsTest, CancelGoal) +{ + executor->add_node(server_node); + executor->add_node(client_node); + send_goal_options.result_callback = nullptr; + + bool server_available = action_client->wait_for_action_server(std::chrono::seconds(1)); + ASSERT_TRUE(server_available); + + auto goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options); + auto accepted_response_wait = goal_handle_future.wait_for(std::chrono::seconds(5)); + ASSERT_TRUE(accepted_response_wait == std::future_status::ready) << "Goal was rejected by server"; + auto goal_handle = goal_handle_future.get(); + ASSERT_TRUE(goal_handle != nullptr) << "Invalid goal"; + + auto cancel_result_future = action_client->async_cancel_goal(goal_handle); + auto cancel_response_wait = cancel_result_future.wait_for(std::chrono::seconds(5)); + ASSERT_TRUE(cancel_response_wait == std::future_status::ready) << "Cancel response not on time"; + auto cancel_result = cancel_result_future.get(); + ASSERT_TRUE(cancel_result != nullptr) << "Invalid cancel result"; + EXPECT_NE(cancel_result->return_code, action_msgs::srv::CancelGoal::Response::ERROR_REJECTED); + + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + + auto result_future = action_client->async_get_result(goal_handle); + auto result_response_wait = result_future.wait_for(std::chrono::seconds(5)); + ASSERT_TRUE(result_response_wait == std::future_status::ready) << "Cancel result response not on time"; + auto wrapped_result = result_future.get(); + EXPECT_EQ(wrapped_result.code, rclcpp_action::ResultCode::CANCELED); + EXPECT_TRUE(test_info->result_is_correct( + wrapped_result.result->sequence, rclcpp_action::ResultCode::CANCELED)); +} + +TEST_P(ActionsTest, AbortGoal) +{ + executor->add_node(server_node); + executor->add_node(client_node); + + bool server_available = action_client->wait_for_action_server(std::chrono::seconds(1)); + ASSERT_TRUE(server_available); + + auto goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options); + auto accepted_response_wait = goal_handle_future.wait_for(std::chrono::seconds(5)); + ASSERT_TRUE(accepted_response_wait == std::future_status::ready) << "Goal was rejected by server"; + auto goal_handle = goal_handle_future.get(); + ASSERT_TRUE(goal_handle != nullptr) << "Invalid goal"; + auto result_future = action_client->async_get_result(goal_handle); + + test_info->abort_goal(); + + auto result_wait = result_future.wait_for(std::chrono::seconds(5)); + ASSERT_TRUE(result_wait == std::future_status::ready) << "Abort response not arrived"; + auto wrapped_result = result_future.get(); + EXPECT_EQ(wrapped_result.code, rclcpp_action::ResultCode::ABORTED); + EXPECT_TRUE(test_info->result_is_correct( + wrapped_result.result->sequence, rclcpp_action::ResultCode::ABORTED)); +} + +TEST_P(ActionsTest, TestReject) +{ + executor->add_node(server_node); + executor->add_node(client_node); + + bool server_available = action_client->wait_for_action_server(std::chrono::seconds(1)); + ASSERT_TRUE(server_available); + + goal_msg.order = 21; // Goals over 20 rejected + + auto goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options); + auto result_wait = goal_handle_future.wait_for(std::chrono::seconds(5)); + ASSERT_TRUE(result_wait == std::future_status::ready) << "Response not arrived"; + auto goal_handle = goal_handle_future.get(); + + ASSERT_TRUE(goal_handle == nullptr); +} + +TEST_P(ActionsTest, TestOnReadyCallback) +{ + bool server_available = action_client->wait_for_action_server(std::chrono::seconds(1)); + ASSERT_TRUE(server_available); + + auto goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options); + + // Add node: set "on_ready" callback and process the "unread" event + executor->add_node(server_node); + + // Give time to server to be executed and generate event into client + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + + // Add node: set "on_ready" callback and process the "unread" event + executor->add_node(client_node); + auto result_wait = goal_handle_future.wait_for(std::chrono::seconds(5)); + ASSERT_TRUE(result_wait == std::future_status::ready) << "Response not arrived"; + + auto goal_handle = goal_handle_future.get(); + ASSERT_TRUE(goal_handle != nullptr) << "Invalid goal"; + + auto result_future = action_client->async_get_result(goal_handle); + test_info->succeed_goal(); + + auto succeed_wait = result_future.wait_for(std::chrono::seconds(5)); + ASSERT_TRUE(succeed_wait == std::future_status::ready) << "Response not arrived"; + + auto wrapped_result = result_future.get(); + EXPECT_EQ(wrapped_result.code, rclcpp_action::ResultCode::SUCCEEDED); + EXPECT_TRUE(test_info->result_is_correct( + wrapped_result.result->sequence, rclcpp_action::ResultCode::SUCCEEDED)); +} + +TEST_P(ActionsTest, InstantSuccess) +{ + executor->add_node(server_node); + executor->add_node(client_node); + + // Unset result callback, we want to test having the result before + // having a callback set + send_goal_options.result_callback = nullptr; + + bool server_available = action_client->wait_for_action_server(std::chrono::seconds(1)); + ASSERT_TRUE(server_available); + + auto goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options); + auto result_wait = goal_handle_future.wait_for(std::chrono::seconds(5)); + ASSERT_TRUE(result_wait == std::future_status::ready) << "Response not arrived"; + + auto goal_handle = goal_handle_future.get(); + ASSERT_TRUE(goal_handle != nullptr) << "Invalid goal"; + + test_info->succeed_goal(); + + auto result_future = action_client->async_get_result(goal_handle); + auto succeed_wait = result_future.wait_for(std::chrono::seconds(5)); + ASSERT_TRUE(succeed_wait == std::future_status::ready) << "Response not arrived"; + + auto wrapped_result = result_future.get(); + EXPECT_EQ(wrapped_result.code, rclcpp_action::ResultCode::SUCCEEDED); + EXPECT_TRUE(test_info->result_is_correct( + wrapped_result.result->sequence, rclcpp_action::ResultCode::SUCCEEDED)); +} + +// See https://github.com/ros2/rclcpp/issues/2451#issuecomment-1999749919 +TEST_P(ActionsTest, FeedbackRace) +{ + executor->add_node(server_node); + + auto test_params = GetParam(); + auto client_executor = test_info->create_executor(test_params.use_events_executor); + client_executor->add_node(client_node); + + bool server_available = action_client->wait_for_action_server(std::chrono::seconds(1)); + ASSERT_TRUE(server_available); + + auto goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options); + + rclcpp::Rate rate(double(test_info->server_rate_hz) * 0.4); // A bit slower than the server's feedback rate + + for (size_t i = 0; i < 10 && !test_info->result_callback_called(); i++) { + rate.sleep(); + client_executor->spin_some(); + if (i == 5) { + test_info->succeed_goal(); + } + } + + EXPECT_TRUE(test_info->result_callback_called()); +} + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/rclcpp_action/test/test_actions.hpp b/rclcpp_action/test/test_actions.hpp new file mode 100644 index 0000000000..ef54b8d13d --- /dev/null +++ b/rclcpp_action/test/test_actions.hpp @@ -0,0 +1,235 @@ +// Copyright 2024 iRobot Corporation. All Rights Reserved. + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +using Fibonacci = test_msgs::action::Fibonacci; +using ActionGoalHandle = rclcpp_action::ClientGoalHandle; +using GoalHandleFibonacci = typename rclcpp_action::ServerGoalHandle; +using GoalHandleSharedPtr = typename std::shared_ptr; + +using rclcpp::experimental::executors::EventsExecutor; + +// Define a structure to hold test info and utilities +class TestInfo +{ +public: + ~TestInfo() + { + this->exit_thread = true; + + if (server_thread.joinable()) { + server_thread.join(); + } + } + + rclcpp::Node::SharedPtr + create_node(std::string name, bool ipc_enabled) + { + auto node_options = rclcpp::NodeOptions(); + node_options.use_intra_process_comms(ipc_enabled); + + return rclcpp::Node::make_shared(name, "test_namespace", node_options); + } + + rclcpp_action::Client::SharedPtr + create_action_client(rclcpp::Node::SharedPtr & node) + { + return rclcpp_action::create_client( + node, "fibonacci" + ); + } + + // The server executes the following in a thread when accepting the goal + void execute() + { + auto & goal_handle = this->server_goal_handle_; + + rclcpp::Rate loop_rate(double(this->server_rate_hz)); // 100Hz + auto feedback = std::make_shared(); + feedback->sequence = this->feedback_sequence; + + while(!this->exit_thread && rclcpp::ok()) + { + if (goal_handle->is_canceling()) { + auto result = std::make_shared(); + result->sequence = this->canceled_sequence; + goal_handle->canceled(result); + return; + } + + goal_handle->publish_feedback(feedback); + loop_rate.sleep(); + } + } + + void succeed_goal() + { + // Wait for feedback to be received, otherwise succeding the goal + // will remove the goal handle, and feedback callback will not + // be called + wait_for_feedback_called(); + this->exit_thread = true; + auto result = std::make_shared(); + result->sequence = this->succeeded_sequence; + this->server_goal_handle_->succeed(result); + } + + void abort_goal() + { + wait_for_feedback_called(); + this->exit_thread = true; + auto result = std::make_shared(); + result->sequence = this->aborted_sequence; + this->server_goal_handle_->abort(result); + } + + // Server: Handle goal callback + rclcpp_action::GoalResponse + handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal) + { + (void)uuid; + if (goal->order > 20) { + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + void handle_accepted(const std::shared_ptr goal_handle) + { + this->server_goal_handle_ = goal_handle; + this->server_thread = std::thread([&]() { execute(); }); + } + + rclcpp_action::Server::SharedPtr + create_action_server(rclcpp::Node::SharedPtr & node) + { + return rclcpp_action::create_server( + node, + "fibonacci", + [this] (const rclcpp_action::GoalUUID & guuid, + std::shared_ptr goal) + { + return this->handle_goal(guuid, goal); + }, + [this] (const std::shared_ptr goal_handle) + { + (void) goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + }, + [this] (const std::shared_ptr goal_handle) + { + return this->handle_accepted(goal_handle); + } + ); + } + + rclcpp::Executor::UniquePtr create_executor(bool use_events_executor) + { + if (use_events_executor) { + auto events_queue = std::make_unique(); + + return std::make_unique(std::move(events_queue), false, rclcpp::ExecutorOptions()); + } else { + return std::make_unique(); + } + } + + rclcpp_action::Client::SendGoalOptions create_goal_options() + { + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + + send_goal_options.result_callback = + [this](const typename ActionGoalHandle::WrappedResult & result) + { + this->result_cb_called = true; + (void)result; + }; + + send_goal_options.goal_response_callback = + [this](typename ActionGoalHandle::SharedPtr goal_handle) + { + this->goal_response_cb_called = true; + (void)goal_handle; + }; + + send_goal_options.feedback_callback = [this]( + typename ActionGoalHandle::SharedPtr handle, + const std::shared_ptr feedback) + { + (void) handle; + this->feedback_cb_called = result_is_correct( + feedback->sequence, rclcpp_action::ResultCode::UNKNOWN); + }; + + return send_goal_options; + } + + void wait_for_feedback_called() + { + rclcpp::Rate loop_rate(100); + while(!this->feedback_cb_called && rclcpp::ok()) { + loop_rate.sleep(); + } + } + + bool result_is_correct( + std::vector result_sequence, + rclcpp_action::ResultCode result_code) + { + std::vector expected_sequence; + + switch (result_code) { + case rclcpp_action::ResultCode::SUCCEEDED: + expected_sequence = this->succeeded_sequence; + break; + case rclcpp_action::ResultCode::CANCELED: + expected_sequence = this->canceled_sequence; + break; + case rclcpp_action::ResultCode::ABORTED: + expected_sequence = this->aborted_sequence; + break; + case rclcpp_action::ResultCode::UNKNOWN: + expected_sequence = this->feedback_sequence; + } + + if (result_sequence.size() != expected_sequence.size()) { + return false; + } + + for (size_t i = 0; i < result_sequence.size(); i++) { + if (result_sequence[i] != expected_sequence[i]) { + return false; + } + } + + return true; + } + + bool result_callback_called() { return result_cb_called; } + size_t server_rate_hz{500}; + +private: + GoalHandleSharedPtr server_goal_handle_; + std::atomic result_cb_called{false}; + std::atomic feedback_cb_called{false}; + std::atomic goal_response_cb_called{false}; + std::atomic exit_thread{false}; + std::vector succeeded_sequence{0, 1, 1, 2, 3}; + std::vector feedback_sequence{1, 2, 3}; + std::vector canceled_sequence{42}; + std::vector aborted_sequence{6, 6, 6}; + std::thread server_thread; +};