16.0.9

Signed-off-by: Audrow Nash <audrow@intrinsic.ai>

rclcpp/CHANGELOG.rst

@@ -2,6 +2,47 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+16.0.9 (2024-05-15)
+-------------------
+* Fix logging macros to build with msvc and cpp20 (`#2063 <https://github.com/ros2/rclcpp/issues/2063>`_) (`#2529 <https://github.com/ros2/rclcpp/issues/2529>`_)
+  (cherry picked from commit 86335dd4acd91d5dd973c4e4e97014e5e8a916bc)
+  Co-authored-by: Mateusz Szczygielski <112629916+msz-rai@users.noreply.github.com>
+* address ambiguous auto variable. (`#2481 <https://github.com/ros2/rclcpp/issues/2481>`_) (`#2485 <https://github.com/ros2/rclcpp/issues/2485>`_)
+  (cherry picked from commit 3cdb25934ed261c78bdfbcf5ec9f06e0573be81e)
+  Co-authored-by: Tomoya Fujita <Tomoya.Fujita@sony.com>
+* Fix clang warning: bugprone-use-after-move (`#2116 <https://github.com/ros2/rclcpp/issues/2116>`_) (`#2459 <https://github.com/ros2/rclcpp/issues/2459>`_)
+  Co-authored-by: mauropasse <mauropasse@hotmail.com>
+  Co-authored-by: Mauro Passerino <mpasserino@irobot.com>
+* Contributors: Tamaki Nishino, mergify[bot]
+
+16.0.8 (2024-01-24)
+-------------------
+* Add missing stdexcept include (`#2186 <https://github.com/ros2/rclcpp/issues/2186>`_) (`#2394 <https://github.com/ros2/rclcpp/issues/2394>`_)
+* Contributors: gentoo90
+
+16.0.7 (2023-11-13)
+-------------------
+* 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]
+
+16.0.6 (2023-09-19)
+-------------------
+* 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>`_)

rclcpp/include/rclcpp/client.hpp

@@ -816,7 +816,7 @@ protected:
         "Received invalid sequence number. Ignoring...");
       return std::nullopt;
     }
-    auto value = std::move(it->second.second);
+    std::optional<CallbackInfoVariant> value = std::move(it->second.second);
     this->pending_requests_.erase(request_number);
     return value;
   }

rclcpp/include/rclcpp/clock.hpp

@@ -137,6 +137,51 @@ 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/context.hpp

@@ -26,6 +26,7 @@
 #include <unordered_set>
 #include <utility>
 #include <vector>
+#include <stdexcept>
 
 #include "rcl/context.h"
 #include "rcl/guard_condition.h"

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

@@ -454,6 +454,8 @@ private:
         if (std::next(it) == subscription_ids.end()) {
           // If this is the last subscription, give up ownership
           subscription->provide_intra_process_data(std::move(message));
+          // Last message delivered, break from for loop
+          break;
         } else {
           // Copy the message since we have additional subscriptions to serve
           Deleter deleter = message.get_deleter();
@@ -479,13 +481,13 @@ private:
                 "subscription use different allocator types, which is not supported");
       }
 
-      if constexpr (rclcpp::TypeAdapter<MessageT>::is_specialized::value) {
+      if constexpr (rclcpp::TypeAdapter<MessageT, ROSMessageType>::is_specialized::value) {
         ROSMessageTypeAllocator ros_message_alloc(allocator);
-        auto ptr = ros_message_alloc.allocate(1);
-        ros_message_alloc.construct(ptr);
+        auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_alloc, 1);
+        ROSMessageTypeAllocatorTraits::construct(ros_message_alloc, ptr);
         ROSMessageTypeDeleter deleter;
         allocator::set_allocator_for_deleter(&deleter, &allocator);
-        rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*message, *ptr);
+        rclcpp::TypeAdapter<MessageT, ROSMessageType>::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 {
@@ -493,6 +495,8 @@ private:
           if (std::next(it) == subscription_ids.end()) {
             // If this is the last subscription, give up ownership
             ros_message_subscription->provide_intra_process_message(std::move(message));
+            // Last message delivered, break from for loop
+            break;
           } else {
             // Copy the message since we have additional subscriptions to serve
             Deleter deleter = message.get_deleter();

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -118,6 +118,13 @@ public:
         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>(

rclcpp/include/rclcpp/node_options.hpp

@@ -42,6 +42,7 @@ 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/service.hpp

@@ -481,6 +481,14 @@ 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/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.4</version>
+  <version>16.0.9</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/resource/logging.hpp.em

@@ -125,7 +125,7 @@ def get_rclcpp_suffix_from_features(features):
 ) \
   do { \
     static_assert( \
-      ::std::is_same<typename std::remove_cv<typename std::remove_reference<decltype(logger)>::type>::type, \
+      ::std::is_same<typename std::remove_cv_t<typename std::remove_reference_t<decltype(logger)>>, \
       typename ::rclcpp::Logger>::value, \
       "First argument to logging macros must be an rclcpp::Logger"); \
 @[ if 'throttle' in feature_combination]@ \

rclcpp/src/rclcpp/clock.cpp

@@ -182,6 +182,71 @@ 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

@@ -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, nanoseconds);
+      interrupt_condition_variable_.wait_for(lock, time_left);
       time_left -= std::chrono::steady_clock::now() - start;
     }
   } while (time_left > std::chrono::nanoseconds::zero() && this->is_valid());

rclcpp/src/rclcpp/executor.cpp

@@ -646,6 +646,11 @@ 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);
   }
 }

rclcpp/src/rclcpp/logging_mutex.cpp

@@ -14,6 +14,7 @@
 
 #include <memory>
 #include <mutex>
+#include <stdexcept>
 
 #include "rcutils/macros.h"
 

rclcpp/src/rclcpp/signal_handler.cpp

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

rclcpp/src/rclcpp/subscription_base.cpp

@@ -229,7 +229,20 @@ SubscriptionBase::setup_intra_process(
 bool
 SubscriptionBase::can_loan_messages() const
 {
-  return rcl_subscription_can_loan_messages(subscription_handle_.get());
+  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;
 }
 
 rclcpp::Waitable::SharedPtr

rclcpp/test/rclcpp/executors/test_executors.cpp

@@ -593,3 +593,106 @@ 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/test_publisher_with_type_adapter.cpp

@@ -152,33 +152,54 @@ 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_F(
-  TestPublisher,
+TEST_P(
+  TestPublisherFixture,
   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);
-  auto sub = node->create_subscription<rclcpp::msg::String>(topic_name, 1, callback);
+  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 wait_for_message_to_be_received = [&is_received, &node]() {
       rclcpp::executors::SingleThreadedExecutor executor;
@@ -239,6 +260,14 @@ TEST_F(
   }
 }
 
+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_rate.cpp

@@ -18,10 +18,24 @@
 
 #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(TestRate, rate_basics) {
+TEST_F(TestRate, rate_basics) {
   auto period = std::chrono::milliseconds(1000);
   auto offset = std::chrono::milliseconds(500);
   auto epsilon = std::chrono::milliseconds(100);
@@ -61,7 +75,7 @@ TEST(TestRate, rate_basics) {
   ASSERT_TRUE(epsilon > delta);
 }
 
-TEST(TestRate, wall_rate_basics) {
+TEST_F(TestRate, wall_rate_basics) {
   auto period = std::chrono::milliseconds(100);
   auto offset = std::chrono::milliseconds(50);
   auto epsilon = std::chrono::milliseconds(1);
@@ -101,7 +115,7 @@ TEST(TestRate, wall_rate_basics) {
   EXPECT_GT(epsilon, delta);
 }
 
-TEST(TestRate, from_double) {
+TEST_F(TestRate, from_double) {
   {
     rclcpp::WallRate rate(1.0);
     EXPECT_EQ(std::chrono::seconds(1), rate.period());

rclcpp/test/rclcpp/test_time.cpp

@@ -19,6 +19,7 @@
 #include <limits>
 #include <memory>
 #include <string>
+#include <thread>
 
 #include "rcl/error_handling.h"
 #include "rcl/time.h"
@@ -848,3 +849,72 @@ 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_action/CHANGELOG.rst

@@ -3,6 +3,23 @@ Changelog for package rclcpp_action
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+16.0.9 (2024-05-15)
+-------------------
+* Do not generate the exception when action service response timeout. (`#2464 <https://github.com/ros2/rclcpp/issues/2464>`_) (`#2518 <https://github.com/ros2/rclcpp/issues/2518>`_)
+* Contributors: mergify[bot]
+
+16.0.8 (2024-01-24)
+-------------------
+
+16.0.7 (2023-11-13)
+-------------------
+
+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>`_)

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.4</version>
+  <version>16.0.9</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/server.cpp

@@ -345,7 +345,16 @@ ServerBase::execute_goal_request_received(std::shared_ptr<void> & data)
   }
 
   if (RCL_RET_OK != ret) {
-    rclcpp::exceptions::throw_from_rcl_error(ret);
+    if (ret == RCL_RET_TIMEOUT) {
+      RCLCPP_WARN(
+        pimpl_->logger_,
+        "Failed to send goal response %s (timeout): %s",
+        to_string(uuid).c_str(), rcl_get_error_string().str);
+      rcl_reset_error();
+      return;
+    } else {
+      rclcpp::exceptions::throw_from_rcl_error(ret);
+    }
   }
 
   const auto status = response_pair.first;
@@ -484,6 +493,15 @@ ServerBase::execute_cancel_request_received(std::shared_ptr<void> & data)
       pimpl_->action_server_.get(), &request_header, response.get());
   }
 
+  if (ret == RCL_RET_TIMEOUT) {
+    GoalUUID uuid = request->goal_info.goal_id.uuid;
+    RCLCPP_WARN(
+      pimpl_->logger_,
+      "Failed to send cancel response %s (timeout): %s",
+      to_string(uuid).c_str(), rcl_get_error_string().str);
+    rcl_reset_error();
+    return;
+  }
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
@@ -539,6 +557,14 @@ ServerBase::execute_result_request_received(std::shared_ptr<void> & data)
     std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
     rcl_ret_t rcl_ret = rcl_action_send_result_response(
       pimpl_->action_server_.get(), &request_header, result_response.get());
+    if (rcl_ret == RCL_RET_TIMEOUT) {
+      RCLCPP_WARN(
+        pimpl_->logger_,
+        "Failed to send result response %s (timeout): %s",
+        to_string(uuid).c_str(), rcl_get_error_string().str);
+      rcl_reset_error();
+      return;
+    }
     if (RCL_RET_OK != rcl_ret) {
       rclcpp::exceptions::throw_from_rcl_error(rcl_ret);
     }
@@ -672,7 +698,13 @@ ServerBase::publish_result(const GoalUUID & uuid, std::shared_ptr<void> result_m
       for (auto & request_header : iter->second) {
         rcl_ret_t ret = rcl_action_send_result_response(
           pimpl_->action_server_.get(), &request_header, result_msg.get());
-        if (RCL_RET_OK != ret) {
+        if (ret == RCL_RET_TIMEOUT) {
+          RCLCPP_WARN(
+            pimpl_->logger_,
+            "Failed to send result response %s (timeout): %s",
+            to_string(uuid).c_str(), rcl_get_error_string().str);
+          rcl_reset_error();
+        } else if (RCL_RET_OK != ret) {
           rclcpp::exceptions::throw_from_rcl_error(ret);
         }
       }

rclcpp_components/CHANGELOG.rst

@@ -2,6 +2,24 @@
 Changelog for package rclcpp_components
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+16.0.9 (2024-05-15)
+-------------------
+
+16.0.8 (2024-01-24)
+-------------------
+* Add missing header required by the rclcpp::NodeOptions type (`#2324 <https://github.com/ros2/rclcpp/issues/2324>`_) (`#2407 <https://github.com/ros2/rclcpp/issues/2407>`_)
+* fix(rclcpp_components): increase the service queue sizes in component_container (backport `#2363 <https://github.com/ros2/rclcpp/issues/2363>`_) (`#2380 <https://github.com/ros2/rclcpp/issues/2380>`_)
+* Contributors: mergify[bot]
+
+16.0.7 (2023-11-13)
+-------------------
+
+16.0.6 (2023-09-19)
+-------------------
+
+16.0.5 (2023-07-17)
+-------------------
+
 16.0.4 (2023-04-25)
 -------------------
 

