From 0f3e1d3f382a83a3db795b05016e1ebb991c00a0 Mon Sep 17 00:00:00 2001 From: Skyler Medeiros Date: Thu, 15 Jan 2026 12:53:37 -0800 Subject: [PATCH 1/9] 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 1f53f937c6..9c6aba0796 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 a4a8b13952b019a8838e7d249186c31f7090629c Mon Sep 17 00:00:00 2001 From: Skyler Medeiros Date: Fri, 16 Jan 2026 13:37:07 -0800 Subject: [PATCH 2/9] 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 9c6aba0796..b1c328928a 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 5a88770466214d2b703f1f3b72405f52bf188bd1 Mon Sep 17 00:00:00 2001 From: Skyler Medeiros Date: Fri, 16 Jan 2026 13:37:35 -0800 Subject: [PATCH 3/9] 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 8e13147eb954db15e9c9bf00d8b5462edd3b91bf Mon Sep 17 00:00:00 2001 From: Skyler Medeiros Date: Fri, 16 Jan 2026 17:45:08 -0800 Subject: [PATCH 4/9] 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 b1c328928a..bc5b6a1d50 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 4d06eac29dfdc7ba511ae52ee5f5db10ebf6aaab Mon Sep 17 00:00:00 2001 From: Skyler Medeiros Date: Fri, 16 Jan 2026 17:45:41 -0800 Subject: [PATCH 5/9] 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 bc5b6a1d50..1c92b9d910 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 8a90ec199acb86bca570198e90f21da78dd04f03 Mon Sep 17 00:00:00 2001 From: Skyler Medeiros Date: Sat, 17 Jan 2026 00:30:42 -0800 Subject: [PATCH 6/9] 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 1c92b9d910..17fe554f67 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 3fce752e21e8bb2e6fe945fa54409bd33a0b0c9c Mon Sep 17 00:00:00 2001 From: Skyler Medeiros Date: Sat, 17 Jan 2026 02:05:35 -0800 Subject: [PATCH 7/9] 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 0aaab92cc86745e19637b32e83b2af026ea83e12 Mon Sep 17 00:00:00 2001 From: Skyler Medeiros Date: Mon, 19 Jan 2026 11:26:43 -0800 Subject: [PATCH 8/9] 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); From dbec660ac8a033622ba63779c83d5b8ac38a9164 Mon Sep 17 00:00:00 2001 From: Skyler Medeiros Date: Fri, 13 Mar 2026 11:19:42 -0700 Subject: [PATCH 9/9] use new rcl set expire callback API Signed-off-by: Skyler Medeiros --- rclcpp_action/src/server.cpp | 23 +++++++++++------------ 1 file changed, 11 insertions(+), 12 deletions(-) diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 17fe554f67..93ed976d55 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -157,17 +157,10 @@ 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() - ); - } - ; - }; + // This timer callback will be exchanged at the RCL layer + // with a _timer_ callback that will call the _event_ callback + // passed in by set_on_ready_callback. + std::function timer_callback = [&] () {}; 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); @@ -827,6 +820,7 @@ ServerBase::set_on_ready_callback(std::function callback) set_callback_to_entity(EntityType::GoalService, callback); set_callback_to_entity(EntityType::ResultService, callback); set_callback_to_entity(EntityType::CancelService, callback); + set_callback_to_entity(EntityType::Expired, callback); } void @@ -926,7 +920,11 @@ ServerBase::set_on_ready_callback( case EntityType::Expired: { - throw std::runtime_error("Expired entity type does not support callbacks"); + ret = rcl_action_server_set_expired_event_callback( + pimpl_->action_server_.get(), + callback, + user_data); + break; } } @@ -945,6 +943,7 @@ ServerBase::clear_on_ready_callback() set_on_ready_callback(EntityType::GoalService, nullptr, nullptr); set_on_ready_callback(EntityType::ResultService, nullptr, nullptr); set_on_ready_callback(EntityType::CancelService, nullptr, nullptr); + set_on_ready_callback(EntityType::Expired, nullptr, nullptr); on_ready_callback_set_ = false; }