16.2.0

Signed-off-by: Barry Xu <barry.xu@sony.com>

rclcpp/CHANGELOG.rst

@@ -2,49 +2,16 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-16.0.7 (2023-11-13)
+16.2.0 (2022-05-03)
 -------------------
-* Disable the loaned messages inside the executor. (backport `#2335 <https://github.com/ros2/rclcpp/issues/2335>`_) (`#2364 <https://github.com/ros2/rclcpp/issues/2364>`_)
-* Add missing 'enable_rosout' comments (`#2345 <https://github.com/ros2/rclcpp/issues/2345>`_) (`#2347 <https://github.com/ros2/rclcpp/issues/2347>`_)
-* address rate related flaky tests. (`#2329 <https://github.com/ros2/rclcpp/issues/2329>`_) (`#2342 <https://github.com/ros2/rclcpp/issues/2342>`_)
-* Update SignalHandler get_global_signal_handler to avoid complex types in static memory (`#2316 <https://github.com/ros2/rclcpp/issues/2316>`_) (`#2321 <https://github.com/ros2/rclcpp/issues/2321>`_)
-* Fix C++20 allocator construct deprecation (`#2292 <https://github.com/ros2/rclcpp/issues/2292>`_) (`#2319 <https://github.com/ros2/rclcpp/issues/2319>`_)
-* Contributors: mergify[bot]
+* Update get_parameter_from_event to follow the function description (`#1922 <https://github.com/ros2/rclcpp/issues/1922>`_)
+* Add 'best available' QoS enum values and methods (`#1920 <https://github.com/ros2/rclcpp/issues/1920>`_)
+* Contributors: Barry Xu, Jacob Perron
 
-16.0.6 (2023-09-19)
+16.1.0 (2022-04-29)
 -------------------
-* Topic correct typeadapter deduction (`#2294 <https://github.com/ros2/rclcpp/issues/2294>`_) (`#2297 <https://github.com/ros2/rclcpp/issues/2297>`_)
-* check thread whether joinable before join (`#2019 <https://github.com/ros2/rclcpp/issues/2019>`_) (`#2275 <https://github.com/ros2/rclcpp/issues/2275>`_)
-* Do not crash Executor when send_response fails due to client failure. (`#2276 <https://github.com/ros2/rclcpp/issues/2276>`_) (`#2280 <https://github.com/ros2/rclcpp/issues/2280>`_)
-* Contributors: mergify[bot]
-
-16.0.5 (2023-07-17)
--------------------
-* warning: comparison of integer expressions of different signedness (`#2219 <https://github.com/ros2/rclcpp/issues/2219>`_) (`#2223 <https://github.com/ros2/rclcpp/issues/2223>`_)
-* Trigger the intraprocess guard condition with data (`#2164 <https://github.com/ros2/rclcpp/issues/2164>`_) (`#2167 <https://github.com/ros2/rclcpp/issues/2167>`_)
-* Implement validity checks for rclcpp::Clock (`#2040 <https://github.com/ros2/rclcpp/issues/2040>`_) (`#2210 <https://github.com/ros2/rclcpp/issues/2210>`_)
-* Contributors: Tomoya Fujita, mergify[bot]
-
-16.0.4 (2023-04-25)
--------------------
-* use allocator via init_options argument. (`#2129 <https://github.com/ros2/rclcpp/issues/2129>`_) (`#2131 <https://github.com/ros2/rclcpp/issues/2131>`_)
-* Contributors: mergify[bot]
-
-16.0.3 (2023-01-10)
--------------------
-* Fix SharedFuture from async_send_request never becomes valid (`#2044 <https://github.com/ros2/rclcpp/issues/2044>`_) (`#2076 <https://github.com/ros2/rclcpp/issues/2076>`_)
-* do not throw exception if trying to dequeue an empty intra-process buffer (`#2061 <https://github.com/ros2/rclcpp/issues/2061>`_) (`#2070 <https://github.com/ros2/rclcpp/issues/2070>`_)
-* fix nullptr dereference in prune_requests_older_than (`#2008 <https://github.com/ros2/rclcpp/issues/2008>`_) (`#2065 <https://github.com/ros2/rclcpp/issues/2065>`_)
-* Fix bug that a callback not reached (`#1640 <https://github.com/ros2/rclcpp/issues/1640>`_) (`#2033 <https://github.com/ros2/rclcpp/issues/2033>`_)
-* Contributors: mergify[bot]
-
-16.0.2 (2022-11-07)
--------------------
-* fix mismatched issue if using zero_allocate (`#1995 <https://github.com/ros2/rclcpp/issues/1995>`_) (`#2026 <https://github.com/ros2/rclcpp/issues/2026>`_)
-* use regex for wildcard matching (backport `#1839 <https://github.com/ros2/rclcpp/issues/1839>`_) (`#1986 <https://github.com/ros2/rclcpp/issues/1986>`_)
-* Drop wrong template specialization (`#1926 <https://github.com/ros2/rclcpp/issues/1926>`_) (`#1937 <https://github.com/ros2/rclcpp/issues/1937>`_)
-* Add statistics for handle_loaned_message (`#1927 <https://github.com/ros2/rclcpp/issues/1927>`_) (`#1932 <https://github.com/ros2/rclcpp/issues/1932>`_)
-* Contributors: mergify[bot]
+* use reinterpret_cast for function pointer conversion. (`#1919 <https://github.com/ros2/rclcpp/issues/1919>`_)
+* Contributors: Tomoya Fujita
 
 16.0.1 (2022-04-13)
 -------------------

rclcpp/include/rclcpp/allocator/allocator_common.hpp

@@ -15,7 +15,6 @@
 #ifndef RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_
 #define RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_
 
-#include <cstring>
 #include <memory>
 
 #include "rcl/allocator.h"
@@ -40,22 +39,6 @@ void * retyped_allocate(size_t size, void * untyped_allocator)
   return std::allocator_traits<Alloc>::allocate(*typed_allocator, size);
 }
 
-template<typename Alloc>
-void * retyped_zero_allocate(size_t number_of_elem, size_t size_of_elem, void * untyped_allocator)
-{
-  auto typed_allocator = static_cast<Alloc *>(untyped_allocator);
-  if (!typed_allocator) {
-    throw std::runtime_error("Received incorrect allocator type");
-  }
-  size_t size = number_of_elem * size_of_elem;
-  void * allocated_memory =
-    std::allocator_traits<Alloc>::allocate(*typed_allocator, size);
-  if (allocated_memory) {
-    std::memset(allocated_memory, 0, size);
-  }
-  return allocated_memory;
-}
-
 template<typename T, typename Alloc>
 void retyped_deallocate(void * untyped_pointer, void * untyped_allocator)
 {
@@ -90,7 +73,6 @@ rcl_allocator_t get_rcl_allocator(Alloc & allocator)
   rcl_allocator_t rcl_allocator = rcl_get_default_allocator();
 #ifndef _WIN32
   rcl_allocator.allocate = &retyped_allocate<Alloc>;
-  rcl_allocator.zero_allocate = &retyped_zero_allocate<Alloc>;
   rcl_allocator.deallocate = &retyped_deallocate<T, Alloc>;
   rcl_allocator.reallocate = &retyped_reallocate<T, Alloc>;
   rcl_allocator.state = &allocator;

rclcpp/include/rclcpp/callback_group.hpp

@@ -16,14 +16,11 @@
 #define RCLCPP__CALLBACK_GROUP_HPP_
 
 #include <atomic>
-#include <functional>
-#include <memory>
 #include <mutex>
+#include <string>
 #include <vector>
 
 #include "rclcpp/client.hpp"
-#include "rclcpp/context.hpp"
-#include "rclcpp/guard_condition.hpp"
 #include "rclcpp/publisher_base.hpp"
 #include "rclcpp/service.hpp"
 #include "rclcpp/subscription_base.hpp"
@@ -98,10 +95,6 @@ public:
     CallbackGroupType group_type,
     bool automatically_add_to_executor_with_node = true);
 
-  /// Default destructor.
-  RCLCPP_PUBLIC
-  ~CallbackGroup();
-
   template<typename Function>
   rclcpp::SubscriptionBase::SharedPtr
   find_subscription_ptrs_if(Function func) const
@@ -178,16 +171,6 @@ public:
   bool
   automatically_add_to_executor_with_node() const;
 
-  /// Defer creating the notify guard condition and return it.
-  RCLCPP_PUBLIC
-  rclcpp::GuardCondition::SharedPtr
-  get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr);
-
-  /// Trigger the notify guard condition.
-  RCLCPP_PUBLIC
-  void
-  trigger_notify_guard_condition();
-
 protected:
   RCLCPP_DISABLE_COPY(CallbackGroup)
 
@@ -230,9 +213,6 @@ protected:
   std::vector<rclcpp::Waitable::WeakPtr> waitable_ptrs_;
   std::atomic_bool can_be_taken_from_;
   const bool automatically_add_to_executor_with_node_;
-  // defer the creation of the guard condition
-  std::shared_ptr<rclcpp::GuardCondition> notify_guard_condition_ = nullptr;
-  std::recursive_mutex notify_guard_condition_mutex_;
 
 private:
   template<typename TypeT, typename Function>

rclcpp/include/rclcpp/client.hpp

@@ -769,9 +769,7 @@ public:
     auto old_size = pending_requests_.size();
     for (auto it = pending_requests_.begin(), last = pending_requests_.end(); it != last; ) {
       if (it->second.first < time_point) {
-        if (pruned_requests) {
-          pruned_requests->push_back(it->first);
-        }
+        pruned_requests->push_back(it->first);
         it = pending_requests_.erase(it);
       } else {
         ++it;
@@ -794,14 +792,16 @@ protected:
   async_send_request_impl(const Request & request, CallbackInfoVariant value)
   {
     int64_t sequence_number;
-    std::lock_guard<std::mutex> lock(pending_requests_mutex_);
     rcl_ret_t ret = rcl_send_request(get_client_handle().get(), &request, &sequence_number);
     if (RCL_RET_OK != ret) {
       rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request");
     }
-    pending_requests_.try_emplace(
-      sequence_number,
-      std::make_pair(std::chrono::system_clock::now(), std::move(value)));
+    {
+      std::lock_guard<std::mutex> lock(pending_requests_mutex_);
+      pending_requests_.try_emplace(
+        sequence_number,
+        std::make_pair(std::chrono::system_clock::now(), std::move(value)));
+    }
     return sequence_number;
   }
 

rclcpp/include/rclcpp/clock.hpp

@@ -137,51 +137,6 @@ public:
     Duration rel_time,
     Context::SharedPtr context = contexts::get_global_default_context());
 
-  /**
-   * Check if the clock is started.
-   *
-   * A started clock is a clock that reflects non-zero time.
-   * Typically a clock will be unstarted if it is using RCL_ROS_TIME with ROS time and
-   * nothing has been published on the clock topic yet.
-   *
-   * \return true if clock is started
-   * \throws std::runtime_error if the clock is not rcl_clock_valid
-   */
-  RCLCPP_PUBLIC
-  bool
-  started();
-
-  /**
-   * Wait until clock to start.
-   *
-   * \rclcpp::Clock::started
-   * \param context the context to wait in
-   * \return true if clock was already started or became started
-   * \throws std::runtime_error if the context is invalid or clock is not rcl_clock_valid
-   */
-  RCLCPP_PUBLIC
-  bool
-  wait_until_started(Context::SharedPtr context = contexts::get_global_default_context());
-
-  /**
-   * Wait for clock to start, with timeout.
-   *
-   * The timeout is waited in steady time.
-   *
-   * \rclcpp::Clock::started
-   * \param timeout the maximum time to wait for.
-   * \param context the context to wait in.
-   * \param wait_tick_ns the time to wait between each iteration of the wait loop (in nanoseconds).
-   * \return true if clock was or became valid
-   * \throws std::runtime_error if the context is invalid or clock is not rcl_clock_valid
-   */
-  RCLCPP_PUBLIC
-  bool
-  wait_until_started(
-    const rclcpp::Duration & timeout,
-    Context::SharedPtr context = contexts::get_global_default_context(),
-    const rclcpp::Duration & wait_tick_ns = rclcpp::Duration(0, static_cast<uint32_t>(1e7)));
-
   /**
    * Returns the clock of the type `RCL_ROS_TIME` is active.
    *

rclcpp/include/rclcpp/executor.hpp

@@ -560,14 +560,14 @@ protected:
   virtual void
   spin_once_impl(std::chrono::nanoseconds timeout);
 
-  typedef std::map<rclcpp::CallbackGroup::WeakPtr,
+  typedef std::map<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
       const rclcpp::GuardCondition *,
-      std::owner_less<rclcpp::CallbackGroup::WeakPtr>>
-    WeakCallbackGroupsToGuardConditionsMap;
+      std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>
+    WeakNodesToGuardConditionsMap;
 
-  /// maps callback groups to guard conditions
-  WeakCallbackGroupsToGuardConditionsMap
-  weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+  /// maps nodes to guard conditions
+  WeakNodesToGuardConditionsMap
+  weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
 
   /// maps callback groups associated to nodes
   WeakCallbackGroupsToNodesMap

rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp

@@ -86,7 +86,8 @@ public:
     std::lock_guard<std::mutex> lock(mutex_);
 
     if (!has_data_()) {
-      return BufferT();
+      RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Calling dequeue on empty intra-process buffer");
+      throw std::runtime_error("Calling dequeue on empty intra-process buffer");
     }
 
     auto request = std::move(ring_buffer_[read_index_]);

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

@@ -479,13 +479,13 @@ private:
                 "subscription use different allocator types, which is not supported");
       }
 
-      if constexpr (rclcpp::TypeAdapter<MessageT, ROSMessageType>::is_specialized::value) {
+      if constexpr (rclcpp::TypeAdapter<MessageT>::is_specialized::value) {
         ROSMessageTypeAllocator ros_message_alloc(allocator);
-        auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_alloc, 1);
-        ROSMessageTypeAllocatorTraits::construct(ros_message_alloc, ptr);
+        auto ptr = ros_message_alloc.allocate(1);
+        ros_message_alloc.construct(ptr);
         ROSMessageTypeDeleter deleter;
         allocator::set_allocator_for_deleter(&deleter, &allocator);
-        rclcpp::TypeAdapter<MessageT, ROSMessageType>::convert_to_ros_message(*message, *ptr);
+        rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*message, *ptr);
         auto ros_msg = std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>(ptr, deleter);
         ros_message_subscription->provide_intra_process_message(std::move(ros_msg));
       } else {

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -109,22 +109,9 @@ public:
 
     if (any_callback_.use_take_shared_method()) {
       shared_msg = this->buffer_->consume_shared();
-      if (!shared_msg) {
-        return nullptr;
-      }
     } else {
       unique_msg = this->buffer_->consume_unique();
-      if (!unique_msg) {
-        return nullptr;
-      }
     }
-
-    if (this->buffer_->has_data()) {
-      // If there is data still to be processed, indicate to the
-      // executor or waitset by triggering the guard condition.
-      this->trigger_guard_condition();
-    }
-
     return std::static_pointer_cast<void>(
       std::make_shared<std::pair<ConstMessageSharedPtr, MessageUniquePtr>>(
         std::pair<ConstMessageSharedPtr, MessageUniquePtr>(
@@ -151,7 +138,7 @@ protected:
   execute_impl(std::shared_ptr<void> & data)
   {
     if (!data) {
-      return;
+      throw std::runtime_error("'data' is empty");
     }
 
     rmw_message_info_t msg_info;

rclcpp/include/rclcpp/node_options.hpp

@@ -42,7 +42,6 @@ public:
    *   - arguments = {}
    *   - parameter_overrides = {}
    *   - use_global_arguments = true
-   *   - enable_rosout = true
    *   - use_intra_process_comms = false
    *   - enable_topic_statistics = false
    *   - start_parameter_services = true

rclcpp/include/rclcpp/parameter_event_handler.hpp

@@ -268,6 +268,7 @@ public:
    * \param[in] parameter_name Name of parameter.
    * \param[in] node_name Name of node which hosts the parameter.
    * \returns The resultant rclcpp::Parameter from the event.
+   * \throws std::runtime_error if input node name doesn't match the node name in parameter event.
    */
   RCLCPP_PUBLIC
   static rclcpp::Parameter

rclcpp/include/rclcpp/parameter_map.hpp

@@ -41,16 +41,6 @@ RCLCPP_PUBLIC
 ParameterMap
 parameter_map_from(const rcl_params_t * const c_params);
 
-/// Convert parameters from rcl_yaml_param_parser into C++ class instances.
-/// \param[in] c_params C structures containing parameters for multiple nodes.
-/// \param[in] node_fqn a Fully Qualified Name of node, default value is nullptr.
-///   If it's not nullptr, return the relative node parameters belonging to this node_fqn.
-/// \returns a map where the keys are fully qualified node names and values a list of parameters.
-/// \throws InvalidParametersException if the `rcl_params_t` is inconsistent or invalid.
-RCLCPP_PUBLIC
-ParameterMap
-parameter_map_from(const rcl_params_t * const c_params, const char * node_fqn);
-
 /// Convert parameter value from rcl_yaml_param_parser into a C++ class instance.
 /// \param[in] c_value C structure containing a value of a parameter.
 /// \returns an instance of a parameter value

rclcpp/include/rclcpp/publisher_options.hpp

@@ -72,7 +72,7 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase
   /// Optional custom allocator.
   std::shared_ptr<Allocator> allocator = nullptr;
 
-  PublisherOptionsWithAllocator() {}
+  PublisherOptionsWithAllocator<Allocator>() {}
 
   /// Constructor using base class as input.
   explicit PublisherOptionsWithAllocator(const PublisherOptionsBase & publisher_options_base)

rclcpp/include/rclcpp/qos.hpp

@@ -44,6 +44,7 @@ enum class ReliabilityPolicy
   BestEffort = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
   Reliable = RMW_QOS_POLICY_RELIABILITY_RELIABLE,
   SystemDefault = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT,
+  BestAvailable = RMW_QOS_POLICY_RELIABILITY_BEST_AVAILABLE,
   Unknown = RMW_QOS_POLICY_RELIABILITY_UNKNOWN,
 };
 
