monotonic clock for callback queue timeouts (#1250)

ros · Feb 9, 2018 · faab4cb · faab4cb
1 parent 311312f
commit faab4cb
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 3 deletions.
diff --git a/clients/roscpp/include/ros/callback_queue.h b/clients/roscpp/include/ros/callback_queue.h
@@ -35,14 +35,28 @@
 #ifndef ROSCPP_CALLBACK_QUEUE_H
 #define ROSCPP_CALLBACK_QUEUE_H
 
+// check if we might need to include our own backported version boost::condition_variable
+// in order to use CLOCK_MONOTONIC for the condition variable
+// the include order here is important!
+#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/callback_queue_interface.h"
 #include "ros/time.h"
 #include "common.h"
 
 #include <boost/shared_ptr.hpp>
 #include <boost/thread/mutex.hpp>
 #include <boost/thread/shared_mutex.hpp>
-#include <boost/thread/condition_variable.hpp>
 #include <boost/thread/tss.hpp>
 
 #include <list>

diff --git a/clients/roscpp/src/libros/callback_queue.cpp b/clients/roscpp/src/libros/callback_queue.cpp
@@ -32,10 +32,23 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
+// Make sure we use CLOCK_MONOTONIC for the condition variable wait_for if not Apple.
+#ifndef __APPLE__
+#define BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
+#endif
+
 #include "ros/callback_queue.h"
 #include "ros/assert.h"
 #include <boost/scope_exit.hpp>
 
+// check if we have really included the backported boost condition variable
+// just in case someone messes with the include order...
+#if BOOST_VERSION < 106100
+#ifndef USING_BACKPORTED_BOOST_CONDITION_VARIABLE
+#error "needs boost version >= 1.61 or the backported headers!"
+#endif
+#endif
+
 namespace ros
 {
 
@@ -229,7 +242,7 @@ CallbackQueue::CallOneResult CallbackQueue::callOne(ros::WallDuration timeout)
     {
       if (!timeout.isZero())
       {
-        condition_.timed_wait(lock, boost::posix_time::microseconds(timeout.toSec() * 1000000.0f));
+        condition_.wait_for(lock, boost::chrono::nanoseconds(timeout.toNSec()));
       }
 
       if (callbacks_.empty())
@@ -305,7 +318,7 @@ void CallbackQueue::callAvailable(ros::WallDuration timeout)
     {
       if (!timeout.isZero())
       {
-        condition_.timed_wait(lock, boost::posix_time::microseconds(timeout.toSec() * 1000000.0f));
+        condition_.wait_for(lock, boost::chrono::nanoseconds(timeout.toNSec()));
       }
 
       if (callbacks_.empty() || !enabled_)