Execute multiple events using event.num_events

Signed-off-by: Mauro Passerino <[email protected]>

Author: Mauro Passerino

rclcpp/include/rclcpp/experimental/buffers/simple_events_queue.hpp

@@ -48,9 +48,7 @@ class SimpleEventsQueue : public EventsQueue
   void
   push(const rclcpp::executors::ExecutorEvent & event)
   {
-    for (size_t i = 0; i < event.num_events; i++) {
-      event_queue_.push(event);
-    }
+    event_queue_.push(event);
   }
 
   /**

rclcpp/src/rclcpp/executors/events_executor.cpp

@@ -183,7 +183,7 @@ EventsExecutor::spin_once_impl(std::chrono::nanoseconds timeout)
   // When condition variable is notified, check this predicate to proceed
   auto has_event_predicate = [this]() {return !events_queue_->empty();};
 
-  ExecutorEvent event;
+  ExecutorEvent single_event;
   bool has_event = false;
 
   {
@@ -194,15 +194,39 @@ EventsExecutor::spin_once_impl(std::chrono::nanoseconds timeout)
     // Grab first event from queue if it exists
     has_event = !events_queue_->empty();
     if (has_event) {
-      event = events_queue_->front();
-      events_queue_->pop();
+      // If the event has num_events > 1, we have to only execute a single event and
+      // decrement the event counter, leaving the event in the queue for future processing.
+      // But we can't modify an element from a temporary object (the queue is a shared_ptr)
+      // so, we need a local copy to manipulate and then update the original queue.
+      std::queue<ExecutorEvent> local_events_queue = events_queue_->pop_all_events();
+
+      ExecutorEvent & front_event = local_events_queue.front();
+
+      if (front_event.num_events > 1) {
+        // Decrement the counter by one, keeping the event in the front.
+        front_event.num_events--;
+      } else {
+        // We have a single event, pop it from queue.
+        local_events_queue.pop();
+      }
+
+      // Make sure we only execute a single event
+      single_event = front_event;
+      single_event.num_events = 1;
+
+      // Update the global queue
+      while (!local_events_queue.empty()) {
+        ExecutorEvent event = local_events_queue.front();
+        local_events_queue.pop();
+        events_queue_->push(event);
+      }
     }
   }
 
   // If we wake up from the wait with an event, it means that it
   // arrived before any of the timers expired.
   if (has_event) {
-    this->execute_event(event);
+    this->execute_event(single_event);
   } else {
     timers_manager_->execute_head_timer();
   }
@@ -254,7 +278,9 @@ EventsExecutor::execute_event(const ExecutorEvent & event)
         auto subscription = entities_collector_->get_subscription(event.entity_id);
 
         if (subscription) {
-          execute_subscription(subscription);
+          for (size_t i = 0; i < event.num_events; i++) {
+            execute_subscription(subscription);
+          }
         }
         break;
       }
@@ -264,7 +290,9 @@ EventsExecutor::execute_event(const ExecutorEvent & event)
         auto service = entities_collector_->get_service(event.entity_id);
 
         if (service) {
-          execute_service(service);
+          for (size_t i = 0; i < event.num_events; i++) {
+            execute_service(service);
+          }
         }
         break;
       }
@@ -274,7 +302,9 @@ EventsExecutor::execute_event(const ExecutorEvent & event)
         auto client = entities_collector_->get_client(event.entity_id);
 
         if (client) {
-          execute_client(client);
+          for (size_t i = 0; i < event.num_events; i++) {
+            execute_client(client);
+          }
         }
         break;
       }
@@ -284,8 +314,10 @@ EventsExecutor::execute_event(const ExecutorEvent & event)
         auto waitable = entities_collector_->get_waitable(event.entity_id);
 
         if (waitable) {
-          auto data = waitable->take_data();
-          waitable->execute(data);
+          for (size_t i = 0; i < event.num_events; i++) {
+            auto data = waitable->take_data();
+            waitable->execute(data);
+          }
         }
         break;
       }

rclcpp/test/rclcpp/executors/test_executors.cpp

@@ -473,10 +473,7 @@ class TestWaitable : public rclcpp::Waitable
     const void * user_data) const override
   {
     rcl_ret_t ret = rcl_guard_condition_set_listener_callback(
-      &gc_,
-      callback,
-      user_data,
-      true /*Use previous events*/);
+      &gc_, callback, user_data);
 
     if (RCL_RET_OK != ret) {
       throw std::runtime_error(std::string("Couldn't set guard condition callback"));