diff --git a/rcl_action/include/rcl_action/action_server.h b/rcl_action/include/rcl_action/action_server.h
index 0849de2..86a103e 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.
+/**
+ * <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?
diff --git a/rcl_action/include/rcl_action/wait.h b/rcl_action/include/rcl_action/wait.h
index f3b0d7c..741592c 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 90c2bce..a9295ac 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, &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;
 }
 
diff --git a/rcl_action/test/rcl_action/test_action_communication.cpp b/rcl_action/test/rcl_action/test_action_communication.cpp
index 0f27fe4..d965d9d 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 @@ 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);
 }
diff --git a/rcl_action/test/rcl_action/test_action_server.cpp b/rcl_action/test/rcl_action/test_action_server.cpp
index cfabf29..4a44345 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 <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));
 }
diff --git a/rcl_action/test/rcl_action/test_goal_handle.cpp b/rcl_action/test/rcl_action/test_goal_handle.cpp
index e5b0b59..e345f2c 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 =