EventsExecutor to use std::function

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

Author: Mauro Passerino

rclcpp/include/rclcpp/executors/events_executor.hpp

@@ -209,31 +209,6 @@ class EventsExecutor : public rclcpp::Executor
 private:
   RCLCPP_DISABLE_COPY(EventsExecutor)
 
-  // Executor callback: Push new events into the queue and trigger cv.
-  // This function is called by the DDS entities when an event happened,
-  // like a subscription receiving a message.
-  static void
-  push_event(const void * event_data, size_t num_events)
-  {
-    if (!event_data) {
-      throw std::runtime_error("Executor event data not valid.");
-    }
-
-    auto data = static_cast<const executors::EventsExecutorCallbackData *>(event_data);
-
-    executors::EventsExecutor * this_executor = data->executor;
-    ExecutorEvent event = data->event;
-    event.num_events = num_events;
-
-    // Event queue mutex scope
-    {
-      std::unique_lock<std::mutex> lock(this_executor->push_mutex_);
-      this_executor->events_queue_->push(event);
-    }
-    // Notify that the event queue has some events in it.
-    this_executor->events_queue_cv_.notify_one();
-  }
-
   // Execute a single event
   RCLCPP_PUBLIC
   void

rclcpp/include/rclcpp/executors/events_executor_entities_collector.hpp

@@ -214,11 +214,8 @@ class EventsExecutorEntitiesCollector final
   void
   unset_guard_condition_callback(const rclcpp::GuardCondition * guard_condition);
 