@@ -52,6 +53,7 @@ enum class DurabilityPolicy
   Volatile = RMW_QOS_POLICY_DURABILITY_VOLATILE,
   TransientLocal = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL,
   SystemDefault = RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT,
+  BestAvailable = RMW_QOS_POLICY_DURABILITY_BEST_AVAILABLE,
   Unknown = RMW_QOS_POLICY_DURABILITY_UNKNOWN,
 };
 
@@ -60,6 +62,7 @@ enum class LivelinessPolicy
   Automatic = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC,
   ManualByTopic = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC,
   SystemDefault = RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
+  BestAvailable = RMW_QOS_POLICY_LIVELINESS_BEST_AVAILABLE,
   Unknown = RMW_QOS_POLICY_LIVELINESS_UNKNOWN,
 };
 
@@ -180,6 +183,10 @@ public:
   QoS &
   best_effort();
 
+  /// Set the reliability setting to best available.
+  QoS &
+  reliability_best_available();
+
   /// Set the durability setting.
   QoS &
   durability(rmw_qos_durability_policy_t durability);
@@ -199,6 +206,10 @@ public:
   QoS &
   transient_local();
 
+  /// Set the durability setting to best available.
+  QoS &
+  durability_best_available();
+
   /// Set the deadline setting.
   QoS &
   deadline(rmw_time_t deadline);
@@ -488,6 +499,36 @@ public:
   ));
 };
 
+/**
+ * Best available QoS class
+ *
+ * Match majority of endpoints currently available while maintaining the highest level of service.
+ * Policies are chosen at the time of creating a subscription or publisher.
+ * The middleware is not expected to update policies after creating a subscription or publisher,
+ * even if one or more policies are incompatible with newly discovered endpoints.
+ * Therefore, this profile should be used with care since non-deterministic behavior can occur due
+ * to races with discovery.
+ *
+ *    - History: Keep last,
+ *    - Depth: 10,
+ *    - Reliability: Best available,
+ *    - Durability: Best available,
+ *    - Deadline: Best available,
+ *    - Lifespan: Default,
+ *    - Liveliness: Best available,
+ *    - Liveliness lease duration: Best available,
+ *    - avoid ros namespace conventions: false
+ */
+class RCLCPP_PUBLIC BestAvailableQoS : public QoS
+{
+public:
+  explicit
+  BestAvailableQoS(
+    const QoSInitialization & qos_initialization = (
+      QoSInitialization::from_rmw(rmw_qos_profile_best_available)
+  ));
+};
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__QOS_HPP_

rclcpp/include/rclcpp/service.hpp

@@ -481,14 +481,6 @@ public:
   {
     rcl_ret_t ret = rcl_send_response(get_service_handle().get(), &req_id, &response);
 
-    if (ret == RCL_RET_TIMEOUT) {
-      RCLCPP_WARN(
-        node_logger_.get_child("rclcpp"),
-        "failed to send response to %s (timeout): %s",
-        this->get_service_name(), rcl_get_error_string().str);
-      rcl_reset_error();
-      return;
-    }
     if (ret != RCL_RET_OK) {
       rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send response");
     }

rclcpp/include/rclcpp/subscription.hpp

@@ -363,31 +363,11 @@ public:
     void * loaned_message,
     const rclcpp::MessageInfo & message_info) override
   {
-    if (matches_any_intra_process_publishers(&message_info.get_rmw_message_info().publisher_gid)) {
-      // In this case, the message will be delivered via intra process and
-      // we should ignore this copy of the message.
-      return;
-    }
-
     auto typed_message = static_cast<ROSMessageType *>(loaned_message);
     // message is loaned, so we have to make sure that the deleter does not deallocate the message
     auto sptr = std::shared_ptr<ROSMessageType>(
       typed_message, [](ROSMessageType * msg) {(void) msg;});
-
-    std::chrono::time_point<std::chrono::system_clock> now;
-    if (subscription_topic_statistics_) {
-      // get current time before executing callback to
-      // exclude callback duration from topic statistics result.
-      now = std::chrono::system_clock::now();
-    }
-
     any_callback_.dispatch(sptr, message_info);
-
-    if (subscription_topic_statistics_) {
-      const auto nanos = std::chrono::time_point_cast<std::chrono::nanoseconds>(now);
-      const auto time = rclcpp::Time(nanos.time_since_epoch().count());
-      subscription_topic_statistics_->handle_message(*typed_message, time);
-    }
   }
 
   /// Return the borrowed message.

rclcpp/include/rclcpp/subscription_options.hpp

@@ -97,7 +97,7 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
   /// Optional custom allocator.
   std::shared_ptr<Allocator> allocator = nullptr;
 
-  SubscriptionOptionsWithAllocator() {}
+  SubscriptionOptionsWithAllocator<Allocator>() {}
 
   /// Constructor using base class as input.
   explicit SubscriptionOptionsWithAllocator(

rclcpp/include/rclcpp/timer.hpp

@@ -189,10 +189,10 @@ public:
     TRACEPOINT(
       rclcpp_timer_callback_added,
       static_cast<const void *>(get_timer_handle().get()),
-      static_cast<const void *>(&callback_));
+      reinterpret_cast<const void *>(&callback_));
     TRACEPOINT(
       rclcpp_callback_register,
-      static_cast<const void *>(&callback_),
+      reinterpret_cast<const void *>(&callback_),
       tracetools::get_symbol(callback_));
   }
 
@@ -226,9 +226,9 @@ public:
   void
   execute_callback() override
   {
-    TRACEPOINT(callback_start, static_cast<const void *>(&callback_), false);
+    TRACEPOINT(callback_start, reinterpret_cast<const void *>(&callback_), false);
     execute_callback_delegate<>();
-    TRACEPOINT(callback_end, static_cast<const void *>(&callback_));
+    TRACEPOINT(callback_end, reinterpret_cast<const void *>(&callback_));
   }
 
   // void specialization

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>16.0.7</version>
+  <version>16.2.0</version>
   <description>The ROS client library in C++.</description>
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
   <maintainer email="jacob@openrobotics.org">Jacob Perron</maintainer>

rclcpp/src/rclcpp/callback_group.cpp

@@ -12,19 +12,9 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <algorithm>
-#include <atomic>
-#include <functional>
-#include <memory>
-#include <mutex>
-#include <stdexcept>
-
 #include "rclcpp/callback_group.hpp"
-#include "rclcpp/client.hpp"
-#include "rclcpp/service.hpp"
-#include "rclcpp/subscription_base.hpp"
-#include "rclcpp/timer.hpp"
-#include "rclcpp/waitable.hpp"
+
+#include <vector>
 
 using rclcpp::CallbackGroup;
 using rclcpp::CallbackGroupType;
@@ -37,10 +27,6 @@ CallbackGroup::CallbackGroup(
   automatically_add_to_executor_with_node_(automatically_add_to_executor_with_node)
 {}
 
-CallbackGroup::~CallbackGroup()
-{
-  trigger_notify_guard_condition();
-}
 
 std::atomic_bool &
 CallbackGroup::can_be_taken_from()
@@ -111,33 +97,6 @@ CallbackGroup::automatically_add_to_executor_with_node() const
   return automatically_add_to_executor_with_node_;
 }
 
-rclcpp::GuardCondition::SharedPtr
-CallbackGroup::get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr)
-{
-  std::lock_guard<std::recursive_mutex> lock(notify_guard_condition_mutex_);
-  if (notify_guard_condition_ && context_ptr != notify_guard_condition_->get_context()) {
-    if (associated_with_executor_) {
-      trigger_notify_guard_condition();
-    }
-    notify_guard_condition_ = nullptr;
-  }
-
-  if (!notify_guard_condition_) {
-    notify_guard_condition_ = std::make_shared<rclcpp::GuardCondition>(context_ptr);
-  }
-
-  return notify_guard_condition_;
-}
-
-void
-CallbackGroup::trigger_notify_guard_condition()
-{
-  std::lock_guard<std::recursive_mutex> lock(notify_guard_condition_mutex_);
-  if (notify_guard_condition_) {
-    notify_guard_condition_->trigger();
-  }
-}
-
 void
 CallbackGroup::add_subscription(
   const rclcpp::SubscriptionBase::SharedPtr subscription_ptr)

rclcpp/src/rclcpp/clock.cpp

@@ -182,71 +182,6 @@ Clock::sleep_for(Duration rel_time, Context::SharedPtr context)
   return sleep_until(now() + rel_time, context);
 }
 
