From d9521dbd0582d1d7efa3eafa52cbbcd19d2d1f3a Mon Sep 17 00:00:00 2001 From: Mike Purvis Date: Wed, 15 Nov 2017 11:20:40 -0500 Subject: [PATCH] add time when timer expired to timer events (#1130) --- clients/roscpp/include/ros/forwards.h | 6 ++++++ clients/roscpp/include/ros/timer_manager.h | 12 ++++++++++-- clients/roscpp/src/libros/steady_timer.cpp | 2 +- test/test_roscpp/test/src/timer_callbacks.cpp | 14 ++++++++------ 4 files changed, 25 insertions(+), 9 deletions(-) diff --git a/clients/roscpp/include/ros/forwards.h b/clients/roscpp/include/ros/forwards.h index 8ac7406b1b..d08937c843 100644 --- a/clients/roscpp/include/ros/forwards.h +++ b/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 { diff --git a/clients/roscpp/include/ros/timer_manager.h b/clients/roscpp/include/ros/timer_manager.h index 27f8ca1f64..27cc346386 100644 --- a/clients/roscpp/include/ros/timer_manager.h +++ b/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* parent, const TimerInfoPtr& info, T last_expected, T last_real, T current_expected) + TimerQueueCallback(TimerManager* 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::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(this, info, info->last_expected, info->last_real, info->next_expected)); + CallbackInterfacePtr cb(boost::make_shared(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(); diff --git a/clients/roscpp/src/libros/steady_timer.cpp b/clients/roscpp/src/libros/steady_timer.cpp index ff3bac56ab..69af3ac9b5 100644 --- a/clients/roscpp/src/libros/steady_timer.cpp +++ b/clients/roscpp/src/libros/steady_timer.cpp @@ -73,7 +73,7 @@ void TimerManager::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(this, info, info->last_expected, info->last_real, info->next_expected)); + CallbackInterfacePtr cb(boost::make_shared(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(); diff --git a/test/test_roscpp/test/src/timer_callbacks.cpp b/test/test_roscpp/test/src/timer_callbacks.cpp index 8ce262a48e..36014347de 100644 --- a/test/test_roscpp/test/src/timer_callbacks.cpp +++ b/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; } }