From 3a98900d27f46f5c524bca7c774dd7f882ce4db5 Mon Sep 17 00:00:00 2001
From: Johannes Meyer <johannes@intermodalics.eu>
Date: Tue, 18 Jun 2019 18:05:24 +0200
Subject: [PATCH 1/2] roscpp: simplify implementation of
 CallbackQueue::callOne()

Replace duplicate wait_for() call in CallbackQueue::callOne() with a loop and a call to wait_until().
---
 clients/roscpp/src/libros/callback_queue.cpp | 70 +++++++++-----------
 1 file changed, 32 insertions(+), 38 deletions(-)

diff --git a/clients/roscpp/src/libros/callback_queue.cpp b/clients/roscpp/src/libros/callback_queue.cpp
index cef18e11d7..2e89effe7e 100644
--- a/clients/roscpp/src/libros/callback_queue.cpp
+++ b/clients/roscpp/src/libros/callback_queue.cpp
@@ -228,13 +228,39 @@ CallbackQueue::CallOneResult CallbackQueue::callOne(ros::WallDuration timeout)
       return Disabled;
     }
 
-    ros::SteadyTime start_time(ros::SteadyTime::now());
+    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 (callbacks_.empty())
-    {
+        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::no_timeout;
       if (!timeout.isZero())
       {
-        condition_.wait_for(lock, boost::chrono::nanoseconds(timeout.toNSec()));
+        wait_status = condition_.wait_until(lock, wait_until);
       }
 
       if (callbacks_.empty())
@@ -246,43 +272,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)
-      {
-        it = callbacks_.erase(it);
-        continue;
-      }
 
-      if (info.callback->ready())
+      if (wait_status == boost::cv_status::timeout)
       {
-        cb_info = info;
-        it = callbacks_.erase(it);
-        break;
+        return TryAgain;
       }
-
-      ++it;
-    }
-
-    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::SteadyTime now(ros::SteadyTime::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;
     }
 
     ++calling_;

From 7b69f4426f6901a1c5a3fafd75cf92a1df6ba7af Mon Sep 17 00:00:00 2001
From: Johannes Meyer <johannes@intermodalics.eu>
Date: Fri, 31 Jul 2020 04:17:10 +0200
Subject: [PATCH 2/2] roscpp: return TryAgain from
 CallbackQueue::callOne(timeout) immediately if timeout.isZero()

... and if none of the other result conditions holds (i.e. the queue is Empty or Disabled).
---
 clients/roscpp/src/libros/callback_queue.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/clients/roscpp/src/libros/callback_queue.cpp b/clients/roscpp/src/libros/callback_queue.cpp
index 2e89effe7e..d0ffd7477d 100644
--- a/clients/roscpp/src/libros/callback_queue.cpp
+++ b/clients/roscpp/src/libros/callback_queue.cpp
@@ -257,7 +257,7 @@ CallbackQueue::CallOneResult CallbackQueue::callOne(ros::WallDuration timeout)
         break;
       }
 
-      boost::cv_status wait_status = boost::cv_status::no_timeout;
+      boost::cv_status wait_status = boost::cv_status::timeout;
       if (!timeout.isZero())
       {
         wait_status = condition_.wait_until(lock, wait_until);