From fd77323b9de570ebab5976dff9cc638902f0f716 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 30 Nov 2018 18:24:17 -0800 Subject: [PATCH] Add timer to action server to check expired goals + asan fixes (#343) * Add timer to action server to check expired goals * Fix leak in action_server * Fix leaks and heap overflow errors * Fix leaks in tests --- rcl_action/include/rcl_action/action_server.h | 24 ++++ rcl_action/include/rcl_action/wait.h | 4 +- rcl_action/src/rcl_action/action_server.c | 128 +++++++++++++++++- .../rcl_action/test_action_communication.cpp | 12 +- .../test/rcl_action/test_action_server.cpp | 32 +++++ .../test/rcl_action/test_goal_handle.cpp | 1 + 6 files changed, 191 insertions(+), 10 deletions(-) diff --git a/rcl_action/include/rcl_action/action_server.h b/rcl_action/include/rcl_action/action_server.h index 0849de27e..86a103e9f 100644 --- a/rcl_action/include/rcl_action/action_server.h +++ b/rcl_action/include/rcl_action/action_server.h @@ -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. +/** + *
+ * 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? diff --git a/rcl_action/include/rcl_action/wait.h b/rcl_action/include/rcl_action/wait.h index f3b0d7c24..741592c0b 100644 --- a/rcl_action/include/rcl_action/wait.h +++ b/rcl_action/include/rcl_action/wait.h @@ -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 } diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c index 90c2bce9f..a9295ac60 100644 --- a/rcl_action/src/rcl_action/action_server.c +++ b/rcl_action/src/rcl_action/action_server.c @@ -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, ¤t_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; } diff --git a/rcl_action/test/rcl_action/test_action_communication.cpp b/rcl_action/test/rcl_action/test_action_communication.cpp index 0f27fe48a..d965d9d35 100644 --- a/rcl_action/test/rcl_action/test_action_communication.cpp +++ b/rcl_action/test/rcl_action/test_action_communication.cpp @@ -145,6 +145,7 @@ class CLASSNAME (TestActionCommunication, RMW_IMPLEMENTATION) : public ::testing 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); } diff --git a/rcl_action/test/rcl_action/test_action_server.cpp b/rcl_action/test/rcl_action/test_action_server.cpp index cfabf295b..4a443450a 100644 --- a/rcl_action/test/rcl_action/test_action_server.cpp +++ b/rcl_action/test/rcl_action/test_action_server.cpp @@ -15,6 +15,7 @@ #include #include +#include #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 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 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 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 @@ class TestActionServerCancelPolicy : public TestActionServer } 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 @@ class TestActionServerCancelPolicy : public TestActionServer 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 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(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(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)); } diff --git a/rcl_action/test/rcl_action/test_goal_handle.cpp b/rcl_action/test/rcl_action/test_goal_handle.cpp index e5b0b59d9..e345f2c57 100644 --- a/rcl_action/test/rcl_action/test_goal_handle.cpp +++ b/rcl_action/test/rcl_action/test_goal_handle.cpp @@ -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 =