Fix for timer/srv-server finalization issue 2283

clients/roscpp/src/libros/callback_queue.cpp

@@ -166,9 +166,10 @@ void CallbackQueue::removeByID(uint64_t removal_id)
  }
 
  {
- boost::unique_lock<boost::shared_mutex> rw_lock(id_info->calling_rw_mutex, boost::defer_lock);
- if (rw_lock.try_lock())
+ // Unless we're removing from callback, we lock the calling mutex to ensure callback is not being executed.
+ if (tls_->calling_in_this_thread != id_info->id)
  {
+ boost::unique_lock<boost::shared_mutex> rw_lock(id_info->calling_rw_mutex);
  boost::mutex::scoped_lock lock(mutex_);
  D_CallbackInfo::iterator it = callbacks_.begin();
  for (; it != callbacks_.end();)
@@ -186,8 +187,8 @@ void CallbackQueue::removeByID(uint64_t removal_id)
  }
  else
  {
- // We failed to acquire the lock, it can be that we are trying to remove something from the callback queue
- // while it is being executed. Mark it for removal and let it be cleaned up later.
+ // Since we're removing from callback, locking twice would deadlock.
+ // Instead, mark callback for removal and let it be cleaned up later.
  boost::mutex::scoped_lock lock(mutex_);
  for (D_CallbackInfo::iterator it = callbacks_.begin(); it != callbacks_.end(); it++)
  {

test/test_roscpp/test/test_callback_queue.cpp

@@ -42,6 +42,7 @@
 #include <boost/shared_ptr.hpp>
 #include <boost/bind.hpp>
 #include <boost/thread.hpp>
+#include <boost/thread/reverse_lock.hpp>
 #include <boost/function.hpp>
 
 using namespace ros;
@@ -66,6 +67,26 @@ class CountingCallback : public CallbackInterface
 };
 typedef boost::shared_ptr<CountingCallback> CountingCallbackPtr;
 
+struct CustomCallback : public CallbackInterface
+{
+ template <typename Func>
+ CustomCallback(Func fun)
+ : function(fun), count(0)
+ {}
+
+ virtual CallResult call()
+ {
+ function();
+ ++count;
+
+ return Success;
+ }
+
+ boost::function<void()> function;
+ size_t count;
+};
+typedef boost::shared_ptr<CustomCallback> CustomCallbackPtr;
+
 void callAvailableThread(CallbackQueue* queue, bool& done)
 {
  while (!done)
@@ -177,80 +198,90 @@ TEST(CallbackQueue, removeSelf)
  queue.callOne();
 
  queue.addCallback(cb2, 1);
- 
+
  queue.callAvailable();
 
  EXPECT_EQ(cb1->count, 1U);
  EXPECT_EQ(cb2->count, 1U);
 }
 
-class BlockingCallback : public CallbackInterface
-{
-public:
- BlockingCallback()
- : count(0)
- {}
-
- virtual CallResult call()
- {
- mutex_.lock();
- ++count;
-
- return Success;
- }
-
- boost::mutex mutex_;
- size_t count;
-};
-typedef boost::shared_ptr<BlockingCallback> BlockingCallbackPtr;
-
-
-// This test checks whether removing callbacks by an id doesn't block if one of those callback is being executed.
-TEST(CallbackQueue, removeCallbackWhileExecuting)
+// This test checks whether self-removing callbacks by an id doesn't block if one of those callback is being executed.
+TEST(CallbackQueue, selfRemoveCallbackWhileExecuting)
 {
  const uint64_t owner_id = 1;
  const uint64_t unrelated_id = 2;
 
+ boost::mutex external_mtx;
+
  CallbackQueue queue;
- BlockingCallbackPtr cb1(boost::make_shared<BlockingCallback>());
+ CustomCallbackPtr cb1(boost::make_shared<CustomCallback>([&]() {
+ boost::unique_lock<boost::mutex> external_lock(external_mtx);
+ boost::this_thread::sleep_for(boost::chrono::milliseconds(300));
+
+ {
+ boost::reverse_lock<boost::unique_lock<boost::mutex>> unlocker(external_lock);
+ queue.removeByID(owner_id); // external thread blocks here, spinner doesn't
+ }
+ }));
  CountingCallbackPtr cb2(boost::make_shared<CountingCallback>());
  CountingCallbackPtr cb3(boost::make_shared<CountingCallback>());
 
- cb1->mutex_.lock(); // lock the mutex to ensure the blocking callback will stall processing of callback queue.
-
- queue.addCallback(cb1, owner_id); // Add the blocking callback.
-
- // Now, we need to serve the callback queue from another thread.
- bool done = false;
- boost::thread t = boost::thread(boost::bind(&callAvailableThread, &queue, boost::ref(done)));
-
- ros::WallDuration(1.0).sleep(); // Callback 1 should be being served now.
-
+ queue.addCallback(cb1, owner_id); // Add the self-removing callback.
  queue.addCallback(cb2, unrelated_id); // Add a second callback with different owner.
  queue.addCallback(cb3, owner_id); // Add a third with same owner, this one should never get executed.
 
- // Now try to remove the callback that's being executed.
- queue.removeByID(owner_id); // This should not block because cb1 is being served, it should prevent cb3 from running.
+ // Let's use an external thread to execute cb function and hold its external lock
+ boost::thread t1([&]() { cb1->call(); });
+ boost::this_thread::sleep_for(boost::chrono::milliseconds(100));
 
- ros::WallDuration(1.0).sleep();
-
- // The removeByID should not block, so now we can unblock the blocking callback.
- cb1->mutex_.unlock(); // This allows processing of cb1 to continue.
+ // Now, we need to serve the callback queue from another thread.
+ bool done = false;
+ boost::thread t2([&]() { callAvailableThread(&queue, done); });
 
  while (!queue.isEmpty()) // Wait until the queue is empty.
  {
  ros::WallDuration(0.01).sleep();
  }
 
- // Properly shut down our callback serving thread.
+ // Properly shut down our threads.
  done = true;
- t.join();
+ t2.join();
+ t1.join();
 
- EXPECT_EQ(cb1->count, 1U);
+ EXPECT_EQ(cb1->count, 2U);
  EXPECT_EQ(cb2->count, 1U);
  EXPECT_EQ(cb3->count, 0U);
+}
+
+// This test checks whether non-spinner thread blocks on removeByID until currently executing callback finishes
+TEST(CallbackQueue, removeCallbackWhileExecuting)
+{
+ const uint64_t cb_id = 1;
+ boost::barrier barrier(2);
+
+ CallbackQueue queue;
+ CustomCallbackPtr cb(boost::make_shared<CustomCallback>([&]() {
+ barrier.wait();
+ barrier.wait();
+ }));
+ queue.addCallback(cb, cb_id);
+
+ // Let's ensure spinner thread executes callback now
+ bool done = false;
+ boost::thread t1([&]() { callAvailableThread(&queue, done); });
+ barrier.wait();
 
- cb1->mutex_.unlock(); // Ensure the mutex is unlocked before destruction.
+ // External removing thread blocks on removeByID
+ boost::thread t2([&]() { queue.removeByID(cb_id); });
+ EXPECT_FALSE(t2.try_join_for(boost::chrono::milliseconds(200))); // removebyID blocks until cb finishes
+
+ // When callback finishes, external thread proceeds
+ barrier.wait();
+ t2.join();
+ EXPECT_EQ(cb->count, 1U);
+
+ done = true;
+ t1.join();
 }
 
 class RecursiveCallback : public CallbackInterface
@@ -549,6 +580,3 @@ int main(int argc, char** argv)
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
 }
-
-
-