Skip to content

Commit 30a047a

Browse files
Barry-Xu-2018Barry-Xu-2018
authored
Change the starting time of the goal expiration timeout (#1121)
Signed-off-by: Barry Xu <[email protected]>
1 parent bff91c3 commit 30a047a

6 files changed

Lines changed: 169 additions & 10 deletions

File tree

‎rcl/include/rcl/types.h‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -126,6 +126,10 @@ typedef rmw_ret_t rcl_ret_t;
126126
/// rcl_lifecycle state not registered
127127
#define RCL_RET_LIFECYCLE_STATE_NOT_REGISTERED 3001
128128

129+
// rcl action specific ret codes in 40XX
130+
/// No terminal timestamp for the goal as it has not reached a terminal state.
131+
#define RCL_ACTION_RET_NOT_TERMINATED_YET 4001
132+
129133
/// typedef for rmw_serialized_message_t;
130134
typedef rmw_serialized_message_t rcl_serialized_message_t;
131135

‎rcl_action/include/rcl_action/goal_handle.h‎

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@ extern "C"
2424
#include "rcl_action/types.h"
2525
#include "rcl_action/visibility_control.h"
2626
#include "rcl/allocator.h"
27+
#include "rcl/time.h"
2728

2829

2930
/// Internal rcl action goal implementation struct.
@@ -36,6 +37,9 @@ typedef struct rcl_action_goal_handle_s
3637
rcl_action_goal_handle_impl_t * impl;
3738
} rcl_action_goal_handle_t;
3839

