Fix timers

rclcpp/include/rclcpp/timer.hpp

@@ -120,6 +120,15 @@ class TimerBase
   std::chrono::nanoseconds
   time_until_trigger();
 
+  /// Returns a time object indicating when the timer has its next scheduled callback.
+  /**
+   * \return A rclcpp::Time representing when the next callback should be executed.
+   * \throws std::runtime_error if the rcl_timer_get_next_call_time returns a failure
+   */
+  RCLCPP_PUBLIC
+  rclcpp::Time
+  next_call_time();
+
   /// Is the clock steady (i.e. is the time between ticks constant?)
   /** \return True if the clock used by this timer is steady. */
   virtual bool is_steady() = 0;

rclcpp/include/rclcpp/timers_manager.hpp

@@ -460,8 +460,7 @@ class TimersManager
      */
     static bool timer_greater(TimerPtr a, TimerPtr b)
     {
-      // FIXME!
-      return a->time_until_trigger() > b->time_until_trigger();
+      return a->next_call_time() >= b->next_call_time();
     }
 
     std::vector<TimerPtr> owned_heap_;

rclcpp/src/rclcpp/timer.cpp

@@ -116,15 +116,45 @@ TimerBase::is_ready()
 std::chrono::nanoseconds
 TimerBase::time_until_trigger()
 {
+  bool is_canceled = false;
+  rcl_ret_t ret = rcl_timer_is_canceled(timer_handle_.get(), &is_canceled);
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't get timer cancelled state");
+  }
+  if (is_canceled) {
+    return std::chrono::nanoseconds::max();
+  }
+
   int64_t time_until_next_call = 0;
-  rcl_ret_t ret = rcl_timer_get_time_until_next_call(
+  ret = rcl_timer_get_time_until_next_call(
     timer_handle_.get(), &time_until_next_call);
   if (ret != RCL_RET_OK) {
     rclcpp::exceptions::throw_from_rcl_error(ret, "Timer could not get time until next call");
   }
   return std::chrono::nanoseconds(time_until_next_call);
 }
 
+rclcpp::Time
+TimerBase::next_call_time()
+{
+  bool is_canceled = false;
+  rcl_ret_t ret = rcl_timer_is_canceled(timer_handle_.get(), &is_canceled);
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't get timer cancelled state");
+  }
+  if (is_canceled) {
+    // We don't use rclcpp::Time::max() because we want to specify the clock type
+    return rclcpp::Time(std::numeric_limits<int32_t>::max(), 999999999, clock_->get_clock_type());
+  }
+
+  rcl_time_point_value_t time_point = 0;
+  ret = rcl_timer_get_next_call_time(timer_handle_.get(), &time_point);
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Timer could not get next call time");
+  }
+  return rclcpp::Time(time_point, clock_->get_clock_type());
+}
+
 std::shared_ptr<const rcl_timer_t>
 TimerBase::get_timer_handle()
 {

rclcpp/src/rclcpp/timers_manager.cpp

@@ -165,6 +165,7 @@ void TimersManager::execute_ready_timer(const void * timer_id)
     ready_timer = weak_timers_heap_.get_timer(timer_id);
   }
   if (ready_timer) {
+    ready_timer->call();
     ready_timer->execute_callback();
   }
 }