[WIP] Fix subscription busy wait (melodic-devel)

clients/roscpp/include/ros/callback_queue.h

@@ -35,21 +35,6 @@
 #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"

clients/roscpp/include/ros/callback_queue_interface.h

@@ -35,6 +35,21 @@
 #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"
@@ -70,6 +85,8 @@ 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;
 

clients/roscpp/include/ros/subscription_queue.h

@@ -74,6 +74,8 @@ 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:
@@ -87,6 +89,8 @@ 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_;
 };
 

clients/roscpp/src/libros/callback_queue.cpp

@@ -137,7 +137,10 @@ void CallbackQueue::addCallback(const CallbackInterfacePtr& callback, uint64_t r
     callbacks_.push_back(info);
   }
 
-  condition_.notify_one();
+  if (callback->ready())
+    condition_.notify_one();
+  else
+    callback->setNotifyWhenReady(&condition_);
 }
 
 CallbackQueue::IDInfoPtr CallbackQueue::getIDInfo(uint64_t id)
@@ -238,6 +241,8 @@ CallbackQueue::CallOneResult CallbackQueue::callOne(ros::WallDuration timeout)
       return Disabled;
     }
 
+    ros::WallTime start_time(ros::WallTime::now());
+
     if (callbacks_.empty())
     {
       if (!timeout.isZero())
@@ -279,6 +284,16 @@ CallbackQueue::CallOneResult CallbackQueue::callOne(ros::WallDuration timeout)
 
     if (!cb_info.callback)
     {
+      // do not spend more than `timeout` seconds in the callback; we already waited for some time when waiting for
+      // nonempty queue
+      ros::WallTime now(ros::WallTime::now());
+      ros::WallDuration time_spent = now - start_time;
+      ros::WallDuration time_to_wait = timeout - time_spent;
+
+      if (time_to_wait.toNSec() > 0) {
+        condition_.wait_for(lock, boost::chrono::nanoseconds(time_to_wait.toNSec()));
+      }
+
       return TryAgain;
     }
 

clients/roscpp/src/libros/subscription_queue.cpp

@@ -39,6 +39,7 @@ 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()
@@ -164,12 +165,19 @@ CallbackInterface::CallResult SubscriptionQueue::call()
     i.helper->call(params);
   }
 
+  if (notify_when_ready_condition_)
+    notify_when_ready_condition_->notify_one();
+
   return CallbackInterface::Success;
 }
 
 bool SubscriptionQueue::ready()
 {
-  return true;
+  if (!allow_concurrent_callbacks_) {
+    boost::recursive_mutex::scoped_try_lock lock(callback_mutex_, boost::try_to_lock);
+    return lock.owns_lock();
+  } else
+    return true;
 }
 
 bool SubscriptionQueue::full()
@@ -183,5 +191,9 @@ bool SubscriptionQueue::fullNoLock()
   return (size_ > 0) && (queue_size_ >= (uint32_t)size_);
 }
 
+void SubscriptionQueue::setNotifyWhenReady(boost::condition_variable* condition) {
+  notify_when_ready_condition_ = condition;
+}
+
 }
 

test/test_roscpp/test/fake_message.cpp

@@ -0,0 +1,89 @@
+/*
+ * Copyright (c) 2009, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/* Author: Josh Faust */
+
+/*
+ * Subscription queue test helper classes
+ */
+
+#include "ros/subscription_callback_helper.h"
+
+using namespace ros;
+
+class FakeMessage
+{
+public:
+    virtual const std::string __getDataType() const { return ""; }
+    virtual const std::string __getMD5Sum() const { return ""; }
+    virtual const std::string __getMessageDefinition() const { return ""; }
+    virtual uint32_t serializationLength() const { return 0; }
+    virtual uint8_t *serialize(uint8_t *write_ptr, uint32_t seq) const { (void)seq; return write_ptr; }
+    virtual uint8_t *deserialize(uint8_t *read_ptr) { return read_ptr; }
+};
+
+class FakeSubHelper : public SubscriptionCallbackHelper
+{
+public:
+    FakeSubHelper()
+        : calls_(0)
+    {}
+
+    virtual VoidConstPtr deserialize(const SubscriptionCallbackHelperDeserializeParams&)
+    {
+      return boost::make_shared<FakeMessage>();
+    }
+
+    virtual std::string getMD5Sum() { return ""; }
+    virtual std::string getDataType() { return ""; }
+
+    virtual void call(SubscriptionCallbackHelperCallParams& params)
+    {
+      (void)params;
+      {
+        boost::mutex::scoped_lock lock(mutex_);
+        ++calls_;
+      }
+
+      if (cb_)
+      {
+        cb_();
+      }
+    }
+
+    virtual const std::type_info& getTypeInfo() { return typeid(FakeMessage); }
+    virtual bool isConst() { return true; }
+    virtual bool hasHeader() { return false; }
+
+    boost::mutex mutex_;
+    int32_t calls_;
+
+    boost::function<void(void)> cb_;
+};
+typedef boost::shared_ptr<FakeSubHelper> FakeSubHelperPtr;