diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 1a484b3..d3e9051 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -156,9 +156,11 @@ protected: { std::shared_ptr message = subscription->create_message(); bool taken = false; - rmw_take(subscription->subscription_handle_, message.get(), &taken); - if (taken) { - subscription->handle_message(message); + rmw_ret_t status = rmw_take(subscription->subscription_handle_, message.get(), &taken); + if (status == RMW_RET_OK) { + if (taken) { + subscription->handle_message(message); + } } else { std::cout << "[rclcpp::error] take failed for subscription on topic: " << subscription->get_topic_name() << std::endl; @@ -179,13 +181,15 @@ protected: std::shared_ptr request_header = service->create_request_header(); std::shared_ptr request = service->create_request(); bool taken = false; - rmw_take_request( + rmw_ret_t status = rmw_take_request( service->service_handle_, request_header.get(), request.get(), &taken); - if (taken) { - service->handle_request(request_header, request); + if (status == RMW_RET_OK) { + if (taken) { + service->handle_request(request_header, request); + } } else { std::cout << "[rclcpp::error] take failed for service on service: " << service->get_service_name() << std::endl; diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index f0bbafd..f4b5962 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -126,7 +126,8 @@ public: std::string topic_name, size_t queue_size, std::function &)> callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr, + bool ignore_local_publications = false); /* Create a timer. */ rclcpp::timer::WallTimer::SharedPtr diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 61970e4..161f3f9 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -106,12 +106,13 @@ Node::create_subscription( std::string topic_name, size_t queue_size, std::function &)> callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group) + rclcpp::callback_group::CallbackGroup::SharedPtr group, + bool ignore_local_publications) { using rosidl_generator_cpp::get_message_type_support_handle; auto type_support_handle = get_message_type_support_handle(); rmw_subscription_t * subscriber_handle = rmw_create_subscription( - node_handle_, type_support_handle, topic_name.c_str(), queue_size); + node_handle_, type_support_handle, topic_name.c_str(), queue_size, ignore_local_publications); if (!subscriber_handle) { throw std::runtime_error( std::string("could not create subscription: ") + @@ -123,6 +124,7 @@ Node::create_subscription( auto sub = Subscription::make_shared( subscriber_handle, topic_name, + ignore_local_publications, callback); auto sub_base_ptr = std::dynamic_pointer_cast(sub); if (group) { diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 8c13e7a..b93dbd2 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -44,8 +44,11 @@ public: SubscriptionBase( rmw_subscription_t * subscription_handle, - std::string & topic_name) - : subscription_handle_(subscription_handle), topic_name_(topic_name) + std::string & topic_name, + bool ignore_local_publications) + : subscription_handle_(subscription_handle), + topic_name_(topic_name), + ignore_local_publications_(ignore_local_publications) {} std::string get_topic_name() @@ -61,6 +64,7 @@ private: rmw_subscription_t * subscription_handle_; std::string topic_name_; + bool ignore_local_publications_; }; @@ -74,8 +78,10 @@ public: Subscription( rmw_subscription_t * subscription_handle, std::string & topic_name, + bool ignore_local_publications, CallbackType callback) - : SubscriptionBase(subscription_handle, topic_name), callback_(callback) + : SubscriptionBase(subscription_handle, topic_name, ignore_local_publications), + callback_(callback) {} std::shared_ptr create_message()