Fix "roscpp multithreaded spinners eat up CPU when callbacks take too long"

clients/roscpp/src/libros/callback_queue.cpp

@@ -124,7 +124,10 @@ void CallbackQueue::addCallback(const CallbackInterfacePtr& callback, uint64_t r
     callbacks_.push_back(info);
   }
 
-  condition_.notify_one();
+  if (callback->ready())
+  {
+    condition_.notify_one();
+  }
 }
 
 CallbackQueue::IDInfoPtr CallbackQueue::getIDInfo(uint64_t id)
@@ -242,11 +245,39 @@ CallbackQueue::CallOneResult CallbackQueue::callOne(ros::WallDuration timeout)
       return Disabled;
     }
 
-    if (callbacks_.empty())
-    {
+    boost::chrono::steady_clock::time_point wait_until =
+        boost::chrono::steady_clock::now() + boost::chrono::nanoseconds(timeout.toNSec());
+    while (!cb_info.callback) {
+      D_CallbackInfo::iterator it = callbacks_.begin();
+      for (; it != callbacks_.end();)
+      {
+        CallbackInfo& info = *it;
+
+        if (info.marked_for_removal)
+        {
+          it = callbacks_.erase(it);
+          continue;
+        }
+
+        if (info.callback->ready())
+        {
+          cb_info = info;
+          it = callbacks_.erase(it);
+          break;
+        }
+
+        ++it;
+      }
+
+      // Found a ready callback?
+      if (cb_info.callback) {
+        break;
+      }
+
+      boost::cv_status wait_status = boost::cv_status::timeout;
       if (!timeout.isZero())
       {
-        condition_.wait_for(lock, boost::chrono::nanoseconds(timeout.toNSec()));
+        wait_status = condition_.wait_until(lock, wait_until);
       }
 
       if (callbacks_.empty())
@@ -258,32 +289,11 @@ CallbackQueue::CallOneResult CallbackQueue::callOne(ros::WallDuration timeout)
       {
         return Disabled;
       }
-    }
-
-    D_CallbackInfo::iterator it = callbacks_.begin();
-    for (; it != callbacks_.end();)
-    {
-      CallbackInfo& info = *it;
 
-      if (info.marked_for_removal)
+      if (wait_status == boost::cv_status::timeout)
       {
-        it = callbacks_.erase(it);
-        continue;
+        return TryAgain;
       }
-
-      if (info.callback->ready())
-      {
-        cb_info = info;
-        it = callbacks_.erase(it);
-        break;
-      }
-
-      ++it;
-    }
-
-    if (!cb_info.callback)
-    {
-      return TryAgain;
     }
 
     ++calling_;
@@ -408,6 +418,10 @@ CallbackQueue::CallOneResult CallbackQueue::callOneCB(TLS* tls)
       {
         tls->cb_it = tls->callbacks.erase(tls->cb_it);
         result = cb->call();
+        if (result == CallbackInterface::Success)
+        {
+          condition_.notify_one();
+        }
       }
     }
 

clients/roscpp/src/libros/subscription_queue.cpp

@@ -169,7 +169,12 @@ CallbackInterface::CallResult SubscriptionQueue::call()
 
 bool SubscriptionQueue::ready()
 {
-  return true;
+  if (allow_concurrent_callbacks_)
+  {
+    return true;
+  }
+  boost::recursive_mutex::scoped_try_lock lock(callback_mutex_, boost::try_to_lock);
+  return lock.owns_lock();
 }
 
 bool SubscriptionQueue::full()

test/test_roscpp/test/fake_message.h

@@ -0,0 +1,91 @@
+/*
+ * 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
+ */
+#ifndef TEST_ROSCPP_FAKE_MESSAGE_H
+#define TEST_ROSCPP_FAKE_MESSAGE_H
+
+#include "ros/subscription_callback_helper.h"
+
+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 ros::SubscriptionCallbackHelper
+{
+public:
+  FakeSubHelper()
+    : calls_(0)
+  {}
+
+  virtual ros::VoidConstPtr deserialize(const ros::SubscriptionCallbackHelperDeserializeParams&)
+  {
+    return boost::make_shared<FakeMessage>();
+  }
+
+  virtual std::string getMD5Sum() { return ""; }
+  virtual std::string getDataType() { return ""; }
+
+  virtual void call(ros::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_;
+  uint32_t calls_;
+
+  boost::function<void(void)> cb_;
+};
+typedef boost::shared_ptr<FakeSubHelper> FakeSubHelperPtr;
+
+#endif // TEST_ROSCPP_FAKE_MESSAGE_H