Skip to content

Commit

Permalink
Finish rcl_action_clear_expired_goals() implementation
Browse files Browse the repository at this point in the history 
* Change internal goal handle array to be an array of pointers.
* Add check for invalid action names
  • Loading branch information
@jacobperron
jacobperron committed Nov 13, 2018
1 parent 7c3b20f commit 539bd5e
Show file tree
Hide file tree
Showing 3 changed files with 79 additions and 42 deletions.
16 changes: 8 additions & 8 deletions rcl_action/include/rcl_action/action_server.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -746,17 +746,13 @@ RCL_WARN_UNUSED
const rcl_action_server_options_t *
rcl_action_server_get_options(const rcl_action_server_t * action_server);

/// Return the goal handles for all active or terminated goals.
/// Get the goal handles for all active or terminated goals.
/**
* A pointer to the internally held array of goal handle structs is returned
* A pointer to the internally held array of pointers to goal handle structs is returned
* along with the number of items in the array.
* Goals that have terminated, successfully responded to a client with a
* result, and have expired (timed out) are not present in the array.
*
* This function can fail, and therefore return `NULL`, if the:
* - action server is `NULL`
* - action server is invalid (never called init, called fini, or invalid)
*
* The returned handle is made invalid if the action server is finalized or if
* rcl_shutdown() is called.
* The returned handle is not guaranteed to be valid for the life time of the
Expand All @@ -774,15 +770,19 @@ rcl_action_server_get_options(const rcl_action_server_t * action_server);
* Lock-Free | Yes
*
* \param[in] action_server pointer to the rcl action server
* \param[out] goal_handles is set to the array of pointers to goal handles if successful.
* \param[out] num_goals is set to the number of goals in the returned array if successful,
* not set otherwise.
* \return pointer to an array goal handles if successful, otherwise `NULL`
* \return RCL_RET_OK if successful, or
* \return RCL_RET_ACTION_SERVER_INVALID if the action server is invalid, or
* \return RCL_RET_INVALID_ARGUMENT if any arguments are invalid
*/
RCL_ACTION_PUBLIC
RCL_WARN_UNUSED
const rcl_action_goal_handle_t *
rcl_ret_t
rcl_action_server_get_goal_handles(
const rcl_action_server_t * action_server,
rcl_action_goal_handle_t *** goal_handles,
size_t * num_goals);

