Add support for timers on reset callback

Signed-off-by: Mauro Passerino <[email protected]>

Author: Mauro Passerino

irobot_events_executor/include/rclcpp/timers_manager.hpp

@@ -430,6 +430,13 @@ class TimersManager
       std::make_heap(owned_heap_.begin(), owned_heap_.end(), timer_greater);
     }
 
+    void clear_callbacks()
+    {
+      for (TimerPtr & t : owned_heap_) {
+        t->clear_on_reset_callback();
+      }
+    }
+
     /**
      * @brief Friend declaration to allow the `validate_and_lock()` function to access the
      * underlying heap container

irobot_events_executor/src/rclcpp/timers_manager.cpp

@@ -41,6 +41,14 @@ void TimersManager::add_timer(rclcpp::TimerBase::SharedPtr timer)
     timers_updated_ = timers_updated_ || added;
   }
 
+  timer->set_on_reset_callback([this](){
+    {
+      std::unique_lock<std::mutex> lock(timers_mutex_);
+      timers_updated_ = true;
+    }
+    timers_cv_.notify_one();
+  });
+
   if (added) {
     // Notify that a timer has been added
     timers_cv_.notify_one();
@@ -265,6 +273,10 @@ void TimersManager::clear()
   {
     // Lock mutex and then clear all data structures
     rclcpp::Lock lock(timers_mutex_);
+
+    TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
+    locked_heap.clear_callbacks();
+
     weak_timers_heap_.clear();
 
     timers_updated_ = true;
@@ -288,4 +300,5 @@ void TimersManager::remove_timer(TimerPtr timer)
     // Notify timers thread such that it can re-compute its timeout
     timers_cv_.notify_one();
   }
+  timer->clear_on_reset_callback();
 }