From e60cb8ae63d2e2e7b73627c44d48a089664ad791 Mon Sep 17 00:00:00 2001
From: Barry Xu <barry.xu@sony.com>
Date: Wed, 29 Nov 2023 09:26:36 +0800
Subject: [PATCH 01/12] Change the starting time of the goal expiration timeout

Use the time of entering goal terminal state as the starting point
for the goal expiration timeout.

Signed-off-by: Barry Xu <barry.xu@sony.com>
---
 rcl_action/include/rcl_action/goal_handle.h   | 55 ++++++++++++++++++-
 rcl_action/src/rcl_action/action_server.c     | 33 +++++++++--
 rcl_action/src/rcl_action/goal_handle.c       | 39 +++++++++++++
 .../test/rcl_action/test_action_server.cpp    |  3 +
 .../test/rcl_action/test_goal_handle.cpp      | 52 ++++++++++++++++++
 5 files changed, 174 insertions(+), 8 deletions(-)

diff --git a/rcl_action/include/rcl_action/goal_handle.h b/rcl_action/include/rcl_action/goal_handle.h
index 1cabc7088..a0ddff1ad 100644
--- a/rcl_action/include/rcl_action/goal_handle.h
+++ b/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.
@@ -190,6 +191,56 @@ 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          | No
+ *
+ * \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
+ */
+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);
+
+/// Set the goal terminal timestamp.
+/**
+ * This is a non-blocking call.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | No
+ * Uses Atomics       | No
+ * Lock-Free          | No
+ *
+ * \param[in] goal_handle struct containing the goal and metadata
+ * \param[in] timestamp The timestamp of goal termination
+ * \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
+ */
+RCL_ACTION_PUBLIC
+RCL_WARN_UNUSED
+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);
+
 /// Check if a goal is active using a rcl_action_goal_handle_t.
 /**
  * This is a non-blocking call.
@@ -200,7 +251,7 @@ rcl_action_goal_handle_get_status(
  * Allocates Memory   | No
  * Thread-Safe        | No
  * Uses Atomics       | No
- * Lock-Free          | Yes
+ * Lock-Free          | No
  *
  * \param[in] goal_handle struct containing the goal and metadata
  * \return `true` if the goal is in one of the following states: ACCEPTED, EXECUTING, or CANCELING, or
@@ -222,7 +273,7 @@ rcl_action_goal_handle_is_active(const rcl_action_goal_handle_t * goal_handle);
  * Allocates Memory   | No
  * Thread-Safe        | No
  * Uses Atomics       | No
- * Lock-Free          | Yes
+ * Lock-Free          | No
  *
  * \param[in] goal_handle struct containing the goal and metadata
  * \return `true` if the goal can be transitioned to CANCELING from its current state, or
diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c
index db48096ed..3c52aab36 100644
--- a/rcl_action/src/rcl_action/action_server.c
+++ b/rcl_action/src/rcl_action/action_server.c
@@ -451,13 +451,28 @@ _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_RET_OK != ret) {
         return RCL_RET_ERROR;
       }
 
-      int64_t delta = timeout - (current_time - _goal_info_stamp_to_nanosec(&goal_info));
+      // After state of goal is updated to terminal state (success or cancel or abort),
+      // rcl_action_notify_goal_done() is called.
+      // This function is called in rcl_action_notify_goal_done().
+      // If goal_terminal_timestamp of goal is invaild, this goal is just in terminal state.
+      // So current time is set to goal_terminal_timestamp of this goal.
+      if (goal_terminal_timestamp == INT64_MAX) {
+        goal_terminal_timestamp = current_time;
+        ret = rcl_action_goal_handle_set_goal_terminal_timestamp(
+          goal_handle, goal_terminal_timestamp);
+        if (RCL_RET_OK != ret) {
+          return RCL_RET_ERROR;
+        }
+      }
+
+      int64_t delta = timeout - (current_time - goal_terminal_timestamp);
       if (delta < minimum_period) {
         minimum_period = delta;
       }
@@ -624,7 +639,7 @@ rcl_action_expire_goals(
   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) {
@@ -645,8 +660,14 @@ rcl_action_expire_goals(
       ret_final = RCL_RET_ERROR;
       continue;
     }
-    goal_time = _goal_info_stamp_to_nanosec(info_ptr);
-    if ((current_time - goal_time) > timeout) {
+
+    ret = rcl_action_goal_handle_get_goal_terminal_timestamp(goal_handle, &goal_terminal_timestamp);
+    if (RCL_RET_OK != ret) {
+      ret_final = RCL_RET_ERROR;
+      continue;
+    }
+
+    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;
diff --git a/rcl_action/src/rcl_action/goal_handle.c b/rcl_action/src/rcl_action/goal_handle.c
index 3a67786b0..50cfcbf0b 100644
--- a/rcl_action/src/rcl_action/goal_handle.c
+++ b/rcl_action/src/rcl_action/goal_handle.c
@@ -26,6 +26,7 @@ typedef struct rcl_action_goal_handle_impl_s
 {
   rcl_action_goal_info_t info;
   rcl_action_goal_state_t state;
+  rcl_time_point_value_t goal_terminal_timestamp;
   rcl_allocator_t allocator;
 } rcl_action_goal_handle_impl_t;
 
@@ -68,6 +69,8 @@ rcl_action_goal_handle_init(
   goal_handle->impl->state = GOAL_STATE_ACCEPTED;
   // Copy the allocator
   goal_handle->impl->allocator = allocator;
+  // Set invaild time
+  goal_handle->impl->goal_terminal_timestamp = INT64_MAX;
   return RCL_RET_OK;
 }
 
@@ -172,6 +175,42 @@ 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);
+  *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 == INT64_MAX) {
+    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
diff --git a/rcl_action/test/rcl_action/test_action_server.cpp b/rcl_action/test/rcl_action/test_action_server.cpp
index 7f533ea74..421a5a9eb 100644
--- a/rcl_action/test/rcl_action/test_action_server.cpp
+++ b/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)));
 
diff --git a/rcl_action/test/rcl_action/test_goal_handle.cpp b/rcl_action/test/rcl_action/test_goal_handle.cpp
index 9dfa2bc65..ccb87edf7 100644
--- a/rcl_action/test/rcl_action/test_goal_handle.cpp
+++ b/rcl_action/test/rcl_action/test_goal_handle.cpp
@@ -170,6 +170,58 @@ 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();
+
+  EXPECT_EQ(RCL_RET_OK, rcl_action_goal_handle_fini(&goal_handle));
+}
+
+TEST(TestGoalHandle, rcl_action_goal_handle_set_goal_terminal_timestamp)
+{
+  rcl_time_point_value_t invaild_time = INT64_MAX;
+
+  // Check with null argument
+  rcl_ret_t ret = rcl_action_goal_handle_set_goal_terminal_timestamp(nullptr, 100);
+  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_set_goal_terminal_timestamp(&goal_handle, 100);
+  EXPECT_EQ(ret, RCL_RET_ACTION_GOAL_HANDLE_INVALID) << rcl_get_error_string().str;
+  rcl_reset_error();
+
+  // Check with invalid 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_set_goal_terminal_timestamp(&goal_handle, invaild_time);
+  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << 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>;

From d12c7ded2233abb780022e3759f04a7a2a42dc3e Mon Sep 17 00:00:00 2001
From: Barry Xu <barry.xu@sony.com>
Date: Thu, 7 Dec 2023 13:21:22 +0800
Subject: [PATCH 02/12] Address review comments

Signed-off-by: Barry Xu <barry.xu@sony.com>
---
 rcl_action/include/rcl_action/goal_handle.h | 8 ++++----
 rcl_action/src/rcl_action/action_server.c   | 2 +-
 rcl_action/src/rcl_action/goal_handle.c     | 4 ++--
 3 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/rcl_action/include/rcl_action/goal_handle.h b/rcl_action/include/rcl_action/goal_handle.h
index a0ddff1ad..adcfe007a 100644
--- a/rcl_action/include/rcl_action/goal_handle.h
+++ b/rcl_action/include/rcl_action/goal_handle.h
@@ -201,7 +201,7 @@ rcl_action_goal_handle_get_status(
  * Allocates Memory   | No
  * Thread-Safe        | No
  * Uses Atomics       | No
- * Lock-Free          | 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
@@ -226,7 +226,7 @@ rcl_action_goal_handle_get_goal_terminal_timestamp(
  * Allocates Memory   | No
  * Thread-Safe        | No
  * Uses Atomics       | No
- * Lock-Free          | No
+ * Lock-Free          | Yes
  *
  * \param[in] goal_handle struct containing the goal and metadata
  * \param[in] timestamp The timestamp of goal termination
@@ -251,7 +251,7 @@ rcl_action_goal_handle_set_goal_terminal_timestamp(
  * Allocates Memory   | No
  * Thread-Safe        | No
  * Uses Atomics       | No
- * Lock-Free          | No
+ * Lock-Free          | Yes
  *
  * \param[in] goal_handle struct containing the goal and metadata
  * \return `true` if the goal is in one of the following states: ACCEPTED, EXECUTING, or CANCELING, or
@@ -273,7 +273,7 @@ rcl_action_goal_handle_is_active(const rcl_action_goal_handle_t * goal_handle);
  * Allocates Memory   | No
  * Thread-Safe        | No
  * Uses Atomics       | No
- * Lock-Free          | No
+ * Lock-Free          | Yes
  *
  * \param[in] goal_handle struct containing the goal and metadata
  * \return `true` if the goal can be transitioned to CANCELING from its current state, or
diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c
index 3c52aab36..14ac93340 100644
--- a/rcl_action/src/rcl_action/action_server.c
+++ b/rcl_action/src/rcl_action/action_server.c
@@ -463,7 +463,7 @@ _recalculate_expire_timer(
       // This function is called in rcl_action_notify_goal_done().
       // If goal_terminal_timestamp of goal is invaild, this goal is just in terminal state.
       // So current time is set to goal_terminal_timestamp of this goal.
-      if (goal_terminal_timestamp == INT64_MAX) {
+      if (goal_terminal_timestamp == 0) {
         goal_terminal_timestamp = current_time;
         ret = rcl_action_goal_handle_set_goal_terminal_timestamp(
           goal_handle, goal_terminal_timestamp);
diff --git a/rcl_action/src/rcl_action/goal_handle.c b/rcl_action/src/rcl_action/goal_handle.c
index 50cfcbf0b..1fe129344 100644
--- a/rcl_action/src/rcl_action/goal_handle.c
+++ b/rcl_action/src/rcl_action/goal_handle.c
@@ -69,8 +69,8 @@ rcl_action_goal_handle_init(
   goal_handle->impl->state = GOAL_STATE_ACCEPTED;
   // Copy the allocator
   goal_handle->impl->allocator = allocator;
-  // Set invaild time
-  goal_handle->impl->goal_terminal_timestamp = INT64_MAX;
+  // Set invalid time
+  goal_handle->impl->goal_terminal_timestamp = 0;
   return RCL_RET_OK;
 }
 

From 70a284e231c3912cb842b42fb6301f96c4f699b3 Mon Sep 17 00:00:00 2001
From: Barry Xu <barry.xu@sony.com>
Date: Tue, 12 Dec 2023 16:06:40 +0800
Subject: [PATCH 03/12] Avoid using invalid time

Signed-off-by: Barry Xu <barry.xu@sony.com>
---
 rcl_action/src/rcl_action/action_server.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c
index 14ac93340..a4a3b4b01 100644
--- a/rcl_action/src/rcl_action/action_server.c
+++ b/rcl_action/src/rcl_action/action_server.c
@@ -667,7 +667,7 @@ rcl_action_expire_goals(
       continue;
     }
 
-    if ((current_time - goal_terminal_timestamp) > timeout) {
+    if ((goal_terminal_timestamp != 0) && (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;

From f6a1845a8b68ed4d24f362fc0cddbccb40979288 Mon Sep 17 00:00:00 2001
From: Barry Xu <barry.xu@sony.com>
Date: Tue, 25 Jun 2024 13:55:27 +0800
Subject: [PATCH 04/12] Address review comments

1. Define macro INVAILD_GOAL_TERMINAL_TIMESTAMP
2. rcl_action_goal_handle_set_goal_terminal_timestamp() is changed to
   internal function
3. Remove test for rcl_action_goal_handle_set_goal_terminal_timestamp()

Signed-off-by: Barry Xu <barry.xu@sony.com>
---
 rcl_action/include/rcl_action/goal_handle.h   |  8 ++++--
 rcl_action/src/rcl_action/goal_handle.c       |  5 ++--
 .../test/rcl_action/test_goal_handle.cpp      | 26 -------------------
 3 files changed, 9 insertions(+), 30 deletions(-)

diff --git a/rcl_action/include/rcl_action/goal_handle.h b/rcl_action/include/rcl_action/goal_handle.h
index adcfe007a..5cc5d4800 100644
--- a/rcl_action/include/rcl_action/goal_handle.h
+++ b/rcl_action/include/rcl_action/goal_handle.h
@@ -37,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 0
+
 /// 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
@@ -204,7 +207,8 @@ rcl_action_goal_handle_get_status(
  * 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
+ * \param[out] timestamp a preallocated struct where goal terminal timestamp is copied.
+     Return `INVAILD_GOAL_TERMINAL_TIMESTAMP` if goal terminal timestamp isn't set.
  * \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
@@ -234,7 +238,7 @@ rcl_action_goal_handle_get_goal_terminal_timestamp(
  * \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
  */
