Skip to content

Commit 2b32d25

Browse files
apojomovskyapojomovsky
committed
Add test_actions
1 parent 982b403 commit 2b32d25

File tree

4 files changed

+535
-2
lines changed

4 files changed

+535
-2
lines changed

‎rclcpp_action/CMakeLists.txt‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@ find_package(ament_cmake_ros REQUIRED)
66
find_package(action_msgs REQUIRED)
77
find_package(rclcpp REQUIRED)
88
find_package(rcl_action REQUIRED)
9+
find_package(example_interfaces REQUIRED)
910
find_package(rcpputils REQUIRED)
1011
find_package(rosidl_runtime_c REQUIRED)
1112

@@ -110,6 +111,19 @@ if(BUILD_TESTING)
110111
)
111112
endif()
112113

114+
ament_add_gtest(test_actions test/test_actions.cpp TIMEOUT 180)
115+
if(TARGET test_actions)
116+
ament_target_dependencies(test_actions
117+
"example_interfaces"
118+
"rcpputils"
119+
"test_msgs"
120+
)
121+
target_link_libraries(test_actions
122+
${PROJECT_NAME}
123+
mimick
124+
)
125+
endif()
126+
113127
ament_add_gtest(test_server_goal_handle test/test_server_goal_handle.cpp)
114128
if(TARGET test_server_goal_handle)
115129
ament_target_dependencies(test_server_goal_handle

‎rclcpp_action/include/rclcpp_action/server.hpp‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -675,8 +675,7 @@ class Server : public ServerBase, public std::enable_shared_from_this<Server<Act
675675
rclcpp::get_logger("rclcpp_action"),
676676
"Action server can't send result response, missing IPC Action client: %s. "
677677
"Will do inter-process publish",
678-
this->action_name_);
679-
678+
this->action_name_.c_str());
680679
return true;
681680
}
682681

‎rclcpp_action/test/test_actions.cpp‎

