From 4404c47adff6795f3ea731d2f86aacb00366e3ca Mon Sep 17 00:00:00 2001
From: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>
Date: Fri, 10 Jun 2022 17:18:25 -0300
Subject: [PATCH 1/2] Fix subscription.is_serialized() for callbacks with
 message info (#1950)

* Fix subscription.is_serialized() for callbacks with message info argument

Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>

* Add tests + please linters

Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>
---
 .../rclcpp/any_subscription_callback.hpp      |   8 +-
 .../rclcpp/test_any_subscription_callback.cpp | 105 ++++++++++++++++++
 2 files changed, 112 insertions(+), 1 deletion(-)

diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp
index ea26fad397..d4f5fc309b 100644
--- a/rclcpp/include/rclcpp/any_subscription_callback.hpp
+++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp
@@ -950,7 +950,13 @@ class AnySubscriptionCallback
       std::holds_alternative<UniquePtrSerializedMessageCallback>(callback_variant_) ||
       std::holds_alternative<SharedConstPtrSerializedMessageCallback>(callback_variant_) ||
       std::holds_alternative<ConstRefSharedConstPtrSerializedMessageCallback>(callback_variant_) ||
-      std::holds_alternative<SharedPtrSerializedMessageCallback>(callback_variant_);
+      std::holds_alternative<SharedPtrSerializedMessageCallback>(callback_variant_) ||
+      std::holds_alternative<ConstRefSerializedMessageWithInfoCallback>(callback_variant_) ||
+      std::holds_alternative<UniquePtrSerializedMessageWithInfoCallback>(callback_variant_) ||
+      std::holds_alternative<SharedConstPtrSerializedMessageWithInfoCallback>(callback_variant_) ||
+      std::holds_alternative<ConstRefSharedConstPtrSerializedMessageWithInfoCallback>(
+      callback_variant_) ||
+      std::holds_alternative<SharedPtrSerializedMessageWithInfoCallback>(callback_variant_);
   }
 
   void
diff --git a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp
index 4fd3f32626..45fe091f07 100644
--- a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp
+++ b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp
@@ -93,6 +93,111 @@ TEST_F(TestAnySubscriptionCallback, construct_destruct) {
   rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc2(allocator);
 }
 