-RCL_ACTION_PUBLIC
+RCL_ACTION_LOCAL
 RCL_WARN_UNUSED
 rcl_ret_t
 rcl_action_goal_handle_set_goal_terminal_timestamp(
diff --git a/rcl_action/src/rcl_action/goal_handle.c b/rcl_action/src/rcl_action/goal_handle.c
index 1fe129344..1f1c01446 100644
--- a/rcl_action/src/rcl_action/goal_handle.c
+++ b/rcl_action/src/rcl_action/goal_handle.c
@@ -26,6 +26,7 @@ typedef struct rcl_action_goal_handle_impl_s
 {
   rcl_action_goal_info_t info;
   rcl_action_goal_state_t state;
+  // If goal_terminal_timestamp isn't set, INVAILD_GOAL_TERMINAL_TIMESTAMP is the initial value.
   rcl_time_point_value_t goal_terminal_timestamp;
   rcl_allocator_t allocator;
 } rcl_action_goal_handle_impl_t;
@@ -70,7 +71,7 @@ rcl_action_goal_handle_init(
   // Copy the allocator
   goal_handle->impl->allocator = allocator;
   // Set invalid time
-  goal_handle->impl->goal_terminal_timestamp = 0;
+  goal_handle->impl->goal_terminal_timestamp = INVAILD_GOAL_TERMINAL_TIMESTAMP;
   return RCL_RET_OK;
 }
 
@@ -203,7 +204,7 @@ rcl_action_goal_handle_set_goal_terminal_timestamp(
     return RCL_RET_ACTION_GOAL_HANDLE_INVALID;  // error message is set
   }
 
-  if (timestamp == INT64_MAX) {
+  if (timestamp == INVAILD_GOAL_TERMINAL_TIMESTAMP) {
     RCL_SET_ERROR_MSG("Timestamp argument is invaild !");
     return RCL_RET_INVALID_ARGUMENT;
   }
diff --git a/rcl_action/test/rcl_action/test_goal_handle.cpp b/rcl_action/test/rcl_action/test_goal_handle.cpp
index ccb87edf7..aebdc9f02 100644
--- a/rcl_action/test/rcl_action/test_goal_handle.cpp
+++ b/rcl_action/test/rcl_action/test_goal_handle.cpp
@@ -196,32 +196,6 @@ TEST(TestGoalHandle, rcl_action_goal_handle_get_goal_terminal_timestamp)
   EXPECT_EQ(RCL_RET_OK, rcl_action_goal_handle_fini(&goal_handle));
 }
 
-TEST(TestGoalHandle, rcl_action_goal_handle_set_goal_terminal_timestamp)
-{
-  rcl_time_point_value_t invaild_time = INT64_MAX;
-
-  // Check with null argument
-  rcl_ret_t ret = rcl_action_goal_handle_set_goal_terminal_timestamp(nullptr, 100);
-  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_set_goal_terminal_timestamp(&goal_handle, 100);
-  EXPECT_EQ(ret, RCL_RET_ACTION_GOAL_HANDLE_INVALID) << rcl_get_error_string().str;
-  rcl_reset_error();
-
-  // Check with invalid 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_set_goal_terminal_timestamp(&goal_handle, invaild_time);
-  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << 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>;

From 34db399fdc6281b051da58d6b17f6318cf414098 Mon Sep 17 00:00:00 2001
From: Barry Xu <barry.xu@sony.com>
Date: Thu, 4 Jul 2024 10:42:41 +0800
Subject: [PATCH 05/12] Address review comments

Signed-off-by: Barry Xu <barry.xu@sony.com>
---
 rcl_action/include/rcl_action/goal_handle.h | 29 +------------
 rcl_action/src/rcl_action/action_server.c   | 46 ++++++++++++++-------
 rcl_action/src/rcl_action/goal_handle.c     |  3 +-
 3 files changed, 36 insertions(+), 42 deletions(-)

diff --git a/rcl_action/include/rcl_action/goal_handle.h b/rcl_action/include/rcl_action/goal_handle.h
index 5cc5d4800..956b6c8aa 100644
--- a/rcl_action/include/rcl_action/goal_handle.h
+++ b/rcl_action/include/rcl_action/goal_handle.h
@@ -38,7 +38,7 @@ typedef struct rcl_action_goal_handle_s
 } rcl_action_goal_handle_t;
 
 /// Define invaild value for goal terminal timestamp
-#define INVAILD_GOAL_TERMINAL_TIMESTAMP 0
+#define INVAILD_GOAL_TERMINAL_TIMESTAMP -1
 
 /// Return a rcl_action_goal_handle_t struct with members set to `NULL`.
 /**
@@ -208,7 +208,7 @@ rcl_action_goal_handle_get_status(
  *
  * \param[in] goal_handle struct containing the goal and metadata
  * \param[out] timestamp a preallocated struct where goal terminal timestamp is copied.
-     Return `INVAILD_GOAL_TERMINAL_TIMESTAMP` if goal terminal timestamp isn't set.
+     Return `INVAILD_GOAL_TERMINAL_TIMESTAMP` if the goal has not reached terminal state.
  * \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
@@ -220,31 +220,6 @@ rcl_action_goal_handle_get_goal_terminal_timestamp(
   const rcl_action_goal_handle_t * goal_handle,
   rcl_time_point_value_t * timestamp);
 
-/// Set 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[in] timestamp The timestamp of goal termination
- * \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
- */
-RCL_ACTION_LOCAL
-RCL_WARN_UNUSED
-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);
-
 /// Check if a goal is active using a rcl_action_goal_handle_t.
 /**
  * This is a non-blocking call.
diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c
index a4a3b4b01..321f29b5a 100644
--- a/rcl_action/src/rcl_action/action_server.c
+++ b/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)
@@ -458,20 +462,6 @@ _recalculate_expire_timer(
         return RCL_RET_ERROR;
       }
 
-      // After state of goal is updated to terminal state (success or cancel or abort),
-      // rcl_action_notify_goal_done() is called.
-      // This function is called in rcl_action_notify_goal_done().
-      // If goal_terminal_timestamp of goal is invaild, this goal is just in terminal state.
-      // So current time is set to goal_terminal_timestamp of this goal.
-      if (goal_terminal_timestamp == 0) {
-        goal_terminal_timestamp = current_time;
-        ret = rcl_action_goal_handle_set_goal_terminal_timestamp(
-          goal_handle, goal_terminal_timestamp);
-        if (RCL_RET_OK != ret) {
-          return RCL_RET_ERROR;
-        }
-      }
-
       int64_t delta = timeout - (current_time - goal_terminal_timestamp);
       if (delta < minimum_period) {
         minimum_period = delta;
@@ -727,6 +717,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_RET_OK != ret) {
+        return RCL_RET_ERROR;
+      }
+
+      if (goal_terminal_timestamp == INVAILD_GOAL_TERMINAL_TIMESTAMP) {
+        ret = rcl_action_goal_handle_set_goal_terminal_timestamp(goal_handle, current_time);
+        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,
diff --git a/rcl_action/src/rcl_action/goal_handle.c b/rcl_action/src/rcl_action/goal_handle.c
index 1f1c01446..be4444e26 100644
--- a/rcl_action/src/rcl_action/goal_handle.c
+++ b/rcl_action/src/rcl_action/goal_handle.c
@@ -26,7 +26,8 @@ typedef struct rcl_action_goal_handle_impl_s
 {
   rcl_action_goal_info_t info;
   rcl_action_goal_state_t state;
-  // If goal_terminal_timestamp isn't set, INVAILD_GOAL_TERMINAL_TIMESTAMP is the initial value.
+  // 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;

From be8f3c10c26f43bafa9b3ab16deea628b9f298e3 Mon Sep 17 00:00:00 2001
From: Barry Xu <barry.xu@sony.com>
Date: Thu, 4 Jul 2024 14:03:43 +0800
Subject: [PATCH 06/12] Add a new ret type RCL_RET_NOT_TERMINATED_YET

Signed-off-by: Barry Xu <barry.xu@sony.com>
---
 rcl/include/rcl/types.h                     | 4 ++++
 rcl_action/include/rcl_action/goal_handle.h | 4 ++--
 rcl_action/src/rcl_action/goal_handle.c     | 6 ++++++
 3 files changed, 12 insertions(+), 2 deletions(-)

diff --git a/rcl/include/rcl/types.h b/rcl/include/rcl/types.h
index f54d67b3a..f36535f4e 100644
--- a/rcl/include/rcl/types.h
+++ b/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_RET_NOT_TERMINATED_YET 4001
+
 /// typedef for rmw_serialized_message_t;
 typedef rmw_serialized_message_t rcl_serialized_message_t;
 
diff --git a/rcl_action/include/rcl_action/goal_handle.h b/rcl_action/include/rcl_action/goal_handle.h
index 956b6c8aa..c128f5075 100644
--- a/rcl_action/include/rcl_action/goal_handle.h
+++ b/rcl_action/include/rcl_action/goal_handle.h
@@ -208,10 +208,10 @@ rcl_action_goal_handle_get_status(
  *
  * \param[in] goal_handle struct containing the goal and metadata
  * \param[out] timestamp a preallocated struct where goal terminal timestamp is copied.
-     Return `INVAILD_GOAL_TERMINAL_TIMESTAMP` if the goal has not reached terminal state.
  * \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
+ * \return `RCL_RET_INVALID_ARGUMENT` if the timestamp argument is invalid or
+ * \return `RCL_RET_NOT_TERMINATED_YET` if the goal has not reached terminal state
  */
 RCL_ACTION_PUBLIC
 RCL_WARN_UNUSED
diff --git a/rcl_action/src/rcl_action/goal_handle.c b/rcl_action/src/rcl_action/goal_handle.c
index be4444e26..34933c88a 100644
--- a/rcl_action/src/rcl_action/goal_handle.c
+++ b/rcl_action/src/rcl_action/goal_handle.c
@@ -189,7 +189,13 @@ rcl_action_goal_handle_get_goal_terminal_timestamp(
     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_RET_NOT_TERMINATED_YET;
+  }
+
   *timestamp = goal_handle->impl->goal_terminal_timestamp;
+
   return RCL_RET_OK;
 }
 

From 52b85671e68f01d5cdd6422fd7010d10fe2d825e Mon Sep 17 00:00:00 2001
From: Barry Xu <barry.xu@sony.com>
Date: Thu, 4 Jul 2024 14:30:39 +0800
Subject: [PATCH 07/12] Update test code to check RCL_RET_NOT_TERMINATED_YET

Signed-off-by: Barry Xu <barry.xu@sony.com>
---
 rcl_action/test/rcl_action/test_goal_handle.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/rcl_action/test/rcl_action/test_goal_handle.cpp b/rcl_action/test/rcl_action/test_goal_handle.cpp
index aebdc9f02..bae522de4 100644
--- a/rcl_action/test/rcl_action/test_goal_handle.cpp
+++ b/rcl_action/test/rcl_action/test_goal_handle.cpp
@@ -193,6 +193,11 @@ TEST(TestGoalHandle, rcl_action_goal_handle_get_goal_terminal_timestamp)
   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_RET_NOT_TERMINATED_YET) << rcl_get_error_string().str;
