rclcpp QoS implementation

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/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_,