Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

rcl_action_expire_goals() outputs goals that expire #342

Merged
merged 4 commits into from
Nov 29, 2018
Merged

rcl_action_expire_goals() outputs goals that expire #342

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
11c9de6
rcl_action_expire_goals() outputs goals that expire
sloretz Nov 28, 2018
70f9330
undo things that didn't need to change
sloretz Nov 28, 2018
4fdadc4
style
sloretz Nov 28, 2018
b574a08
Fix use-after-free
sloretz Nov 28, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 11 additions & 2 deletions rcl_action/include/rcl_action/action_server.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
*
* <hr>
* Attribute | Adherence
Expand All @@ -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
Expand All @@ -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.
Expand Down
22 changes: 20 additions & 2 deletions rcl_action/src/rcl_action/action_server.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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
Expand Down
33 changes: 27 additions & 6 deletions rcl_action/test/rcl_action/test_action_server.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -149,7 +149,7 @@ class TestActionServer : public ::testing::Test
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);
Expand Down Expand Up @@ -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)
Expand Down