+  rcl_reset_error();
+
   EXPECT_EQ(RCL_RET_OK, rcl_action_goal_handle_fini(&goal_handle));
 }
 

From 4860612509fd1c294c900199b9539c9f7618e569 Mon Sep 17 00:00:00 2001
From: Barry Xu <barry.xu@sony.com>
Date: Thu, 4 Jul 2024 18:27:14 +0800
Subject: [PATCH 08/12] Fix code to deal with return value
 RCL_RET_NOT_TERMINATED_YET

Signed-off-by: Barry Xu <barry.xu@sony.com>
---
 rcl_action/src/rcl_action/action_server.c | 18 ++++++++++++------
 1 file changed, 12 insertions(+), 6 deletions(-)

diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c
index 321f29b5a..6298d39a6 100644
--- a/rcl_action/src/rcl_action/action_server.c
+++ b/rcl_action/src/rcl_action/action_server.c
@@ -458,6 +458,9 @@ _recalculate_expire_timer(
       rcl_time_point_value_t goal_terminal_timestamp;
       ret = rcl_action_goal_handle_get_goal_terminal_timestamp(
         goal_handle, &goal_terminal_timestamp);
+      if (RCL_RET_NOT_TERMINATED_YET == ret) {
+        continue;
+      }
       if (RCL_RET_OK != ret) {
         return RCL_RET_ERROR;
       }
@@ -652,12 +655,15 @@ rcl_action_expire_goals(
     }
 
     ret = rcl_action_goal_handle_get_goal_terminal_timestamp(goal_handle, &goal_terminal_timestamp);
+    if (RCL_RET_NOT_TERMINATED_YET == ret) {
+      continue;
+    }
     if (RCL_RET_OK != ret) {
       ret_final = RCL_RET_ERROR;
       continue;
     }
 
-    if ((goal_terminal_timestamp != 0) && (current_time - goal_terminal_timestamp) > 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;
@@ -732,15 +738,15 @@ rcl_action_notify_goal_done(
       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_RET_OK != ret) {
-        return RCL_RET_ERROR;
-      }
-
-      if (goal_terminal_timestamp == INVAILD_GOAL_TERMINAL_TIMESTAMP) {
+      if (RCL_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;
       }
     }
   }

From a07e119073f1d94413c8840e40a2ab069e73477f Mon Sep 17 00:00:00 2001
From: Barry Xu <barry.xu@sony.com>
Date: Thu, 4 Jul 2024 20:16:28 +0800
Subject: [PATCH 09/12] Remove the code that's no longer needed

Signed-off-by: Barry Xu <barry.xu@sony.com>
---
 rcl_action/src/rcl_action/action_server.c | 14 --------------
 1 file changed, 14 deletions(-)

diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c
index 6298d39a6..16911c0e5 100644
--- a/rcl_action/src/rcl_action/action_server.c
+++ b/rcl_action/src/rcl_action/action_server.c
@@ -631,7 +631,6 @@ 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;
   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) {
@@ -640,19 +639,6 @@ rcl_action_expire_goals(
       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;
-    }
-    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;
-    }
 
     ret = rcl_action_goal_handle_get_goal_terminal_timestamp(goal_handle, &goal_terminal_timestamp);
     if (RCL_RET_NOT_TERMINATED_YET == ret) {

From 77fd72bef29a742d0f2a5d27484a4ffa2f2cd654 Mon Sep 17 00:00:00 2001
From: Barry Xu <barry.xu@sony.com>
Date: Fri, 5 Jul 2024 21:40:41 +0800
Subject: [PATCH 10/12] Revert "Remove the code that's no longer needed"

This reverts commit a07e119073f1d94413c8840e40a2ab069e73477f.

Signed-off-by: Barry Xu <barry.xu@sony.com>
---
 rcl_action/src/rcl_action/action_server.c | 14 ++++++++++++++
 1 file changed, 14 insertions(+)

diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c
index 16911c0e5..6298d39a6 100644
--- a/rcl_action/src/rcl_action/action_server.c
+++ b/rcl_action/src/rcl_action/action_server.c
@@ -631,6 +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;
   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) {
@@ -639,6 +640,19 @@ rcl_action_expire_goals(
       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;
+    }
+    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;
+    }
 
     ret = rcl_action_goal_handle_get_goal_terminal_timestamp(goal_handle, &goal_terminal_timestamp);
     if (RCL_RET_NOT_TERMINATED_YET == ret) {

From 03555f55f6c101b3c14b899434a9c494a02f407e Mon Sep 17 00:00:00 2001
From: Barry Xu <barry.xu@sony.com>
Date: Mon, 8 Jul 2024 10:54:32 +0800
Subject: [PATCH 11/12] Goal info is retrieved only when the expired goals need
 to be output

Signed-off-by: Barry Xu <barry.xu@sony.com>
---
 rcl_action/src/rcl_action/action_server.c | 15 +++++++--------
 1 file changed, 7 insertions(+), 8 deletions(-)

diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c
index 6298d39a6..f96e40018 100644
--- a/rcl_action/src/rcl_action/action_server.c
+++ b/rcl_action/src/rcl_action/action_server.c
@@ -631,7 +631,6 @@ 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;
   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) {
@@ -644,14 +643,14 @@ 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, info_ptr);
-    if (RCL_RET_OK != ret) {
-      ret_final = RCL_RET_ERROR;
-      continue;
+      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);

