From 43df9eff37e3930b387a5480fea697e59a0accfb Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 8 Jun 2020 11:03:34 -0300 Subject: [PATCH] Increase rclcpp_action test coverage (#1153) Signed-off-by: Michel Hidalgo --- .../include/rclcpp_action/create_client.hpp | 25 ++- .../include/rclcpp_action/create_server.hpp | 25 ++- rclcpp_action/test/test_client.cpp | 163 +++++++++++++++--- rclcpp_action/test/test_server.cpp | 129 ++++++++++++-- 4 files changed, 277 insertions(+), 65 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/create_client.hpp b/rclcpp_action/include/rclcpp_action/create_client.hpp index 07cf950..7c3f210 100644 --- a/rclcpp_action/include/rclcpp_action/create_client.hpp +++ b/rclcpp_action/include/rclcpp_action/create_client.hpp @@ -59,20 +59,19 @@ create_client( return; } auto shared_node = weak_node.lock(); - if (!shared_node) { - return; - } - // API expects a shared pointer, give it one with a deleter that does nothing. - std::shared_ptr> fake_shared_ptr(ptr, [](Client *) {}); + if (shared_node) { + // API expects a shared pointer, give it one with a deleter that does nothing. + std::shared_ptr> fake_shared_ptr(ptr, [](Client *) {}); - if (group_is_null) { - // Was added to default group - shared_node->remove_waitable(fake_shared_ptr, nullptr); - } else { - // Was added to a specfic group - auto shared_group = weak_group.lock(); - if (shared_group) { - shared_node->remove_waitable(fake_shared_ptr, shared_group); + if (group_is_null) { + // Was added to default group + shared_node->remove_waitable(fake_shared_ptr, nullptr); + } else { + // Was added to a specific group + auto shared_group = weak_group.lock(); + if (shared_group) { + shared_node->remove_waitable(fake_shared_ptr, shared_group); + } } } delete ptr; diff --git a/rclcpp_action/include/rclcpp_action/create_server.hpp b/rclcpp_action/include/rclcpp_action/create_server.hpp index 9d198c8..189bc71 100644 --- a/rclcpp_action/include/rclcpp_action/create_server.hpp +++ b/rclcpp_action/include/rclcpp_action/create_server.hpp @@ -78,20 +78,19 @@ create_server( return; } auto shared_node = weak_node.lock(); - if (!shared_node) { - return; - } - // API expects a shared pointer, give it one with a deleter that does nothing. - std::shared_ptr> fake_shared_ptr(ptr, [](Server *) {}); + if (shared_node) { + // API expects a shared pointer, give it one with a deleter that does nothing. + std::shared_ptr> fake_shared_ptr(ptr, [](Server *) {}); - if (group_is_null) { - // Was added to default group - shared_node->remove_waitable(fake_shared_ptr, nullptr); - } else { - // Was added to a specfic group - auto shared_group = weak_group.lock(); - if (shared_group) { - shared_node->remove_waitable(fake_shared_ptr, shared_group); + if (group_is_null) { + // Was added to default group + shared_node->remove_waitable(fake_shared_ptr, nullptr); + } else { + // Was added to a specific group + auto shared_group = weak_group.lock(); + if (shared_group) { + shared_node->remove_waitable(fake_shared_ptr, shared_group); + } } } delete ptr; diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 8735717..4b631e7 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -32,6 +32,7 @@ #include +#include #include #include #include @@ -73,7 +74,7 @@ protected: rclcpp::init(0, nullptr); } - void SetUp() + void SetUpServer() { rcl_allocator_t allocator = rcl_get_default_allocator(); @@ -211,7 +212,10 @@ protected: ASSERT_TRUE(status_publisher != nullptr); allocator.deallocate(status_topic_name, allocator.state); server_executor.add_node(server_node); + } + void SetUp() override + { client_node = std::make_shared(client_node_name, namespace_name); client_executor.add_node(client_node); @@ -219,7 +223,12 @@ protected: ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(1))); } - void TearDown() + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void TearDownServer() { status_publisher.reset(); feedback_publisher.reset(); @@ -227,6 +236,10 @@ protected: result_service.reset(); goal_service.reset(); server_node.reset(); + } + + void TearDown() override + { client_node.reset(); } @@ -265,11 +278,37 @@ protected: typename rclcpp::Publisher::SharedPtr status_publisher; }; +class TestClientAgainstServer : public TestClient +{ +protected: + void SetUp() override + { + SetUpServer(); + TestClient::SetUp(); + } + + void TearDown() override + { + TestClient::TearDown(); + TearDownServer(); + } +}; + + TEST_F(TestClient, construction_and_destruction) { ASSERT_NO_THROW(rclcpp_action::create_client(client_node, action_name).reset()); } +TEST_F(TestClient, construction_and_destruction_after_node) +{ + ASSERT_NO_THROW( + { + auto action_client = rclcpp_action::create_client(client_node, action_name); + client_node.reset(); + }); +} + TEST_F(TestClient, construction_and_destruction_callback_group) { auto group = client_node->create_callback_group( @@ -285,7 +324,24 @@ TEST_F(TestClient, construction_and_destruction_callback_group) ).reset()); } -TEST_F(TestClient, async_send_goal_no_callbacks) +TEST_F(TestClient, wait_for_action_server) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + EXPECT_FALSE(action_client->wait_for_action_server(0ms)); + EXPECT_FALSE(action_client->wait_for_action_server(100ms)); + auto future = std::async( + std::launch::async, [&action_client]() { + return action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT); + }); + SetUpServer(); + EXPECT_TRUE(future.get()); + TearDownServer(); + + client_node.reset(); // Drop node before action client + EXPECT_THROW(action_client->wait_for_action_server(0ms), rclcpp::exceptions::InvalidNodeError); +} + +TEST_F(TestClientAgainstServer, async_send_goal_no_callbacks) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); @@ -306,7 +362,24 @@ TEST_F(TestClient, async_send_goal_no_callbacks) EXPECT_FALSE(goal_handle->is_result_aware()); } -TEST_F(TestClient, async_send_goal_no_callbacks_wait_for_result) +TEST_F(TestClientAgainstServer, bad_goal_handles) +{ + auto action_client0 = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client0->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 0; + auto future_goal_handle = action_client0->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + + auto action_client1 = rclcpp_action::create_client(client_node, action_name); + using rclcpp_action::exceptions::UnknownGoalHandleError; + EXPECT_THROW(action_client1->async_get_result(goal_handle), UnknownGoalHandleError); + EXPECT_THROW(action_client1->async_cancel_goal(goal_handle), UnknownGoalHandleError); +} + +TEST_F(TestClientAgainstServer, async_send_goal_no_callbacks_wait_for_result) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); @@ -329,13 +402,33 @@ TEST_F(TestClient, async_send_goal_no_callbacks_wait_for_result) EXPECT_EQ(5, wrapped_result.result->sequence[5]); } -TEST_F(TestClient, async_send_goal_with_goal_response_callback_wait_for_result) +TEST_F(TestClientAgainstServer, async_send_goal_no_callbacks_then_invalidate) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); ActionGoal goal; - goal.order = 4; + goal.order = 5; + auto future_goal_handle = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + ASSERT_NE(nullptr, goal_handle); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); + auto future_result = action_client->async_get_result(goal_handle); + EXPECT_TRUE(goal_handle->is_result_aware()); + + action_client.reset(); // Ensure goal handle is invalidated once client goes out of scope + + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_UNKNOWN, goal_handle->get_status()); + using rclcpp_action::exceptions::UnawareGoalHandleError; + EXPECT_THROW(future_result.get(), UnawareGoalHandleError); +} + +TEST_F(TestClientAgainstServer, async_send_goal_with_goal_response_callback_wait_for_result) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + bool goal_response_received = false; auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); send_goal_ops.goal_response_callback = @@ -347,23 +440,37 @@ TEST_F(TestClient, async_send_goal_with_goal_response_callback_wait_for_result) goal_response_received = true; } }; - auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); - dual_spin_until_future_complete(future_goal_handle); - auto goal_handle = future_goal_handle.get(); - EXPECT_TRUE(goal_response_received); - EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); - EXPECT_FALSE(goal_handle->is_feedback_aware()); - EXPECT_FALSE(goal_handle->is_result_aware()); - auto future_result = action_client->async_get_result(goal_handle); - EXPECT_TRUE(goal_handle->is_result_aware()); - dual_spin_until_future_complete(future_result); - auto wrapped_result = future_result.get(); - ASSERT_EQ(5u, wrapped_result.result->sequence.size()); - EXPECT_EQ(3, wrapped_result.result->sequence.back()); + { + ActionGoal bad_goal; + bad_goal.order = -1; + auto future_goal_handle = action_client->async_send_goal(bad_goal, send_goal_ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_FALSE(goal_response_received); + EXPECT_EQ(nullptr, goal_handle); + } + + { + ActionGoal goal; + goal.order = 4; + auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_TRUE(goal_response_received); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); + EXPECT_FALSE(goal_handle->is_feedback_aware()); + EXPECT_FALSE(goal_handle->is_result_aware()); + auto future_result = action_client->async_get_result(goal_handle); + EXPECT_TRUE(goal_handle->is_result_aware()); + dual_spin_until_future_complete(future_result); + auto wrapped_result = future_result.get(); + ASSERT_EQ(5u, wrapped_result.result->sequence.size()); + EXPECT_EQ(3, wrapped_result.result->sequence.back()); + } } -TEST_F(TestClient, async_send_goal_with_feedback_callback_wait_for_result) +TEST_F(TestClientAgainstServer, async_send_goal_with_feedback_callback_wait_for_result) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); @@ -397,7 +504,7 @@ TEST_F(TestClient, async_send_goal_with_feedback_callback_wait_for_result) EXPECT_EQ(5, feedback_count); } -TEST_F(TestClient, async_send_goal_with_result_callback_wait_for_result) +TEST_F(TestClientAgainstServer, async_send_goal_with_result_callback_wait_for_result) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); @@ -432,7 +539,7 @@ TEST_F(TestClient, async_send_goal_with_result_callback_wait_for_result) EXPECT_EQ(3, wrapped_result.result->sequence.back()); } -TEST_F(TestClient, async_get_result_with_callback) +TEST_F(TestClientAgainstServer, async_get_result_with_callback) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); @@ -467,7 +574,7 @@ TEST_F(TestClient, async_get_result_with_callback) EXPECT_EQ(3, wrapped_result.result->sequence.back()); } -TEST_F(TestClient, async_cancel_one_goal) +TEST_F(TestClientAgainstServer, async_cancel_one_goal) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); @@ -485,7 +592,7 @@ TEST_F(TestClient, async_cancel_one_goal) EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); } -TEST_F(TestClient, async_cancel_one_goal_with_callback) +TEST_F(TestClientAgainstServer, async_cancel_one_goal_with_callback) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); @@ -519,7 +626,7 @@ TEST_F(TestClient, async_cancel_one_goal_with_callback) EXPECT_TRUE(cancel_response_received); } -TEST_F(TestClient, async_cancel_all_goals) +TEST_F(TestClientAgainstServer, async_cancel_all_goals) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); @@ -555,7 +662,7 @@ TEST_F(TestClient, async_cancel_all_goals) EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle1->get_status()); } -TEST_F(TestClient, async_cancel_all_goals_with_callback) +TEST_F(TestClientAgainstServer, async_cancel_all_goals_with_callback) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); @@ -605,7 +712,7 @@ TEST_F(TestClient, async_cancel_all_goals_with_callback) EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle1->get_status()); } -TEST_F(TestClient, async_cancel_some_goals) +TEST_F(TestClientAgainstServer, async_cancel_some_goals) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); @@ -636,7 +743,7 @@ TEST_F(TestClient, async_cancel_some_goals) EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status()); } -TEST_F(TestClient, async_cancel_some_goals_with_callback) +TEST_F(TestClientAgainstServer, async_cancel_some_goals_with_callback) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 7c873f5..0ef8aa5 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -82,17 +82,42 @@ TEST_F(TestServer, construction_and_destruction) { auto node = std::make_shared("construct_node", "/rclcpp_action/construct"); - using GoalHandle = rclcpp_action::ServerGoalHandle; - auto as = rclcpp_action::create_server( - node, "fibonacci", - [](const GoalUUID &, std::shared_ptr) { - return rclcpp_action::GoalResponse::REJECT; - }, - [](std::shared_ptr) { - return rclcpp_action::CancelResponse::REJECT; - }, - [](std::shared_ptr) {}); - (void)as; + ASSERT_NO_THROW( + { + using GoalHandle = rclcpp_action::ServerGoalHandle; + auto as = rclcpp_action::create_server( + node, "fibonacci", + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::REJECT; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, + [](std::shared_ptr) {}); + (void)as; + }); +} + +TEST_F(TestServer, construction_and_destruction_after_node) +{ + auto node = std::make_shared("construct_node", "/rclcpp_action/construct"); + + ASSERT_NO_THROW( + { + using GoalHandle = rclcpp_action::ServerGoalHandle; + auto as = rclcpp_action::create_server( + node, "fibonacci", + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::REJECT; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, + [](std::shared_ptr) {}); + (void)as; + + node.reset(); + }); } TEST_F(TestServer, construction_and_destruction_callback_group) @@ -733,6 +758,84 @@ TEST_F(TestServer, get_result) using GoalHandle = rclcpp_action::ServerGoalHandle; + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::REJECT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + const std::chrono::milliseconds result_timeout{50}; + + rcl_action_server_options_t options = rcl_action_server_get_default_options(); + options.result_timeout.nanoseconds = RCL_MS_TO_NS(result_timeout.count()); + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted, + options); + (void)as; + + send_goal_request(node, uuid); + + // Send result request + auto result_client = node->create_client( + "fibonacci/_action/get_result"); + if (!result_client->wait_for_service(std::chrono::seconds(20))) { + throw std::runtime_error("get result service didn't become available"); + } + auto request = std::make_shared(); + request->goal_id.uuid = uuid; + auto future = result_client->async_send_request(request); + + // Send a result + auto result = std::make_shared(); + result->sequence = {5, 8, 13, 21}; + received_handle->succeed(result); + + // Wait for the result request to be received + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future)); + + auto response = future.get(); + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status); + EXPECT_EQ(result->sequence, response->result.sequence); + + // Wait for goal expiration + rclcpp::sleep_for(2 * result_timeout); + + // Allow for expiration to take place + rclcpp::spin_some(node); + + // Send and wait for another result request + future = result_client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future)); + + response = future.get(); + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_UNKNOWN, response->status); +} + +TEST_F(TestServer, get_result_deferred) +{ + auto node = std::make_shared("get_result", "/rclcpp_action/get_result"); + const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 80, 90, 10, 11, 12, 13, 14, 15, 160}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + auto handle_cancel = [](std::shared_ptr) { return rclcpp_action::CancelResponse::REJECT; @@ -763,6 +866,10 @@ TEST_F(TestServer, get_result) request->goal_id.uuid = uuid; auto future = result_client->async_send_request(request); + // Process request first + rclcpp::sleep_for(std::chrono::milliseconds(10)); // Give a chance for the request to be served + rclcpp::spin_some(node); + // Send a result auto result = std::make_shared(); result->sequence = {5, 8, 13, 21};