add time when timer expired to timer events (#1130)

clients/roscpp/include/ros/forwards.h

@@ -132,9 +132,11 @@ struct TimerEvent
 {
   Time last_expected;                     ///< In a perfect world, this is when the last callback should have happened
   Time last_real;                         ///< When the last callback actually happened
+  Time last_expired;                      ///< When the last timer actually expired and the callback was added to the queue
 
   Time current_expected;                  ///< In a perfect world, this is when the current callback should be happening
   Time current_real;                      ///< This is when the current callback was actually called (Time::now() as of the beginning of the callback)
+  Time current_expired;                   ///< When the current timer actually expired and the callback was added to the queue
 
   struct
   {
@@ -150,9 +152,11 @@ struct WallTimerEvent
 {
   WallTime last_expected;                 ///< In a perfect world, this is when the last callback should have happened
   WallTime last_real;                     ///< When the last callback actually happened
+  WallTime last_expired;                  ///< When the last timer actually expired and the callback was added to the queue
 
   WallTime current_expected;              ///< In a perfect world, this is when the current callback should be happening
   WallTime current_real;                  ///< This is when the current callback was actually called (Time::now() as of the beginning of the callback)
+  WallTime current_expired;               ///< When the current timer actually expired and the callback was added to the queue
 
   struct
   {
@@ -168,9 +172,11 @@ struct SteadyTimerEvent
 {
   SteadyTime last_expected;            ///< In a perfect world, this is when the last callback should have happened
   SteadyTime last_real;                ///< When the last callback actually happened
+  SteadyTime last_expired;             ///< When the last timer actually expired and the callback was added to the queue
 
   SteadyTime current_expected;         ///< In a perfect world, this is when the current callback should be happening
   SteadyTime current_real;             ///< This is when the current callback was actually called (SteadyTime::now() as of the beginning of the callback)
+  SteadyTime current_expired;          ///< When the current timer actually expired and the callback was added to the queue
 
   struct
   {

clients/roscpp/include/ros/timer_manager.h

@@ -78,6 +78,7 @@ class TimerManager
     T next_expected;
 
     T last_real;
+    T last_expired;
 
     bool removed;
 
@@ -143,12 +144,14 @@ class TimerManager
   class TimerQueueCallback : public CallbackInterface
   {
   public:
-    TimerQueueCallback(TimerManager<T, D, E>* parent, const TimerInfoPtr& info, T last_expected, T last_real, T current_expected)
+    TimerQueueCallback(TimerManager<T, D, E>* parent, const TimerInfoPtr& info, T last_expected, T last_real, T current_expected, T last_expired, T current_expired)
     : parent_(parent)
     , info_(info)
     , last_expected_(last_expected)
     , last_real_(last_real)
     , current_expected_(current_expected)
+    , last_expired_(last_expired)
+    , current_expired_(current_expired)
     , called_(false)
     {
       boost::mutex::scoped_lock lock(info->waiting_mutex);
@@ -190,8 +193,10 @@ class TimerManager
         E event;
         event.last_expected = last_expected_;
         event.last_real = last_real_;
+        event.last_expired = last_expired_;
         event.current_expected = current_expected_;
         event.current_real = T::now();
+        event.current_expired = current_expired_;
         event.profile.last_duration = info->last_cb_duration;
 
         SteadyTime cb_start = SteadyTime::now();
@@ -200,6 +205,7 @@ class TimerManager
         info->last_cb_duration = cb_end - cb_start;
 
         info->last_real = event.current_real;
+        info->last_expired = event.current_expired;
 
         parent_->schedule(info);
       }
@@ -213,6 +219,8 @@ class TimerManager
     T last_expected_;
     T last_real_;
     T current_expected_;
+    T last_expired_;
+    T current_expired_;
 
     bool called_;
   };
@@ -529,7 +537,7 @@ void TimerManager<T, D, E>::threadFunc()
           current = T::now();
 
           //ROS_DEBUG("Scheduling timer callback for timer [%d] of period [%f], [%f] off expected", info->handle, info->period.toSec(), (current - info->next_expected).toSec());
-          CallbackInterfacePtr cb(boost::make_shared<TimerQueueCallback>(this, info, info->last_expected, info->last_real, info->next_expected));
+          CallbackInterfacePtr cb(boost::make_shared<TimerQueueCallback>(this, info, info->last_expected, info->last_real, info->next_expected, info->last_expired, current));
           info->callback_queue->addCallback(cb, (uint64_t)info.get());
 
           waiting_.pop_front();

clients/roscpp/src/libros/steady_timer.cpp

@@ -73,7 +73,7 @@ void TimerManager<SteadyTime, WallDuration, SteadyTimerEvent>::threadFunc()
           current = SteadyTime::now();
 
           //ROS_DEBUG("Scheduling timer callback for timer [%d] of period [%f], [%f] off expected", info->handle, info->period.toSec(), (current - info->next_expected).toSec());
-          CallbackInterfacePtr cb(boost::make_shared<TimerQueueCallback>(this, info, info->last_expected, info->last_real, info->next_expected));
+          CallbackInterfacePtr cb(boost::make_shared<TimerQueueCallback>(this, info, info->last_expected, info->last_real, info->next_expected, info->last_expired, current));
           info->callback_queue->addCallback(cb, (uint64_t)info.get());
 
           waiting_.pop_front();

test/test_roscpp/test/src/timer_callbacks.cpp

@@ -74,16 +74,17 @@ class SteadyTimerHelper
     void callback(const SteadyTimerEvent& e)
     {
       bool first = last_call_.isZero();
-      last_call_ = e.current_real;
+      last_call_ = e.current_expired;
 
       if (!first)
       {
-        double time_error = e.current_real.toSec() - e.current_expected.toSec();
+        double time_error = e.current_expired.toSec() - e.current_expected.toSec();
         // Strict check if called early, loose check if called late.
         // Yes, this is very loose, but must pass in high-load, containerized/virtualized, contentious environments.
         if (time_error > 5.0 || time_error < -0.01)
         {
-          ROS_ERROR("Call came at wrong time (expected: %f, actual %f)", e.current_expected.toSec(), e.current_real.toSec());
+          ROS_ERROR("Call came at wrong time (expected: %f, expired: %f, callback: %f)",
+                    e.current_expected.toSec(), e.current_expired.toSec(), e.current_real.toSec());
           failed_ = true;
         }
       }
@@ -367,16 +368,17 @@ class WallTimerHelper
   void callback(const WallTimerEvent& e)
   {
     bool first = last_call_.isZero();
-    last_call_ = e.current_real;
+    last_call_ = e.current_expired;
 
     if (!first)
     {
-      double time_error = e.current_real.toSec() - e.current_expected.toSec();
+      double time_error = e.current_expired.toSec() - e.current_expected.toSec();
       // Strict check if called early, loose check if called late.
       // Yes, this is very loose, but must pass in high-load, containerized/virtualized, contentious environments.
       if (time_error > 5.0 || time_error < -0.01)
       {
-        ROS_ERROR("Call came at wrong time (expected: %f, actual %f)", e.current_expected.toSec(), e.current_real.toSec());
+        ROS_ERROR("Call came at wrong time (expected: %f, expired: %f, callback: %f)",
+                  e.current_expected.toSec(), e.current_expired.toSec(), e.current_real.toSec());
         failed_ = true;
       }
     }