rclcpp_components/include/rclcpp_components/node_factory.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP_COMPONENTS__NODE_FACTORY_HPP__
 #define RCLCPP_COMPONENTS__NODE_FACTORY_HPP__
 
+#include "rclcpp/node_options.hpp"
 #include "rclcpp_components/node_instance_wrapper.hpp"
 
 namespace rclcpp_components

rclcpp_components/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp_components</name>
-  <version>16.0.4</version>
+  <version>16.0.9</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_components/src/component_manager.cpp

@@ -37,12 +37,16 @@ ComponentManager::ComponentManager(
 : Node(std::move(node_name), node_options),
   executor_(executor)
 {
+  rmw_qos_profile_t service_qos = rmw_qos_profile_services_default;
+  service_qos.depth = 200;
   loadNode_srv_ = create_service<LoadNode>(
     "~/_container/load_node",
-    std::bind(&ComponentManager::on_load_node, this, _1, _2, _3));
+    std::bind(&ComponentManager::on_load_node, this, _1, _2, _3),
+    service_qos);
   unloadNode_srv_ = create_service<UnloadNode>(
     "~/_container/unload_node",
-    std::bind(&ComponentManager::on_unload_node, this, _1, _2, _3));
+    std::bind(&ComponentManager::on_unload_node, this, _1, _2, _3),
+    service_qos);
   listNodes_srv_ = create_service<ListNodes>(
     "~/_container/list_nodes",
     std::bind(&ComponentManager::on_list_nodes, this, _1, _2, _3));

