Merge pull request #55 from mauropasse/mauro/fix-canceled-timer-2

time_until_trigger return MAX if canceled

Author: iRobot ROS

rclcpp/src/rclcpp/executors/timers_manager.cpp

@@ -154,25 +154,30 @@ std::chrono::nanoseconds TimersManager::get_head_timeout_unsafe()
   if (weak_timers_heap_.empty()) {
     return MAX_TIME;
   }
-
-  // Weak heap is not empty, so try to lock the first element
-  TimerPtr head_timer = weak_timers_heap_.front().lock();
+  // Weak heap is not empty, so try to lock the first element.
   // If it is still a valid pointer, it is guaranteed to be the correct head
-  if (head_timer != nullptr) {
-    return head_timer->time_until_trigger();
-  }
+  TimerPtr head_timer = weak_timers_heap_.front().lock();
 
-  // If the first elements has expired, we can't make other assumptions on the heap
-  // and we need to entirely validate it.
-  TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
+  if (head_timer == nullptr) {
+    // The first element has expired, we can't make other assumptions on the heap
+    // and we need to entirely validate it.
+    TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
+    // NOTE: the following operations will not modify any element in the heap, so we
+    // don't have to call `weak_timers_heap_.store(locked_heap)` at the end.
+
+    if (locked_heap.empty()) {
+      return MAX_TIME;
+    }
+    head_timer = locked_heap.front();
+  }
 
-  // NOTE: the following operations will not modify any element in the heap, so we
-  // don't have to call `weak_timers_heap_.store(locked_heap)` at the end.
+  auto time_until_trigger = head_timer->time_until_trigger();
 
-  if (locked_heap.empty()) {
+  // A canceled timer will return a nanoseconds::max duration
+  if (time_until_trigger == std::chrono::nanoseconds::max()) {
     return MAX_TIME;
   }
-  return locked_heap.front()->time_until_trigger();
+  return time_until_trigger;
 }
 
 void TimersManager::execute_ready_timers_unsafe()

rclcpp/src/rclcpp/timer.cpp

@@ -116,8 +116,16 @@ 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");