/// Check if a goal is already being tracked by an action server
Expand Down
92 changes: 61 additions & 31 deletions rcl_action/src/rcl_action/action_server.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ typedef struct rcl_action_server_impl_t
char * action_name;
rcl_action_server_options_t options;
// Array of goal handles
rcl_action_goal_handle_t * goal_handles;
rcl_action_goal_handle_t ** goal_handles;
size_t num_goal_handles;
// Clock
rcl_clock_t clock;
Expand Down Expand Up @@ -83,6 +83,8 @@ rcl_action_get_zero_initialized_server(void)
if (RCL_RET_OK != ret) { \
if (RCL_RET_BAD_ALLOC == ret) { \
ret = RCL_RET_BAD_ALLOC; \
} else if (RCL_RET_SERVICE_NAME_INVALID == ret) { \
ret = RCL_RET_ACTION_NAME_INVALID; \
} else { \
ret = RCL_RET_ERROR; \
} \
Expand Down Expand Up @@ -115,6 +117,8 @@ rcl_action_get_zero_initialized_server(void)
if (RCL_RET_OK != ret) { \
if (RCL_RET_BAD_ALLOC == ret) { \
ret = RCL_RET_BAD_ALLOC; \
} else if (RCL_RET_TOPIC_NAME_INVALID == ret) { \
ret = RCL_RET_ACTION_NAME_INVALID; \
} else { \
ret = RCL_RET_ERROR; \
} \
Expand Down Expand Up @@ -158,9 +162,6 @@ rcl_action_server_init(
return RCL_RET_ERROR;
}

// TODO(jacobperron): Expand the given action name
// Test if name is valid

// Initialize services
SERVICE_INIT(goal);
SERVICE_INIT(cancel);
Expand All @@ -180,11 +181,11 @@ rcl_action_server_init(
// Copy options
action_server->impl->options = *options;

// Initialize goal handle array
action_server->impl->goal_handles = NULL;
action_server->impl->num_goal_handles = 0;

ret = RCL_RET_OK;
goto cleanup;
return RCL_RET_OK;
fail:
{
// Finalize any services/publishers that were initialized and deallocate action_server->impl
Expand All @@ -193,8 +194,6 @@ rcl_action_server_init(
// the action servers members
(void)ret_throwaway;
}
cleanup:
// TODO(jacobperron) Cleanup from expanded action name
return ret;
}

Expand Down Expand Up @@ -335,20 +334,28 @@ rcl_action_accept_new_goal(
return NULL;
}

// Allocate space for a new goal handle
// Allocate space in the goal handle pointer array
rcl_allocator_t allocator = action_server->impl->options.allocator;
rcl_action_goal_handle_t * goal_handles = action_server->impl->goal_handles;
rcl_action_goal_handle_t ** goal_handles = action_server->impl->goal_handles;
const size_t num_goal_handles = action_server->impl->num_goal_handles;
// TODO(jacobperron): Don't allocate for every accepted goal handle,
// instead double the memory allocated if needed.
const size_t new_num_goal_handles = num_goal_handles + 1;
void * tmp_ptr = allocator.reallocate(
goal_handles, new_num_goal_handles * sizeof(rcl_action_goal_handle_t), allocator.state);
goal_handles, new_num_goal_handles * sizeof(rcl_action_goal_handle_t *), allocator.state);
if (!tmp_ptr) {
RCL_SET_ERROR_MSG("memory allocation failed for goal handle pointer");
return NULL;
}
goal_handles = (rcl_action_goal_handle_t **)tmp_ptr;

// Allocate space for a new goal handle
tmp_ptr = allocator.allocate(sizeof(rcl_action_goal_handle_t), allocator.state);
if (!tmp_ptr) {
RCL_SET_ERROR_MSG("memory allocation failed for new goal handle");
return NULL;
}
goal_handles = (rcl_action_goal_handle_t *)tmp_ptr;
goal_handles[num_goal_handles] = (rcl_action_goal_handle_t *) tmp_ptr;

// Re-stamp goal info with current time
rcl_action_goal_info_t goal_info_stamp_now = rcl_action_get_zero_initialized_goal_info();
Expand All @@ -361,17 +368,17 @@ rcl_action_accept_new_goal(
_nanosec_to_goal_info_stamp(&now_time_point, &goal_info_stamp_now);

// Create a new goal handle
goal_handles[num_goal_handles] = rcl_action_get_zero_initialized_goal_handle();
*goal_handles[num_goal_handles] = rcl_action_get_zero_initialized_goal_handle();
ret = rcl_action_goal_handle_init(
&goal_handles[num_goal_handles], &goal_info_stamp_now, allocator);
goal_handles[num_goal_handles], &goal_info_stamp_now, allocator);
if (RCL_RET_OK != ret) {
RCL_SET_ERROR_MSG("failed to initialize goal handle");
return NULL;
}

action_server->impl->goal_handles = goal_handles;
action_server->impl->num_goal_handles = new_num_goal_handles;
return &goal_handles[num_goal_handles];
return goal_handles[num_goal_handles];
}

rcl_ret_t
Expand Down Expand Up @@ -421,12 +428,12 @@ rcl_action_get_goal_status_array(
// Populate status array
for (size_t i = 0; i < num_goals; ++i) {
ret = rcl_action_goal_handle_get_info(
&action_server->impl->goal_handles[i], &status_message->msg.status_list.data[i].goal_info);
action_server->impl->goal_handles[i], &status_message->msg.status_list.data[i].goal_info);
if (RCL_RET_OK != ret) {
return RCL_RET_ERROR;
}
ret = rcl_action_goal_handle_get_status(
&action_server->impl->goal_handles[i], &status_message->msg.status_list.data[i].status);
action_server->impl->goal_handles[i], &status_message->msg.status_list.data[i].status);
if (RCL_RET_OK != ret) {
return RCL_RET_ERROR;
}
Expand Down Expand Up @@ -484,14 +491,18 @@ rcl_action_clear_expired_goals(
return RCL_RET_ERROR;
}

// Used for shrinking goal handle array
rcl_allocator_t allocator = action_server->impl->options.allocator;

*num_expired = 0u;
rcl_ret_t ret_final = RCL_RET_OK;
const int64_t timeout = (int64_t)action_server->impl->options.result_timeout.nanoseconds;
rcl_action_goal_handle_t * goal_handle;
rcl_action_goal_info_t goal_info;
int64_t goal_time;
for (size_t i = 0; i < action_server->impl->num_goal_handles; ++i) {
goal_handle = &action_server->impl->goal_handles[i];
size_t num_goal_handles = action_server->impl->num_goal_handles;
for (size_t i = 0; i < num_goal_handles; ++i) {
goal_handle = action_server->impl->goal_handles[i];
// Expiration only applys to terminated goals
if (rcl_action_goal_handle_is_active(goal_handle)) {
continue;
Expand All @@ -502,14 +513,30 @@ rcl_action_clear_expired_goals(
assert(current_time > goal_time);
if ((current_time - goal_time) > timeout) {
// Stop tracking goal handle
// ret = rcl_action_goal_handle_fini(goal_handle);
// if (RCL_RET_OK != ret) {
// ret_final = RCL_RET_ERROR;
// }
// action_server->impl->goal_handles[i] = NULL;
ret = rcl_action_goal_handle_fini(goal_handle);
if (RCL_RET_OK != ret) {
ret_final = RCL_RET_ERROR;
}
// Fill in any gaps left in the array with pointers from the end
action_server->impl->goal_handles[i] = action_server->impl->goal_handles[num_goal_handles];
action_server->impl->goal_handles[num_goal_handles--] = NULL;

++(*num_expired);
}
}

// Shrink goal handle array if some goals expired
if (*num_expired > 0u) {
void * tmp_ptr = allocator.reallocate(
action_server->impl->goal_handles, num_goal_handles, allocator.state);
if (!tmp_ptr) {
RCL_SET_ERROR_MSG("failed to shrink size of goal handle array");
ret_final = RCL_RET_ERROR;
} else {
action_server->impl->goal_handles = (rcl_action_goal_handle_t **)tmp_ptr;
action_server->impl->num_goal_handles = num_goal_handles;
}
}
return ret_final;
}

Expand Down Expand Up @@ -550,7 +577,7 @@ rcl_action_process_cancel_request(
rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info();
rcl_action_goal_handle_t * goal_handle;
for (size_t i = 0; i < total_num_goals; ++i) {
goal_handle = &action_server->impl->goal_handles[i];
goal_handle = action_server->impl->goal_handles[i];
rcl_ret_t ret = rcl_action_goal_handle_get_info(goal_handle, &goal_info);
assert(RCL_RET_OK == ret);

Expand All @@ -573,7 +600,7 @@ rcl_action_process_cancel_request(
rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info();
rcl_action_goal_handle_t * goal_handle;
for (size_t i = 0; i < total_num_goals; ++i) {
goal_handle = &action_server->impl->goal_handles[i];
goal_handle = action_server->impl->goal_handles[i];
rcl_ret_t ret = rcl_action_goal_handle_get_info(goal_handle, &goal_info);
assert(RCL_RET_OK == ret);

Expand Down Expand Up @@ -645,17 +672,20 @@ rcl_action_server_get_options(const rcl_action_server_t * action_server)
return &action_server->impl->options;
}

const rcl_action_goal_handle_t *
rcl_ret_t
rcl_action_server_get_goal_handles(
const rcl_action_server_t * action_server,
rcl_action_goal_handle_t *** goal_handles,
size_t * num_goals)
{
if (!rcl_action_server_is_valid(action_server)) {
return NULL; // error already set
return RCL_RET_ACTION_SERVER_INVALID; // error already set
}
RCL_CHECK_ARGUMENT_FOR_NULL(num_goals, NULL);
RCL_CHECK_ARGUMENT_FOR_NULL(goal_handles, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(num_goals, RCL_RET_INVALID_ARGUMENT);
*goal_handles = action_server->impl->goal_handles;
*num_goals = action_server->impl->num_goal_handles;
return action_server->impl->goal_handles;
return RCL_RET_OK;
}

bool
Expand All @@ -671,7 +701,7 @@ rcl_action_server_goal_exists(
rcl_action_goal_info_t gh_goal_info = rcl_action_get_zero_initialized_goal_info();
rcl_ret_t ret;
for (size_t i = 0; i < action_server->impl->num_goal_handles; ++i) {
ret = rcl_action_goal_handle_get_info(&action_server->impl->goal_handles[i], &gh_goal_info);
ret = rcl_action_goal_handle_get_info(action_server->impl->goal_handles[i], &gh_goal_info);
if (RCL_RET_OK != ret) {
RCL_SET_ERROR_MSG("failed to get info for goal handle");
return false;
Expand Down
13 changes: 10 additions & 3 deletions rcl_action/test/rcl_action/test_action_server.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -207,10 +207,12 @@ TEST_F(TestActionServer, test_action_server_accept_new_goal)
ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
EXPECT_TRUE(uuidcmp(goal_info_out.uuid, goal_info_in.uuid));
size_t num_goals;
const rcl_action_goal_handle_t * goal_handle_array = rcl_action_server_get_goal_handles(
&this->action_server, &num_goals);
rcl_action_goal_handle_t ** goal_handle_array;
ret = rcl_action_server_get_goal_handles(&this->action_server, &goal_handle_array, &num_goals);
ASSERT_EQ(ret, RCL_RET_OK);
EXPECT_EQ(num_goals, 1u);
EXPECT_NE(goal_handle_array, nullptr) << rcl_get_error_string().str;
EXPECT_NE(goal_handle_array[0], nullptr) << rcl_get_error_string().str;

// Accept with the same goal ID
goal_handle = rcl_action_accept_new_goal(&this->action_server, &goal_info_in);
Expand All @@ -225,9 +227,12 @@ TEST_F(TestActionServer, test_action_server_accept_new_goal)
ret = rcl_action_goal_handle_get_info(goal_handle, &goal_info_out);
ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
EXPECT_TRUE(uuidcmp(goal_info_out.uuid, goal_info_in.uuid));
goal_handle_array = rcl_action_server_get_goal_handles(&this->action_server, &num_goals);
ret = rcl_action_server_get_goal_handles(&this->action_server, &goal_handle_array, &num_goals);
ASSERT_EQ(ret, RCL_RET_OK);
EXPECT_EQ(num_goals, 2u);
EXPECT_NE(goal_handle_array, nullptr) << rcl_get_error_string().str;
EXPECT_NE(goal_handle_array[0], nullptr) << rcl_get_error_string().str;
EXPECT_NE(goal_handle_array[1], nullptr) << rcl_get_error_string().str;
}

TEST_F(TestActionServer, test_action_clear_expired_goals)
Expand All @@ -253,6 +258,8 @@ TEST_F(TestActionServer, test_action_clear_expired_goals)
ret = rcl_action_clear_expired_goals(&this->action_server, &num_expired);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
EXPECT_EQ(num_expired, 0u);

// TODO(jacobperron): Test with goals that actually expire
}

TEST_F(TestActionServer, test_action_process_cancel_request)
Expand Down

0 comments on commit 539bd5e

Please sign in to comment.