From 3b86956f38d966cb8ccfcac99f40958fc7f7afba Mon Sep 17 00:00:00 2001 From: Miaofei Date: Mon, 15 Apr 2019 15:13:16 -0700 Subject: [PATCH 1/8] implement deadline and liveliness qos callbacks Signed-off-by: Miaofei --- rclcpp/CMakeLists.txt | 1 + rclcpp/include/rclcpp/callback_group.hpp | 5 + rclcpp/include/rclcpp/create_publisher.hpp | 8 +- rclcpp/include/rclcpp/create_subscription.hpp | 6 +- rclcpp/include/rclcpp/memory_strategy.hpp | 1 + rclcpp/include/rclcpp/node.hpp | 10 ++ rclcpp/include/rclcpp/node_impl.hpp | 3 + .../rclcpp/node_interfaces/node_base.hpp | 5 + .../node_interfaces/node_base_interface.hpp | 6 + .../rclcpp/node_interfaces/node_topics.hpp | 3 +- .../node_interfaces/node_topics_interface.hpp | 3 +- rclcpp/include/rclcpp/parameter_client.hpp | 1 + rclcpp/include/rclcpp/publisher.hpp | 49 ++++++- rclcpp/include/rclcpp/publisher_factory.hpp | 9 +- rclcpp/include/rclcpp/publisher_options.hpp | 6 +- rclcpp/include/rclcpp/qos_event.hpp | 124 ++++++++++++++++++ .../strategies/allocator_memory_strategy.hpp | 9 ++ rclcpp/include/rclcpp/subscription.hpp | 47 ++++++- .../include/rclcpp/subscription_factory.hpp | 4 +- .../include/rclcpp/subscription_options.hpp | 7 +- rclcpp/include/rclcpp/waitable.hpp | 11 ++ rclcpp/src/rclcpp/executor.cpp | 6 +- rclcpp/src/rclcpp/graph_listener.cpp | 3 +- rclcpp/src/rclcpp/node.cpp | 6 + .../src/rclcpp/node_interfaces/node_base.cpp | 6 + .../node_interfaces/node_parameters.cpp | 2 + .../rclcpp/node_interfaces/node_topics.cpp | 27 +++- rclcpp/src/rclcpp/publisher.cpp | 16 +++ rclcpp/src/rclcpp/qos_event.cpp | 59 +++++++++ rclcpp/src/rclcpp/subscription.cpp | 7 + rclcpp/src/rclcpp/time_source.cpp | 1 + rclcpp/src/rclcpp/waitable.cpp | 6 + .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 6 + .../rclcpp_lifecycle/lifecycle_publisher.hpp | 3 +- 34 files changed, 438 insertions(+), 28 deletions(-) create mode 100644 rclcpp/include/rclcpp/qos_event.hpp create mode 100644 rclcpp/src/rclcpp/qos_event.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 0de4deb495..d0db972892 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -65,6 +65,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/parameter_map.cpp src/rclcpp/parameter_service.cpp src/rclcpp/publisher.cpp + src/rclcpp/qos_event.cpp src/rclcpp/service.cpp src/rclcpp/signal_handler.cpp src/rclcpp/subscription.cpp diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index c53bffbd7d..c4d17c44ca 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -22,6 +22,7 @@ #include "rclcpp/client.hpp" #include "rclcpp/service.hpp" +#include "rclcpp/publisher.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/timer.hpp" #include "rclcpp/visibility_control.hpp" @@ -92,6 +93,10 @@ class CallbackGroup protected: RCLCPP_DISABLE_COPY(CallbackGroup) + RCLCPP_PUBLIC + void + add_publisher(const rclcpp::PublisherBase::SharedPtr publisher_ptr); + RCLCPP_PUBLIC void add_subscription(const rclcpp::SubscriptionBase::SharedPtr subscription_ptr); diff --git a/rclcpp/include/rclcpp/create_publisher.hpp b/rclcpp/include/rclcpp/create_publisher.hpp index f136f33653..e21b00687b 100644 --- a/rclcpp/include/rclcpp/create_publisher.hpp +++ b/rclcpp/include/rclcpp/create_publisher.hpp @@ -31,6 +31,8 @@ create_publisher( rclcpp::node_interfaces::NodeTopicsInterface * node_topics, const std::string & topic_name, const rmw_qos_profile_t & qos_profile, + const PublisherEventCallbacks & event_callbacks, + rclcpp::callback_group::CallbackGroup::SharedPtr group, bool use_intra_process_comms, std::shared_ptr allocator) { @@ -39,10 +41,12 @@ create_publisher( auto pub = node_topics->create_publisher( topic_name, - rclcpp::create_publisher_factory(allocator), + rclcpp::create_publisher_factory(event_callbacks, allocator), publisher_options, use_intra_process_comms); - node_topics->add_publisher(pub); + + node_topics->add_publisher(pub, group); + return std::dynamic_pointer_cast(pub); } diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index a4ea581092..e765d376e2 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -38,6 +38,7 @@ create_subscription( const std::string & topic_name, CallbackT && callback, const rmw_qos_profile_t & qos_profile, + const SubscriptionEventCallbacks & event_callbacks, rclcpp::callback_group::CallbackGroup::SharedPtr group, bool ignore_local_publications, bool use_intra_process_comms, @@ -52,7 +53,10 @@ create_subscription( auto factory = rclcpp::create_subscription_factory ( - std::forward(callback), msg_mem_strat, allocator); + std::forward(callback), + event_callbacks, + msg_mem_strat, + allocator); auto sub = node_topics->create_subscription( topic_name, diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index 416890ed6c..70db12237a 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -51,6 +51,7 @@ class RCLCPP_PUBLIC MemoryStrategy virtual size_t number_of_ready_subscriptions() const = 0; virtual size_t number_of_ready_services() const = 0; virtual size_t number_of_ready_clients() const = 0; + virtual size_t number_of_ready_events() const = 0; virtual size_t number_of_ready_timers() const = 0; virtual size_t number_of_guard_conditions() const = 0; virtual size_t number_of_waitables() const = 0; diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 62e8a5ca54..c5aa276270 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -1120,6 +1120,16 @@ class Node : public std::enable_shared_from_this const NodeOptions & get_node_options() const; + /// Manually assert that this Node is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE) + /** + * If the rmw Liveliness policy is set to RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE, the creator + * of this node may manually call `assert_liveliness` at some point in time to signal to the rest + * of the system that this Node is still alive. + */ + RCLCPP_PUBLIC + bool + assert_liveliness() const; + protected: /// Construct a sub-node, which will extend the namespace of all entities created with it. /** diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 78fdd8de95..a3eb298ccc 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -97,6 +97,8 @@ Node::create_publisher( this->node_topics_.get(), extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), qos_profile, + options.event_callbacks, + options.callback_group, use_intra_process, allocator); } @@ -181,6 +183,7 @@ Node::create_subscription( extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), std::forward(callback), qos_profile, + options.event_callbacks, options.callback_group, options.ignore_local_publications, use_intra_process, diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index b7470269f4..77379c2858 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -86,6 +86,11 @@ class NodeBase : public NodeBaseInterface std::shared_ptr get_shared_rcl_node_handle() const; + RCLCPP_PUBLIC + virtual + bool + assert_liveliness() const; + RCLCPP_PUBLIC virtual rclcpp::callback_group::CallbackGroup::SharedPtr diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp index eb376c0353..ee562e41f8 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp @@ -102,6 +102,12 @@ class NodeBaseInterface std::shared_ptr get_shared_rcl_node_handle() const = 0; + /// Manually assert that this Node is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE) + RCLCPP_PUBLIC + virtual + bool + assert_liveliness() const = 0; + /// Create and return a callback group. RCLCPP_PUBLIC virtual diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp index 31cf62e54c..40beb3aacb 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp @@ -58,7 +58,8 @@ class NodeTopics : public NodeTopicsInterface virtual void add_publisher( - rclcpp::PublisherBase::SharedPtr publisher); + rclcpp::PublisherBase::SharedPtr publisher, + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group); RCLCPP_PUBLIC virtual diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp index b3d46ff334..d888e3ccd2 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp @@ -57,7 +57,8 @@ class NodeTopicsInterface virtual void add_publisher( - rclcpp::PublisherBase::SharedPtr publisher) = 0; + rclcpp::PublisherBase::SharedPtr publisher, + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0; RCLCPP_PUBLIC virtual diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index 879f261836..6b68a793fa 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -127,6 +127,7 @@ class AsyncParametersClient "parameter_events", std::forward(callback), rmw_qos_profile_default, + SubscriptionEventCallbacks(), nullptr, // group, false, // ignore_local_publications, false, // use_intra_process_comms_, diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 3bc5814112..9a27a04c25 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -31,17 +32,21 @@ #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/allocator/allocator_deleter.hpp" +#include "rclcpp/exceptions.hpp" #include "rclcpp/macros.hpp" -#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/qos_event.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp { -// Forward declaration is used for friend statement. namespace node_interfaces { +// NOTE(emersonknapp) Forward declaration avoids including node_base_interface.hpp which causes +// circular inclusion from callback_group.hpp +class NodeBaseInterface; +// Forward declaration is used for friend statement. class NodeTopicsInterface; } @@ -116,6 +121,10 @@ class PublisherBase const rcl_publisher_t * get_publisher_handle() const; + RCLCPP_PUBLIC + const std::vector> & + get_event_handlers() const; + /// Get subscription count /** \return The number of subscriptions. */ RCLCPP_PUBLIC @@ -128,6 +137,16 @@ class PublisherBase size_t get_intra_process_subscription_count() const; + /// Manually assert that this Publisher is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC) + /** + * If the rmw Liveliness policy is set to RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC, the creator + * of this publisher may manually call `assert_liveliness` at some point in time to signal to the + * rest of the system that this Node is still alive. + */ + RCLCPP_PUBLIC + bool + assert_liveliness() const; + /// Get the actual QoS settings, after the defaults have been determined. /** * The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT @@ -176,11 +195,27 @@ class PublisherBase const rcl_publisher_options_t & intra_process_options); protected: + template + void + add_event_handler( + const EventCallbackT & callback, + const rcl_publisher_event_type_t event_type) + { + auto handler = std::make_shared>( + callback, + rcl_publisher_event_init, + &publisher_handle_, + event_type); + event_handlers_.emplace_back(handler); + } + std::shared_ptr rcl_node_handle_; rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher(); rcl_publisher_t intra_process_publisher_handle_ = rcl_get_zero_initialized_publisher(); + std::vector> event_handlers_; + using IntraProcessManagerWeakPtr = std::weak_ptr; bool intra_process_is_enabled_; @@ -208,6 +243,7 @@ class Publisher : public PublisherBase rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic, const rcl_publisher_options_t & publisher_options, + const PublisherEventCallbacks & event_callbacks, const std::shared_ptr & allocator) : PublisherBase( node_base, @@ -217,6 +253,15 @@ class Publisher : public PublisherBase message_allocator_(allocator) { allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get()); + + if (event_callbacks.deadline_callback) { + this->add_event_handler(event_callbacks.deadline_callback, + RCL_PUBLISHER_OFFERED_DEADLINE_MISSED); + } + if (event_callbacks.liveliness_callback) { + this->add_event_handler(event_callbacks.liveliness_callback, + RCL_PUBLISHER_LIVELINESS_LOST); + } } virtual ~Publisher() diff --git a/rclcpp/include/rclcpp/publisher_factory.hpp b/rclcpp/include/rclcpp/publisher_factory.hpp index 4044d1a6cc..98ab04dc6a 100644 --- a/rclcpp/include/rclcpp/publisher_factory.hpp +++ b/rclcpp/include/rclcpp/publisher_factory.hpp @@ -75,13 +75,15 @@ struct PublisherFactory /// Return a PublisherFactory with functions setup for creating a PublisherT. template PublisherFactory -create_publisher_factory(std::shared_ptr allocator) +create_publisher_factory( + const PublisherEventCallbacks & event_callbacks, + std::shared_ptr allocator) { PublisherFactory factory; // factory function that creates a MessageT specific PublisherT factory.create_typed_publisher = - [allocator]( + [event_callbacks, allocator]( rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic_name, rcl_publisher_options_t & publisher_options) -> std::shared_ptr @@ -89,7 +91,8 @@ create_publisher_factory(std::shared_ptr allocator) auto message_alloc = std::make_shared(*allocator.get()); publisher_options.allocator = allocator::get_rcl_allocator(*message_alloc.get()); - return std::make_shared(node_base, topic_name, publisher_options, message_alloc); + return std::make_shared(node_base, topic_name, publisher_options, + event_callbacks, message_alloc); }; // function to add a publisher to the intra process manager diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp index 86ce4435ef..e73c6886bc 100644 --- a/rclcpp/include/rclcpp/publisher_options.hpp +++ b/rclcpp/include/rclcpp/publisher_options.hpp @@ -19,6 +19,8 @@ #include #include +#include "rclcpp/qos_event.hpp" +#include "rclcpp/callback_group.hpp" #include "rclcpp/intra_process_setting.hpp" #include "rclcpp/visibility_control.hpp" @@ -29,10 +31,12 @@ namespace rclcpp template struct PublisherOptionsWithAllocator { + PublisherEventCallbacks event_callbacks; /// The quality of service profile to pass on to the rmw implementation. rmw_qos_profile_t qos_profile = rmw_qos_profile_default; + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group; /// Optional custom allocator. - std::shared_ptr allocator = nullptr; + std::shared_ptr allocator; /// Setting to explicitly set intraprocess communications. IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault; }; diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp new file mode 100644 index 0000000000..e7a259c83f --- /dev/null +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -0,0 +1,124 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__QOS_EVENT_HPP_ +#define RCLCPP__QOS_EVENT_HPP_ + +#include + +#include "rcl/error_handling.h" + +#include "rcutils/logging_macros.h" + +#include "rclcpp/waitable.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/function_traits.hpp" + + +namespace rclcpp +{ + +using QOSDeadlineRequestedInfo = rmw_requested_deadline_missed_status_t; +using QOSDeadlineOfferedInfo = rmw_offered_deadline_missed_status_t; +using QOSLivelinessChangedInfo = rmw_liveliness_changed_status_t; +using QOSLivelinessLostInfo = rmw_liveliness_lost_status_t; + +using QOSDeadlineRequestedCallbackType = std::function; +using QOSDeadlineOfferedCallbackType = std::function; +using QOSLivelinessChangedCallbackType = std::function; +using QOSLivelinessLostCallbackType = std::function; + +/// Contains callbacks for various types of events a Publisher can receive from the middleware +struct PublisherEventCallbacks +{ + QOSDeadlineOfferedCallbackType deadline_callback; + QOSLivelinessLostCallbackType liveliness_callback; +}; + +/// Contains callbacks for non-message events that a Subscriber can receive from the middleware +struct SubscriptionEventCallbacks +{ + QOSDeadlineRequestedCallbackType deadline_callback; + QOSLivelinessChangedCallbackType liveliness_callback; +}; + +class QOSEventHandlerBase : public Waitable +{ +public: + virtual ~QOSEventHandlerBase(); + + /// Get the number of ready events + size_t + get_number_of_ready_events() override; + + /// Add the Waitable to a wait set. + bool + add_to_wait_set(rcl_wait_set_t * wait_set) override; + + /// Check if the Waitable is ready. + bool + is_ready(rcl_wait_set_t * wait_set) override; + +protected: + rcl_event_t event_handle_; + size_t wait_set_event_index_; +}; + + +template +class QOSEventHandler : public QOSEventHandlerBase +{ +public: + template + QOSEventHandler( + const EventCallbackT & callback, + InitFuncT init_func, + ParentHandleT parent_handle, + EventTypeEnum event_type) + : event_callback_(callback) + { + event_handle_ = rcl_get_zero_initialized_event(); + rcl_ret_t ret = init_func(&event_handle_, parent_handle, event_type); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create event"); + } + } + + /// Execute any entities of the Waitable that are ready. + void + execute() override + { + EventCallbackInfoT callback_info; + + rcl_ret_t ret = rcl_take_event(&event_handle_, &callback_info); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't take event info: %s", rcl_get_error_string().str); + return; + } + + event_callback_(callback_info); + } + +private: + using EventCallbackInfoT = typename std::remove_reference::template argument_type<0>>::type; + + EventCallbackT event_callback_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__QOS_EVENT_HPP_ diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index ca1d2fba3c..8bf54656bc 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -430,6 +430,15 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy return number_of_services; } + size_t number_of_ready_events() const + { + size_t number_of_events = 0; + for (auto waitable : waitable_handles_) { + number_of_events += waitable->get_number_of_ready_events(); + } + return number_of_events; + } + size_t number_of_ready_clients() const { size_t number_of_clients = client_handles_.size(); diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 2a2a3e4eef..a9fe21bb81 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -34,9 +35,11 @@ #include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/message_memory_strategy.hpp" +#include "rclcpp/qos_event.hpp" #include "rclcpp/subscription_traits.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/waitable.hpp" namespace rclcpp { @@ -99,6 +102,10 @@ class SubscriptionBase virtual const std::shared_ptr get_intra_process_subscription_handle() const; + RCLCPP_PUBLIC + const std::vector> & + get_event_handlers() const; + /// Borrow a new message. /** \return Shared pointer to the fresh message. */ virtual std::shared_ptr @@ -145,13 +152,35 @@ class SubscriptionBase get_publisher_count() const; protected: - std::shared_ptr intra_process_subscription_handle_; - std::shared_ptr subscription_handle_; - std::shared_ptr node_handle_; + template + void + add_event_handler( + const EventCallbackT & callback, + const rcl_subscription_event_type_t event_type) + { + auto handler = std::make_shared>( + callback, + rcl_subscription_event_init, + get_subscription_handle().get(), + event_type); + event_handlers_.emplace_back(handler); + } using IntraProcessManagerWeakPtr = std::weak_ptr; + + std::shared_ptr node_handle_; + + std::shared_ptr subscription_handle_; + size_t wait_set_subscription_index_; + bool subscription_ready_; + + std::vector> event_handlers_; + bool use_intra_process_; + std::shared_ptr intra_process_subscription_handle_; + size_t wait_set_intra_process_subscription_index_; + bool intra_process_subscription_ready_; IntraProcessManagerWeakPtr weak_ipm_; uint64_t intra_process_subscription_id_; @@ -195,6 +224,7 @@ class Subscription : public SubscriptionBase const std::string & topic_name, const rcl_subscription_options_t & subscription_options, AnySubscriptionCallback callback, + const SubscriptionEventCallbacks & event_callbacks, typename message_memory_strategy::MessageMemoryStrategy::SharedPtr memory_strategy = message_memory_strategy::MessageMemoryStrategy::create_default()) @@ -208,7 +238,16 @@ class Subscription : public SubscriptionBase message_memory_strategy_(memory_strategy), get_intra_process_message_callback_(nullptr), matches_any_intra_process_publishers_(nullptr) - {} + { + if (event_callbacks.deadline_callback) { + this->add_event_handler(event_callbacks.deadline_callback, + RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); + } + if (event_callbacks.liveliness_callback) { + this->add_event_handler(event_callbacks.liveliness_callback, + RCL_SUBSCRIPTION_LIVELINESS_CHANGED); + } + } /// Support dynamically setting the message memory strategy. /** diff --git a/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index e2bdec7ef2..905e10d3de 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -75,6 +75,7 @@ template< SubscriptionFactory create_subscription_factory( CallbackT && callback, + const SubscriptionEventCallbacks & event_callbacks, typename rclcpp::message_memory_strategy::MessageMemoryStrategy< CallbackMessageT, Alloc>::SharedPtr msg_mem_strat, @@ -91,7 +92,7 @@ create_subscription_factory( // factory function that creates a MessageT specific SubscriptionT factory.create_typed_subscription = - [allocator, msg_mem_strat, any_subscription_callback, message_alloc]( + [allocator, msg_mem_strat, any_subscription_callback, event_callbacks, message_alloc]( rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic_name, rcl_subscription_options_t & subscription_options @@ -109,6 +110,7 @@ create_subscription_factory( topic_name, subscription_options, any_subscription_callback, + event_callbacks, msg_mem_strat); auto sub_base_ptr = std::dynamic_pointer_cast(sub); return sub_base_ptr; diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index 38c34b950d..9cbc5d75eb 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -19,6 +19,8 @@ #include #include +#include "rclcpp/qos_event.hpp" +#include "rclcpp/callback_group.hpp" #include "rclcpp/intra_process_setting.hpp" #include "rclcpp/visibility_control.hpp" @@ -29,14 +31,15 @@ namespace rclcpp template struct SubscriptionOptionsWithAllocator { + SubscriptionEventCallbacks event_callbacks; /// The quality of service profile to pass on to the rmw implementation. rmw_qos_profile_t qos_profile = rmw_qos_profile_default; /// True to ignore local publications. bool ignore_local_publications = false; /// The callback group for this subscription. NULL to use the default callback group. - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group = nullptr; + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group; /// Optional custom allocator. - std::shared_ptr allocator = nullptr; + std::shared_ptr allocator; /// Setting to explicitly set intraprocess communications. IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault; }; diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 4f48bbc238..5ff01792b7 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -61,6 +61,17 @@ class Waitable size_t get_number_of_ready_clients(); + /// Get the number of ready events + /** + * Returns a value of 0 by default. + * This should be overridden if the Waitable contains one or more events. + * \return The number of events associated with the Waitable. + */ + RCLCPP_PUBLIC + virtual + size_t + get_number_of_ready_events(); + /// Get the number of ready services /** * Returns a value of 0 by default. diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index ac837314a4..f833bcf0a3 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -60,7 +60,8 @@ Executor::Executor(const ExecutorArgs & args) // Store the context for later use. context_ = args.context; - ret = rcl_wait_set_init(&wait_set_, 0, 2, 0, 0, 0, context_->get_rcl_context().get(), allocator); + ret = rcl_wait_set_init(&wait_set_, 0, 2, 0, 0, 0, 0, + context_->get_rcl_context().get(), allocator); if (RCL_RET_OK != ret) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", @@ -434,7 +435,8 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) rcl_ret_t ret = rcl_wait_set_resize( &wait_set_, memory_strategy_->number_of_ready_subscriptions(), memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(), - memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services()); + memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services(), + memory_strategy_->number_of_ready_events()); if (RCL_RET_OK != ret) { throw std::runtime_error( std::string("Couldn't resize the wait set : ") + rcl_get_error_string().str); diff --git a/rclcpp/src/rclcpp/graph_listener.cpp b/rclcpp/src/rclcpp/graph_listener.cpp index 1a181898c4..b097564333 100644 --- a/rclcpp/src/rclcpp/graph_listener.cpp +++ b/rclcpp/src/rclcpp/graph_listener.cpp @@ -79,6 +79,7 @@ GraphListener::start_if_not_started() 0, // number_of_timers 0, // number_of_clients 0, // number_of_services + 0, // number_of_events this->parent_context_->get_rcl_context().get(), rcl_get_default_allocator()); if (RCL_RET_OK != ret) { @@ -145,7 +146,7 @@ GraphListener::run_loop() const size_t node_graph_interfaces_size = node_graph_interfaces_.size(); // Add 2 for the interrupt and shutdown guard conditions if (wait_set_.size_of_guard_conditions < (node_graph_interfaces_size + 2)) { - ret = rcl_wait_set_resize(&wait_set_, 0, node_graph_interfaces_size + 2, 0, 0, 0); + ret = rcl_wait_set_resize(&wait_set_, 0, node_graph_interfaces_size + 2, 0, 0, 0, 0); if (RCL_RET_OK != ret) { throw_from_rcl_error(ret, "failed to resize wait set"); } diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index a10f51bec8..8313932621 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -470,3 +470,9 @@ Node::get_node_options() const { return this->node_options_; } + +bool +Node::assert_liveliness() const +{ + return node_base_->assert_liveliness(); +} diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 845dcd7b6d..a9198ccd73 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -197,6 +197,12 @@ NodeBase::get_shared_rcl_node_handle() const return node_handle_; } +bool +NodeBase::assert_liveliness() const +{ + return RCL_RET_OK == rcl_node_assert_liveliness(get_rcl_node_handle()); +} + rclcpp::callback_group::CallbackGroup::SharedPtr NodeBase::create_callback_group(rclcpp::callback_group::CallbackGroupType group_type) { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 6a38d631a0..346a14b69a 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -75,6 +75,8 @@ NodeParameters::NodeParameters( node_topics.get(), "parameter_events", parameter_event_qos_profile, + PublisherEventCallbacks(), + nullptr, use_intra_process, allocator); } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index b380e3c2ec..e233cc89ce 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -64,11 +64,22 @@ NodeTopics::create_publisher( void NodeTopics::add_publisher( - rclcpp::PublisherBase::SharedPtr publisher) + rclcpp::PublisherBase::SharedPtr publisher, + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) { - // The publisher is not added to a callback group or anthing like that for now. - // It may be stored within the NodeTopics class or the NodeBase class in the future. - (void)publisher; + // Assign to a group. + if (callback_group) { + if (!node_base_->callback_group_in_node(callback_group)) { + throw std::runtime_error("Cannot create publisher, callback group not in node."); + } + } else { + callback_group = node_base_->get_default_callback_group(); + } + + for (auto & publisher_event : publisher->get_event_handlers()) { + callback_group->add_waitable(publisher_event); + } + // Notify the executor that a new publisher was created using the parent Node. { auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); @@ -114,9 +125,13 @@ NodeTopics::add_subscription( // TODO(jacquelinekay): use custom exception throw std::runtime_error("Cannot create subscription, callback group not in node."); } - callback_group->add_subscription(subscription); } else { - node_base_->get_default_callback_group()->add_subscription(subscription); + callback_group = node_base_->get_default_callback_group(); + } + + callback_group->add_subscription(subscription); + for (auto & subscription_event : subscription->get_event_handlers()) { + callback_group->add_waitable(subscription_event); } // Notify the executor that a new subscription was created using the parent Node. diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp index a580bb56d0..fa6221a267 100644 --- a/rclcpp/src/rclcpp/publisher.cpp +++ b/rclcpp/src/rclcpp/publisher.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -81,6 +82,9 @@ PublisherBase::PublisherBase( PublisherBase::~PublisherBase() { + // must fini the events before fini-ing the publisher + event_handlers_.clear(); + if (rcl_publisher_fini(&intra_process_publisher_handle_, rcl_node_handle_.get()) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", @@ -154,6 +158,12 @@ PublisherBase::get_publisher_handle() const return &publisher_handle_; } +const std::vector> & +PublisherBase::get_event_handlers() const +{ + return event_handlers_; +} + size_t PublisherBase::get_subscription_count() const { @@ -208,6 +218,12 @@ PublisherBase::get_actual_qos() const return *qos; } +bool +PublisherBase::assert_liveliness() const +{ + return RCL_RET_OK == rcl_publisher_assert_liveliness(&publisher_handle_); +} + bool PublisherBase::operator==(const rmw_gid_t & gid) const { diff --git a/rclcpp/src/rclcpp/qos_event.cpp b/rclcpp/src/rclcpp/qos_event.cpp new file mode 100644 index 0000000000..5300c64cdb --- /dev/null +++ b/rclcpp/src/rclcpp/qos_event.cpp @@ -0,0 +1,59 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/qos_event.hpp" + + +namespace rclcpp +{ + +QOSEventHandlerBase::~QOSEventHandlerBase() +{ + if (rcl_event_fini(&event_handle_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Error in destruction of rcl event handle: %s", rcl_get_error_string().str); + rcl_reset_error(); + } +} + +/// Get the number of ready events +size_t +QOSEventHandlerBase::get_number_of_ready_events() +{ + return 1; +} + +/// Add the Waitable to a wait set. +bool +QOSEventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set) +{ + if (rcl_wait_set_add_event(wait_set, &event_handle_, &wait_set_event_index_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add event to wait set: %s", rcl_get_error_string().str); + return false; + } + + return true; +} + +/// Check if the Waitable is ready. +bool +QOSEventHandlerBase::is_ready(rcl_wait_set_t * wait_set) +{ + return wait_set->events[wait_set_event_index_] == &event_handle_; +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/subscription.cpp b/rclcpp/src/rclcpp/subscription.cpp index ed26acaf17..1180c1bcf5 100644 --- a/rclcpp/src/rclcpp/subscription.cpp +++ b/rclcpp/src/rclcpp/subscription.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" @@ -121,6 +122,12 @@ SubscriptionBase::get_intra_process_subscription_handle() const return intra_process_subscription_handle_; } +const std::vector> & +SubscriptionBase::get_event_handlers() const +{ + return event_handlers_; +} + const rosidl_message_type_support_t & SubscriptionBase::get_message_type_support_handle() const { diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index f54cea4d5a..ca2ad27da2 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -223,6 +223,7 @@ void TimeSource::create_clock_sub() topic_name, std::move(cb), rmw_qos_profile_default, + rclcpp::SubscriptionEventCallbacks(), group, false, false, diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp index a29b80de1d..542b10a016 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -34,6 +34,12 @@ Waitable::get_number_of_ready_clients() return 0u; } +size_t +Waitable::get_number_of_ready_events() +{ + return 0u; +} + size_t Waitable::get_number_of_ready_services() { diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 290b93d5b4..dc19f3c5b2 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -23,10 +23,12 @@ #include "rclcpp/contexts/default_context.hpp" #include "rclcpp/intra_process_manager.hpp" +#include "rclcpp/event.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/create_publisher.hpp" #include "rclcpp/create_service.hpp" #include "rclcpp/create_subscription.hpp" +#include "rclcpp/subscription_options.hpp" #include "rclcpp/type_support_decl.hpp" #include "lifecycle_publisher.hpp" @@ -65,6 +67,8 @@ LifecycleNode::create_publisher( this->node_topics_.get(), topic_name, qos_profile, + rclcpp::PublisherEventCallbacks(), + nullptr, use_intra_process_comms_, allocator); } @@ -99,6 +103,7 @@ LifecycleNode::create_subscription( topic_name, std::forward(callback), qos_profile, + rclcpp::SubscriptionEventCallbacks(), group, ignore_local_publications, use_intra_process_comms_, @@ -128,6 +133,7 @@ LifecycleNode::create_subscription( topic_name, std::forward(callback), qos, + rclcpp::SubscriptionEventCallbacks(), group, ignore_local_publications, msg_mem_strat, diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp index 867714e600..10096a84c4 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp @@ -61,9 +61,10 @@ class LifecyclePublisher : public LifecyclePublisherInterface, rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic, const rcl_publisher_options_t & publisher_options, + const rclcpp::PublisherEventCallbacks event_callbacks, std::shared_ptr allocator) : rclcpp::Publisher( - node_base, topic, publisher_options, allocator), + node_base, topic, publisher_options, event_callbacks, allocator), enabled_(false), logger_(rclcpp::get_logger("LifecyclePublisher")) { From 1da795e02f02618dfa97d592672037901e090528 Mon Sep 17 00:00:00 2001 From: Miaofei Date: Tue, 16 Apr 2019 15:56:40 -0700 Subject: [PATCH 2/8] fix windows build Signed-off-by: Miaofei --- rclcpp/include/rclcpp/qos_event.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index e7a259c83f..da68ab0f39 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -56,17 +56,21 @@ struct SubscriptionEventCallbacks class QOSEventHandlerBase : public Waitable { public: + RCLCPP_PUBLIC virtual ~QOSEventHandlerBase(); /// Get the number of ready events + RCLCPP_PUBLIC size_t get_number_of_ready_events() override; /// Add the Waitable to a wait set. + RCLCPP_PUBLIC bool add_to_wait_set(rcl_wait_set_t * wait_set) override; /// Check if the Waitable is ready. + RCLCPP_PUBLIC bool is_ready(rcl_wait_set_t * wait_set) override; From 364217df2e98793faf6e0692d837ae08c84dfacf Mon Sep 17 00:00:00 2001 From: Miaofei Date: Thu, 25 Apr 2019 17:26:26 -0700 Subject: [PATCH 3/8] address feedback from pull request Signed-off-by: Miaofei --- rclcpp/include/rclcpp/callback_group.hpp | 2 +- rclcpp/include/rclcpp/node.hpp | 5 ++++- .../node_interfaces/node_base_interface.hpp | 2 +- rclcpp/include/rclcpp/publisher.hpp | 17 +++++++++++------ rclcpp/include/rclcpp/publisher_factory.hpp | 2 +- rclcpp/include/rclcpp/publisher_options.hpp | 2 +- rclcpp/include/rclcpp/qos_event.hpp | 10 ++++------ rclcpp/include/rclcpp/subscription.hpp | 8 +++++--- rclcpp/include/rclcpp/subscription_options.hpp | 2 +- rclcpp/src/rclcpp/executor.cpp | 2 +- rclcpp/src/rclcpp/publisher.cpp | 4 ++-- rclcpp/src/rclcpp/qos_event.cpp | 5 ++--- 12 files changed, 34 insertions(+), 27 deletions(-) diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index c4d17c44ca..57319b2c23 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -21,8 +21,8 @@ #include #include "rclcpp/client.hpp" -#include "rclcpp/service.hpp" #include "rclcpp/publisher.hpp" +#include "rclcpp/service.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/timer.hpp" #include "rclcpp/visibility_control.hpp" diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index c5aa276270..ad165e040d 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -1120,13 +1120,16 @@ class Node : public std::enable_shared_from_this const NodeOptions & get_node_options() const; - /// Manually assert that this Node is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE) + /// Manually assert that this Node is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE). /** * If the rmw Liveliness policy is set to RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE, the creator * of this node may manually call `assert_liveliness` at some point in time to signal to the rest * of the system that this Node is still alive. + * + * \return `true` if the liveliness was asserted successfully, otherwise `false` */ RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED bool assert_liveliness() const; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp index ee562e41f8..a2228fb617 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp @@ -102,7 +102,7 @@ class NodeBaseInterface std::shared_ptr get_shared_rcl_node_handle() const = 0; - /// Manually assert that this Node is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE) + /// Manually assert that this Node is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE). RCLCPP_PUBLIC virtual bool diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 9a27a04c25..50de1b782f 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -21,9 +21,9 @@ #include #include #include -#include #include #include +#include #include "rcl/error_handling.h" #include "rcl/publisher.h" @@ -43,8 +43,8 @@ namespace rclcpp namespace node_interfaces { -// NOTE(emersonknapp) Forward declaration avoids including node_base_interface.hpp which causes -// circular inclusion from callback_group.hpp +// Forward declaration avoids including node_base_interface.hpp +// which causes circular inclusion from callback_group.hpp class NodeBaseInterface; // Forward declaration is used for friend statement. class NodeTopicsInterface; @@ -121,8 +121,10 @@ class PublisherBase const rcl_publisher_t * get_publisher_handle() const; + /// Get all the QoS event handlers associated with this publisher. + /** \return The vector of QoS event handlers. */ RCLCPP_PUBLIC - const std::vector> & + const std::vector> & get_event_handlers() const; /// Get subscription count @@ -137,13 +139,16 @@ class PublisherBase size_t get_intra_process_subscription_count() const; - /// Manually assert that this Publisher is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC) + /// Manually assert that this Publisher is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC). /** * If the rmw Liveliness policy is set to RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC, the creator * of this publisher may manually call `assert_liveliness` at some point in time to signal to the * rest of the system that this Node is still alive. + * + * \return `true` if the liveliness was asserted successfully, otherwise `false` */ RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED bool assert_liveliness() const; @@ -214,7 +219,7 @@ class PublisherBase rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher(); rcl_publisher_t intra_process_publisher_handle_ = rcl_get_zero_initialized_publisher(); - std::vector> event_handlers_; + std::vector> event_handlers_; using IntraProcessManagerWeakPtr = std::weak_ptr; diff --git a/rclcpp/include/rclcpp/publisher_factory.hpp b/rclcpp/include/rclcpp/publisher_factory.hpp index 98ab04dc6a..a80b2d698f 100644 --- a/rclcpp/include/rclcpp/publisher_factory.hpp +++ b/rclcpp/include/rclcpp/publisher_factory.hpp @@ -92,7 +92,7 @@ create_publisher_factory( publisher_options.allocator = allocator::get_rcl_allocator(*message_alloc.get()); return std::make_shared(node_base, topic_name, publisher_options, - event_callbacks, message_alloc); + event_callbacks, message_alloc); }; // function to add a publisher to the intra process manager diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp index e73c6886bc..acdec02718 100644 --- a/rclcpp/include/rclcpp/publisher_options.hpp +++ b/rclcpp/include/rclcpp/publisher_options.hpp @@ -19,9 +19,9 @@ #include #include -#include "rclcpp/qos_event.hpp" #include "rclcpp/callback_group.hpp" #include "rclcpp/intra_process_setting.hpp" +#include "rclcpp/qos_event.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index da68ab0f39..d14af2bbe9 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -1,4 +1,4 @@ -// Copyright 2016 Open Source Robotics Foundation, Inc. +// Copyright 2019 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -21,10 +21,9 @@ #include "rcutils/logging_macros.h" -#include "rclcpp/waitable.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/function_traits.hpp" - +#include "rclcpp/waitable.hpp" namespace rclcpp { @@ -39,14 +38,14 @@ using QOSDeadlineOfferedCallbackType = std::function; using QOSLivelinessLostCallbackType = std::function; -/// Contains callbacks for various types of events a Publisher can receive from the middleware +/// Contains callbacks for various types of events a Publisher can receive from the middleware. struct PublisherEventCallbacks { QOSDeadlineOfferedCallbackType deadline_callback; QOSLivelinessLostCallbackType liveliness_callback; }; -/// Contains callbacks for non-message events that a Subscriber can receive from the middleware +/// Contains callbacks for non-message events that a Subscription can receive from the middleware. struct SubscriptionEventCallbacks { QOSDeadlineRequestedCallbackType deadline_callback; @@ -79,7 +78,6 @@ class QOSEventHandlerBase : public Waitable size_t wait_set_event_index_; }; - template class QOSEventHandler : public QOSEventHandlerBase { diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index a9fe21bb81..65691f8760 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -21,9 +21,9 @@ #include #include #include -#include #include #include +#include #include "rcl/error_handling.h" #include "rcl/subscription.h" @@ -102,8 +102,10 @@ class SubscriptionBase virtual const std::shared_ptr get_intra_process_subscription_handle() const; + /// Get all the QoS event handlers associated with this subscription. + /** \return The vector of QoS event handlers. */ RCLCPP_PUBLIC - const std::vector> & + const std::vector> & get_event_handlers() const; /// Borrow a new message. @@ -175,7 +177,7 @@ class SubscriptionBase size_t wait_set_subscription_index_; bool subscription_ready_; - std::vector> event_handlers_; + std::vector> event_handlers_; bool use_intra_process_; std::shared_ptr intra_process_subscription_handle_; diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index 9cbc5d75eb..a86612e25b 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -19,9 +19,9 @@ #include #include -#include "rclcpp/qos_event.hpp" #include "rclcpp/callback_group.hpp" #include "rclcpp/intra_process_setting.hpp" +#include "rclcpp/qos_event.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index f833bcf0a3..b9a479d73e 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -61,7 +61,7 @@ Executor::Executor(const ExecutorArgs & args) context_ = args.context; ret = rcl_wait_set_init(&wait_set_, 0, 2, 0, 0, 0, 0, - context_->get_rcl_context().get(), allocator); + context_->get_rcl_context().get(), allocator); if (RCL_RET_OK != ret) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp index fa6221a267..5848d8b24c 100644 --- a/rclcpp/src/rclcpp/publisher.cpp +++ b/rclcpp/src/rclcpp/publisher.cpp @@ -20,9 +20,9 @@ #include #include #include -#include #include #include +#include #include "rcl_interfaces/msg/intra_process_message.hpp" #include "rcutils/logging_macros.h" @@ -31,10 +31,10 @@ #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/allocator/allocator_deleter.hpp" #include "rclcpp/exceptions.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/intra_process_manager.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node.hpp" -#include "rclcpp/expand_topic_or_service_name.hpp" using rclcpp::PublisherBase; diff --git a/rclcpp/src/rclcpp/qos_event.cpp b/rclcpp/src/rclcpp/qos_event.cpp index 5300c64cdb..57db46c7ec 100644 --- a/rclcpp/src/rclcpp/qos_event.cpp +++ b/rclcpp/src/rclcpp/qos_event.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Open Source Robotics Foundation, Inc. +// Copyright 2019 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -14,7 +14,6 @@ #include "rclcpp/qos_event.hpp" - namespace rclcpp { @@ -28,7 +27,7 @@ QOSEventHandlerBase::~QOSEventHandlerBase() } } -/// Get the number of ready events +/// Get the number of ready events. size_t QOSEventHandlerBase::get_number_of_ready_events() { From 19da4b7e8671e879a9207bccf80c79cb15fc37b3 Mon Sep 17 00:00:00 2001 From: Miaofei Date: Fri, 26 Apr 2019 00:06:30 -0700 Subject: [PATCH 4/8] update formatting to be compatible with ros2 coding style and ament_uncrustify Signed-off-by: Miaofei --- rclcpp/include/rclcpp/publisher_factory.hpp | 8 ++++++-- rclcpp/src/rclcpp/executor.cpp | 7 +++++-- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher_factory.hpp b/rclcpp/include/rclcpp/publisher_factory.hpp index a80b2d698f..0a744a0685 100644 --- a/rclcpp/include/rclcpp/publisher_factory.hpp +++ b/rclcpp/include/rclcpp/publisher_factory.hpp @@ -91,8 +91,12 @@ create_publisher_factory( auto message_alloc = std::make_shared(*allocator.get()); publisher_options.allocator = allocator::get_rcl_allocator(*message_alloc.get()); - return std::make_shared(node_base, topic_name, publisher_options, - event_callbacks, message_alloc); + return std::make_shared( + node_base, + topic_name, + publisher_options, + event_callbacks, + message_alloc); }; // function to add a publisher to the intra process manager diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index b9a479d73e..00bd2c5973 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -60,8 +60,11 @@ Executor::Executor(const ExecutorArgs & args) // Store the context for later use. context_ = args.context; - ret = rcl_wait_set_init(&wait_set_, 0, 2, 0, 0, 0, 0, - context_->get_rcl_context().get(), allocator); + ret = rcl_wait_set_init( + &wait_set_, + 0, 2, 0, 0, 0, 0, + context_->get_rcl_context().get(), + allocator); if (RCL_RET_OK != ret) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", From 763963a2a19561a8282a10523102738f8dbfd166 Mon Sep 17 00:00:00 2001 From: Miaofei Date: Fri, 26 Apr 2019 00:27:01 -0700 Subject: [PATCH 5/8] make QOSEventHandlerBase::add_to_wait_set() throw Signed-off-by: Miaofei --- rclcpp/include/rclcpp/waitable.hpp | 1 + rclcpp/src/rclcpp/qos_event.cpp | 8 +++----- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 5ff01792b7..401b88e541 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -99,6 +99,7 @@ class Waitable /** * \param[in] wait_set A handle to the wait set to add the Waitable to. * \return `true` if the Waitable is added successfully, `false` otherwise. + * \throws rclcpp::execptions::RCLError from rcl_wait_set_add_*() */ RCLCPP_PUBLIC virtual diff --git a/rclcpp/src/rclcpp/qos_event.cpp b/rclcpp/src/rclcpp/qos_event.cpp index 57db46c7ec..2de324d0e0 100644 --- a/rclcpp/src/rclcpp/qos_event.cpp +++ b/rclcpp/src/rclcpp/qos_event.cpp @@ -38,13 +38,11 @@ QOSEventHandlerBase::get_number_of_ready_events() bool QOSEventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set) { - if (rcl_wait_set_add_event(wait_set, &event_handle_, &wait_set_event_index_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add event to wait set: %s", rcl_get_error_string().str); + rcl_ret_t ret = rcl_wait_set_add_event(wait_set, &event_handle_, &wait_set_event_index_); + if (RCL_RET_OK != ret) { + exceptions::throw_from_rcl_error(ret, "Couldn't add event to wait set"); return false; } - return true; } From e35711b006c96eaf879210b5ed0f81ed14605f19 Mon Sep 17 00:00:00 2001 From: Miaofei Date: Sat, 27 Apr 2019 15:25:55 -0700 Subject: [PATCH 6/8] mark throw_from_rcl_error as [[noreturn]] Signed-off-by: Miaofei --- rclcpp/include/rclcpp/exceptions.hpp | 1 + rclcpp/src/rclcpp/qos_event.cpp | 1 - 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/exceptions.hpp b/rclcpp/include/rclcpp/exceptions.hpp index 4744afe662..ee72402091 100644 --- a/rclcpp/include/rclcpp/exceptions.hpp +++ b/rclcpp/include/rclcpp/exceptions.hpp @@ -111,6 +111,7 @@ class InvalidServiceNameError : public NameValidationError * \throws RCLErrorBase some child class exception based on ret */ RCLCPP_PUBLIC +[[noreturn]] void throw_from_rcl_error( rcl_ret_t ret, diff --git a/rclcpp/src/rclcpp/qos_event.cpp b/rclcpp/src/rclcpp/qos_event.cpp index 2de324d0e0..75072f799a 100644 --- a/rclcpp/src/rclcpp/qos_event.cpp +++ b/rclcpp/src/rclcpp/qos_event.cpp @@ -41,7 +41,6 @@ QOSEventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set) rcl_ret_t ret = rcl_wait_set_add_event(wait_set, &event_handle_, &wait_set_event_index_); if (RCL_RET_OK != ret) { exceptions::throw_from_rcl_error(ret, "Couldn't add event to wait set"); - return false; } return true; } From 6fdaf2cfe33b809207bbfe5f0f07d0d503b52e3a Mon Sep 17 00:00:00 2001 From: Miaofei Date: Wed, 1 May 2019 23:37:42 -0700 Subject: [PATCH 7/8] fix windows compilation error Signed-off-by: Miaofei --- rclcpp/include/rclcpp/exceptions.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/exceptions.hpp b/rclcpp/include/rclcpp/exceptions.hpp index ee72402091..5d1c75037f 100644 --- a/rclcpp/include/rclcpp/exceptions.hpp +++ b/rclcpp/include/rclcpp/exceptions.hpp @@ -111,9 +111,8 @@ class InvalidServiceNameError : public NameValidationError * \throws RCLErrorBase some child class exception based on ret */ RCLCPP_PUBLIC -[[noreturn]] void -throw_from_rcl_error( +throw_from_rcl_error [[noreturn]] ( rcl_ret_t ret, const std::string & prefix = "", const rcl_error_state_t * error_state = nullptr, From da1d8f4af88c85ee27866f2a786dcd1a9320adfd Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Thu, 2 May 2019 16:32:05 -0700 Subject: [PATCH 8/8] Ignore uncrustify for single [[noreturn]] syntax instance Signed-off-by: Emerson Knapp --- rclcpp/include/rclcpp/exceptions.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rclcpp/include/rclcpp/exceptions.hpp b/rclcpp/include/rclcpp/exceptions.hpp index 5d1c75037f..c65df54f1f 100644 --- a/rclcpp/include/rclcpp/exceptions.hpp +++ b/rclcpp/include/rclcpp/exceptions.hpp @@ -110,6 +110,7 @@ class InvalidServiceNameError : public NameValidationError * \throws std::runtime_error if the rcl_get_error_state returns 0 * \throws RCLErrorBase some child class exception based on ret */ +/* *INDENT-OFF* */ // Uncrustify cannot yet understand [[noreturn]] properly RCLCPP_PUBLIC void throw_from_rcl_error [[noreturn]] ( @@ -117,6 +118,7 @@ throw_from_rcl_error [[noreturn]] ( const std::string & prefix = "", const rcl_error_state_t * error_state = nullptr, void (* reset_error)() = rcl_reset_error); +/* *INDENT-ON* */ class RCLErrorBase {