Merge pull request #125 from cactusdynamics/fair-dequeue-publisher

Publisher dequeue changes
cactusdynamics · Aug 21, 2024 · 93254fc · 93254fc
2 parents dc53ca0 + 1042897
commit 93254fc
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 9 deletions.
diff --git a/include/cactus_rt/ros2/ros2_adapter.h b/include/cactus_rt/ros2/ros2_adapter.h
@@ -15,7 +15,11 @@
 
 namespace cactus_rt::ros2 {
 
+class Ros2ExecutorThread;
+
 class Ros2Adapter {
+  friend class Ros2ExecutorThread;
+
  public:
   struct Config {
     /**

diff --git a/src/cactus_rt/ros2/app.cc b/src/cactus_rt/ros2/app.cc
@@ -25,8 +25,7 @@ void Ros2ExecutorThread::Run() {
     executor_->spin_once();
   }
 
-  // Execute one more time to ensure everything is processed.
-  executor_->spin_once();
+  ros2_adapter_->DrainQueues();
 
   executor_->remove_node(node_ptr);
 }

diff --git a/src/cactus_rt/ros2/ros2_adapter.cc b/src/cactus_rt/ros2/ros2_adapter.cc
@@ -16,13 +16,15 @@ void Ros2Adapter::TimerCallback() {
 void Ros2Adapter::DrainQueues() {
   const std::scoped_lock lock(mut_);
 
-  for (const auto& publisher : publishers_) {
-    // Hopefully the thread is not publishing so quickly that a single
-    // publisher monopolizes all resources. That said, if that happens the
-    // program is likely in bigger trouble anyway.
-    //
-    // TODO: make it so we dequeue once.
-    publisher->FullyDrainAndPublishToRos();
+  bool has_data = true;
+  while (has_data) {
+    has_data = false;
+
+    for (const auto& publisher : publishers_) {
+      if (publisher->DequeueAndPublishToRos()) {
+        has_data = true;
+      }
+    }
   }
 }