From b2c6def402adc5a2125176cc6edfd1cd903110a9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Laurenz=20Altenm=C3=BCller?=
 <laurenz.altenmueller@fernride.com>
Date: Tue, 25 Feb 2025 11:30:07 +0100
Subject: [PATCH] Patch rmw_cyclonedds

---
 .../rmw_cyclonedds-cache-writers.patch        | 46 +++++++++++++++++++
 .../patches/rmw_cyclonedds-lsan.-510.patch    | 32 +++++++++++++
 repositories/ros2_repositories_impl.bzl       |  4 ++
 3 files changed, 82 insertions(+)
 create mode 100644 repositories/patches/rmw_cyclonedds-cache-writers.patch
 create mode 100644 repositories/patches/rmw_cyclonedds-lsan.-510.patch

diff --git a/repositories/patches/rmw_cyclonedds-cache-writers.patch b/repositories/patches/rmw_cyclonedds-cache-writers.patch
new file mode 100644
index 00000000..1ab65a9b
--- /dev/null
+++ b/repositories/patches/rmw_cyclonedds-cache-writers.patch
@@ -0,0 +1,46 @@
+diff --git rmw_cyclonedds_cpp/src/rmw_node.cpp rmw_cyclonedds_cpp/src/rmw_node.cpp
+index 740ae8b..d2b76a2 100644
+--- rmw_cyclonedds_cpp/src/rmw_node.cpp
++++ rmw_cyclonedds_cpp/src/rmw_node.cpp
+@@ -181,6 +181,12 @@ static rmw_ret_t destroy_guard_condition(rmw_guard_condition_t * gc);
+ struct CddsDomain;
+ struct CddsWaitset;
+ 
++static std::unordered_map<const rosidl_message_type_support_t*, std::unique_ptr<rmw_cyclonedds_cpp::BaseCDRWriter>>& get_writers()
++{
++  static std::unordered_map<const rosidl_message_type_support_t*, std::unique_ptr<rmw_cyclonedds_cpp::BaseCDRWriter>> writers;
++  return writers;
++}
++
+ struct Cdds
+ {
+   std::mutex lock;
+@@ -1494,6 +1500,7 @@ extern "C" rmw_ret_t rmw_context_fini(rmw_context_t * context)
+     return RMW_RET_INVALID_ARGUMENT;
+   }
+   rmw_ret_t ret = rmw_init_options_fini(&context->options);
++  get_writers().clear();
+   delete context->impl;
+   *context = rmw_get_zero_initialized_context();
+   return ret;
+@@ -1674,8 +1681,18 @@ extern "C" rmw_ret_t rmw_serialize(
+   rmw_serialized_message_t * serialized_message)
+ {
+   try {
+-    auto writer = rmw_cyclonedds_cpp::make_cdr_writer(
+-      rmw_cyclonedds_cpp::make_message_value_type(type_support));
++    static std::mutex writers_mutex;
++    rmw_cyclonedds_cpp::BaseCDRWriter* writer;
++    std::lock_guard<std::mutex> writers_lock(writers_mutex);
++    {
++      auto& writers = get_writers();
++      auto it = writers.find(type_support);
++      if (it == writers.end()) {
++        std::tie(it, std::ignore) = writers.emplace(type_support, rmw_cyclonedds_cpp::make_cdr_writer(
++          rmw_cyclonedds_cpp::make_message_value_type(type_support)));
++      }
++      writer = it->second.get();
++    }
+ 
+     auto size = writer->get_serialized_size(ros_message);
+     rmw_ret_t ret = rmw_serialized_message_resize(serialized_message, size);
diff --git a/repositories/patches/rmw_cyclonedds-lsan.-510.patch b/repositories/patches/rmw_cyclonedds-lsan.-510.patch
new file mode 100644
index 00000000..78a7d0f0
--- /dev/null
+++ b/repositories/patches/rmw_cyclonedds-lsan.-510.patch
@@ -0,0 +1,32 @@
+diff --git rmw_cyclonedds_cpp/src/rmw_node.cpp rmw_cyclonedds_cpp/src/rmw_node.cpp
+index af641101..83bd1928 100644
+--- rmw_cyclonedds_cpp/src/rmw_node.cpp
++++ rmw_cyclonedds_cpp/src/rmw_node.cpp
+@@ -191,6 +191,17 @@ struct Cdds
+   {}
+ };
+ 
++#if defined(__has_feature)
++#if __has_feature(address_sanitizer)   // for clang
++#define __SANITIZE_ADDRESS__           // GCC already sets this
++#endif
++#endif
++
++#if defined(__SANITIZE_ADDRESS__)
++#include <sanitizer/lsan_interface.h>
++#endif
++
++
+ /* Use construct-on-first-use for the global state rather than a plain global variable to
+    prevent its destructor from running prior to last use by some other component in the
+    system.  E.g., some rclcpp tests (at the time of this commit) drop a guard condition in
+@@ -209,6 +220,9 @@ struct Cdds
+ static Cdds & gcdds()
+ {
+   static Cdds * x = new Cdds();
++#if defined(__SANITIZE_ADDRESS__)
++  __lsan_ignore_object(x);
++#endif
+   return *x;
+ }
+ 
\ No newline at end of file
diff --git a/repositories/ros2_repositories_impl.bzl b/repositories/ros2_repositories_impl.bzl
index 4d1ea4b0..7a55fa58 100644
--- a/repositories/ros2_repositories_impl.bzl
+++ b/repositories/ros2_repositories_impl.bzl
@@ -235,6 +235,10 @@ def ros2_repositories_impl():
         http_archive,
         name = "ros2_rmw_cyclonedds",
         build_file = "@com_github_mvukov_rules_ros2//repositories:rmw_cyclonedds.BUILD.bazel",
+        patches = [
+            "@com_github_mvukov_rules_ros2//repositories/patches:rmw_cyclonedds-lsan.-510.patch",
+            "@com_github_mvukov_rules_ros2//repositories/patches:rmw_cyclonedds-cache-writers.patch",
+        ],
         sha256 = "58ef4fe3fd18eb723906df77eb10df1e69222b451e479c6ec85426ba48e16a8a",
         strip_prefix = "rmw_cyclonedds-1.3.4",
         url = "https://github.com/ros2/rmw_cyclonedds/archive/refs/tags/1.3.4.tar.gz",