21.0.7

Signed-off-by: Yadunund <yadunund@gmail.com>

rclcpp/CHANGELOG.rst

@@ -2,6 +2,12 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+21.0.7 (2024-07-10)
+-------------------
+* Add test creating two content filter topics with the same topic name (`#2550 <https://github.com/ros2/rclcpp/issues/2550>`_)
+* Revise the description of service configure_introspection() (`#2514 <https://github.com/ros2/rclcpp/issues/2514>`_)
+* Contributors: Alejandro Hernández Cordero, Barry Xu
+
 21.0.6 (2024-04-19)
 -------------------
 * address ambiguous auto variable. (`#2486 <https://github.com/ros2/rclcpp/issues/2486>`_)

rclcpp/include/rclcpp/service.hpp

@@ -495,7 +495,7 @@ public:
     }
   }
 
-  /// Configure client introspection.
+  /// Configure service introspection.
   /**
    * \param[in] clock clock to use to generate introspection timestamps
    * \param[in] qos_service_event_pub QoS settings to use when creating the introspection publisher

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>21.0.6</version>
+  <version>21.0.7</version>
   <description>The ROS client library in C++.</description>
 
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>

rclcpp/test/rclcpp/test_subscription_content_filter.cpp

@@ -310,3 +310,25 @@ TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_fil
     }
   }
 }
+
+TEST_F(
+  CLASSNAME(
+    TestContentFilterSubscription,
+    RMW_IMPLEMENTATION), create_two_content_filters_with_same_topic_name_and_destroy) {
+
+  // Create another content filter
+  auto options = rclcpp::SubscriptionOptions();
+
+  std::string filter_expression = "int32_value > %0";
+  std::vector<std::string> expression_parameters = {"4"};
+
+  options.content_filter_options.filter_expression = filter_expression;
+  options.content_filter_options.expression_parameters = expression_parameters;
+
+  auto callback = [](std::shared_ptr<const test_msgs::msg::BasicTypes>) {};
+  auto sub_2 = node->create_subscription<test_msgs::msg::BasicTypes>(
+    "content_filter_topic", qos, callback, options);
+
+  EXPECT_NE(nullptr, sub_2);
+  sub_2.reset();
+}

rclcpp_action/CHANGELOG.rst

@@ -3,6 +3,12 @@ Changelog for package rclcpp_action
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+21.0.7 (2024-07-10)
+-------------------
+* fix: Fixed race condition in action server between is_ready and take"… (`#2531 <https://github.com/ros2/rclcpp/issues/2531>`_)
+* Do not generate the exception when action service response timeout. (`#2519 <https://github.com/ros2/rclcpp/issues/2519>`_)
+* Contributors: Janosch Machowinski, Tomoya Fujita, William Woodall
+
 21.0.6 (2024-04-19)
 -------------------
 

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>21.0.6</version>
+  <version>21.0.7</version>
   <description>Adds action APIs for C++.</description>
 
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>

rclcpp_action/src/client.cpp

@@ -19,6 +19,7 @@
 #include <string>
 #include <tuple>
 #include <utility>
+#include <variant>
 
 #include "rcl_action/action_client.h"
 #include "rcl_action/wait.h"
@@ -31,6 +32,67 @@
 namespace rclcpp_action
 {
 
+struct ClientBaseData
+{
+  struct FeedbackReadyData
+  {
+    FeedbackReadyData(rcl_ret_t retIn, std::shared_ptr<void> msg)
+    : ret(retIn), feedback_message(msg) {}
+    rcl_ret_t ret;
+    std::shared_ptr<void> feedback_message;
+  };
+  struct StatusReadyData
+  {
+    StatusReadyData(rcl_ret_t retIn, std::shared_ptr<void> msg)
+    : ret(retIn), status_message(msg) {}
+    rcl_ret_t ret;
+    std::shared_ptr<void> status_message;
+  };
+  struct GoalResponseData
+  {
+    GoalResponseData(rcl_ret_t retIn, rmw_request_id_t header, std::shared_ptr<void> response)
+    : ret(retIn), response_header(header), goal_response(response) {}
+    rcl_ret_t ret;
+    rmw_request_id_t response_header;
+    std::shared_ptr<void> goal_response;
+  };
+  struct CancelResponseData
+  {
+    CancelResponseData(rcl_ret_t retIn, rmw_request_id_t header, std::shared_ptr<void> response)
+    : ret(retIn), response_header(header), cancel_response(response) {}
+    rcl_ret_t ret;
+    rmw_request_id_t response_header;
+    std::shared_ptr<void> cancel_response;
+  };
+  struct ResultResponseData
+  {
+    ResultResponseData(rcl_ret_t retIn, rmw_request_id_t header, std::shared_ptr<void> response)
+    : ret(retIn), response_header(header), result_response(response) {}
+    rcl_ret_t ret;
+    rmw_request_id_t response_header;
+    std::shared_ptr<void> result_response;
+  };
+
+  std::variant<
+    FeedbackReadyData,
+    StatusReadyData,
+    GoalResponseData,
+    CancelResponseData,
+    ResultResponseData
+  > data;
+
+  explicit ClientBaseData(FeedbackReadyData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ClientBaseData(StatusReadyData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ClientBaseData(GoalResponseData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ClientBaseData(CancelResponseData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ClientBaseData(ResultResponseData && data_in)
+  : data(std::move(data_in)) {}
+};
+
 class ClientBaseImpl
 {
 public:
@@ -94,11 +156,13 @@ public:
   size_t num_clients{0u};
   size_t num_services{0u};
 
-  bool is_feedback_ready{false};
-  bool is_status_ready{false};
-  bool is_goal_response_ready{false};
-  bool is_cancel_response_ready{false};
-  bool is_result_response_ready{false};
+  // Lock for action_client_
+  std::recursive_mutex action_client_mutex_;
+
+  // next ready event for taking, will be set by is_ready and will be processed by take_data
+  std::atomic<size_t> next_ready_event;
+  // used to indicate that next_ready_event has no ready event for processing
+  static constexpr size_t NO_EVENT_READY = std::numeric_limits<size_t>::max();
 
   rclcpp::Context::SharedPtr context_;
   rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
@@ -142,6 +206,7 @@ bool
 ClientBase::action_server_is_ready() const
 {
   bool is_ready;
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
   rcl_ret_t ret = rcl_action_server_is_available(
     this->pimpl_->node_handle.get(),
     this->pimpl_->client_handle.get(),
@@ -255,6 +320,7 @@ ClientBase::get_number_of_ready_services()
 void
 ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set)
 {
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
   rcl_ret_t ret = rcl_action_wait_set_add_action_client(
     wait_set, pimpl_->client_handle.get(), nullptr, nullptr);
   if (RCL_RET_OK != ret) {
@@ -265,23 +331,56 @@ ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set)
 bool
 ClientBase::is_ready(rcl_wait_set_t * wait_set)
 {
-  rcl_ret_t ret = rcl_action_client_wait_set_get_entities_ready(
-    wait_set, pimpl_->client_handle.get(),
-    &pimpl_->is_feedback_ready,
-    &pimpl_->is_status_ready,
-    &pimpl_->is_goal_response_ready,
-    &pimpl_->is_cancel_response_ready,
-    &pimpl_->is_result_response_ready);
-  if (RCL_RET_OK != ret) {
-    rclcpp::exceptions::throw_from_rcl_error(
-      ret, "failed to check for any ready entities");
+  bool is_feedback_ready{false};
+  bool is_status_ready{false};
+  bool is_goal_response_ready{false};
+  bool is_cancel_response_ready{false};
+  bool is_result_response_ready{false};
+
+  rcl_ret_t ret;
+  {
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
+    ret = rcl_action_client_wait_set_get_entities_ready(
+      wait_set, pimpl_->client_handle.get(),
+      &is_feedback_ready,
+      &is_status_ready,
+      &is_goal_response_ready,
+      &is_cancel_response_ready,
+      &is_result_response_ready);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(
+        ret, "failed to check for any ready entities");
+    }
   }
-  return
-    pimpl_->is_feedback_ready ||
-    pimpl_->is_status_ready ||
-    pimpl_->is_goal_response_ready ||
-    pimpl_->is_cancel_response_ready ||
-    pimpl_->is_result_response_ready;
+
+  pimpl_->next_ready_event = ClientBaseImpl::NO_EVENT_READY;
+
+  if (is_feedback_ready) {
+    pimpl_->next_ready_event = static_cast<size_t>(EntityType::FeedbackSubscription);
+    return true;
+  }
+
+  if (is_status_ready) {
+    pimpl_->next_ready_event = static_cast<size_t>(EntityType::StatusSubscription);
+    return true;
+  }
+
+  if (is_goal_response_ready) {
+    pimpl_->next_ready_event = static_cast<size_t>(EntityType::GoalClient);
+    return true;
+  }
+
+  if (is_result_response_ready) {
+    pimpl_->next_ready_event = static_cast<size_t>(EntityType::ResultClient);
+    return true;
+  }
+
+  if (is_cancel_response_ready) {
+    pimpl_->next_ready_event = static_cast<size_t>(EntityType::CancelClient);
+    return true;
+  }
+
+  return false;
 }
 
 void
@@ -432,7 +531,6 @@ ClientBase::set_callback_to_entity(
       }
     };
 
-
   // Set it temporarily to the new callback, while we replace the old one.
   // This two-step setting, prevents a gap where the old std::function has
   // been replaced but the middleware hasn't been told about the new one yet.
@@ -550,140 +648,159 @@ ClientBase::clear_on_ready_callback()
 std::shared_ptr<void>
 ClientBase::take_data()
 {
-  if (pimpl_->is_feedback_ready) {
-    std::shared_ptr<void> feedback_message = this->create_feedback_message();
-    rcl_ret_t ret = rcl_action_take_feedback(
-      pimpl_->client_handle.get(), feedback_message.get());
-    return std::static_pointer_cast<void>(
-      std::make_shared<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(
-        ret, feedback_message));
-  } else if (pimpl_->is_status_ready) {
-    std::shared_ptr<void> status_message = this->create_status_message();
-    rcl_ret_t ret = rcl_action_take_status(
-      pimpl_->client_handle.get(), status_message.get());
-    return std::static_pointer_cast<void>(
-      std::make_shared<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(
-        ret, status_message));
-  } else if (pimpl_->is_goal_response_ready) {
-    rmw_request_id_t response_header;
-    std::shared_ptr<void> goal_response = this->create_goal_response();
-    rcl_ret_t ret = rcl_action_take_goal_response(
-      pimpl_->client_handle.get(), &response_header, goal_response.get());
-    return std::static_pointer_cast<void>(
-      std::make_shared<std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(
-        ret, response_header, goal_response));
-  } else if (pimpl_->is_result_response_ready) {
-    rmw_request_id_t response_header;
-    std::shared_ptr<void> result_response = this->create_result_response();
-    rcl_ret_t ret = rcl_action_take_result_response(
-      pimpl_->client_handle.get(), &response_header, result_response.get());
-    return std::static_pointer_cast<void>(
-      std::make_shared<std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(
-        ret, response_header, result_response));
-  } else if (pimpl_->is_cancel_response_ready) {
-    rmw_request_id_t response_header;
-    std::shared_ptr<void> cancel_response = this->create_cancel_response();
-    rcl_ret_t ret = rcl_action_take_cancel_response(
-      pimpl_->client_handle.get(), &response_header, cancel_response.get());
-    return std::static_pointer_cast<void>(
-      std::make_shared<std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(
-        ret, response_header, cancel_response));
-  } else {
-    throw std::runtime_error("Taking data from action client but nothing is ready");
+  // next_ready_event is an atomic, caching localy
+  size_t next_ready_event = pimpl_->next_ready_event.exchange(ClientBaseImpl::NO_EVENT_READY);
+
+  if (next_ready_event == ClientBaseImpl::NO_EVENT_READY) {
+    // there is a known bug in iron, that take_data might be called multiple
+    // times. Therefore instead of throwing, we just return a nullptr as a workaround.
+    return nullptr;
   }
+
+  return take_data_by_entity_id(next_ready_event);
 }
 
 std::shared_ptr<void>
 ClientBase::take_data_by_entity_id(size_t id)
 {
+  std::shared_ptr<ClientBaseData> data_ptr;
+  rcl_ret_t ret;
+
   // Mark as ready the entity from which we want to take data
   switch (static_cast<EntityType>(id)) {
     case EntityType::GoalClient:
-      pimpl_->is_goal_response_ready = true;
+      {
+        rmw_request_id_t response_header;
+        std::shared_ptr<void> goal_response;
+        {
+          std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
+
+          goal_response = this->create_goal_response();
+          ret = rcl_action_take_goal_response(
+            pimpl_->client_handle.get(), &response_header, goal_response.get());
+        }
+        data_ptr = std::make_shared<ClientBaseData>(
+          ClientBaseData::GoalResponseData(
+            ret, response_header, goal_response));
+      }
       break;
     case EntityType::ResultClient:
-      pimpl_->is_result_response_ready = true;
+      {
+        rmw_request_id_t response_header;
+        std::shared_ptr<void> result_response;
+        {
+          std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
+          result_response = this->create_result_response();
+          ret = rcl_action_take_result_response(
+            pimpl_->client_handle.get(), &response_header, result_response.get());
+        }
+        data_ptr =
+          std::make_shared<ClientBaseData>(
+          ClientBaseData::ResultResponseData(
+            ret, response_header, result_response));
+      }
       break;
     case EntityType::CancelClient:
-      pimpl_->is_cancel_response_ready = true;
+      {
+        rmw_request_id_t response_header;
+        std::shared_ptr<void> cancel_response;
+        {
+          std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
+          cancel_response = this->create_cancel_response();
+          ret = rcl_action_take_cancel_response(
+            pimpl_->client_handle.get(), &response_header, cancel_response.get());
+        }
+        data_ptr =
+          std::make_shared<ClientBaseData>(
+          ClientBaseData::CancelResponseData(
+            ret, response_header, cancel_response));
+      }
       break;
     case EntityType::FeedbackSubscription:
-      pimpl_->is_feedback_ready = true;
+      {
+        std::shared_ptr<void> feedback_message;
+        {
+          std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
+          feedback_message = this->create_feedback_message();
+          ret = rcl_action_take_feedback(
+            pimpl_->client_handle.get(), feedback_message.get());
+        }
+        data_ptr =
+          std::make_shared<ClientBaseData>(
+          ClientBaseData::FeedbackReadyData(
+            ret, feedback_message));
+      }
       break;
     case EntityType::StatusSubscription:
-      pimpl_->is_status_ready = true;
+      {
+        std::shared_ptr<void> status_message;
+        {
+          std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
+          status_message = this->create_status_message();
+          ret = rcl_action_take_status(
+            pimpl_->client_handle.get(), status_message.get());
+        }
+        data_ptr =
+          std::make_shared<ClientBaseData>(
+          ClientBaseData::StatusReadyData(
+            ret, status_message));
+      }
       break;
   }
 
-  return take_data();
+  return std::static_pointer_cast<void>(data_ptr);
 }
 
 void
-ClientBase::execute(std::shared_ptr<void> & data)
+ClientBase::execute(std::shared_ptr<void> & data_in)
 {
-  if (!data) {
-    throw std::runtime_error("'data' is empty");
+  if (!data_in) {
+    // workaround, if take_data was called multiple timed, it returns a nullptr
+    // normally we should throw here, but as an API stable bug fix, we just ignore this...
+    return;
   }
 
-  if (pimpl_->is_feedback_ready) {
-    auto shared_ptr = std::static_pointer_cast<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(data);
-    auto ret = std::get<0>(*shared_ptr);
-    pimpl_->is_feedback_ready = false;
-    if (RCL_RET_OK == ret) {
-      auto feedback_message = std::get<1>(*shared_ptr);
-      this->handle_feedback_message(feedback_message);
-    } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking feedback");
-    }
-  } else if (pimpl_->is_status_ready) {
-    auto shared_ptr = std::static_pointer_cast<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(data);
-    auto ret = std::get<0>(*shared_ptr);
-    pimpl_->is_status_ready = false;
-    if (RCL_RET_OK == ret) {
-      auto status_message = std::get<1>(*shared_ptr);
-      this->handle_status_message(status_message);
-    } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking status");
-    }
-  } else if (pimpl_->is_goal_response_ready) {
-    auto shared_ptr = std::static_pointer_cast<
-      std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(data);
-    auto ret = std::get<0>(*shared_ptr);
-    pimpl_->is_goal_response_ready = false;
-    if (RCL_RET_OK == ret) {
-      auto response_header = std::get<1>(*shared_ptr);
-      auto goal_response = std::get<2>(*shared_ptr);
-      this->handle_goal_response(response_header, goal_response);
-    } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking goal response");
-    }
-  } else if (pimpl_->is_result_response_ready) {
-    auto shared_ptr = std::static_pointer_cast<
-      std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(data);
-    auto ret = std::get<0>(*shared_ptr);
-    pimpl_->is_result_response_ready = false;
-    if (RCL_RET_OK == ret) {
-      auto response_header = std::get<1>(*shared_ptr);
-      auto result_response = std::get<2>(*shared_ptr);
-      this->handle_result_response(response_header, result_response);
-    } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking result response");
-    }
-  } else if (pimpl_->is_cancel_response_ready) {
-    auto shared_ptr = std::static_pointer_cast<
-      std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(data);
-    auto ret = std::get<0>(*shared_ptr);
-    pimpl_->is_cancel_response_ready = false;
-    if (RCL_RET_OK == ret) {
-      auto response_header = std::get<1>(*shared_ptr);
-      auto cancel_response = std::get<2>(*shared_ptr);
-      this->handle_cancel_response(response_header, cancel_response);
-    } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking cancel response");
-    }
-  } else {
-    throw std::runtime_error("Executing action client but nothing is ready");
-  }
+  std::shared_ptr<ClientBaseData> data_ptr = std::static_pointer_cast<ClientBaseData>(data_in);
+
+  std::visit(
+    [&](auto && data) -> void {
+      using T = std::decay_t<decltype(data)>;
+      if constexpr (std::is_same_v<T, ClientBaseData::FeedbackReadyData>) {
+        if (RCL_RET_OK == data.ret) {
+          this->handle_feedback_message(data.feedback_message);
+        } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) {
+          rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking feedback");
+        }
+      }
+      if constexpr (std::is_same_v<T, ClientBaseData::StatusReadyData>) {
+        if (RCL_RET_OK == data.ret) {
+          this->handle_status_message(data.status_message);
+        } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) {
+          rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking status");
+        }
+      }
+      if constexpr (std::is_same_v<T, ClientBaseData::GoalResponseData>) {
+        if (RCL_RET_OK == data.ret) {
+          this->handle_goal_response(data.response_header, data.goal_response);
+        } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) {
+          rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking goal response");
+        }
+      }
+      if constexpr (std::is_same_v<T, ClientBaseData::ResultResponseData>) {
+        if (RCL_RET_OK == data.ret) {
+          this->handle_result_response(data.response_header, data.result_response);
+        } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) {
+          rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking result response");
+        }
+      }
+      if constexpr (std::is_same_v<T, ClientBaseData::CancelResponseData>) {
+        if (RCL_RET_OK == data.ret) {
+          this->handle_cancel_response(data.response_header, data.cancel_response);
+        } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) {
+          rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking cancel response");
+        }
+      }
+    }, data_ptr->data);
 }
 
 }  // namespace rclcpp_action

rclcpp_action/src/server.cpp

@@ -18,6 +18,7 @@
 #include <tuple>
 #include <unordered_map>
 #include <utility>
+#include <variant>
 #include <vector>
 
 #include "rcl_action/action_server.h"
@@ -33,8 +34,50 @@
 using rclcpp_action::ServerBase;
 using rclcpp_action::GoalUUID;
 
+struct ServerBaseData;
+
 namespace rclcpp_action
 {
+
+struct ServerBaseData
+{
+  using GoalRequestData = std::tuple<
+    rcl_ret_t,
+    const rcl_action_goal_info_t,
+    rmw_request_id_t,
+    std::shared_ptr<void>
+  >;
+
+  using CancelRequestData = std::tuple<
+    rcl_ret_t,
+    std::shared_ptr<action_msgs::srv::CancelGoal::Request>,
+    rmw_request_id_t
+  >;
+
+  using ResultRequestData = std::tuple<rcl_ret_t, std::shared_ptr<void>, rmw_request_id_t>;
+
+  using GoalExpiredData = struct Empty {};
+
+  std::variant<GoalRequestData, CancelRequestData, ResultRequestData, GoalExpiredData> data;
+
+  explicit ServerBaseData(GoalRequestData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ServerBaseData(CancelRequestData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ServerBaseData(ResultRequestData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ServerBaseData(GoalExpiredData && data_in)
+  : data(std::move(data_in)) {}
+};
+
+enum class ActionEventType : std::size_t
+{
+  GoalService,
+  ResultService,
+  CancelService,
+  Expired,
+};
+
 class ServerBaseImpl
 {
 public:
@@ -60,11 +103,6 @@ public:
   size_t num_services_ = 0;
   size_t num_guard_conditions_ = 0;
 
-  std::atomic<bool> goal_request_ready_{false};
-  std::atomic<bool> cancel_request_ready_{false};
-  std::atomic<bool> result_request_ready_{false};
-  std::atomic<bool> goal_expired_{false};
-
   // Lock for unordered_maps
   std::recursive_mutex unordered_map_mutex_;
 
@@ -75,8 +113,15 @@ public:
   // rcl goal handles are kept so api to send result doesn't try to access freed memory
   std::unordered_map<GoalUUID, std::shared_ptr<rcl_action_goal_handle_t>> goal_handles_;
 
+
+  // next ready event for taking, will be set by is_ready and will be processed by take_data
+  std::atomic<size_t> next_ready_event;
+  // used to indicate that next_ready_event has no ready event for processing
+  static constexpr size_t NO_EVENT_READY = std::numeric_limits<size_t>::max();
+
   rclcpp::Logger logger_;
 };
+
 }  // namespace rclcpp_action
 
 ServerBase::ServerBase(
@@ -194,124 +239,170 @@ ServerBase::is_ready(rcl_wait_set_t * wait_set)
       &goal_expired);
   }
 
-  pimpl_->goal_request_ready_ = goal_request_ready;
-  pimpl_->cancel_request_ready_ = cancel_request_ready;
-  pimpl_->result_request_ready_ = result_request_ready;
-  pimpl_->goal_expired_ = goal_expired;
-
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
 
-  return pimpl_->goal_request_ready_.load() ||
-         pimpl_->cancel_request_ready_.load() ||
-         pimpl_->result_request_ready_.load() ||
-         pimpl_->goal_expired_.load();
+  pimpl_->next_ready_event = ServerBaseImpl::NO_EVENT_READY;
+
+  if (goal_request_ready) {
+    pimpl_->next_ready_event = static_cast<uint32_t>(ActionEventType::GoalService);
+    return true;
+  }
+
+  if (cancel_request_ready) {
+    pimpl_->next_ready_event = static_cast<uint32_t>(ActionEventType::CancelService);
+    return true;
+  }
+
+  if (result_request_ready) {
+    pimpl_->next_ready_event = static_cast<uint32_t>(ActionEventType::ResultService);
+    return true;
+  }
+
+  if (goal_expired) {
+    pimpl_->next_ready_event = static_cast<uint32_t>(ActionEventType::Expired);
+    return true;
+  }
+
+  return false;
 }
 
 std::shared_ptr<void>
 ServerBase::take_data()
 {
-  if (pimpl_->goal_request_ready_.load()) {
-    rcl_ret_t ret;
-    rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info();
-    rmw_request_id_t request_header;
+  size_t next_ready_event = pimpl_->next_ready_event.exchange(ServerBaseImpl::NO_EVENT_READY);
 
-    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
-
-    std::shared_ptr<void> message = create_goal_request();
-    ret = rcl_action_take_goal_request(
-      pimpl_->action_server_.get(),
-      &request_header,
-      message.get());
-
-    return std::static_pointer_cast<void>(
-      std::make_shared
-      <std::tuple<rcl_ret_t, rcl_action_goal_info_t, rmw_request_id_t, std::shared_ptr<void>>>(
-        ret,
-        goal_info,
-        request_header, message));
-  } else if (pimpl_->cancel_request_ready_.load()) {
-    rcl_ret_t ret;
-    rmw_request_id_t request_header;
-
-    // Initialize cancel request
-    auto request = std::make_shared<action_msgs::srv::CancelGoal::Request>();
-
-    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
-    ret = rcl_action_take_cancel_request(
-      pimpl_->action_server_.get(),
-      &request_header,
-      request.get());
-
-    return std::static_pointer_cast<void>(
-      std::make_shared
-      <std::tuple<rcl_ret_t, std::shared_ptr<action_msgs::srv::CancelGoal::Request>,
-      rmw_request_id_t>>(ret, request, request_header));
-  } else if (pimpl_->result_request_ready_.load()) {
-    rcl_ret_t ret;
-    // Get the result request message
-    rmw_request_id_t request_header;
-    std::shared_ptr<void> result_request = create_result_request();
-    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
-    ret = rcl_action_take_result_request(
-      pimpl_->action_server_.get(), &request_header, result_request.get());
-
-    return std::static_pointer_cast<void>(
-      std::make_shared<std::tuple<rcl_ret_t, std::shared_ptr<void>, rmw_request_id_t>>(
-        ret, result_request, request_header));
-  } else if (pimpl_->goal_expired_.load()) {
+  if (next_ready_event == ServerBaseImpl::NO_EVENT_READY) {
+    // there is a known bug in iron, that take_data might be called multiple
+    // times. Therefore instead of throwing, we just return a nullptr as a workaround.
     return nullptr;
-  } else {
-    throw std::runtime_error("Taking data from action server but nothing is ready");
   }
+
+  return take_data_by_entity_id(next_ready_event);
 }
 
 std::shared_ptr<void>
 ServerBase::take_data_by_entity_id(size_t id)
 {
+  static_assert(
+    static_cast<size_t>(EntityType::GoalService) ==
+    static_cast<size_t>(ActionEventType::GoalService));
+  static_assert(
+    static_cast<size_t>(EntityType::ResultService) ==
+    static_cast<size_t>(ActionEventType::ResultService));
+  static_assert(
+    static_cast<size_t>(EntityType::CancelService) ==
+    static_cast<size_t>(ActionEventType::CancelService));
+
+  std::shared_ptr<ServerBaseData> data_ptr;
   // Mark as ready the entity from which we want to take data
-  switch (static_cast<EntityType>(id)) {
-    case EntityType::GoalService:
-      pimpl_->goal_request_ready_ = true;
+  switch (static_cast<ActionEventType>(id)) {
+    case ActionEventType::GoalService:
+      {
+        rcl_ret_t ret;
+        rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info();
+        rmw_request_id_t request_header;
+
+        std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+
+        std::shared_ptr<void> message = create_goal_request();
+        ret = rcl_action_take_goal_request(
+          pimpl_->action_server_.get(),
+          &request_header,
+          message.get());
+
+        data_ptr = std::make_shared<ServerBaseData>(
+          ServerBaseData::GoalRequestData(ret, goal_info, request_header, message));
+      }
       break;
-    case EntityType::ResultService:
-      pimpl_->result_request_ready_ = true;
+    case ActionEventType::ResultService:
+      {
+        rcl_ret_t ret;
+        // Get the result request message
+        rmw_request_id_t request_header;
+        std::shared_ptr<void> result_request = create_result_request();
+        std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+        ret = rcl_action_take_result_request(
+          pimpl_->action_server_.get(), &request_header, result_request.get());
+
+        data_ptr =
+          std::make_shared<ServerBaseData>(
+          ServerBaseData::ResultRequestData(ret, result_request, request_header));
+      }
       break;
-    case EntityType::CancelService:
-      pimpl_->cancel_request_ready_ = true;
+    case ActionEventType::CancelService:
+      {
+        rcl_ret_t ret;
+        rmw_request_id_t request_header;
+
+        // Initialize cancel request
+        auto request = std::make_shared<action_msgs::srv::CancelGoal::Request>();
+
+        std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+        ret = rcl_action_take_cancel_request(
+          pimpl_->action_server_.get(),
+          &request_header,
+          request.get());
+
+        data_ptr =
+          std::make_shared<ServerBaseData>(
+          ServerBaseData::CancelRequestData(ret, request, request_header));
+      }
+      break;
+    case ActionEventType::Expired:
+      {
+        data_ptr =
+          std::make_shared<ServerBaseData>(ServerBaseData::GoalExpiredData());
+      }
       break;
   }
 
-  return take_data();
+  return std::static_pointer_cast<void>(data_ptr);
 }
 
 void
-ServerBase::execute(std::shared_ptr<void> & data)
+ServerBase::execute(std::shared_ptr<void> & data_in)
 {
-  if (!data && !pimpl_->goal_expired_.load()) {
-    throw std::runtime_error("'data' is empty");
+  if (!data_in) {
+    // workaround, if take_data was called multiple timed, it returns a nullptr
+    // normally we should throw here, but as an API stable bug fix, we just ignore this...
+    return;
   }
 
-  if (pimpl_->goal_request_ready_.load()) {
-    execute_goal_request_received(data);
-  } else if (pimpl_->cancel_request_ready_.load()) {
-    execute_cancel_request_received(data);
-  } else if (pimpl_->result_request_ready_.load()) {
-    execute_result_request_received(data);
-  } else if (pimpl_->goal_expired_.load()) {
-    execute_check_expired_goals();
-  } else {
-    throw std::runtime_error("Executing action server but nothing is ready");
-  }
+  std::shared_ptr<ServerBaseData> data_ptr = std::static_pointer_cast<ServerBaseData>(data_in);
+
+  std::visit(
+    [&](auto && data) -> void {
+      using T = std::decay_t<decltype(data)>;
+      if constexpr (std::is_same_v<T, ServerBaseData::GoalRequestData>) {
+        execute_goal_request_received(data_in);
+      }
+      if constexpr (std::is_same_v<T, ServerBaseData::CancelRequestData>) {
+        execute_cancel_request_received(data_in);
+      }
+      if constexpr (std::is_same_v<T, ServerBaseData::ResultRequestData>) {
+        execute_result_request_received(data_in);
+      }
+      if constexpr (std::is_same_v<T, ServerBaseData::GoalExpiredData>) {
+        execute_check_expired_goals();
+      }
+    },
+    data_ptr->data);
 }
 
 void
 ServerBase::execute_goal_request_received(std::shared_ptr<void> & data)
 {
-  auto shared_ptr = std::static_pointer_cast
-    <std::tuple<rcl_ret_t, rcl_action_goal_info_t, rmw_request_id_t, std::shared_ptr<void>>>(data);
-  rcl_ret_t ret = std::get<0>(*shared_ptr);
+  std::shared_ptr<ServerBaseData> data_ptr = std::static_pointer_cast<ServerBaseData>(data);
+  const ServerBaseData::GoalRequestData & gData(
+    std::get<ServerBaseData::GoalRequestData>(data_ptr->data));
+
+  rcl_ret_t ret = std::get<0>(gData);
+  rcl_action_goal_info_t goal_info = std::get<1>(gData);
+  rmw_request_id_t request_header = std::get<2>(gData);
+  const std::shared_ptr<void> message = std::get<3>(gData);
+
   if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) {
     // Ignore take failure because connext fails if it receives a sample without valid data.
     // This happens when a client shuts down and connext receives a sample saying the client is
@@ -320,14 +411,6 @@ ServerBase::execute_goal_request_received(std::shared_ptr<void> & data)
   } else if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
-  rcl_action_goal_info_t goal_info = std::get<1>(*shared_ptr);
-  rmw_request_id_t request_header = std::get<2>(*shared_ptr);
-  std::shared_ptr<void> message = std::get<3>(*shared_ptr);
-
-  bool expected = true;
-  if (!pimpl_->goal_request_ready_.compare_exchange_strong(expected, false)) {
-    return;
-  }
 
   GoalUUID uuid = get_goal_id_from_goal_request(message.get());
   convert(uuid, &goal_info);
@@ -344,7 +427,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;
@@ -402,10 +494,15 @@ ServerBase::execute_goal_request_received(std::shared_ptr<void> & data)
 void
 ServerBase::execute_cancel_request_received(std::shared_ptr<void> & data)
 {
-  auto shared_ptr = std::static_pointer_cast
-    <std::tuple<rcl_ret_t, std::shared_ptr<action_msgs::srv::CancelGoal::Request>,
-      rmw_request_id_t>>(data);
-  auto ret = std::get<0>(*shared_ptr);
+  std::shared_ptr<ServerBaseData> data_ptr = std::static_pointer_cast<ServerBaseData>(data);
+  const ServerBaseData::CancelRequestData & gData(
+    std::get<ServerBaseData::CancelRequestData>(data_ptr->data));
+
+  rcl_ret_t ret = std::get<0>(gData);
+  std::shared_ptr<action_msgs::srv::CancelGoal::Request> request = std::get<1>(gData);
+  rmw_request_id_t request_header = std::get<2>(gData);
+
+
   if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) {
     // Ignore take failure because connext fails if it receives a sample without valid data.
     // This happens when a client shuts down and connext receives a sample saying the client is
@@ -414,9 +511,6 @@ ServerBase::execute_cancel_request_received(std::shared_ptr<void> & data)
   } else if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
-  auto request = std::get<1>(*shared_ptr);
-  auto request_header = std::get<2>(*shared_ptr);
-  pimpl_->cancel_request_ready_ = false;
 
   // Convert c++ message to C message
   rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request();
@@ -483,6 +577,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);
   }
@@ -492,9 +595,14 @@ ServerBase::execute_cancel_request_received(std::shared_ptr<void> & data)
 void
 ServerBase::execute_result_request_received(std::shared_ptr<void> & data)
 {
-  auto shared_ptr = std::static_pointer_cast
-    <std::tuple<rcl_ret_t, std::shared_ptr<void>, rmw_request_id_t>>(data);
-  auto ret = std::get<0>(*shared_ptr);
+  std::shared_ptr<ServerBaseData> data_ptr = std::static_pointer_cast<ServerBaseData>(data);
+  const ServerBaseData::ResultRequestData & gData(
+    std::get<ServerBaseData::ResultRequestData>(data_ptr->data));
+
+  rcl_ret_t ret = std::get<0>(gData);
+  std::shared_ptr<void> result_request = std::get<1>(gData);
+  rmw_request_id_t request_header = std::get<2>(gData);
+
   if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) {
     // Ignore take failure because connext fails if it receives a sample without valid data.
     // This happens when a client shuts down and connext receives a sample saying the client is
@@ -503,10 +611,7 @@ ServerBase::execute_result_request_received(std::shared_ptr<void> & data)
   } else if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
-  auto result_request = std::get<1>(*shared_ptr);
-  auto request_header = std::get<2>(*shared_ptr);
 
-  pimpl_->result_request_ready_ = false;
   std::shared_ptr<void> result_response;
 
   // check if the goal exists
@@ -538,6 +643,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);
     }
@@ -671,7 +784,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,9 @@
 Changelog for package rclcpp_components
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+21.0.7 (2024-07-10)
+-------------------
+
 21.0.6 (2024-04-19)
 -------------------
 

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>21.0.6</version>
+  <version>21.0.7</version>
   <description>Package containing tools for dynamically loadable components</description>
 
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>

rclcpp_lifecycle/CHANGELOG.rst

@@ -3,6 +3,13 @@ Changelog for package rclcpp_lifecycle
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+21.0.7 (2024-07-10)
+-------------------
+* revert call shutdown in LifecycleNode destructor (Iron) (`#2559 <https://github.com/ros2/rclcpp/issues/2559>`_)
+* lifecycle node dtor shutdown should be called only in primary state. (`#2543 <https://github.com/ros2/rclcpp/issues/2543>`_)
+* rclcpp::shutdown should not be called before LifecycleNode dtor. (`#2539 <https://github.com/ros2/rclcpp/issues/2539>`_)
+* Contributors: Tomoya Fujita
+
 21.0.6 (2024-04-19)
 -------------------
 * call shutdown in LifecycleNode dtor to avoid leaving the device in un… (`#2490 <https://github.com/ros2/rclcpp/issues/2490>`_)

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>21.0.6</version>
+  <version>21.0.7</version>
   <description>Package containing a prototype for lifecycle implementation</description>
 
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>

rclcpp_lifecycle/src/lifecycle_node.cpp

@@ -146,22 +146,6 @@ 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/test/test_lifecycle_node.cpp

@@ -414,146 +414,6 @@ 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");
 

rclcpp_lifecycle/test/test_lifecycle_publisher.cpp

@@ -55,12 +55,6 @@ public:
   explicit EmptyLifecycleNode(const std::string & node_name, const TimerType & timer_type)
   : rclcpp_lifecycle::LifecycleNode(node_name)
   {
-    rclcpp::PublisherOptionsWithAllocator<std::allocator<void>> options;
-    publisher_ =
-      std::make_shared<rclcpp_lifecycle::LifecyclePublisher<test_msgs::msg::Empty>>(
-      get_node_base_interface().get(), std::string("topic"), rclcpp::QoS(10), options);
-    add_managed_entity(publisher_);
-
     // For coverage this is being added here
     switch (timer_type) {
       case TimerType::WALL_TIMER:
@@ -77,14 +71,6 @@ public:
         }
     }
   }
-
-  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<test_msgs::msg::Empty>> publisher()
-  {
-    return publisher_;
-  }
-
-private:
-  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<test_msgs::msg::Empty>> publisher_;
 };
 
 class TestLifecyclePublisher : public ::testing::TestWithParam<TimerType>
@@ -93,95 +79,103 @@ public:
   void SetUp()
   {
     rclcpp::init(0, nullptr);
-    node_ = std::make_shared<EmptyLifecycleNode>("node", GetParam());
   }
 
   void TearDown()
   {
     rclcpp::shutdown();
   }
-
-protected:
-  std::shared_ptr<EmptyLifecycleNode> node_;
 };
 
 TEST_P(TestLifecyclePublisher, publish_managed_by_node) {
+  auto node = std::make_shared<EmptyLifecycleNode>("node", GetParam());
+
+  rclcpp::PublisherOptionsWithAllocator<std::allocator<void>> options;
+  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<test_msgs::msg::Empty>> publisher =
+    node->create_publisher<test_msgs::msg::Empty>(std::string("topic"), rclcpp::QoS(10), options);
+
   // transition via LifecycleNode
   auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
   auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
   auto ret = reset_key;
 
-  EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, node_->get_current_state().id());
-  node_->trigger_transition(
+  EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, node->get_current_state().id());
+  node->trigger_transition(
     rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE), ret);
   ASSERT_EQ(success, ret);
   ret = reset_key;
-  node_->trigger_transition(
+  node->trigger_transition(
     rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE), ret);
   ASSERT_EQ(success, ret);
   ret = reset_key;
-  EXPECT_TRUE(node_->publisher()->is_activated());
+  EXPECT_TRUE(publisher->is_activated());
   {
     auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
-    EXPECT_NO_THROW(node_->publisher()->publish(*msg_ptr));
+    EXPECT_NO_THROW(publisher->publish(*msg_ptr));
   }
   {
     auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
-    EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr)));
+    EXPECT_NO_THROW(publisher->publish(std::move(msg_ptr)));
   }
   {
-    auto loaned_msg = node_->publisher()->borrow_loaned_message();
-    EXPECT_NO_THROW(node_->publisher()->publish(std::move(loaned_msg)));
+    auto loaned_msg = publisher->borrow_loaned_message();
+    EXPECT_NO_THROW(publisher->publish(std::move(loaned_msg)));
   }
-  node_->trigger_transition(
+  node->trigger_transition(
     rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE), ret);
   ASSERT_EQ(success, ret);
   ret = reset_key;
   (void)ret;  // Just to make clang happy
-  EXPECT_FALSE(node_->publisher()->is_activated());
+  EXPECT_FALSE(publisher->is_activated());
   {
     auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
-    EXPECT_NO_THROW(node_->publisher()->publish(*msg_ptr));
+    EXPECT_NO_THROW(publisher->publish(*msg_ptr));
   }
   {
     auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
-    EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr)));
+    EXPECT_NO_THROW(publisher->publish(std::move(msg_ptr)));
   }
   {
-    auto loaned_msg = node_->publisher()->borrow_loaned_message();
-    EXPECT_NO_THROW(node_->publisher()->publish(std::move(loaned_msg)));
+    auto loaned_msg = publisher->borrow_loaned_message();
+    EXPECT_NO_THROW(publisher->publish(std::move(loaned_msg)));
   }
 }
 
 TEST_P(TestLifecyclePublisher, publish) {
+  auto node = std::make_shared<EmptyLifecycleNode>("node", GetParam());
+
+  rclcpp::PublisherOptionsWithAllocator<std::allocator<void>> options;
+  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<test_msgs::msg::Empty>> publisher =
+    node->create_publisher<test_msgs::msg::Empty>(std::string("topic"), rclcpp::QoS(10), options);
+
   // transition via LifecyclePublisher
-  node_->publisher()->on_deactivate();
-  EXPECT_FALSE(node_->publisher()->is_activated());
+  publisher->on_deactivate();
+  EXPECT_FALSE(publisher->is_activated());
   {
     auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
-    EXPECT_NO_THROW(node_->publisher()->publish(*msg_ptr));
+    EXPECT_NO_THROW(publisher->publish(*msg_ptr));
   }
   {
     auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
-    EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr)));
+    EXPECT_NO_THROW(publisher->publish(std::move(msg_ptr)));
   }
   {
-    auto loaned_msg = node_->publisher()->borrow_loaned_message();
-    EXPECT_NO_THROW(node_->publisher()->publish(std::move(loaned_msg)));
+    auto loaned_msg = publisher->borrow_loaned_message();
+    EXPECT_NO_THROW(publisher->publish(std::move(loaned_msg)));
   }
-  node_->publisher()->on_activate();
-  EXPECT_TRUE(node_->publisher()->is_activated());
+  publisher->on_activate();
+  EXPECT_TRUE(publisher->is_activated());
   {
     auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
-    EXPECT_NO_THROW(node_->publisher()->publish(*msg_ptr));
+    EXPECT_NO_THROW(publisher->publish(*msg_ptr));
   }
   {
     auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
-    EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr)));
+    EXPECT_NO_THROW(publisher->publish(std::move(msg_ptr)));
   }
   {
-    auto loaned_msg = node_->publisher()->borrow_loaned_message();
-    EXPECT_NO_THROW(node_->publisher()->publish(std::move(loaned_msg)));
+    auto loaned_msg = publisher->borrow_loaned_message();
+    EXPECT_NO_THROW(publisher->publish(std::move(loaned_msg)));
   }
 }