diff --git a/clients/roscpp/include/ros/callback_queue.h b/clients/roscpp/include/ros/callback_queue.h
index 52324b3ed1..4fe811d745 100644
--- a/clients/roscpp/include/ros/callback_queue.h
+++ b/clients/roscpp/include/ros/callback_queue.h
@@ -35,6 +35,21 @@
 #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"
diff --git a/clients/roscpp/include/ros/callback_queue_interface.h b/clients/roscpp/include/ros/callback_queue_interface.h
index c54a6f7892..d3b994b9b2 100644
--- a/clients/roscpp/include/ros/callback_queue_interface.h
+++ b/clients/roscpp/include/ros/callback_queue_interface.h
@@ -35,21 +35,6 @@
 #ifndef ROSCPP_CALLBACK_QUEUE_INTERFACE_H
 #define ROSCPP_CALLBACK_QUEUE_INTERFACE_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 <boost/shared_ptr.hpp>
 #include "common.h"
 #include "ros/types.h"
@@ -85,8 +70,6 @@ class ROSCPP_DECL CallbackInterface
    * before call() actually takes place.
    */
   virtual bool ready() { return true; }
-
-  virtual void setNotifyWhenReady(boost::condition_variable *condition) {};
 };
 typedef boost::shared_ptr<CallbackInterface> CallbackInterfacePtr;
 
diff --git a/clients/roscpp/include/ros/subscription_queue.h b/clients/roscpp/include/ros/subscription_queue.h
index 6a381ce1fa..1df0d5fc3e 100644
--- a/clients/roscpp/include/ros/subscription_queue.h
+++ b/clients/roscpp/include/ros/subscription_queue.h
@@ -28,11 +28,6 @@
 #ifndef ROSCPP_SUBSCRIPTION_QUEUE_H
 #define ROSCPP_SUBSCRIPTION_QUEUE_H
 
-// 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 "forwards.h"
 #include "common.h"
 #include "ros/message_event.h"
@@ -79,8 +74,6 @@ class ROSCPP_DECL SubscriptionQueue : public CallbackInterface, public boost::en
 
   virtual CallbackInterface::CallResult call();
   virtual bool ready();
-  virtual void setNotifyWhenReady(boost::condition_variable* condition);
-
   bool full();
 
 private:
@@ -94,8 +87,6 @@ class ROSCPP_DECL SubscriptionQueue : public CallbackInterface, public boost::en
   uint32_t queue_size_;
   bool allow_concurrent_callbacks_;
 
-  boost::condition_variable* notify_when_ready_condition_;
-
   boost::recursive_mutex callback_mutex_;
 };
 
diff --git a/clients/roscpp/src/libros/callback_queue.cpp b/clients/roscpp/src/libros/callback_queue.cpp
index fa22d99fa5..a0801db101 100644
--- a/clients/roscpp/src/libros/callback_queue.cpp
+++ b/clients/roscpp/src/libros/callback_queue.cpp
@@ -134,8 +134,6 @@ void CallbackQueue::addCallback(const CallbackInterfacePtr& callback, uint64_t r
 
   if (callback->ready())
     condition_.notify_one();
-  else
-    callback->setNotifyWhenReady(&condition_);
 }
 
 CallbackQueue::IDInfoPtr CallbackQueue::getIDInfo(uint64_t id)
@@ -414,6 +412,7 @@ CallbackQueue::CallOneResult CallbackQueue::callOneCB(TLS* tls)
       {
         tls->cb_it = tls->callbacks.erase(tls->cb_it);
         result = cb->call();
+        condition_.notify_one();
       }
     }
 
diff --git a/clients/roscpp/src/libros/subscription_queue.cpp b/clients/roscpp/src/libros/subscription_queue.cpp
index b32d071f8c..d103f8b6d4 100644
--- a/clients/roscpp/src/libros/subscription_queue.cpp
+++ b/clients/roscpp/src/libros/subscription_queue.cpp
@@ -39,7 +39,6 @@ SubscriptionQueue::SubscriptionQueue(const std::string& topic, int32_t queue_siz
 , full_(false)
 , queue_size_(0)
 , allow_concurrent_callbacks_(allow_concurrent_callbacks)
-, notify_when_ready_condition_(NULL)
 {}
 
 SubscriptionQueue::~SubscriptionQueue()
@@ -165,9 +164,6 @@ CallbackInterface::CallResult SubscriptionQueue::call()
     i.helper->call(params);
   }
 
-  if (notify_when_ready_condition_)
-    notify_when_ready_condition_->notify_one();
-
   return CallbackInterface::Success;
 }
 
@@ -191,9 +187,5 @@ bool SubscriptionQueue::fullNoLock()
   return (size_ > 0) && (queue_size_ >= (uint32_t)size_);
 }
 
-void SubscriptionQueue::setNotifyWhenReady(boost::condition_variable* condition) {
-  notify_when_ready_condition_ = condition;
-}
-
 }