40+
/// Define invaild value for goal terminal timestamp
41+
#define INVAILD_GOAL_TERMINAL_TIMESTAMP -1
42+
3943
/// Return a rcl_action_goal_handle_t struct with members set to `NULL`.
4044
/**
4145
* 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(
190194
const rcl_action_goal_handle_t * goal_handle,
191195
rcl_action_goal_state_t * status);
192196

197+
/// Get the goal terminal timestamp.
198+
/**
199+
* This is a non-blocking call.
200+
*
201+
* <hr>
202+
* Attribute | Adherence
203+
* ------------------ | -------------
204+
* Allocates Memory | No
205+
* Thread-Safe | No
206+
* Uses Atomics | No
207+
* Lock-Free | Yes
208+
*
209+
* \param[in] goal_handle struct containing the goal and metadata
210+
* \param[out] timestamp a preallocated struct where goal terminal timestamp is copied.
211+
* \return `RCL_RET_OK` if the goal ID was accessed successfully, or
212+
* \return `RCL_RET_ACTION_GOAL_HANDLE_INVALID` if the goal handle is invalid, or
213+
* \return `RCL_RET_INVALID_ARGUMENT` if the timestamp argument is invalid or
214+
* \return `RCL_ACTION_RET_NOT_TERMINATED_YET` if the goal has not reached terminal state
215+
*/
216+
RCL_ACTION_PUBLIC
217+
RCL_WARN_UNUSED
218+
rcl_ret_t
219+
rcl_action_goal_handle_get_goal_terminal_timestamp(
220+
const rcl_action_goal_handle_t * goal_handle,
221+
rcl_time_point_value_t * timestamp);
222+
193223
/// Check if a goal is active using a rcl_action_goal_handle_t.
194224
/**
195225
* This is a non-blocking call.

‎rcl_action/src/rcl_action/action_server.c‎

Lines changed: 54 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,10 @@ extern "C"
3636

3737
#include "rmw/rmw.h"
3838

39+
extern rcl_ret_t
40+
rcl_action_goal_handle_set_goal_terminal_timestamp(
41+
const rcl_action_goal_handle_t * goal_handle,
42+
rcl_time_point_value_t timestamp);
3943

4044
rcl_action_server_t
4145
rcl_action_get_zero_initialized_server(void)
@@ -451,13 +455,17 @@ _recalculate_expire_timer(
451455
if (!rcl_action_goal_handle_is_active(goal_handle)) {
452456
++num_inactive_goals;
453457

454-
rcl_action_goal_info_t goal_info;
455-
ret = rcl_action_goal_handle_get_info(goal_handle, &goal_info);
458+
rcl_time_point_value_t goal_terminal_timestamp;
459+
ret = rcl_action_goal_handle_get_goal_terminal_timestamp(
460+
goal_handle, &goal_terminal_timestamp);
461+
if (RCL_ACTION_RET_NOT_TERMINATED_YET == ret) {
462+
continue;
463+
}
456464
if (RCL_RET_OK != ret) {
457465
return RCL_RET_ERROR;
458466
}
459467

460-
int64_t delta = timeout - (current_time - _goal_info_stamp_to_nanosec(&goal_info));
468+
int64_t delta = timeout - (current_time - goal_terminal_timestamp);
461469
if (delta < minimum_period) {
462470
minimum_period = delta;
463471
}
@@ -623,8 +631,7 @@ rcl_action_expire_goals(
623631
rcl_ret_t ret_final = RCL_RET_OK;
624632
const int64_t timeout = (int64_t)action_server->impl->options.result_timeout.nanoseconds;
625633
rcl_action_goal_handle_t * goal_handle;
626-
rcl_action_goal_info_t goal_info;
627-
int64_t goal_time;
634+
rcl_time_point_value_t goal_terminal_timestamp;
628635
size_t num_goal_handles = action_server->impl->num_goal_handles;
629636
for (size_t i = 0u; i < num_goal_handles; ++i) {
630637
if (output_expired && num_goals_expired >= expired_goals_capacity) {
@@ -636,17 +643,26 @@ rcl_action_expire_goals(
636643
if (rcl_action_goal_handle_is_active(goal_handle)) {
637644
continue;
638645
}
639-
rcl_action_goal_info_t * info_ptr = &goal_info;
646+
647+
// Retrieve the information of expired goals for output
640648
if (output_expired) {
641-
info_ptr = &(expired_goals[num_goals_expired]);
649+
ret = rcl_action_goal_handle_get_info(goal_handle, &(expired_goals[num_goals_expired]));
650+
if (RCL_RET_OK != ret) {
651+
ret_final = RCL_RET_ERROR;
652+
continue;
653+
}
654+
}
655+
656+
ret = rcl_action_goal_handle_get_goal_terminal_timestamp(goal_handle, &goal_terminal_timestamp);
657+
if (RCL_ACTION_RET_NOT_TERMINATED_YET == ret) {
658+
continue;
642659
}
643-
ret = rcl_action_goal_handle_get_info(goal_handle, info_ptr);
644660
if (RCL_RET_OK != ret) {
645661
ret_final = RCL_RET_ERROR;
646662
continue;
647663
}
648-
goal_time = _goal_info_stamp_to_nanosec(info_ptr);
649-
if ((current_time - goal_time) > timeout) {
664+
665+
if ((current_time - goal_terminal_timestamp) > timeout) {
650666
// Deallocate space used to store pointer to goal handle
651667
allocator.deallocate(action_server->impl->goal_handles[i], allocator.state);
652668
action_server->impl->goal_handles[i] = NULL;
@@ -706,6 +722,34 @@ rcl_action_notify_goal_done(
706722
if (!rcl_action_server_is_valid(action_server)) {
707723
return RCL_RET_ACTION_SERVER_INVALID;
708724
}
725+
726+
// Get current time (nanosec)
727+
int64_t current_time;
728+
rcl_ret_t ret = rcl_clock_get_now(action_server->impl->clock, &current_time);
729+
if (RCL_RET_OK != ret) {
730+
return RCL_RET_ERROR;
731+
}
732+
733+
// Set current time to goal_terminal_timestamp of goal which has reached terminal state
734+
for (size_t i = 0; i < action_server->impl->num_goal_handles; ++i) {
735+
rcl_action_goal_handle_t * goal_handle = action_server->impl->goal_handles[i];
736+
if (!rcl_action_goal_handle_is_active(goal_handle)) {
737+
rcl_time_point_value_t goal_terminal_timestamp;
738+
rcl_ret_t ret = rcl_action_goal_handle_get_goal_terminal_timestamp(
739+
goal_handle, &goal_terminal_timestamp);
740+
if (RCL_ACTION_RET_NOT_TERMINATED_YET == ret) {
741+
ret = rcl_action_goal_handle_set_goal_terminal_timestamp(goal_handle, current_time);
742+
if (RCL_RET_OK != ret) {
743+
return RCL_RET_ERROR;
744+
}
745+
continue;
746+
}
747+
if (RCL_RET_OK != ret) {
748+
return RCL_RET_ERROR;
749+
}
750+
}
751+
}
752+
709753
return _recalculate_expire_timer(
710754
&action_server->impl->expire_timer,
711755
action_server->impl->options.result_timeout.nanoseconds,

‎rcl_action/src/rcl_action/goal_handle.c‎

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,9 @@ typedef struct rcl_action_goal_handle_impl_s
2626
{
2727
rcl_action_goal_info_t info;
2828
rcl_action_goal_state_t state;
29+
// As long as the goal has not reached terminal state, this variable is set to
30+
// INVAILD_GOAL_TERMINAL_TIMESTAMP
31+
rcl_time_point_value_t goal_terminal_timestamp;
2932
rcl_allocator_t allocator;
3033
} rcl_action_goal_handle_impl_t;
3134

@@ -68,6 +71,8 @@ rcl_action_goal_handle_init(
6871
goal_handle->impl->state = GOAL_STATE_ACCEPTED;
6972
// Copy the allocator
7073
goal_handle->impl->allocator = allocator;
74+
// Set invalid time
75+
goal_handle->impl->goal_terminal_timestamp = INVAILD_GOAL_TERMINAL_TIMESTAMP;
7176
return RCL_RET_OK;
7277
}
7378

@@ -172,6 +177,48 @@ rcl_action_goal_handle_is_valid(const rcl_action_goal_handle_t * goal_handle)
172177
return true;
173178
}
174179

180+
rcl_ret_t
181+
rcl_action_goal_handle_get_goal_terminal_timestamp(
182+
const rcl_action_goal_handle_t * goal_handle,
183+
rcl_time_point_value_t * timestamp)
184+
{
185+
RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ACTION_GOAL_HANDLE_INVALID);
186+
RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT);
187+
188+
if (!rcl_action_goal_handle_is_valid(goal_handle)) {
189+
return RCL_RET_ACTION_GOAL_HANDLE_INVALID; // error message is set
190+
}
191+
RCL_CHECK_ARGUMENT_FOR_NULL(timestamp, RCL_RET_INVALID_ARGUMENT);
192+
193+
if (goal_handle->impl->goal_terminal_timestamp == INVAILD_GOAL_TERMINAL_TIMESTAMP) {
194+
return RCL_ACTION_RET_NOT_TERMINATED_YET;
195+
}
196+
197+
*timestamp = goal_handle->impl->goal_terminal_timestamp;
198+
199+
return RCL_RET_OK;
200+
}
201+
202+
rcl_ret_t
203+
rcl_action_goal_handle_set_goal_terminal_timestamp(
204+
const rcl_action_goal_handle_t * goal_handle,
205+
rcl_time_point_value_t timestamp)
206+
{
207+
RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ACTION_GOAL_HANDLE_INVALID);
208+
RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT);
209+
210+
if (!rcl_action_goal_handle_is_valid(goal_handle)) {
211+
return RCL_RET_ACTION_GOAL_HANDLE_INVALID; // error message is set
212+
}
213+
214+
if (timestamp <= INVAILD_GOAL_TERMINAL_TIMESTAMP) {
215+
RCL_SET_ERROR_MSG("Timestamp argument is invaild !");
216+
return RCL_RET_INVALID_ARGUMENT;
217+
}
218+
goal_handle->impl->goal_terminal_timestamp = timestamp;
219+
return RCL_RET_OK;
220+
}
221+
175222
#ifdef __cplusplus
176223
}
177224
#endif

‎rcl_action/test/rcl_action/test_action_server.cpp‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -592,6 +592,9 @@ TEST_F(TestActionServer, test_action_clear_expired_goals)
592592
ASSERT_EQ(RCL_RET_OK, rcl_action_update_goal_state(goal_handle, GOAL_EVENT_EXECUTE));
593593
ASSERT_EQ(RCL_RET_OK, rcl_action_update_goal_state(goal_handle, GOAL_EVENT_ABORT));
594594

595+
// recalculate the expired goal timer after entering terminal state
596+
ASSERT_EQ(RCL_RET_OK, rcl_action_notify_goal_done(&this->action_server));
597+
595598
// Set time to something far in the future
596599
ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&this->clock, RCUTILS_S_TO_NS(99999)));
597600

‎rcl_action/test/rcl_action/test_goal_handle.cpp‎

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -170,6 +170,37 @@ TEST(TestGoalHandle, test_goal_handle_update_state_invalid)
170170
EXPECT_EQ(RCL_RET_OK, rcl_action_goal_handle_fini(&goal_handle));
171171
}
172172

173+
TEST(TestGoalHandle, rcl_action_goal_handle_get_goal_terminal_timestamp)
174+
{
175+
rcl_time_point_value_t time;
176+
177+
// Check with null argument
178+
rcl_ret_t ret = rcl_action_goal_handle_get_goal_terminal_timestamp(nullptr, &time);
179+
EXPECT_EQ(ret, RCL_RET_ACTION_GOAL_HANDLE_INVALID) << rcl_get_error_string().str;
180+
rcl_reset_error();
181+
182+
// Check with invalid goal handle
183+
rcl_action_goal_handle_t goal_handle = rcl_action_get_zero_initialized_goal_handle();
184+
ret = rcl_action_goal_handle_get_goal_terminal_timestamp(&goal_handle, &time);
185+
EXPECT_EQ(ret, RCL_RET_ACTION_GOAL_HANDLE_INVALID) << rcl_get_error_string().str;
186+
rcl_reset_error();
187+
188+
// Check with null timestamp
189+
rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info();
190+
ret = rcl_action_goal_handle_init(&goal_handle, &goal_info, rcl_get_default_allocator());
191+
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
192+
ret = rcl_action_goal_handle_get_goal_terminal_timestamp(&goal_handle, nullptr);
193+
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str;
194+
rcl_reset_error();
195+
196+
rcl_time_point_value_t timestamp;
197+
ret = rcl_action_goal_handle_get_goal_terminal_timestamp(&goal_handle, &timestamp);
198+
EXPECT_EQ(ret, RCL_ACTION_RET_NOT_TERMINATED_YET) << rcl_get_error_string().str;
199+
rcl_reset_error();
200+
201+
EXPECT_EQ(RCL_RET_OK, rcl_action_goal_handle_fini(&goal_handle));
202+
}
203+
173204
using EventStateActiveCancelableTuple =
174205
std::tuple<rcl_action_goal_event_t, rcl_action_goal_state_t, bool, bool>;
175206
using StateTransitionSequence = std::vector<EventStateActiveCancelableTuple>;

0 commit comments

Comments
 (0)