-bool
-Clock::started()
-{
-  if (!rcl_clock_valid(get_clock_handle())) {
-    throw std::runtime_error("clock is not rcl_clock_valid");
-  }
-  return rcl_clock_time_started(get_clock_handle());
-}
-
-bool
-Clock::wait_until_started(Context::SharedPtr context)
-{
-  if (!context || !context->is_valid()) {
-    throw std::runtime_error("context cannot be slept with because it's invalid");
-  }
-  if (!rcl_clock_valid(get_clock_handle())) {
-    throw std::runtime_error("clock cannot be waited on as it is not rcl_clock_valid");
-  }
-
-  if (started()) {
-    return true;
-  } else {
-    // Wait until the first non-zero time
-    return sleep_until(rclcpp::Time(0, 1, get_clock_type()), context);
-  }
-}
-
-bool
-Clock::wait_until_started(
-  const Duration & timeout,
-  Context::SharedPtr context,
-  const Duration & wait_tick_ns)
-{
-  if (!context || !context->is_valid()) {
-    throw std::runtime_error("context cannot be slept with because it's invalid");
-  }
-  if (!rcl_clock_valid(get_clock_handle())) {
-    throw std::runtime_error("clock cannot be waited on as it is not rcl_clock_valid");
-  }
-
-  Clock timeout_clock = Clock(RCL_STEADY_TIME);
-  Time start = timeout_clock.now();
-
-  // Check if the clock has started every wait_tick_ns nanoseconds
-  // Context check checks for rclcpp::shutdown()
-  while (!started() && context->is_valid()) {
-    if (timeout < wait_tick_ns) {
-      timeout_clock.sleep_for(timeout);
-    } else {
-      Duration time_left = start + timeout - timeout_clock.now();
-      if (time_left > wait_tick_ns) {
-        timeout_clock.sleep_for(Duration(wait_tick_ns));
-      } else {
-        timeout_clock.sleep_for(time_left);
-      }
-    }
-
-    if (timeout_clock.now() - start > timeout) {
-      return started();
-    }
-  }
-  return started();
-}
-
-
 bool
 Clock::ros_time_is_active()
 {

rclcpp/src/rclcpp/context.cpp

@@ -217,7 +217,7 @@ Context::init(
     if (0u == count) {
       ret = rcl_logging_configure_with_output_handler(
         &rcl_context_->global_arguments,
-        rcl_init_options_get_allocator(init_options.get_rcl_init_options()),
+        rcl_init_options_get_allocator(init_options_.get_rcl_init_options()),
         rclcpp_logging_output_handler);
       if (RCL_RET_OK != ret) {
         rcl_context_.reset();
@@ -497,7 +497,7 @@ Context::sleep_for(const std::chrono::nanoseconds & nanoseconds)
       std::unique_lock<std::mutex> lock(interrupt_mutex_);
       auto start = std::chrono::steady_clock::now();
       // this will release the lock while waiting
-      interrupt_condition_variable_.wait_for(lock, time_left);
+      interrupt_condition_variable_.wait_for(lock, nanoseconds);
       time_left -= std::chrono::steady_clock::now() - start;
     }
   } while (time_left > std::chrono::nanoseconds::zero() && this->is_valid());

rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp

@@ -51,13 +51,18 @@ rclcpp::detail::resolve_parameter_overrides(
         [params]() {
           rcl_yaml_node_struct_fini(params);
         });
-      rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params, node_fqn.c_str());
+      rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params);
 
-      if (initial_map.count(node_fqn) > 0) {
-        // Combine parameter yaml files, overwriting values in older ones
-        for (const rclcpp::Parameter & param : initial_map.at(node_fqn)) {
-          result[param.get_name()] =
-            rclcpp::ParameterValue(param.get_value_message());
+      // Enforce wildcard matching precedence
+      // TODO(cottsay) implement further wildcard matching
+      const std::array<std::string, 2> node_matching_names{"/**", node_fqn};
+      for (const auto & node_name : node_matching_names) {
+        if (initial_map.count(node_name) > 0) {
+          // Combine parameter yaml files, overwriting values in older ones
+          for (const rclcpp::Parameter & param : initial_map.at(node_name)) {
+            result[param.get_name()] =
+              rclcpp::ParameterValue(param.get_value_message());
+          }
         }
       }
     }

rclcpp/src/rclcpp/executor.cpp

@@ -106,11 +106,11 @@ Executor::~Executor()
   weak_groups_associated_with_executor_to_nodes_.clear();
   weak_groups_to_nodes_associated_with_executor_.clear();
   weak_groups_to_nodes_.clear();
-  for (const auto & pair : weak_groups_to_guard_conditions_) {
+  for (const auto & pair : weak_nodes_to_guard_conditions_) {
     auto guard_condition = pair.second;
     memory_strategy_->remove_guard_condition(guard_condition);
   }
-  weak_groups_to_guard_conditions_.clear();
+  weak_nodes_to_guard_conditions_.clear();
 
   // Finalize the wait set.
   if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) {
@@ -204,7 +204,8 @@ Executor::add_callback_group_to_map(
   if (has_executor.exchange(true)) {
     throw std::runtime_error("Callback group has already been added to an executor.");
   }
-
+  bool is_new_node = !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) &&
+    !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_);
   rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr;
   auto insert_info =
     weak_groups_to_nodes.insert(std::make_pair(weak_group_ptr, node_ptr));
@@ -214,24 +215,21 @@ Executor::add_callback_group_to_map(
   }
   // Also add to the map that contains all callback groups
   weak_groups_to_nodes_.insert(std::make_pair(weak_group_ptr, node_ptr));
-
-  if (node_ptr->get_context()->is_valid()) {
-    auto callback_group_guard_condition =
-      group_ptr->get_notify_guard_condition(node_ptr->get_context());
-    weak_groups_to_guard_conditions_[weak_group_ptr] = callback_group_guard_condition.get();
-    // Add the callback_group's notify condition to the guard condition handles
-    memory_strategy_->add_guard_condition(*callback_group_guard_condition);
-  }
-
-  if (notify) {
-    // Interrupt waiting to handle new node
-    try {
-      interrupt_guard_condition_.trigger();
-    } catch (const rclcpp::exceptions::RCLError & ex) {
-      throw std::runtime_error(
-              std::string(
-                "Failed to trigger guard condition on callback group add: ") + ex.what());
+  if (is_new_node) {
+    const auto & gc = node_ptr->get_notify_guard_condition();
+    weak_nodes_to_guard_conditions_[node_ptr] = &gc;
+    if (notify) {
+      // Interrupt waiting to handle new node
+      try {
+        interrupt_guard_condition_.trigger();
+      } catch (const rclcpp::exceptions::RCLError & ex) {
+        throw std::runtime_error(
+                std::string(
+                  "Failed to trigger guard condition on callback group add: ") + ex.what());
+      }
     }
+    // Add the node's notify condition to the guard condition handles
+    memory_strategy_->add_guard_condition(gc);
   }
 }
 
@@ -302,12 +300,7 @@ Executor::remove_callback_group_from_map(
   if (!has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) &&
     !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_))
   {
-    auto iter = weak_groups_to_guard_conditions_.find(weak_group_ptr);
-    if (iter != weak_groups_to_guard_conditions_.end()) {
-      memory_strategy_->remove_guard_condition(iter->second);
-    }
-    weak_groups_to_guard_conditions_.erase(weak_group_ptr);
-
+    weak_nodes_to_guard_conditions_.erase(node_ptr);
     if (notify) {
       try {
         interrupt_guard_condition_.trigger();
@@ -317,6 +310,7 @@ Executor::remove_callback_group_from_map(
                   "Failed to trigger guard condition on callback group remove: ") + ex.what());
       }
     }
+    memory_strategy_->remove_guard_condition(&node_ptr->get_notify_guard_condition());
   }
 }
 
@@ -646,11 +640,6 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription)
       subscription->get_topic_name(),
       [&]() {return subscription->take_type_erased(message.get(), message_info);},
       [&]() {subscription->handle_message(message, message_info);});
-    // TODO(clalancette): In the case that the user is using the MessageMemoryPool,
-    // and they take a shared_ptr reference to the message in the callback, this can
-    // inadvertently return the message to the pool when the user is still using it.
-    // This is a bug that needs to be fixed in the pool, and we should probably have
-    // a custom deleter for the message that actually does the return_message().
     subscription->return_message(message);
   }
 }
@@ -711,6 +700,12 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
         auto weak_node_ptr = pair.second;
         if (weak_group_ptr.expired() || weak_node_ptr.expired()) {
           invalid_group_ptrs.push_back(weak_group_ptr);
+          auto node_guard_pair = weak_nodes_to_guard_conditions_.find(weak_node_ptr);
+          if (node_guard_pair != weak_nodes_to_guard_conditions_.end()) {
+            auto guard_condition = node_guard_pair->second;
+            weak_nodes_to_guard_conditions_.erase(weak_node_ptr);
+            memory_strategy_->remove_guard_condition(guard_condition);
+          }
         }
       }
       std::for_each(
@@ -726,12 +721,6 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
           {
             weak_groups_associated_with_executor_to_nodes_.erase(group_ptr);
           }
-          auto callback_guard_pair = weak_groups_to_guard_conditions_.find(group_ptr);
-          if (callback_guard_pair != weak_groups_to_guard_conditions_.end()) {
-            auto guard_condition = callback_guard_pair->second;
-            weak_groups_to_guard_conditions_.erase(group_ptr);
-            memory_strategy_->remove_guard_condition(guard_condition);
-          }
           weak_groups_to_nodes_.erase(group_ptr);
         });
     }

rclcpp/src/rclcpp/node_interfaces/node_services.cpp

@@ -35,17 +35,15 @@ NodeServices::add_service(
       // TODO(jacquelinekay): use custom exception
       throw std::runtime_error("Cannot create service, group not in node.");
     }
+    group->add_service(service_base_ptr);
   } else {
-    group = node_base_->get_default_callback_group();
+    node_base_->get_default_callback_group()->add_service(service_base_ptr);
   }
 
-  group->add_service(service_base_ptr);
-
   // Notify the executor that a new service was created using the parent Node.
   auto & node_gc = node_base_->get_notify_guard_condition();
   try {
     node_gc.trigger();
-    group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string("failed to notify wait set on service creation: ") + ex.what());
@@ -62,17 +60,15 @@ NodeServices::add_client(
       // TODO(jacquelinekay): use custom exception
       throw std::runtime_error("Cannot create client, group not in node.");
     }
+    group->add_client(client_base_ptr);
   } else {
-    group = node_base_->get_default_callback_group();
+    node_base_->get_default_callback_group()->add_client(client_base_ptr);
   }
 
-  group->add_client(client_base_ptr);
-
   // Notify the executor that a new client was created using the parent Node.
   auto & node_gc = node_base_->get_notify_guard_condition();
   try {
     node_gc.trigger();
-    group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string("failed to notify wait set on client creation: ") + ex.what());

rclcpp/src/rclcpp/node_interfaces/node_timers.cpp

@@ -37,15 +37,14 @@ NodeTimers::add_timer(
       // TODO(jacquelinekay): use custom exception
       throw std::runtime_error("Cannot create timer, group not in node.");
     }
+    callback_group->add_timer(timer);
   } else {
-    callback_group = node_base_->get_default_callback_group();
+    node_base_->get_default_callback_group()->add_timer(timer);
   }
-  callback_group->add_timer(timer);
 
   auto & node_gc = node_base_->get_notify_guard_condition();
   try {
     node_gc.trigger();
-    callback_group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string("failed to notify wait set on timer creation: ") + ex.what());

rclcpp/src/rclcpp/node_interfaces/node_topics.cpp

@@ -73,7 +73,6 @@ NodeTopics::add_publisher(
   auto & node_gc = node_base_->get_notify_guard_condition();
   try {
     node_gc.trigger();
-    callback_group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string("failed to notify wait set on publisher creation: ") + ex.what());
@@ -122,7 +121,6 @@ NodeTopics::add_subscription(
   auto & node_gc = node_base_->get_notify_guard_condition();
   try {
     node_gc.trigger();
-    callback_group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string("failed to notify wait set on subscription creation: ") + ex.what());

rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp

@@ -35,17 +35,15 @@ NodeWaitables::add_waitable(
       // TODO(jacobperron): use custom exception
       throw std::runtime_error("Cannot create waitable, group not in node.");
     }
+    group->add_waitable(waitable_ptr);
   } else {
-    group = node_base_->get_default_callback_group();
+    node_base_->get_default_callback_group()->add_waitable(waitable_ptr);
   }
 
-  group->add_waitable(waitable_ptr);
-
   // Notify the executor that a new waitable was created using the parent Node.
   auto & node_gc = node_base_->get_notify_guard_condition();
   try {
     node_gc.trigger();
-    group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string("failed to notify wait set on waitable creation: ") + ex.what());

rclcpp/src/rclcpp/parameter_event_handler.cpp

@@ -133,9 +133,13 @@ ParameterEventHandler::get_parameter_from_event(
 {
   rclcpp::Parameter p;
   if (!get_parameter_from_event(event, p, parameter_name, node_name)) {
-    throw std::runtime_error(
-            "Parameter '" + parameter_name + "' of node '" + node_name +
-            "' is not part of parameter event");
+    if (event.node == node_name) {
+      return rclcpp::Parameter(parameter_name, rclcpp::PARAMETER_NOT_SET);
+    } else {
+      throw std::runtime_error(
+              "The node name '" + node_name + "' of parameter '" + parameter_name +
+              +"' doesn't match the node name '" + event.node + "' in parameter event");
+    }
   }
   return p;
 }

rclcpp/src/rclcpp/parameter_map.cpp

@@ -13,10 +13,8 @@
 // limitations under the License.
 
 #include <string>
-#include <regex>
 #include <vector>
 
-#include "rcpputils/find_and_replace.hpp"
 #include "rclcpp/parameter_map.hpp"
 
 using rclcpp::exceptions::InvalidParametersException;
@@ -26,12 +24,6 @@ using rclcpp::ParameterValue;
 
 ParameterMap
 rclcpp::parameter_map_from(const rcl_params_t * const c_params)
-{
-  return parameter_map_from(c_params, nullptr);
-}
-
-ParameterMap
-rclcpp::parameter_map_from(const rcl_params_t * const c_params, const char * node_fqn)
 {
   if (NULL == c_params) {
     throw InvalidParametersException("parameters struct is NULL");
@@ -57,17 +49,6 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params, const char * nod
       node_name = c_node_name;
     }
 
-    if (node_fqn) {
-      // Update the regular expression ["/*" -> "(/\\w+)" and "/**" -> "(/\\w+)*"]
-      std::string regex = rcpputils::find_and_replace(node_name, "/*", "(/\\w+)");
-      if (!std::regex_match(node_fqn, std::regex(regex))) {
-        // No need to parse the items because the user just care about node_fqn
-        continue;
-      }
-
-      node_name = node_fqn;
-    }
-
     const rcl_node_params_t * const c_params_node = &(c_params->params[n]);
 
     std::vector<Parameter> & params_node = parameters[node_name];
@@ -84,7 +65,6 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params, const char * nod
       params_node.emplace_back(c_param_name, parameter_value_from(c_param_value));
     }
   }
-
   return parameters;
 }
 

