From e7c9157dc8e9ae40f0fe079f3c617ccd25a7b0fc Mon Sep 17 00:00:00 2001
From: Josh Faust <jfaustwg@willowgarage.com>
Date: Fri, 11 Dec 2009 23:13:19 +0000
Subject: [PATCH] Fix crash in SubscriptionQueue::full() which was causing
 subpub_n_fast test to fail

---
 core/roscpp/include/ros/subscription_queue.h  |  2 ++
 core/roscpp/src/libros/subscription_queue.cpp | 14 ++++++++++++--
 2 files changed, 14 insertions(+), 2 deletions(-)

diff --git a/core/roscpp/include/ros/subscription_queue.h b/core/roscpp/include/ros/subscription_queue.h
index d64c779d..70645f3d 100644
--- a/core/roscpp/include/ros/subscription_queue.h
+++ b/core/roscpp/include/ros/subscription_queue.h
@@ -68,6 +68,7 @@ public:
   void remove(uint64_t id);
 
 private:
+  bool fullNoLock();
   std::string topic_;
   int32_t size_;
   bool full_;
@@ -75,6 +76,7 @@ private:
 
   boost::mutex queue_mutex_;
   L_Item queue_;
+  uint32_t queue_size_;
 
   boost::recursive_mutex callback_mutex_;
 };
diff --git a/core/roscpp/src/libros/subscription_queue.cpp b/core/roscpp/src/libros/subscription_queue.cpp
index 1389f187..a6ce281b 100644
--- a/core/roscpp/src/libros/subscription_queue.cpp
+++ b/core/roscpp/src/libros/subscription_queue.cpp
@@ -37,15 +37,17 @@ SubscriptionQueue::SubscriptionQueue(const std::string& topic, int32_t queue_siz
 , size_(queue_size)
 , full_(false)
 , id_counter_(0)
+, queue_size_(0)
 {}
 
 uint64_t SubscriptionQueue::push(const SubscriptionMessageHelperPtr& helper, const MessageDeserializerPtr& deserializer, bool has_tracked_object, const VoidWPtr& tracked_object)
 {
   boost::mutex::scoped_lock lock(queue_mutex_);
 
-  if(full())
+  if(fullNoLock())
   {
     queue_.pop_front();
+    --queue_size_;
 
     if (!full_)
     {
@@ -68,6 +70,7 @@ uint64_t SubscriptionQueue::push(const SubscriptionMessageHelperPtr& helper, con
   i.tracked_object = tracked_object;
   i.id = count;
   queue_.push_back(i);
+  ++queue_size_;
 
   return count;
 }
@@ -151,6 +154,7 @@ CallbackInterface::CallResult SubscriptionQueue::call(uint64_t id)
     }
 
     queue_.pop_front();
+    --queue_size_;
   }
 
   MessagePtr msg = i.deserializer->deserialize();
@@ -177,7 +181,13 @@ bool SubscriptionQueue::ready(uint64_t id)
 
 bool SubscriptionQueue::full()
 {
-  return (size_ > 0) && (queue_.size() >= (uint32_t)size_);
+  boost::mutex::scoped_lock lock(queue_mutex_);
+  return fullNoLock();
+}
+
+bool SubscriptionQueue::fullNoLock()
+{
+  return (size_ > 0) && (queue_size_ >= (uint32_t)size_);
 }
 
 }