diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 3885861647..129f7209af 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -37,7 +37,6 @@ #include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/visibility_control.hpp" -#include "rcutils/executor_event_types.h" #include "rcutils/logging_macros.h" #include "rmw/error_handling.h" diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index 2ca4c3d02f..58bfa0331d 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -21,7 +21,6 @@ #include "rcl/error_handling.h" #include "rmw/incompatible_qos_events_statuses.h" -#include "rcutils/executor_event_types.h" #include "rcutils/logging_macros.h" #include "rclcpp/exceptions.hpp" @@ -156,12 +155,16 @@ class QOSEventHandler : public QOSEventHandlerBase void * executor_context, ExecutorEventCallback executor_callback) const override { - rcl_event_set_events_executor_callback( + rcl_ret_t ret = rcl_event_set_events_executor_callback( executor_context, executor_callback, this, &event_handle_, false /* Discard previous events */); + + if (RCL_RET_OK != ret) { + throw std::runtime_error("Couldn't set EventsExecutor's callback to event"); + } } private: diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index c9e87384fa..4f8604ed90 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -204,9 +204,13 @@ ClientBase::set_events_executor_callback( const void * executor_context, ExecutorEventCallback executor_callback) const { - rcl_client_set_events_executor_callback( + rcl_ret_t ret = rcl_client_set_events_executor_callback( executor_context, executor_callback, this, client_handle_.get()); + + if (RCL_RET_OK != ret) { + throw std::runtime_error("Couldn't set the EventsExecutor's callback to client"); + } } diff --git a/rclcpp/src/rclcpp/executors/events_executor.cpp b/rclcpp/src/rclcpp/executors/events_executor.cpp index def8289c12..414d988f0e 100644 --- a/rclcpp/src/rclcpp/executors/events_executor.cpp +++ b/rclcpp/src/rclcpp/executors/events_executor.cpp @@ -31,21 +31,31 @@ EventsExecutor::EventsExecutor( timers_manager_ = std::make_shared(context_); entities_collector_ = std::make_shared(this, timers_manager_); + rcl_ret_t ret; + // Set the global ctrl-c guard condition callback - rcl_guard_condition_set_events_executor_callback( + ret = rcl_guard_condition_set_events_executor_callback( this, &EventsExecutor::push_event, entities_collector_.get(), options.context->get_interrupt_guard_condition(&wait_set_), false /* Discard previous events */); + if (ret != RCL_RET_OK) { + throw std::runtime_error("Couldn't set ctrl-c guard condition callback"); + } + // Set the executor interrupt guard condition callback - rcl_guard_condition_set_events_executor_callback( + ret = rcl_guard_condition_set_events_executor_callback( this, &EventsExecutor::push_event, entities_collector_.get(), &interrupt_guard_condition_, false /* Discard previous events */); + + if (ret != RCL_RET_OK) { + throw std::runtime_error("Couldn't set interrupt guard condition callback"); + } } EventsExecutor::~EventsExecutor() {} diff --git a/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp index eb821c4bb4..8fe18b4f1b 100644 --- a/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp @@ -151,14 +151,18 @@ EventsExecutorEntitiesCollector::set_entities_callbacks( }); } - // Set an event callback for the node's guard condition, so if new entities are added + // Set an event callback for the node's notify guard condition, so if new entities are added // or remove to this node we will receive an event. - rcl_guard_condition_set_events_executor_callback( + rcl_ret_t ret = rcl_guard_condition_set_events_executor_callback( associated_executor_, &EventsExecutor::push_event, this, - node->get_notify_guard_condition(), + node_ptr->get_notify_guard_condition(), false /* Discard previous events */); + + if (ret != RCL_RET_OK) { + throw std::runtime_error("Couldn't set node guard condition callback"); + } } void @@ -213,10 +217,14 @@ EventsExecutorEntitiesCollector::unset_entities_callbacks( }); } - // Unset the event callback for the node's guard condition, to stop receiving events + // Unset the event callback for the node's notify guard condition, to stop receiving events // if entities are added or removed to this node. - rcl_guard_condition_set_events_executor_callback( + rcl_ret_t ret = rcl_guard_condition_set_events_executor_callback( nullptr, nullptr, nullptr, node->get_notify_guard_condition(), false); + + if (ret != RCL_RET_OK) { + throw std::runtime_error("Couldn't set node guard condition callback"); + } } diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index f23761e235..914582e4ed 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -90,9 +90,13 @@ ServiceBase::set_events_executor_callback( const void * executor_context, ExecutorEventCallback executor_callback) const { - rcl_service_set_events_executor_callback( + rcl_ret_t ret = rcl_service_set_events_executor_callback( executor_context, executor_callback, this, service_handle_.get()); + + if (RCL_RET_OK != ret) { + throw std::runtime_error("Couldn't set the EventsExecutor's callback to service"); + } } diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 1cab2fcdd8..345bbf7181 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -294,9 +294,13 @@ SubscriptionBase::set_events_executor_callback( const void * executor_context, ExecutorEventCallback executor_callback) const { - rcl_subscription_set_events_executor_callback( + rcl_ret_t ret = rcl_subscription_set_events_executor_callback( executor_context, executor_callback, this, subscription_handle_.get()); + + if (RCL_RET_OK != ret) { + throw std::runtime_error("Couldn't set the EventsExecutor's callback to subscription"); + } } diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index 673a5d27c8..ae2bad93f4 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -44,10 +44,14 @@ SubscriptionIntraProcessBase::set_events_executor_callback( void * executor_context, ExecutorEventCallback executor_callback) const { - rcl_guard_condition_set_events_executor_callback( + rcl_ret_t ret = rcl_guard_condition_set_events_executor_callback( executor_context, executor_callback, this, &gc_, true /*Use previous events*/); + + if (RCL_RET_OK != ret) { + throw std::runtime_error(std::string("Couldn't set guard condition callback")); + } } diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index a4dfabad72..d9e34305bb 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -457,12 +457,16 @@ class TestWaitable : public rclcpp::Waitable void * executor_context, ExecutorEventCallback executor_callback) const override { - rcl_guard_condition_set_events_executor_callback( + rcl_ret_t ret = rcl_guard_condition_set_events_executor_callback( executor_context, executor_callback, this, &gc_, true /*Use previous events*/); + + if (RCL_RET_OK != ret) { + throw std::runtime_error(std::string("Couldn't set guard condition callback")); + } } private: