Some fixes

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

Author: Mauro Passerino

rclcpp/include/rclcpp/executors/events_executor_notify_waitable.hpp

@@ -60,7 +60,7 @@ class EventsExecutorNotifyWaitable final : public EventWaitable
 
   RCLCPP_PUBLIC
   void
-  set_listener_callback(std::function<void(size_t, int)> callback) override
+  set_on_ready_callback(std::function<void(size_t, int)> callback) override
   {
     (void)callback;
     // for (auto gc : notify_guard_conditions_) {

rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp

@@ -66,36 +66,13 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase
     const std::string & topic_name,
     rmw_qos_profile_t qos_profile,
     rclcpp::IntraProcessBufferType buffer_type)
-  : SubscriptionIntraProcessBase(topic_name, qos_profile)
+  : SubscriptionIntraProcessBase(context, topic_name, qos_profile)
   {
     // Create the intra-process buffer.
     buffer_ = rclcpp::experimental::create_intra_process_buffer<MessageT, Alloc, Deleter>(
       buffer_type,
       qos_profile,
       allocator);
-
-    // Create the guard condition.
-    rcl_guard_condition_options_t guard_condition_options =
-      rcl_guard_condition_get_default_options();
-
-    gc_ = rcl_get_zero_initialized_guard_condition();
-    rcl_ret_t ret = rcl_guard_condition_init(
-      &gc_, context->get_rcl_context().get(), guard_condition_options);
-
-    if (RCL_RET_OK != ret) {
-      throw std::runtime_error(
-              "SubscriptionIntraProcessBuffer init error initializing guard condition");
-    }
-  }
-
-  virtual ~SubscriptionIntraProcessBuffer()
-  {
-    if (rcl_guard_condition_fini(&gc_) != RCL_RET_OK) {
-      RCUTILS_LOG_ERROR_NAMED(
-        "rclcpp",
-        "Failed to destroy guard condition: %s",
-        rcutils_get_error_string().str);
-    }
   }
 
   bool
@@ -135,15 +112,15 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase
     if (on_new_message_callback_) {
       on_new_message_callback_(1);
     } else {
+      gc_.trigger();
       unread_count_++;
     }
   }
 
   void
   trigger_guard_condition()
   {
-    rcl_ret_t ret = rcl_trigger_guard_condition(&gc_);
-    (void)ret;
+    gc_.trigger();
   }
 
   BufferUniquePtr buffer_;

rclcpp/src/rclcpp/executor.cpp

@@ -76,12 +76,6 @@ Executor::Executor(const rclcpp::ExecutorOptions & options)
       "rclcpp",
       "failed to create wait set: %s", rcl_get_error_string().str);
     rcl_reset_error();
-    if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) {
-      RCUTILS_LOG_ERROR_NAMED(
-        "rclcpp",
-        "failed to destroy guard condition: %s", rcl_get_error_string().str);
-      rcl_reset_error();
-    }
     throw_from_rcl_error(ret, "Failed to create wait set in Executor constructor");
   }
 }
@@ -214,17 +208,14 @@ Executor::add_callback_group_to_map(
   // Also add to the map that contains all callback groups
   weak_groups_to_nodes_.insert(std::make_pair(weak_group_ptr, node_ptr));
   if (is_new_node) {
-    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr);
-    weak_nodes_to_guard_conditions_[node_weak_ptr] = node_ptr->get_notify_guard_condition();
+    const auto & node_gc = node_ptr->get_notify_rclcpp_guard_condition();
+    weak_nodes_to_guard_conditions_[node_ptr] = node_gc;
     if (notify) {
       // Interrupt waiting to handle new node
-      rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_);
-      if (ret != RCL_RET_OK) {
-        throw_from_rcl_error(ret, "Failed to trigger guard condition on callback group add");
-      }
+      interrupt_guard_condition_.trigger();
     }
     // Add the node's notify condition to the guard condition handles
-    memory_strategy_->add_guard_condition(node_ptr->get_notify_guard_condition());
+    memory_strategy_->add_guard_condition(node_gc);
   }
 }
 
@@ -291,15 +282,11 @@ Executor::remove_callback_group_from_map(
   if (!has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) &&
     !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_))
   {
-    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr);
-    weak_nodes_to_guard_conditions_.erase(node_weak_ptr);
+    weak_nodes_to_guard_conditions_.erase(node_ptr);
     if (notify) {
-      rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_);
-      if (ret != RCL_RET_OK) {
-        throw_from_rcl_error(ret, "Failed to trigger guard condition on callback group remove");
-      }
+      interrupt_guard_condition_.trigger();
     }
