From 07dc81ac27cd7422fbc3cf48a7bf433ae7b64fec Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Tue, 27 Aug 2024 11:51:37 +0100 Subject: [PATCH 1/6] Add test for actions Signed-off-by: Mauro Passerino --- rclcpp_action/CMakeLists.txt | 14 ++ rclcpp_action/test/test_actions.cpp | 286 ++++++++++++++++++++++++++++ rclcpp_action/test/test_actions.hpp | 250 ++++++++++++++++++++++++ 3 files changed, 550 insertions(+) create mode 100644 rclcpp_action/test/test_actions.cpp create mode 100644 rclcpp_action/test/test_actions.hpp diff --git a/rclcpp_action/CMakeLists.txt b/rclcpp_action/CMakeLists.txt index f5537d4f30..f8a09f0ee5 100644 --- a/rclcpp_action/CMakeLists.txt +++ b/rclcpp_action/CMakeLists.txt @@ -79,6 +79,20 @@ if(BUILD_TESTING) add_subdirectory(test/benchmark) + ament_add_gtest(test_ros2_actions test/test_actions.cpp TIMEOUT 180) + ament_add_test_label(test_ros2_actions mimick) + if(TARGET test_ros2_actions) + target_link_libraries(test_ros2_actions + ${PROJECT_NAME} + mimick + rcl::rcl + rcl_action::rcl_action + rclcpp::rclcpp + rcutils::rcutils + ${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/test/test_actions.cpp b/rclcpp_action/test/test_actions.cpp new file mode 100644 index 0000000000..64681dc6a2 --- /dev/null +++ b/rclcpp_action/test/test_actions.cpp @@ -0,0 +1,286 @@ +// 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 "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); + + rclcpp::Rate spin_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++) { + spin_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..3e18eb7046 --- /dev/null +++ b/rclcpp_action/test/test_actions.hpp @@ -0,0 +1,250 @@ +// 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 + +#pragma once + +#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) { + 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; +}; From eb57f71f21763d7bccabc562e4dafaab1856b911 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Tue, 27 Aug 2024 12:08:17 +0100 Subject: [PATCH 2/6] Fix actions feedback race - See https://github.com/ros2/rclcpp/issues/2451 Signed-off-by: Mauro Passerino --- rclcpp_action/src/client.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 3ea9b1fb1a..cc85f9647b 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -353,16 +353,6 @@ 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; - } - - if (is_status_ready) { - pimpl_->next_ready_event = static_cast(EntityType::StatusSubscription); - return true; - } - if (is_goal_response_ready) { pimpl_->next_ready_event = static_cast(EntityType::GoalClient); return true; @@ -378,6 +368,16 @@ 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; + } + + if (is_status_ready) { + pimpl_->next_ready_event = static_cast(EntityType::StatusSubscription); + return true; + } + return false; } From 13a27f6f4215f2a539b7e98be6111b0a0b7a0d85 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Tue, 17 Sep 2024 15:53:11 +0100 Subject: [PATCH 3/6] Address PR comments and fix test Signed-off-by: Mauro Passerino --- rclcpp_action/CMakeLists.txt | 12 +- rclcpp_action/src/client.cpp | 9 +- rclcpp_action/test/test_actions.cpp | 395 ++++++++++++++-------------- rclcpp_action/test/test_actions.hpp | 94 +++---- rclcpp_action/test/test_client.cpp | 6 +- 5 files changed, 262 insertions(+), 254 deletions(-) diff --git a/rclcpp_action/CMakeLists.txt b/rclcpp_action/CMakeLists.txt index f8a09f0ee5..dd4d859acf 100644 --- a/rclcpp_action/CMakeLists.txt +++ b/rclcpp_action/CMakeLists.txt @@ -79,16 +79,12 @@ if(BUILD_TESTING) add_subdirectory(test/benchmark) - ament_add_gtest(test_ros2_actions test/test_actions.cpp TIMEOUT 180) - ament_add_test_label(test_ros2_actions mimick) - if(TARGET test_ros2_actions) - target_link_libraries(test_ros2_actions + 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} - mimick - rcl::rcl - rcl_action::rcl_action rclcpp::rclcpp - rcutils::rcutils ${test_msgs_TARGETS} ) endif() diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index cc85f9647b..e3d0b93410 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -352,6 +352,10 @@ ClientBase::is_ready(const rcl_wait_set_t & wait_set) } pimpl_->next_ready_event = std::numeric_limits::max(); + if (is_status_ready) { + pimpl_->next_ready_event = static_cast(EntityType::StatusSubscription); + return true; + } if (is_goal_response_ready) { pimpl_->next_ready_event = static_cast(EntityType::GoalClient); @@ -373,11 +377,6 @@ ClientBase::is_ready(const rcl_wait_set_t & wait_set) return true; } - if (is_status_ready) { - pimpl_->next_ready_event = static_cast(EntityType::StatusSubscription); - return true; - } - return false; } diff --git a/rclcpp_action/test/test_actions.cpp b/rclcpp_action/test/test_actions.cpp index 64681dc6a2..eee8df4fe2 100644 --- a/rclcpp_action/test/test_actions.cpp +++ b/rclcpp_action/test/test_actions.cpp @@ -13,274 +13,283 @@ // 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; + bool use_events_executor; + bool use_server_ipc; + bool use_client_ipc; }; class ActionsTest -: public testing::Test, public testing::WithParamInterface + : 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(); + 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::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; + 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 } - )); + /* */ + 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); + 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); + 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_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 goal_handle = goal_handle_future.get(); + ASSERT_TRUE(goal_handle != nullptr) << "Invalid goal"; - auto result_future = action_client->async_get_result(goal_handle); + auto result_future = action_client->async_get_result(goal_handle); - test_info->succeed_goal(); + 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 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)); + 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)); + 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)); + 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); + 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); + 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 + 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(); + 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); + 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); + 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 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); + // 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)); + // 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"; + // 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 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_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 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)); + 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); + 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; + // 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); + 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_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"; + auto goal_handle = goal_handle_future.get(); + ASSERT_TRUE(goal_handle != nullptr) << "Invalid goal"; - test_info->succeed_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 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)); + 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); + 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); + 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); + 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 goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options); - rclcpp::Rate spin_rate(double(test_info->server_rate_hz) * 0.4); // A bit slower than the server's feedback rate + // 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(); - } + 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()); -} - -int main(int argc, char **argv) -{ - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + EXPECT_TRUE(test_info->result_callback_called()); } diff --git a/rclcpp_action/test/test_actions.hpp b/rclcpp_action/test/test_actions.hpp index 3e18eb7046..19afb50241 100644 --- a/rclcpp_action/test/test_actions.hpp +++ b/rclcpp_action/test/test_actions.hpp @@ -12,9 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License.#include -#pragma once +#ifndef TEST_ACTIONS_HPP_ +#define TEST_ACTIONS_HPP_ +#include +#include +#include #include +#include #include #include @@ -46,18 +51,17 @@ class TestInfo rclcpp::Node::SharedPtr create_node(std::string name, bool ipc_enabled) { - auto node_options = rclcpp::NodeOptions(); - node_options.use_intra_process_comms(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); + 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" - ); + return rclcpp_action::create_client( + node, "fibonacci"); } // The server executes the following in a thread when accepting the goal @@ -65,12 +69,11 @@ class TestInfo { auto & goal_handle = this->server_goal_handle_; - rclcpp::Rate loop_rate(double(this->server_rate_hz)); // 100Hz + 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()) - { + while (!this->exit_thread && rclcpp::ok()) { if (goal_handle->is_canceling()) { auto result = std::make_shared(); result->sequence = this->canceled_sequence; @@ -120,30 +123,28 @@ class TestInfo void handle_accepted(const std::shared_ptr goal_handle) { this->server_goal_handle_ = goal_handle; - this->server_thread = std::thread([&]() { execute(); }); + 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); - } - ); + 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) @@ -168,19 +169,19 @@ class TestInfo 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); - }; + { + 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; } @@ -189,9 +190,10 @@ class TestInfo { rclcpp::Rate loop_rate(100); auto start_time = std::chrono::steady_clock::now(); - while(!this->feedback_cb_called && rclcpp::ok()) { + 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(); + auto elapsed_time = + std::chrono::duration_cast(current_time - start_time).count(); if (elapsed_time >= 5) { break; } @@ -232,8 +234,8 @@ class TestInfo return true; } - bool result_callback_called() { return result_cb_called; } - bool feedback_callback_called() { return feedback_cb_called; } + bool result_callback_called() {return result_cb_called;} + bool feedback_callback_called() {return feedback_cb_called;} size_t server_rate_hz{500}; private: @@ -248,3 +250,5 @@ class TestInfo 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( From e66d6e514dadbc704c49c268078483ecc03a305a Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Wed, 18 Sep 2024 07:46:48 +0100 Subject: [PATCH 4/6] add comment Signed-off-by: Mauro Passerino --- rclcpp_action/src/client.cpp | 5 +++++ rclcpp_action/test/test_actions.hpp | 2 -- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index e3d0b93410..cdcdd111f4 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -352,6 +352,11 @@ ClientBase::is_ready(const rcl_wait_set_t & wait_set) } pimpl_->next_ready_event = std::numeric_limits::max(); + + // 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; diff --git a/rclcpp_action/test/test_actions.hpp b/rclcpp_action/test/test_actions.hpp index 19afb50241..97ef162973 100644 --- a/rclcpp_action/test/test_actions.hpp +++ b/rclcpp_action/test/test_actions.hpp @@ -35,7 +35,6 @@ using GoalHandleSharedPtr = typename std::shared_ptr; using rclcpp::experimental::executors::EventsExecutor; -// Define a structure to hold test info and utilities class TestInfo { public: @@ -107,7 +106,6 @@ class TestInfo this->server_goal_handle_->abort(result); } - // Server: Handle goal callback rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID & uuid, From ca5856ddd3bc3196ec72d9add217542611a8ffda Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Thu, 24 Oct 2024 11:03:03 +0100 Subject: [PATCH 5/6] fix cpplint Signed-off-by: Mauro Passerino --- rclcpp_action/src/client.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index cdcdd111f4..b95a64a1fd 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -354,9 +354,10 @@ ClientBase::is_ready(const rcl_wait_set_t & wait_set) pimpl_->next_ready_event = std::numeric_limits::max(); // 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. + // The order of priority for action components are: + // 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; From 1888892e5f7bd2aaa35e5b673424a351713d710b Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Fri, 1 Nov 2024 18:00:50 +0000 Subject: [PATCH 6/6] Test: Correct feedback expected count Signed-off-by: Mauro Passerino --- rclcpp_action/test/test_client.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 1ac7cd3a4b..40693e79a3 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(2, feedback_count); + EXPECT_GE(feedback_count, 2); } TEST_F(TestClientAgainstServer, async_send_goal_with_result_callback_wait_for_result)