GuardCondition to have callback <WIP>

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

Author: Mauro Passerino

rclcpp/include/rclcpp/executors/events_executor_entities_collector.hpp

@@ -215,7 +215,10 @@ class EventsExecutorEntitiesCollector final
   unset_guard_condition_callback(const rclcpp::GuardCondition * guard_condition);
 
   std::function<void(size_t)>
-  create_entity_callback(void * entity_id, ExecutorEventType type);
+  create_entity_callback(void * exec_entity_id, ExecutorEventType type);
+
+  std::function<void(size_t, int)>
+  create_waitable_callback(void * waitable_id);
 
   /// Return true if the node belongs to the collector
   /**

rclcpp/include/rclcpp/executors/events_executor_event_types.hpp

@@ -30,7 +30,8 @@ enum ExecutorEventType
 
 struct ExecutorEvent
 {
-  const void * entity_id;
+  const void * exec_entity_id;
+  int gen_entity_id;
   ExecutorEventType type;
   size_t num_events;
 };

rclcpp/include/rclcpp/executors/events_executor_notify_waitable.hpp

@@ -60,13 +60,12 @@ class EventsExecutorNotifyWaitable final : public EventWaitable
 
   RCLCPP_PUBLIC
   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
   {
-    for (auto gc : notify_guard_conditions_) {
-      // gc->set_listener_callback();
-    }
+    (void)callback;
+    // for (auto gc : notify_guard_conditions_) {
+    //   gc->set_listener_callback();
+    // }
   }
 
   RCLCPP_PUBLIC

rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp

@@ -79,9 +79,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable
 
   RCLCPP_PUBLIC
   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;
 
 protected:
   std::recursive_mutex reentrant_mutex_;

rclcpp/include/rclcpp/qos_event.hpp

@@ -171,9 +171,7 @@ class QOSEventHandlerBase : public Waitable
 
   RCLCPP_PUBLIC
   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;
 
 protected:
   RCLCPP_PUBLIC

rclcpp/include/rclcpp/waitable.hpp

@@ -207,9 +207,7 @@ class Waitable
   RCLCPP_PUBLIC
   virtual
   void
-  set_listener_callback(
-    rmw_listener_callback_t callback,
-    const void * user_data);
+  set_listener_callback(std::function<void(size_t, int)> callback);
 
 private:
   std::atomic<bool> in_use_by_wait_set_{false};

rclcpp/src/rclcpp/executors/events_executor.cpp

@@ -253,7 +253,7 @@ EventsExecutor::execute_event(const ExecutorEvent & event)
   switch (event.type) {
     case SUBSCRIPTION_EVENT:
       {
-        auto subscription = entities_collector_->get_subscription(event.entity_id);
+        auto subscription = entities_collector_->get_subscription(event.exec_entity_id);
 
         if (subscription) {
           for (size_t i = 0; i < event.num_events; i++) {
@@ -265,7 +265,7 @@ EventsExecutor::execute_event(const ExecutorEvent & event)
 
     case SERVICE_EVENT:
       {
-        auto service = entities_collector_->get_service(event.entity_id);
+        auto service = entities_collector_->get_service(event.exec_entity_id);
 
         if (service) {
           for (size_t i = 0; i < event.num_events; i++) {
@@ -277,7 +277,7 @@ EventsExecutor::execute_event(const ExecutorEvent & event)
 
     case CLIENT_EVENT:
       {
-        auto client = entities_collector_->get_client(event.entity_id);
+        auto client = entities_collector_->get_client(event.exec_entity_id);
 
         if (client) {
           for (size_t i = 0; i < event.num_events; i++) {
@@ -289,7 +289,7 @@ EventsExecutor::execute_event(const ExecutorEvent & event)
 
     case WAITABLE_EVENT:
       {
-        auto waitable = entities_collector_->get_waitable(event.entity_id);
+        auto waitable = entities_collector_->get_waitable(event.exec_entity_id);
 
         if (waitable) {
           for (size_t i = 0; i < event.num_events; i++) {

rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp

@@ -478,7 +478,7 @@ void
 EventsExecutorEntitiesCollector::set_guard_condition_callback(
   const rclcpp::GuardCondition * guard_condition)
 {
-  // create_entity_callback(this, WAITABLE_EVENT);
+  create_waitable_callback(this);
 }
 
 void
@@ -573,10 +573,25 @@ EventsExecutorEntitiesCollector::add_waitable(rclcpp::Waitable::SharedPtr waitab
 
 std::function<void(size_t)>
 EventsExecutorEntitiesCollector::create_entity_callback(
-  void * entity_id, ExecutorEventType event_type)
+  void * exec_entity_id, ExecutorEventType event_type)
 {
-  return [this, entity_id, event_type](size_t num_events) {
-    ExecutorEvent event = {entity_id, event_type, num_events};
+  return [this, exec_entity_id, event_type](size_t num_events) {
+    ExecutorEvent event = {exec_entity_id, -1, 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();
+  };
+}
+
+std::function<void(size_t, int)>
+EventsExecutorEntitiesCollector::create_waitable_callback(void * exec_entity_id)
+{
+  return [this, exec_entity_id](size_t num_events, int gen_entity_id) {
+    ExecutorEvent event = {exec_entity_id, gen_entity_id, WAITABLE_EVENT, num_events};
     // Event queue mutex scope
     {
       std::unique_lock<std::mutex> lock(associated_executor_->push_mutex_);

rclcpp/test/rclcpp/executors/test_events_queue.cpp

@@ -60,6 +60,6 @@ TEST(TestEventsQueue, SimpleQueueTest)
   rclcpp::executors::ExecutorEvent front_event = simple_queue->front();
 
   // The events should be equal
-  EXPECT_EQ(push_event.entity_id, front_event.entity_id);
+  EXPECT_EQ(push_event.exec_entity_id, front_event.exec_entity_id);
   EXPECT_EQ(push_event.type, front_event.type);
 }