rclcpp QoS implementation

CONTRIBUTING.md

@@ -11,3 +11,8 @@ be under the Apache 2 License, as dictated by that
    the terms of any separate license agreement you may have executed
    with Licensor regarding such Contributions.
 ~~~
+
+Contributors must sign-off each commit by adding a `Signed-off-by: ...`
+line to commit messages to certify that they have the right to submit
+the code they are contributing to the project according to the
+[Developer Certificate of Origin (DCO)](https://developercertificate.org/).

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/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/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<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>(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<PublisherT>(pub);
 }
 

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
     <MessageT, CallbackT, AllocatorT, CallbackMessageT, SubscriptionT>(
-    std::forward<CallbackT>(callback), msg_mem_strat, allocator);
+    std::forward<CallbackT>(callback),
+    event_callbacks, 
+    msg_mem_strat,
+    allocator);
 
   auto sub = node_topics->create_subscription(
     topic_name,

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -78,7 +78,6 @@ class MultiThreadedExecutor : public executor::Executor
   size_t number_of_threads_;
   bool yield_before_execute_;
 
-  std::mutex scheduled_timers_mutex_;
   std::set<TimerBase::SharedPtr> scheduled_timers_;
 };
 

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;

rclcpp/include/rclcpp/node.hpp

@@ -142,76 +142,27 @@ class Node : public std::enable_shared_from_this<Node>
   /// Create and return a Publisher.
   /**
    * \param[in] topic_name The topic for this publisher to publish on.
-   * \param[in] qos_history_depth The depth of the publisher message queue.
-   * \param[in] allocator Optional custom allocator.
+   * \param[in] group The callback group for this publisher. NULL for no callback group.
+   * \param[in] options Additional options to control creation of the publisher.
    * \return Shared pointer to the created publisher.
    */
   template<