Lines changed: 284 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,284 @@
1+
// Copyright 2024 iRobot Corporation. All Rights Reserved.
2+
#include <gtest/gtest.h>
3+
4+
#include "test_actions.hpp"
5+
6+
/*
7+
Todo: Add more action tests:
8+
- First request not ready, second yes
9+
- Cancel 1st - 2nd normal
10+
- Cancel 2nd - 1st normal
11+
- Abort 1st - 2nd normal
12+
- Abort 2nd - 1st normal
13+
- Combine executors
14+
*/
15+
16+
struct actions_test_data_t
17+
{
18+
bool use_events_executor;
19+
bool use_server_ipc;
20+
bool use_client_ipc;
21+
};
22+
23+
class ActionsTest
24+
: public testing::Test, public testing::WithParamInterface<actions_test_data_t>
25+
{
26+
public:
27+
void SetUp() override
28+
{
29+
test_info = std::make_shared<TestInfo>();
30+
rclcpp::init(0, nullptr);
31+
auto p = GetParam();
32+
std::cout << "Test permutation: "
33+
<< (p.use_events_executor ? "{ EventsExecutor, " : "{ SingleThreadedExecutor, ")
34+
<< (p.use_server_ipc ? "IPC Server, " : "Non-IPC Server, ")
35+
<< (p.use_client_ipc ? "IPC Client }" : "Non-IPC Client }") << std::endl;
36+
37+
executor = test_info->create_executor(p.use_events_executor);
38+
executor_thread = std::thread([&]() {
39+
executor->spin();
40+
});
41+
client_node = test_info->create_node("client_node", p.use_client_ipc);
42+
server_node = test_info->create_node("server_node", p.use_server_ipc);
43+
action_client = test_info->create_action_client(client_node);
44+
action_server = test_info->create_action_server(server_node);
45+
send_goal_options = test_info->create_goal_options();
46+
goal_msg = Fibonacci::Goal();
47+
}
48+
49+
void TearDown() override
50+
{
51+
test_info.reset();
52+
executor->cancel();
53+
if (executor_thread.joinable()) {
54+
executor_thread.join();
55+
}
56+
rclcpp::shutdown();
57+
}
58+
59+
rclcpp::Executor::UniquePtr executor;
60+
std::thread executor_thread;
61+
rclcpp::Node::SharedPtr client_node;
62+
rclcpp::Node::SharedPtr server_node;
63+
rclcpp_action::Client<Fibonacci>::SharedPtr action_client;
64+
rclcpp_action::Server<Fibonacci>::SharedPtr action_server;
65+
rclcpp_action::Client<Fibonacci>::SendGoalOptions send_goal_options;
66+
Fibonacci::Goal goal_msg;
67+
std::shared_ptr<TestInfo> test_info;
68+
};
69+
70+
INSTANTIATE_TEST_SUITE_P(
71+
ActionsTest,
72+
ActionsTest,
73+
testing::Values(
74+
/* <UseEventsExecutor> <ServerIsIntraProcess> <ClientIsIntraProcess> */
75+
actions_test_data_t{ false, false, false },
76+
actions_test_data_t{ false, false, true },
77+
actions_test_data_t{ false, true, false },
78+
actions_test_data_t{ false, true, true },
79+
actions_test_data_t{ true, false, false },
80+
actions_test_data_t{ true, false, true },
81+
actions_test_data_t{ true, true, false },
82+
actions_test_data_t{ true, true, true }
83+
));
84+
85+
TEST_P(ActionsTest, SucceedGoal)
86+
{
87+
executor->add_node(server_node);
88+
executor->add_node(client_node);
89+
90+
bool server_available = action_client->wait_for_action_server(std::chrono::seconds(1));
91+
ASSERT_TRUE(server_available);
92+
93+
auto goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options);
94+
auto accepted_response_wait = goal_handle_future.wait_for(std::chrono::seconds(5));
95+
ASSERT_TRUE(accepted_response_wait == std::future_status::ready) << "Goal was rejected by server";
96+
97+
auto goal_handle = goal_handle_future.get();
98+
ASSERT_TRUE(goal_handle != nullptr) << "Invalid goal";
99+
100+
auto result_future = action_client->async_get_result(goal_handle);
101+
102+
test_info->succeed_goal();
103+
104+
auto result_wait = result_future.wait_for(std::chrono::seconds(5));
105+
ASSERT_TRUE(result_wait == std::future_status::ready) << "Goal not completed on time";
106+
107+
auto wrapped_result = result_future.get();
108+
EXPECT_EQ(wrapped_result.code, rclcpp_action::ResultCode::SUCCEEDED);
109+
EXPECT_TRUE(test_info->result_is_correct(
110+
wrapped_result.result->sequence, rclcpp_action::ResultCode::SUCCEEDED));
111+
}
112+
113+
TEST_P(ActionsTest, CancelGoal)
114+
{
115+
executor->add_node(server_node);
116+
executor->add_node(client_node);
117+
send_goal_options.result_callback = nullptr;
118+
119+
bool server_available = action_client->wait_for_action_server(std::chrono::seconds(1));
120+
ASSERT_TRUE(server_available);
121+
122+
auto goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options);
123+
auto accepted_response_wait = goal_handle_future.wait_for(std::chrono::seconds(5));
124+
ASSERT_TRUE(accepted_response_wait == std::future_status::ready) << "Goal was rejected by server";
125+
auto goal_handle = goal_handle_future.get();
126+
ASSERT_TRUE(goal_handle != nullptr) << "Invalid goal";
127+
128+
auto cancel_result_future = action_client->async_cancel_goal(goal_handle);
129+
auto cancel_response_wait = cancel_result_future.wait_for(std::chrono::seconds(5));
130+
ASSERT_TRUE(cancel_response_wait == std::future_status::ready) << "Cancel response not on time";
131+
auto cancel_result = cancel_result_future.get();
132+
ASSERT_TRUE(cancel_result != nullptr) << "Invalid cancel result";
133+
EXPECT_NE(cancel_result->return_code, action_msgs::srv::CancelGoal::Response::ERROR_REJECTED);
134+
135+
std::this_thread::sleep_for(std::chrono::milliseconds(10));
136+
137+
auto result_future = action_client->async_get_result(goal_handle);
138+
auto result_response_wait = result_future.wait_for(std::chrono::seconds(5));
139+
ASSERT_TRUE(result_response_wait == std::future_status::ready) << "Cancel result response not on time";
140+
auto wrapped_result = result_future.get();
141+
EXPECT_EQ(wrapped_result.code, rclcpp_action::ResultCode::CANCELED);
142+
EXPECT_TRUE(test_info->result_is_correct(
143+
wrapped_result.result->sequence, rclcpp_action::ResultCode::CANCELED));
144+
}
145+
146+
TEST_P(ActionsTest, AbortGoal)
147+
{
148+
executor->add_node(server_node);
149+
executor->add_node(client_node);
150+
151+
bool server_available = action_client->wait_for_action_server(std::chrono::seconds(1));
152+
ASSERT_TRUE(server_available);
153+
154+
auto goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options);
155+
auto accepted_response_wait = goal_handle_future.wait_for(std::chrono::seconds(5));
156+
ASSERT_TRUE(accepted_response_wait == std::future_status::ready) << "Goal was rejected by server";
157+
auto goal_handle = goal_handle_future.get();
158+
ASSERT_TRUE(goal_handle != nullptr) << "Invalid goal";
159+
auto result_future = action_client->async_get_result(goal_handle);
160+
161+
test_info->abort_goal();
162+
163+
auto result_wait = result_future.wait_for(std::chrono::seconds(5));
164+
ASSERT_TRUE(result_wait == std::future_status::ready) << "Abort response not arrived";
165+
auto wrapped_result = result_future.get();
166+
EXPECT_EQ(wrapped_result.code, rclcpp_action::ResultCode::ABORTED);
167+
EXPECT_TRUE(test_info->result_is_correct(
168+
wrapped_result.result->sequence, rclcpp_action::ResultCode::ABORTED));
169+
}
170+
171+
TEST_P(ActionsTest, TestReject)
172+
{
173+
executor->add_node(server_node);
174+
executor->add_node(client_node);
175+
176+
bool server_available = action_client->wait_for_action_server(std::chrono::seconds(1));
177+
ASSERT_TRUE(server_available);
178+
179+
goal_msg.order = 21; // Goals over 20 rejected
180+
181+
auto goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options);
182+
auto result_wait = goal_handle_future.wait_for(std::chrono::seconds(5));
183+
ASSERT_TRUE(result_wait == std::future_status::ready) << "Response not arrived";
184+
auto goal_handle = goal_handle_future.get();
185+
186+
ASSERT_TRUE(goal_handle == nullptr);
187+
}
188+
189+
TEST_P(ActionsTest, TestOnReadyCallback)
190+
{
191+
bool server_available = action_client->wait_for_action_server(std::chrono::seconds(1));
192+
ASSERT_TRUE(server_available);
193+
194+
auto goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options);
195+
196+
// Add node: set "on_ready" callback and process the "unread" event
197+
executor->add_node(server_node);
198+
199+
// Give time to server to be executed and generate event into client
200+
std::this_thread::sleep_for(std::chrono::milliseconds(1));
201+
202+
// Add node: set "on_ready" callback and process the "unread" event
203+
executor->add_node(client_node);
204+
auto result_wait = goal_handle_future.wait_for(std::chrono::seconds(5));
205+
ASSERT_TRUE(result_wait == std::future_status::ready) << "Response not arrived";
206+
207+
auto goal_handle = goal_handle_future.get();
208+
ASSERT_TRUE(goal_handle != nullptr) << "Invalid goal";
209+
210+
auto result_future = action_client->async_get_result(goal_handle);
211+
test_info->succeed_goal();
212+
213+
auto succeed_wait = result_future.wait_for(std::chrono::seconds(5));
214+
ASSERT_TRUE(succeed_wait == std::future_status::ready) << "Response not arrived";
215+
216+
auto wrapped_result = result_future.get();
217+
EXPECT_EQ(wrapped_result.code, rclcpp_action::ResultCode::SUCCEEDED);
218+
EXPECT_TRUE(test_info->result_is_correct(
219+
wrapped_result.result->sequence, rclcpp_action::ResultCode::SUCCEEDED));
220+
}
221+
222+
TEST_P(ActionsTest, InstantSuccess)
223+
{
224+
executor->add_node(server_node);
225+
executor->add_node(client_node);
226+
227+
// Unset result callback, we want to test having the result before
228+
// having a callback set
229+
send_goal_options.result_callback = nullptr;
230+
231+
bool server_available = action_client->wait_for_action_server(std::chrono::seconds(1));
232+
ASSERT_TRUE(server_available);
233+
234+
auto goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options);
235+
auto result_wait = goal_handle_future.wait_for(std::chrono::seconds(5));
236+
ASSERT_TRUE(result_wait == std::future_status::ready) << "Response not arrived";
237+
238+
auto goal_handle = goal_handle_future.get();
239+
ASSERT_TRUE(goal_handle != nullptr) << "Invalid goal";
240+
241+
test_info->succeed_goal();
242+
243+
auto result_future = action_client->async_get_result(goal_handle);
244+
auto succeed_wait = result_future.wait_for(std::chrono::seconds(5));
245+
ASSERT_TRUE(succeed_wait == std::future_status::ready) << "Response not arrived";
246+
247+
auto wrapped_result = result_future.get();
248+
EXPECT_EQ(wrapped_result.code, rclcpp_action::ResultCode::SUCCEEDED);
249+
EXPECT_TRUE(test_info->result_is_correct(
250+
wrapped_result.result->sequence, rclcpp_action::ResultCode::SUCCEEDED));
251+
}
252+
253+
// See https://github.com/ros2/rclcpp/issues/2451#issuecomment-1999749919
254+
TEST_P(ActionsTest, FeedbackRace)
255+
{
256+
executor->add_node(server_node);
257+
258+
auto test_params = GetParam();
259+
auto client_executor = test_info->create_executor(test_params.use_events_executor);
260+
client_executor->add_node(client_node);
261+
262+
bool server_available = action_client->wait_for_action_server(std::chrono::seconds(1));
263+
ASSERT_TRUE(server_available);
264+
265+
auto goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options);
266+
267+
rclcpp::Rate rate(double(test_info->server_rate_hz) * 0.4); // A bit slower than the server's feedback rate
268+
269+
for (size_t i = 0; i < 10 && !test_info->result_callback_called(); i++) {
270+
rate.sleep();
271+
client_executor->spin_some();
272+
if (i == 5) {
273+
test_info->succeed_goal();
274+
}
275+
}
276+
277+
EXPECT_TRUE(test_info->result_callback_called());
278+
}
279+
280+
int main(int argc, char **argv)
281+
{
282+
::testing::InitGoogleTest(&argc, argv);
283+
return RUN_ALL_TESTS();
284+
}

0 commit comments

Comments
 (0)