rclcpp/src/rclcpp/qos.cpp

@@ -150,6 +150,12 @@ QoS::best_effort()
   return this->reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
 }
 
+QoS &
+QoS::reliability_best_available()
+{
+  return this->reliability(RMW_QOS_POLICY_RELIABILITY_BEST_AVAILABLE);
+}
+
 QoS &
 QoS::durability(rmw_qos_durability_policy_t durability)
 {
@@ -176,6 +182,12 @@ QoS::transient_local()
   return this->durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
 }
 
+QoS &
+QoS::durability_best_available()
+{
+  return this->durability(RMW_QOS_POLICY_DURABILITY_BEST_AVAILABLE);
+}
+
 QoS &
 QoS::deadline(rmw_time_t deadline)
 {
@@ -380,4 +392,8 @@ SystemDefaultsQoS::SystemDefaultsQoS(const QoSInitialization & qos_initializatio
 : QoS(qos_initialization, rmw_qos_profile_system_default)
 {}
 
+BestAvailableQoS::BestAvailableQoS(const QoSInitialization & qos_initialization)
+: QoS(qos_initialization, rmw_qos_profile_best_available)
+{}
+
 }  // namespace rclcpp

rclcpp/src/rclcpp/signal_handler.cpp

@@ -113,7 +113,7 @@ SignalHandler::get_logger()
 SignalHandler &
 SignalHandler::get_global_signal_handler()
 {
-  static SignalHandler & signal_handler = *new SignalHandler();
+  static SignalHandler signal_handler;
   return signal_handler;
 }
 
@@ -191,9 +191,7 @@ SignalHandler::uninstall()
     signal_handlers_options_ = SignalHandlerOptions::None;
     RCLCPP_DEBUG(get_logger(), "SignalHandler::uninstall(): notifying deferred signal handler");
     notify_signal_handler();
-    if (signal_handler_thread_.joinable()) {
-      signal_handler_thread_.join();
-    }
+    signal_handler_thread_.join();
     teardown_wait_for_signal();
   } catch (...) {
     installed_.exchange(true);

rclcpp/src/rclcpp/subscription_base.cpp

@@ -229,20 +229,7 @@ SubscriptionBase::setup_intra_process(
 bool
 SubscriptionBase::can_loan_messages() const
 {
-  bool retval = rcl_subscription_can_loan_messages(subscription_handle_.get());
-  if (retval) {
-    // TODO(clalancette): The loaned message interface is currently not safe to use with
-    // shared_ptr callbacks.  If a user takes a copy of the shared_ptr, it can get freed from
-    // underneath them via rcl_return_loaned_message_from_subscription().  The correct solution is
-    // to return the loaned message in a custom deleter, but that needs to be carefully handled
-    // with locking.  Warn the user about this until we fix it.
-    RCLCPP_WARN_ONCE(
-      this->node_logger_,
-      "Loaned messages are only safe with const ref subscription callbacks. "
-      "If you are using any other kind of subscriptions, "
-      "set the ROS_DISABLE_LOANED_MESSAGES environment variable to 1 (the default).");
-  }
-  return retval;
+  return rcl_subscription_can_loan_messages(subscription_handle_.get());
 }
 
 rclcpp::Waitable::SharedPtr

rclcpp/test/rclcpp/CMakeLists.txt

@@ -100,20 +100,15 @@ if(TARGET test_create_subscription)
     "test_msgs"
   )
 endif()
-function(test_add_callback_groups_to_executor_for_rmw_implementation)
-  set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation})
-  ament_add_gmock(test_add_callback_groups_to_executor${target_suffix} test_add_callback_groups_to_executor.cpp
-    ENV ${rmw_implementation_env_var}
-    TIMEOUT 120
+ament_add_gtest(test_add_callback_groups_to_executor
+  test_add_callback_groups_to_executor.cpp
+  TIMEOUT 120)
+if(TARGET test_add_callback_groups_to_executor)
+  target_link_libraries(test_add_callback_groups_to_executor ${PROJECT_NAME})
+  ament_target_dependencies(test_add_callback_groups_to_executor
+    "test_msgs"
   )
-  if(TARGET test_add_callback_groups_to_executor${target_suffix})
-    target_link_libraries(test_add_callback_groups_to_executor${target_suffix} ${PROJECT_NAME})
-    ament_target_dependencies(test_add_callback_groups_to_executor${target_suffix}
-      "test_msgs"
-    )
-  endif()
-endfunction()
-call_for_each_rmw_implementation(test_add_callback_groups_to_executor_for_rmw_implementation)
+endif()
 ament_add_gtest(test_expand_topic_or_service_name test_expand_topic_or_service_name.cpp)
 if(TARGET test_expand_topic_or_service_name)
   ament_target_dependencies(test_expand_topic_or_service_name

rclcpp/test/rclcpp/allocator/test_allocator_common.cpp

@@ -51,53 +51,6 @@ TEST(TestAllocatorCommon, retyped_allocate) {
   EXPECT_NO_THROW(code2());
 }
 
-TEST(TestAllocatorCommon, retyped_zero_allocate_basic) {
-  std::allocator<int> allocator;
-  void * untyped_allocator = &allocator;
-  void * allocated_mem =
-    rclcpp::allocator::retyped_zero_allocate<std::allocator<char>>(20u, 1u, untyped_allocator);
-  ASSERT_TRUE(nullptr != allocated_mem);
-
-  auto code = [&untyped_allocator, allocated_mem]() {
-      rclcpp::allocator::retyped_deallocate<char, std::allocator<char>>(
-        allocated_mem, untyped_allocator);
-    };
-  EXPECT_NO_THROW(code());
-}
-
-TEST(TestAllocatorCommon, retyped_zero_allocate) {
-  std::allocator<int> allocator;
-  void * untyped_allocator = &allocator;
-  void * allocated_mem =
-    rclcpp::allocator::retyped_zero_allocate<std::allocator<char>>(20u, 1u, untyped_allocator);
-  // The more natural check here is ASSERT_NE(nullptr, ptr), but clang static
-  // analysis throws a false-positive memory leak warning.  Use ASSERT_TRUE instead.
-  ASSERT_TRUE(nullptr != allocated_mem);
-
-  auto code = [&untyped_allocator, allocated_mem]() {
-      rclcpp::allocator::retyped_deallocate<int, std::allocator<int>>(
-        allocated_mem, untyped_allocator);
-    };
-  EXPECT_NO_THROW(code());
-
-  allocated_mem = allocator.allocate(1);
-  // The more natural check here is ASSERT_NE(nullptr, ptr), but clang static
-  // analysis throws a false-positive memory leak warning.  Use ASSERT_TRUE instead.
-  ASSERT_TRUE(nullptr != allocated_mem);
-  void * reallocated_mem =
-    rclcpp::allocator::retyped_reallocate<int, std::allocator<int>>(
-    allocated_mem, 2u, untyped_allocator);
-  // The more natural check here is ASSERT_NE(nullptr, ptr), but clang static
-  // analysis throws a false-positive memory leak warning.  Use ASSERT_TRUE instead.
-  ASSERT_TRUE(nullptr != reallocated_mem);
-
-  auto code2 = [&untyped_allocator, reallocated_mem]() {
-      rclcpp::allocator::retyped_deallocate<int, std::allocator<int>>(
-        reallocated_mem, untyped_allocator);
-    };
-  EXPECT_NO_THROW(code2());
-}
-
 TEST(TestAllocatorCommon, get_rcl_allocator) {
   std::allocator<int> allocator;
   auto rcl_allocator = rclcpp::allocator::get_rcl_allocator<int>(allocator);

rclcpp/test/rclcpp/executors/test_executors.cpp

@@ -593,106 +593,3 @@ TEST(TestExecutors, testSpinUntilFutureCompleteNodePtr) {
 
   rclcpp::shutdown();
 }
-
-template<typename T>
-class TestIntraprocessExecutors : public ::testing::Test
-{
-public:
-  static void SetUpTestCase()
-  {
-    rclcpp::init(0, nullptr);
-  }
-
-  static void TearDownTestCase()
-  {
-    rclcpp::shutdown();
-  }
-
-  void SetUp()
-  {
-    const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info();
-    std::stringstream test_name;
-    test_name << test_info->test_case_name() << "_" << test_info->name();
-    node = std::make_shared<rclcpp::Node>("node", test_name.str());
-
-    callback_count = 0u;
-
-    const std::string topic_name = std::string("topic_") + test_name.str();
-
-    rclcpp::PublisherOptions po;
-    po.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable;
-    publisher = node->create_publisher<test_msgs::msg::Empty>(topic_name, rclcpp::QoS(1), po);
-
-    auto callback = [this](test_msgs::msg::Empty::ConstSharedPtr) {
-        this->callback_count.fetch_add(1u);
-      };
-
-    rclcpp::SubscriptionOptions so;
-    so.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable;
-    subscription =
-      node->create_subscription<test_msgs::msg::Empty>(
-      topic_name, rclcpp::QoS(kNumMessages), std::move(callback), so);
-  }
-
-  void TearDown()
-  {
-    publisher.reset();
-    subscription.reset();
-    node.reset();
-  }
-
-  const size_t kNumMessages = 100;
-
-  rclcpp::Node::SharedPtr node;
-  rclcpp::Publisher<test_msgs::msg::Empty>::SharedPtr publisher;
-  rclcpp::Subscription<test_msgs::msg::Empty>::SharedPtr subscription;
-  std::atomic_size_t callback_count;
-};
-
-TYPED_TEST_SUITE(TestIntraprocessExecutors, ExecutorTypes, ExecutorTypeNames);
-
-TYPED_TEST(TestIntraprocessExecutors, testIntraprocessRetrigger) {
-  // This tests that executors will continue to service intraprocess subscriptions in the case
-  // that publishers aren't continuing to publish.
-  // This was previously broken in that intraprocess guard conditions were only triggered on
-  // publish and the test was added to prevent future regressions.
-  const size_t kNumMessages = 100;
-
-  using ExecutorType = TypeParam;
-  ExecutorType executor;
-  executor.add_node(this->node);
-
-  EXPECT_EQ(0u, this->callback_count.load());
-  this->publisher->publish(test_msgs::msg::Empty());
-
-  // Wait for up to 5 seconds for the first message to come available.
-  const std::chrono::milliseconds sleep_per_loop(10);
-  int loops = 0;
-  while (1u != this->callback_count.load() && loops < 500) {
-    rclcpp::sleep_for(sleep_per_loop);
-    executor.spin_some();
-    loops++;
-  }
-  EXPECT_EQ(1u, this->callback_count.load());
-
-  // reset counter
-  this->callback_count.store(0u);
-
-  for (size_t ii = 0; ii < kNumMessages; ++ii) {
-    this->publisher->publish(test_msgs::msg::Empty());
-  }
-
-  // Fire a timer every 10ms up to 5 seconds waiting for subscriptions to be read.
-  loops = 0;
-  auto timer = this->node->create_wall_timer(
-    std::chrono::milliseconds(10), [this, &executor, &loops, &kNumMessages]() {
-      loops++;
-      if (kNumMessages == this->callback_count.load() ||
-      loops == 500)
-      {
-        executor.cancel();
-      }
-    });
-  executor.spin();
-  EXPECT_EQ(kNumMessages, this->callback_count.load());
-}

rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp

@@ -31,8 +31,6 @@
 #include "../../mocking_utils/patch.hpp"
 #include "../../utils/rclcpp_gtest_macros.hpp"
 
-#include "rcpputils/filesystem_helper.hpp"
-
 class TestNodeParameters : public ::testing::Test
 {
 public:
@@ -49,7 +47,6 @@ public:
       dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
       node->get_node_parameters_interface().get());
     ASSERT_NE(nullptr, node_parameters);
-    test_resources_path /= "test_node_parameters";
   }
 
   void TearDown()
@@ -60,8 +57,6 @@ public:
 protected:
   std::shared_ptr<rclcpp::Node> node;
   rclcpp::node_interfaces::NodeParameters * node_parameters;
-
-  rcpputils::fs::path test_resources_path{TEST_RESOURCES_DIRECTORY};
 };
 
 TEST_F(TestNodeParameters, construct_destruct_rcl_errors) {
@@ -204,130 +199,3 @@ TEST_F(TestNodeParameters, add_remove_parameters_callback) {
     node_parameters->remove_on_set_parameters_callback(handle.get()),
     std::runtime_error("Callback doesn't exist"));
 }
-
-TEST_F(TestNodeParameters, wildcard_with_namespace)
-{
-  rclcpp::NodeOptions opts;
-  opts.arguments(
-  {
-    "--ros-args",
-    "--params-file", (test_resources_path / "wildcards.yaml").string()
-  });
-
-  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node2", "ns", opts);
-
-  auto * node_parameters =
-    dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
-    node->get_node_parameters_interface().get());
-  ASSERT_NE(nullptr, node_parameters);
-
-  const auto & parameter_overrides = node_parameters->get_parameter_overrides();
-  EXPECT_EQ(7u, parameter_overrides.size());
-  EXPECT_EQ(parameter_overrides.at("full_wild").get<std::string>(), "full_wild");
-  EXPECT_EQ(parameter_overrides.at("namespace_wild").get<std::string>(), "namespace_wild");
-  EXPECT_EQ(
-    parameter_overrides.at("namespace_wild_another").get<std::string>(),
-    "namespace_wild_another");
-  EXPECT_EQ(
-    parameter_overrides.at("namespace_wild_one_star").get<std::string>(),
-    "namespace_wild_one_star");
-  EXPECT_EQ(parameter_overrides.at("node_wild_in_ns").get<std::string>(), "node_wild_in_ns");
-  EXPECT_EQ(
-    parameter_overrides.at("node_wild_in_ns_another").get<std::string>(),
-    "node_wild_in_ns_another");
-  EXPECT_EQ(parameter_overrides.at("explicit_in_ns").get<std::string>(), "explicit_in_ns");
-  EXPECT_EQ(parameter_overrides.count("should_not_appear"), 0u);
-}
-
-TEST_F(TestNodeParameters, wildcard_no_namespace)
-{
-  rclcpp::NodeOptions opts;
-  opts.arguments(
-  {
-    "--ros-args",
-    "--params-file", (test_resources_path / "wildcards.yaml").string()
-  });
-
-  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node2", opts);
-
-  auto * node_parameters =
-    dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
-    node->get_node_parameters_interface().get());
-  ASSERT_NE(nullptr, node_parameters);
-
-  const auto & parameter_overrides = node_parameters->get_parameter_overrides();
-  EXPECT_EQ(5u, parameter_overrides.size());
-  EXPECT_EQ(parameter_overrides.at("full_wild").get<std::string>(), "full_wild");
-  EXPECT_EQ(parameter_overrides.at("namespace_wild").get<std::string>(), "namespace_wild");
-  EXPECT_EQ(
-    parameter_overrides.at("namespace_wild_another").get<std::string>(),
-    "namespace_wild_another");
-  EXPECT_EQ(parameter_overrides.at("node_wild_no_ns").get<std::string>(), "node_wild_no_ns");
-  EXPECT_EQ(parameter_overrides.at("explicit_no_ns").get<std::string>(), "explicit_no_ns");
-  EXPECT_EQ(parameter_overrides.count("should_not_appear"), 0u);
-  // "/*" match exactly one token, not expect to get `namespace_wild_one_star`
-  EXPECT_EQ(parameter_overrides.count("namespace_wild_one_star"), 0u);
-}
-
-TEST_F(TestNodeParameters, params_by_order)
-{
-  rclcpp::NodeOptions opts;
-  opts.arguments(
-  {
-    "--ros-args",
-    "--params-file", (test_resources_path / "params_by_order.yaml").string()
-  });
-
-  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node2", "ns", opts);
-
-  auto * node_parameters =
-    dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
-    node->get_node_parameters_interface().get());
-  ASSERT_NE(nullptr, node_parameters);
-
-  const auto & parameter_overrides = node_parameters->get_parameter_overrides();
-  EXPECT_EQ(3u, parameter_overrides.size());
-  EXPECT_EQ(parameter_overrides.at("a_value").get<std::string>(), "last_one_win");
-  EXPECT_EQ(parameter_overrides.at("foo").get<std::string>(), "foo");
-  EXPECT_EQ(parameter_overrides.at("bar").get<std::string>(), "bar");
-}
-
-TEST_F(TestNodeParameters, complicated_wildcards)
-{
-  rclcpp::NodeOptions opts;
-  opts.arguments(
-  {
-    "--ros-args",
-    "--params-file", (test_resources_path / "complicated_wildcards.yaml").string()
-  });
-
-  {
-    // regex matched: /**/foo/*/bar
-    std::shared_ptr<rclcpp::Node> node =
-      std::make_shared<rclcpp::Node>("node2", "/a/b/c/foo/d/bar", opts);
-
-    auto * node_parameters =
-      dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
-      node->get_node_parameters_interface().get());
-    ASSERT_NE(nullptr, node_parameters);
-
-    const auto & parameter_overrides = node_parameters->get_parameter_overrides();
-    EXPECT_EQ(2u, parameter_overrides.size());
-    EXPECT_EQ(parameter_overrides.at("foo").get<std::string>(), "foo");
-    EXPECT_EQ(parameter_overrides.at("bar").get<std::string>(), "bar");
-  }
-
-  {
-    // regex not matched: /**/foo/*/bar
-    std::shared_ptr<rclcpp::Node> node =
-      std::make_shared<rclcpp::Node>("node2", "/a/b/c/foo/bar", opts);
-
-    auto * node_parameters =
-      dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
-      node->get_node_parameters_interface().get());
-    ASSERT_NE(nullptr, node_parameters);
-
-    const auto & parameter_overrides = node_parameters->get_parameter_overrides();
-    EXPECT_EQ(0u, parameter_overrides.size());
-  }
-}

rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp

@@ -276,70 +276,6 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, one_node_many_callback_groups_many_e
   ASSERT_EQ(timer_executor.get_all_callback_groups().size(), 2u);
 }
 
-/*
- * Test callback groups from one node to many executors.
- * A subscriber on a new executor with a callback group not received a message
- * because the executor can't be triggered while a subscriber created, see
- * https://github.com/ros2/rclcpp/issues/1611
-*/
-TYPED_TEST(TestAddCallbackGroupsToExecutor, subscriber_triggered_to_receive_message)
-{
-  auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
-
-  // create a thread running an executor with a new callback group for a coming subscriber
-  rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group(
-    rclcpp::CallbackGroupType::MutuallyExclusive, false);
-  rclcpp::executors::SingleThreadedExecutor cb_grp_executor;
-
-  std::promise<bool> received_message_promise;
-  auto received_message_future = received_message_promise.get_future();
-  rclcpp::FutureReturnCode return_code = rclcpp::FutureReturnCode::TIMEOUT;
-  std::thread cb_grp_thread = std::thread(
-    [&cb_grp, &node, &cb_grp_executor, &received_message_future, &return_code]() {
-      cb_grp_executor.add_callback_group(cb_grp, node->get_node_base_interface());
-      return_code = cb_grp_executor.spin_until_future_complete(received_message_future, 10s);
-    });
-
-  // expect the subscriber to receive a message
-  auto sub_callback = [&received_message_promise](test_msgs::msg::Empty::ConstSharedPtr) {
-      received_message_promise.set_value(true);
-    };
-
-  rclcpp::Subscription<test_msgs::msg::Empty>::SharedPtr subscription;
-  rclcpp::Publisher<test_msgs::msg::Empty>::SharedPtr publisher;
-  // to create a timer with a callback run on another executor
-  rclcpp::TimerBase::SharedPtr timer = nullptr;
-  std::promise<void> timer_promise;
-  auto timer_callback =
-    [&subscription, &publisher, &timer, &cb_grp, &node, &sub_callback, &timer_promise]() {
-      if (timer) {
-        timer.reset();
-      }
-
-      // create a subscription using the `cb_grp` callback group
-      rclcpp::QoS qos = rclcpp::QoS(1).reliable();
-      auto options = rclcpp::SubscriptionOptions();
-      options.callback_group = cb_grp;
-      subscription =
-        node->create_subscription<test_msgs::msg::Empty>("topic_name", qos, sub_callback, options);
-      // create a publisher to send data
-      publisher =
-        node->create_publisher<test_msgs::msg::Empty>("topic_name", qos);
-      publisher->publish(test_msgs::msg::Empty());
-      timer_promise.set_value();
-    };
-
-  rclcpp::executors::SingleThreadedExecutor timer_executor;
-  timer = node->create_wall_timer(100ms, timer_callback);
-  timer_executor.add_node(node);
-  auto future = timer_promise.get_future();
-  timer_executor.spin_until_future_complete(future);
-  cb_grp_thread.join();
-
-  ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code);
-  EXPECT_TRUE(received_message_future.get());
-}
-
 /*
  * Test removing callback group from executor that its not associated with.
  */

rclcpp/test/rclcpp/test_client.cpp

@@ -282,27 +282,6 @@ TEST_F(TestClientWithServer, test_client_remove_pending_request) {
   EXPECT_TRUE(client->remove_pending_request(future));
 }
 
-TEST_F(TestClientWithServer, prune_requests_older_than_no_pruned) {
-  auto client = node->create_client<test_msgs::srv::Empty>(service_name);
-  auto request = std::make_shared<test_msgs::srv::Empty::Request>();
-  auto future = client->async_send_request(request);
-  auto time = std::chrono::system_clock::now() + 1s;
-
-  EXPECT_EQ(1u, client->prune_requests_older_than(time));
-}
-
-TEST_F(TestClientWithServer, prune_requests_older_than_with_pruned) {
-  auto client = node->create_client<test_msgs::srv::Empty>(service_name);
-  auto request = std::make_shared<test_msgs::srv::Empty::Request>();
-  auto future = client->async_send_request(request);
-  auto time = std::chrono::system_clock::now() + 1s;
-
-  std::vector<int64_t> pruned_requests;
-  EXPECT_EQ(1u, client->prune_requests_older_than(time, &pruned_requests));
-  ASSERT_EQ(1u, pruned_requests.size());
-  EXPECT_EQ(future.request_id, pruned_requests[0]);
-}
-
 TEST_F(TestClientWithServer, async_send_request_rcl_send_request_error) {
   // Checking rcl_send_request in rclcpp::Client::async_send_request()
   auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_request, RCL_RET_ERROR);

rclcpp/test/rclcpp/test_create_timer.cpp

@@ -116,3 +116,20 @@ TEST(TestCreateTimer, call_wall_timer_with_bad_arguments)
     std::invalid_argument);
   rclcpp::shutdown();
 }
+
+static void test_timer_callback(void) {}
+
+TEST(TestCreateTimer, timer_function_pointer)
+{
+  rclcpp::init(0, nullptr);
+  auto node = std::make_shared<rclcpp::Node>("timer_function_pointer_node");
+
+  // make sure build succeeds with function pointer instead of lambda
+  auto some_timer = rclcpp::create_timer(
+    node,
+    node->get_clock(),
+    rclcpp::Duration(0ms),
+    test_timer_callback);
+
+  rclcpp::shutdown();
+}

rclcpp/test/rclcpp/test_parameter_event_handler.cpp

@@ -202,9 +202,17 @@ TEST_F(TestNode, GetParameterFromEvent)
   EXPECT_THROW(
     ParameterEventHandler::get_parameter_from_event(*multiple, "my_int", wrong_name),
     std::runtime_error);
-  // Throws if parameter not part of event
+
+  // Parameter not part of event
+  // with correct node
+  rclcpp::Parameter expect_notset_ret("my_notset", rclcpp::PARAMETER_NOT_SET);
+  rclcpp::Parameter ret;
+  EXPECT_NO_THROW(
+    ret = ParameterEventHandler::get_parameter_from_event(*multiple, "my_notset", node_name););
+  EXPECT_EQ(ret, expect_notset_ret);
+  // with incorrect node
   EXPECT_THROW(
-    ParameterEventHandler::get_parameter_from_event(*diff_ns_bool, "my_int", node_name),
+    ParameterEventHandler::get_parameter_from_event(*multiple, "my_notset", wrong_name),
     std::runtime_error);
 }
 

rclcpp/test/rclcpp/test_parameter_map.cpp

@@ -19,7 +19,6 @@
 
 #include <cstdio>
 #include <string>
-#include <unordered_map>
 #include <vector>
 
 #include "rclcpp/parameter_map.hpp"
@@ -354,132 +353,3 @@ TEST(Test_parameter_map_from, string_array_param_value)
   c_params->params[0].parameter_values[0].string_array_value = NULL;
   rcl_yaml_node_struct_fini(c_params);
 }
