Add timer to action server to check expired goals + asan fixes

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?

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
 }

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;
 }
 

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;
@@ -188,7 +189,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();
 
@@ -305,7 +307,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();
 
@@ -436,7 +439,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();
 
@@ -597,6 +601,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)
@@ -1116,6 +1121,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);
 }