-    memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition());
+    memory_strategy_->remove_guard_condition(node_ptr->get_notify_rclcpp_guard_condition());
   }
 }
 
@@ -470,10 +457,7 @@ void
 Executor::cancel()
 {
   spinning.store(false);
-  rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_);
-  if (ret != RCL_RET_OK) {
-    throw_from_rcl_error(ret, "Failed to trigger guard condition in cancel");
-  }
+  interrupt_guard_condition_.trigger();
 }
 
 void

rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp

@@ -267,7 +267,7 @@ EventsExecutorEntitiesCollector::set_callback_group_entities_callbacks(
       if (waitable) {
         weak_waitables_map_.emplace(waitable.get(), waitable);
 
-        waitable->set_listener_callback(
+        waitable->set_on_ready_callback(
           create_waitable_callback(waitable.get()));
       }
       return false;
@@ -291,31 +291,31 @@ EventsExecutorEntitiesCollector::unset_callback_group_entities_callbacks(
   group->find_subscription_ptrs_if(
     [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) {
       if (subscription) {
-        subscription->set_on_new_message_callback(nullptr);
+        subscription->clear_on_new_message_callback();
         weak_subscriptions_map_.erase(subscription.get());
       }
       return false;
     });
   group->find_service_ptrs_if(
     [this](const rclcpp::ServiceBase::SharedPtr & service) {
       if (service) {
-        service->set_on_new_request_callback(nullptr);
+        service->clear_on_new_request_callback();
         weak_services_map_.erase(service.get());
       }
       return false;
     });
   group->find_client_ptrs_if(
     [this](const rclcpp::ClientBase::SharedPtr & client) {
       if (client) {
-        client->set_on_new_response_callback(nullptr);
+        client->clear_on_new_response_callback();
         weak_clients_map_.erase(client.get());
       }
       return false;
     });
   group->find_waitable_ptrs_if(
     [this](const rclcpp::Waitable::SharedPtr & waitable) {
       if (waitable) {
-        waitable->set_listener_callback(nullptr);
+        waitable->clear_on_ready_callback();
         weak_waitables_map_.erase(waitable.get());
       }
       return false;
@@ -583,7 +583,7 @@ EventsExecutorEntitiesCollector::add_waitable(rclcpp::Waitable::SharedPtr waitab
 {
   weak_waitables_map_.emplace(waitable.get(), waitable);
 
-  waitable->set_listener_callback(
+  waitable->set_on_ready_callback(
     create_waitable_callback(waitable.get()));
 }
 

rclcpp/src/rclcpp/qos_event.cpp

@@ -84,11 +84,4 @@ QOSEventHandlerBase::set_on_new_event_callback(
   }
 }
 
-void
-QOSEventHandlerBase::set_listener_callback(std::function<void(size_t, int)> callback)
-{
-  // set_on_new_event_callback(callback);
-}
-
-
 }  // namespace rclcpp

rclcpp/src/rclcpp/subscription_intra_process_base.cpp

@@ -39,22 +39,3 @@ SubscriptionIntraProcessBase::get_actual_qos() const
 {
   return qos_profile_;
 }
-
-void
-SubscriptionIntraProcessBase::set_listener_callback(std::function<void(size_t, int)> callback)
-{
-  if (callback) {
-    on_new_message_callback_ = std::bind(callback, std::placeholders::_1, -1);
-      if (unread_count_) {
-        if (unread_count_ < qos_profile_.depth) {
-          on_new_message_callback_(unread_count_);
-        } else {
-          on_new_message_callback_(qos_profile_.depth);
-        }
-        unread_count_ = 0;
-      }
-    return;
-  }
-
-  on_new_message_callback_ = nullptr;
-}

rclcpp/test/rclcpp/executors/test_events_queue.cpp

@@ -52,8 +52,8 @@ TEST(TestEventsQueue, SimpleQueueTest)
   EXPECT_TRUE(simple_queue->empty());
 
   // Lets push an event into the queue and get it back
-  rclcpp::executors::ExecutorEvent push_event = {simple_queue.get(),
-    rclcpp::executors::ExecutorEventType::SUBSCRIPTION_EVENT};
+  rclcpp::executors::ExecutorEvent push_event = {simple_queue.get(), 0,
+    rclcpp::executors::ExecutorEventType::SUBSCRIPTION_EVENT, 1};
 
   simple_queue->push(push_event);
 

rclcpp/test/rclcpp/executors/test_executors.cpp

@@ -404,7 +404,7 @@ class TestWaitable : public rclcpp::Waitable
   }
 
   void
-  set_listener_callback(std::function<void(size_t, int)> callback) override
+  set_on_ready_callback(std::function<void(size_t, int)> callback) override
   {
   }