-
-TEST(Test_parameter_map_from, one_node_one_param_by_node_fqn)
-{
-  rcl_params_t * c_params = make_params({"foo"});
-  make_node_params(c_params, 0, {"string_param"});
-
-  std::string hello_world = "hello world";
-  char * c_hello_world = new char[hello_world.length() + 1];
-  std::snprintf(c_hello_world, hello_world.size() + 1, "%s", hello_world.c_str());
-  c_params->params[0].parameter_values[0].string_value = c_hello_world;
-
-  rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params, "/foo");
-  const std::vector<rclcpp::Parameter> & params = map.at("/foo");
-  EXPECT_STREQ("string_param", params.at(0).get_name().c_str());
-  EXPECT_STREQ(hello_world.c_str(), params.at(0).get_value<std::string>().c_str());
-
-  c_params->params[0].parameter_values[0].string_value = NULL;
-  delete[] c_hello_world;
-  rcl_yaml_node_struct_fini(c_params);
-}
-
-TEST(Test_parameter_map_from, multi_nodes_same_param_name_by_node_fqn)
-{
-  std::vector<std::string> node_names_keys = {
-    "/**",              // index: 0
-    "/*",               // index: 1
-    "/**/node",         // index: 2
-    "/*/node",          // index: 3
-    "/ns/node"          // index: 4
-  };
-
-  rcl_params_t * c_params = make_params(node_names_keys);
-
-  std::vector<char *> param_values;
-  for (size_t i = 0; i < node_names_keys.size(); ++i) {
-    make_node_params(c_params, i, {"string_param"});
-    std::string hello_world = "hello world" + std::to_string(i);
-    char * c_hello_world = new char[hello_world.length() + 1];
-    std::snprintf(c_hello_world, hello_world.size() + 1, "%s", hello_world.c_str());
-    c_params->params[i].parameter_values[0].string_value = c_hello_world;
-    param_values.push_back(c_hello_world);
-  }
-
-  std::unordered_map<std::string, std::vector<size_t>> node_fqn_expected = {
-    {"/ns/foo/another_node", {0}},
-    {"/another", {0, 1}},
-    {"/node", {0, 1, 2}},
-    {"/another_ns/node", {0, 2, 3}},
-    {"/ns/node", {0, 2, 3, 4}},
-  };
-
-  for (auto & kv : node_fqn_expected) {
-    rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params, kv.first.c_str());
-    const std::vector<rclcpp::Parameter> & params = map.at(kv.first);
-
-    EXPECT_EQ(kv.second.size(), params.size());
-    for (size_t i = 0; i < params.size(); ++i) {
-      std::string param_value = "hello world" + std::to_string(kv.second[i]);
-      EXPECT_STREQ("string_param", params.at(i).get_name().c_str());
-      EXPECT_STREQ(param_value.c_str(), params.at(i).get_value<std::string>().c_str());
-    }
-  }
-
-  for (size_t i = 0; i < node_names_keys.size(); ++i) {
-    c_params->params[i].parameter_values[0].string_value = NULL;
-  }
-  for (auto c_hello_world : param_values) {
-    delete[] c_hello_world;
-  }
-  rcl_yaml_node_struct_fini(c_params);
-}
-
-TEST(Test_parameter_map_from, multi_nodes_diff_param_name_by_node_fqn)
-{
-  std::vector<std::string> node_names_keys = {
-    "/**",              // index: 0
-    "/*",               // index: 1
-    "/**/node",         // index: 2
-    "/*/node",          // index: 3
-    "/ns/**",           // index: 4
-    "/ns/*",            // index: 5
-    "/ns/**/node",      // index: 6
-    "/ns/*/node",       // index: 7
-    "/ns/**/a/*/node",  // index: 8
-    "/ns/node"          // index: 9
-  };
-
-  rcl_params_t * c_params = make_params(node_names_keys);
-
-  for (size_t i = 0; i < node_names_keys.size(); ++i) {
-    std::string param_name = "string_param" + std::to_string(i);
-    make_node_params(c_params, i, {param_name});
-  }
-
-  std::string hello_world = "hello world";
-  char * c_hello_world = new char[hello_world.length() + 1];
-  std::snprintf(c_hello_world, hello_world.size() + 1, "%s", hello_world.c_str());
-
-  for (size_t i = 0; i < node_names_keys.size(); ++i) {
-    c_params->params[i].parameter_values[0].string_value = c_hello_world;
-  }
-
-  std::unordered_map<std::string, std::vector<size_t>> node_fqn_expected = {
-    {"/ns/node", {0, 2, 3, 4, 5, 6, 9}},
-    {"/node", {0, 1, 2}},
-    {"/ns/foo/node", {0, 2, 4, 6, 7}},
-    {"/ns/foo/a/node", {0, 2, 4, 6}},
-    {"/ns/foo/a/bar/node", {0, 2, 4, 6, 8}},
-    {"/ns/a/bar/node", {0, 2, 4, 6, 8}},
-    {"/ns/foo/zoo/a/bar/node", {0, 2, 4, 6, 8}},
-  };
-
-  for (auto & kv : node_fqn_expected) {
-    rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params, kv.first.c_str());
-    const std::vector<rclcpp::Parameter> & params = map.at(kv.first);
-    EXPECT_EQ(kv.second.size(), params.size());
-    for (size_t i = 0; i < params.size(); ++i) {
-      std::string param_name = "string_param" + std::to_string(kv.second[i]);
-      EXPECT_STREQ(param_name.c_str(), params.at(i).get_name().c_str());
-      EXPECT_STREQ(hello_world.c_str(), params.at(i).get_value<std::string>().c_str());
-    }
-  }
-
-  for (size_t i = 0; i < node_names_keys.size(); ++i) {
-    c_params->params[i].parameter_values[0].string_value = NULL;
-  }
-  delete[] c_hello_world;
-  rcl_yaml_node_struct_fini(c_params);
-}

rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp

@@ -152,54 +152,33 @@ TEST_F(TestPublisher, conversion_exception_is_passed_up) {
   }
 }
 
-using UseTakeSharedMethod = bool;
-class TestPublisherFixture
-  : public TestPublisher,
-  public ::testing::WithParamInterface<UseTakeSharedMethod>
-{
-};
-
 /*
  * Testing that publisher sends type adapted types and ROS message types with intra proccess communications.
  */
-TEST_P(
-  TestPublisherFixture,
+TEST_F(
+  TestPublisher,
   check_type_adapted_message_is_sent_and_received_intra_process) {
   using StringTypeAdapter = rclcpp::TypeAdapter<std::string, rclcpp::msg::String>;
   const std::string message_data = "Message Data";
   const std::string topic_name = "topic_name";
   bool is_received;
 
+  auto callback =
+    [message_data, &is_received](
+    const rclcpp::msg::String::ConstSharedPtr msg,
+    const rclcpp::MessageInfo & message_info
+    ) -> void
+    {
+      is_received = true;
+      ASSERT_STREQ(message_data.c_str(), msg->data.c_str());
+      ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process);
+    };
+
   auto node = rclcpp::Node::make_shared(
     "test_intra_process",
     rclcpp::NodeOptions().use_intra_process_comms(true));
   auto pub = node->create_publisher<StringTypeAdapter>(topic_name, 10);
-  rclcpp::Subscription<rclcpp::msg::String>::SharedPtr sub;
-  if (GetParam()) {
-    auto callback =
-      [message_data, &is_received](
-      const rclcpp::msg::String::ConstSharedPtr msg,
-      const rclcpp::MessageInfo & message_info
-      ) -> void
-      {
-        is_received = true;
-        ASSERT_STREQ(message_data.c_str(), msg->data.c_str());
-        ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process);
-      };
-    sub = node->create_subscription<rclcpp::msg::String>(topic_name, 1, callback);
-  } else {
-    auto callback_unique =
-      [message_data, &is_received](
-      rclcpp::msg::String::UniquePtr msg,
-      const rclcpp::MessageInfo & message_info
-      ) -> void
-      {
-        is_received = true;
-        ASSERT_STREQ(message_data.c_str(), msg->data.c_str());
-        ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process);
-      };
-    sub = node->create_subscription<rclcpp::msg::String>(topic_name, 1, callback_unique);
-  }
+  auto sub = node->create_subscription<rclcpp::msg::String>(topic_name, 1, callback);
 
   auto wait_for_message_to_be_received = [&is_received, &node]() {
       rclcpp::executors::SingleThreadedExecutor executor;
@@ -260,14 +239,6 @@ TEST_P(
   }
 }
 
-INSTANTIATE_TEST_SUITE_P(
-  TestPublisherFixtureWithParam,
-  TestPublisherFixture,
-  ::testing::Values(
-    true,   // use take shared method
-    false   // not use take shared method
-));
-
 /*
  * Testing that publisher sends type adapted types and ROS message types with inter proccess communications.
  */

rclcpp/test/rclcpp/test_qos.cpp