+TEST_F(TestAnySubscriptionCallback, is_serialized_message_callback) {
+  {
+    rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc;
+    asc.set([](const rclcpp::SerializedMessage &) {});
+    EXPECT_TRUE(asc.is_serialized_message_callback());
+    EXPECT_NO_THROW(
+      asc.dispatch(
+        std::make_shared<rclcpp::SerializedMessage>(),
+        rclcpp::MessageInfo{}));
+  }
+  {
+    rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc;
+    asc.set([](const rclcpp::SerializedMessage &, const rclcpp::MessageInfo &) {});
+    EXPECT_TRUE(asc.is_serialized_message_callback());
+    EXPECT_NO_THROW(
+      asc.dispatch(
+        std::make_shared<rclcpp::SerializedMessage>(),
+        rclcpp::MessageInfo{}));
+  }
+  {
+    rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc;
+    asc.set([](const rclcpp::SerializedMessage &, const rclcpp::MessageInfo &) {});
+    EXPECT_TRUE(asc.is_serialized_message_callback());
+    EXPECT_NO_THROW(
+      asc.dispatch(
+        std::make_shared<rclcpp::SerializedMessage>(),
+        rclcpp::MessageInfo{}));
+  }
+  {
+    rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc;
+    asc.set([](std::unique_ptr<rclcpp::SerializedMessage>) {});
+    EXPECT_TRUE(asc.is_serialized_message_callback());
+    EXPECT_NO_THROW(
+      asc.dispatch(
+        std::make_shared<rclcpp::SerializedMessage>(),
+        rclcpp::MessageInfo{}));
+  }
+  {
+    rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc;
+    asc.set([](std::unique_ptr<rclcpp::SerializedMessage>, const rclcpp::MessageInfo &) {});
+    EXPECT_TRUE(asc.is_serialized_message_callback());
+    EXPECT_NO_THROW(
+      asc.dispatch(
+        std::make_shared<rclcpp::SerializedMessage>(),
+        rclcpp::MessageInfo{}));
+  }
+  {
+    rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc;
+    asc.set([](std::shared_ptr<const rclcpp::SerializedMessage>) {});
+    EXPECT_TRUE(asc.is_serialized_message_callback());
+    EXPECT_NO_THROW(
+      asc.dispatch(
+        std::make_shared<rclcpp::SerializedMessage>(),
+        rclcpp::MessageInfo{}));
+  }
+  {
+    rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc;
+    asc.set([](std::shared_ptr<const rclcpp::SerializedMessage>, const rclcpp::MessageInfo &) {});
+    EXPECT_TRUE(asc.is_serialized_message_callback());
+    EXPECT_NO_THROW(
+      asc.dispatch(
+        std::make_shared<rclcpp::SerializedMessage>(),
+        rclcpp::MessageInfo{}));
+  }
+  {
+    rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc;
+    asc.set([](const std::shared_ptr<const rclcpp::SerializedMessage> &) {});
+    EXPECT_TRUE(asc.is_serialized_message_callback());
+    EXPECT_NO_THROW(
+      asc.dispatch(
+        std::make_shared<rclcpp::SerializedMessage>(),
+        rclcpp::MessageInfo{}));
+  }
+  {
+    rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc;
+    asc.set(
+      [](
+        const std::shared_ptr<const rclcpp::SerializedMessage> &,
+        const rclcpp::MessageInfo &) {});
+    EXPECT_TRUE(asc.is_serialized_message_callback());
+    EXPECT_NO_THROW(
+      asc.dispatch(
+        std::make_shared<rclcpp::SerializedMessage>(),
+        rclcpp::MessageInfo{}));
+  }
+  {
+    rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc;
+    asc.set([](std::shared_ptr<rclcpp::SerializedMessage>) {});
+    EXPECT_TRUE(asc.is_serialized_message_callback());
+    EXPECT_NO_THROW(
+      asc.dispatch(
+        std::make_shared<rclcpp::SerializedMessage>(),
+        rclcpp::MessageInfo{}));
+  }
+  {
+    rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc;
+    asc.set([](std::shared_ptr<rclcpp::SerializedMessage>, const rclcpp::MessageInfo &) {});
+    EXPECT_TRUE(asc.is_serialized_message_callback());
+    EXPECT_NO_THROW(
+      asc.dispatch(
+        std::make_shared<rclcpp::SerializedMessage>(),
+        rclcpp::MessageInfo{}));
+  }
+}
+
 TEST_F(TestAnySubscriptionCallback, unset_dispatch_throw) {
   EXPECT_THROW(
     any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_),

From 913070c6670e4ca444625cbe1a92503a6f290686 Mon Sep 17 00:00:00 2001
From: mauropasse <mauropasse@hotmail.com>
Date: Mon, 15 May 2023 20:55:08 +0100
Subject: [PATCH 2/2] Declare rclcpp callbacks before the rcl entities (#2024)

This is to ensure callbacks are destroyed last
on entities destruction, avoiding the gap in time
in which rmw entities hold a reference to a
destroyed function.

Signed-off-by: Mauro Passerino <mpasserino@irobot.com>
Co-authored-by: Mauro Passerino <mpasserino@irobot.com>
---
 rclcpp/include/rclcpp/client.hpp            | 10 +++++++---
 rclcpp/include/rclcpp/service.hpp           | 10 +++++++---
 rclcpp/include/rclcpp/subscription_base.hpp | 11 ++++++++---
 3 files changed, 22 insertions(+), 9 deletions(-)

diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp
index d1751ae120..5242304f92 100644
--- a/rclcpp/include/rclcpp/client.hpp
+++ b/rclcpp/include/rclcpp/client.hpp
@@ -359,12 +359,16 @@ class ClientBase
   std::shared_ptr<rclcpp::Context> context_;
   rclcpp::Logger node_logger_;
 
+  std::recursive_mutex callback_mutex_;
+  // It is important to declare on_new_response_callback_ before
+  // client_handle_, so on destruction the client is
+  // destroyed first. Otherwise, the rmw client callback
+  // would point briefly to a destroyed function.
+  std::function<void(size_t)> on_new_response_callback_{nullptr};
+  // Declare client_handle_ after callback
   std::shared_ptr<rcl_client_t> client_handle_;
 
   std::atomic<bool> in_use_by_wait_set_{false};
-
-  std::recursive_mutex callback_mutex_;
-  std::function<void(size_t)> on_new_response_callback_{nullptr};
 };
 
 template<typename ServiceT>
diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp
index 1b69d4a89e..fc47fee122 100644
--- a/rclcpp/include/rclcpp/service.hpp
+++ b/rclcpp/include/rclcpp/service.hpp
@@ -262,15 +262,19 @@ class ServiceBase
 
   std::shared_ptr<rcl_node_t> node_handle_;
 
+  std::recursive_mutex callback_mutex_;
+  // It is important to declare on_new_request_callback_ before
+  // service_handle_, so on destruction the service is
+  // destroyed first. Otherwise, the rmw service callback
+  // would point briefly to a destroyed function.
+  std::function<void(size_t)> on_new_request_callback_{nullptr};
+  // Declare service_handle_ after callback
   std::shared_ptr<rcl_service_t> service_handle_;
   bool owns_rcl_handle_ = true;
 
   rclcpp::Logger node_logger_;
 
   std::atomic<bool> in_use_by_wait_set_{false};
-
-  std::recursive_mutex callback_mutex_;
-  std::function<void(size_t)> on_new_request_callback_{nullptr};
 };
 
 template<typename ServiceT>
diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp
index f21f27e864..7f24062039 100644
--- a/rclcpp/include/rclcpp/subscription_base.hpp
+++ b/rclcpp/include/rclcpp/subscription_base.hpp
@@ -557,6 +557,14 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
   rclcpp::node_interfaces::NodeBaseInterface * const node_base_;
 
   std::shared_ptr<rcl_node_t> node_handle_;
+
+  std::recursive_mutex callback_mutex_;
+  // It is important to declare on_new_message_callback_ before
+  // subscription_handle_, so on destruction the subscription is
+  // destroyed first. Otherwise, the rmw subscription callback
+  // would point briefly to a destroyed function.
+  std::function<void(size_t)> on_new_message_callback_{nullptr};
+  // Declare subscription_handle_ after callback
   std::shared_ptr<rcl_subscription_t> subscription_handle_;
   std::shared_ptr<rcl_subscription_t> intra_process_subscription_handle_;
   rclcpp::Logger node_logger_;
@@ -579,9 +587,6 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
   std::atomic<bool> intra_process_subscription_waitable_in_use_by_wait_set_{false};
   std::unordered_map<rclcpp::QOSEventHandlerBase *,
     std::atomic<bool>> qos_events_in_use_by_wait_set_;
-
-  std::recursive_mutex callback_mutex_;
-  std::function<void(size_t)> on_new_message_callback_{nullptr};
 };
 
 }  // namespace rclcpp