Change the starting time of the goal expiration timeout (#1121)

Signed-off-by: Barry Xu <[email protected]>

rcl/include/rcl/types.h

@@ -126,6 +126,10 @@ typedef rmw_ret_t rcl_ret_t;
 /// rcl_lifecycle state not registered
 #define RCL_RET_LIFECYCLE_STATE_NOT_REGISTERED 3001
 
+// rcl action specific ret codes in 40XX
+/// No terminal timestamp for the goal as it has not reached a terminal state.
+#define RCL_ACTION_RET_NOT_TERMINATED_YET 4001
+
 /// typedef for rmw_serialized_message_t;
 typedef rmw_serialized_message_t rcl_serialized_message_t;
 

rcl_action/include/rcl_action/goal_handle.h

@@ -24,6 +24,7 @@ extern "C"
 #include "rcl_action/types.h"
 #include "rcl_action/visibility_control.h"
 #include "rcl/allocator.h"
+#include "rcl/time.h"
 
 
 /// Internal rcl action goal implementation struct.
@@ -36,6 +37,9 @@ typedef struct rcl_action_goal_handle_s
   rcl_action_goal_handle_impl_t * impl;
 } rcl_action_goal_handle_t;
 
+/// Define invaild value for goal terminal timestamp
+#define INVAILD_GOAL_TERMINAL_TIMESTAMP -1
+
 /// Return a rcl_action_goal_handle_t struct with members set to `NULL`.
 /**
  * Should be called to get a null rcl_action_goal_handle_t before passing to
@@ -190,6 +194,32 @@ rcl_action_goal_handle_get_status(
   const rcl_action_goal_handle_t * goal_handle,
   rcl_action_goal_state_t * status);
 
+/// Get the goal terminal timestamp.
+/**
+ * This is a non-blocking call.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | No
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[in] goal_handle struct containing the goal and metadata
+ * \param[out] timestamp a preallocated struct where goal terminal timestamp is copied.
+ * \return `RCL_RET_OK` if the goal ID was accessed successfully, or
+ * \return `RCL_RET_ACTION_GOAL_HANDLE_INVALID` if the goal handle is invalid, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if the timestamp argument is invalid or
+ * \return `RCL_ACTION_RET_NOT_TERMINATED_YET` if the goal has not reached terminal state
+ */
+RCL_ACTION_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_action_goal_handle_get_goal_terminal_timestamp(
+  const rcl_action_goal_handle_t * goal_handle,
+  rcl_time_point_value_t * timestamp);
+
 /// Check if a goal is active using a rcl_action_goal_handle_t.
 /**
  * This is a non-blocking call.

rcl_action/src/rcl_action/action_server.c

@@ -36,6 +36,10 @@ extern "C"
 
 #include "rmw/rmw.h"
 
+extern rcl_ret_t
+rcl_action_goal_handle_set_goal_terminal_timestamp(
+  const rcl_action_goal_handle_t * goal_handle,
+  rcl_time_point_value_t timestamp);
 
 rcl_action_server_t
 rcl_action_get_zero_initialized_server(void)
@@ -451,13 +455,17 @@ _recalculate_expire_timer(
     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);
+      rcl_time_point_value_t goal_terminal_timestamp;
+      ret = rcl_action_goal_handle_get_goal_terminal_timestamp(
+        goal_handle, &goal_terminal_timestamp);
+      if (RCL_ACTION_RET_NOT_TERMINATED_YET == ret) {
+        continue;
+      }
       if (RCL_RET_OK != ret) {
         return RCL_RET_ERROR;
       }
 
-      int64_t delta = timeout - (current_time - _goal_info_stamp_to_nanosec(&goal_info));
+      int64_t delta = timeout - (current_time - goal_terminal_timestamp);
       if (delta < minimum_period) {
         minimum_period = delta;
       }
@@ -623,8 +631,7 @@ rcl_action_expire_goals(
   rcl_ret_t ret_final = RCL_RET_OK;
   const int64_t timeout = (int64_t)action_server->impl->options.result_timeout.nanoseconds;
   rcl_action_goal_handle_t * goal_handle;
-  rcl_action_goal_info_t goal_info;
-  int64_t goal_time;
+  rcl_time_point_value_t goal_terminal_timestamp;
   size_t num_goal_handles = action_server->impl->num_goal_handles;
   for (size_t i = 0u; i < num_goal_handles; ++i) {
     if (output_expired && num_goals_expired >= expired_goals_capacity) {
@@ -636,17 +643,26 @@ rcl_action_expire_goals(
     if (rcl_action_goal_handle_is_active(goal_handle)) {
       continue;
     }
-    rcl_action_goal_info_t * info_ptr = &goal_info;
+
+    // Retrieve the information of expired goals for output
     if (output_expired) {
-      info_ptr = &(expired_goals[num_goals_expired]);
+      ret = rcl_action_goal_handle_get_info(goal_handle, &(expired_goals[num_goals_expired]));
+      if (RCL_RET_OK != ret) {
+        ret_final = RCL_RET_ERROR;
+        continue;
+      }
+    }
+
+    ret = rcl_action_goal_handle_get_goal_terminal_timestamp(goal_handle, &goal_terminal_timestamp);
+    if (RCL_ACTION_RET_NOT_TERMINATED_YET == ret) {
+      continue;
     }
-    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(info_ptr);
-    if ((current_time - goal_time) > timeout) {
+
+    if ((current_time - goal_terminal_timestamp) > timeout) {
       // Deallocate space used to store pointer to goal handle
       allocator.deallocate(action_server->impl->goal_handles[i], allocator.state);
       action_server->impl->goal_handles[i] = NULL;
@@ -706,6 +722,34 @@ rcl_action_notify_goal_done(
   if (!rcl_action_server_is_valid(action_server)) {
     return RCL_RET_ACTION_SERVER_INVALID;
   }
+
+  // Get current time (nanosec)
+  int64_t current_time;
+  rcl_ret_t ret = rcl_clock_get_now(action_server->impl->clock, &current_time);
+  if (RCL_RET_OK != ret) {
+    return RCL_RET_ERROR;
+  }
+
+  // Set current time to goal_terminal_timestamp of goal which has reached terminal state
+  for (size_t i = 0; i < action_server->impl->num_goal_handles; ++i) {
+    rcl_action_goal_handle_t * goal_handle = action_server->impl->goal_handles[i];
+    if (!rcl_action_goal_handle_is_active(goal_handle)) {
+      rcl_time_point_value_t goal_terminal_timestamp;
+      rcl_ret_t ret = rcl_action_goal_handle_get_goal_terminal_timestamp(
+        goal_handle, &goal_terminal_timestamp);
+      if (RCL_ACTION_RET_NOT_TERMINATED_YET == ret) {
+        ret = rcl_action_goal_handle_set_goal_terminal_timestamp(goal_handle, current_time);
+        if (RCL_RET_OK != ret) {
+          return RCL_RET_ERROR;
+        }
+        continue;
+      }
+      if (RCL_RET_OK != ret) {
+        return RCL_RET_ERROR;
+      }
+    }
+  }
+
   return _recalculate_expire_timer(
     &action_server->impl->expire_timer,
     action_server->impl->options.result_timeout.nanoseconds,

rcl_action/src/rcl_action/goal_handle.c

@@ -26,6 +26,9 @@ typedef struct rcl_action_goal_handle_impl_s
 {
   rcl_action_goal_info_t info;
   rcl_action_goal_state_t state;
+  // As long as the goal has not reached terminal state, this variable is set to
+  // INVAILD_GOAL_TERMINAL_TIMESTAMP
+  rcl_time_point_value_t goal_terminal_timestamp;
   rcl_allocator_t allocator;
 } rcl_action_goal_handle_impl_t;
 
@@ -68,6 +71,8 @@ rcl_action_goal_handle_init(
   goal_handle->impl->state = GOAL_STATE_ACCEPTED;
   // Copy the allocator
   goal_handle->impl->allocator = allocator;
+  // Set invalid time
+  goal_handle->impl->goal_terminal_timestamp = INVAILD_GOAL_TERMINAL_TIMESTAMP;
   return RCL_RET_OK;
 }
 
@@ -172,6 +177,48 @@ rcl_action_goal_handle_is_valid(const rcl_action_goal_handle_t * goal_handle)
   return true;
 }
 
+rcl_ret_t
+rcl_action_goal_handle_get_goal_terminal_timestamp(
+  const rcl_action_goal_handle_t * goal_handle,
+  rcl_time_point_value_t * timestamp)
+{
+  RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ACTION_GOAL_HANDLE_INVALID);
+  RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT);
+
+  if (!rcl_action_goal_handle_is_valid(goal_handle)) {
+    return RCL_RET_ACTION_GOAL_HANDLE_INVALID;  // error message is set
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(timestamp, RCL_RET_INVALID_ARGUMENT);
+
+  if (goal_handle->impl->goal_terminal_timestamp == INVAILD_GOAL_TERMINAL_TIMESTAMP) {
+    return RCL_ACTION_RET_NOT_TERMINATED_YET;
+  }
+
+  *timestamp = goal_handle->impl->goal_terminal_timestamp;
+
+  return RCL_RET_OK;
+}
+
+rcl_ret_t
+rcl_action_goal_handle_set_goal_terminal_timestamp(
+  const rcl_action_goal_handle_t * goal_handle,
+  rcl_time_point_value_t timestamp)
+{
+  RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ACTION_GOAL_HANDLE_INVALID);
+  RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT);
+
+  if (!rcl_action_goal_handle_is_valid(goal_handle)) {
+    return RCL_RET_ACTION_GOAL_HANDLE_INVALID;  // error message is set
+  }
+
+  if (timestamp <= INVAILD_GOAL_TERMINAL_TIMESTAMP) {
+    RCL_SET_ERROR_MSG("Timestamp argument is invaild !");
+    return RCL_RET_INVALID_ARGUMENT;
+  }
+  goal_handle->impl->goal_terminal_timestamp = timestamp;
+  return RCL_RET_OK;
+}
+
 #ifdef __cplusplus
 }
 #endif

rcl_action/test/rcl_action/test_action_server.cpp

@@ -592,6 +592,9 @@ TEST_F(TestActionServer, test_action_clear_expired_goals)
   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_ABORT));
 
+  // recalculate the expired goal timer after entering terminal state
+  ASSERT_EQ(RCL_RET_OK, rcl_action_notify_goal_done(&this->action_server));
+
   // 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)));
 

rcl_action/test/rcl_action/test_goal_handle.cpp

@@ -170,6 +170,37 @@ TEST(TestGoalHandle, test_goal_handle_update_state_invalid)
   EXPECT_EQ(RCL_RET_OK, rcl_action_goal_handle_fini(&goal_handle));
 }
 
+TEST(TestGoalHandle, rcl_action_goal_handle_get_goal_terminal_timestamp)
+{
+  rcl_time_point_value_t time;
+
+  // Check with null argument
+  rcl_ret_t ret = rcl_action_goal_handle_get_goal_terminal_timestamp(nullptr, &time);
+  EXPECT_EQ(ret, RCL_RET_ACTION_GOAL_HANDLE_INVALID) << rcl_get_error_string().str;
+  rcl_reset_error();
+
+  // Check with invalid goal handle
+  rcl_action_goal_handle_t goal_handle = rcl_action_get_zero_initialized_goal_handle();
+  ret = rcl_action_goal_handle_get_goal_terminal_timestamp(&goal_handle, &time);
+  EXPECT_EQ(ret, RCL_RET_ACTION_GOAL_HANDLE_INVALID) << rcl_get_error_string().str;
+  rcl_reset_error();
+
+  // Check with null timestamp
+  rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info();
+  ret = rcl_action_goal_handle_init(&goal_handle, &goal_info, rcl_get_default_allocator());
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+  ret = rcl_action_goal_handle_get_goal_terminal_timestamp(&goal_handle, nullptr);
+  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str;
+  rcl_reset_error();
+
+  rcl_time_point_value_t timestamp;
+  ret = rcl_action_goal_handle_get_goal_terminal_timestamp(&goal_handle, &timestamp);
+  EXPECT_EQ(ret, RCL_ACTION_RET_NOT_TERMINATED_YET) << rcl_get_error_string().str;
+  rcl_reset_error();
+
+  EXPECT_EQ(RCL_RET_OK, rcl_action_goal_handle_fini(&goal_handle));
+}
+
 using EventStateActiveCancelableTuple =
   std::tuple<rcl_action_goal_event_t, rcl_action_goal_state_t, bool, bool>;
 using StateTransitionSequence = std::vector<EventStateActiveCancelableTuple>;