@@ -93,6 +93,9 @@ TEST(TestQoS, setters_and_getters) {
   qos.reliable();
   EXPECT_EQ(rclcpp::ReliabilityPolicy::Reliable, qos.reliability());
 
+  qos.reliability_best_available();
+  EXPECT_EQ(rclcpp::ReliabilityPolicy::BestAvailable, qos.reliability());
+
   qos.reliability(rclcpp::ReliabilityPolicy::BestEffort);
   EXPECT_EQ(rclcpp::ReliabilityPolicy::BestEffort, qos.reliability());
 
@@ -102,6 +105,9 @@ TEST(TestQoS, setters_and_getters) {
   qos.transient_local();
   EXPECT_EQ(rclcpp::DurabilityPolicy::TransientLocal, qos.durability());
 
+  qos.durability_best_available();
+  EXPECT_EQ(rclcpp::DurabilityPolicy::BestAvailable, qos.durability());
+
   qos.durability(rclcpp::DurabilityPolicy::Volatile);
   EXPECT_EQ(rclcpp::DurabilityPolicy::Volatile, qos.durability());
 
@@ -183,6 +189,9 @@ TEST(TestQoS, DerivedTypes) {
   const rclcpp::KeepLast expected_initialization(RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT);
   const rclcpp::QoS expected_default(expected_initialization);
   EXPECT_EQ(expected_default.get_rmw_qos_profile(), system_default_qos.get_rmw_qos_profile());
+
+  rclcpp::BestAvailableQoS best_available_qos;
+  EXPECT_EQ(rmw_qos_profile_best_available, best_available_qos.get_rmw_qos_profile());
 }
 
 TEST(TestQoS, policy_name_from_kind) {

rclcpp/test/rclcpp/test_rate.cpp

@@ -18,24 +18,10 @@
 
 #include "rclcpp/rate.hpp"
 
-class TestRate : public ::testing::Test
-{
-public:
-  void SetUp()
-  {
-    rclcpp::init(0, nullptr);
-  }
-
-  void TearDown()
-  {
-    rclcpp::shutdown();
-  }
-};
-
 /*
    Basic tests for the Rate and WallRate classes.
  */
-TEST_F(TestRate, rate_basics) {
+TEST(TestRate, rate_basics) {
   auto period = std::chrono::milliseconds(1000);
   auto offset = std::chrono::milliseconds(500);
   auto epsilon = std::chrono::milliseconds(100);
@@ -75,7 +61,7 @@ TEST_F(TestRate, rate_basics) {
   ASSERT_TRUE(epsilon > delta);
 }
 
-TEST_F(TestRate, wall_rate_basics) {
+TEST(TestRate, wall_rate_basics) {
   auto period = std::chrono::milliseconds(100);
   auto offset = std::chrono::milliseconds(50);
   auto epsilon = std::chrono::milliseconds(1);
@@ -115,7 +101,7 @@ TEST_F(TestRate, wall_rate_basics) {
   EXPECT_GT(epsilon, delta);
 }
 
-TEST_F(TestRate, from_double) {
+TEST(TestRate, from_double) {
   {
     rclcpp::WallRate rate(1.0);
     EXPECT_EQ(std::chrono::seconds(1), rate.period());

rclcpp/test/rclcpp/test_time.cpp

@@ -19,7 +19,6 @@
 #include <limits>
 #include <memory>
 #include <string>
-#include <thread>
 
 #include "rcl/error_handling.h"
 #include "rcl/time.h"
@@ -849,72 +848,3 @@ TEST_F(TestClockSleep, sleep_for_basic_ros) {
   sleep_thread.join();
   EXPECT_TRUE(sleep_succeeded);
 }
-
-class TestClockStarted : public ::testing::Test
-{
-protected:
-  void SetUp()
-  {
-    rclcpp::init(0, nullptr);
-  }
-
-  void TearDown()
-  {
-    rclcpp::shutdown();
-  }
-};
-
-TEST_F(TestClockStarted, started) {
-  // rclcpp::Clock ros_clock(RCL_ROS_TIME);
-  // auto ros_clock_handle = ros_clock.get_clock_handle();
-  //
-  // // At this point, the ROS clock is reading system time since the ROS time override isn't on
-  // // So we expect it to be started (it's extremely unlikely that system time is at epoch start)
-  // EXPECT_TRUE(ros_clock.started());
-  // EXPECT_TRUE(ros_clock.wait_until_started());
-  // EXPECT_TRUE(ros_clock.wait_until_started(rclcpp::Duration(0, static_cast<uint32_t>(1e7))));
-  // EXPECT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(ros_clock_handle));
-  // EXPECT_TRUE(ros_clock.ros_time_is_active());
-  // EXPECT_FALSE(ros_clock.started());
-  // EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(ros_clock_handle, 1));
-  // EXPECT_TRUE(ros_clock.started());
-  //
-  // rclcpp::Clock system_clock(RCL_SYSTEM_TIME);
-  // EXPECT_TRUE(system_clock.started());
-  // EXPECT_TRUE(system_clock.wait_until_started());
-  // EXPECT_TRUE(system_clock.wait_until_started(rclcpp::Duration(0, static_cast<uint32_t>(1e7))));
-  //
-  // rclcpp::Clock steady_clock(RCL_STEADY_TIME);
-  // EXPECT_TRUE(steady_clock.started());
-  // EXPECT_TRUE(steady_clock.wait_until_started());
-  // EXPECT_TRUE(steady_clock.wait_until_started(rclcpp::Duration(0, static_cast<uint32_t>(1e7))));
-  //
-  // rclcpp::Clock uninit_clock(RCL_CLOCK_UNINITIALIZED);
-  // RCLCPP_EXPECT_THROW_EQ(
-  //   uninit_clock.started(), std::runtime_error("clock is not rcl_clock_valid"));
-  // RCLCPP_EXPECT_THROW_EQ(
-  //   uninit_clock.wait_until_started(rclcpp::Duration(0, static_cast<uint32_t>(1e7))),
-  //   std::runtime_error("clock cannot be waited on as it is not rcl_clock_valid"));
-}
-
-TEST_F(TestClockStarted, started_timeout) {
-  rclcpp::Clock ros_clock(RCL_ROS_TIME);
-  auto ros_clock_handle = ros_clock.get_clock_handle();
-
-  EXPECT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(ros_clock_handle));
-  EXPECT_TRUE(ros_clock.ros_time_is_active());
-
-  EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(ros_clock_handle, 0));
-
-  EXPECT_FALSE(ros_clock.started());
-  EXPECT_FALSE(ros_clock.wait_until_started(rclcpp::Duration(0, static_cast<uint32_t>(1e7))));
-
-  std::thread t([]() {
-      std::this_thread::sleep_for(std::chrono::seconds(1));
-      rclcpp::shutdown();
-    });
-
-  // Test rclcpp shutdown escape hatch (otherwise this waits indefinitely)
-  EXPECT_FALSE(ros_clock.wait_until_started());
-  t.join();
-}

rclcpp/test/resources/test_node_parameters/complicated_wildcards.yaml

@@ -1,5 +0,0 @@
-/**/foo/*/bar:
-  node2:
-    ros__parameters:
-      foo: "foo"
-      bar: "bar"

rclcpp/test/resources/test_node_parameters/params_by_order.yaml

@@ -1,16 +0,0 @@
-/**:
-  node2:
-    ros__parameters:
-      a_value: "first"
-      foo: "foo"
-
-/ns:
-  node2:
-    ros__parameters:
-      a_value: "second"
-      bar: "bar"
-
-/*:
-  node2:
-    ros__parameters:
-      a_value: "last_one_win"

rclcpp/test/resources/test_node_parameters/wildcards.yaml

@@ -1,57 +0,0 @@
-/**:
-  ros__parameters:
-    full_wild: "full_wild"
-
-/**:
-  node2:
-    ros__parameters:
-      namespace_wild: "namespace_wild"
-
-/**/node2:
-  ros__parameters:
-    namespace_wild_another: "namespace_wild_another"
-
-/*:
-  node2:
-    ros__parameters:
-      namespace_wild_one_star: "namespace_wild_one_star"
-
-ns:
-  "*":
-    ros__parameters:
-      node_wild_in_ns: "node_wild_in_ns"
-
-/ns/*:
-  ros__parameters:
-    node_wild_in_ns_another: "node_wild_in_ns_another"
-
-ns:
-  node2:
-    ros__parameters:
-      explicit_in_ns: "explicit_in_ns"
-
-"*":
-  ros__parameters:
-    node_wild_no_ns: "node_wild_no_ns"
-
-node2:
-  ros__parameters:
-    explicit_no_ns: "explicit_no_ns"
-
-ns:
-  nodeX:
-    ros__parameters:
-      should_not_appear: "incorrect_node_name"
-
-/**/nodeX:
-  ros__parameters:
-    should_not_appear: "incorrect_node_name"
-
-nsX:
-  node2:
-    ros__parameters:
-      should_not_appear: "incorrect_namespace"
-
-/nsX/*:
-  ros__parameters:
-    should_not_appear: "incorrect_namespace"

rclcpp_action/CHANGELOG.rst

@@ -3,26 +3,10 @@ Changelog for package rclcpp_action
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
-16.0.7 (2023-11-13)
+16.2.0 (2022-05-03)
 -------------------
 
-16.0.6 (2023-09-19)
--------------------
-
-16.0.5 (2023-07-17)
--------------------
-
-16.0.4 (2023-04-25)
--------------------
-* Revert "Revert "extract the result response before the callback is issued. (`#2133 <https://github.com/ros2/rclcpp/issues/2133>`_)" (`#2148 <https://github.com/ros2/rclcpp/issues/2148>`_)" (`#2152 <https://github.com/ros2/rclcpp/issues/2152>`_)
-* Revert "extract the result response before the callback is issued. (`#2133 <https://github.com/ros2/rclcpp/issues/2133>`_)" (`#2148 <https://github.com/ros2/rclcpp/issues/2148>`_)
-* extract the result response before the callback is issued. (`#2133 <https://github.com/ros2/rclcpp/issues/2133>`_)
-* Contributors: Tomoya Fujita
-
-16.0.3 (2023-01-10)
--------------------
-
-16.0.2 (2022-11-07)
+16.1.0 (2022-04-29)
 -------------------
 
 16.0.1 (2022-04-13)

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>16.0.7</version>
+  <version>16.2.0</version>
   <description>Adds action APIs for C++.</description>
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
   <maintainer email="jacob@openrobotics.org">Jacob Perron</maintainer>

rclcpp_action/src/client.cpp

@@ -319,18 +319,14 @@ ClientBase::handle_result_response(
   const rmw_request_id_t & response_header,
   std::shared_ptr<void> response)
 {
-  ResponseCallback response_callback;
-  {
-    std::lock_guard<std::mutex> guard(pimpl_->result_requests_mutex);
-    const int64_t & sequence_number = response_header.sequence_number;
-    if (pimpl_->pending_result_responses.count(sequence_number) == 0) {
-      RCLCPP_ERROR(pimpl_->logger, "unknown result response, ignoring...");
-      return;
-    }
-    response_callback = std::move(pimpl_->pending_result_responses[sequence_number]);
-    pimpl_->pending_result_responses.erase(sequence_number);
+  std::lock_guard<std::mutex> guard(pimpl_->result_requests_mutex);
+  const int64_t & sequence_number = response_header.sequence_number;
+  if (pimpl_->pending_result_responses.count(sequence_number) == 0) {
+    RCLCPP_ERROR(pimpl_->logger, "unknown result response, ignoring...");
+    return;
   }
-  response_callback(response);
+  pimpl_->pending_result_responses[sequence_number](response);
+  pimpl_->pending_result_responses.erase(sequence_number);
 }
 
 void

rclcpp_components/CHANGELOG.rst

@@ -2,22 +2,10 @@
 Changelog for package rclcpp_components
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-16.0.7 (2023-11-13)
+16.2.0 (2022-05-03)
 -------------------
 
-16.0.6 (2023-09-19)
--------------------
-
-16.0.5 (2023-07-17)
--------------------
-
-16.0.4 (2023-04-25)
--------------------
-
-16.0.3 (2023-01-10)
--------------------
-
-16.0.2 (2022-11-07)
+16.1.0 (2022-04-29)
 -------------------
 
 16.0.1 (2022-04-13)

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>16.0.7</version>
+  <version>16.2.0</version>
   <description>Package containing tools for dynamically loadable components</description>
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
   <maintainer email="jacob@openrobotics.org">Jacob Perron</maintainer>

rclcpp_lifecycle/CHANGELOG.rst

@@ -3,30 +3,10 @@ Changelog for package rclcpp_lifecycle
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
-16.0.7 (2023-11-13)
+16.2.0 (2022-05-03)
 -------------------
 
-16.0.6 (2023-09-19)
--------------------
-* Switch lifecycle to use the RCLCPP macros Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> (`#2234 <https://github.com/ros2/rclcpp/issues/2234>`_)
-* Contributors: Tony Najjar
-
-16.0.5 (2023-07-17)
--------------------
-* Fix thread safety in LifecycleNode::get_current_state() for Humble (`#2183 <https://github.com/ros2/rclcpp/issues/2183>`_)
-  * add initially-failing test case
-  * apply changes to LifecycleNodeInterfaceImpl from `#1756 <https://github.com/ros2/rclcpp/issues/1756>`_
-  * add static member to State for managing state_handle\_ access
-  * allow parallel read access in MutexMap
-* Contributors: Joseph Schornak
-
-16.0.4 (2023-04-25)
--------------------
-
-16.0.3 (2023-01-10)
--------------------
-
-16.0.2 (2022-11-07)
+16.1.0 (2022-04-29)
 -------------------
 
 16.0.1 (2022-04-13)

rclcpp_lifecycle/CMakeLists.txt

@@ -20,8 +20,6 @@ find_package(lifecycle_msgs REQUIRED)
 add_library(rclcpp_lifecycle
   src/lifecycle_node.cpp
   src/managed_entity.cpp
-  src/mutex_map.cpp
-  src/mutex_map.hpp
   src/node_interfaces/lifecycle_node_interface.cpp
   src/state.cpp
   src/transition.cpp

rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp

@@ -24,8 +24,6 @@
 
 namespace rclcpp_lifecycle
 {
-/// Forward declaration of mutex helper class
-class MutexMap;
 
 /// Abstract class for the Lifecycle's states.
 /**
@@ -94,21 +92,6 @@ protected:
   bool owns_rcl_state_handle_;
 
   rcl_lifecycle_state_t * state_handle_;
-
-private:
-  /// Maps state handle mutexes to each instance of State.
-  /**
-   * \details A mutex is added to this map when each new instance of State is constructed.
-   *
-   * The mutex is removed when the instance of State is destroyed.
-   *
-   * The mutex is locked while state_handle_ is being accessed.
-   *
-   * This static member exists to allow implementing the fix described in ros2/rclcpp#1756
-   * in Humble without breaking ABI compatibility, since adding a new static data
-   * member is permitted under REP-0009.
-   */
-  static MutexMap state_handle_mutex_map_;
 };
 
 }  // namespace 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>16.0.7</version>
+  <version>16.2.0</version>
   <description>Package containing a prototype for lifecycle implementation</description>
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
   <maintainer email="jacob@openrobotics.org">Jacob Perron</maintainer>

rclcpp_lifecycle/src/lifecycle_node.cpp

@@ -107,7 +107,7 @@ LifecycleNode::LifecycleNode(
     )),
   node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())),
   node_options_(options),
-  impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_, node_logging_))
+  impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_))
 {
   impl_->init(enable_communication_interface);
 

rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp

@@ -20,7 +20,6 @@
 #include <functional>
 #include <map>
 #include <memory>
-#include <mutex>
 #include <string>
 #include <vector>
 #include <utility>
@@ -39,7 +38,6 @@
 #include "rcl_lifecycle/transition_map.h"
 
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
-#include "rclcpp/node_interfaces/node_logging_interface.hpp"
 #include "rclcpp/node_interfaces/node_services_interface.hpp"
 
 #include "rcutils/logging_macros.h"
@@ -58,24 +56,18 @@ class LifecycleNode::LifecycleNodeInterfaceImpl
 public:
   LifecycleNodeInterfaceImpl(
     std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base_interface,
-    std::shared_ptr<rclcpp::node_interfaces::NodeServicesInterface> node_services_interface,
-    std::shared_ptr<rclcpp::node_interfaces::NodeLoggingInterface> node_logging_interface)
+    std::shared_ptr<rclcpp::node_interfaces::NodeServicesInterface> node_services_interface)
   : node_base_interface_(node_base_interface),
-    node_services_interface_(node_services_interface),
-    node_logging_interface_(node_logging_interface)
+    node_services_interface_(node_services_interface)
   {}
 
   ~LifecycleNodeInterfaceImpl()
   {
     rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle();
-    rcl_ret_t ret;
-    {
-      std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
-      ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle);
-    }
+    auto ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle);
     if (ret != RCL_RET_OK) {
-      RCLCPP_FATAL(
-        node_logging_interface_->get_logger(),
+      RCUTILS_LOG_FATAL_NAMED(
+        "rclcpp_lifecycle",
         "failed to destroy rcl_state_machine");
     }
   }
@@ -86,6 +78,7 @@ public:
     rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle();
     const rcl_node_options_t * node_options =
       rcl_node_get_options(node_base_interface_->get_rcl_node_handle());
+    state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine();
     auto state_machine_options = rcl_lifecycle_get_default_state_machine_options();
     state_machine_options.enable_com_interface = enable_communication_interface;
     state_machine_options.allocator = node_options->allocator;
@@ -96,8 +89,6 @@ public:
     // The publisher takes a C-Typesupport since the publishing (i.e. creating
     // the message) is done fully in RCL.
     // Services are handled in C++, so that it needs a C++ typesupport structure.
-    std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
-    state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine();
     rcl_ret_t ret = rcl_lifecycle_state_machine_init(
       &state_machine_,
       node_handle,
@@ -114,8 +105,6 @@ public:
               node_base_interface_->get_name());
     }
 
