2.4.0

* Guard against integer overflow in duration conversion (#1584)

Guard against overflow when converting from rclcpp::Duration to builtin_interfaces::msg::Duration,
which is a unsigned to signed conversion.

Use non-std int types for consistency

Handle large negative values

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

* Fix test

rclcpp::Duration::from_nanoseconds is not available in Foxy.
Signed-off-by: Jacob Perron <jacob@openrobotics.org>

rclcpp/CHANGELOG.rst

@@ -2,6 +2,25 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.4.0 (2021-09-01)
+------------------
+* Guard against integer overflow in duration conversion (`#1584 <https://github.com/ros2/rclcpp/issues/1584>`_) (`#1761 <https://github.com/ros2/rclcpp/issues/1761>`_)
+* Update for checking correct variable (`#1534 <https://github.com/ros2/rclcpp/issues/1534>`_) (`#1760 <https://github.com/ros2/rclcpp/issues/1760>`_)
+* Fix SEGV caused by order of destruction of Node sub-interfaces (`#1469 <https://github.com/ros2/rclcpp/issues/1469>`_) (`#1736 <https://github.com/ros2/rclcpp/issues/1736>`_)
+* Add wait for message API (`#1705 <https://github.com/ros2/rclcpp/issues/1705>`_) (`#1737 <https://github.com/ros2/rclcpp/issues/1737>`_)
+* Fix documentation bug (`#1719 <https://github.com/ros2/rclcpp/issues/1719>`_) (`#1721 <https://github.com/ros2/rclcpp/issues/1721>`_)
+* Fix clock thread issue (`#1266 <https://github.com/ros2/rclcpp/issues/1266>`_) (`#1267 <https://github.com/ros2/rclcpp/issues/1267>`_) (`#1685 <https://github.com/ros2/rclcpp/issues/1685>`_)
+* Allow timers to keep up the intended rate in MultiThreadedExecutor `#1516 <https://github.com/ros2/rclcpp/issues/1516>`_ (`#1636 <https://github.com/ros2/rclcpp/issues/1636>`_)
+  Backports `#1516 <https://github.com/ros2/rclcpp/issues/1516>`_ and follow-up fix `#1628 <https://github.com/ros2/rclcpp/issues/1628>`_
+* Contributors: Chen Lihui, Colin MacKenzie, Daisuke Sato, Jacob Perron, Karsten Knese, Tomoya Fujita, hsgwa, William Woodall
+
+2.3.1 (2021-04-14)
+------------------
+* Update quality declaration links (re: `ros2/docs.ros2.org#52 <https://github.com/ros2/docs.ros2.org/issues/52>`_) (`#1616 <https://github.com/ros2/rclcpp/issues/1616>`_)
+* Fix documented example in create_publisher (`#1558 <https://github.com/ros2/rclcpp/issues/1558>`_) (`#1559 <https://github.com/ros2/rclcpp/issues/1559>`_)
+* Fix runtime error: reference binding to null pointer of type (`#1547 <https://github.com/ros2/rclcpp/issues/1547>`_) (`#1548 <https://github.com/ros2/rclcpp/issues/1548>`_)
+* Contributors: Jacob Perron, Simon Honigmann, Tomoya Fujita
+
 2.3.0 (2020-12-09)
 ------------------
 * Update QD to QL 1 (`#1480 <https://github.com/ros2/rclcpp/issues/1480>`_)

rclcpp/CMakeLists.txt

@@ -39,6 +39,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/clock.cpp
   src/rclcpp/context.cpp
   src/rclcpp/contexts/default_context.cpp
+  src/rclcpp/detail/mutex_two_priorities.cpp
   src/rclcpp/detail/rmw_implementation_specific_payload.cpp
   src/rclcpp/detail/rmw_implementation_specific_publisher_payload.cpp
   src/rclcpp/detail/rmw_implementation_specific_subscription_payload.cpp

rclcpp/QUALITY_DECLARATION.md

@@ -4,13 +4,13 @@ This document is a declaration of software quality for the `rclcpp` package, bas
 
 The package `rclcpp` claims to be in the **Quality Level 1** category when it is used with a **Quality Level 1** middleware.
 
-Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-quality-categories) of the ROS2 developer guide.
+Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://www.ros.org/reps/rep-2004.html) of the ROS2 developer guide.
 
 ## Version Policy [1]
 
 ### Version Scheme [1.i]
 
-`rclcpp` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning).
+`rclcpp` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#versioning).
 
 ### Version Stability [1.ii]
 
@@ -39,11 +39,11 @@ Headers under the folder `detail` are not considered part of the public API and
 
 ## Change Control Process [2]
 
-`rclcpp` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process).
+`rclcpp` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#change-control-process).
 
 ### Change Requests [2.i]
 
