diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c index 626101e..6ef4a5d 100644 --- a/rcl_action/src/rcl_action/action_server.c +++ b/rcl_action/src/rcl_action/action_server.c @@ -158,7 +158,7 @@ rcl_action_server_init( action_server->impl->options = *options; // copy options action_server->impl->goal_handles = NULL; 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; // Initialize services @@ -170,10 +170,13 @@ rcl_action_server_init( PUBLISHER_INIT(feedback); PUBLISHER_INIT(status); - // Initialize Timer + // Store reference to clock + action_server->impl->clock = clock; + +// Initialize Timer ret = rcl_timer_init( - &action_server->impl->expire_timer, clock, node->context, options->result_timeout.nanoseconds, - NULL, allocator); + &action_server->impl->expire_timer, action_server->impl->clock, node->context, + options->result_timeout.nanoseconds, NULL, allocator); if (RCL_RET_OK != ret) { goto fail; } @@ -183,9 +186,6 @@ rcl_action_server_init( goto fail; } - // Copy clock - action_server->impl->clock = *clock; - // Copy action name action_server->impl->action_name = rcutils_strdup(action_name, allocator); 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) { ret = RCL_RET_ERROR; } + // Ditch clock reference + action_server->impl->clock = NULL; // Deallocate action name rcl_allocator_t allocator = action_server->impl->options.allocator; 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(); goal_info_stamp_now = *goal_info; 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) { return NULL; // Error already set } @@ -584,7 +586,7 @@ rcl_action_expire_goals( // Get current time (nanosec) int64_t current_time; - rcl_ret_t ret = rcl_clock_get_now(&action_server->impl->clock, ¤t_time); + rcl_ret_t ret = rcl_clock_get_now(action_server->impl->clock, ¤t_time); if (RCL_RET_OK != ret) { return RCL_RET_ERROR; } @@ -659,7 +661,7 @@ rcl_action_expire_goals( action_server->impl->options.result_timeout.nanoseconds, action_server->impl->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 (NULL != num_expired) { @@ -680,7 +682,7 @@ rcl_action_notify_goal_done( action_server->impl->options.result_timeout.nanoseconds, action_server->impl->goal_handles, action_server->impl->num_goal_handles, - &action_server->impl->clock); + action_server->impl->clock); } rcl_ret_t diff --git a/rcl_action/src/rcl_action/action_server_impl.h b/rcl_action/src/rcl_action/action_server_impl.h index af718a1..025cc8b 100644 --- a/rcl_action/src/rcl_action/action_server_impl.h +++ b/rcl_action/src/rcl_action/action_server_impl.h @@ -33,7 +33,7 @@ typedef struct rcl_action_server_impl_t rcl_action_goal_handle_t ** goal_handles; size_t num_goal_handles; // Clock - rcl_clock_t clock; + rcl_clock_t * clock; // Wait set records size_t wait_set_goal_service_index; size_t wait_set_cancel_service_index;