[IR] Implementation of QoS Features

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

rclcpp/include/rclcpp/any_executable.hpp

@@ -35,18 +35,16 @@ namespace executor
 struct AnyExecutable
 {
   RCLCPP_PUBLIC
-  AnyExecutable();
+  AnyExecutable() = default;
 
   RCLCPP_PUBLIC
   virtual ~AnyExecutable();
 
-  // Only one of the following pointers will be set.
-  rclcpp::SubscriptionBase::SharedPtr subscription;
-  rclcpp::SubscriptionBase::SharedPtr subscription_intra_process;
-  rclcpp::TimerBase::SharedPtr timer;
-  rclcpp::ServiceBase::SharedPtr service;
-  rclcpp::ClientBase::SharedPtr client;
+  bool
+  has_timer() const;
+
   rclcpp::Waitable::SharedPtr waitable;
+
   // These are used to keep the scope on the containing items
   rclcpp::callback_group::CallbackGroup::SharedPtr callback_group;
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base;

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"
@@ -61,6 +62,10 @@ class CallbackGroup
   RCLCPP_PUBLIC
   explicit CallbackGroup(CallbackGroupType group_type);
 
+  RCLCPP_PUBLIC
+  const std::vector<rclcpp::PublisherBase::WeakPtr> &
+  get_publisher_ptrs() const;
+
   RCLCPP_PUBLIC
   const std::vector<rclcpp::SubscriptionBase::WeakPtr> &
   get_subscription_ptrs() const;
@@ -92,6 +97,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);
@@ -119,6 +128,7 @@ class CallbackGroup
   CallbackGroupType type_;
   // Mutex to protect the subsequent vectors of pointers.
   mutable std::mutex mutex_;
+  std::vector<rclcpp::PublisherBase::WeakPtr> publisher_ptrs_;
   std::vector<rclcpp::SubscriptionBase::WeakPtr> subscription_ptrs_;
   std::vector<rclcpp::TimerBase::WeakPtr> timer_ptrs_;
   std::vector<rclcpp::ServiceBase::WeakPtr> service_ptrs_;

rclcpp/include/rclcpp/client.hpp

@@ -27,6 +27,7 @@
 #include "rcl/error_handling.h"
 #include "rcl/wait.h"
 
+#include "rclcpp/waitable.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/function_traits.hpp"
 #include "rclcpp/macros.hpp"
@@ -49,7 +50,7 @@ namespace node_interfaces
 class NodeBaseInterface;
 }  // namespace node_interfaces
 
-class ClientBase
+class ClientBase : public Waitable
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientBase)
@@ -74,6 +75,34 @@ class ClientBase
   std::shared_ptr<const rcl_client_t>
   get_client_handle() const;
 
+  RCLCPP_PUBLIC
+  std::shared_ptr<rcl_event_t>
+  get_event_handle();
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<const rcl_event_t>
+  get_event_handle() const;
+
+  RCLCPP_PUBLIC
+  size_t
+  get_number_of_ready_clients() override;
+
+  RCLCPP_PUBLIC
+  size_t
+  get_number_of_ready_events() override;
+
+  RCLCPP_PUBLIC
+  bool
+  add_to_wait_set(rcl_wait_set_t * wait_set) override;
+
+  RCLCPP_PUBLIC
+  bool
+  is_ready(rcl_wait_set_t * wait_set) override;
+
+  RCLCPP_PUBLIC
+  void
+  execute() override;
+
   RCLCPP_PUBLIC
   bool
   service_is_ready() const;
@@ -113,6 +142,13 @@ class ClientBase
   std::shared_ptr<rclcpp::Context> context_;
 
   std::shared_ptr<rcl_client_t> client_handle_;
+  std::shared_ptr<rcl_event_t> event_handle_;
+
+  size_t wait_set_client_index_;
+  size_t wait_set_event_index_;
+
+  bool client_ready_;
+  bool event_ready_;
 };
 
 template<typename ServiceT>
@@ -146,6 +182,7 @@ class Client : public ClientBase
     using rosidl_typesupport_cpp::get_service_type_support_handle;
     auto service_type_support_handle =
       get_service_type_support_handle<ServiceT>();
+
     rcl_ret_t ret = rcl_client_init(
       this->get_client_handle().get(),
       this->get_rcl_node_handle(),
@@ -165,6 +202,12 @@ class Client : public ClientBase
       }
       rclcpp::exceptions::throw_from_rcl_error(ret, "could not create client");
     }
