niklas/rcl
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

[Foxy backport] Use valid clock in case of issue in rcl_timer_init (#795) Store reference to rcl_clock_t instead of copy (#797) (#805)

Browse source 
* Use valid clock in case of issue in rcl_timer_init (#795)

* Use valid clock in case of issue

Signed-off-by: Stephen Brawner <brawner@gmail.com>

* Cleanup

Signed-off-by: Stephen Brawner <brawner@gmail.com>
Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Store reference to rcl_clock_t instead of copy (#797)

rcl_clock_t can't be trivially copied because adding or removing clock
jump callbacks to a copy will free the pointer held by the original
instance.

Signed-off-by: Shane Loretz<sloretz@openrobotics.org>
Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Co-authored-by: brawner <brawner@gmail.com>
This commit is contained in:
Shane Loretz 2020-09-23 07:56:42 -07:00 • committed by GitHub
parent 055d7eba62
commit c5e903a38a
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 14 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

22
rcl_action/src/rcl_action/action_server.c
View file

@ -158,7 +158,7 @@ rcl_action_server_init(
action_server->impl->options = *options; // copy options action_server->impl->options = *options; // copy options
action_server->impl->goal_handles = NULL; action_server->impl->goal_handles = NULL;
action_server->impl->num_goal_handles = 0u; action_server->impl->num_goal_handles = 0u;
action_server->impl->clock.type = RCL_CLOCK_UNINITIALIZED; action_server->impl->clock = NULL;
rcl_ret_t ret = RCL_RET_OK; rcl_ret_t ret = RCL_RET_OK;
// Initialize services // Initialize services
@ -170,10 +170,13 @@ rcl_action_server_init(
PUBLISHER_INIT(feedback); PUBLISHER_INIT(feedback);
PUBLISHER_INIT(status); PUBLISHER_INIT(status);
// Store reference to clock
action_server->impl->clock = clock;
// Initialize Timer // Initialize Timer
ret = rcl_timer_init( ret = rcl_timer_init(
&action_server->impl->expire_timer, clock, node->context, options->result_timeout.nanoseconds, &action_server->impl->expire_timer, action_server->impl->clock, node->context,
NULL, allocator); options->result_timeout.nanoseconds, NULL, allocator);
if (RCL_RET_OK != ret) { if (RCL_RET_OK != ret) {
goto fail; goto fail;
} }
@ -183,9 +186,6 @@ rcl_action_server_init(
goto fail; goto fail;
} }
// Copy clock
action_server->impl->clock = *clock;
// Copy action name // Copy action name
action_server->impl->action_name = rcutils_strdup(action_name, allocator); action_server->impl->action_name = rcutils_strdup(action_name, allocator);
if (NULL == action_server->impl->action_name) { if (NULL == action_server->impl->action_name) {
@ -235,6 +235,8 @@ rcl_action_server_fini(rcl_action_server_t * action_server, rcl_node_t * node)
if (rcl_timer_fini(&action_server->impl->expire_timer) != RCL_RET_OK) { if (rcl_timer_fini(&action_server->impl->expire_timer) != RCL_RET_OK) {
ret = RCL_RET_ERROR; ret = RCL_RET_ERROR;
} }
// Ditch clock reference
action_server->impl->clock = NULL;
// Deallocate action name // Deallocate action name
rcl_allocator_t allocator = action_server->impl->options.allocator; rcl_allocator_t allocator = action_server->impl->options.allocator;
if (action_server->impl->action_name) { if (action_server->impl->action_name) {
@ -380,7 +382,7 @@ rcl_action_accept_new_goal(
rcl_action_goal_info_t goal_info_stamp_now = rcl_action_get_zero_initialized_goal_info(); rcl_action_goal_info_t goal_info_stamp_now = rcl_action_get_zero_initialized_goal_info();
goal_info_stamp_now = *goal_info; goal_info_stamp_now = *goal_info;
rcl_time_point_value_t now_time_point; rcl_time_point_value_t now_time_point;
rcl_ret_t ret = rcl_clock_get_now(&action_server->impl->clock, &now_time_point); rcl_ret_t ret = rcl_clock_get_now(action_server->impl->clock, &now_time_point);
if (RCL_RET_OK != ret) { if (RCL_RET_OK != ret) {
return NULL; // Error already set return NULL; // Error already set
} }
@ -584,7 +586,7 @@ rcl_action_expire_goals(
// Get current time (nanosec) // Get current time (nanosec)
int64_t current_time; int64_t current_time;
rcl_ret_t ret = rcl_clock_get_now(&action_server->impl->clock, &current_time); rcl_ret_t ret = rcl_clock_get_now(action_server->impl->clock, &current_time);
if (RCL_RET_OK != ret) { if (RCL_RET_OK != ret) {
return RCL_RET_ERROR; return RCL_RET_ERROR;
} }
@ -659,7 +661,7 @@ rcl_action_expire_goals(
action_server->impl->options.result_timeout.nanoseconds, action_server->impl->options.result_timeout.nanoseconds,
action_server->impl->goal_handles, action_server->impl->goal_handles,
action_server->impl->num_goal_handles, action_server->impl->num_goal_handles,
&action_server->impl->clock); action_server->impl->clock);
// If argument is not null, then set it // If argument is not null, then set it
if (NULL != num_expired) { if (NULL != num_expired) {
@ -680,7 +682,7 @@ rcl_action_notify_goal_done(
action_server->impl->options.result_timeout.nanoseconds, action_server->impl->options.result_timeout.nanoseconds,
action_server->impl->goal_handles, action_server->impl->goal_handles,
action_server->impl->num_goal_handles, action_server->impl->num_goal_handles,
&action_server->impl->clock); action_server->impl->clock);
} }
rcl_ret_t rcl_ret_t

2
rcl_action/src/rcl_action/action_server_impl.h
View file

@ -33,7 +33,7 @@ typedef struct rcl_action_server_impl_t
rcl_action_goal_handle_t ** goal_handles; rcl_action_goal_handle_t ** goal_handles;
size_t num_goal_handles; size_t num_goal_handles;
// Clock // Clock
rcl_clock_t clock; rcl_clock_t * clock;
// Wait set records // Wait set records
size_t wait_set_goal_service_index; size_t wait_set_goal_service_index;
size_t wait_set_cancel_service_index; size_t wait_set_cancel_service_index;