From 30a047af69c27ef36ecf80c22b2680a0e90a2f98 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Tue, 30 Jul 2024 02:03:39 +0800 Subject: [PATCH] Change the starting time of the goal expiration timeout (#1121) Signed-off-by: Barry Xu --- rcl/include/rcl/types.h | 4 ++ rcl_action/include/rcl_action/goal_handle.h | 30 +++++++++ rcl_action/src/rcl_action/action_server.c | 64 ++++++++++++++++--- rcl_action/src/rcl_action/goal_handle.c | 47 ++++++++++++++ .../test/rcl_action/test_action_server.cpp | 3 + .../test/rcl_action/test_goal_handle.cpp | 31 +++++++++ 6 files changed, 169 insertions(+), 10 deletions(-) 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;