+
+    ret = rcl_client_event_init(get_event_handle().get(), get_client_handle().get(),
+      &client_options, RCL_CLIENT_EVENT_UNIMPLEMENTED);
+    if (ret != RCL_RET_OK) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "could not create client event");
+    }
   }
 
   virtual ~Client()

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 & callbacks,
+  rclcpp::callback_group::CallbackGroup::SharedPtr group,
   bool use_intra_process_comms,
   std::shared_ptr<AllocatorT> allocator)
 {
@@ -39,10 +41,12 @@ create_publisher(
 
   auto pub = node_topics->create_publisher(
     topic_name,
-    rclcpp::create_publisher_factory<MessageT, AllocatorT, PublisherT>(allocator),
+    rclcpp::create_publisher_factory<MessageT, AllocatorT, PublisherT>(callbacks, allocator),
     publisher_options,
     use_intra_process_comms);
-  node_topics->add_publisher(pub);
+
+  node_topics->add_publisher(pub, group);
+
   return std::dynamic_pointer_cast<PublisherT>(pub);
 }
 

rclcpp/include/rclcpp/create_service.hpp

@@ -46,10 +46,11 @@ create_service(
   service_options.qos = qos_profile;
 
   auto serv = Service<ServiceT>::make_shared(
-    node_base->get_shared_rcl_node_handle(),
-    service_name, any_service_callback, service_options);
+    node_base->get_shared_rcl_node_handle(), service_name, service_options, any_service_callback);
   auto serv_base_ptr = std::dynamic_pointer_cast<ServiceBase>(serv);
+
   node_services->add_service(serv_base_ptr, group);
+
   return serv;
 }
 

rclcpp/include/rclcpp/create_subscription.hpp

@@ -38,28 +38,32 @@ create_subscription(
   const std::string & topic_name,
   CallbackT && callback,
   const rmw_qos_profile_t & qos_profile,
+  const SubscriptionEventCallbacks & callbacks,
   rclcpp::callback_group::CallbackGroup::SharedPtr group,
   bool ignore_local_publications,
   bool use_intra_process_comms,
-  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
-    CallbackMessageT, AllocatorT>::SharedPtr
-  msg_mem_strat,
+  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<CallbackMessageT,
+  AllocatorT>::SharedPtr msg_mem_strat,
   typename std::shared_ptr<AllocatorT> allocator)
 {
   auto subscription_options = rcl_subscription_get_default_options();
   subscription_options.qos = qos_profile;
   subscription_options.ignore_local_publications = ignore_local_publications;
 
-  auto factory = rclcpp::create_subscription_factory
-    <MessageT, CallbackT, AllocatorT, CallbackMessageT, SubscriptionT>(
-    std::forward<CallbackT>(callback), msg_mem_strat, allocator);
+  auto factory = rclcpp::create_subscription_factory<MessageT, CallbackT, AllocatorT,
+      CallbackMessageT, SubscriptionT>(
+    std::forward<CallbackT>(callback),
+    callbacks,
+    msg_mem_strat,
+    allocator);
 
   auto sub = node_topics->create_subscription(
     topic_name,
     factory,
     subscription_options,
     use_intra_process_comms);
   node_topics->add_subscription(sub, group);
+
   return std::dynamic_pointer_cast<SubscriptionT>(sub);
 }
 

rclcpp/include/rclcpp/executor.hpp

@@ -298,28 +298,6 @@ class Executor
   void
   execute_any_executable(AnyExecutable & any_exec);
 
-  RCLCPP_PUBLIC
-  static void
-  execute_subscription(
-    rclcpp::SubscriptionBase::SharedPtr subscription);
-
-  RCLCPP_PUBLIC
-  static void
-  execute_intra_process_subscription(
-    rclcpp::SubscriptionBase::SharedPtr subscription);
-
-  RCLCPP_PUBLIC
-  static void
-  execute_timer(rclcpp::TimerBase::SharedPtr timer);
-
-  RCLCPP_PUBLIC
-  static void
-  execute_service(rclcpp::ServiceBase::SharedPtr service);
-
-  RCLCPP_PUBLIC
-  static void
-  execute_client(rclcpp::ClientBase::SharedPtr client);
-
   RCLCPP_PUBLIC
   void
   wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -79,7 +79,7 @@ class MultiThreadedExecutor : public executor::Executor
   bool yield_before_execute_;
 
   std::mutex scheduled_timers_mutex_;
-  std::set<TimerBase::SharedPtr> scheduled_timers_;
+  std::set<Waitable::SharedPtr> scheduled_timers_;
 };
 
 }  // namespace executors

rclcpp/include/rclcpp/graph_event.hpp

@@ -0,0 +1,28 @@
+// 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__GRAPH_EVENT_HPP_
+#define RCLCPP__GRAPH_EVENT_HPP_
+
+#include "rclcpp/event.hpp"
+
+
+namespace rclcpp
+{
+
+class GraphEvent : public Event {};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__GRAPH_EVENT_HPP_

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;
@@ -63,21 +64,6 @@ class RCLCPP_PUBLIC MemoryStrategy
 
   virtual void remove_guard_condition(const rcl_guard_condition_t * guard_condition) = 0;
 
-  virtual void
-  get_next_subscription(
-    rclcpp::executor::AnyExecutable & any_exec,
-    const WeakNodeVector & weak_nodes) = 0;
-
-  virtual void
-  get_next_service(
-    rclcpp::executor::AnyExecutable & any_exec,
-    const WeakNodeVector & weak_nodes) = 0;
-
-  virtual void
-  get_next_client(
-    rclcpp::executor::AnyExecutable & any_exec,
-    const WeakNodeVector & weak_nodes) = 0;
-
   virtual void
   get_next_waitable(
     rclcpp::executor::AnyExecutable & any_exec,