niklas/rcl
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Add timer to action server to check expired goals + asan fixes (#343)

Browse source 
* Add timer to action server to check expired goals

* Fix leak in action_server

* Fix leaks and heap overflow errors

* Fix leaks in tests
This commit is contained in:
Shane Loretz 2018-11-30 18:24:17 -08:00 • committed by GitHub
parent 128f28499b
commit fd77323b9d
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
6 changed files with 191 additions and 10 deletions
Download patch file Download diff file Expand all files Collapse all files

24
rcl_action/include/rcl_action/action_server.h
View file

@ -343,6 +343,8 @@ rcl_action_send_goal_response(
* rcl_action_send_goal_response().
*
* After calling this function, the action server will start tracking the goal.
* The pointer to the goal handle becomes invalid after `rcl_action_server_fini()` is called.
* The caller becomes responsible for finalizing the goal handle later.
*
* Example usage:
*
@ -621,6 +623,28 @@ rcl_action_expire_goals(
size_t expired_goals_capacity,
size_t * num_expired);
/// Notifies action server that a goal handle reached a terminal state.
/**
* <hr>
* Attribute | Adherence
* ------------------ | -------------
* Allocates Memory | No
* Thread-Safe | No
* Uses Atomics | No
* Lock-Free | Yes
*
* \param[in] action_server handle to the action server
* \return `RCL_RET_OK` if everything is ok, or
* \return `RCL_RET_ACTION_SERVER_INVALID` if the action server is invalid, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
* \return `RCL_RET_ERROR` if an unspecified error occurs.
*/
RCL_ACTION_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_action_notify_goal_done(
const rcl_action_server_t * action_server);
/// Take a pending cancel request using an action server.
/**
* \todo TODO(jacobperron) blocking of take?

4
rcl_action/include/rcl_action/wait.h
View file

@ -258,6 +258,7 @@ rcl_action_client_wait_set_get_entities_ready(
* to take, `false` otherwise
* \param[out] is_result_request_ready `true` if there is a result request message ready
* to take, `false` otherwise
* \param[out] is_goal_expired `true` if there is a goal that expired, `false` otherwise
* \return `RCL_RET_OK` if call is successful, or
* \return `RCL_RET_WAIT_SET_INVALID` if the wait set is invalid, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
@ -272,7 +273,8 @@ rcl_action_server_wait_set_get_entities_ready(
const rcl_action_server_t * action_server,
bool * is_goal_request_ready,
bool * is_cancel_request_ready,
bool * is_result_request_ready);
bool * is_result_request_ready,
bool * is_goal_expired);
#ifdef __cplusplus
}

128
rcl_action/src/rcl_action/action_server.c
View file

@ -42,6 +42,7 @@ typedef struct rcl_action_server_impl_t
rcl_service_t result_service;
rcl_publisher_t feedback_publisher;
rcl_publisher_t status_publisher;
rcl_timer_t expire_timer;
char * action_name;
rcl_action_server_options_t options;
// Array of goal handles
@ -53,6 +54,7 @@ typedef struct rcl_action_server_impl_t
size_t wait_set_goal_service_index;
size_t wait_set_cancel_service_index;
size_t wait_set_result_service_index;
size_t wait_set_expire_timer_index;
} rcl_action_server_impl_t;
rcl_action_server_t
@ -170,6 +172,7 @@ rcl_action_server_init(
action_server->impl->goal_service = rcl_get_zero_initialized_service();
action_server->impl->cancel_service = rcl_get_zero_initialized_service();
action_server->impl->result_service = rcl_get_zero_initialized_service();
action_server->impl->expire_timer = rcl_get_zero_initialized_timer();
action_server->impl->feedback_publisher = rcl_get_zero_initialized_publisher();
action_server->impl->status_publisher = rcl_get_zero_initialized_publisher();
action_server->impl->action_name = NULL;
@ -188,6 +191,19 @@ rcl_action_server_init(
PUBLISHER_INIT(feedback);
PUBLISHER_INIT(status);
// Initialize Timer
ret = rcl_timer_init(
&action_server->impl->expire_timer, clock, node->context, options->result_timeout.nanoseconds,
NULL, allocator);
if (RCL_RET_OK != ret) {
goto fail;
}
// Cancel timer so it doesn't start firing
ret = rcl_timer_cancel(&action_server->impl->expire_timer);
if (RCL_RET_OK != ret) {
goto fail;
}
// Copy clock
action_server->impl->clock = *clock;
@ -236,12 +252,22 @@ rcl_action_server_fini(rcl_action_server_t * action_server, rcl_node_t * node)
if (rcl_publisher_fini(&action_server->impl->status_publisher, node) != RCL_RET_OK) {
ret = RCL_RET_ERROR;
}
// Finalize timer
if (rcl_timer_fini(&action_server->impl->expire_timer) != RCL_RET_OK) {
ret = RCL_RET_ERROR;
}
// Deallocate action name
rcl_allocator_t allocator = action_server->impl->options.allocator;
if (action_server->impl->action_name) {
allocator.deallocate(action_server->impl->action_name, allocator.state);
action_server->impl->action_name = NULL;
}
// Deallocate goal handles storage, but don't fini them.
for (size_t i = 0; i < action_server->impl->num_goal_handles; ++i) {
allocator.deallocate(action_server->impl->goal_handles[i], allocator.state);
}
allocator.deallocate(action_server->impl->goal_handles, allocator.state);
action_server->impl->goal_handles = NULL;
// Deallocate struct
allocator.deallocate(action_server->impl, allocator.state);
action_server->impl = NULL;
@ -395,6 +421,66 @@ rcl_action_accept_new_goal(
return goal_handles[num_goal_handles];
}
// Implementation only
static rcl_ret_t
_recalculate_expire_timer(
rcl_timer_t * expire_timer,
const int64_t timeout,
rcl_action_goal_handle_t ** goal_handles,
size_t num_goal_handles,
rcl_clock_t * clock)
{
size_t num_inactive_goals = 0u;
int64_t minimum_period = 0;
// Get current time (nanosec)
int64_t current_time;
rcl_ret_t ret = rcl_clock_get_now(clock, &current_time);
if (RCL_RET_OK != ret) {
return RCL_RET_ERROR;
}
for (size_t i = 0; i < num_goal_handles; ++i) {
rcl_action_goal_handle_t * goal_handle = goal_handles[i];
if (!rcl_action_goal_handle_is_active(goal_handle)) {
++num_inactive_goals;
rcl_action_goal_info_t goal_info;
ret = rcl_action_goal_handle_get_info(goal_handle, &goal_info);
if (RCL_RET_OK != ret) {
return RCL_RET_ERROR;
}
int64_t delta = timeout - current_time - _goal_info_stamp_to_nanosec(&goal_info);
if (delta < minimum_period) {
minimum_period = delta;
}
}
}
if (0u == num_goal_handles || 0u == num_inactive_goals) {
// No idea when the next goal will expire, so cancel timer
return rcl_timer_cancel(expire_timer);
} else {
if (minimum_period < 0) {
// Time jumped backwards
minimum_period = 0;
}
// Un-cancel timer
ret = rcl_timer_reset(expire_timer);
if (RCL_RET_OK != ret) {
return ret;
}
// Make timer fire when next goal expires
int64_t old_period;
ret = rcl_timer_exchange_period(expire_timer, minimum_period, &old_period);
if (RCL_RET_OK != ret) {
return ret;
}
}
return RCL_RET_OK;
}
rcl_ret_t
rcl_action_publish_feedback(
const rcl_action_server_t * action_server,
@ -554,20 +640,23 @@ rcl_action_expire_goals(
continue;
}
goal_time = _goal_info_stamp_to_nanosec(info_ptr);
assert(current_time > goal_time);
if ((current_time - goal_time) > timeout) {
// Stop tracking goal handle
// Fill in any gaps left in the array with pointers from the end
--num_goal_handles;
action_server->impl->goal_handles[i] = action_server->impl->goal_handles[num_goal_handles];
action_server->impl->goal_handles[num_goal_handles--] = NULL;
allocator.deallocate(action_server->impl->goal_handles[num_goal_handles], allocator.state);
action_server->impl->goal_handles[num_goal_handles] = NULL;
++num_goals_expired;
}
}
// Shrink goal handle array if some goals expired
if (num_goals_expired > 0u) {
// Shrink goal handle array if some goals expired
if (0u == num_goal_handles) {
allocator.deallocate(action_server->impl->goal_handles, allocator.state);
action_server->impl->goal_handles = NULL;
action_server->impl->num_goal_handles = num_goal_handles;
} else {
void * tmp_ptr = allocator.reallocate(
action_server->impl->goal_handles,
@ -582,6 +671,12 @@ rcl_action_expire_goals(
}
}
}
ret_final = _recalculate_expire_timer(
&action_server->impl->expire_timer,
action_server->impl->options.result_timeout.nanoseconds,
action_server->impl->goal_handles,
action_server->impl->num_goal_handles,
&action_server->impl->clock);
// If argument is not null, then set it
if (NULL != num_expired) {
@ -590,6 +685,18 @@ rcl_action_expire_goals(
return ret_final;
}
rcl_ret_t
rcl_action_notify_goal_done(
const rcl_action_server_t * action_server)
{
return _recalculate_expire_timer(
&action_server->impl->expire_timer,
action_server->impl->options.result_timeout.nanoseconds,
action_server->impl->goal_handles,
action_server->impl->num_goal_handles,
&action_server->impl->clock);
}
rcl_ret_t
rcl_action_take_cancel_request(
const rcl_action_server_t * action_server,
@ -841,6 +948,13 @@ rcl_action_wait_set_add_action_server(
if (RCL_RET_OK != ret) {
return ret;
}
ret = rcl_wait_set_add_timer(
wait_set,
&action_server->impl->expire_timer,
&action_server->impl->wait_set_expire_timer_index);
if (RCL_RET_OK != ret) {
return ret;
}
if (NULL != service_index) {
// The goal service was the first added
@ -868,7 +982,7 @@ rcl_action_server_wait_set_get_num_entities(
RCL_CHECK_ARGUMENT_FOR_NULL(num_services, RCL_RET_INVALID_ARGUMENT);
*num_subscriptions = 0u;
*num_guard_conditions = 0u;
*num_timers = 0u;
*num_timers = 1u;
*num_clients = 0u;
*num_services = 3u;
return RCL_RET_OK;
@ -880,7 +994,8 @@ rcl_action_server_wait_set_get_entities_ready(
const rcl_action_server_t * action_server,
bool * is_goal_request_ready,
bool * is_cancel_request_ready,
bool * is_result_request_ready)
bool * is_result_request_ready,
bool * is_goal_expired)
{
RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_WAIT_SET_INVALID);
if (!rcl_action_server_is_valid(action_server)) {
@ -889,14 +1004,17 @@ rcl_action_server_wait_set_get_entities_ready(
RCL_CHECK_ARGUMENT_FOR_NULL(is_goal_request_ready, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(is_cancel_request_ready, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(is_result_request_ready, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(is_goal_expired, RCL_RET_INVALID_ARGUMENT);
const rcl_action_server_impl_t * impl = action_server->impl;
const rcl_service_t * goal_service = wait_set->services[impl->wait_set_goal_service_index];
const rcl_service_t * cancel_service = wait_set->services[impl->wait_set_cancel_service_index];
const rcl_service_t * result_service = wait_set->services[impl->wait_set_result_service_index];
const rcl_timer_t * expire_timer = wait_set->timers[impl->wait_set_expire_timer_index];
*is_goal_request_ready = (&impl->goal_service == goal_service);
*is_cancel_request_ready = (&impl->cancel_service == cancel_service);
*is_result_request_ready = (&impl->result_service == result_service);
*is_goal_expired = (&impl->expire_timer == expire_timer);
return RCL_RET_OK;
}

12
rcl_action/test/rcl_action/test_action_communication.cpp
View file

@ -145,6 +145,7 @@ protected:
bool is_goal_request_ready;
bool is_cancel_request_ready;
bool is_result_request_ready;
bool is_goal_expired;
bool is_feedback_ready;
bool is_status_ready;
@ -189,7 +190,8 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_goal_c
&this->action_server,
&this->is_goal_request_ready,
&this->is_cancel_request_ready,
&this->is_result_request_ready);
&this->is_result_request_ready,
&this->is_goal_expired);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
rcl_reset_error();
@ -306,7 +308,8 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_cancel
&this->action_server,
&this->is_goal_request_ready,
&this->is_cancel_request_ready,
&this->is_result_request_ready);
&this->is_result_request_ready,
&this->is_goal_expired);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
rcl_reset_error();
@ -437,7 +440,8 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_result
&this->action_server,
&this->is_goal_request_ready,
&this->is_cancel_request_ready,
&this->is_result_request_ready);
&this->is_result_request_ready,
&this->is_goal_expired);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
rcl_reset_error();
@ -598,6 +602,7 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_status
ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
action_msgs__msg__GoalStatusArray__fini(&incoming_status_array);
EXPECT_EQ(RCL_RET_OK, rcl_action_goal_handle_fini(goal_handle));
}
TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_feedback_comm)
@ -1117,6 +1122,5 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_stat
ret = rcl_action_goal_status_array_fini(&status_array);
ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
action_msgs__msg__GoalStatusArray__fini(&incoming_status_array);
}

32
rcl_action/test/rcl_action/test_action_server.cpp
View file

@ -15,6 +15,7 @@
#include <chrono>
#include <thread>
#include <vector>
#include "rcl_action/action_server.h"
@ -244,9 +245,12 @@ TEST_F(TestActionServer, test_action_accept_new_goal)
EXPECT_EQ(goal_handle, nullptr);
rcl_reset_error();
std::vector<rcl_action_goal_handle_t> handles;
// Accept with valid arguments
goal_handle = rcl_action_accept_new_goal(&this->action_server, &goal_info_in);
EXPECT_NE(goal_handle, nullptr) << rcl_get_error_string().str;
handles.push_back(*goal_handle);
rcl_action_goal_info_t goal_info_out = rcl_action_get_zero_initialized_goal_info();
rcl_ret_t ret = rcl_action_goal_handle_get_info(goal_handle, &goal_info_out);
ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
@ -269,6 +273,7 @@ TEST_F(TestActionServer, test_action_accept_new_goal)
init_test_uuid1(goal_info_in.uuid);
goal_handle = rcl_action_accept_new_goal(&this->action_server, &goal_info_in);
EXPECT_NE(goal_handle, nullptr) << rcl_get_error_string().str;
handles.push_back(*goal_handle);
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));
@ -278,6 +283,10 @@ TEST_F(TestActionServer, test_action_accept_new_goal)
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;
for (auto & handle : handles) {
EXPECT_EQ(RCL_RET_OK, rcl_action_goal_handle_fini(&handle));
}
}
TEST_F(TestActionServer, test_action_clear_expired_goals)
@ -305,6 +314,8 @@ TEST_F(TestActionServer, test_action_clear_expired_goals)
ret = rcl_action_expire_goals(&this->action_server, nullptr, 0u, nullptr);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
std::vector<rcl_action_goal_handle_t> handles;
// Test with goals that actually expire
// Set ROS time
ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&this->clock));
@ -315,6 +326,7 @@ TEST_F(TestActionServer, test_action_clear_expired_goals)
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;
handles.push_back(*goal_handle);
// 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));
@ -325,6 +337,10 @@ TEST_F(TestActionServer, test_action_clear_expired_goals)
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));
for (auto & handle : handles) {
EXPECT_EQ(RCL_RET_OK, rcl_action_goal_handle_fini(&handle));
}
}
TEST_F(TestActionServer, test_action_process_cancel_request)
@ -390,12 +406,15 @@ TEST_F(TestActionServer, test_action_server_get_goal_status_array)
ret = rcl_action_goal_status_array_fini(&status_array);
ASSERT_EQ(ret, RCL_RET_OK);
std::vector<rcl_action_goal_handle_t> handles;
// Add a goal before getting the status array
rcl_action_goal_info_t goal_info_in = rcl_action_get_zero_initialized_goal_info();
init_test_uuid0(goal_info_in.uuid);
rcl_action_goal_handle_t * goal_handle;
goal_handle = rcl_action_accept_new_goal(&this->action_server, &goal_info_in);
ASSERT_NE(goal_handle, nullptr) << rcl_get_error_string().str;
handles.push_back(*goal_handle);
ret = rcl_action_get_goal_status_array(&this->action_server, &status_array);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
EXPECT_NE(status_array.msg.status_list.data, nullptr);
@ -412,6 +431,7 @@ TEST_F(TestActionServer, test_action_server_get_goal_status_array)
}
goal_handle = rcl_action_accept_new_goal(&this->action_server, &goal_info_in);
ASSERT_NE(goal_handle, nullptr) << rcl_get_error_string().str;
handles.push_back(*goal_handle);
}
ret = rcl_action_get_goal_status_array(&this->action_server, &status_array);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
@ -425,6 +445,9 @@ TEST_F(TestActionServer, test_action_server_get_goal_status_array)
}
ret = rcl_action_goal_status_array_fini(&status_array);
ASSERT_EQ(ret, RCL_RET_OK);
for (auto & handle : handles) {
EXPECT_EQ(RCL_RET_OK, rcl_action_goal_handle_fini(&handle));
}
}
TEST_F(TestActionServer, test_action_server_get_action_name)
@ -480,6 +503,7 @@ protected:
}
goal_handle = rcl_action_accept_new_goal(&this->action_server, &goal_info_in);
ASSERT_NE(goal_handle, nullptr) << rcl_get_error_string().str;
handles.push_back(*goal_handle);
goal_infos_out[i] = rcl_action_get_zero_initialized_goal_info();
ret = rcl_action_goal_handle_get_info(goal_handle, &goal_infos_out[i]);
ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
@ -490,11 +514,15 @@ protected:
void TearDown() override
{
for (auto & handle : handles) {
EXPECT_EQ(RCL_RET_OK, rcl_action_goal_handle_fini(&handle));
}
TestActionServer::TearDown();
}
static const int NUM_GOALS = 10;
rcl_action_goal_info_t goal_infos_out[NUM_GOALS];
std::vector<rcl_action_goal_handle_t> handles;
};
TEST_F(TestActionServerCancelPolicy, test_action_process_cancel_request_all_goals)
@ -516,6 +544,7 @@ TEST_F(TestActionServerCancelPolicy, test_action_process_cancel_request_all_goal
EXPECT_EQ(goal_info_out->uuid[j], static_cast<uint8_t>(i + j));
}
}
EXPECT_EQ(RCL_RET_OK, rcl_action_cancel_response_fini(&cancel_response));
}
TEST_F(TestActionServerCancelPolicy, test_action_process_cancel_request_single_goal)
@ -531,6 +560,7 @@ TEST_F(TestActionServerCancelPolicy, test_action_process_cancel_request_single_g
ASSERT_EQ(cancel_response.msg.goals_canceling.size, 1u);
rcl_action_goal_info_t * goal_info = &cancel_response.msg.goals_canceling.data[0];
EXPECT_TRUE(uuidcmp(goal_info->uuid, cancel_request.goal_info.uuid));
EXPECT_EQ(RCL_RET_OK, rcl_action_cancel_response_fini(&cancel_response));
}
TEST_F(TestActionServerCancelPolicy, test_action_process_cancel_request_by_time)
@ -552,6 +582,7 @@ TEST_F(TestActionServerCancelPolicy, test_action_process_cancel_request_by_time)
EXPECT_EQ(goal_info_out->uuid[j], static_cast<uint8_t>(i + j));
}
}
EXPECT_EQ(RCL_RET_OK, rcl_action_cancel_response_fini(&cancel_response));
}
TEST_F(TestActionServerCancelPolicy, test_action_process_cancel_request_by_time_and_id)
@ -580,4 +611,5 @@ TEST_F(TestActionServerCancelPolicy, test_action_process_cancel_request_by_time_
}
goal_info_out = &cancel_response.msg.goals_canceling.data[num_goals_canceling - 1];
EXPECT_TRUE(uuidcmp(goal_info_out->uuid, cancel_request.goal_info.uuid));
EXPECT_EQ(RCL_RET_OK, rcl_action_cancel_response_fini(&cancel_response));
}

1
rcl_action/test/rcl_action/test_goal_handle.cpp
View file

@ -154,6 +154,7 @@ TEST(TestGoalHandle, test_goal_handle_update_state_invalid)
ret = rcl_action_update_goal_state(&goal_handle, GOAL_EVENT_NUM_EVENTS);
EXPECT_EQ(ret, RCL_RET_ACTION_GOAL_EVENT_INVALID) << rcl_get_error_string().str;
rcl_reset_error();
EXPECT_EQ(RCL_RET_OK, rcl_action_goal_handle_fini(&goal_handle));
}
using EventStateActiveCancelableTuple =