diff --git a/rcl/include/rcl/types.h b/rcl/include/rcl/types.h
index f54d67b3a..6ae462873 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_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 1cabc7088..46ea29e56 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.
@@ -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.
+ *
+ *
+ * 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.
diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c
index db48096ed..47e662d7f 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)
@@ -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, ¤t_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,
diff --git a/rcl_action/src/rcl_action/goal_handle.c b/rcl_action/src/rcl_action/goal_handle.c
index 3a67786b0..f326049d6 100644
--- a/rcl_action/src/rcl_action/goal_handle.c
+++ b/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
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..2b96d9862 100644
--- a/rcl_action/test/rcl_action/test_goal_handle.cpp
+++ b/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, ×tamp);
+ 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;
using StateTransitionSequence = std::vector;