diff --git a/rcl_action/include/rcl_action/action_server.h b/rcl_action/include/rcl_action/action_server.h
index a7e7ba8..0849de2 100644
--- a/rcl_action/include/rcl_action/action_server.h
+++ b/rcl_action/include/rcl_action/action_server.h
@@ -583,8 +583,12 @@ rcl_action_send_result_response(
* \attention If one or more goals are expired then a previously returned goal handle
* array from rcl_action_server_get_goal_handles() becomes invalid.
*
- * `num_expired` is an optional argument. If it is not `NULL`, then it is set to the
- * number of goals that were expired.
+ * `expired_goals`, `expired_goals_capacity` and `num_expired` are optional arguments.
+ * If set to (`NULL`, 0u, `NULL`) then they are not used.
+ * To use them allocate an array with size equal to the maximum number of goals that you want to
+ * expire.
+ * Pass the number of goals the array can hold in as `expired_goals_capacity`.
+ * This function will set `num_expired` to the number of goals that were expired.
*
*
* Attribute | Adherence
@@ -598,6 +602,9 @@ rcl_action_send_result_response(
*
* \param[in] action_server handle to the action server from which expired goals
* will be cleared.
+ * \param[in] expired_goals_allocator allocator to use to allocate expired_goals output
+ * \param[inout] expired_goals the identifiers of goals that expired, or set to `NULL` if unused
+ * \param[inout] expired_goals_capacity the allocated size of `expired_goals`, or 0 if unused
* \param[out] num_expired the number of expired goals, or set to `NULL` if unused
* \return `RCL_RET_OK` if the response was sent successfully, or
* \return `RCL_RET_ACTION_SERVER_INVALID` if the action server is invalid, or
@@ -610,6 +617,8 @@ RCL_WARN_UNUSED
rcl_ret_t
rcl_action_expire_goals(
const rcl_action_server_t * action_server,
+ rcl_action_goal_info_t * expired_goals,
+ size_t expired_goals_capacity,
size_t * num_expired);
/// Take a pending cancel request using an action server.
diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c
index c79db68..90c2bce 100644
--- a/rcl_action/src/rcl_action/action_server.c
+++ b/rcl_action/src/rcl_action/action_server.c
@@ -501,11 +501,21 @@ rcl_action_send_result_response(
rcl_ret_t
rcl_action_expire_goals(
const rcl_action_server_t * action_server,
+ rcl_action_goal_info_t * expired_goals,
+ size_t expired_goals_capacity,
size_t * num_expired)
{
if (!rcl_action_server_is_valid(action_server)) {
return RCL_RET_ACTION_SERVER_INVALID;
}
+ const bool output_expired =
+ NULL != expired_goals && NULL != num_expired && expired_goals_capacity > 0u;
+ if (!output_expired &&
+ (NULL != expired_goals || NULL != num_expired || expired_goals_capacity != 0u))
+ {
+ RCL_SET_ERROR_MSG("expired_goals, expired_goals_capacity, and num_expired inconsistent");
+ return RCL_RET_INVALID_ARGUMENT;
+ }
// Get current time (nanosec)
int64_t current_time;
@@ -525,17 +535,25 @@ rcl_action_expire_goals(
int64_t goal_time;
size_t num_goal_handles = action_server->impl->num_goal_handles;
for (size_t i = 0u; i < num_goal_handles; ++i) {
+ if (output_expired && i >= expired_goals_capacity) {
+ // no more space to output expired goals, so stop expiring them
+ break;
+ }
goal_handle = action_server->impl->goal_handles[i];
// Expiration only applys to terminated goals
if (rcl_action_goal_handle_is_active(goal_handle)) {
continue;
}
- ret = rcl_action_goal_handle_get_info(goal_handle, &goal_info);
+ rcl_action_goal_info_t * info_ptr = &goal_info;
+ if (output_expired) {
+ info_ptr = &(expired_goals[num_goals_expired]);
+ }
+ ret = rcl_action_goal_handle_get_info(goal_handle, info_ptr);
if (RCL_RET_OK != ret) {
ret_final = RCL_RET_ERROR;
continue;
}
- goal_time = _goal_info_stamp_to_nanosec(&goal_info);
+ goal_time = _goal_info_stamp_to_nanosec(info_ptr);
assert(current_time > goal_time);
if ((current_time - goal_time) > timeout) {
// Stop tracking goal handle
diff --git a/rcl_action/test/rcl_action/test_action_server.cpp b/rcl_action/test/rcl_action/test_action_server.cpp
index 9c81e91..8aa2da4 100644
--- a/rcl_action/test/rcl_action/test_action_server.cpp
+++ b/rcl_action/test/rcl_action/test_action_server.cpp
@@ -149,7 +149,7 @@ protected:
rcl_node_options_t node_options = rcl_node_get_default_options();
ret = rcl_node_init(&this->node, "test_action_server_node", "", &node_options);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
- ret = rcl_clock_init(RCL_STEADY_TIME, &this->clock, &allocator);
+ ret = rcl_clock_init(RCL_ROS_TIME, &this->clock, &allocator);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
const rosidl_action_type_support_t * ts = ROSIDL_GET_ACTION_TYPE_SUPPORT(
test_msgs, Fibonacci);
@@ -271,28 +271,49 @@ TEST_F(TestActionServer, test_action_accept_new_goal)
TEST_F(TestActionServer, test_action_clear_expired_goals)
{
+ const size_t capacity = 1u;
+ rcl_action_goal_info_t expired_goals[1u];
size_t num_expired = 1u;
// Clear expired goals with null action server
- rcl_ret_t ret = rcl_action_expire_goals(nullptr, &num_expired);
+ rcl_ret_t ret = rcl_action_expire_goals(nullptr, expired_goals, capacity, &num_expired);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str;
rcl_reset_error();
// Clear with invalid action server
rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server();
- ret = rcl_action_expire_goals(&invalid_action_server, &num_expired);
+ ret = rcl_action_expire_goals(&invalid_action_server, expired_goals, capacity, &num_expired);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str;
rcl_reset_error();
// Clear with valid arguments
- ret = rcl_action_expire_goals(&this->action_server, &num_expired);
+ ret = rcl_action_expire_goals(&this->action_server, expired_goals, capacity, &num_expired);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
EXPECT_EQ(num_expired, 0u);
// Clear with valid arguments (optional num_expired)
- ret = rcl_action_expire_goals(&this->action_server, nullptr);
+ ret = rcl_action_expire_goals(&this->action_server, nullptr, 0u, nullptr);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
- // TODO(jacobperron): Test with goals that actually expire
+ // Test with goals that actually expire
+ // Set ROS time
+ ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&this->clock));
+ ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&this->clock, RCUTILS_S_TO_NS(1)));
+ // Accept a goal to create a new handle
+ rcl_action_goal_info_t goal_info_in = rcl_action_get_zero_initialized_goal_info();
+ init_test_uuid1(goal_info_in.uuid);
+ rcl_action_goal_handle_t * goal_handle =
+ rcl_action_accept_new_goal(&this->action_server, &goal_info_in);
+ ASSERT_NE(goal_handle, nullptr) << rcl_get_error_string().str;
+ // Transition executing to aborted
+ ASSERT_EQ(RCL_RET_OK, rcl_action_update_goal_state(goal_handle, GOAL_EVENT_EXECUTE));
+ ASSERT_EQ(RCL_RET_OK, rcl_action_update_goal_state(goal_handle, GOAL_EVENT_SET_ABORTED));
+ // Set time to something far in the future
+ ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&this->clock, RCUTILS_S_TO_NS(99999)));
+ // Clear with valid arguments
+ ret = rcl_action_expire_goals(&this->action_server, expired_goals, capacity, &num_expired);
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+ EXPECT_EQ(num_expired, 1u);
+ EXPECT_TRUE(uuidcmp(expired_goals[0].uuid, goal_info_in.uuid));
}
TEST_F(TestActionServer, test_action_process_cancel_request)