Allow specifying an initial timer trigger time in create_timer/create_wall_timer

rclcpp/include/rclcpp/create_timer.hpp

@@ -170,6 +170,57 @@ create_timer(
   return timer;
 }
 
+/// Convenience method to create a general timer with an initial trigger time using node resources.
+/**
+ *
+ * \tparam DurationRepT
+ * \tparam DurationT
+ * \tparam CallbackT
+ * \param clock clock to be used
+ * \param initial_call_time time at which the callback should be initially triggered
+ * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max()
+ * \param callback callback to execute via the timer period
+ * \param group callback group
+ * \param node_base node base interface
+ * \param node_timers node timer interface
+ * \param autostart defines if the timer should start it's countdown on initialization or not.
+ * \return shared pointer to a generic timer
+ * \throws std::invalid_argument if either clock, node_base or node_timers
+ * are nullptr, or period is negative or too large
+ * \throws std::runtime_error if initial_call_time's clock type does not match clock's
+ */
+template<typename DurationRepT, typename DurationT, typename CallbackT>
+typename rclcpp::GenericTimer<CallbackT>::SharedPtr
+create_timer(
+  rclcpp::Clock::SharedPtr clock,
+  Time initial_call_time,
+  std::chrono::duration<DurationRepT, DurationT> period,
+  CallbackT callback,
+  rclcpp::CallbackGroup::SharedPtr group,
+  node_interfaces::NodeBaseInterface * node_base,
+  node_interfaces::NodeTimersInterface * node_timers,
+  bool autostart = true)
+{
+  if (clock == nullptr) {
+    throw std::invalid_argument{"clock cannot be null"};
+  }
+  if (node_base == nullptr) {
+    throw std::invalid_argument{"input node_base cannot be null"};
+  }
+  if (node_timers == nullptr) {
+    throw std::invalid_argument{"input node_timers cannot be null"};
+  }
+
+  const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period);
+
+  // Add a new generic timer.
+  auto timer = rclcpp::GenericTimer<CallbackT>::make_shared(
+    std::move(clock), initial_call_time, period_ns, std::move(callback),
+    node_base->get_context(), autostart);
+  node_timers->add_timer(timer, group);
+  return timer;
+}
+
 /// Convenience method to create a wall timer with node resources.
 /**
  *
@@ -211,6 +262,51 @@ create_wall_timer(
   node_timers->add_timer(timer, group);
   return timer;
 }
+
+/// Convenience method to create a wall timer with an initial trigger time using node resources.
+/**
+ *
+ * \tparam DurationRepT
+ * \tparam DurationT
+ * \tparam CallbackT
+ * \param initial_call_time time at which the callback should be initially triggered
+ * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max()
+ * \param callback callback to execute via the timer period
+ * \param group callback group
+ * \param node_base node base interface
+ * \param node_timers node timer interface
+ * \return shared pointer to a wall timer
+ * \throws std::invalid_argument if either node_base or node_timers
+ * are null, or period is negative or too large
+ * \throws std::runtime_error if initial_call_time's clock type is not RCL_STEADY_TIME
+ */
+template<typename DurationRepT, typename DurationT, typename CallbackT>
+typename rclcpp::WallTimer<CallbackT>::SharedPtr
+create_wall_timer(
+  Time initial_call_time,
+  std::chrono::duration<DurationRepT, DurationT> period,
+  CallbackT callback,
+  rclcpp::CallbackGroup::SharedPtr group,
+  node_interfaces::NodeBaseInterface * node_base,
+  node_interfaces::NodeTimersInterface * node_timers,
+  bool autostart = true)
+{
+  if (node_base == nullptr) {
+    throw std::invalid_argument{"input node_base cannot be null"};
+  }
+
+  if (node_timers == nullptr) {
+    throw std::invalid_argument{"input node_timers cannot be null"};
+  }
+
+  const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period);
+
+  // Add a new wall timer.
+  auto timer = rclcpp::WallTimer<CallbackT>::make_shared(
+    initial_call_time, period_ns, std::move(callback), node_base->get_context(), autostart);
+  node_timers->add_timer(timer, group);
+  return timer;
+}
 }  // namespace rclcpp
 
 #endif  // RCLCPP__CREATE_TIMER_HPP_

rclcpp/include/rclcpp/node.hpp

@@ -242,6 +242,24 @@ class Node : public std::enable_shared_from_this<Node>
     const rclcpp::CallbackGroup::SharedPtr & group = nullptr,
     bool autostart = true);
 
