rcl_action_expire_goals() outputs goals that expire (#342)

* rcl_action_expire_goals() outputs goals that expire

rcl_action/include/rcl_action/action_server.h

@@ -583,8 +583,12 @@ rcl_action_send_result_response(
  * \attention If one or more goals are expired then a previously returned goal handle
  * array from rcl_action_server_get_goal_handles() becomes invalid.
  *
- * `num_expired` is an optional argument. If it is not `NULL`, then it is set to the
- * number of goals that were expired.
+ * `expired_goals`, `expired_goals_capacity` and `num_expired` are optional arguments.
+ * If set to (`NULL`, 0u, `NULL`) then they are not used.
+ * To use them allocate an array with size equal to the maximum number of goals that you want to
+ * expire.
+ * Pass the number of goals the array can hold in as `expired_goals_capacity`.
+ * This function will set `num_expired` to the number of goals that were expired.
  *
  * <hr>
  * Attribute          | Adherence
@@ -598,6 +602,9 @@ rcl_action_send_result_response(
  *
  * \param[in] action_server handle to the action server from which expired goals
  *   will be cleared.
+ * \param[in] expired_goals_allocator allocator to use to allocate expired_goals output
+ * \param[inout] expired_goals the identifiers of goals that expired, or set to `NULL` if unused
+ * \param[inout] expired_goals_capacity the allocated size of `expired_goals`, or 0 if unused
  * \param[out] num_expired the number of expired goals, or set to `NULL` if unused
  * \return `RCL_RET_OK` if the response was sent successfully, or
  * \return `RCL_RET_ACTION_SERVER_INVALID` if the action server is invalid, or
@@ -610,6 +617,8 @@ RCL_WARN_UNUSED
 rcl_ret_t
 rcl_action_expire_goals(
   const rcl_action_server_t * action_server,
+  rcl_action_goal_info_t * expired_goals,
+  size_t expired_goals_capacity,
   size_t * num_expired);
 
 /// Take a pending cancel request using an action server.

rcl_action/src/rcl_action/action_server.c

@@ -501,11 +501,21 @@ rcl_action_send_result_response(
 rcl_ret_t
 rcl_action_expire_goals(
   const rcl_action_server_t * action_server,
+  rcl_action_goal_info_t * expired_goals,
+  size_t expired_goals_capacity,
   size_t * num_expired)
 {
   if (!rcl_action_server_is_valid(action_server)) {
     return RCL_RET_ACTION_SERVER_INVALID;
   }
+  const bool output_expired =
+    NULL != expired_goals && NULL != num_expired && expired_goals_capacity > 0u;
+  if (!output_expired &&
+    (NULL != expired_goals || NULL != num_expired || expired_goals_capacity != 0u))
+  {
+    RCL_SET_ERROR_MSG("expired_goals, expired_goals_capacity, and num_expired inconsistent");
+    return RCL_RET_INVALID_ARGUMENT;
+  }
 
   // Get current time (nanosec)
   int64_t current_time;
@@ -525,17 +535,25 @@ rcl_action_expire_goals(
   int64_t goal_time;
   size_t num_goal_handles = action_server->impl->num_goal_handles;
   for (size_t i = 0u; i < num_goal_handles; ++i) {
+    if (output_expired && i >= expired_goals_capacity) {
+      // no more space to output expired goals, so stop expiring them
+      break;
+    }
     goal_handle = action_server->impl->goal_handles[i];
     // Expiration only applys to terminated goals
     if (rcl_action_goal_handle_is_active(goal_handle)) {
       continue;
     }
-    ret = rcl_action_goal_handle_get_info(goal_handle, &goal_info);
+    rcl_action_goal_info_t * info_ptr = &goal_info;
+    if (output_expired) {
+      info_ptr = &(expired_goals[num_goals_expired]);
+    }
+    ret = rcl_action_goal_handle_get_info(goal_handle, info_ptr);
     if (RCL_RET_OK != ret) {
       ret_final = RCL_RET_ERROR;
       continue;
     }
-    goal_time = _goal_info_stamp_to_nanosec(&goal_info);
+    goal_time = _goal_info_stamp_to_nanosec(info_ptr);
     assert(current_time > goal_time);
     if ((current_time - goal_time) > timeout) {
       // Stop tracking goal handle

rcl_action/test/rcl_action/test_action_server.cpp

@@ -149,7 +149,7 @@ protected:
     rcl_node_options_t node_options = rcl_node_get_default_options();
     ret = rcl_node_init(&this->node, "test_action_server_node", "", &node_options);
     ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
-    ret = rcl_clock_init(RCL_STEADY_TIME, &this->clock, &allocator);
+    ret = rcl_clock_init(RCL_ROS_TIME, &this->clock, &allocator);
     ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
     const rosidl_action_type_support_t * ts = ROSIDL_GET_ACTION_TYPE_SUPPORT(
       test_msgs, Fibonacci);
@@ -271,28 +271,49 @@ TEST_F(TestActionServer, test_action_accept_new_goal)
 
 TEST_F(TestActionServer, test_action_clear_expired_goals)
 {
+  const size_t capacity = 1u;
+  rcl_action_goal_info_t expired_goals[1u];
   size_t num_expired = 1u;
   // Clear expired goals with null action server
-  rcl_ret_t ret = rcl_action_expire_goals(nullptr, &num_expired);
+  rcl_ret_t ret = rcl_action_expire_goals(nullptr, expired_goals, capacity, &num_expired);
   EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str;
   rcl_reset_error();
 
   // Clear with invalid action server
   rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server();
-  ret = rcl_action_expire_goals(&invalid_action_server, &num_expired);
+  ret = rcl_action_expire_goals(&invalid_action_server, expired_goals, capacity, &num_expired);
   EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str;
   rcl_reset_error();
 
   // Clear with valid arguments
-  ret = rcl_action_expire_goals(&this->action_server, &num_expired);
+  ret = rcl_action_expire_goals(&this->action_server, expired_goals, capacity, &num_expired);
   EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
   EXPECT_EQ(num_expired, 0u);
 
   // Clear with valid arguments (optional num_expired)
-  ret = rcl_action_expire_goals(&this->action_server, nullptr);
+  ret = rcl_action_expire_goals(&this->action_server, nullptr, 0u, nullptr);
   EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
 
-  // TODO(jacobperron): Test with goals that actually expire
+  // Test with goals that actually expire
+  // Set ROS time
+  ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&this->clock));
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&this->clock, RCUTILS_S_TO_NS(1)));
+  // Accept a goal to create a new handle
+  rcl_action_goal_info_t goal_info_in = rcl_action_get_zero_initialized_goal_info();
+  init_test_uuid1(goal_info_in.uuid);
+  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;
+  // 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));
+  // Set time to something far in the future
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&this->clock, RCUTILS_S_TO_NS(99999)));
+  // Clear with valid arguments
+  ret = rcl_action_expire_goals(&this->action_server, expired_goals, capacity, &num_expired);
+  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));
 }
 
 TEST_F(TestActionServer, test_action_process_cancel_request)