-All changes will occur through a pull request, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information.
+All changes will occur through a pull request, check [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#change-control-process) for additional information.
 
 ### Contributor Origin [2.ii]
 
@@ -51,7 +51,7 @@ This package uses DCO as its confirmation of contributor origin policy. More inf
 
 ### Peer Review Policy [2.iii]
 
-All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information.
+All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#change-control-process) for additional information.
 
 ### Continuous Integration [2.iv]
 
@@ -109,7 +109,7 @@ The tests aim to cover both typical usage and corner cases, but are quantified b
 
 ### Coverage [4.iii]
 
-`rclcpp` follows the recommendations for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#code-coverage), and opts to use line coverage instead of branch coverage.
+`rclcpp` follows the recommendations for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#code-coverage), and opts to use line coverage instead of branch coverage.
 
 This includes:
 
@@ -119,13 +119,13 @@ This includes:
 
 Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers.
 
-Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_foxy_coverage/lastCompletedBuild/cobertura/src_ros2_rclcpp_rclcpp_src_rclcpp/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://index.ros.org/doc/ros2/Contributing/ROS-2-On-boarding-Guide/#note-on-coverage-runs).
+Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_foxy_coverage/lastCompletedBuild/cobertura/src_ros2_rclcpp_rclcpp_src_rclcpp/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#note-on-coverage-runs).
 
 `rclcpp` has a line coverage `>= 95%`, which is calculated over all directories within `rclcpp` with the exception of the `experimental` directory.
 
 ### Performance [4.iv]
 
-`rclcpp` follows the recommendations for performance testing of C/C++ code in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#performance), and opts to do performance analysis on each release rather than each change.
+`rclcpp` follows the recommendations for performance testing of C/C++ code in the [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#performance), and opts to do performance analysis on each release rather than each change.
 
 The performance tests of `rclcpp` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/foxy/rclcpp/test/benchmark).
 
@@ -137,7 +137,7 @@ Changes that introduce regressions in performance must be adequately justified i
 
 ### Linters and Static Analysis [4.v]
 
-`rclcpp` uses and passes all the ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms.
+`rclcpp` uses and passes all the ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms.
 
 Currently nightly test results can be seen [here](http://build.ros2.org/view/Fpr/job/Fpr__rclcpp__ubuntu_focal_amd64/lastCompletedBuild/testReport/rclcpp).
 

rclcpp/include/rclcpp/detail/mutex_two_priorities.hpp

@@ -0,0 +1,76 @@
+// Copyright 2021 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__DETAIL__MUTEX_TWO_PRIORITIES_HPP_
+#define RCLCPP__DETAIL__MUTEX_TWO_PRIORITIES_HPP_
+
+#include <condition_variable>
+#include <mutex>
+
+namespace rclcpp
+{
+namespace detail
+{
+/// \internal A mutex that has two locking mechanism, one with higher priority than the other.
+/**
+ * After the current mutex owner release the lock, a thread that used the high
+ * priority mechanism will have priority over threads that used the low priority mechanism.
+ */
+class MutexTwoPriorities
+{
+public:
+  class HighPriorityLockable
+  {
+public:
+    explicit HighPriorityLockable(MutexTwoPriorities & parent);
+
+    void lock();
+
+    void unlock();
+
+private:
+    MutexTwoPriorities & parent_;
+  };
+
+  class LowPriorityLockable
+  {
+public:
+    explicit LowPriorityLockable(MutexTwoPriorities & parent);
+
+    void lock();
+
+    void unlock();
+
+private:
+    MutexTwoPriorities & parent_;
+  };
+
+  HighPriorityLockable
+  get_high_priority_lockable();
+
+  LowPriorityLockable
+  get_low_priority_lockable();
+
+private:
+  std::condition_variable hp_cv_;
+  std::condition_variable lp_cv_;
+  std::mutex cv_mutex_;
+  size_t hp_waiting_count_{0u};
+  bool data_taken_{false};
+};
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__MUTEX_TWO_PRIORITIES_HPP_

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -22,6 +22,7 @@
 #include <thread>
 #include <unordered_map>
 
+#include "rclcpp/detail/mutex_two_priorities.hpp"
 #include "rclcpp/executor.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/memory_strategies.hpp"
@@ -81,12 +82,17 @@ protected:
 private:
   RCLCPP_DISABLE_COPY(MultiThreadedExecutor)
 
-  std::mutex wait_mutex_;
+  std::mutex wait_mutex_;  // Unused. Leave it for ABI compatibility.
   size_t number_of_threads_;
   bool yield_before_execute_;
   std::chrono::nanoseconds next_exec_timeout_;
 
   std::set<TimerBase::SharedPtr> scheduled_timers_;
+  static std::unordered_map<MultiThreadedExecutor *,
+    std::shared_ptr<detail::MutexTwoPriorities>> wait_mutex_set_;
+  static std::mutex shared_wait_mutex_;
+  // These variables are declared as static variables for ABI-compatibiliity.
+  // And they mimic member variables needed to backport from master.
 };
 
 }  // namespace executors

rclcpp/include/rclcpp/node.hpp

@@ -161,7 +161,7 @@ public:
    * pub = node->create_publisher<MsgT>("chatter", QoS(10));  // implicitly KeepLast
    * pub = node->create_publisher<MsgT>("chatter", QoS(KeepLast(10)));
    * pub = node->create_publisher<MsgT>("chatter", QoS(KeepAll()));
-   * pub = node->create_publisher<MsgT>("chatter", QoS(1).best_effort().volatile());
+   * pub = node->create_publisher<MsgT>("chatter", QoS(1).best_effort().durability_volatile());
    * {
    *   rclcpp::QoS custom_qos(KeepLast(10), rmw_qos_profile_sensor_data);
    *   pub = node->create_publisher<MsgT>("chatter", custom_qos);
@@ -892,12 +892,15 @@ public:
   std::map<std::string, std::vector<std::string>>
   get_service_names_and_types() const;
 
-  /// Return the number of publishers that are advertised on a given topic.
+  /// Return a map of existing service names to list of service types for a specific node.
   /**
-   * \param[in] node_name the node_name on which to count the publishers.
-   * \param[in] namespace_ the namespace of the node associated with the name
-   * \return number of publishers that are advertised on a given topic.
-   * \throws std::runtime_error if publishers could not be counted
+   * This function only considers services - not clients.
+   * The returned names are the actual names used and do not have remap rules applied.
+   *
+   * \param[in] node_name name of the node.
+   * \param[in] namespace_ namespace of the node.
+   * \return a map of existing service names to list of service types.
+   * \throws std::runtime_error anything that rcl_error can throw.
    */
   RCLCPP_PUBLIC
   std::map<std::string, std::vector<std::string>>

rclcpp/include/rclcpp/publisher_options.hpp

@@ -92,12 +92,7 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase
   get_allocator() const
   {
     if (!this->allocator) {
-      // TODO(wjwwood): I would like to use the commented line instead, but
-      //   cppcheck 1.89 fails with:
-      //     Syntax Error: AST broken, binary operator '>' doesn't have two operands.
-      // return std::make_shared<Allocator>();
-      std::shared_ptr<Allocator> tmp(new Allocator());
-      return tmp;
+      return std::make_shared<Allocator>();
     }
     return this->allocator;
   }

rclcpp/include/rclcpp/service.hpp

@@ -180,7 +180,7 @@ public:
     std::weak_ptr<rcl_node_t> weak_node_handle(node_handle_);
     // rcl does the static memory allocation here
     service_handle_ = std::shared_ptr<rcl_service_t>(
-      new rcl_service_t, [weak_node_handle](rcl_service_t * service)
+      new rcl_service_t, [weak_node_handle, service_name](rcl_service_t * service)
       {
         auto handle = weak_node_handle.lock();
         if (handle) {
@@ -192,10 +192,10 @@ public:
             rcl_reset_error();
           }
         } else {
-          RCLCPP_ERROR(
+          RCLCPP_ERROR_STREAM(
             rclcpp::get_logger("rclcpp"),
-            "Error in destruction of rcl service handle: "
-            "the Node Handle was destructed too early. You will leak memory");
+            "Error in destruction of rcl service handle " << service_name <<
+              ": the Node Handle was destructed too early. You will leak memory");
         }
         delete service;
       });

rclcpp/include/rclcpp/subscription_options.hpp

@@ -101,7 +101,7 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
     rcl_subscription_options_t result = rcl_subscription_get_default_options();
     using AllocatorTraits = std::allocator_traits<Allocator>;
     using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc<MessageT>;
-    auto message_alloc = std::make_shared<MessageAllocatorT>(*allocator.get());
+    auto message_alloc = std::make_shared<MessageAllocatorT>(*this->get_allocator().get());
     result.allocator = allocator::get_rcl_allocator<MessageT>(*message_alloc);
     result.qos = qos.get_rmw_qos_profile();
     result.rmw_subscription_options.ignore_local_publications = this->ignore_local_publications;

rclcpp/include/rclcpp/time_source.hpp

@@ -148,8 +148,6 @@ private:
   void disable_ros_time();
 
   // Internal helper functions used inside iterators
-  static void enable_ros_time(rclcpp::Clock::SharedPtr clock);
-  static void disable_ros_time(rclcpp::Clock::SharedPtr clock);
   static void set_clock(
     const builtin_interfaces::msg::Time::SharedPtr msg,
     bool set_ros_time_enabled,

rclcpp/include/rclcpp/wait_for_message.hpp

@@ -0,0 +1,100 @@
+// Copyright 2021 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__WAIT_FOR_MESSAGE_HPP_
+#define RCLCPP__WAIT_FOR_MESSAGE_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/node.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/wait_set.hpp"
+
+namespace rclcpp
+{
+/// Wait for the next incoming message.
+/**
+ * Given an already initialized subscription,
+ * wait for the next incoming message to arrive before the specified timeout.
+ *
+ * \param[out] out is the message to be filled when a new message is arriving.
+ * \param[in] subscription shared pointer to a previously initialized subscription.
+ * \param[in] context shared pointer to a context to watch for SIGINT requests.
+ * \param[in] time_to_wait parameter specifying the timeout before returning.
+ * \return true if a message was successfully received, false if message could not
+ * be obtained or shutdown was triggered asynchronously on the context.
+ */
+template<class MsgT, class Rep = int64_t, class Period = std::milli>
+bool wait_for_message(
+  MsgT & out,
+  std::shared_ptr<rclcpp::Subscription<MsgT>> subscription,
+  std::shared_ptr<rclcpp::Context> context,
+  std::chrono::duration<Rep, Period> time_to_wait = std::chrono::duration<Rep, Period>(-1))
+{
+  auto gc = std::make_shared<rclcpp::GuardCondition>(context);
+  auto shutdown_callback = context->on_shutdown(
+    [weak_gc = std::weak_ptr<rclcpp::GuardCondition>{gc}]() {
+      auto strong_gc = weak_gc.lock();
+      if (strong_gc) {
+        strong_gc->trigger();
+      }
+    });
+
+  rclcpp::WaitSet wait_set;
+  wait_set.add_subscription(subscription);
+  wait_set.add_guard_condition(gc);
+  auto ret = wait_set.wait(time_to_wait);
+  if (ret.kind() != rclcpp::WaitResultKind::Ready) {
+    return false;
+  }
+
+  if (wait_set.get_rcl_wait_set().guard_conditions[0]) {
+    return false;
+  }
+
+  rclcpp::MessageInfo info;
+  if (!subscription->take(out, info)) {
+    return false;
+  }
+
+  return true;
+}
+
+/// Wait for the next incoming message.
+/**
+ * Wait for the next incoming message to arrive on a specified topic before the specified timeout.
+ *
+ * \param[out] out is the message to be filled when a new message is arriving.
+ * \param[in] node the node pointer to initialize the subscription on.
+ * \param[in] topic the topic to wait for messages.
+ * \param[in] time_to_wait parameter specifying the timeout before returning.
+ * \return true if a message was successfully received, false if message could not
+ * be obtained or shutdown was triggered asynchronously on the context.
+ */
+template<class MsgT, class Rep = int64_t, class Period = std::milli>
+bool wait_for_message(
+  MsgT & out,
+  rclcpp::Node::SharedPtr node,
+  const std::string & topic,
+  std::chrono::duration<Rep, Period> time_to_wait = std::chrono::duration<Rep, Period>(-1))
+{
+  auto sub = node->create_subscription<MsgT>(topic, 1, [](const std::shared_ptr<MsgT>) {});
+  return wait_for_message<MsgT, Rep, Period>(
+    out, sub, node->get_node_options().context(), time_to_wait);
+}
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_FOR_MESSAGE_HPP_

rclcpp/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp</name>
-  <version>2.3.0</version>
+  <version>2.4.0</version>
   <description>The ROS client library in C++.</description>
   <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
   <license>Apache License 2.0</license>

rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp

@@ -0,0 +1,104 @@
+// Copyright 2021 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/detail/mutex_two_priorities.hpp"
+
+#include <mutex>
+
+namespace rclcpp
+{
+namespace detail
+{
+
+using LowPriorityLockable = MutexTwoPriorities::LowPriorityLockable;
+using HighPriorityLockable = MutexTwoPriorities::HighPriorityLockable;
+
+HighPriorityLockable::HighPriorityLockable(MutexTwoPriorities & parent)
+: parent_(parent)
+{}
+
+void
+HighPriorityLockable::lock()
+{
+  std::unique_lock<std::mutex> guard{parent_.cv_mutex_};
+  if (parent_.data_taken_) {
+    ++parent_.hp_waiting_count_;
+    while (parent_.data_taken_) {
+      parent_.hp_cv_.wait(guard);
+    }
+    --parent_.hp_waiting_count_;
+  }
+  parent_.data_taken_ = true;
+}
+
+void
+HighPriorityLockable::unlock()
+{
+  bool notify_lp{false};
+  {
+    std::lock_guard<std::mutex> guard{parent_.cv_mutex_};
+    parent_.data_taken_ = false;
+    notify_lp = 0u == parent_.hp_waiting_count_;
+  }
+  if (notify_lp) {
+    parent_.lp_cv_.notify_one();
+  } else {
+    parent_.hp_cv_.notify_one();
+  }
+}
+
+LowPriorityLockable::LowPriorityLockable(MutexTwoPriorities & parent)
+: parent_(parent)
+{}
+
+void
+LowPriorityLockable::lock()
+{
+  std::unique_lock<std::mutex> guard{parent_.cv_mutex_};
+  while (parent_.data_taken_ || parent_.hp_waiting_count_) {
+    parent_.lp_cv_.wait(guard);
+  }
+  parent_.data_taken_ = true;
+}
+
+void
+LowPriorityLockable::unlock()
+{
+  bool notify_lp{false};
+  {
+    std::lock_guard<std::mutex> guard{parent_.cv_mutex_};
+    parent_.data_taken_ = false;
+    notify_lp = 0u == parent_.hp_waiting_count_;
+  }
+  if (notify_lp) {
+    parent_.lp_cv_.notify_one();
+  } else {
+    parent_.hp_cv_.notify_one();
+  }
+}
+
+HighPriorityLockable
+MutexTwoPriorities::get_high_priority_lockable()
+{
+  return HighPriorityLockable{*this};
+}
+
+LowPriorityLockable
+MutexTwoPriorities::get_low_priority_lockable()
+{
+  return LowPriorityLockable{*this};
+}
+
+}  // namespace detail
+}  // namespace rclcpp

rclcpp/src/rclcpp/duration.cpp

@@ -67,13 +67,27 @@ Duration::operator builtin_interfaces::msg::Duration() const
 {
   builtin_interfaces::msg::Duration msg_duration;
   constexpr rcl_duration_value_t kDivisor = RCL_S_TO_NS(1);
+  constexpr int32_t max_s = std::numeric_limits<int32_t>::max();
+  constexpr int32_t min_s = std::numeric_limits<int32_t>::min();
+  constexpr uint32_t max_ns = std::numeric_limits<uint32_t>::max();
   const auto result = std::div(rcl_duration_.nanoseconds, kDivisor);
   if (result.rem >= 0) {
-    msg_duration.sec = static_cast<std::int32_t>(result.quot);
-    msg_duration.nanosec = static_cast<std::uint32_t>(result.rem);
+    // saturate if we will overflow
+    if (result.quot > max_s) {
+      msg_duration.sec = max_s;
+      msg_duration.nanosec = max_ns;
+    } else {
+      msg_duration.sec = static_cast<int32_t>(result.quot);
+      msg_duration.nanosec = static_cast<uint32_t>(result.rem);
+    }
   } else {
-    msg_duration.sec = static_cast<std::int32_t>(result.quot - 1);
-    msg_duration.nanosec = static_cast<std::uint32_t>(kDivisor + result.rem);
+    if (result.quot <= min_s) {
+      msg_duration.sec = min_s;
+      msg_duration.nanosec = 0u;
+    } else {
+      msg_duration.sec = static_cast<int32_t>(result.quot - 1);
+      msg_duration.nanosec = static_cast<uint32_t>(kDivisor + result.rem);
+    }
   }
   return msg_duration;
 }
@@ -238,11 +252,14 @@ Duration::to_rmw_time() const
     throw std::runtime_error("rmw_time_t cannot be negative");
   }
 
-  // reuse conversion logic from msg creation
-  builtin_interfaces::msg::Duration msg = *this;
+  // Purposefully avoid creating from builtin_interfaces::msg::Duration
+  // to avoid possible overflow converting from int64_t to int32_t, then back to uint64_t
   rmw_time_t result;
-  result.sec = static_cast<uint64_t>(msg.sec);
-  result.nsec = static_cast<uint64_t>(msg.nanosec);
+  constexpr rcl_duration_value_t kDivisor = RCL_S_TO_NS(1);
+  const auto div_result = std::div(rcl_duration_.nanoseconds, kDivisor);
+  result.sec = static_cast<uint64_t>(div_result.quot);
+  result.nsec = static_cast<uint64_t>(div_result.rem);
+
   return result;
 }
 

rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp

@@ -17,13 +17,19 @@
 #include <chrono>
 #include <functional>
 #include <memory>
+#include <unordered_map>
 #include <vector>
 
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/scope_exit.hpp"
 
+using rclcpp::detail::MutexTwoPriorities;
 using rclcpp::executors::MultiThreadedExecutor;
 
+std::unordered_map<MultiThreadedExecutor *, std::shared_ptr<MutexTwoPriorities>>
+MultiThreadedExecutor::wait_mutex_set_;
+std::mutex MultiThreadedExecutor::shared_wait_mutex_;
+
 MultiThreadedExecutor::MultiThreadedExecutor(
   const rclcpp::ExecutorOptions & options,
   size_t number_of_threads,
@@ -33,6 +39,11 @@ MultiThreadedExecutor::MultiThreadedExecutor(
   yield_before_execute_(yield_before_execute),
   next_exec_timeout_(next_exec_timeout)
 {
+  {
+    std::lock_guard<std::mutex> wait_lock(
+      MultiThreadedExecutor::shared_wait_mutex_);
+    wait_mutex_set_[this] = std::make_shared<MutexTwoPriorities>();
+  }
   number_of_threads_ = number_of_threads ? number_of_threads : std::thread::hardware_concurrency();
   if (number_of_threads_ == 0) {
     number_of_threads_ = 1;
@@ -51,7 +62,9 @@ MultiThreadedExecutor::spin()
   std::vector<std::thread> threads;
   size_t thread_id = 0;
   {
-    std::lock_guard<std::mutex> wait_lock(wait_mutex_);
+    auto wait_mutex = MultiThreadedExecutor::wait_mutex_set_[this];
+    auto low_priority_wait_mutex = wait_mutex->get_low_priority_lockable();
+    std::lock_guard<MutexTwoPriorities::LowPriorityLockable> wait_lock(low_priority_wait_mutex);
     for (; thread_id < number_of_threads_ - 1; ++thread_id) {
       auto func = std::bind(&MultiThreadedExecutor::run, this, thread_id);
       threads.emplace_back(func);
@@ -76,7 +89,9 @@ MultiThreadedExecutor::run(size_t)
   while (rclcpp::ok(this->context_) && spinning.load()) {
     rclcpp::AnyExecutable any_exec;
     {
-      std::lock_guard<std::mutex> wait_lock(wait_mutex_);
+      auto wait_mutex = MultiThreadedExecutor::wait_mutex_set_[this];
+      auto low_priority_wait_mutex = wait_mutex->get_low_priority_lockable();
+      std::lock_guard<MutexTwoPriorities::LowPriorityLockable> wait_lock(low_priority_wait_mutex);
       if (!rclcpp::ok(this->context_) || !spinning.load()) {
         return;
       }
@@ -103,7 +118,9 @@ MultiThreadedExecutor::run(size_t)
     execute_any_executable(any_exec);
 
     if (any_exec.timer) {
-      std::lock_guard<std::mutex> wait_lock(wait_mutex_);
+      auto wait_mutex = MultiThreadedExecutor::wait_mutex_set_[this];
+      auto high_priority_wait_mutex = wait_mutex->get_high_priority_lockable();
+      std::lock_guard<MutexTwoPriorities::HighPriorityLockable> wait_lock(high_priority_wait_mutex);
       auto it = scheduled_timers_.find(any_exec.timer);
       if (it != scheduled_timers_.end()) {
         scheduled_timers_.erase(it);

rclcpp/src/rclcpp/node.cpp

@@ -184,7 +184,18 @@ Node::Node(
 }
 
 Node::~Node()
-{}
+{
+  // release sub-interfaces in an order that allows them to consult with node_base during tear-down
+  node_waitables_.reset();
+  node_time_source_.reset();
+  node_parameters_.reset();
+  node_clock_.reset();
+  node_services_.reset();
+  node_topics_.reset();
+  node_timers_.reset();
+  node_logging_.reset();
+  node_graph_.reset();
+}
 
 const char *
 Node::get_name() const

rclcpp/src/rclcpp/time_source.cpp

@@ -185,11 +185,21 @@ void TimeSource::set_clock(
   const builtin_interfaces::msg::Time::SharedPtr msg, bool set_ros_time_enabled,
   std::shared_ptr<rclcpp::Clock> clock)
 {
+  std::lock_guard<std::mutex> clock_guard(clock->get_clock_mutex());
+
   // Do change
   if (!set_ros_time_enabled && clock->ros_time_is_active()) {
-    disable_ros_time(clock);
+    auto ret = rcl_disable_ros_time_override(clock->get_clock_handle());
+    if (ret != RCL_RET_OK) {
+      rclcpp::exceptions::throw_from_rcl_error(
+        ret, "Failed to disable ros_time_override_status");
+    }
   } else if (set_ros_time_enabled && !clock->ros_time_is_active()) {
-    enable_ros_time(clock);
+    auto ret = rcl_enable_ros_time_override(clock->get_clock_handle());
+    if (ret != RCL_RET_OK) {
+      rclcpp::exceptions::throw_from_rcl_error(
+        ret, "Failed to enable ros_time_override_status");
+    }
   }
 
   auto ret = rcl_set_ros_time_override(clock->get_clock_handle(), rclcpp::Time(*msg).nanoseconds());
@@ -273,24 +283,6 @@ void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::S
   }
 }
 
-void TimeSource::enable_ros_time(std::shared_ptr<rclcpp::Clock> clock)
-{
-  auto ret = rcl_enable_ros_time_override(clock->get_clock_handle());
-  if (ret != RCL_RET_OK) {
-    rclcpp::exceptions::throw_from_rcl_error(
-      ret, "Failed to enable ros_time_override_status");
-  }
-}
-
-void TimeSource::disable_ros_time(std::shared_ptr<rclcpp::Clock> clock)
-{
-  auto ret = rcl_disable_ros_time_override(clock->get_clock_handle());
-  if (ret != RCL_RET_OK) {
-    rclcpp::exceptions::throw_from_rcl_error(
-      ret, "Failed to enable ros_time_override_status");
-  }
-}
-
 void TimeSource::enable_ros_time()
 {
   if (ros_time_active_) {

rclcpp/test/rclcpp/CMakeLists.txt

@@ -508,6 +508,13 @@ if(TARGET test_utilities)
   target_link_libraries(test_utilities ${PROJECT_NAME} mimick)
 endif()
 
+ament_add_gtest(test_wait_for_message test_wait_for_message.cpp)
+if(TARGET test_wait_for_message)
+  ament_target_dependencies(test_wait_for_message
+    "test_msgs")
+  target_link_libraries(test_wait_for_message ${PROJECT_NAME})
+endif()
+
 ament_add_gtest(test_init test_init.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}")
 if(TARGET test_init)

rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp

@@ -125,7 +125,7 @@ TEST_F(TestNodeParameters, parameter_overrides)
   auto * node_parameters_interface =
     dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
     node2->get_node_parameters_interface().get());
-  ASSERT_NE(nullptr, node_parameters);
+  ASSERT_NE(nullptr, node_parameters_interface);
 
   const auto & parameter_overrides = node_parameters_interface->get_parameter_overrides();
   EXPECT_EQ(2u, parameter_overrides.size());

rclcpp/test/rclcpp/test_duration.cpp

@@ -138,6 +138,7 @@ TEST_F(TestDuration, maximum_duration) {
 static const int64_t HALF_SEC_IN_NS = 500 * 1000 * 1000;
 static const int64_t ONE_SEC_IN_NS = 1000 * 1000 * 1000;
 static const int64_t ONE_AND_HALF_SEC_IN_NS = 3 * HALF_SEC_IN_NS;
+static const int64_t MAX_NANOSECONDS = std::numeric_limits<int64_t>::max();
 
 TEST_F(TestDuration, from_seconds) {
   EXPECT_EQ(rclcpp::Duration(0), rclcpp::Duration::from_seconds(0.0));
@@ -237,6 +238,34 @@ TEST_F(TestDuration, conversions) {
     auto chrono_duration = duration.to_chrono<std::chrono::nanoseconds>();
     EXPECT_EQ(chrono_duration.count(), -ONE_AND_HALF_SEC_IN_NS);
   }
+
+  {
+    auto duration = rclcpp::Duration(MAX_NANOSECONDS);
+
+    const auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(duration);
+    EXPECT_EQ(duration_msg.sec, std::numeric_limits<int32_t>::max());
+    EXPECT_EQ(duration_msg.nanosec, std::numeric_limits<uint32_t>::max());
+
+    auto rmw_time = duration.to_rmw_time();
+    EXPECT_EQ(rmw_time.sec, 9223372036u);
+    EXPECT_EQ(rmw_time.nsec, 854775807u);
+
+    auto chrono_duration = duration.to_chrono<std::chrono::nanoseconds>();
+    EXPECT_EQ(chrono_duration.count(), MAX_NANOSECONDS);
+  }
+
+  {
+    auto duration = rclcpp::Duration(-MAX_NANOSECONDS);
+
+    const auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(duration);
+    EXPECT_EQ(duration_msg.sec, std::numeric_limits<int32_t>::min());
+    EXPECT_EQ(duration_msg.nanosec, 0u);
+
+    EXPECT_THROW(duration.to_rmw_time(), std::runtime_error);
+
+    auto chrono_duration = duration.to_chrono<std::chrono::nanoseconds>();
+    EXPECT_EQ(chrono_duration.count(), -MAX_NANOSECONDS);
+  }
 }
 
 TEST_F(TestDuration, test_some_constructors) {

rclcpp/test/rclcpp/test_wait_for_message.cpp

@@ -0,0 +1,74 @@
+// Copyright 2021 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 <gtest/gtest.h>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/node.hpp"
+#include "rclcpp/wait_for_message.hpp"
+
+#include "test_msgs/msg/strings.hpp"
+#include "test_msgs/message_fixtures.hpp"
+
+using namespace std::chrono_literals;
+
+TEST(TestUtilities, wait_for_message) {
+  rclcpp::init(0, nullptr);
+
+  auto node = std::make_shared<rclcpp::Node>("wait_for_message_node");
+
+  using MsgT = test_msgs::msg::Strings;
+  auto pub = node->create_publisher<MsgT>("wait_for_message_topic", 10);
+
+  MsgT out;
+  auto received = false;
+  auto wait = std::async(
+    [&]() {
+      auto ret = rclcpp::wait_for_message(out, node, "wait_for_message_topic", 5s);
+      EXPECT_TRUE(ret);
+      received = true;
+    });
+
+  for (auto i = 0u; i < 10 && received == false; ++i) {
+    pub->publish(*get_messages_strings()[0]);
+    std::this_thread::sleep_for(1s);
+  }
+  ASSERT_TRUE(received);
+  EXPECT_EQ(out, *get_messages_strings()[0]);
+
+  rclcpp::shutdown();
+}
+
+TEST(TestUtilities, wait_for_message_indefinitely) {
+  rclcpp::init(0, nullptr);
+
+  auto node = std::make_shared<rclcpp::Node>("wait_for_message_node2");
+
+  using MsgT = test_msgs::msg::Strings;
+  MsgT out;
+  auto received = false;
+  auto wait = std::async(
+    [&]() {
+      auto ret = rclcpp::wait_for_message(out, node, "wait_for_message_topic" /*, -1 */);
+      EXPECT_TRUE(ret);
+      received = true;
+    });
+
+  rclcpp::shutdown();
+
+  ASSERT_FALSE(received);
+}

rclcpp_action/CHANGELOG.rst

@@ -3,6 +3,20 @@ Changelog for package rclcpp_action
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+2.4.0 (2021-09-01)
+------------------
+* Fix occasionally missing goal result caused by race condition (`#1677 <https://github.com/ros2/rclcpp/issues/1677>`_) (`#1682 <https://github.com/ros2/rclcpp/issues/1682>`_)
+* Returns CancelResponse::REJECT while goal handle failed to transit to CANCELING state (`#1641 <https://github.com/ros2/rclcpp/issues/1641>`_) (`#1659 <https://github.com/ros2/rclcpp/issues/1659>`_)
+* Fix action server deadlock issue that caused by other mutexes locked in CancelCallback (`#1635 <https://github.com/ros2/rclcpp/issues/1635>`_) (`#1654 <https://github.com/ros2/rclcpp/issues/1654>`_)
+* Contributors: Kaven Yau, Tomoya Fujita
+
+2.3.1 (2021-04-14)
+------------------
+* Update quality declaration links (re: `ros2/docs.ros2.org#52 <https://github.com/ros2/docs.ros2.org/issues/52>`_) (`#1616 <https://github.com/ros2/rclcpp/issues/1616>`_)
+* node_handle must be destroyed after client_handle to prevent memory leak (`#1562 <https://github.com/ros2/rclcpp/issues/1562>`_) (`#1565 <https://github.com/ros2/rclcpp/issues/1565>`_)
+* Fix action server deadlock (`#1285 <https://github.com/ros2/rclcpp/issues/1285>`_) (`#1313 <https://github.com/ros2/rclcpp/issues/1313>`_)  (`#1518 <https://github.com/ros2/rclcpp/issues/1518>`_)
+* Contributors: Chris Lalancette, Daisuke Sato, Simon Honigmann, Tomoya Fujita
+
 2.3.0 (2020-12-09)
 ------------------
 * Update QD to QL 1 (`#1480 <https://github.com/ros2/rclcpp/issues/1480>`_)

rclcpp_action/QUALITY_DECLARATION.md

@@ -4,13 +4,13 @@ This document is a declaration of software quality for the `rclcpp_action` packa
 
 The package `rclcpp_action` claims to be in the **Quality Level 1** category when it is used with a **Quality Level 1** middleware.
 
-Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-quality-categories) of the ROS2 developer guide.
+Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://www.ros.org/reps/rep-2004.html) of the ROS2 developer guide.
 
 ## Version Policy [1]
 
 ### Version Scheme [1.i]
 
-`rclcpp_action` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning).
+`rclcpp_action` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#versioning).
 
 ### Version Stability [1.ii]
 
@@ -37,11 +37,11 @@ All installed headers are in the `include` directory of the package, headers in
 
 ## Change Control Process [2]
 
-`rclcpp_action` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process).
+`rclcpp_action` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#change-control-process).
 
 ### Change Requests [2.i]
 
-All changes will occur through a pull request, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information.
+All changes will occur through a pull request, check [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#change-control-process) for additional information.
 
 ### Contributor Origin [2.ii]
 
@@ -49,7 +49,7 @@ This package uses DCO as its confirmation of contributor origin policy. More inf
 
 ### Peer Review Policy [2.iii]
 
-All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information.
+All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#change-control-process) for additional information.
 
 ### Continuous Integration [2.iv]
 
@@ -107,7 +107,7 @@ The tests aim to cover both typical usage and corner cases, but are quantified b
 
 ### Coverage [4.iii]
 
-`rclcpp_action` follows the recommendations for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#code-coverage), and opts to use line coverage instead of branch coverage.
+`rclcpp_action` follows the recommendations for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#code-coverage), and opts to use line coverage instead of branch coverage.
 
 This includes:
 
@@ -117,11 +117,11 @@ This includes:
 
 Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers.
 
-Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_foxy_coverage/lastCompletedBuild/cobertura/src_ros2_rclcpp_rclcpp_action_src/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://index.ros.org/doc/ros2/Contributing/ROS-2-On-boarding-Guide/#note-on-coverage-runs).
+Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_foxy_coverage/lastCompletedBuild/cobertura/src_ros2_rclcpp_rclcpp_action_src/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#note-on-coverage-runs).
 
 ### Performance [4.iv]
 
-`rclcpp_action` follows the recommendations for performance testing of C/C++ code in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#performance), and opts to do performance analysis on each release rather than each change.
+`rclcpp_action` follows the recommendations for performance testing of C/C++ code in the [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#performance), and opts to do performance analysis on each release rather than each change.
 
 The performance tests of `rclcpp_action` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/foxy/rclcpp_action/test/benchmark).
 
@@ -133,7 +133,7 @@ Changes that introduce regressions in performance must be adequately justified i
 
 ### Linters and Static Analysis [4.v]
 
-`rclcpp_action` uses and passes all the ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms.
+`rclcpp_action` uses and passes all the ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms.
 
 Currently nightly test results can be seen [here](http://build.ros2.org/view/Fpr/job/Fpr__rclcpp__ubuntu_focal_amd64/lastCompletedBuild/testReport/rclcpp_action).
 

rclcpp_action/include/rclcpp_action/server.hpp

@@ -352,15 +352,26 @@ protected:
   CancelResponse
   call_handle_cancel_callback(const GoalUUID & uuid) override
   {
-    std::lock_guard<std::mutex> lock(goal_handles_mutex_);
+    std::shared_ptr<ServerGoalHandle<ActionT>> goal_handle;
+    {
+      std::lock_guard<std::mutex> lock(goal_handles_mutex_);
+      auto element = goal_handles_.find(uuid);
+      if (element != goal_handles_.end()) {
+        goal_handle = element->second.lock();
+      }
+    }
+
     CancelResponse resp = CancelResponse::REJECT;
-    auto element = goal_handles_.find(uuid);
-    if (element != goal_handles_.end()) {
-      std::shared_ptr<ServerGoalHandle<ActionT>> goal_handle = element->second.lock();
-      if (goal_handle) {
-        resp = handle_cancel_(goal_handle);
-        if (CancelResponse::ACCEPT == resp) {
+    if (goal_handle) {
+      resp = handle_cancel_(goal_handle);
+      if (CancelResponse::ACCEPT == resp) {
+        try {
           goal_handle->_cancel_goal();
+        } catch (const rclcpp::exceptions::RCLError & ex) {
+          RCLCPP_DEBUG(
+            rclcpp::get_logger("rclcpp_action"),
+            "Failed to cancel goal in call_handle_cancel_callback: %s", ex.what());
+          return CancelResponse::REJECT;
         }
       }
     }

rclcpp_action/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp_action</name>
-  <version>2.3.0</version>
+  <version>2.4.0</version>
   <description>Adds action APIs for C++.</description>
   <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
   <license>Apache License 2.0</license>

rclcpp_action/src/client.cpp

@@ -100,8 +100,9 @@ public:
 
   rclcpp::Context::SharedPtr context_;
   rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
-  std::shared_ptr<rcl_action_client_t> client_handle{nullptr};
+  // node_handle must be destroyed after client_handle to prevent memory leak
   std::shared_ptr<rcl_node_t> node_handle{nullptr};
+  std::shared_ptr<rcl_action_client_t> client_handle{nullptr};
   rclcpp::Logger logger;
 
   using ResponseCallback = std::function<void (std::shared_ptr<void> response)>;

rclcpp_action/src/server.cpp

@@ -43,8 +43,8 @@ public:
   {
   }
 
-  // Lock everything except user callbacks
-  std::recursive_mutex reentrant_mutex_;
+  // Lock for action_server_
+  std::recursive_mutex action_server_reentrant_mutex_;
 
   std::shared_ptr<rcl_action_server_t> action_server_;
 
@@ -56,10 +56,13 @@ public:
   size_t num_services_ = 0;
   size_t num_guard_conditions_ = 0;
 
-  bool goal_request_ready_ = false;
-  bool cancel_request_ready_ = false;
-  bool result_request_ready_ = false;
-  bool goal_expired_ = false;
+  std::atomic<bool> goal_request_ready_{false};
+  std::atomic<bool> cancel_request_ready_{false};
+  std::atomic<bool> result_request_ready_{false};
+  std::atomic<bool> goal_expired_{false};
+
+  // Lock for unordered_maps
+  std::recursive_mutex unordered_map_mutex_;
 
   // Results to be kept until the goal expires after reaching a terminal state
   std::unordered_map<GoalUUID, std::shared_ptr<void>> goal_results_;
@@ -88,12 +91,13 @@ ServerBase::ServerBase(
       if (nullptr != ptr) {
         rcl_node_t * rcl_node = node_base->get_rcl_node_handle();
         rcl_ret_t ret = rcl_action_server_fini(ptr, rcl_node);
-        (void)ret;
-        RCLCPP_DEBUG(
-          rclcpp::get_logger("rclcpp_action"),
-          "failed to fini rcl_action_server_t in deleter");
+        if (RCL_RET_OK != ret) {
+          RCLCPP_DEBUG(
+            rclcpp::get_logger("rclcpp_action"),
+            "failed to fini rcl_action_server_t in deleter");
+        }
+        delete ptr;
       }
-      delete ptr;
     };
 
   pimpl_->action_server_.reset(new rcl_action_server_t, deleter);
@@ -159,7 +163,7 @@ ServerBase::get_number_of_ready_guard_conditions()
 bool
 ServerBase::add_to_wait_set(rcl_wait_set_t * wait_set)
 {
-  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
   rcl_ret_t ret = rcl_action_wait_set_add_action_server(
     wait_set, pimpl_->action_server_.get(), NULL);
   return RCL_RET_OK == ret;
@@ -168,35 +172,47 @@ ServerBase::add_to_wait_set(rcl_wait_set_t * wait_set)
 bool
 ServerBase::is_ready(rcl_wait_set_t * wait_set)
 {
-  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
-  rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready(
-    wait_set,
-    pimpl_->action_server_.get(),
-    &pimpl_->goal_request_ready_,
-    &pimpl_->cancel_request_ready_,
-    &pimpl_->result_request_ready_,
-    &pimpl_->goal_expired_);
+  bool goal_request_ready;
+  bool cancel_request_ready;
+  bool result_request_ready;
+  bool goal_expired;
+  rcl_ret_t ret;
+  {
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+    ret = rcl_action_server_wait_set_get_entities_ready(
+      wait_set,
+      pimpl_->action_server_.get(),
+      &goal_request_ready,
+      &cancel_request_ready,
+      &result_request_ready,
+      &goal_expired);
+  }
+
+  pimpl_->goal_request_ready_ = goal_request_ready;
+  pimpl_->cancel_request_ready_ = cancel_request_ready;
+  pimpl_->result_request_ready_ = result_request_ready;
+  pimpl_->goal_expired_ = goal_expired;
 
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
 
-  return pimpl_->goal_request_ready_ ||
-         pimpl_->cancel_request_ready_ ||
-         pimpl_->result_request_ready_ ||
-         pimpl_->goal_expired_;
+  return pimpl_->goal_request_ready_.load() ||
+         pimpl_->cancel_request_ready_.load() ||
+         pimpl_->result_request_ready_.load() ||
+         pimpl_->goal_expired_.load();
 }
 
 void
 ServerBase::execute()
 {
-  if (pimpl_->goal_request_ready_) {
+  if (pimpl_->goal_request_ready_.load()) {
     execute_goal_request_received();
-  } else if (pimpl_->cancel_request_ready_) {
+  } else if (pimpl_->cancel_request_ready_.load()) {
     execute_cancel_request_received();
-  } else if (pimpl_->result_request_ready_) {
+  } else if (pimpl_->result_request_ready_.load()) {
     execute_result_request_received();
-  } else if (pimpl_->goal_expired_) {
+  } else if (pimpl_->goal_expired_.load()) {
     execute_check_expired_goals();
   } else {
     throw std::runtime_error("Executing action server but nothing is ready");
@@ -210,15 +226,19 @@ ServerBase::execute_goal_request_received()
   rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info();
   rmw_request_id_t request_header;
 
-  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
-
   std::shared_ptr<void> message = create_goal_request();
-  ret = rcl_action_take_goal_request(
-    pimpl_->action_server_.get(),
-    &request_header,
-    message.get());
+  {
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+    ret = rcl_action_take_goal_request(
+      pimpl_->action_server_.get(),
+      &request_header,
+      message.get());
+  }
 
-  pimpl_->goal_request_ready_ = false;
+  bool expected = true;
+  if (!pimpl_->goal_request_ready_.compare_exchange_strong(expected, false)) {
+    return;
+  }
 
   if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) {
     // Ignore take failure because connext fails if it receives a sample without valid data.
@@ -235,10 +255,13 @@ ServerBase::execute_goal_request_received()
   // Call user's callback, getting the user's response and a ros message to send back
   auto response_pair = call_handle_goal_callback(uuid, message);
 
-  ret = rcl_action_send_goal_response(
-    pimpl_->action_server_.get(),
-    &request_header,
-    response_pair.second.get());
+  {
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+    ret = rcl_action_send_goal_response(
+      pimpl_->action_server_.get(),
+      &request_header,
+      response_pair.second.get());
+  }
 
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
@@ -254,15 +277,19 @@ ServerBase::execute_goal_request_received()
       {
         if (nullptr != ptr) {
           rcl_ret_t fail_ret = rcl_action_goal_handle_fini(ptr);
-          (void)fail_ret;
-          RCLCPP_DEBUG(
-            rclcpp::get_logger("rclcpp_action"),
-            "failed to fini rcl_action_goal_handle_t in deleter");
+          if (RCL_RET_OK != fail_ret) {
+            RCLCPP_DEBUG(
+              rclcpp::get_logger("rclcpp_action"),
+              "failed to fini rcl_action_goal_handle_t in deleter");
+          }
           delete ptr;
         }
       };
     rcl_action_goal_handle_t * rcl_handle;
-    rcl_handle = rcl_action_accept_new_goal(pimpl_->action_server_.get(), &goal_info);
+    {
+      std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+      rcl_handle = rcl_action_accept_new_goal(pimpl_->action_server_.get(), &goal_info);
+    }
     if (!rcl_handle) {
       throw std::runtime_error("Failed to accept new goal\n");
     }
@@ -271,7 +298,11 @@ ServerBase::execute_goal_request_received()
     // Copy out goal handle since action server storage disappears when it is fini'd
     *handle = *rcl_handle;
 
-    pimpl_->goal_handles_[uuid] = handle;
+
+    {
+      std::lock_guard<std::recursive_mutex> lock(pimpl_->unordered_map_mutex_);
+      pimpl_->goal_handles_[uuid] = handle;
+    }
 
     if (GoalResponse::ACCEPT_AND_EXECUTE == status) {
       // Change status to executing
@@ -297,13 +328,18 @@ ServerBase::execute_cancel_request_received()
   // Initialize cancel request
   auto request = std::make_shared<action_msgs::srv::CancelGoal::Request>();
 
-  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
-  ret = rcl_action_take_cancel_request(
-    pimpl_->action_server_.get(),
-    &request_header,
-    request.get());
+  {
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+    ret = rcl_action_take_cancel_request(
+      pimpl_->action_server_.get(),
+      &request_header,
+      request.get());
+  }
 
-  pimpl_->cancel_request_ready_ = false;
+  bool expected = true;
+  if (!pimpl_->cancel_request_ready_.compare_exchange_strong(expected, false)) {
+    return;
+  }
 
   if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) {
     // Ignore take failure because connext fails if it receives a sample without valid data.
@@ -323,10 +359,14 @@ ServerBase::execute_cancel_request_received()
   // Get a list of goal info that should be attempted to be cancelled
   rcl_action_cancel_response_t cancel_response = rcl_action_get_zero_initialized_cancel_response();
 
-  ret = rcl_action_process_cancel_request(
-    pimpl_->action_server_.get(),
-    &cancel_request,
-    &cancel_response);
+  {
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+    ret = rcl_action_process_cancel_request(
+      pimpl_->action_server_.get(),
+      &cancel_request,
+      &cancel_response);
+  }
+
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
@@ -369,8 +409,12 @@ ServerBase::execute_cancel_request_received()
     publish_status();
   }
 
-  ret = rcl_action_send_cancel_response(
-    pimpl_->action_server_.get(), &request_header, response.get());
+  {
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+    ret = rcl_action_send_cancel_response(
+      pimpl_->action_server_.get(), &request_header, response.get());
+  }
+
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
@@ -383,11 +427,16 @@ ServerBase::execute_result_request_received()
   // Get the result request message
   rmw_request_id_t request_header;
   std::shared_ptr<void> result_request = create_result_request();
-  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
-  ret = rcl_action_take_result_request(
-    pimpl_->action_server_.get(), &request_header, result_request.get());
+  {
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+    ret = rcl_action_take_result_request(
+      pimpl_->action_server_.get(), &request_header, result_request.get());
+  }
 
-  pimpl_->result_request_ready_ = false;
+  bool expected = true;
+  if (!pimpl_->result_request_ready_.compare_exchange_strong(expected, false)) {
+    return;
+  }
 
   if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) {
     // Ignore take failure because connext fails if it receives a sample without valid data.
@@ -405,28 +454,33 @@ ServerBase::execute_result_request_received()
   rcl_action_goal_info_t goal_info;
   convert(uuid, &goal_info);
   bool goal_exists;
-  goal_exists = rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info);
+  {
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+    goal_exists = rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info);
+  }
   if (!goal_exists) {
     // Goal does not exists
     result_response = create_result_response(action_msgs::msg::GoalStatus::STATUS_UNKNOWN);
   } else {
     // Goal exists, check if a result is already available
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->unordered_map_mutex_);
     auto iter = pimpl_->goal_results_.find(uuid);
     if (iter != pimpl_->goal_results_.end()) {
       result_response = iter->second;
+    } else {
+      // Store the request so it can be responded to later
+      pimpl_->result_requests_[uuid].push_back(request_header);
     }
   }
 
   if (result_response) {
     // Send the result now
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
     ret = rcl_action_send_result_response(
       pimpl_->action_server_.get(), &request_header, result_response.get());
     if (RCL_RET_OK != ret) {
       rclcpp::exceptions::throw_from_rcl_error(ret);
     }
-  } else {
-    // Store the request so it can be responded to later
-    pimpl_->result_requests_[uuid].push_back(request_header);
   }
 }
 
@@ -439,9 +493,11 @@ ServerBase::execute_check_expired_goals()
 
   // Loop in case more than 1 goal expired
   while (num_expired > 0u) {
-    std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
     rcl_ret_t ret;
-    ret = rcl_action_expire_goals(pimpl_->action_server_.get(), expired_goals, 1, &num_expired);
+    {
+      std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+      ret = rcl_action_expire_goals(pimpl_->action_server_.get(), expired_goals, 1, &num_expired);
+    }
     if (RCL_RET_OK != ret) {
       rclcpp::exceptions::throw_from_rcl_error(ret);
     } else if (num_expired) {
@@ -449,6 +505,7 @@ ServerBase::execute_check_expired_goals()
       GoalUUID uuid;
       convert(expired_goals[0], &uuid);
       RCLCPP_DEBUG(pimpl_->logger_, "Expired goal %s", to_string(uuid).c_str());
+      std::lock_guard<std::recursive_mutex> lock(pimpl_->unordered_map_mutex_);
       pimpl_->goal_results_.erase(uuid);
       pimpl_->result_requests_.erase(uuid);
       pimpl_->goal_handles_.erase(uuid);
@@ -461,6 +518,11 @@ ServerBase::publish_status()
 {
   rcl_ret_t ret;
 
+  // We need to hold the lock across this entire method because
+  // rcl_action_server_get_goal_handles() returns an internal pointer to the
+  // goal data.
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+
   // Get all goal handles known to C action server
   rcl_action_goal_handle_t ** goal_handles = NULL;
   size_t num_goals = 0;
@@ -516,24 +578,41 @@ ServerBase::publish_result(const GoalUUID & uuid, std::shared_ptr<void> result_m
   // Check that the goal exists
   rcl_action_goal_info_t goal_info;
   convert(uuid, &goal_info);
-  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
   bool goal_exists;
-  goal_exists = rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info);
+  {
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+    goal_exists = rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info);
+  }
 
   if (!goal_exists) {
     throw std::runtime_error("Asked to publish result for goal that does not exist");
   }
 
-  pimpl_->goal_results_[uuid] = result_msg;
+  {
+    /**
+    * NOTE: There is a potential deadlock issue if both unordered_map_mutex_ and
+    * action_server_reentrant_mutex_ locked in other block scopes. Unless using
+    * std::scoped_lock, locking order must be consistent with the current.
+    *
+    * Current locking order:
+    *
+    *   1. unordered_map_mutex_
+    *   2. action_server_reentrant_mutex_
+    *
+    */
+    std::lock_guard<std::recursive_mutex> unordered_map_lock(pimpl_->unordered_map_mutex_);
+    pimpl_->goal_results_[uuid] = result_msg;
 
-  // if there are clients who already asked for the result, send it to them
-  auto iter = pimpl_->result_requests_.find(uuid);
-  if (iter != pimpl_->result_requests_.end()) {
-    for (auto & request_header : iter->second) {
-      rcl_ret_t ret = rcl_action_send_result_response(
-        pimpl_->action_server_.get(), &request_header, result_msg.get());
-      if (RCL_RET_OK != ret) {
-        rclcpp::exceptions::throw_from_rcl_error(ret);
+    // if there are clients who already asked for the result, send it to them
+    auto iter = pimpl_->result_requests_.find(uuid);
+    if (iter != pimpl_->result_requests_.end()) {
+      std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+      for (auto & request_header : iter->second) {
+        rcl_ret_t ret = rcl_action_send_result_response(
+          pimpl_->action_server_.get(), &request_header, result_msg.get());
+        if (RCL_RET_OK != ret) {
+          rclcpp::exceptions::throw_from_rcl_error(ret);
+        }
       }
     }
   }
@@ -542,7 +621,7 @@ ServerBase::publish_result(const GoalUUID & uuid, std::shared_ptr<void> result_m
 void
 ServerBase::notify_goal_terminal_state()
 {
-  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
   rcl_ret_t ret = rcl_action_notify_goal_done(pimpl_->action_server_.get());
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
@@ -552,7 +631,7 @@ ServerBase::notify_goal_terminal_state()
 void
 ServerBase::publish_feedback(std::shared_ptr<void> feedback_msg)
 {
-  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
   rcl_ret_t ret = rcl_action_publish_feedback(pimpl_->action_server_.get(), feedback_msg.get());
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to publish feedback");

rclcpp_components/CHANGELOG.rst

@@ -2,6 +2,14 @@
 Changelog for package rclcpp_components
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.4.0 (2021-09-01)
+------------------
+
+2.3.1 (2021-04-14)
+------------------
+* Update quality declaration links (re: `ros2/docs.ros2.org#52 <https://github.com/ros2/docs.ros2.org/issues/52>`_) (`#1616 <https://github.com/ros2/rclcpp/issues/1616>`_)
+* Contributors: Simon Honigmann
+
 2.3.0 (2020-12-09)
 ------------------
 * Update QD to QL 1 (`#1480 <https://github.com/ros2/rclcpp/issues/1480>`_)

rclcpp_components/QUALITY_DECLARATION.md

@@ -4,13 +4,13 @@ This document is a declaration of software quality for the `rclcpp_components` p
 
 The package `rclcpp_components` claims to be in the **Quality Level 1** category.
 
-Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-quality-categories) of the ROS2 developer guide.
+Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://www.ros.org/reps/rep-2004.html) of the ROS2 developer guide.
 
 ## Version Policy [1]
 
 ### Version Scheme [1.i]
 
-`rclcpp_components` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning).
+`rclcpp_components` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#versioning).
 
 ### Version Stability [1.ii]
 
@@ -37,11 +37,11 @@ All installed headers are in the `include` directory of the package, headers in
 
 ## Change Control Process [2]
 
-`rclcpp_components` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process).
+`rclcpp_components` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#change-control-process).
 
 ### Change Requests [2.i]
 
-All changes will occur through a pull request, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information.
+All changes will occur through a pull request, check [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#change-control-process) for additional information.
 
 ### Contributor Origin [2.ii]
 
@@ -49,7 +49,7 @@ This package uses DCO as its confirmation of contributor origin policy. More inf
 
 ### Peer Review Policy [2.iii]
 
-All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information.
+All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#change-control-process) for additional information.
 
 ### Continuous Integration [2.iv]
 
@@ -107,7 +107,7 @@ The tests aim to cover both typical usage and corner cases, but are quantified b
 
 ### Coverage [4.iii]
 
-`rclcpp_components` follows the recommendations for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#code-coverage), and opts to use line coverage instead of branch coverage.
+`rclcpp_components` follows the recommendations for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#code-coverage), and opts to use line coverage instead of branch coverage.
 
 This includes:
 
@@ -117,11 +117,11 @@ This includes:
 
 Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers.
 
-Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_foxy_coverage/lastCompletedBuild/cobertura/src_ros2_rclcpp_rclcpp_components_src/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://index.ros.org/doc/ros2/Contributing/ROS-2-On-boarding-Guide/#note-on-coverage-runs).
+Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_foxy_coverage/lastCompletedBuild/cobertura/src_ros2_rclcpp_rclcpp_components_src/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#note-on-coverage-runs).
 
 ### Performance [4.iv]
 
-`rclcpp_components` follows the recommendations for performance testing of C/C++ code in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#performance), and opts to do performance analysis on each release rather than each change.
+`rclcpp_components` follows the recommendations for performance testing of C/C++ code in the [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#performance), and opts to do performance analysis on each release rather than each change.
 
 The performance tests of `rclcpp_components` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/foxy/rclcpp_components/test/benchmark).
 
@@ -133,7 +133,7 @@ Changes that introduce regressions in performance must be adequately justified i
 
 ### Linters and Static Analysis [4.v]
 
-`rclcpp_components` uses and passes all the ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms.
+`rclcpp_components` uses and passes all the ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms.
 
 Currently nightly test results can be seen [here](http://build.ros2.org/view/Fpr/job/Fpr__rclcpp__ubuntu_focal_amd64/lastCompletedBuild/testReport/rclcpp_components).
 

rclcpp_components/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp_components</name>
-  <version>2.3.0</version>
+  <version>2.4.0</version>
   <description>Package containing tools for dynamically loadable components</description>
   <maintainer email="michael@openrobotics.org">Michael Carroll</maintainer>
   <license>Apache License 2.0</license>

rclcpp_lifecycle/CHANGELOG.rst

@@ -3,6 +3,16 @@ Changelog for package rclcpp_lifecycle
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+2.4.0 (2021-09-01)
+------------------
+* Fix SEGV caused by order of destruction of Node sub-interfaces (`#1469 <https://github.com/ros2/rclcpp/issues/1469>`_) (`#1736 <https://github.com/ros2/rclcpp/issues/1736>`_)
+* Contributors: Colin MacKenzie
+
+2.3.1 (2021-04-14)
+------------------
+* Update quality declaration links (re: `ros2/docs.ros2.org#52 <https://github.com/ros2/docs.ros2.org/issues/52>`_) (`#1616 <https://github.com/ros2/rclcpp/issues/1616>`_)
+* Contributors: Simon Honigmann
+
 2.3.0 (2020-12-09)
 ------------------
 * Reserve vector capacities and use emplace_back for constructing vectors (`#1464 <https://github.com/ros2/rclcpp/issues/1464>`_) (`#1489 <https://github.com/ros2/rclcpp/issues/1489>`_)

rclcpp_lifecycle/QUALITY_DECLARATION.md

@@ -4,13 +4,13 @@ This document is a declaration of software quality for the `rclcpp_lifecycle` pa
 
 The package `rclcpp_lifecycle` claims to be in the **Quality Level 1** category when used with a **Quality Level 1** middleware.
 
-Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-quality-categories) of the ROS2 developer guide.
+Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://www.ros.org/reps/rep-2004.html) of the ROS2 developer guide.
 
 ## Version Policy [1]
 
 ### Version Scheme [1.i]
 
-`rclcpp_lifecycle` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning).
+`rclcpp_lifecycle` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#versioning).
 
 ### Version Stability [1.ii]
 
@@ -37,11 +37,11 @@ All installed headers are in the `include` directory of the package, headers in
 
 ## Change Control Process [2]
 
-`rclcpp_lifecycle` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process).
+`rclcpp_lifecycle` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#change-control-process).
 
 ### Change Requests [2.i]
 
-All changes will occur through a pull request, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information.
+All changes will occur through a pull request, check [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#change-control-process) for additional information.
 
 ### Contributor Origin [2.ii]
 
@@ -49,7 +49,7 @@ This package uses DCO as its confirmation of contributor origin policy. More inf
 
 ### Peer Review Policy [2.iii]
 
-All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information.
+All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#change-control-process) for additional information.
 
 ### Continuous Integration [2.iv]
 
@@ -107,7 +107,7 @@ The tests aim to cover both typical usage and corner cases, but are quantified b
 
 ### Coverage [4.iii]
 
-`rclcpp_lifecycle` follows the recommendations for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#code-coverage), and opts to use line coverage instead of branch coverage.
+`rclcpp_lifecycle` follows the recommendations for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#code-coverage), and opts to use line coverage instead of branch coverage.
 
 This includes:
 
@@ -117,11 +117,11 @@ This includes:
 
 Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers.
 
-Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_foxy_coverage/lastCompletedBuild/cobertura/src_ros2_rclcpp_rclcpp_lifecycle_src/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://index.ros.org/doc/ros2/Contributing/ROS-2-On-boarding-Guide/#note-on-coverage-runs).
+Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_foxy_coverage/lastCompletedBuild/cobertura/src_ros2_rclcpp_rclcpp_lifecycle_src/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#note-on-coverage-runs).
 
 ### Performance [4.iv]
 
-`rclcpp_lifecycle` follows the recommendations for performance testing of C/C++ code in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#performance), and opts to do performance analysis on each release rather than each change.
+`rclcpp_lifecycle` follows the recommendations for performance testing of C/C++ code in the [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#performance), and opts to do performance analysis on each release rather than each change.
 
 The performance tests of `rclcpp_lifecycle` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/foxy/rclcpp_lifecycle/test/benchmark).
 
@@ -133,7 +133,7 @@ Changes that introduce regressions in performance must be adequately justified i
 
 ### Linters and Static Analysis [4.v]
 
-`rclcpp_lifecycle` uses and passes all the ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms.
+`rclcpp_lifecycle` uses and passes all the ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms.
 
 Currently nightly test results can be seen [here](http://build.ros2.org/view/Fpr/job/Fpr__rclcpp__ubuntu_focal_amd64/lastCompletedBuild/testReport/rclcpp_lifecycle/).
 

rclcpp_lifecycle/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp_lifecycle</name>
-  <version>2.3.0</version>
+  <version>2.4.0</version>
   <description>Package containing a prototype for lifecycle implementation</description>
   <maintainer email="karsten@osrfoundation.org">Karsten Knese</maintainer>
   <license>Apache License 2.0</license>

rclcpp_lifecycle/src/lifecycle_node.cpp

@@ -126,7 +126,18 @@ LifecycleNode::LifecycleNode(
 }
 
 LifecycleNode::~LifecycleNode()
-{}
+{
+  // release sub-interfaces in an order that allows them to consult with node_base during tear-down
+  node_waitables_.reset();
+  node_time_source_.reset();
+  node_parameters_.reset();
+  node_clock_.reset();
+  node_services_.reset();
+  node_topics_.reset();
+  node_timers_.reset();
+  node_logging_.reset();
+  node_graph_.reset();
+}
 
 const char *
 LifecycleNode::get_name() const