From ef785ccf5caedb9da6ab84267913f37fadd171a0 Mon Sep 17 00:00:00 2001
From: Johannes Meyer <johannes@intermodalics.eu>
Date: Fri, 14 Dec 2018 14:47:35 +0100
Subject: [PATCH] roscpp: remove specialized implementation of
 TimerManager<T,D,E>::threadFunc() in steady_timer.cpp

The updated generic definition in timer_manager.h should do the same with a minor update.
In all cases we can call boost::condition_variable::wait_until() with an absolute time_point of the respective clock.
The conversion from system_clock to steady_clock for Time and WallTime is done internally in
boost::condition_variable::wait_until(lock_type& lock, const chrono::time_point<Clock, Duration>& t).
---
 clients/roscpp/include/ros/timer_manager.h | 20 ++++++-
 clients/roscpp/src/libros/steady_timer.cpp | 68 ----------------------
 2 files changed, 18 insertions(+), 70 deletions(-)

diff --git a/clients/roscpp/include/ros/timer_manager.h b/clients/roscpp/include/ros/timer_manager.h
index 1cf1346d33..1fa960ef7b 100644
--- a/clients/roscpp/include/ros/timer_manager.h
+++ b/clients/roscpp/include/ros/timer_manager.h
@@ -50,6 +50,22 @@
 namespace ros
 {
 
+namespace {
+  template<class T>
+  class TimerManagerTraits
+  {
+  public:
+    typedef boost::chrono::system_clock::time_point time_point;
+  };
+
+  template<>
+  class TimerManagerTraits<SteadyTime>
+  {
+  public:
+    typedef boost::chrono::steady_clock::time_point time_point;
+  };
+}
+
 template<class T, class D, class E>
 class TimerManager
 {
@@ -572,8 +588,8 @@ void TimerManager<T, D, E>::threadFunc()
       {
         // On system time we can simply sleep for the rest of the wait time, since anything else requiring processing will
         // signal the condition variable
-        int64_t remaining_time = std::max<int64_t>((sleep_end - current).toSec() * 1000.0f, 1);
-        timers_cond_.wait_for(lock, boost::chrono::milliseconds(remaining_time));
+        typename TimerManagerTraits<T>::time_point end_tp(boost::chrono::nanoseconds(sleep_end.toNSec()));
+        timers_cond_.wait_until(lock, end_tp);
       }
     }
 
diff --git a/clients/roscpp/src/libros/steady_timer.cpp b/clients/roscpp/src/libros/steady_timer.cpp
index aefc1d1e83..bb252899f9 100644
--- a/clients/roscpp/src/libros/steady_timer.cpp
+++ b/clients/roscpp/src/libros/steady_timer.cpp
@@ -31,74 +31,6 @@
 namespace ros
 {
 
-// specialization for SteadyTimer, to make sure we use a version with wait_until that uses the monotonic clock
-template<>
-void TimerManager<SteadyTime, WallDuration, SteadyTimerEvent>::threadFunc()
-{
-  SteadyTime current;
-  while (!quit_)
-  {
-    SteadyTime sleep_end;
-
-    boost::mutex::scoped_lock lock(timers_mutex_);
-
-    current = SteadyTime::now();
-
-    {
-      boost::mutex::scoped_lock waitlock(waiting_mutex_);
-
-      if (waiting_.empty())
-      {
-        sleep_end = current + WallDuration(0.1);
-      }
-      else
-      {
-        TimerInfoPtr info = findTimer(waiting_.front());
-
-        while (!waiting_.empty() && info && info->next_expected <= current)
-        {
-          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));
-          info->callback_queue->addCallback(cb, (uint64_t)info.get());
-
-          waiting_.pop_front();
-
-          if (waiting_.empty())
-          {
-            break;
-          }
-
-          info = findTimer(waiting_.front());
-        }
-
-        if (info)
-        {
-          sleep_end = info->next_expected;
-        }
-      }
-    }
-
-    while (!new_timer_ && SteadyTime::now() < sleep_end && !quit_)
-    {
-      current = SteadyTime::now();
-
-      if (current >= sleep_end)
-      {
-        break;
-      }
-
-      // requires boost 1.61 for wait_until to actually use the steady clock
-      // see: https://svn.boost.org/trac/boost/ticket/6377
-      boost::chrono::steady_clock::time_point end_tp(boost::chrono::nanoseconds(sleep_end.toNSec()));
-      timers_cond_.wait_until(lock, end_tp);
-    }
-
-    new_timer_ = false;
-  }
-}
-
 SteadyTimer::Impl::Impl()
   : started_(false)
   , timer_handle_(-1)