use SteadyTime for some timeouts

clients/roscpp/include/ros/internal_timer_manager.h

@@ -36,7 +36,7 @@ namespace ros
 {
 
 template<typename T, typename D, typename E> class TimerManager;
-typedef TimerManager<WallTime, WallDuration, WallTimerEvent> InternalTimerManager;
+typedef TimerManager<SteadyTime, WallDuration, SteadyTimerEvent> InternalTimerManager;
 typedef boost::shared_ptr<InternalTimerManager> InternalTimerManagerPtr;
 
 ROSCPP_DECL void initInternalTimerManager();

clients/roscpp/include/ros/timer_manager.h

@@ -28,14 +28,27 @@
 #ifndef ROSCPP_TIMER_MANAGER_H
 #define ROSCPP_TIMER_MANAGER_H
 
+// check if we might need to include our own backported version boost::condition_variable
+// in order to use CLOCK_MONOTONIC for the SteadyTimer
+#ifdef BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
+#include <boost/version.hpp>
+#if BOOST_VERSION < 106100
+// use backported version of boost condition variable, see https://svn.boost.org/trac/boost/ticket/6377
+#include "boost_161_condition_variable.h"
+#else // Boost version is 1.61 or greater and has the steady clock fixes
+#include <boost/thread/condition_variable.hpp>
+#endif
+#else // !BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
+#include <boost/thread/condition_variable.hpp>
+#endif // BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
+
 #include "ros/forwards.h"
 #include "ros/time.h"
 #include "ros/file_log.h"
 
 #include <boost/thread/thread.hpp>
 #include <boost/thread/mutex.hpp>
 #include <boost/thread/recursive_mutex.hpp>
-#include <boost/thread/condition_variable.hpp>
 
 #include "ros/assert.h"
 #include "ros/callback_queue_interface.h"
@@ -180,9 +193,9 @@ class TimerManager
         event.current_real = T::now();
         event.profile.last_duration = info->last_cb_duration;
 
-        WallTime cb_start = WallTime::now();
+        SteadyTime cb_start = SteadyTime::now();
         info->callback(event);
-        WallTime cb_end = WallTime::now();
+        SteadyTime cb_end = SteadyTime::now();
         info->last_cb_duration = cb_end - cb_start;
 
         info->last_real = event.current_real;
@@ -560,10 +573,21 @@ void TimerManager<T, D, E>::threadFunc()
       }
       else
       {
-        // On system time we can simply sleep for the rest of the wait time, since anything else requiring processing will
-        // signal the condition variable
-        int32_t remaining_time = std::max((int32_t)((sleep_end - current).toSec() * 1000.0f), 1);
-        timers_cond_.timed_wait(lock, boost::posix_time::milliseconds(remaining_time));
+        // we have to distinguish between SteadyTime and WallTime here,
+        // because boost timed_wait with duration just adds the duration to current system time
+        // this however requires boost 1.61, see: https://svn.boost.org/trac/boost/ticket/6377
+        if (typeid(T) == typeid(SteadyTime))
+        {
+          boost::chrono::steady_clock::time_point end_tp(boost::chrono::nanoseconds(sleep_end.toNSec()));
+          timers_cond_.wait_until(lock, end_tp);
+        }
+        else
+        {
+          // On system time we can simply sleep for the rest of the wait time, since anything else requiring processing will
+          // signal the condition variable
+          int32_t remaining_time = std::max((int32_t) ((sleep_end - current).toSec() * 1000.0f), 1);
+          timers_cond_.timed_wait(lock, boost::posix_time::milliseconds(remaining_time));
+        }
       }
     }
 

clients/roscpp/include/ros/transport_publisher_link.h

@@ -42,7 +42,7 @@ typedef boost::weak_ptr<Subscription> SubscriptionWPtr;
 class Connection;
 typedef boost::shared_ptr<Connection> ConnectionPtr;
 
