implement timer.c

rcl/CMakeLists.txt

@@ -26,6 +26,7 @@ set(${PROJECT_NAME}_sources
   src/rcl/subscription.c
   src/rcl/wait.c
   src/rcl/time.c
+  src/rcl/timer.c
 )
 add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_sources})
 ament_target_dependencies(${PROJECT_NAME}

rcl/include/rcl/time.h

@@ -23,13 +23,13 @@ extern "C"
 #include "rcl/types.h"
 #include "rmw/rmw.h"
 
-#define RCL_S_TO_NS(seconds) seconds * 1000000000
-#define RCL_MS_TO_NS(milliseconds) milliseconds * 1000000
-#define RCL_US_TO_NS(microseconds) microseconds * 1000
+#define RCL_S_TO_NS(seconds) (seconds * 1000000000)
+#define RCL_MS_TO_NS(milliseconds) (milliseconds * 1000000)
+#define RCL_US_TO_NS(microseconds) (microseconds * 1000)
 
-#define RCL_NS_TO_S(nanoseconds) nanoseconds / 1000000000
-#define RCL_NS_TO_MS(nanoseconds) nanoseconds / 1000000
-#define RCL_NS_TO_US(nanoseconds) nanoseconds / 1000
+#define RCL_NS_TO_S(nanoseconds) (nanoseconds / 1000000000)
+#define RCL_NS_TO_MS(nanoseconds) (nanoseconds / 1000000)
+#define RCL_NS_TO_US(nanoseconds) (nanoseconds / 1000)
 
 typedef rmw_time_t rcl_time_t;
 
@@ -41,6 +41,11 @@ rcl_time_from_int64_t_nanoseconds(int64_t nanoseconds);
 rcl_time_t
 rcl_time_from_uint64_t_nanoseconds(uint64_t nanoseconds);
 
+/// Convert a rcl_time_t struct into an unsigned number of nanoseconds.
+/* This function will return 0 if the time argument is NULL. */
+uint64_t
+rcl_time_to_uint64_t_nanoseconds(const rcl_time_t * time);
+
 /// Retrieve the current time as a rcl_time_t struct.
 /* The now argument must point to an allocated rcl_time_t struct, as the
  * result is copied into this variable.

rcl/include/rcl/timer.h

@@ -32,7 +32,7 @@ struct rcl_timer_impl_t;
 /// Handle for a ROS timer.
 typedef struct rcl_timer_t {
   /// Private implementation pointer.
-  rcl_timer_impl_t * impl;
+  struct rcl_timer_impl_t * impl;
 } rcl_timer_t;
 
 /// User callback signature for timers.
@@ -77,7 +77,25 @@ rcl_get_zero_initialized_timer();
  * struct which is the time of the previous call or when the timer was created
  * if it is the first call to the callback.
  *
+ * Expected usage:
+ *
+ *   #include <rcl/rcl.h>
+ *
+ *   void my_timer_callback(rcl_timer_t * timer, rcl_time_t last_call_time)
+ *   {
+ *     // Do timer work...
+ *     // Optionally reconfigure, cancel, or reset the timer...
+ *   }
+ *
+ *   rcl_timer_t timer = rcl_get_zero_initialized_timer();
+ *   rcl_ret_t ret =
+ *     rcl_timer_init(&timer, RCL_MS_TO_NS(100), my_timer_callback, rcl_get_default_allocator());
+ *   // ... error handling, use the timer with a wait set, or poll it manually, then cleanup
+ *   ret = rcl_timer_fini(&timer);
+ *   // ... error handling
+ *
  * This function is not thread-safe.
+ * This function is lock-free.
  *
  * \param[inout] timer the timer handle to be initialized
  * \param[in] period the duration between calls to the callback in nanoseconds
@@ -85,6 +103,7 @@ rcl_get_zero_initialized_timer();
  * \param[in] allocator the allocator to use for allocations
  * \return RCL_RET_OK if the timer was initialized successfully, or
  *         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+ *         RCL_RET_ALREADY_INIT if the timer was already initialized, or
  *         RCL_RET_BAD_ALLOC if allocating memory failed, or
  *         RCL_RET_ERROR an unspecified error occur.
  */
@@ -120,7 +139,8 @@ rcl_timer_fini(rcl_timer_t * timer);
  *  - Ensure the timer has not been canceled.
  *  - Get the current time into a temporary rcl_time_t.
  *  - Exchange the current time with the last call time of the timer.
- *  - Call the callback, with this timer and the exchanged last call time.
+ *  - Call the callback, with this timer and the last call time as arguments.
+ *  - Return after the callback has completed.
  *
  * During the callback the timer can be canceled or have its period and/or
  * callback modified.
@@ -213,6 +233,48 @@ rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_unt
 rcl_ret_t
 rcl_timer_get_last_call_time(const rcl_timer_t * timer, rcl_time_t * last_call_time);
 
+/// Retrieve the period of the timer.
+/* This function retrieves the period and copies it into the give variable.
+ *
+ * The period argument must be a pointer to an already allocated uint64_t.
+ *
+ * This function is thread-safe.
+ * This function is lock-free so long as the C11's stdatomic.h function
+ * atomic_is_lock_free() returns true for atomic_int_least64_t.
+ *
+ * \param[in] timer the handle to the timer which is being queried
+ * \param[out] period the uint64_t in which the period is stored
+ * \return RCL_RET_OK if the period was retrieved successfully, or
+ *         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+ *         RCL_RET_TIMER_INVALID if the timer is invalid, or
+ *         RCL_RET_ERROR an unspecified error occur.
+ */
+rcl_ret_t
+rcl_timer_get_period(const rcl_timer_t * timer, uint64_t * period);
+
+/// Exchange the period of the timer and return the previous period.
+/* This function exchanges the period in the timer and copies the old one into
+ * the give variable.
+ *
+ * Exchanging (changing) the period will not affect already waiting wait sets.
+ *
+ * The old_period argument must be a pointer to an already allocated uint64_t.
+ *
+ * This function is thread-safe.
+ * This function is lock-free so long as the C11's stdatomic.h function
+ * atomic_is_lock_free() returns true for atomic_int_least64_t.
+ *
+ * \param[in] timer the handle to the timer which is being modified
+ * \param[out] new_period the uint64_t to exchange into the timer
+ * \param[out] old_period the uint64_t in which the previous period is stored
+ * \return RCL_RET_OK if the period was retrieved successfully, or
+ *         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+ *         RCL_RET_TIMER_INVALID if the timer is invalid, or
+ *         RCL_RET_ERROR an unspecified error occur.
+ */
+rcl_ret_t
+rcl_timer_exchange_period(const rcl_timer_t * timer, uint64_t new_period, uint64_t * old_period);
+
 /// Return the current timer callback.
 /* This function can fail, and therefore return NULL, if:
  *   - timer is NULL
@@ -238,12 +300,12 @@ rcl_timer_get_callback(const rcl_timer_t * timer);
  * This function is lock-free so long as the C11's stdatomic.h function
  * atomic_is_lock_free() returns true for atomic_uintptr_t.
  *
- * \param[inout] timer handle to the timer from the callback should be returned
- * \param[in] timer handle to the timer from the callback should be returned
+ * \param[inout] timer handle to the timer from the callback should be exchanged
+ * \param[in] new_callback the callback to be exchanged into the timer
  * \return function pointer to the old callback, or NULL if an error occurred
  */
 rcl_timer_callback_t
-rcl_timer_exchange_callback(rcl_time_t * timer, const rcl_timer_callback_t new_callback);
+rcl_timer_exchange_callback(rcl_timer_t * timer, const rcl_timer_callback_t new_callback);
 
 /// Cancel a timer.
 /* When a timer is canceled, rcl_timer_is_ready() will return false for that
@@ -253,7 +315,8 @@ rcl_timer_exchange_callback(rcl_time_t * timer, const rcl_timer_callback_t new_c
  * Calling this function on an already canceled timer will succeed.
  *
  * This function is thread-safe.
- * This function is lock-free.
+ * This function is lock-free so long as the C11's stdatomic.h function
+ * atomic_is_lock_free() returns true for atomic_bool.
  *
  * \param[inout] timer the timer to be canceled
  * \return RCL_RET_OK if the last call time was retrieved successfully, or
@@ -272,7 +335,8 @@ rcl_timer_cancel(rcl_timer_t * timer);
  * copied into this variable.
  *
  * This function is thread-safe.
- * This function is lock-free.
+ * This function is lock-free so long as the C11's stdatomic.h function
+ * atomic_is_lock_free() returns true for atomic_bool.
  *
  * \param[in] timer the timer to be queried
  * \return RCL_RET_OK if the last call time was retrieved successfully, or

rcl/src/rcl/time.c

@@ -57,6 +57,15 @@ rcl_time_from_uint64_t_nanoseconds(uint64_t nanoseconds)
   return result;
 }
 
+uint64_t
+rcl_time_to_uint64_t_nanoseconds(const rcl_time_t * rcl_time)
+{
+  if (!rcl_time) {
+    return 0;
+  }
+  return RCL_S_TO_NS(rcl_time->sec) + rcl_time->nsec;
+}
+
 static void
 __timespec_get_now(struct timespec * timespec_now)
 {

rcl/src/rcl/timer.c

@@ -0,0 +1,236 @@
+// Copyright 2015 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#if __cplusplus
+extern "C"
+{
+#endif
+
+#include "rcl/timer.h"
+
+#include <stdatomic.h>
+
+#include "./common.h"
+
+typedef struct rcl_timer_impl_t {
+  // The user supplied callback.
+  atomic_uintptr_t callback;
+  // This is a duration in nanoseconds.
+  atomic_uint_least64_t period;
+  // This is an absolute time in nanoseconds since the unix epoch.
+  atomic_uint_least64_t last_call_time;
+  // A flag which indicates if the timer is canceled.
+  atomic_bool canceled;
+  // The user supplied allocator.
+  rcl_allocator_t allocator;
+} rcl_timer_impl_t;
+
+rcl_timer_t
+rcl_get_zero_initialized_timer()
+{
+  static rcl_timer_t null_timer = {0};
+  return null_timer;
+}
+
+rcl_ret_t
+rcl_timer_init(
+  rcl_timer_t * timer,
+  uint64_t period,
+  const rcl_timer_callback_t callback,
+  rcl_allocator_t allocator)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(callback, RCL_RET_INVALID_ARGUMENT);
+  if (timer->impl) {
+    RCL_SET_ERROR_MSG("timer already initailized, or memory was uninitialized");
+    return RCL_RET_ALREADY_INIT;
+  }
+  rcl_time_t now;
+  rcl_ret_t now_ret = rcl_time_now(&now);
+  if (now_ret != RCL_RET_OK) {
+    return now_ret;  // rcl error state should already be set.
+  }
+  rcl_timer_impl_t impl = {
+    .callback = ATOMIC_VAR_INIT((uintptr_t)callback),
+    .period = ATOMIC_VAR_INIT(period),
+    .last_call_time = ATOMIC_VAR_INIT(rcl_time_to_uint64_t_nanoseconds(&now)),
+    .canceled = ATOMIC_VAR_INIT(false),
+    .allocator = allocator,
+  };
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    allocator.allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    allocator.deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT);
+  timer->impl = (rcl_timer_impl_t *)allocator.allocate(sizeof(rcl_timer_impl_t), allocator.state);
+  RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC);
+  *timer->impl = impl;
+  return RCL_RET_OK;
+}
+
+rcl_ret_t
+rcl_timer_fini(rcl_timer_t * timer)
+{
+  if (!timer || !timer->impl) {
+    return RCL_RET_OK;
+  }
+  // Will return either RCL_RET_OK or RCL_RET_ERROR since the timer is valid.
+  rcl_ret_t result = rcl_timer_cancel(timer);
+  rcl_allocator_t allocator = timer->impl->allocator;
+  allocator.deallocate(timer->impl, allocator.state);
+  return result;
+}
+
+rcl_ret_t
+rcl_timer_call(rcl_timer_t * timer)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
+  if (atomic_load(&timer->impl->canceled)) {
+    RCL_SET_ERROR_MSG("timer is canceled");
+    return RCL_RET_TIMER_CANCELED;
+  }
+  rcl_time_t now;
+  rcl_ret_t now_ret = rcl_time_now(&now);
+  if (now_ret != RCL_RET_OK) {
+    return now_ret;  // rcl error state should already be set.
+  }
+  uint64_t previous_ns =
+    atomic_exchange(&timer->impl->last_call_time, rcl_time_to_uint64_t_nanoseconds(&now));
+  rcl_time_t previous = rcl_time_from_uint64_t_nanoseconds(previous_ns);
+  rcl_timer_callback_t typed_callback = (rcl_timer_callback_t)atomic_load(&timer->impl->callback);
+  typed_callback(timer, previous);
+  return RCL_RET_OK;
+}
+
+rcl_ret_t
+rcl_timer_is_ready(const rcl_timer_t * timer, bool * is_ready)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(is_ready, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
+  int64_t time_until_next_call;
+  rcl_ret_t ret = rcl_timer_get_time_until_next_call(timer, &time_until_next_call);
+  if (ret != RCL_RET_OK) {
+    return ret;  // rcl error state should already be set.
+  }
+  *is_ready = (time_until_next_call <= 0) && !atomic_load(&timer->impl->canceled);
+  return RCL_RET_OK;
+}
+
+rcl_ret_t
+rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_until_next_call)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_until_next_call, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
+  rcl_time_t now, last_call_time;
+  rcl_ret_t ret = rcl_time_now(&now);
+  if (ret != RCL_RET_OK) {
+    return ret;  // rcl error state should already be set.
+  }
+  ret = rcl_timer_get_last_call_time(timer, &last_call_time);
+  if (ret != RCL_RET_OK) {
+    return ret;  // rcl error state should already be set.
+  }
+  uint64_t period = atomic_load(&timer->impl->period);
+  int64_t next_call = rcl_time_to_uint64_t_nanoseconds(&last_call_time) + period;
+  *time_until_next_call = next_call - rcl_time_to_uint64_t_nanoseconds(&now);
+  return RCL_RET_OK;
+}
+
+rcl_ret_t
+rcl_timer_get_last_call_time(const rcl_timer_t * timer, rcl_time_t * last_call_time)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(last_call_time, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
+  uint64_t last_call_time_ns = atomic_load(&timer->impl->last_call_time);
+  *last_call_time = rcl_time_from_uint64_t_nanoseconds(last_call_time_ns);
+  return RCL_RET_OK;
+}
+
+rcl_ret_t
+rcl_timer_get_period(const rcl_timer_t * timer, uint64_t * period)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(period, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
+  *period = atomic_load(&timer->impl->period);
+  return RCL_RET_OK;
+}
+
+rcl_ret_t
+rcl_timer_exchange_period(const rcl_timer_t * timer, uint64_t new_period, uint64_t * old_period)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(old_period, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
+  *old_period = atomic_exchange(&timer->impl->period, new_period);
+  return RCL_RET_OK;
+}
+
+rcl_timer_callback_t
+rcl_timer_get_callback(const rcl_timer_t * timer)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, NULL);
+  RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return NULL);
+  return (rcl_timer_callback_t)atomic_load(&timer->impl->callback);
+}
+
+rcl_timer_callback_t
+rcl_timer_exchange_callback(rcl_timer_t * timer, const rcl_timer_callback_t new_callback)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, NULL);
+  RCL_CHECK_ARGUMENT_FOR_NULL(new_callback, NULL);
+  RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return NULL);
+  return (rcl_timer_callback_t)atomic_exchange(&timer->impl->callback, (uintptr_t)new_callback);
+}
+
+rcl_ret_t
+rcl_timer_cancel(rcl_timer_t * timer)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
+  atomic_store(&timer->impl->canceled, true);
+  return RCL_RET_OK;
+}
+
+rcl_ret_t
+rcl_timer_is_canceled(const rcl_timer_t * timer, bool * is_canceled)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(is_canceled, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
+  *is_canceled = atomic_load(&timer->impl->canceled);
+  return RCL_RET_OK;
+}
+
+rcl_ret_t
+rcl_timer_reset(rcl_timer_t * timer)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
+  rcl_time_t now;
+  rcl_ret_t now_ret = rcl_time_now(&now);
+  if (now_ret != RCL_RET_OK) {
+    return now_ret;  // rcl error state should already be set.
+  }
+  atomic_store(&timer->impl->last_call_time, rcl_time_to_uint64_t_nanoseconds(&now));
+  atomic_store(&timer->impl->canceled, false);
+  return RCL_RET_OK;
+}
+
+#if __cplusplus
+}
+#endif