-  void
-  remove_callback_data(void * entity_id, ExecutorEventType type);
-
-  const EventsExecutorCallbackData *
-  get_callback_data(void * entity_id, ExecutorEventType type);
+  std::function<void(size_t)>
+  create_entity_callback(void * entity_id, ExecutorEventType type);
 
   /// Return true if the node belongs to the collector
   /**
@@ -269,17 +266,6 @@ class EventsExecutorEntitiesCollector final
   EventsExecutor * associated_executor_ = nullptr;
   /// Instance of the timers manager used by the associated executor
   TimersManager::SharedPtr timers_manager_;
-
-  /// Callback data objects mapped to the number of listeners sharing the same object.
-  /// When no more listeners use the object, it can be removed from the map.
-  /// For example, the entities collector holds every node's guard condition, which
-  /// share the same EventsExecutorCallbackData object ptr to use as their callback arg:
-  ///    cb_data_object = {executor_ptr, entities_collector_ptr, WAITABLE_EVENT};
-  ///    Node1->gc(&cb_data_object)
-  ///    Node2->gc(&cb_data_object)
-  /// So the maps has: (cb_data_object, 2)
-  /// When both nodes are removed (counter = 0), the cb_data_object can be destroyed.
-  std::unordered_map<EventsExecutorCallbackData, size_t, KeyHasher> callback_data_map_;
 };
 
 }  // namespace executors

rclcpp/include/rclcpp/executors/events_executor_event_types.hpp

@@ -20,9 +20,6 @@ namespace rclcpp
 namespace executors
 {
 
-// forward declaration of EventsExecutor to avoid circular dependency
-class EventsExecutor;
-
 enum ExecutorEventType
 {
   SUBSCRIPTION_EVENT,
@@ -38,42 +35,6 @@ struct ExecutorEvent
   size_t num_events;
 };
 
-// The EventsExecutorCallbackData struct is what the listeners
-// will use as argument when calling their callbacks from the
-// RMW implementation. The listeners get a (void *) of this struct,
-// and the executor is in charge to cast it back and use the data.
-struct EventsExecutorCallbackData
-{
-  EventsExecutorCallbackData(
-    EventsExecutor * _executor,
-    ExecutorEvent _event)
-  {
-    executor = _executor;
-    event = _event;
-  }
-
-  // Equal operator
-  bool operator==(const EventsExecutorCallbackData & other) const
-  {
-    return event.entity_id == other.event.entity_id;
-  }
-
-  // Struct members
-  EventsExecutor * executor;
-  ExecutorEvent event;
-};
-
-// To be able to use std::unordered_map with an EventsExecutorCallbackData
-// as key, we need a hasher. We use the entity ID as hash, since it is
-// unique for each EventsExecutorCallbackData object.
-struct KeyHasher
-{
-  size_t operator()(const EventsExecutorCallbackData & key) const
-  {
-    return std::hash<const void *>()(key.event.entity_id);
-  }
-};
-
 }  // namespace executors
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/waitable.hpp

@@ -16,6 +16,7 @@
 #define RCLCPP__WAITABLE_HPP_
 
 #include <atomic>
+#include <functional>
 #include <memory>
 
 #include "rclcpp/macros.hpp"

rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp

@@ -20,7 +20,7 @@
 #include "rclcpp/executors/events_executor.hpp"
 #include "rclcpp/executors/events_executor_entities_collector.hpp"
 
-using rclcpp::executors::EventsExecutorCallbackData;
+using rclcpp::executors::ExecutorEvent;
 using rclcpp::executors::EventsExecutorEntitiesCollector;
 
 EventsExecutorEntitiesCollector::EventsExecutorEntitiesCollector(
@@ -76,7 +76,6 @@ EventsExecutorEntitiesCollector::~EventsExecutorEntitiesCollector()
   weak_nodes_.clear();
   weak_clients_map_.clear();
   weak_services_map_.clear();
-  callback_data_map_.clear();
   weak_waitables_map_.clear();
   weak_subscriptions_map_.clear();
   weak_nodes_to_guard_conditions_.clear();
@@ -238,9 +237,8 @@ EventsExecutorEntitiesCollector::set_callback_group_entities_callbacks(
       if (subscription) {
         weak_subscriptions_map_.emplace(subscription.get(), subscription);
 
-        subscription->set_listener_callback(
-          &EventsExecutor::push_event,
-          get_callback_data(subscription.get(), SUBSCRIPTION_EVENT));
+        subscription->set_on_new_message_callback(
+          create_entity_callback(subscription.get(), SUBSCRIPTION_EVENT));
       }
       return false;
     });
@@ -249,9 +247,8 @@ EventsExecutorEntitiesCollector::set_callback_group_entities_callbacks(
       if (service) {
         weak_services_map_.emplace(service.get(), service);
 
-        service->set_listener_callback(
-          &EventsExecutor::push_event,
-          get_callback_data(service.get(), SERVICE_EVENT));
+        service->set_on_new_request_callback(
+          create_entity_callback(service.get(), SERVICE_EVENT));
       }
       return false;
     });
@@ -260,9 +257,8 @@ EventsExecutorEntitiesCollector::set_callback_group_entities_callbacks(
       if (client) {
         weak_clients_map_.emplace(client.get(), client);
 
-        client->set_listener_callback(
-          &EventsExecutor::push_event,
-          get_callback_data(client.get(), CLIENT_EVENT));
+        client->set_on_new_response_callback(
+          create_entity_callback(client.get(), CLIENT_EVENT));
       }
       return false;
     });
@@ -271,9 +267,8 @@ EventsExecutorEntitiesCollector::set_callback_group_entities_callbacks(
       if (waitable) {
         weak_waitables_map_.emplace(waitable.get(), waitable);
 
-        waitable->set_listener_callback(
-          &EventsExecutor::push_event,
-          get_callback_data(waitable.get(), WAITABLE_EVENT));
+        // waitable->set_listener_callback(
+        //   create_entity_callback(waitable.get(), WAITABLE_EVENT));
       }
       return false;
     });
@@ -296,36 +291,32 @@ EventsExecutorEntitiesCollector::unset_callback_group_entities_callbacks(
   group->find_subscription_ptrs_if(
     [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) {
       if (subscription) {
-        subscription->set_listener_callback(nullptr, nullptr);
+        subscription->set_on_new_message_callback(nullptr);
         weak_subscriptions_map_.erase(subscription.get());
-        remove_callback_data(subscription.get(), SUBSCRIPTION_EVENT);
       }
       return false;
     });
   group->find_service_ptrs_if(
     [this](const rclcpp::ServiceBase::SharedPtr & service) {
       if (service) {
-        service->set_listener_callback(nullptr, nullptr);
+        service->set_on_new_request_callback(nullptr);
         weak_services_map_.erase(service.get());
-        remove_callback_data(service.get(), SERVICE_EVENT);
       }
       return false;
     });
   group->find_client_ptrs_if(
     [this](const rclcpp::ClientBase::SharedPtr & client) {
       if (client) {
-        client->set_listener_callback(nullptr, nullptr);
+        client->set_on_new_response_callback(nullptr);
         weak_clients_map_.erase(client.get());
-        remove_callback_data(client.get(), CLIENT_EVENT);
       }
       return false;
     });
   group->find_waitable_ptrs_if(
     [this](const rclcpp::Waitable::SharedPtr & waitable) {
       if (waitable) {
-        waitable->set_listener_callback(nullptr, nullptr);
+        waitable->set_listener_callback(nullptr);
         weak_waitables_map_.erase(waitable.get());
-        remove_callback_data(waitable.get(), WAITABLE_EVENT);
       }
       return false;
     });
@@ -487,28 +478,13 @@ void
 EventsExecutorEntitiesCollector::set_guard_condition_callback(
   const rclcpp::GuardCondition * guard_condition)
 {
-  rcl_ret_t ret = rcl_guard_condition_set_listener_callback(
-    guard_condition,
-    &EventsExecutor::push_event,
-    get_callback_data(this, WAITABLE_EVENT));
-
-  if (ret != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't set guard condition event callback");
-  }
+  // create_entity_callback(this, WAITABLE_EVENT);
 }
 
 void
 EventsExecutorEntitiesCollector::unset_guard_condition_callback(
   const rclcpp::GuardCondition * guard_condition)
 {
-  rcl_ret_t ret = rcl_guard_condition_set_listener_callback(
-    guard_condition, nullptr, nullptr);
-
-  if (ret != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't unset guard condition event callback");
-  }
-
-  remove_callback_data(this, WAITABLE_EVENT);
 }
 
 rclcpp::SubscriptionBase::SharedPtr
@@ -592,56 +568,21 @@ EventsExecutorEntitiesCollector::add_waitable(rclcpp::Waitable::SharedPtr waitab
 {
   weak_waitables_map_.emplace(waitable.get(), waitable);
 
-  waitable->set_listener_callback(
-    &EventsExecutor::push_event,
-    get_callback_data(waitable.get(), WAITABLE_EVENT));
+  waitable->set_listener_callback([] (size_t, int){});
 }
 
-const EventsExecutorCallbackData *
-EventsExecutorEntitiesCollector::get_callback_data(
+std::function<void(size_t)>
+EventsExecutorEntitiesCollector::create_entity_callback(
   void * entity_id, ExecutorEventType event_type)
 {
-  // Create an entity callback data object and check if
-  // we already have stored one like it
-  ExecutorEvent event = {entity_id, event_type, 0};
-  EventsExecutorCallbackData data(associated_executor_, event);
-
-  auto it = callback_data_map_.find(data);
-
-  if (it != callback_data_map_.end()) {
-    // We found a callback data matching entity ID and type.
-    // Increment callback data counter and return pointer to data
-    it->second++;
-    return &it->first;
-  }
-
-  // There was no callback data object matching ID and type,
-  // create one and set counter to 1.
-  callback_data_map_.emplace(data, 1);
-
-  // Return a pointer to the just added entity callback data.
-  it = callback_data_map_.find(data);
-  return &it->first;
-}
-
-void
-EventsExecutorEntitiesCollector::remove_callback_data(
-  void * entity_id, ExecutorEventType event_type)
-{
-  // Create an entity callback data object and check if
-  // we already have stored one like it
-  ExecutorEvent event = {entity_id, event_type, 0};
-  EventsExecutorCallbackData data(associated_executor_, event);
-
-  auto it = callback_data_map_.find(data);
-
-  if (it != callback_data_map_.end()) {
-    // We found a callback data matching entity ID and type.
-    // If we have more than 1 decrement counter, otherwise remove it.
-    if (it->second > 1) {
-      it->second--;
-    } else {
-      callback_data_map_.erase(it);
+  return [this, entity_id, event_type](size_t num_events) {
+    ExecutorEvent event = {entity_id, event_type, num_events};
+    // Event queue mutex scope
+    {
+      std::unique_lock<std::mutex> lock(associated_executor_->push_mutex_);
+      associated_executor_->events_queue_->push(event);
     }
-  }
+    // Notify that the event queue has some events in it.
+    associated_executor_->events_queue_cv_.notify_one();
+  };
 }

rclcpp/src/rclcpp/qos_event.cpp

@@ -85,11 +85,9 @@ QOSEventHandlerBase::set_on_new_event_callback(
 }
 
 void
-QOSEventHandlerBase::set_listener_callback(
-  rmw_listener_callback_t callback,
-  const void * user_data)
+QOSEventHandlerBase::set_listener_callback(std::function<void(size_t, int)> callback)
 {
-  set_on_new_event_callback(callback, user_data);
+  // set_on_new_event_callback(callback);
 }
 
 

rclcpp/src/rclcpp/subscription_intra_process_base.cpp

@@ -39,10 +39,7 @@ SubscriptionIntraProcessBase::get_actual_qos() const
 }
 
 void
-SubscriptionIntraProcessBase::set_listener_callback(
-  rmw_listener_callback_t callback,
-  const void * user_data)
+SubscriptionIntraProcessBase::set_listener_callback(std::function<void(size_t, int)> callback)
 {
   (void)callback;
-  (void)user_data;
 }

rclcpp/src/rclcpp/waitable.cpp

@@ -61,12 +61,9 @@ Waitable::exchange_in_use_by_wait_set_state(bool in_use_state)
 }
 
 void
-Waitable::set_listener_callback(
-  rmw_listener_callback_t callback,
-  const void * user_data)
+Waitable::set_listener_callback(std::function<void(size_t, int)> callback)
 {
   (void)callback;
-  (void)user_data;
 
   throw std::runtime_error(
           "Custom waitables should override set_listener_callback() "

rclcpp/test/rclcpp/executors/test_executors.cpp

@@ -447,16 +447,8 @@ class TestWaitable : public rclcpp::Waitable
   }
 
   void
-  set_listener_callback(
-    rmw_listener_callback_t callback,
-    const void * user_data) override
+  set_listener_callback(std::function<void(size_t, int)> callback) override
   {
-    rcl_ret_t ret = rcl_guard_condition_set_listener_callback(
-      &gc_, callback, user_data);
-
-    if (RCL_RET_OK != ret) {
-      throw std::runtime_error(std::string("Couldn't set guard condition callback"));
-    }
   }
 
 private: