diff --git a/rclcpp_action/CMakeLists.txt b/rclcpp_action/CMakeLists.txt index f5537d4f30..dd4d859acf 100644 --- a/rclcpp_action/CMakeLists.txt +++ b/rclcpp_action/CMakeLists.txt @@ -79,6 +79,16 @@ if(BUILD_TESTING) add_subdirectory(test/benchmark) + ament_add_gtest(test_actions test/test_actions.cpp TIMEOUT 180) + ament_add_test_label(test_actions) + if(TARGET test_actions) + target_link_libraries(test_actions + ${PROJECT_NAME} + rclcpp::rclcpp + ${test_msgs_TARGETS} + ) + endif() + ament_add_gtest(test_client test/test_client.cpp TIMEOUT 180) ament_add_test_label(test_client mimick) if(TARGET test_client) diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 3ea9b1fb1a..cdcdd111f4 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -353,11 +353,10 @@ ClientBase::is_ready(const rcl_wait_set_t & wait_set) pimpl_->next_ready_event = std::numeric_limits::max(); - if (is_feedback_ready) { - pimpl_->next_ready_event = static_cast(EntityType::FeedbackSubscription); - return true; - } - + // The following 'if' statements set the priority of execution for different entities. + // The order of priority is: Status > Goal Response > Result Response > Cancel Response > Feedback. + // Feedback has the lowest priority, since if the client spins slower than the server's feedback rate, + // it may never process the action results. if (is_status_ready) { pimpl_->next_ready_event = static_cast(EntityType::StatusSubscription); return true; @@ -378,6 +377,11 @@ ClientBase::is_ready(const rcl_wait_set_t & wait_set) return true; } + if (is_feedback_ready) { + pimpl_->next_ready_event = static_cast(EntityType::FeedbackSubscription); + return true; + } + return false; } diff --git a/rclcpp_action/test/test_actions.cpp b/rclcpp_action/test/test_actions.cpp new file mode 100644 index 0000000000..eee8df4fe2 --- /dev/null +++ b/rclcpp_action/test/test_actions.cpp @@ -0,0 +1,295 @@ +// Copyright 2024 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 +#include + +#include "test_actions.hpp" + +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); + + // Spin a bit slower than the server's feedback rate + rclcpp::Rate spin_rate(static_cast(test_info->server_rate_hz) * 0.4); + + for (size_t i = 0; i < 10 && !test_info->result_callback_called(); i++) { + spin_rate.sleep(); + client_executor->spin_some(); + if (i == 5) { + test_info->succeed_goal(); + } + } + + EXPECT_TRUE(test_info->result_callback_called()); +} diff --git a/rclcpp_action/test/test_actions.hpp b/rclcpp_action/test/test_actions.hpp new file mode 100644 index 0000000000..97ef162973 --- /dev/null +++ b/rclcpp_action/test/test_actions.hpp @@ -0,0 +1,252 @@ +// Copyright 2024 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 + +#ifndef TEST_ACTIONS_HPP_ +#define TEST_ACTIONS_HPP_ + +#include +#include +#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; + +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(static_cast(this->server_rate_hz)); + 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); + } + + 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) { + return std::make_unique(); + } 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); + auto start_time = std::chrono::steady_clock::now(); + while (!this->feedback_cb_called && rclcpp::ok()) { + auto current_time = std::chrono::steady_clock::now(); + auto elapsed_time = + std::chrono::duration_cast(current_time - start_time).count(); + if (elapsed_time >= 5) { + break; + } + 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;} + bool feedback_callback_called() {return feedback_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; +}; + +#endif // TEST_ACTIONS_HPP_ diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 08093cb873..1ac7cd3a4b 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -565,7 +565,7 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_feedback_callback_wait_for_ ASSERT_EQ(5u, wrapped_result.result->sequence.size()); EXPECT_EQ(3, wrapped_result.result->sequence.back()); - EXPECT_EQ(5, feedback_count); + EXPECT_EQ(2, feedback_count); } TEST_F(TestClientAgainstServer, async_send_goal_with_result_callback_wait_for_result) @@ -1010,11 +1010,11 @@ TEST_F(TestClientAgainstServer, execute_rcl_errors) { ActionGoal goal; goal.order = 5; - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp_action", rcl_action_take_result_response, RCL_RET_ERROR); auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); dual_spin_until_future_complete(future_goal_handle); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_take_result_response, RCL_RET_ERROR); auto goal_handle = future_goal_handle.get(); auto future_result = action_client->async_get_result(goal_handle); EXPECT_THROW(