fix(rcl_action): Allow to pass the timer to action during initialization

This change is needed in order to expose the timer to event based
executors.

Signed-off-by: Janosch Machowinski <J.Machowinski@cellumation.com>

rcl_action/include/rcl_action/action_server.h

@@ -27,6 +27,7 @@ extern "C"
 #include "rcl/macros.h"
 #include "rcl/node.h"
 #include "rcl/time.h"
+#include "rcl/timer.h"
 
 #include "rosidl_runtime_c/action_type_support_struct.h"
 
@@ -161,7 +162,8 @@ rcl_action_get_zero_initialized_server(void);
  * \param[out] action_server handle to a preallocated, zero-initialized action server structure
  *   to be initialized.
  * \param[in] node valid node handle
- * \param[in] clock valid clock handle
+ * \param[in] expire_timer An initialized and canceled timer. The caller must guarantee the lifetime of the
+ *                         timer until the action is finished.
  * \param[in] type_support type support object for the action's type
  * \param[in] action_name the name of the action
  * \param[in] options action_server options, including quality of service settings
@@ -175,6 +177,17 @@ rcl_action_get_zero_initialized_server(void);
 RCL_ACTION_PUBLIC
 RCL_WARN_UNUSED
 rcl_ret_t
+rcl_action_server_init2(
+  rcl_action_server_t * action_server,
+  rcl_node_t * node,
+  rcl_timer_t expire_timer,
+  const rosidl_action_type_support_t * type_support,
+  const char * action_name,
+  const rcl_action_server_options_t * options);
+
+RCL_ACTION_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
 rcl_action_server_init(
   rcl_action_server_t * action_server,
   rcl_node_t * node,

rcl_action/src/rcl_action/action_server.c

@@ -117,6 +117,8 @@ rcl_action_get_zero_initialized_server(void)
     goto fail; \
   }
 
+RCL_ACTION_PUBLIC
+RCL_WARN_UNUSED
 rcl_ret_t
 rcl_action_server_init(
   rcl_action_server_t * action_server,
@@ -126,14 +128,56 @@ rcl_action_server_init(
   const char * action_name,
   const rcl_action_server_options_t * options)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(action_server, RCL_RET_INVALID_ARGUMENT);
-  if (!rcl_node_is_valid(node)) {
-    return RCL_RET_NODE_INVALID;  // error already set
-  }
   if (!rcl_clock_valid(clock)) {
     RCL_SET_ERROR_MSG("invalid clock");
     return RCL_RET_INVALID_ARGUMENT;
   }
+  if (!rcl_node_is_valid(node)) {
+    return RCL_RET_NODE_INVALID;  // error already set
+  }
+
+  RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT);
+  rcl_allocator_t allocator = options->allocator;
+  RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
+
+  rcl_timer_t expire_timer = rcl_get_zero_initialized_timer();
+
+  // Initialize Timer
+  rcl_ret_t ret = rcl_timer_init2(
+    &expire_timer, clock, node->context,
+    options->result_timeout.nanoseconds, NULL, allocator, true);
+  if (RCL_RET_OK != ret) {
+    return ret;
+  }
+
+  ret = rcl_action_server_init2(action_server, node, expire_timer,
+                                type_support, action_name, options);
+  action_server->impl->owns_expire_timer = true;
+
+  if(ret != RCL_RET_OK)
+  {
+    // we need to release the timer in error case
+    if (rcl_timer_fini(&action_server->impl->expire_timer) != RCL_RET_OK) {
+      ret = RCL_RET_ERROR;
+    }
+  }
+
+  return ret;
+}
+
+rcl_ret_t
+rcl_action_server_init2(
+  rcl_action_server_t * action_server,
+  rcl_node_t * node,
+  rcl_timer_t expire_timer,
+  const rosidl_action_type_support_t * type_support,
+  const char * action_name,
+  const rcl_action_server_options_t * options)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(action_server, RCL_RET_INVALID_ARGUMENT);
+  if (!rcl_node_is_valid(node)) {
+    return RCL_RET_NODE_INVALID;  // error already set
+  }
   RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT);
   RCL_CHECK_ARGUMENT_FOR_NULL(action_name, RCL_RET_INVALID_ARGUMENT);
   RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT);
@@ -157,7 +201,8 @@ rcl_action_server_init(
   action_server->impl->goal_service = rcl_get_zero_initialized_service();
   action_server->impl->cancel_service = rcl_get_zero_initialized_service();
   action_server->impl->result_service = rcl_get_zero_initialized_service();
-  action_server->impl->expire_timer = rcl_get_zero_initialized_timer();
+  action_server->impl->expire_timer = expire_timer;
+  action_server->impl->owns_expire_timer = false;
   action_server->impl->feedback_publisher = rcl_get_zero_initialized_publisher();
   action_server->impl->status_publisher = rcl_get_zero_initialized_publisher();
   action_server->impl->action_name = NULL;
@@ -178,15 +223,18 @@ rcl_action_server_init(
   PUBLISHER_INIT(status);
 
   // Store reference to clock
-  action_server->impl->clock = clock;
+  ret = rcl_timer_clock(&action_server->impl->expire_timer, &(action_server->impl->clock));
+  if (RCL_RET_OK != ret) {
+    goto fail;
+  }
 
-  // Initialize Timer
-  ret = rcl_timer_init2(
-    &action_server->impl->expire_timer, action_server->impl->clock, node->context,
-    options->result_timeout.nanoseconds, NULL, allocator, true);
+  int64_t do_not_care;
+  ret = rcl_timer_exchange_period(&action_server->impl->expire_timer,
+                                  options->result_timeout.nanoseconds, &do_not_care);
   if (RCL_RET_OK != ret) {
     goto fail;
   }
+
   // Cancel timer so it doesn't start firing
   ret = rcl_timer_cancel(&action_server->impl->expire_timer);
   if (RCL_RET_OK != ret) {
@@ -251,10 +299,17 @@ rcl_action_server_fini(rcl_action_server_t * action_server, rcl_node_t * node)
     if (rcl_publisher_fini(&action_server->impl->status_publisher, node) != RCL_RET_OK) {
       ret = RCL_RET_ERROR;
     }
-    // Finalize timer
-    if (rcl_timer_fini(&action_server->impl->expire_timer) != RCL_RET_OK) {
+
+    // cancel the timer, in case we don't own it
+    if (rcl_timer_cancel(&action_server->impl->expire_timer)  != RCL_RET_OK) {
       ret = RCL_RET_ERROR;
     }
+    if (action_server->impl->owns_expire_timer) {
+      // Finalize timer
+      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_action/src/rcl_action/action_server_impl.h

@@ -27,6 +27,7 @@ typedef struct rcl_action_server_impl_s
   rcl_publisher_t feedback_publisher;
   rcl_publisher_t status_publisher;
   rcl_timer_t expire_timer;
+  bool owns_expire_timer;
   char * action_name;
   rcl_action_server_options_t options;
   // Array of goal handles