diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 6d41f351c0..369f47b414 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -205,6 +205,10 @@ class ServerBase : public rclcpp::Waitable const rclcpp::QoS & qos_service_event_pub, rcl_service_introspection_state_t introspection_state); + RCLCPP_ACTION_PUBLIC + size_t + get_number_of_goal_handles(); + protected: RCLCPP_ACTION_PUBLIC ServerBase( diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index d3812f18ba..e9645b7acf 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -157,9 +157,17 @@ ServerBase::ServerBase( rcl_node_t * rcl_node = node_base->get_rcl_node_handle(); - // This timer callback will never be called, we are only interested in - // weather the timer itself becomes ready or not. - std::function timer_callback = [] () {}; + std::function timer_callback = [this] () { + try { + execute_check_expired_goals(); + } catch (const rclcpp::exceptions::RCLError & ex) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp_action"), + "Failed to check for expired goals: %s", ex.what() + ); + } + ; + }; pimpl_->expire_timer_ = std::make_shared>( node_clock->get_clock(), std::chrono::nanoseconds(options.result_timeout.nanoseconds), std::move(timer_callback), node_base->get_context(), false); @@ -187,6 +195,7 @@ ServerBase::ServerBase( ServerBase::~ServerBase() { + pimpl_->expire_timer_->cancel(); } size_t @@ -639,6 +648,13 @@ ServerBase::execute_result_request_received( } } +size_t +ServerBase::get_number_of_goal_handles() +{ + std::lock_guard lock(pimpl_->unordered_map_mutex_); + return pimpl_->goal_handles_.size(); +} + void ServerBase::execute_check_expired_goals() { diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 6cb15b7391..a7c109d868 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -49,7 +49,9 @@ class TestServer : public ::testing::Test std::shared_ptr send_goal_request( rclcpp::Node::SharedPtr node, GoalUUID uuid, - std::chrono::milliseconds timeout = std::chrono::milliseconds(-1)) + rclcpp::Executor & executor, + std::chrono::milliseconds timeout = std::chrono::milliseconds(-1), + bool executor_owns_node = false) { auto client = node->create_client( "fibonacci/_action/send_goal"); @@ -59,7 +61,10 @@ class TestServer : public ::testing::Test auto request = std::make_shared(); request->goal_id.uuid = uuid; auto future = client->async_send_request(request); - auto return_code = rclcpp::spin_until_future_complete(node, future, timeout); + auto return_code = (executor_owns_node) ? + executor.spin_until_future_complete(future, timeout) : + rclcpp::executors::spin_node_until_future_complete(executor, + node->get_node_base_interface(), future, timeout); if (rclcpp::FutureReturnCode::SUCCESS == return_code) { return request; } else if (rclcpp::FutureReturnCode::TIMEOUT == return_code) { @@ -69,10 +74,24 @@ class TestServer : public ::testing::Test } } + std::shared_ptr + send_goal_request( + rclcpp::Node::SharedPtr node, GoalUUID uuid, + std::chrono::milliseconds timeout = std::chrono::milliseconds(-1)) + { + rclcpp::ExecutorOptions options; + options.context = node->get_node_base_interface()->get_context(); + rclcpp::executors::SingleThreadedExecutor executor(options); + auto ret = send_goal_request(node, uuid, executor, timeout); + return ret; + } + CancelResponse::SharedPtr send_cancel_request( rclcpp::Node::SharedPtr node, GoalUUID uuid, - std::chrono::milliseconds timeout = std::chrono::milliseconds(-1)) + rclcpp::Executor & executor, + std::chrono::milliseconds timeout = std::chrono::milliseconds(-1), + bool executor_owns_node = false) { auto cancel_client = node->create_client( "fibonacci/_action/cancel_goal"); @@ -82,7 +101,10 @@ class TestServer : public ::testing::Test auto request = std::make_shared(); request->goal_info.goal_id.uuid = uuid; auto future = cancel_client->async_send_request(request); - auto return_code = rclcpp::spin_until_future_complete(node, future, timeout); + auto return_code = (executor_owns_node) ? + executor.spin_until_future_complete(future, timeout) : + rclcpp::executors::spin_node_until_future_complete(executor, + node->get_node_base_interface(), future, timeout); if (rclcpp::FutureReturnCode::SUCCESS == return_code) { return future.get(); } else if (rclcpp::FutureReturnCode::TIMEOUT == return_code) { @@ -91,6 +113,18 @@ class TestServer : public ::testing::Test throw std::runtime_error("cancel request future didn't complete succesfully"); } } + + CancelResponse::SharedPtr + send_cancel_request( + rclcpp::Node::SharedPtr node, GoalUUID uuid, + std::chrono::milliseconds timeout = std::chrono::milliseconds(-1)) + { + rclcpp::ExecutorOptions options; + options.context = node->get_node_base_interface()->get_context(); + rclcpp::executors::SingleThreadedExecutor executor(options); + auto ret = send_cancel_request(node, uuid, executor, timeout); + return ret; + } }; TEST_F(TestServer, construction_and_destruction) @@ -1022,6 +1056,102 @@ TEST_F(TestServer, deferred_execution) EXPECT_TRUE(received_handle->is_executing()); } +TEST_F(TestServer, goals_expired_with_events_executor) +{ + // Because timer expiration was typically tied to the waitsets for + // the SingleThreadedExecutor and MultiThreadedExecutor, + // We specifically want to test with the EventsExecutor here + // so we can verify the timer based goal expiration works + // and expired goals are properly cleared. + auto node = std::make_shared("expire_goals", "/rclcpp_action/expire_goals"); + rclcpp::ExecutorOptions opts; + opts.context = node->get_node_base_interface()->get_context(); + + rclcpp::experimental::executors::EventsExecutor executor(opts); + executor.add_node(node); + const std::vector uuids{ + {{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}, + {{10, 2, 3, 40, 50, 6, 70, 8, 9, 1, 11, 12, 13, 140, 15, 160}}, + {{12, 23, 34, 45, 50, 6, 70, 8, 9, 100, 11, 120, 13, 140, 15, 170}}, + {{12, 23, 34, 45, 50, 6, 70, 8, 9, 100, 11, 120, 13, 140, 115, 16}} + }; + + 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::ACCEPT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + const std::chrono::milliseconds result_timeout{25}; + + 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); + + + for (const auto & uuid : uuids) { + constexpr bool owns_node {true}; + send_goal_request(node, uuid, executor, std::chrono::milliseconds(-1), owns_node); + + EXPECT_TRUE(received_handle->is_active()); + EXPECT_TRUE(received_handle->is_executing()); + + // 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, + executor.spin_until_future_complete(future)); + + auto response = future.get(); + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status); + EXPECT_EQ(result->sequence, response->result.sequence); + + auto start = std::chrono::steady_clock::now(); + while (as->get_number_of_goal_handles() != 0 && + std::chrono::steady_clock::now() - start < std::chrono::seconds(5)) + { + executor.spin_some(); + rclcpp::sleep_for(std::chrono::milliseconds(10)); + } + } + executor.remove_node(node); + + ASSERT_EQ(as->get_number_of_goal_handles(), 0); +} + + class TestBasicServer : public TestServer { public: