diff --git a/clients/roscpp/CMakeLists.txt b/clients/roscpp/CMakeLists.txt
index e6c48b928d..ecafba1d79 100644
--- a/clients/roscpp/CMakeLists.txt
+++ b/clients/roscpp/CMakeLists.txt
@@ -22,7 +22,7 @@ list(GET roscpp_VERSION_LIST 2 roscpp_VERSION_PATCH)
 
 configure_file(${CMAKE_CURRENT_SOURCE_DIR}/include/ros/common.h.in ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}/ros/common.h)
 
-find_package(Boost REQUIRED COMPONENTS signals filesystem system)
+find_package(Boost REQUIRED COMPONENTS signals filesystem system chrono)
 
 include_directories(include ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}/ros ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
 
diff --git a/clients/roscpp/include/boost_161_condition_variable.h b/clients/roscpp/include/boost_161_condition_variable.h
new file mode 100644
index 0000000000..ee2ff46188
--- /dev/null
+++ b/clients/roscpp/include/boost_161_condition_variable.h
@@ -0,0 +1,23 @@
+#ifndef BOOST_THREAD_CONDITION_VARIABLE_HPP
+#define BOOST_THREAD_CONDITION_VARIABLE_HPP
+
+//  condition_variable.hpp
+//
+//  (C) Copyright 2007 Anthony Williams
+//
+//  Distributed under the Boost Software License, Version 1.0. (See
+//  accompanying file LICENSE_1_0.txt or copy at
+//  http://www.boost.org/LICENSE_1_0.txt)
+
+#include <boost/thread/detail/platform.hpp>
+#if defined(BOOST_THREAD_PLATFORM_WIN32)
+#include <boost/thread/win32/condition_variable.hpp>
+#elif defined(BOOST_THREAD_PLATFORM_PTHREAD)
+//#include <boost/thread/pthread/condition_variable.hpp>
+#include "boost_161_pthread_condition_variable.h"
+#else
+#error "Boost threads unavailable on this platform"
+#endif
+
+#endif
+
diff --git a/clients/roscpp/include/boost_161_pthread_condition_variable.h b/clients/roscpp/include/boost_161_pthread_condition_variable.h
new file mode 100644
index 0000000000..14de189903
--- /dev/null
+++ b/clients/roscpp/include/boost_161_pthread_condition_variable.h
@@ -0,0 +1,429 @@
+#ifndef BOOST_THREAD_CONDITION_VARIABLE_PTHREAD_HPP
+#define BOOST_THREAD_CONDITION_VARIABLE_PTHREAD_HPP
+// Distributed under the Boost Software License, Version 1.0. (See
+// accompanying file LICENSE_1_0.txt or copy at
+// http://www.boost.org/LICENSE_1_0.txt)
+// (C) Copyright 2007-10 Anthony Williams
+// (C) Copyright 2011-2012 Vicente J. Botet Escriba
+
+#include "boost_161_pthread_condition_variable_fwd.h"
+
+#include <boost/thread/pthread/timespec.hpp>
+#include <boost/thread/pthread/pthread_mutex_scoped_lock.hpp>
+#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
+#include <boost/thread/pthread/thread_data.hpp>
+#endif
+//#include <boost/thread/pthread/condition_variable_fwd.hpp>
+#ifdef BOOST_THREAD_USES_CHRONO
+#include <boost/chrono/system_clocks.hpp>
+#include <boost/chrono/ceil.hpp>
+#endif
+#include <boost/thread/detail/delete.hpp>
+
+#include <boost/config/abi_prefix.hpp>
+
+namespace boost
+{
+#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
+    namespace this_thread
+    {
+        void BOOST_THREAD_DECL interruption_point();
+    }
+#endif
+
+    namespace thread_cv_detail
+    {
+        template<typename MutexType>
+        struct lock_on_exit
+        {
+            MutexType* m;
+
+            lock_on_exit():
+                m(0)
+            {}
+
+            void activate(MutexType& m_)
+            {
+                m_.unlock();
+                m=&m_;
+            }
+            ~lock_on_exit()
+            {
+                if(m)
+                {
+                    m->lock();
+                }
+           }
+        };
+    }
+
+    inline void condition_variable::wait(unique_lock<mutex>& m)
+    {
+#if defined BOOST_THREAD_THROW_IF_PRECONDITION_NOT_SATISFIED
+        if(! m.owns_lock())
+        {
+            boost::throw_exception(condition_error(-1, "boost::condition_variable::wait() failed precondition mutex not owned"));
+        }
+#endif
+        int res=0;
+        {
+#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
+            thread_cv_detail::lock_on_exit<unique_lock<mutex> > guard;
+            detail::interruption_checker check_for_interruption(&internal_mutex,&cond);
+            pthread_mutex_t* the_mutex = &internal_mutex;
+            guard.activate(m);
+#else
+            pthread_mutex_t* the_mutex = m.mutex()->native_handle();
+#endif
+            res = pthread_cond_wait(&cond,the_mutex);
+        }
+#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
+        this_thread::interruption_point();
+#endif
+        if(res && res != EINTR)
+        {
+            boost::throw_exception(condition_error(res, "boost::condition_variable::wait failed in pthread_cond_wait"));
+        }
+    }
+
+    inline bool condition_variable::do_wait_until(
+                unique_lock<mutex>& m,
+                struct timespec const &timeout)
+    {
+#if defined BOOST_THREAD_THROW_IF_PRECONDITION_NOT_SATISFIED
+        if (!m.owns_lock())
+        {
+            boost::throw_exception(condition_error(EPERM, "boost::condition_variable::do_wait_until() failed precondition mutex not owned"));
+        }
+#endif
+        int cond_res;
+        {
+#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
+            thread_cv_detail::lock_on_exit<unique_lock<mutex> > guard;
+            detail::interruption_checker check_for_interruption(&internal_mutex,&cond);
+            pthread_mutex_t* the_mutex = &internal_mutex;
+            guard.activate(m);
+#else
+            pthread_mutex_t* the_mutex = m.mutex()->native_handle();
+#endif
+            cond_res=pthread_cond_timedwait(&cond,the_mutex,&timeout);
+        }
+#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
+        this_thread::interruption_point();
+#endif
+        if(cond_res==ETIMEDOUT)
+        {
+            return false;
+        }
+        if(cond_res)
+        {
+            boost::throw_exception(condition_error(cond_res, "boost::condition_variable::do_wait_until failed in pthread_cond_timedwait"));
+        }
+        return true;
+    }
+
+    inline void condition_variable::notify_one() BOOST_NOEXCEPT
+    {
+#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
+        boost::pthread::pthread_mutex_scoped_lock internal_lock(&internal_mutex);
+#endif
+        BOOST_VERIFY(!pthread_cond_signal(&cond));
+    }
+
+    inline void condition_variable::notify_all() BOOST_NOEXCEPT
+    {
+#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
+        boost::pthread::pthread_mutex_scoped_lock internal_lock(&internal_mutex);
+#endif
+        BOOST_VERIFY(!pthread_cond_broadcast(&cond));
+    }
+
+    class condition_variable_any
+    {
+        pthread_mutex_t internal_mutex;
+        pthread_cond_t cond;
+
+    public:
+        BOOST_THREAD_NO_COPYABLE(condition_variable_any)
+        condition_variable_any()
+        {
+            int const res=pthread_mutex_init(&internal_mutex,NULL);
+            if(res)
+            {
+                boost::throw_exception(thread_resource_error(res, "boost::condition_variable_any::condition_variable_any() failed in pthread_mutex_init"));
+            }
+            int const res2 = detail::monotonic_pthread_cond_init(cond);
+            if(res2)
+            {
+                BOOST_VERIFY(!pthread_mutex_destroy(&internal_mutex));
+                boost::throw_exception(thread_resource_error(res2, "boost::condition_variable_any::condition_variable_any() failed in detail::monotonic_pthread_cond_init"));
+            }
+        }
+        ~condition_variable_any()
+        {
+            BOOST_VERIFY(!pthread_mutex_destroy(&internal_mutex));
+            BOOST_VERIFY(!pthread_cond_destroy(&cond));
+        }
+
+        template<typename lock_type>
+        void wait(lock_type& m)
+        {
+            int res=0;
+            {
+                thread_cv_detail::lock_on_exit<lock_type> guard;
+#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
+                detail::interruption_checker check_for_interruption(&internal_mutex,&cond);
+#else
+                boost::pthread::pthread_mutex_scoped_lock check_for_interruption(&internal_mutex);
+#endif
+                guard.activate(m);
+                res=pthread_cond_wait(&cond,&internal_mutex);
+            }
+#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
+            this_thread::interruption_point();
+#endif
+            if(res)
+            {
+                boost::throw_exception(condition_error(res, "boost::condition_variable_any::wait() failed in pthread_cond_wait"));
+            }
+        }
+
+        template<typename lock_type,typename predicate_type>
+        void wait(lock_type& m,predicate_type pred)
+        {
+            while(!pred()) wait(m);
+        }
+
+#if defined BOOST_THREAD_USES_DATETIME
+        template<typename lock_type>
+        bool timed_wait(lock_type& m,boost::system_time const& abs_time)
+        {
+            struct timespec const timeout=detail::to_timespec(abs_time);
+            return do_wait_until(m, timeout);
+        }
+        template<typename lock_type>
+        bool timed_wait(lock_type& m,xtime const& abs_time)
+        {
+            return timed_wait(m,system_time(abs_time));
+        }
+
+        template<typename lock_type,typename duration_type>
+        bool timed_wait(lock_type& m,duration_type const& wait_duration)
+        {
+            return timed_wait(m,get_system_time()+wait_duration);
+        }
+
+        template<typename lock_type,typename predicate_type>
+        bool timed_wait(lock_type& m,boost::system_time const& abs_time, predicate_type pred)
+        {
+            while (!pred())
+            {
+                if(!timed_wait(m, abs_time))
+                    return pred();
+            }
+            return true;
+        }
+
+        template<typename lock_type,typename predicate_type>
+        bool timed_wait(lock_type& m,xtime const& abs_time, predicate_type pred)
+        {
+            return timed_wait(m,system_time(abs_time),pred);
+        }
+
+        template<typename lock_type,typename duration_type,typename predicate_type>
+        bool timed_wait(lock_type& m,duration_type const& wait_duration,predicate_type pred)
+        {
+            return timed_wait(m,get_system_time()+wait_duration,pred);
+        }
+#endif
+#ifndef BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
+
+#ifdef BOOST_THREAD_USES_CHRONO
+        template <class lock_type,class Duration>
+        cv_status
+        wait_until(
+                lock_type& lock,
+                const chrono::time_point<chrono::system_clock, Duration>& t)
+        {
+          using namespace chrono;
+          typedef time_point<system_clock, nanoseconds> nano_sys_tmpt;
+          wait_until(lock,
+                        nano_sys_tmpt(ceil<nanoseconds>(t.time_since_epoch())));
+          return system_clock::now() < t ? cv_status::no_timeout :
+                                             cv_status::timeout;
+        }
+
+        template <class lock_type, class Clock, class Duration>
+        cv_status
+        wait_until(
+                lock_type& lock,
+                const chrono::time_point<Clock, Duration>& t)
+        {
+          using namespace chrono;
+          system_clock::time_point     s_now = system_clock::now();
+          typename Clock::time_point  c_now = Clock::now();
+          wait_until(lock, s_now + ceil<nanoseconds>(t - c_now));
+          return Clock::now() < t ? cv_status::no_timeout : cv_status::timeout;
+        }
+
+        template <class lock_type, class Rep, class Period>
+        cv_status
+        wait_for(
+                lock_type& lock,
+                const chrono::duration<Rep, Period>& d)
+        {
+          using namespace chrono;
+          system_clock::time_point s_now = system_clock::now();
+          steady_clock::time_point c_now = steady_clock::now();
+          wait_until(lock, s_now + ceil<nanoseconds>(d));
+          return steady_clock::now() - c_now < d ? cv_status::no_timeout :
+                                                   cv_status::timeout;
+
+        }
+
+        template <class lock_type>
+        cv_status wait_until(
+            lock_type& lk,
+            chrono::time_point<chrono::system_clock, chrono::nanoseconds> tp)
+        {
+            using namespace chrono;
+            nanoseconds d = tp.time_since_epoch();
+            timespec ts = boost::detail::to_timespec(d);
+            if (do_wait_until(lk, ts)) return cv_status::no_timeout;
+            else return cv_status::timeout;
+        }
+#endif
+#else // defined BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
+#ifdef BOOST_THREAD_USES_CHRONO
+
+        template <class lock_type, class Duration>
+        cv_status
+        wait_until(
+            lock_type& lock,
+            const chrono::time_point<chrono::steady_clock, Duration>& t)
+        {
+            using namespace chrono;
+            typedef time_point<steady_clock, nanoseconds> nano_sys_tmpt;
+            wait_until(lock,
+                        nano_sys_tmpt(ceil<nanoseconds>(t.time_since_epoch())));
+            return steady_clock::now() < t ? cv_status::no_timeout :
+                                             cv_status::timeout;
+        }
+
+        template <class lock_type, class Clock, class Duration>
+        cv_status
+        wait_until(
+            lock_type& lock,
+            const chrono::time_point<Clock, Duration>& t)
+        {
+            using namespace chrono;
+            steady_clock::time_point     s_now = steady_clock::now();
+            typename Clock::time_point  c_now = Clock::now();
+            wait_until(lock, s_now + ceil<nanoseconds>(t - c_now));
+            return Clock::now() < t ? cv_status::no_timeout : cv_status::timeout;
+        }
+
+        template <class lock_type, class Rep, class Period>
+        cv_status
+        wait_for(
+            lock_type& lock,
+            const chrono::duration<Rep, Period>& d)
+        {
+            using namespace chrono;
+            steady_clock::time_point c_now = steady_clock::now();
+            wait_until(lock, c_now + ceil<nanoseconds>(d));
+            return steady_clock::now() - c_now < d ? cv_status::no_timeout :
+                                                   cv_status::timeout;
+        }
+
+        template <class lock_type>
+        inline cv_status wait_until(
+            lock_type& lock,
+            chrono::time_point<chrono::steady_clock, chrono::nanoseconds> tp)
+        {
+            using namespace chrono;
+            nanoseconds d = tp.time_since_epoch();
+            timespec ts = boost::detail::to_timespec(d);
+            if (do_wait_until(lock, ts)) return cv_status::no_timeout;
+            else return cv_status::timeout;
+        }
+
+#endif
+#endif // defined BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
+
+#ifdef BOOST_THREAD_USES_CHRONO
+        template <class lock_type, class Clock, class Duration, class Predicate>
+        bool
+        wait_until(
+                lock_type& lock,
+                const chrono::time_point<Clock, Duration>& t,
+                Predicate pred)
+        {
+            while (!pred())
+            {
+                if (wait_until(lock, t) == cv_status::timeout)
+                    return pred();
+            }
+            return true;
+        }
+
+        template <class lock_type, class Rep, class Period, class Predicate>
+        bool
+        wait_for(
+                lock_type& lock,
+                const chrono::duration<Rep, Period>& d,
+                Predicate pred)
+        {
+          return wait_until(lock, chrono::steady_clock::now() + d, boost::move(pred));
+        }
+#endif
+
+        void notify_one() BOOST_NOEXCEPT
+        {
+            boost::pthread::pthread_mutex_scoped_lock internal_lock(&internal_mutex);
+            BOOST_VERIFY(!pthread_cond_signal(&cond));
+        }
+
+        void notify_all() BOOST_NOEXCEPT
+        {
+            boost::pthread::pthread_mutex_scoped_lock internal_lock(&internal_mutex);
+            BOOST_VERIFY(!pthread_cond_broadcast(&cond));
+        }
+    private: // used by boost::thread::try_join_until
+
+        template <class lock_type>
+        bool do_wait_until(
+          lock_type& m,
+          struct timespec const &timeout)
+        {
+          int res=0;
+          {
+              thread_cv_detail::lock_on_exit<lock_type> guard;
+#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
+              detail::interruption_checker check_for_interruption(&internal_mutex,&cond);
+#else
+              boost::pthread::pthread_mutex_scoped_lock check_for_interruption(&internal_mutex);
+#endif
+              guard.activate(m);
+              res=pthread_cond_timedwait(&cond,&internal_mutex,&timeout);
+          }
+#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
+          this_thread::interruption_point();
+#endif
+          if(res==ETIMEDOUT)
+          {
+              return false;
+          }
+          if(res)
+          {
+              boost::throw_exception(condition_error(res, "boost::condition_variable_any::do_wait_until() failed in pthread_cond_timedwait"));
+          }
+          return true;
+        }
+    };
+
+}
+
+#include <boost/config/abi_suffix.hpp>
+
+#endif
diff --git a/clients/roscpp/include/boost_161_pthread_condition_variable_fwd.h b/clients/roscpp/include/boost_161_pthread_condition_variable_fwd.h
new file mode 100644
index 0000000000..802a5cc674
--- /dev/null
+++ b/clients/roscpp/include/boost_161_pthread_condition_variable_fwd.h
@@ -0,0 +1,375 @@
+#ifndef BOOST_THREAD_PTHREAD_CONDITION_VARIABLE_FWD_HPP
+#define BOOST_THREAD_PTHREAD_CONDITION_VARIABLE_FWD_HPP
+// Distributed under the Boost Software License, Version 1.0. (See
+// accompanying file LICENSE_1_0.txt or copy at
+// http://www.boost.org/LICENSE_1_0.txt)
+// (C) Copyright 2007-8 Anthony Williams
+// (C) Copyright 2011-2012 Vicente J. Botet Escriba
+
+#include <boost/assert.hpp>
+#include <boost/throw_exception.hpp>
+#include <pthread.h>
+#include <boost/thread/cv_status.hpp>
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/lock_types.hpp>
+#include <boost/thread/thread_time.hpp>
+#include <boost/thread/pthread/timespec.hpp>
+#if defined BOOST_THREAD_USES_DATETIME
+#include <boost/thread/xtime.hpp>
+#endif
+
+#ifdef BOOST_THREAD_USES_CHRONO
+#include <boost/chrono/system_clocks.hpp>
+#include <boost/chrono/ceil.hpp>
+#endif
+#include <boost/thread/detail/delete.hpp>
+#include <boost/date_time/posix_time/posix_time_duration.hpp>
+
+#include <boost/config/abi_prefix.hpp>
+
+namespace boost
+{
+  namespace detail {
+    inline int monotonic_pthread_cond_init(pthread_cond_t& cond) {
+
+#ifdef BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
+            pthread_condattr_t attr;
+            int res = pthread_condattr_init(&attr);
+            if (res)
+            {
+              return res;
+            }
+            pthread_condattr_setclock(&attr, CLOCK_MONOTONIC);
+            res=pthread_cond_init(&cond,&attr);
+            pthread_condattr_destroy(&attr);
+            return res;
+#else
+            return pthread_cond_init(&cond,NULL);
+#endif
+
+    }
+  }
+
+    class condition_variable
+    {
+    private:
+#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
+        pthread_mutex_t internal_mutex;
+#endif
+        pthread_cond_t cond;
+
+    public:
+    //private: // used by boost::thread::try_join_until
+
+        inline bool do_wait_until(
+            unique_lock<mutex>& lock,
+            struct timespec const &timeout);
+
+        bool do_wait_for(
+            unique_lock<mutex>& lock,
+            struct timespec const &timeout)
+        {
+#if ! defined BOOST_THREAD_USEFIXES_TIMESPEC
+            return do_wait_until(lock, boost::detail::timespec_plus(timeout, boost::detail::timespec_now()));
+#elif ! defined BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
+            //using namespace chrono;
+            //nanoseconds ns = chrono::system_clock::now().time_since_epoch();
+
+            struct timespec ts = boost::detail::timespec_now_realtime();
+            //ts.tv_sec = static_cast<long>(chrono::duration_cast<chrono::seconds>(ns).count());
+            //ts.tv_nsec = static_cast<long>((ns - chrono::duration_cast<chrono::seconds>(ns)).count());
+            return do_wait_until(lock, boost::detail::timespec_plus(timeout, ts));
+#else
+            // old behavior was fine for monotonic
+            return do_wait_until(lock, boost::detail::timespec_plus(timeout, boost::detail::timespec_now_realtime()));
+#endif
+        }
+
+    public:
+      BOOST_THREAD_NO_COPYABLE(condition_variable)
+        condition_variable()
+        {
+            int res;
+#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
+            res=pthread_mutex_init(&internal_mutex,NULL);
+            if(res)
+            {
+                boost::throw_exception(thread_resource_error(res, "boost::condition_variable::condition_variable() constructor failed in pthread_mutex_init"));
+            }
+#endif
+            res = detail::monotonic_pthread_cond_init(cond);
+            if (res)
+            {
+#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
+                BOOST_VERIFY(!pthread_mutex_destroy(&internal_mutex));
+#endif
+                boost::throw_exception(thread_resource_error(res, "boost::condition_variable::condition_variable() constructor failed in detail::monotonic_pthread_cond_init"));
+            }
+        }
+        ~condition_variable()
+        {
+            int ret;
+#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
+            do {
+              ret = pthread_mutex_destroy(&internal_mutex);
+            } while (ret == EINTR);
+            BOOST_ASSERT(!ret);
+#endif
+            do {
+              ret = pthread_cond_destroy(&cond);
+            } while (ret == EINTR);
+            BOOST_ASSERT(!ret);
+        }
+
+        void wait(unique_lock<mutex>& m);
+
+        template<typename predicate_type>
+        void wait(unique_lock<mutex>& m,predicate_type pred)
+        {
+            while(!pred()) wait(m);
+        }
+
+#if defined BOOST_THREAD_USES_DATETIME
+        inline bool timed_wait(
+            unique_lock<mutex>& m,
+            boost::system_time const& abs_time)
+        {
+#if defined BOOST_THREAD_WAIT_BUG
+            struct timespec const timeout=detail::to_timespec(abs_time + BOOST_THREAD_WAIT_BUG);
+            return do_wait_until(m, timeout);
+#else
+            struct timespec const timeout=detail::to_timespec(abs_time);
+            return do_wait_until(m, timeout);
+#endif
+        }
+        bool timed_wait(
+            unique_lock<mutex>& m,
+            xtime const& abs_time)
+        {
+            return timed_wait(m,system_time(abs_time));
+        }
+
+        template<typename duration_type>
+        bool timed_wait(
+            unique_lock<mutex>& m,
+            duration_type const& wait_duration)
+        {
+            if (wait_duration.is_pos_infinity())
+            {
+                wait(m); // or do_wait(m,detail::timeout::sentinel());
+                return true;
+            }
+            if (wait_duration.is_special())
+            {
+                return true;
+            }
+            return timed_wait(m,get_system_time()+wait_duration);
+        }
+
+        template<typename predicate_type>
+        bool timed_wait(
+            unique_lock<mutex>& m,
+            boost::system_time const& abs_time,predicate_type pred)
+        {
+            while (!pred())
+            {
+                if(!timed_wait(m, abs_time))
+                    return pred();
+            }
+            return true;
+        }
+
+        template<typename predicate_type>
+        bool timed_wait(
+            unique_lock<mutex>& m,
+            xtime const& abs_time,predicate_type pred)
+        {
+            return timed_wait(m,system_time(abs_time),pred);
+        }
+
+        template<typename duration_type,typename predicate_type>
+        bool timed_wait(
+            unique_lock<mutex>& m,
+            duration_type const& wait_duration,predicate_type pred)
+        {
+            if (wait_duration.is_pos_infinity())
+            {
+                while (!pred())
+                {
+                  wait(m); // or do_wait(m,detail::timeout::sentinel());
+                }
+                return true;
+            }
+            if (wait_duration.is_special())
+            {
+                return pred();
+            }
+            return timed_wait(m,get_system_time()+wait_duration,pred);
+        }
+#endif
+
+#ifndef BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
+
+#ifdef BOOST_THREAD_USES_CHRONO
+
+        template <class Duration>
+        cv_status
+        wait_until(
+                unique_lock<mutex>& lock,
+                const chrono::time_point<chrono::system_clock, Duration>& t)
+        {
+          using namespace chrono;
+          typedef time_point<system_clock, nanoseconds> nano_sys_tmpt;
+          wait_until(lock,
+                        nano_sys_tmpt(ceil<nanoseconds>(t.time_since_epoch())));
+          return system_clock::now() < t ? cv_status::no_timeout :
+                                             cv_status::timeout;
+        }
+
+        template <class Clock, class Duration>
+        cv_status
+        wait_until(
+                unique_lock<mutex>& lock,
+                const chrono::time_point<Clock, Duration>& t)
+        {
+          using namespace chrono;
+          system_clock::time_point     s_now = system_clock::now();
+          typename Clock::time_point  c_now = Clock::now();
+          wait_until(lock, s_now + ceil<nanoseconds>(t - c_now));
+          return Clock::now() < t ? cv_status::no_timeout : cv_status::timeout;
+        }
+
+
+
+        template <class Rep, class Period>
+        cv_status
+        wait_for(
+                unique_lock<mutex>& lock,
+                const chrono::duration<Rep, Period>& d)
+        {
+          using namespace chrono;
+          system_clock::time_point s_now = system_clock::now();
+          steady_clock::time_point c_now = steady_clock::now();
+          wait_until(lock, s_now + ceil<nanoseconds>(d));
+          return steady_clock::now() - c_now < d ? cv_status::no_timeout :
+                                                   cv_status::timeout;
+
+        }
+
+        inline cv_status wait_until(
+            unique_lock<mutex>& lk,
+            chrono::time_point<chrono::system_clock, chrono::nanoseconds> tp)
+        {
+            using namespace chrono;
+            nanoseconds d = tp.time_since_epoch();
+            timespec ts = boost::detail::to_timespec(d);
+            if (do_wait_until(lk, ts)) return cv_status::no_timeout;
+            else return cv_status::timeout;
+        }
+#endif
+
+#else // defined BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
+#ifdef BOOST_THREAD_USES_CHRONO
+
+        template <class Duration>
+        cv_status
+        wait_until(
+              unique_lock<mutex>& lock,
+              const chrono::time_point<chrono::steady_clock, Duration>& t)
+        {
+            using namespace chrono;
+            typedef time_point<steady_clock, nanoseconds> nano_sys_tmpt;
+            wait_until(lock,
+                        nano_sys_tmpt(ceil<nanoseconds>(t.time_since_epoch())));
+            return steady_clock::now() < t ? cv_status::no_timeout :
+                                             cv_status::timeout;
+        }
+
+        template <class Clock, class Duration>
+        cv_status
+        wait_until(
+            unique_lock<mutex>& lock,
+            const chrono::time_point<Clock, Duration>& t)
+        {
+            using namespace chrono;
+            steady_clock::time_point     s_now = steady_clock::now();
+            typename Clock::time_point  c_now = Clock::now();
+            wait_until(lock, s_now + ceil<nanoseconds>(t - c_now));
+            return Clock::now() < t ? cv_status::no_timeout : cv_status::timeout;
+        }
+
+        template <class Rep, class Period>
+        cv_status
+        wait_for(
+            unique_lock<mutex>& lock,
+            const chrono::duration<Rep, Period>& d)
+        {
+            using namespace chrono;
+            steady_clock::time_point c_now = steady_clock::now();
+            wait_until(lock, c_now + ceil<nanoseconds>(d));
+            return steady_clock::now() - c_now < d ? cv_status::no_timeout :
+                                                   cv_status::timeout;
+        }
+
+        inline cv_status wait_until(
+            unique_lock<mutex>& lk,
+            chrono::time_point<chrono::steady_clock, chrono::nanoseconds> tp)
+        {
+            using namespace chrono;
+            nanoseconds d = tp.time_since_epoch();
+            timespec ts = boost::detail::to_timespec(d);
+            if (do_wait_until(lk, ts)) return cv_status::no_timeout;
+            else return cv_status::timeout;
+        }
+#endif
+
+#endif // defined BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
+
+#ifdef BOOST_THREAD_USES_CHRONO
+        template <class Clock, class Duration, class Predicate>
+        bool
+        wait_until(
+                unique_lock<mutex>& lock,
+                const chrono::time_point<Clock, Duration>& t,
+                Predicate pred)
+        {
+            while (!pred())
+            {
+                if (wait_until(lock, t) == cv_status::timeout)
+                    return pred();
+            }
+            return true;
+        }
+
+        template <class Rep, class Period, class Predicate>
+        bool
+        wait_for(
+                unique_lock<mutex>& lock,
+                const chrono::duration<Rep, Period>& d,
+                Predicate pred)
+        {
+          return wait_until(lock, chrono::steady_clock::now() + d, boost::move(pred));
+        }
+#endif
+
+#define BOOST_THREAD_DEFINES_CONDITION_VARIABLE_NATIVE_HANDLE
+        typedef pthread_cond_t* native_handle_type;
+        native_handle_type native_handle()
+        {
+            return &cond;
+        }
+
+        void notify_one() BOOST_NOEXCEPT;
+        void notify_all() BOOST_NOEXCEPT;
+
+
+    };
+
+    BOOST_THREAD_DECL void notify_all_at_thread_exit(condition_variable& cond, unique_lock<mutex> lk);
+
+}
+
+
+#include <boost/config/abi_suffix.hpp>
+
+#endif
diff --git a/clients/roscpp/include/ros/timer_manager.h b/clients/roscpp/include/ros/timer_manager.h
index e5da5fccdd..b3cd5da5ef 100644
--- a/clients/roscpp/include/ros/timer_manager.h
+++ b/clients/roscpp/include/ros/timer_manager.h
@@ -28,6 +28,20 @@
 #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"
@@ -35,7 +49,6 @@
 #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"
@@ -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 MonotonicTime 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));
+        }
       }
     }
 
diff --git a/clients/roscpp/src/libros/steady_timer.cpp b/clients/roscpp/src/libros/steady_timer.cpp
index bb252899f9..45825c709d 100644
--- a/clients/roscpp/src/libros/steady_timer.cpp
+++ b/clients/roscpp/src/libros/steady_timer.cpp
@@ -25,6 +25,9 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
+// make sure we use CLOCK_MONOTONIC for the condition variable
+#define BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
+
 #include "ros/steady_timer.h"
 #include "ros/timer_manager.h"