rclcpp QoS implementation (#2)

* Add interfaces for events in memory_strategy

Signed-off-by: Miaofei <miaofei@amazon.com>

* refactor waitables

Signed-off-by: Miaofei <miaofei@amazon.com>

* Attempt to fix cppcheck (ros2#646)

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* add event callbacks to publisher, subscriber, client, service

Signed-off-by: Miaofei <miaofei@amazon.com>

* fix some ros2 build issues

Signed-off-by: Miaofei <miaofei@amazon.com>

* Add a method to the LifecycleNode class to get the logging interface (ros2#652)

There are getters for the other interfaces, but the logging interface
appears to have been overlooked.

Signed-off-by: Michael Jeronimo <michael.jeronimo@intel.com>

* Add Doxyfile for rclcpp_action

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* Add documentation to rclcpp_action

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* update to use separated action types (ros2#601)

* match renamed action types

* fix action type casting

* rename type/field to use correct term

* rename custom GoalID type to avoid naming collision, update types using unique_identifier_msgs

* remove obsolete comments

* change signature of set_succeeded / set_canceled

* change signature of     on_terminal_state_(uuid_, result_msg);set_succeeded / set_canceled

* change signature of set_aborted

* change signature of publish_feedback

* update another test

Signed-off-by: Miaofei <miaofei@amazon.com>

* update client-facing API

Signed-off-by: Miaofei <miaofei@amazon.com>

* Don't hardcode int64_t for duration type representations (ros2#648)

In LLVM's `libcxx`, `int64_t` doesn't match chrono literals. See example below. To compile, run  `clang++-6.0 -stdlib=libc++ -std=c++14 TEST.cpp`

```
using namespace std::chrono_literals;

template<typename RatioT = std::milli>
bool
wait_for_service(
   std::chrono::duration<int64_t, RatioT> timeout
)
{
   return timeout == std::chrono::nanoseconds(0);
}

int main() {
   wait_for_service(2s);
   return 0;
}

```

Result of compilation
```
TEST.cpp:6:1: note: candidate template ignored: could not match 'long' against 'long long'
wait_for_service(
```

Signed-off-by: Emerson Knapp <eknapp@amazon.com>
Signed-off-by: Steven! Ragnarök <steven@nuclearsandwich.com>

* improve usability of the SubscriptionOptions and PublisherOptions classes

Signed-off-by: Miaofei <miaofei@amazon.com>

* Fix test_time_source test (ros2#639)

* Fix flakey test

Signed-off-by: Pete Baughman <pete.baughman@apex.ai>

* Fix lint and uncrustify issues

Signed-off-by: Pete Baughman <pete.baughman@apex.ai>

* fix lint errors

Signed-off-by: Miaofei <miaofei@amazon.com>

* apply uncrustify

Signed-off-by: Miaofei <miaofei@amazon.com>

* add section about DCO to CONTRIBUTING.md

* update for rcl API changes

Signed-off-by: Miaofei <miaofei@amazon.com>

* Fix lint and build warnings and API inconsistency

Signed-off-by: Emerson Knapp <eknapp@amazon.com>

* Avoid race that triggers timer too often (ros2#621)

The two distinct operations of acquiring and subsequent checking of a
timer have to be protected by one lock_guard against races with other
threads. The releasing of a timer has to be protected by the same lock.

Given this requirement there is no use for a second mutex.

Signed-off-by: Marko Durkovic <marko@ternaris.com>

* Back out Waitable and GraphEvent-related changes

Signed-off-by: Emerson Knapp <eknapp@amazon.com>

* add publisher and subscription events to AllocatorMemoryStrategy

* Add stub API for assert_liveliness

* Fix use_sim_time issue on LifeCycleNode (ros2#651)

Signed-off-by: vinnamkim <vinnam.kim@gmail.com>

* revert changes to client and services
address PR comments

Signed-off-by: Miaofei <miaofei@amazon.com>

* Add parameter-related templates to LifecycleNode (ros2#645)

* Add parameter-related templates to LifecycleNode

Signed-off-by: vinnamkim <vinnam.kim@gmail.com>

* Update rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp

Co-Authored-By: vinnamkim <vinnam.kim@gmail.com>

* Update rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp

* update API calls into rcl

* fix linter errors in rclcpp_lifecycle (ros2#672)

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

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