From eadbaf031c6b9b54a3d5e903bc46ef78b9f2957b Mon Sep 17 00:00:00 2001 From: Skyler Medeiros Date: Thu, 15 Jan 2026 12:53:37 -0800 Subject: [PATCH 1/7] call execute_check_expired_goals from the timer callback Signed-off-by: Skyler Medeiros --- rclcpp_action/src/server.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index d3812f18ba..90b2bd4274 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -157,9 +157,9 @@ 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] () { + execute_check_expired_goals(); + }; 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); From 45da4a07516a0e43df109af8a1b452434b2877ed Mon Sep 17 00:00:00 2001 From: Skyler Medeiros Date: Fri, 16 Jan 2026 13:37:07 -0800 Subject: [PATCH 2/7] Add get_number_of_goal_handles Signed-off-by: Skyler Medeiros --- rclcpp_action/include/rclcpp_action/server.hpp | 4 ++++ rclcpp_action/src/server.cpp | 11 +++++++++-- 2 files changed, 13 insertions(+), 2 deletions(-) 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 90b2bd4274..621dff856d 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -158,8 +158,8 @@ ServerBase::ServerBase( rcl_node_t * rcl_node = node_base->get_rcl_node_handle(); std::function timer_callback = [this] () { - execute_check_expired_goals(); - }; + execute_check_expired_goals(); + }; 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); @@ -639,6 +639,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() { From a54c66c09f7e63b34c07bd3a4f1deea0abfba234 Mon Sep 17 00:00:00 2001 From: Skyler Medeiros Date: Fri, 16 Jan 2026 13:37:35 -0800 Subject: [PATCH 3/7] Add a test for goal expiration when using the events executor Signed-off-by: Skyler Medeiros --- rclcpp_action/test/test_server.cpp | 124 ++++++++++++++++++++++++++++- 1 file changed, 122 insertions(+), 2 deletions(-) diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index cc0aa015d1..6b6b8736b5 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -49,6 +49,7 @@ class TestServer : public ::testing::Test std::shared_ptr send_goal_request( rclcpp::Node::SharedPtr node, GoalUUID uuid, + rclcpp::Executor & executor, std::chrono::milliseconds timeout = std::chrono::milliseconds(-1)) { auto client = node->create_client( @@ -59,7 +60,8 @@ 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 = 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,9 +71,19 @@ 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::executors::SingleThreadedExecutor ex; + return send_goal_request(node, uuid, ex, timeout); + } + CancelResponse::SharedPtr send_cancel_request( rclcpp::Node::SharedPtr node, GoalUUID uuid, + rclcpp::Executor & executor, std::chrono::milliseconds timeout = std::chrono::milliseconds(-1)) { auto cancel_client = node->create_client( @@ -82,7 +94,8 @@ 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 = 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 +104,15 @@ 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::executors::SingleThreadedExecutor ex; + return send_cancel_request(node, uuid, ex, timeout); + } }; TEST_F(TestServer, construction_and_destruction) @@ -1004,6 +1026,104 @@ TEST_F(TestServer, deferred_execution) EXPECT_TRUE(received_handle->is_executing()); } +TEST_F(TestServer, goals_expired_with_events_executor) +{ + + rclcpp::ExecutorOptions opts; + + // 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. + + rclcpp::experimental::executors::EventsExecutor executor(opts); + auto node = std::make_shared("expire_goals", "/rclcpp_action/expire_goals"); + 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) { + send_goal_request(node, uuid, executor); + + 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 + + executor.add_node(node); + + 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); + + ASSERT_TRUE(as->get_number_of_goal_handles() != 0); + + // Wait for goal expiration + rclcpp::sleep_for(2 * result_timeout); + + // Allow for expiration to take place + executor.spin_some(); + executor.remove_node(node); + } + + ASSERT_TRUE(as->get_number_of_goal_handles() == 0); +} + + class TestBasicServer : public TestServer { public: From 847dd0200288bdf4b25da55a0e4401e95cf05378 Mon Sep 17 00:00:00 2001 From: Skyler Medeiros Date: Sat, 17 Jan 2026 02:05:35 -0800 Subject: [PATCH 4/7] cleanup tests Signed-off-by: Skyler Medeiros --- rclcpp_action/test/test_server.cpp | 48 ++++++++++++++++++------------ 1 file changed, 29 insertions(+), 19 deletions(-) diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 6b6b8736b5..9242bcf8c1 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -50,7 +50,8 @@ class TestServer : public ::testing::Test send_goal_request( rclcpp::Node::SharedPtr node, GoalUUID uuid, rclcpp::Executor & executor, - std::chrono::milliseconds timeout = std::chrono::milliseconds(-1)) + std::chrono::milliseconds timeout = std::chrono::milliseconds(-1), + bool executor_owns_node = false) { auto client = node->create_client( "fibonacci/_action/send_goal"); @@ -60,7 +61,9 @@ 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::executors::spin_node_until_future_complete(executor, + 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; @@ -76,15 +79,19 @@ class TestServer : public ::testing::Test rclcpp::Node::SharedPtr node, GoalUUID uuid, std::chrono::milliseconds timeout = std::chrono::milliseconds(-1)) { - rclcpp::executors::SingleThreadedExecutor ex; - return send_goal_request(node, uuid, ex, timeout); + 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, rclcpp::Executor & executor, - std::chrono::milliseconds timeout = std::chrono::milliseconds(-1)) + std::chrono::milliseconds timeout = std::chrono::milliseconds(-1), + bool executor_owns_node = false) { auto cancel_client = node->create_client( "fibonacci/_action/cancel_goal"); @@ -94,7 +101,9 @@ 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::executors::spin_node_until_future_complete(executor, + 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(); @@ -110,8 +119,11 @@ class TestServer : public ::testing::Test rclcpp::Node::SharedPtr node, GoalUUID uuid, std::chrono::milliseconds timeout = std::chrono::milliseconds(-1)) { - rclcpp::executors::SingleThreadedExecutor ex; - return send_cancel_request(node, uuid, ex, timeout); + 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; } }; @@ -1028,17 +1040,17 @@ TEST_F(TestServer, deferred_execution) TEST_F(TestServer, goals_expired_with_events_executor) { - - rclcpp::ExecutorOptions opts; - // 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); - auto node = std::make_shared("expire_goals", "/rclcpp_action/expire_goals"); + 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}}, @@ -1078,7 +1090,8 @@ TEST_F(TestServer, goals_expired_with_events_executor) for (const auto & uuid : uuids) { - send_goal_request(node, uuid, executor); + 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()); @@ -1099,9 +1112,6 @@ TEST_F(TestServer, goals_expired_with_events_executor) received_handle->succeed(result); // Wait for the result request to be received - - executor.add_node(node); - ASSERT_EQ( rclcpp::FutureReturnCode::SUCCESS, executor.spin_until_future_complete(future)); @@ -1110,17 +1120,17 @@ TEST_F(TestServer, goals_expired_with_events_executor) EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status); EXPECT_EQ(result->sequence, response->result.sequence); - ASSERT_TRUE(as->get_number_of_goal_handles() != 0); + ASSERT_NE(as->get_number_of_goal_handles(), 0); // Wait for goal expiration rclcpp::sleep_for(2 * result_timeout); // Allow for expiration to take place executor.spin_some(); - executor.remove_node(node); } + executor.remove_node(node); - ASSERT_TRUE(as->get_number_of_goal_handles() == 0); + ASSERT_EQ(as->get_number_of_goal_handles(), 0); } From 6e7d759030d7d7f1a7bf8139a3b059d67a2f8c24 Mon Sep 17 00:00:00 2001 From: Skyler Medeiros Date: Mon, 19 Jan 2026 11:26:43 -0800 Subject: [PATCH 5/7] spin until timeout or goal handles are cleared Signed-off-by: Skyler Medeiros --- rclcpp_action/test/test_server.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 9242bcf8c1..4f87dfdbbf 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -1120,13 +1120,13 @@ TEST_F(TestServer, goals_expired_with_events_executor) EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status); EXPECT_EQ(result->sequence, response->result.sequence); - ASSERT_NE(as->get_number_of_goal_handles(), 0); - - // Wait for goal expiration - rclcpp::sleep_for(2 * result_timeout); - - // Allow for expiration to take place - executor.spin_some(); + 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); From def4e29c7ddf172eb76af789b1d01e6d75486d6d Mon Sep 17 00:00:00 2001 From: Skyler Medeiros Date: Mon, 1 Jun 2026 09:48:09 -0700 Subject: [PATCH 6/7] constructor was different in kilted Signed-off-by: Skyler Medeiros --- rclcpp_action/test/test_server.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 4f87dfdbbf..7ac186c024 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -1049,7 +1049,10 @@ TEST_F(TestServer, goals_expired_with_events_executor) rclcpp::ExecutorOptions opts; opts.context = node->get_node_base_interface()->get_context(); - rclcpp::experimental::executors::EventsExecutor executor(opts); + rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue = + std::make_unique(); + rclcpp::experimental::executors::EventsExecutor executor + (std::move(events_queue), false, 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}}, From 015988ba6cda84793c752b351443b2e1dba0c4bf Mon Sep 17 00:00:00 2001 From: Skyler Medeiros Date: Mon, 1 Jun 2026 10:06:52 -0700 Subject: [PATCH 7/7] collapse constructor Signed-off-by: Skyler Medeiros --- rclcpp_action/test/test_server.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 7ac186c024..8406b7b5b0 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -1049,10 +1049,8 @@ TEST_F(TestServer, goals_expired_with_events_executor) rclcpp::ExecutorOptions opts; opts.context = node->get_node_base_interface()->get_context(); - rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue = - std::make_unique(); rclcpp::experimental::executors::EventsExecutor executor - (std::move(events_queue), false, opts); + (std::make_unique(), false, 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}},