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)