Solve a bug that causes a deadlock in MessageFilter

The deadlock happens when the following two things happen at the same instant:

* MessageFilter trying to erase a message from the queue, locks the messages_mutex_ then calls BufferCore::cancelTransformableRequest where it needs the transformable_requests_mutex_

* BufferCore upon receiving a TF callback, locks the transformable_requests_mutex_ in testTransformableRequests and goes through the list of requests where one of the callback it calls is MessageFilter::Transformable that attempts to acquire a unique_lock on message_mutex_

tf2/src/buffer_core.cpp

@@ -37,6 +37,7 @@
 #include <assert.h>
 #include <console_bridge/console.h>
 #include "tf2/LinearMath/Transform.h"
+#include <boost/foreach.hpp>
 
 namespace tf2
 {
@@ -1312,6 +1313,11 @@ void BufferCore::testTransformableRequests()
 {
   boost::mutex::scoped_lock lock(transformable_requests_mutex_);
   V_TransformableRequest::iterator it = transformable_requests_.begin();
+
+  typedef boost::tuple<TransformableCallback&, TransformableRequestHandle, std::string,
+                       std::string, ros::Time&, TransformableResult&> TransformableTuple;
+  std::vector<TransformableTuple> transformables;
+
   for (; it != transformable_requests_.end();)
   {
     TransformableRequest& req = *it;
@@ -1351,8 +1357,12 @@ void BufferCore::testTransformableRequests()
         M_TransformableCallback::iterator it = transformable_callbacks_.find(req.cb_handle);
         if (it != transformable_callbacks_.end())
         {
-          const TransformableCallback& cb = it->second;
-          cb(req.request_handle, lookupFrameString(req.target_id), lookupFrameString(req.source_id), req.time, result);
+          transformables.push_back(boost::make_tuple(boost::ref(it->second),
+                                                     req.request_handle,
+                                                     lookupFrameString(req.target_id),
+                                                     lookupFrameString(req.source_id),
+                                                     boost::ref(req.time),
+                                                     boost::ref(result)));
         }
       }
 
@@ -1369,9 +1379,14 @@ void BufferCore::testTransformableRequests()
     }
   }
 
-  // unlock before allowing possible user callbacks to avoid potential detadlock (#91)
+  // unlock before allowing possible user callbacks to avoid potential deadlock (#91)
   lock.unlock();
 
+  BOOST_FOREACH (TransformableTuple tt, transformables)
+  {
+    tt.get<0>()(tt.get<1>(), tt.get<2>(), tt.get<3>(), tt.get<4>(), tt.get<5>());
+  }
+
   // Backwards compatability callback for tf
   _transforms_changed_();
 }

tf2_ros/include/tf2_ros/message_filter.h

@@ -367,39 +367,30 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi
     }
     else
     {
+      boost::unique_lock< boost::shared_mutex > unique_lock(messages_mutex_);
       // If this message is about to push us past our queue size, erase the oldest message
       if (queue_size_ != 0 && message_count_ + 1 > queue_size_)
       {
-        // While we're using the reference keep a shared lock on the messages.
-        boost::shared_lock< boost::shared_mutex > shared_lock(messages_mutex_);
-
         ++dropped_message_count_;
         const MessageInfo& front = messages_.front();
         TF2_ROS_MESSAGEFILTER_DEBUG("Removed oldest message because buffer is full, count now %d (frame_id=%s, stamp=%f)", message_count_,
                                 (mt::FrameId<M>::value(*front.event.getMessage())).c_str(), mt::TimeStamp<M>::value(*front.event.getMessage()).toSec());
 
         V_TransformableRequestHandle::const_iterator it = front.handles.begin();
         V_TransformableRequestHandle::const_iterator end = front.handles.end();
+
         for (; it != end; ++it)
         {
           bc_.cancelTransformableRequest(*it);
         }
 
         messageDropped(front.event, filter_failure_reasons::Unknown);
-        // Unlock the shared lock and get a unique lock. Upgradeable lock is used in transformable.
-        // There can only be one upgrade lock. It's important the cancelTransformableRequest not deadlock with transformable.
-        // They both require the transformable_requests_mutex_ in BufferCore.
-        shared_lock.unlock();
-        // There is a very slight race condition if an older message arrives in this gap.
-        boost::unique_lock< boost::shared_mutex > unique_lock(messages_mutex_);
         messages_.pop_front();
          --message_count_;
       }
 
       // Add the message to our list
       info.event = evt;
-      // Lock access to the messages_ before modifying them.
-      boost::unique_lock< boost::shared_mutex > unique_lock(messages_mutex_);
       messages_.push_back(info);
       ++message_count_;
     }