-    current_state_ = State(state_machine_.current_state);
-
     if (enable_communication_interface) {
       { // change_state
         auto cb = std::bind(
@@ -217,30 +206,28 @@ public:
     std::shared_ptr<ChangeStateSrv::Response> resp)
   {
     (void)header;
-    std::uint8_t transition_id;
-    {
-      std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
-      if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
-        throw std::runtime_error("Can't get state. State machine is not initialized.");
-      }
-      transition_id = req->transition.id;
-      // if there's a label attached to the request,
-      // we check the transition attached to this label.
-      // we further can't compare the id of the looked up transition
-      // because ros2 service call defaults all intergers to zero.
-      // that means if we call ros2 service call ... {transition: {label: shutdown}}
-      // the id of the request is 0 (zero) whereas the id from the lookup up transition
-      // can be different.
-      // the result of this is that the label takes presedence of the id.
-      if (req->transition.label.size() != 0) {
-        auto rcl_transition = rcl_lifecycle_get_transition_by_label(
-          state_machine_.current_state, req->transition.label.c_str());
-        if (rcl_transition == nullptr) {
-          resp->success = false;
-          return;
-        }
-        transition_id = static_cast<std::uint8_t>(rcl_transition->id);
+    if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
+      throw std::runtime_error(
+              "Can't get state. State machine is not initialized.");
+    }
+
+    auto transition_id = req->transition.id;
+    // if there's a label attached to the request,
+    // we check the transition attached to this label.
+    // we further can't compare the id of the looked up transition
+    // because ros2 service call defaults all intergers to zero.
+    // that means if we call ros2 service call ... {transition: {label: shutdown}}
+    // the id of the request is 0 (zero) whereas the id from the lookup up transition
+    // can be different.
+    // the result of this is that the label takes presedence of the id.
+    if (req->transition.label.size() != 0) {
+      auto rcl_transition = rcl_lifecycle_get_transition_by_label(
+        state_machine_.current_state, req->transition.label.c_str());
+      if (rcl_transition == nullptr) {
+        resp->success = false;
+        return;
       }
+      transition_id = static_cast<std::uint8_t>(rcl_transition->id);
     }
 
     node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code;
@@ -261,7 +248,6 @@ public:
   {
     (void)header;
     (void)req;
-    std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
     if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
       throw std::runtime_error(
               "Can't get state. State machine is not initialized.");
@@ -278,7 +264,6 @@ public:
   {
     (void)header;
     (void)req;
-    std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
     if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
       throw std::runtime_error(
               "Can't get available states. State machine is not initialized.");
@@ -301,7 +286,6 @@ public:
   {
     (void)header;
     (void)req;
-    std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
     if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
       throw std::runtime_error(
               "Can't get available transitions. State machine is not initialized.");
@@ -329,7 +313,6 @@ public:
   {
     (void)header;
     (void)req;
-    std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
     if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
       throw std::runtime_error(
               "Can't get available transitions. State machine is not initialized.");
@@ -360,7 +343,6 @@ public:
   get_available_states()
   {
     std::vector<State> states;
-    std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
     states.reserve(state_machine_.transition_map.states_size);
 
     for (unsigned int i = 0; i < state_machine_.transition_map.states_size; ++i) {
@@ -373,7 +355,6 @@ public:
   get_available_transitions()
   {
     std::vector<Transition> transitions;
-    std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
     transitions.reserve(state_machine_.current_state->valid_transition_size);
 
     for (unsigned int i = 0; i < state_machine_.current_state->valid_transition_size; ++i) {
@@ -386,7 +367,6 @@ public:
   get_transition_graph()
   {
     std::vector<Transition> transitions;
-    std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
     transitions.reserve(state_machine_.transition_map.transitions_size);
 
     for (unsigned int i = 0; i < state_machine_.transition_map.transitions_size; ++i) {
@@ -398,34 +378,26 @@ public:
   rcl_ret_t
   change_state(std::uint8_t transition_id, LifecycleNodeInterface::CallbackReturn & cb_return_code)
   {
+    if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR(
+        "Unable to change state for state machine for %s: %s",
+        node_base_interface_->get_name(), rcl_get_error_string().str);
+      return RCL_RET_ERROR;
+    }
+
     constexpr bool publish_update = true;
-    State initial_state;
-    unsigned int current_state_id;
+    // keep the initial state to pass to a transition callback
+    State initial_state(state_machine_.current_state);
 
+    if (
+      rcl_lifecycle_trigger_transition_by_id(
+        &state_machine_, transition_id, publish_update) != RCL_RET_OK)
     {
-      std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
-      if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
-        RCLCPP_ERROR(
-          node_logging_interface_->get_logger(),
-          "Unable to change state for state machine for %s: %s",
-          node_base_interface_->get_name(), rcl_get_error_string().str);
-        return RCL_RET_ERROR;
-      }
-      // keep the initial state to pass to a transition callback
-      initial_state = State(state_machine_.current_state);
-
-      if (
-        rcl_lifecycle_trigger_transition_by_id(
-          &state_machine_, transition_id, publish_update) != RCL_RET_OK)
-      {
-        RCLCPP_ERROR(
-          node_logging_interface_->get_logger(),
-          "Unable to start transition %u from current state %s: %s",
-          transition_id, state_machine_.current_state->label, rcl_get_error_string().str);
-        rcutils_reset_error();
-        return RCL_RET_ERROR;
-      }
-      current_state_id = state_machine_.current_state->id;
+      RCUTILS_LOG_ERROR(
+        "Unable to start transition %u from current state %s: %s",
+        transition_id, state_machine_.current_state->label, rcl_get_error_string().str);
+      rcutils_reset_error();
+      return RCL_RET_ERROR;
     }
 
     auto get_label_for_return_code =
@@ -439,42 +411,32 @@ public:
         return rcl_lifecycle_transition_error_label;
       };
 
-    cb_return_code = execute_callback(current_state_id, initial_state);
+    cb_return_code = execute_callback(state_machine_.current_state->id, initial_state);
     auto transition_label = get_label_for_return_code(cb_return_code);
 
+    if (
+      rcl_lifecycle_trigger_transition_by_label(
+        &state_machine_, transition_label, publish_update) != RCL_RET_OK)
     {
-      std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
-      if (
-        rcl_lifecycle_trigger_transition_by_label(
-          &state_machine_, transition_label, publish_update) != RCL_RET_OK)
-      {
-        RCLCPP_ERROR(
-          node_logging_interface_->get_logger(),
-          "Failed to finish transition %u. Current state is now: %s (%s)",
-          transition_id, state_machine_.current_state->label, rcl_get_error_string().str);
-        rcutils_reset_error();
-        return RCL_RET_ERROR;
-      }
-      current_state_id = state_machine_.current_state->id;
+      RCUTILS_LOG_ERROR(
+        "Failed to finish transition %u. Current state is now: %s (%s)",
+        transition_id, state_machine_.current_state->label, rcl_get_error_string().str);
+      rcutils_reset_error();
+      return RCL_RET_ERROR;
     }
 
     // error handling ?!
     // TODO(karsten1987): iterate over possible ret value
     if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) {
-      RCLCPP_WARN(
-        node_logging_interface_->get_logger(),
-        "Error occurred while doing error handling.");
+      RCUTILS_LOG_WARN("Error occurred while doing error handling.");
 
-      auto error_cb_code = execute_callback(current_state_id, initial_state);
+      auto error_cb_code = execute_callback(state_machine_.current_state->id, initial_state);
       auto error_cb_label = get_label_for_return_code(error_cb_code);
-      std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
       if (
         rcl_lifecycle_trigger_transition_by_label(
           &state_machine_, error_cb_label, publish_update) != RCL_RET_OK)
       {
-        RCLCPP_ERROR(
-          node_logging_interface_->get_logger(),
-          "Failed to call cleanup on error state: %s", rcl_get_error_string().str);
+        RCUTILS_LOG_ERROR("Failed to call cleanup on error state: %s", rcl_get_error_string().str);
         rcutils_reset_error();
         return RCL_RET_ERROR;
       }
@@ -497,12 +459,8 @@ public:
       try {
         cb_success = callback(State(previous_state));
       } catch (const std::exception & e) {
-        RCLCPP_ERROR(
-          node_logging_interface_->get_logger(),
-          "Caught exception in callback for transition %d", it->first);
-        RCLCPP_ERROR(
-          node_logging_interface_->get_logger(),
-          "Original error: %s", e.what());
+        RCUTILS_LOG_ERROR("Caught exception in callback for transition %d", it->first);
+        RCUTILS_LOG_ERROR("Original error: %s", e.what());
         cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
       }
     }
@@ -518,13 +476,8 @@ public:
   const State & trigger_transition(
     const char * transition_label, LifecycleNodeInterface::CallbackReturn & cb_return_code)
   {
-    const rcl_lifecycle_transition_t * transition;
-    {
-      std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
-
-      transition =
-        rcl_lifecycle_get_transition_by_label(state_machine_.current_state, transition_label);
-    }
+    auto transition =
+      rcl_lifecycle_get_transition_by_label(state_machine_.current_state, transition_label);
     if (transition) {
       change_state(static_cast<uint8_t>(transition->id), cb_return_code);
     }
@@ -581,7 +534,6 @@ public:
     }
   }
 
-  mutable std::recursive_mutex state_machine_mutex_;
   rcl_lifecycle_state_machine_t state_machine_;
   State current_state_;
   std::map<
@@ -590,7 +542,6 @@ public:
 
   using NodeBasePtr = std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>;
   using NodeServicesPtr = std::shared_ptr<rclcpp::node_interfaces::NodeServicesInterface>;
-  using NodeLoggingPtr = std::shared_ptr<rclcpp::node_interfaces::NodeLoggingInterface>;
   using ChangeStateSrvPtr = std::shared_ptr<rclcpp::Service<ChangeStateSrv>>;
   using GetStateSrvPtr = std::shared_ptr<rclcpp::Service<GetStateSrv>>;
   using GetAvailableStatesSrvPtr =
@@ -602,7 +553,6 @@ public:
 
   NodeBasePtr node_base_interface_;
   NodeServicesPtr node_services_interface_;
-  NodeLoggingPtr node_logging_interface_;
   ChangeStateSrvPtr srv_change_state_;
   GetStateSrvPtr srv_get_state_;
   GetAvailableStatesSrvPtr srv_get_available_states_;

rclcpp_lifecycle/src/mutex_map.cpp

@@ -1,43 +0,0 @@
-// Copyright 2023 PickNik, 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 "mutex_map.hpp"
-
-#include <memory>
-#include <shared_mutex>
-#include <thread>
-
-namespace rclcpp_lifecycle
-{
-void MutexMap::add(const State * key)
-{
-  // Adding a new mutex to the map requires exclusive access
-  std::unique_lock lock(map_access_mutex_);
-  mutex_map_.emplace(key, std::make_unique<std::recursive_mutex>());
-}
-
-std::recursive_mutex & MutexMap::getMutex(const State * key) const
-{
-  // Multiple threads can retrieve mutexes from the map at the same time
-  std::shared_lock lock(map_access_mutex_);
-  return *(mutex_map_.at(key));
-}
-
-void MutexMap::remove(const State * key)
-{
-  // Removing a mutex from the map requires exclusive access
-  std::unique_lock lock(map_access_mutex_);
-  mutex_map_.erase(key);
-}
-}  // namespace rclcpp_lifecycle

rclcpp_lifecycle/src/mutex_map.hpp

@@ -1,65 +0,0 @@
-// Copyright 2023 PickNik, 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 MUTEX_MAP_HPP_
-#define MUTEX_MAP_HPP_
-
-#include <map>
-#include <memory>
-#include <mutex>
-#include <shared_mutex>
-
-#include "rclcpp_lifecycle/state.hpp"
-
-namespace rclcpp_lifecycle
-{
-/// @brief Associates instances of recursive_mutex with instances of State.
-class MutexMap
-{
-public:
-  MutexMap() = default;
-
-  /// \brief Add a new mutex for an instance of State.
-  /**
-   * \param[in] key Raw pointer to the instance of State which will use the mutex.
-   */
-  void add(const State * key);
-
-  /// \brief Retrieve the mutex for an instance of State.
-  /**
-   * \param key Raw pointer to an instance of State.
-   * \return A reference to the mutex associated with the key.
-   */
-  std::recursive_mutex & getMutex(const State * key) const;
-
-  /// \brief Remove the mutex for an instance of State.
-  /**
-   * \param key Raw pointer to an instance of State.
-   */
-  void remove(const State * key);
-
-private:
-  /// \brief Map that stores the mutexes
-  /**
-   * \details The mutexes are emplaced as unique_ptrs because mutexes are
-   * not copyable or movable.
-   */
-  std::map<const State *, std::unique_ptr<std::recursive_mutex>> mutex_map_;
-
-  /// @brief Controls access to mutex_map_.
-  mutable std::shared_mutex map_access_mutex_;
-};
-}  // namespace rclcpp_lifecycle
-
-#endif  // MUTEX_MAP_HPP_

rclcpp_lifecycle/src/state.cpp

@@ -25,17 +25,12 @@
 
 #include "rcutils/allocator.h"
 
-#include "mutex_map.hpp"
-
 namespace rclcpp_lifecycle
 {
-MutexMap State::state_handle_mutex_map_;
 
 State::State(rcutils_allocator_t allocator)
 : State(lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, "unknown", allocator)
-{
-  state_handle_mutex_map_.add(this);
-}
+{}
 
 State::State(
   uint8_t id,
@@ -45,8 +40,6 @@ State::State(
   owns_rcl_state_handle_(true),
   state_handle_(nullptr)
 {
-  state_handle_mutex_map_.add(this);
-
   if (label.empty()) {
     throw std::runtime_error("Lifecycle State cannot have an empty label.");
   }
@@ -74,8 +67,6 @@ State::State(
   owns_rcl_state_handle_(false),
   state_handle_(nullptr)
 {
-  state_handle_mutex_map_.add(this);
-
   if (!rcl_lifecycle_state_handle) {
     throw std::runtime_error("rcl_lifecycle_state_handle is null");
   }
@@ -87,15 +78,12 @@ State::State(const State & rhs)
   owns_rcl_state_handle_(false),
   state_handle_(nullptr)
 {
-  state_handle_mutex_map_.add(this);
-
   *this = rhs;
 }
 
 State::~State()
 {
   reset();
-  state_handle_mutex_map_.remove(this);
 }
 
 State &
@@ -105,8 +93,6 @@ State::operator=(const State & rhs)
     return *this;
   }
 
-  const auto lock = std::lock_guard<std::recursive_mutex>(state_handle_mutex_map_.getMutex(this));
-
   // reset all currently used resources
   reset();
 
@@ -142,7 +128,6 @@ State::operator=(const State & rhs)
 uint8_t
 State::id() const
 {
-  const auto lock = std::lock_guard<std::recursive_mutex>(state_handle_mutex_map_.getMutex(this));
   if (!state_handle_) {
     throw std::runtime_error("Error in state! Internal state_handle is NULL.");
   }
@@ -152,7 +137,6 @@ State::id() const
 std::string
 State::label() const
 {
-  const auto lock = std::lock_guard<std::recursive_mutex>(state_handle_mutex_map_.getMutex(this));
   if (!state_handle_) {
     throw std::runtime_error("Error in state! Internal state_handle is NULL.");
   }
@@ -162,8 +146,6 @@ State::label() const
 void
 State::reset() noexcept
 {
-  const auto lock = std::lock_guard<std::recursive_mutex>(state_handle_mutex_map_.getMutex(this));
-
   if (!owns_rcl_state_handle_) {
     state_handle_ = nullptr;
   }

rclcpp_lifecycle/test/test_lifecycle_node.cpp

@@ -377,28 +377,6 @@ TEST_F(TestDefaultStateMachine, call_transitions_without_code) {
   EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED);
 }
 
-TEST_F(TestDefaultStateMachine, get_current_state_thread_safety) {
-  auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
-  test_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
-
-  const auto check_state_fn = [](std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node)
-    {
-      std::size_t count = 0;
-      while (count < 100000) {
-        node->get_current_state().id();
-        count++;
-      }
-    };
-
-  // Call get_current_state() on the same node repeatedly from two different threads.
-  std::thread thread_object_1(check_state_fn, test_node);
-  std::thread thread_object_2(check_state_fn, test_node);
-
-  // Test has succeeded if both threads finish without exceptions.
-  thread_object_1.join();
-  thread_object_2.join();
-}
-
 TEST_F(TestDefaultStateMachine, good_mood) {
   auto test_node = std::make_shared<MoodyLifecycleNode<GoodMood>>("testnode");