+  /// Create a wall timer that uses the wall clock to drive the callback with an initial trigger
+  /// time.
+  /**
+   * \param[in] initial_call_time Time at which the callback should be initially triggered.
+   * \param[in] period Time interval between triggers of the callback.
+   * \param[in] callback User-defined callback function.
+   * \param[in] group Callback group to execute this timer's callback in.
+   * \param[in] autostart The state of the clock on initialization.
+   */
+  template<typename DurationRepT = int64_t, typename DurationT = std::milli, typename CallbackT>
+  typename rclcpp::WallTimer<CallbackT>::SharedPtr
+  create_wall_timer(
+    Time initial_call_time,
+    std::chrono::duration<DurationRepT, DurationT> period,
+    CallbackT callback,
+    const rclcpp::CallbackGroup::SharedPtr & group = nullptr,
+    bool autostart = true);
+
   /// Create a timer that uses the node clock to drive the callback.
   /**
    * \param[in] period Time interval between triggers of the callback.
@@ -255,6 +273,39 @@ class Node : public std::enable_shared_from_this<Node>
     CallbackT callback,
     const rclcpp::CallbackGroup::SharedPtr & group = nullptr);
 
+  /// Create a timer that uses the node clock to drive the callback with an initial trigger time.
+  /**
+   * \param[in] initial_call_time Time at which the callback should be initially triggered.
+   * \param[in] period Time interval between triggers of the callback.
+   * \param[in] callback User-defined callback function.
+   * \param[in] group Callback group to execute this timer's callback in.
+   * \param[in] autostart The state of the timer on initialization.
+   */
+  template<typename DurationRepT = int64_t, typename DurationT = std::milli, typename CallbackT>
+  typename rclcpp::GenericTimer<CallbackT>::SharedPtr
+  create_timer(
+    Time initial_call_time,
+    std::chrono::duration<DurationRepT, DurationT> period,
+    CallbackT callback,
+    const rclcpp::CallbackGroup::SharedPtr & group = nullptr,
+    bool autostart = true);
+
+  /// Create and return a Client.
+  /**
+   * \param[in] service_name The topic to service on.
+   * \param[in] qos_profile rmw_qos_profile_t Quality of service profile for client.
+   * \param[in] group Callback group to call the service.
+   * \return Shared pointer to the created client.
+   * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t
+   */
+  template<typename ServiceT>
+  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
+  typename rclcpp::Client<ServiceT>::SharedPtr
+  create_client(
+    const std::string & service_name,
+    const rmw_qos_profile_t & qos_profile,
+    rclcpp::CallbackGroup::SharedPtr group = nullptr);
+
   /// Create and return a Client.
   /**
    * \param[in] service_name The name on which the service is accessible.

rclcpp/include/rclcpp/node_impl.hpp

@@ -116,6 +116,25 @@ Node::create_wall_timer(
     autostart);
 }
 
+template<typename DurationRepT, typename DurationT, typename CallbackT>
+typename rclcpp::WallTimer<CallbackT>::SharedPtr
+Node::create_wall_timer(
+  rclcpp::Time initial_call_time,
+  std::chrono::duration<DurationRepT, DurationT> period,
+  CallbackT callback,
+  const rclcpp::CallbackGroup::SharedPtr & group,
+  bool autostart)
+{
+  return rclcpp::create_wall_timer(
+    initial_call_time,
+    period,
+    std::move(callback),
+    group,
+    this->node_base_.get(),
+    this->node_timers_.get(),
+    autostart);
+}
+
 template<typename DurationRepT, typename DurationT, typename CallbackT>
 typename rclcpp::GenericTimer<CallbackT>::SharedPtr
 Node::create_timer(
@@ -132,6 +151,26 @@ Node::create_timer(
     this->node_timers_.get());
 }
 
+template<typename DurationRepT, typename DurationT, typename CallbackT>
+typename rclcpp::GenericTimer<CallbackT>::SharedPtr
+Node::create_timer(
+  rclcpp::Time initial_call_time,
+  std::chrono::duration<DurationRepT, DurationT> period,
+  CallbackT callback,
+  const rclcpp::CallbackGroup::SharedPtr & group,
+  bool autostart)
+{
+  return rclcpp::create_timer(
+    this->get_clock(),
+    initial_call_time,
+    period,
+    std::move(callback),
+    group,
+    this->node_base_.get(),
+    this->node_timers_.get(),
+    autostart);
+}
+
 template<typename ServiceT>
 typename Client<ServiceT>::SharedPtr
 Node::create_client(

rclcpp/include/rclcpp/timer.hpp

@@ -47,6 +47,30 @@ struct TimerInfo
   Time actual_call_time;
 };
 
+/// Compute a phase-aligned start time for a periodic timer.
+/**
+ * The result is the smallest time greater than or equal to `clock.now()`
+ * of the form `k * interval + phase` for some non-negative integer `k`.
+ *
+ * This is useful for synchronizing periodic timers across multiple nodes
+ * or processes that share a common, synchronized clock (e.g. ROS time)
+ * without needing to exchange an explicit start time out-of-band: any two
+ * callers with a synchronized clock computing this function with the same
+ * interval and phase will agree on the same sequence of aligned instants.
+ *
+ * \param[in] clock clock used to obtain the current time
+ * \param[in] interval alignment interval; must be greater than zero
+ * \param[in] phase offset added to each interval boundary
+ * \return the computed, phase-aligned start time, using clock's clock type
+ * \throws std::invalid_argument if interval is not greater than zero
+ */
+RCLCPP_PUBLIC
+Time
+compute_phase_aligned_time(
+  const Clock & clock,
+  std::chrono::nanoseconds interval,
+  std::chrono::nanoseconds phase = std::chrono::nanoseconds(0));
+
 class TimerBase
 {
 public:
@@ -69,6 +93,27 @@ class TimerBase
     rclcpp::Context::SharedPtr context,
     bool autostart = true);
 
