Skip to content

Commit

Permalink
Change the starting time of the goal expiration timeout (ros2#1121)
Browse files Browse the repository at this point in the history
Signed-off-by: Barry Xu <barry.xu@sony.com>
  • Loading branch information
Barry-Xu-2018 authored Jul 29, 2024
1 parent bff91c3 commit 30a047a
Show file tree
Hide file tree
Showing 6 changed files with 169 additions and 10 deletions.
4 changes: 4 additions & 0 deletions rcl/include/rcl/types.h
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
30 changes: 30 additions & 0 deletions rcl_action/include/rcl_action/goal_handle.h
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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
Expand Down Expand Up @@ -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.
Expand Down
64 changes: 54 additions & 10 deletions rcl_action/src/rcl_action/action_server.c
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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) {
Expand All @@ -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;
Expand Down Expand Up @@ -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,
Expand Down
47 changes: 47 additions & 0 deletions rcl_action/src/rcl_action/goal_handle.c
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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
3 changes: 3 additions & 0 deletions rcl_action/test/rcl_action/test_action_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)));

Expand Down
31 changes: 31 additions & 0 deletions rcl_action/test/rcl_action/test_goal_handle.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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>;
Expand Down

0 comments on commit 30a047a

Please sign in to comment.