-    typename MessageT, typename Alloc = std::allocator<void>,
-    typename PublisherT = ::rclcpp::Publisher<MessageT, Alloc>>
-  std::shared_ptr<PublisherT>
-  create_publisher(
-    const std::string & topic_name, size_t qos_history_depth,
-    std::shared_ptr<Alloc> allocator = nullptr);
-
-  /// Create and return a Publisher.
-  /**
-   * \param[in] topic_name The topic for this publisher to publish on.
-   * \param[in] qos_profile The quality of service profile to pass on to the rmw implementation.
-   * \param[in] allocator Optional custom allocator.
-   * \return Shared pointer to the created publisher.
-   */
-  template<
-    typename MessageT, typename Alloc = std::allocator<void>,
+    typename MessageT,
+    typename Alloc = std::allocator<void>,
     typename PublisherT = ::rclcpp::Publisher<MessageT, Alloc>>
   std::shared_ptr<PublisherT>
   create_publisher(
     const std::string & topic_name,
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default,
-    std::shared_ptr<Alloc> allocator = nullptr);
-
-  /// Create and return a Subscription.
-  /**
-   * \param[in] topic_name The topic to subscribe on.
-   * \param[in] callback The user-defined callback function.
-   * \param[in] qos_profile The quality of service profile to pass on to the rmw implementation.
-   * \param[in] group The callback group for this subscription. NULL for no callback group.
-   * \param[in] ignore_local_publications True to ignore local publications.
-   * \param[in] msg_mem_strat The message memory strategy to use for allocating messages.
-   * \param[in] allocator Optional custom allocator.
-   * \return Shared pointer to the created subscription.
-   */
-  /* TODO(jacquelinekay):
-     Windows build breaks when static member function passed as default
-     argument to msg_mem_strat, nullptr is a workaround.
-   */
-  template<
-    typename MessageT,
-    typename CallbackT,
-    typename Alloc = std::allocator<void>,
-    typename SubscriptionT = rclcpp::Subscription<
-      typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, Alloc>>
-  std::shared_ptr<SubscriptionT>
-  create_subscription(
-    const std::string & topic_name,
-    CallbackT && callback,
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default,
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr,
-    bool ignore_local_publications = false,
-    typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
-      typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, Alloc>::SharedPtr
-    msg_mem_strat = nullptr,
-    std::shared_ptr<Alloc> allocator = nullptr);
+    const PublisherOptions<Alloc> & options = PublisherOptions<Alloc>());
 
   /// Create and return a Subscription.
   /**
    * \param[in] topic_name The topic to subscribe on.
-   * \param[in] qos_history_depth The depth of the subscription's incoming message queue.
    * \param[in] callback The user-defined callback function.
    * \param[in] group The callback group for this subscription. NULL for no callback group.
-   * \param[in] ignore_local_publications True to ignore local publications.
+   * \param[in] options Additional options to control creation of the subscription.
    * \param[in] msg_mem_strat The message memory strategy to use for allocating messages.
-   * \param[in] allocator Optional custom allocator.
    * \return Shared pointer to the created subscription.
    */
   /* TODO(jacquelinekay):
@@ -228,13 +179,11 @@ class Node : public std::enable_shared_from_this<Node>
   create_subscription(
     const std::string & topic_name,
     CallbackT && callback,
-    size_t qos_history_depth,
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr,
-    bool ignore_local_publications = false,
+    const SubscriptionOptions<Alloc> & options = SubscriptionOptions<Alloc>(),
     typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
       typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, Alloc>::SharedPtr
-    msg_mem_strat = nullptr,
-    std::shared_ptr<Alloc> allocator = nullptr);
+    msg_mem_strat = nullptr);
 
   /// Create a timer.
   /**
@@ -600,6 +549,18 @@ class Node : public std::enable_shared_from_this<Node>
   const NodeOptions &
   get_node_options() const;
 
+
+  /// Manually assert that this Node is alive (for RMW_QOS_POLICY_MANUAL_BY_NODE)
+  /**
+   * If the rmw Liveliness policy is set to RMW_QOS_POLICY_MANUAL_BY_NODE, the creator of this
+   * node must manually call `assert_liveliness` on a regular basis to signal to the rest of the
+   * system that this Node is still alive.
+   * This function must be called at least as often as the qos_profile's liveliness_lease_duration
+   */
+  RCLCPP_PUBLIC
+  void
+  assert_liveliness() {}
+
 protected:
   /// Construct a sub-node, which will extend the namespace of all entities created with it.
   /**

rclcpp/include/rclcpp/node_impl.hpp

@@ -51,20 +51,6 @@
 namespace rclcpp
 {
 
-template<typename MessageT, typename Alloc, typename PublisherT>
-std::shared_ptr<PublisherT>
-Node::create_publisher(
-  const std::string & topic_name, size_t qos_history_depth,
-  std::shared_ptr<Alloc> allocator)
-{
-  if (!allocator) {
-    allocator = std::make_shared<Alloc>();
-  }
-  rmw_qos_profile_t qos = rmw_qos_profile_default;
-  qos.depth = qos_history_depth;
-  return this->create_publisher<MessageT, Alloc, PublisherT>(topic_name, qos, allocator);
-}
-
 RCLCPP_LOCAL
 inline
 std::string
@@ -80,17 +66,21 @@ extend_name_with_sub_namespace(const std::string & name, const std::string & sub
 template<typename MessageT, typename Alloc, typename PublisherT>
 std::shared_ptr<PublisherT>
 Node::create_publisher(
-  const std::string & topic_name, const rmw_qos_profile_t & qos_profile,
-  std::shared_ptr<Alloc> allocator)
+  const std::string & topic_name,
+  rclcpp::callback_group::CallbackGroup::SharedPtr group,
+  const PublisherOptions<Alloc> & options)
 {
+  std::shared_ptr<Alloc> allocator = options.allocator();
   if (!allocator) {
     allocator = std::make_shared<Alloc>();
   }
 
   return rclcpp::create_publisher<MessageT, Alloc, PublisherT>(
     this->node_topics_.get(),
     extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()),
-    qos_profile,
+    options.qos_profile(),
+    options.event_callbacks(),
+    group,
     this->get_node_options().use_intra_process_comms(),
     allocator);
 }
@@ -104,16 +94,15 @@ std::shared_ptr<SubscriptionT>
 Node::create_subscription(
   const std::string & topic_name,
   CallbackT && callback,
-  const rmw_qos_profile_t & qos_profile,
   rclcpp::callback_group::CallbackGroup::SharedPtr group,
-  bool ignore_local_publications,
+  const SubscriptionOptions<Alloc> & options,
   typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
     typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, Alloc>::SharedPtr
-  msg_mem_strat,
-  std::shared_ptr<Alloc> allocator)
+  msg_mem_strat)
 {
   using CallbackMessageT = typename rclcpp::subscription_traits::has_message_type<CallbackT>::type;
 
+  std::shared_ptr<Alloc> allocator = options.allocator();
   if (!allocator) {
     allocator = std::make_shared<Alloc>();
   }
@@ -127,44 +116,15 @@ Node::create_subscription(
     this->node_topics_.get(),
     extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()),
     std::forward<CallbackT>(callback),
-    qos_profile,
+    options.qos_profile(),
+    options.event_callbacks(),
     group,
-    ignore_local_publications,
+    options.ignore_local_publications(),
     this->get_node_options().use_intra_process_comms(),
     msg_mem_strat,
     allocator);
 }
 
-template<
-  typename MessageT,
-  typename CallbackT,
-  typename Alloc,
-  typename SubscriptionT>
-std::shared_ptr<SubscriptionT>
-Node::create_subscription(
-  const std::string & topic_name,
-  CallbackT && callback,
-  size_t qos_history_depth,
-  rclcpp::callback_group::CallbackGroup::SharedPtr group,
-  bool ignore_local_publications,
-  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
-    typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, Alloc>::SharedPtr
-  msg_mem_strat,
-  std::shared_ptr<Alloc> allocator)
-{
-  rmw_qos_profile_t qos = rmw_qos_profile_default;
-  qos.depth = qos_history_depth;
-
-  return this->create_subscription<MessageT>(
-    topic_name,
-    std::forward<CallbackT>(callback),
-    qos,
-    group,
-    ignore_local_publications,
-    msg_mem_strat,
-    allocator);
-}
-
 template<typename DurationRepT, typename DurationT, typename CallbackT>
 typename rclcpp::WallTimer<CallbackT>::SharedPtr
 Node::create_wall_timer(

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

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

rclcpp/include/rclcpp/parameter_client.hpp

@@ -127,6 +127,7 @@ class AsyncParametersClient
       "parameter_events",
       std::forward<CallbackT>(callback),
       rmw_qos_profile_default,
+      SubscriptionEventCallbacks(),
       nullptr,  // group,
       false,  // ignore_local_publications,
       false,  // use_intra_process_comms_,