From 8303851836ac94acd9102be993d7d0045440c44e Mon Sep 17 00:00:00 2001 From: Skyler Medeiros Date: Thu, 15 Jan 2026 12:53:37 -0800 Subject: [PATCH 1/8] 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 2d39c4c78cdd535af171ca6956819693e56738d2 Mon Sep 17 00:00:00 2001 From: Skyler Medeiros Date: Fri, 16 Jan 2026 13:37:07 -0800 Subject: [PATCH 2/8] 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 ddcf67bbd952e36d91df88be1f40ecf4c414f9d9 Mon Sep 17 00:00:00 2001 From: Skyler Medeiros Date: Fri, 16 Jan 2026 13:37:35 -0800 Subject: [PATCH 3/8] 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 6cb15b7391..eac78570b0 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) @@ -1022,6 +1044,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 01b4670d61789d3d79b9afea023e8a6ef0a28830 Mon Sep 17 00:00:00 2001 From: Skyler Medeiros Date: Fri, 16 Jan 2026 17:45:08 -0800 Subject: [PATCH 4/8] error handling Signed-off-by: Skyler Medeiros --- rclcpp_action/src/server.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 621dff856d..1fea7cf311 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -158,7 +158,15 @@ ServerBase::ServerBase( rcl_node_t * rcl_node = node_base->get_rcl_node_handle(); 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), From 3e19b93a1b6f878581b5cbcc28af466b2811bf1a Mon Sep 17 00:00:00 2001 From: Skyler Medeiros Date: Fri, 16 Jan 2026 17:45:41 -0800 Subject: [PATCH 5/8] cancel the timer in the destructor Signed-off-by: Skyler Medeiros --- rclcpp_action/src/server.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 1fea7cf311..c256908798 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -159,7 +159,7 @@ ServerBase::ServerBase( std::function timer_callback = [this] () { try { - execute_check_expired_goals(); + execute_check_expired_goals(); } catch (const rclcpp::exceptions::RCLError & ex) { RCLCPP_ERROR( @@ -195,6 +195,7 @@ ServerBase::ServerBase( ServerBase::~ServerBase() { + pimpl_->expire_timer_->cancel(); } size_t From f0c726cd058cacdc7c1c419412f9c771a5e5ac3e Mon Sep 17 00:00:00 2001 From: Skyler Medeiros Date: Sat, 17 Jan 2026 00:30:42 -0800 Subject: [PATCH 6/8] cpplint Signed-off-by: Skyler Medeiros --- rclcpp_action/src/server.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index c256908798..e9645b7acf 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -158,15 +158,15 @@ ServerBase::ServerBase( rcl_node_t * rcl_node = node_base->get_rcl_node_handle(); std::function timer_callback = [this] () { - try { - execute_check_expired_goals(); - } - catch (const rclcpp::exceptions::RCLError & ex) { - RCLCPP_ERROR( + 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), From 612d0e43d66a741f4a061a3ed51604a32f6fe905 Mon Sep 17 00:00:00 2001 From: Skyler Medeiros Date: Sat, 17 Jan 2026 02:05:35 -0800 Subject: [PATCH 7/8] 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 eac78570b0..93707d9d22 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; } }; @@ -1046,17 +1058,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}}, @@ -1096,7 +1108,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()); @@ -1117,9 +1130,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)); @@ -1128,17 +1138,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 94ed627b86b4bfab670cedd2f5d670be92d88540 Mon Sep 17 00:00:00 2001 From: Skyler Medeiros Date: Mon, 19 Jan 2026 11:26:43 -0800 Subject: [PATCH 8/8] 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 93707d9d22..a7c109d868 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -1138,13 +1138,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);