-struct WallTimerEvent;
+struct SteadyTimerEvent;
 
 /**
  * \brief Handles a connection to a single publisher on a given topic.  Receives messages from a publisher
@@ -76,14 +76,14 @@ class ROSCPP_DECL TransportPublisherLink : public PublisherLink
   void onMessageLength(const ConnectionPtr& conn, const boost::shared_array<uint8_t>& buffer, uint32_t size, bool success);
   void onMessage(const ConnectionPtr& conn, const boost::shared_array<uint8_t>& buffer, uint32_t size, bool success);
 
-  void onRetryTimer(const ros::WallTimerEvent&);
+  void onRetryTimer(const ros::SteadyTimerEvent&);
 
   ConnectionPtr connection_;
 
   int32_t retry_timer_handle_;
   bool needs_retry_;
   WallDuration retry_period_;
-  WallTime next_retry_;
+  SteadyTime next_retry_;
   bool dropping_;
 };
 typedef boost::shared_ptr<TransportPublisherLink> TransportPublisherLinkPtr;

clients/roscpp/src/libros/internal_timer_manager.cpp

@@ -25,8 +25,11 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include "ros/internal_timer_manager.h"
+// make sure we use CLOCK_MONOTONIC for the condition variable
+#define BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
+
 #include "ros/timer_manager.h"
+#include "ros/internal_timer_manager.h"
 
 namespace ros
 {

clients/roscpp/src/libros/master.cpp

@@ -178,7 +178,7 @@ boost::mutex g_xmlrpc_call_mutex;
 
 bool execute(const std::string& method, const XmlRpc::XmlRpcValue& request, XmlRpc::XmlRpcValue& response, XmlRpc::XmlRpcValue& payload, bool wait_for_master)
 {
-  ros::WallTime start_time = ros::WallTime::now();
+  ros::SteadyTime start_time = ros::SteadyTime::now();
 
   std::string master_host = getHost();
   uint32_t master_port = getPort();
@@ -213,7 +213,7 @@ bool execute(const std::string& method, const XmlRpc::XmlRpcValue& request, XmlR
         return false;
       }
 
-      if (!g_retry_timeout.isZero() && (ros::WallTime::now() - start_time) >= g_retry_timeout)
+      if (!g_retry_timeout.isZero() && (ros::SteadyTime::now() - start_time) >= g_retry_timeout)
       {
         ROS_ERROR("[%s] Timed out trying to connect to the master after [%f] seconds", method.c_str(), g_retry_timeout.toSec());
         XMLRPCManager::instance()->releaseXMLRPCClient(c);

clients/roscpp/src/libros/transport_publisher_link.cpp

@@ -198,14 +198,14 @@ void TransportPublisherLink::onMessage(const ConnectionPtr& conn, const boost::s
   }
 }
 
-void TransportPublisherLink::onRetryTimer(const ros::WallTimerEvent&)
+void TransportPublisherLink::onRetryTimer(const ros::SteadyTimerEvent&)
 {
   if (dropping_)
   {
     return;
   }
 
-  if (needs_retry_ && WallTime::now() > next_retry_)
+  if (needs_retry_ && SteadyTime::now() > next_retry_)
   {
     retry_period_ = std::min(retry_period_ * 2, WallDuration(20));
     needs_retry_ = false;
@@ -268,12 +268,12 @@ void TransportPublisherLink::onConnectionDropped(const ConnectionPtr& conn, Conn
 
     ROS_ASSERT(!needs_retry_);
     needs_retry_ = true;
-    next_retry_ = WallTime::now() + retry_period_;
+    next_retry_ = SteadyTime::now() + retry_period_;
 
     if (retry_timer_handle_ == -1)
     {
       retry_period_ = WallDuration(0.1);
-      next_retry_ = WallTime::now() + retry_period_;
+      next_retry_ = SteadyTime::now() + retry_period_;
       // shared_from_this() shared_ptr is used to ensure TransportPublisherLink is not
       // destroyed in the middle of onRetryTimer execution
       retry_timer_handle_ = getInternalTimerManager()->add(WallDuration(retry_period_),