From 959f39b7058fdecde784c13ca97030c61cff1ae5 Mon Sep 17 00:00:00 2001
From: Barry Xu <barry.xu@sony.com>
Date: Tue, 9 Jul 2024 13:02:33 +0800
Subject: [PATCH 12/12] Address review comments

Signed-off-by: Barry Xu <barry.xu@sony.com>
---
 rcl/include/rcl/types.h                         | 2 +-
 rcl_action/include/rcl_action/goal_handle.h     | 2 +-
 rcl_action/src/rcl_action/action_server.c       | 6 +++---
 rcl_action/src/rcl_action/goal_handle.c         | 4 ++--
 rcl_action/test/rcl_action/test_goal_handle.cpp | 2 +-
 5 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/rcl/include/rcl/types.h b/rcl/include/rcl/types.h
index f36535f4e..6ae462873 100644
--- a/rcl/include/rcl/types.h
+++ b/rcl/include/rcl/types.h
@@ -128,7 +128,7 @@ typedef rmw_ret_t rcl_ret_t;
 
 // rcl action specific ret codes in 40XX
 /// No terminal timestamp for the goal as it has not reached a terminal state.
-#define RCL_RET_NOT_TERMINATED_YET 4001
+#define RCL_ACTION_RET_NOT_TERMINATED_YET 4001
 
 /// typedef for rmw_serialized_message_t;
 typedef rmw_serialized_message_t rcl_serialized_message_t;
