[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)

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

* Use valid clock in case of issue

Signed-off-by: Stephen Brawner <[email protected]>

* Cleanup

Signed-off-by: Stephen Brawner <[email protected]>
Signed-off-by: Shane Loretz <[email protected]>

* 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<[email protected]>
Signed-off-by: Shane Loretz <[email protected]>

Co-authored-by: brawner <[email protected]>

Author: sloretz

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, &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;
   }
@@ -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

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;