rclcpp_lifecycle/CHANGELOG.rst

@@ -3,6 +3,31 @@ Changelog for package rclcpp_lifecycle
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+16.0.9 (2024-05-15)
+-------------------
+* call shutdown in LifecycleNode dtor to avoid leaving the device in un… (`#2450 <https://github.com/ros2/rclcpp/issues/2450>`_) (`#2491 <https://github.com/ros2/rclcpp/issues/2491>`_)
+* Contributors: mergify[bot]
+
+16.0.8 (2024-01-24)
+-------------------
+
+16.0.7 (2023-11-13)
+-------------------
+
+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)
 -------------------
 

rclcpp_lifecycle/CMakeLists.txt

@@ -20,6 +20,8 @@ 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,6 +24,8 @@
 
 namespace rclcpp_lifecycle
 {
+/// Forward declaration of mutex helper class
+class MutexMap;
 
 /// Abstract class for the Lifecycle's states.
 /**
@@ -92,6 +94,21 @@ 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.4</version>
+  <version>16.0.9</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_))
+  impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_, node_logging_))
 {
   impl_->init(enable_communication_interface);
 
@@ -133,6 +133,22 @@ LifecycleNode::LifecycleNode(
 
 LifecycleNode::~LifecycleNode()
 {
+  // shutdown if necessary to avoid leaving the device in unknown state
+  if (LifecycleNode::get_current_state().id() !=
+    lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
+  {
+    auto ret = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
+    auto finalized = LifecycleNode::shutdown(ret);
+    if (finalized.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED ||
+      ret != rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS)
+    {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp_lifecycle"),
+        "Shutdown error in destruction of LifecycleNode: final state(%s)",
+        finalized.label().c_str());
+    }
+  }
+
   // release sub-interfaces in an order that allows them to consult with node_base during tear-down
   node_waitables_.reset();
   node_time_source_.reset();

rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp

@@ -20,6 +20,7 @@
 #include <functional>
 #include <map>
 #include <memory>
+#include <mutex>
 #include <string>
 #include <vector>
 #include <utility>
@@ -38,6 +39,7 @@
 #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"
@@ -56,18 +58,24 @@ 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::NodeServicesInterface> node_services_interface,
+    std::shared_ptr<rclcpp::node_interfaces::NodeLoggingInterface> node_logging_interface)
   : node_base_interface_(node_base_interface),
-    node_services_interface_(node_services_interface)
+    node_services_interface_(node_services_interface),
+    node_logging_interface_(node_logging_interface)
   {}
 
   ~LifecycleNodeInterfaceImpl()
   {
     rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle();
-    auto ret = rcl_lifecycle_state_machine_fini(&state_machine_, 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);
+    }
     if (ret != RCL_RET_OK) {
-      RCUTILS_LOG_FATAL_NAMED(
-        "rclcpp_lifecycle",
+      RCLCPP_FATAL(
+        node_logging_interface_->get_logger(),
         "failed to destroy rcl_state_machine");
     }
   }
@@ -78,7 +86,6 @@ 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;
@@ -89,6 +96,8 @@ 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,
@@ -105,6 +114,8 @@ public:
               node_base_interface_->get_name());
     }
 
+    current_state_ = State(state_machine_.current_state);
+
     if (enable_communication_interface) {
       { // change_state
         auto cb = std::bind(
@@ -206,28 +217,30 @@ public:
     std::shared_ptr<ChangeStateSrv::Response> resp)
   {
     (void)header;
-    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;
+    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);
       }
-      transition_id = static_cast<std::uint8_t>(rcl_transition->id);
     }
 
     node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code;
@@ -248,6 +261,7 @@ 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.");
@@ -264,6 +278,7 @@ 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.");
@@ -286,6 +301,7 @@ 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.");
@@ -313,6 +329,7 @@ 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.");
@@ -343,6 +360,7 @@ 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) {
@@ -355,6 +373,7 @@ 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) {
@@ -367,6 +386,7 @@ 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) {
@@ -378,26 +398,34 @@ 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;
-    // keep the initial state to pass to a transition callback
-    State initial_state(state_machine_.current_state);
+    State initial_state;
+    unsigned int current_state_id;
 
-    if (
-      rcl_lifecycle_trigger_transition_by_id(
-        &state_machine_, transition_id, publish_update) != RCL_RET_OK)
     {
-      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;
+      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;
     }
 
     auto get_label_for_return_code =
@@ -411,32 +439,42 @@ public:
         return rcl_lifecycle_transition_error_label;
       };
 
-    cb_return_code = execute_callback(state_machine_.current_state->id, initial_state);
+    cb_return_code = execute_callback(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)
     {
-      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;
+      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;
     }
 
     // error handling ?!
     // TODO(karsten1987): iterate over possible ret value
     if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) {
-      RCUTILS_LOG_WARN("Error occurred while doing error handling.");
+      RCLCPP_WARN(
+        node_logging_interface_->get_logger(),
+        "Error occurred while doing error handling.");
 
-      auto error_cb_code = execute_callback(state_machine_.current_state->id, initial_state);
+      auto error_cb_code = execute_callback(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)
       {
-        RCUTILS_LOG_ERROR("Failed to call cleanup on error state: %s", rcl_get_error_string().str);
+        RCLCPP_ERROR(
+          node_logging_interface_->get_logger(),
+          "Failed to call cleanup on error state: %s", rcl_get_error_string().str);
         rcutils_reset_error();
         return RCL_RET_ERROR;
       }
@@ -459,8 +497,12 @@ public:
       try {
         cb_success = callback(State(previous_state));
       } catch (const std::exception & e) {
-        RCUTILS_LOG_ERROR("Caught exception in callback for transition %d", it->first);
-        RCUTILS_LOG_ERROR("Original error: %s", e.what());
+        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());
         cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
       }
     }
@@ -476,8 +518,13 @@ public:
   const State & trigger_transition(
     const char * transition_label, LifecycleNodeInterface::CallbackReturn & cb_return_code)
   {
-    auto transition =
-      rcl_lifecycle_get_transition_by_label(state_machine_.current_state, transition_label);
+    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);
+    }
     if (transition) {
       change_state(static_cast<uint8_t>(transition->id), cb_return_code);
     }
@@ -534,6 +581,7 @@ public:
     }
   }
 
+  mutable std::recursive_mutex state_machine_mutex_;
   rcl_lifecycle_state_machine_t state_machine_;
   State current_state_;
   std::map<
@@ -542,6 +590,7 @@ 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 =
@@ -553,6 +602,7 @@ 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

@@ -0,0 +1,43 @@
+// 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

@@ -0,0 +1,65 @@
+// 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,12 +25,17 @@
 
 #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,
@@ -40,6 +45,8 @@ 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.");
   }
@@ -67,6 +74,8 @@ 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");
   }
@@ -78,12 +87,15 @@ 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 &
@@ -93,6 +105,8 @@ 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();
 
@@ -128,6 +142,7 @@ 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.");
   }
@@ -137,6 +152,7 @@ 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.");
   }
@@ -146,6 +162,8 @@ 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,6 +377,28 @@ 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");
 
@@ -413,6 +435,146 @@ TEST_F(TestDefaultStateMachine, bad_mood) {
   EXPECT_EQ(1u, test_node->number_of_callbacks);
 }
 
+
+TEST_F(TestDefaultStateMachine, shutdown_from_each_primary_state) {
+  auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+  auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
+
+  // PRIMARY_STATE_UNCONFIGURED to shutdown
+  {
+    auto ret = reset_key;
+    auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
+    auto finalized = test_node->shutdown(ret);
+    EXPECT_EQ(success, ret);
+    EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED);
+  }
+
+  // PRIMARY_STATE_INACTIVE to shutdown
+  {
+    auto ret = reset_key;
+    auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
+    auto configured = test_node->configure(ret);
+    EXPECT_EQ(success, ret);
+    EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE);
+    ret = reset_key;
+    auto finalized = test_node->shutdown(ret);
+    EXPECT_EQ(success, ret);
+    EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED);
+  }
+
+  // PRIMARY_STATE_ACTIVE to shutdown
+  {
+    auto ret = reset_key;
+    auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
+    auto configured = test_node->configure(ret);
+    EXPECT_EQ(success, ret);
+    EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE);
+    ret = reset_key;
+    auto activated = test_node->activate(ret);
+    EXPECT_EQ(success, ret);
+    EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE);
+    ret = reset_key;
+    auto finalized = test_node->shutdown(ret);
+    EXPECT_EQ(success, ret);
+    EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED);
+  }
+
+  // PRIMARY_STATE_FINALIZED to shutdown
+  {
+    auto ret = reset_key;
+    auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
+    auto configured = test_node->configure(ret);
+    EXPECT_EQ(success, ret);
+    EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE);
+    ret = reset_key;
+    auto activated = test_node->activate(ret);
+    EXPECT_EQ(success, ret);
+    EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE);
+    ret = reset_key;
+    auto finalized = test_node->shutdown(ret);
+    EXPECT_EQ(success, ret);
+    EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED);
+    ret = reset_key;
+    auto finalized_again = test_node->shutdown(ret);
+    EXPECT_EQ(reset_key, ret);
+    EXPECT_EQ(finalized_again.id(), State::PRIMARY_STATE_FINALIZED);
+  }
+}
+
+TEST_F(TestDefaultStateMachine, test_shutdown_on_dtor) {
+  auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+  auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
+
+  bool shutdown_cb_called = false;
+  auto on_shutdown_callback =
+    [&shutdown_cb_called](const rclcpp_lifecycle::State &) ->
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn {
+      shutdown_cb_called = true;
+      return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+    };
+
+  // PRIMARY_STATE_UNCONFIGURED to shutdown via dtor
+  shutdown_cb_called = false;
+  {
+    auto test_node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testnode");
+    test_node->register_on_shutdown(std::bind(on_shutdown_callback, std::placeholders::_1));
+    EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id());
+    EXPECT_FALSE(shutdown_cb_called);
+  }
+  EXPECT_TRUE(shutdown_cb_called);
+
+  // PRIMARY_STATE_INACTIVE to shutdown via dtor
+  shutdown_cb_called = false;
+  {
+    auto ret = reset_key;
+    auto test_node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testnode");
+    test_node->register_on_shutdown(std::bind(on_shutdown_callback, std::placeholders::_1));
+    auto configured = test_node->configure(ret);
+    EXPECT_EQ(success, ret);
+    EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE);
+    EXPECT_FALSE(shutdown_cb_called);
+  }
+  EXPECT_TRUE(shutdown_cb_called);
+
+  // PRIMARY_STATE_ACTIVE to shutdown via dtor
+  shutdown_cb_called = false;
+  {
+    auto ret = reset_key;
+    auto test_node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testnode");
+    test_node->register_on_shutdown(std::bind(on_shutdown_callback, std::placeholders::_1));
+    auto configured = test_node->configure(ret);
+    EXPECT_EQ(success, ret);
+    EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE);
+    ret = reset_key;
+    auto activated = test_node->activate(ret);
+    EXPECT_EQ(success, ret);
+    EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE);
+    EXPECT_FALSE(shutdown_cb_called);
+  }
+  EXPECT_TRUE(shutdown_cb_called);
+
+  // PRIMARY_STATE_FINALIZED to shutdown via dtor
+  shutdown_cb_called = false;
+  {
+    auto ret = reset_key;
+    auto test_node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testnode");
+    test_node->register_on_shutdown(std::bind(on_shutdown_callback, std::placeholders::_1));
+    auto configured = test_node->configure(ret);
+    EXPECT_EQ(success, ret);
+    EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE);
+    ret = reset_key;
+    auto activated = test_node->activate(ret);
+    EXPECT_EQ(success, ret);
+    EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE);
+    ret = reset_key;
+    auto finalized = test_node->shutdown(ret);
+    EXPECT_EQ(success, ret);
+    EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED);
+    EXPECT_TRUE(shutdown_cb_called);  // should be called already
+  }
+  EXPECT_TRUE(shutdown_cb_called);
+}
+
 TEST_F(TestDefaultStateMachine, lifecycle_subscriber) {
   auto test_node = std::make_shared<MoodyLifecycleNode<GoodMood>>("testnode");