diff --git a/rcl_action/include/rcl_action/goal_handle.h b/rcl_action/include/rcl_action/goal_handle.h
index c128f5075..46ea29e56 100644
--- a/rcl_action/include/rcl_action/goal_handle.h
+++ b/rcl_action/include/rcl_action/goal_handle.h
@@ -211,7 +211,7 @@ rcl_action_goal_handle_get_status(
  * \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_RET_NOT_TERMINATED_YET` if the goal has not reached terminal state
+ * \return `RCL_ACTION_RET_NOT_TERMINATED_YET` if the goal has not reached terminal state
  */
 RCL_ACTION_PUBLIC
 RCL_WARN_UNUSED
diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c
index f96e40018..47e662d7f 100644
--- a/rcl_action/src/rcl_action/action_server.c
+++ b/rcl_action/src/rcl_action/action_server.c
@@ -458,7 +458,7 @@ _recalculate_expire_timer(
       rcl_time_point_value_t goal_terminal_timestamp;
       ret = rcl_action_goal_handle_get_goal_terminal_timestamp(
         goal_handle, &goal_terminal_timestamp);
-      if (RCL_RET_NOT_TERMINATED_YET == ret) {
+      if (RCL_ACTION_RET_NOT_TERMINATED_YET == ret) {
         continue;
       }
       if (RCL_RET_OK != ret) {
@@ -654,7 +654,7 @@ rcl_action_expire_goals(
     }
 
     ret = rcl_action_goal_handle_get_goal_terminal_timestamp(goal_handle, &goal_terminal_timestamp);
-    if (RCL_RET_NOT_TERMINATED_YET == ret) {
+    if (RCL_ACTION_RET_NOT_TERMINATED_YET == ret) {
       continue;
     }
     if (RCL_RET_OK != ret) {
@@ -737,7 +737,7 @@ rcl_action_notify_goal_done(
       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_RET_NOT_TERMINATED_YET == ret) {
+      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;
diff --git a/rcl_action/src/rcl_action/goal_handle.c b/rcl_action/src/rcl_action/goal_handle.c
index 34933c88a..f326049d6 100644
--- a/rcl_action/src/rcl_action/goal_handle.c
+++ b/rcl_action/src/rcl_action/goal_handle.c
@@ -191,7 +191,7 @@ rcl_action_goal_handle_get_goal_terminal_timestamp(
   RCL_CHECK_ARGUMENT_FOR_NULL(timestamp, RCL_RET_INVALID_ARGUMENT);
 
   if (goal_handle->impl->goal_terminal_timestamp == INVAILD_GOAL_TERMINAL_TIMESTAMP) {
-    return RCL_RET_NOT_TERMINATED_YET;
+    return RCL_ACTION_RET_NOT_TERMINATED_YET;
   }
 
   *timestamp = goal_handle->impl->goal_terminal_timestamp;
@@ -211,7 +211,7 @@ rcl_action_goal_handle_set_goal_terminal_timestamp(
     return RCL_RET_ACTION_GOAL_HANDLE_INVALID;  // error message is set
   }
 
-  if (timestamp == INVAILD_GOAL_TERMINAL_TIMESTAMP) {
+  if (timestamp <= INVAILD_GOAL_TERMINAL_TIMESTAMP) {
     RCL_SET_ERROR_MSG("Timestamp argument is invaild !");
     return RCL_RET_INVALID_ARGUMENT;
   }
diff --git a/rcl_action/test/rcl_action/test_goal_handle.cpp b/rcl_action/test/rcl_action/test_goal_handle.cpp
index bae522de4..2b96d9862 100644
--- a/rcl_action/test/rcl_action/test_goal_handle.cpp
+++ b/rcl_action/test/rcl_action/test_goal_handle.cpp
@@ -195,7 +195,7 @@ TEST(TestGoalHandle, rcl_action_goal_handle_get_goal_terminal_timestamp)
 
   rcl_time_point_value_t timestamp;
   ret = rcl_action_goal_handle_get_goal_terminal_timestamp(&goal_handle, &timestamp);
-  EXPECT_EQ(ret, RCL_RET_NOT_TERMINATED_YET) << rcl_get_error_string().str;
+  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));