+  /// Constructor allowing for specification of an initial trigger time
+  /**
+   * \param clock A clock to use for time and sleeping
+   * \param initial_call_time The time at which the callback should be initially triggered
+   * \param period The interval at which the timer fires
+   * \param context node context
+   * \param autostart timer state on initialization
+   *
+   * In order to activate a timer that is not started on initialization,
+   * user should call the reset() method.
+   *
+   * \throws std::runtime_error if initial_call_time's clock type does not match clock's
+   */
+  RCLCPP_PUBLIC
+  explicit TimerBase(
+    Clock::SharedPtr clock,
+    Time initial_call_time,
+    std::chrono::nanoseconds period,
+    rclcpp::Context::SharedPtr context,
+    bool autostart = true);
+
   /// TimerBase destructor
   RCLCPP_PUBLIC
   virtual
@@ -100,6 +145,28 @@ class TimerBase
   void
   reset();
 
+  /// Resume the timer, preserving its existing schedule phase.
+  /**
+   * Unlike reset(), this does not unconditionally recompute the next call
+   * time from the current time; if the timer's next call time is still in
+   * the future, it is left unchanged.
+   * If it is in the past (e.g. because the timer was canceled and is being
+   * resumed some time later), it is advanced by whole periods until it is
+   * in the future again, without shifting the phase established when the
+   * timer was initialized (or last had its next call time explicitly set).
+   * A canceled timer is also made not canceled by this call.
+   *
+   * This makes it possible to initialize a timer with autostart false and
+   * an explicit initial call time, and later resume it without losing the
+   * originally intended schedule, which is not possible with reset() since
+   * it always recomputes the next call time as now() + period.
+   *
+   * \throws std::runtime_error if the rcl_timer_resume returns a failure
+   */
+  RCLCPP_PUBLIC
+  void
+  resume();
+
   /// Indicate that we're about to execute the callback.
   /**
    * The multithreaded executor takes advantage of this to avoid scheduling
@@ -258,6 +325,40 @@ class GenericTimer : public TimerBase
 #endif
   }
 
+  /// Constructor allowing for specification of an initial trigger time.
+  /**
+   * \param[in] clock The clock providing the current time.
+   * \param[in] initial_call_time The time at which the callback should be initially triggered.
+   * \param[in] period The interval at which the timer fires.
+   * \param[in] callback User-specified callback function.
+   * \param[in] context custom context to be used.
+   * \param autostart timer state on initialization
+   *
+   * \throws std::runtime_error if initial_call_time's clock type does not match clock's
+   */
+  explicit GenericTimer(
+    Clock::SharedPtr clock, Time initial_call_time, std::chrono::nanoseconds period,
+    FunctorT && callback, rclcpp::Context::SharedPtr context, bool autostart = true
+  )
+  : TimerBase(clock, initial_call_time, period, context, autostart),
+    callback_(std::forward<FunctorT>(callback))
+  {
+    TRACETOOLS_TRACEPOINT(
+      rclcpp_timer_callback_added,
+      static_cast<const void *>(get_timer_handle().get()),
+      reinterpret_cast<const void *>(&callback_));
+#ifndef TRACETOOLS_DISABLED
+    if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) {
+      char * symbol = tracetools::get_symbol(callback_);
+      TRACETOOLS_DO_TRACEPOINT(
+        rclcpp_callback_register,
+        reinterpret_cast<const void *>(&callback_),
+        symbol);
+      std::free(symbol);
+    }
+#endif
+  }
+
   /// Default destructor.
   virtual ~GenericTimer()
   {
@@ -377,6 +478,27 @@ class WallTimer : public GenericTimer<FunctorT>
       std::make_shared<Clock>(RCL_STEADY_TIME), period, std::move(callback), context, autostart)
   {}
 
+  /// Wall timer constructor allowing for specification of an initial trigger time
+  /**
+   * \param initial_call_time The time at which the callback should be initially triggered.
+   * \param period The interval at which the timer fires
+   * \param callback The callback function to execute every interval
+   * \param context node context
+   * \param autostart timer state on initialization
+   *
+   * \throws std::runtime_error if initial_call_time's clock type is not RCL_STEADY_TIME
+   */
+  WallTimer(
+    Time initial_call_time,
+    std::chrono::nanoseconds period,
+    FunctorT && callback,
+    rclcpp::Context::SharedPtr context,
+    bool autostart = true)
+  : GenericTimer<FunctorT>(
+      std::make_shared<Clock>(RCL_STEADY_TIME), initial_call_time, period,
+      std::move(callback), context, autostart)
+  {}
+
 protected:
   RCLCPP_DISABLE_COPY(WallTimer)
 };