From a87a9b98f7c0511995fc44608709ef79d48fef98 Mon Sep 17 00:00:00 2001
From: ymski <114902604+ymski@users.noreply.github.com>
Date: Fri, 27 Oct 2023 18:18:04 +0900
Subject: [PATCH 1/7] add tracepoint to cyclonedds for serialized_publish

Signed-off-by: ymski <114902604+ymski@users.noreply.github.com>
---
 CARET_trace/src/hooked_trace_points.cpp | 25 +++++++++++++++++++++++++
 1 file changed, 25 insertions(+)

diff --git a/CARET_trace/src/hooked_trace_points.cpp b/CARET_trace/src/hooked_trace_points.cpp
index 955f857d..e0572315 100644
--- a/CARET_trace/src/hooked_trace_points.cpp
+++ b/CARET_trace/src/hooked_trace_points.cpp
@@ -38,6 +38,7 @@
 
 // #define DEBUG_OUTPUT
 
+#include "dds/ddsi/ddsi_serdata.h"
 #include "fastdds/rtps/common/CacheChange.h"
 
 #define STRINGIFY_(s) #s
@@ -56,6 +57,7 @@ rmw_ret_t rmw_get_gid_for_publisher(const rmw_publisher_t * publisher, rmw_gid_t
 namespace CYCLONEDDS
 {
 void * DDS_WRITE_IMPL;
+void * DDS_WRITECDR_IMPL;
 }
 
 // For FastDDS
@@ -206,6 +208,7 @@ void update_dds_function_addr()
     // clang-format on
   } else if (env_var == "rmw_cyclonedds_cpp") {
     CYCLONEDDS::DDS_WRITE_IMPL = lib->get_symbol("dds_write_impl");
+    CYCLONEDDS::DDS_WRITECDR_IMPL = lib->get_symbol("dds_writecdr_impl");
   }
 }
 
@@ -233,6 +236,28 @@ int dds_write_impl(void * wr, void * data, long tstamp, int action)  // NOLINT
   return dds_return;
 }
 
+// for cyclonedds
+// bind : &ros_message -> source_timestamp
+int dds_writecdr_impl(void *wr, void *xp, struct ddsi_serdata *dinp, bool flush)  // NOLINT
+{
+  static auto & context = Singleton<Context>::get_instance();
+  using functionT = int (*)(void *, void *, struct ddsi_serdata *, bool);  // NOLINT
+
+  // clang-format on
+  if (CYCLONEDDS::DDS_WRITECDR_IMPL == nullptr) {
+    update_dds_function_addr();
+  }
+  int dds_return = ((functionT)CYCLONEDDS::DDS_WRITECDR_IMPL)(wr, xp, dinp, flush);
+
+  if (context.is_recording_allowed()) {
+    tracepoint(TRACEPOINT_PROVIDER, dds_bind_addr_to_stamp, dinp, dinp->timestamp.v);
+#ifdef DEBUG_OUTPUT
+    std::cerr << "dds_bind_addr_to_stamp," << data << "," << tstamp << std::endl;
+#endif
+  }
+  return dds_return;
+}
+
 // for fastdds
 // bind : &ros_message -> source_timestamp
 void _ZN8eprosima8fastrtps4rtps13WriterHistory13set_fragmentsEPNS1_13CacheChange_tE(

From 40f5e4bb2b197f6780812214f6e954a6b87e1304 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Fri, 27 Oct 2023 09:22:35 +0000
Subject: [PATCH 2/7] ci(pre-commit): autofix

---
 CARET_trace/src/hooked_trace_points.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/CARET_trace/src/hooked_trace_points.cpp b/CARET_trace/src/hooked_trace_points.cpp
index e0572315..161569c3 100644
--- a/CARET_trace/src/hooked_trace_points.cpp
+++ b/CARET_trace/src/hooked_trace_points.cpp
@@ -58,7 +58,7 @@ namespace CYCLONEDDS
 {
 void * DDS_WRITE_IMPL;
 void * DDS_WRITECDR_IMPL;
-}
+}  // namespace CYCLONEDDS
 
 // For FastDDS
 namespace FASTDDS
@@ -238,7 +238,7 @@ int dds_write_impl(void * wr, void * data, long tstamp, int action)  // NOLINT
 
 // for cyclonedds
 // bind : &ros_message -> source_timestamp
-int dds_writecdr_impl(void *wr, void *xp, struct ddsi_serdata *dinp, bool flush)  // NOLINT
+int dds_writecdr_impl(void * wr, void * xp, struct ddsi_serdata * dinp, bool flush)  // NOLINT
 {
   static auto & context = Singleton<Context>::get_instance();
   using functionT = int (*)(void *, void *, struct ddsi_serdata *, bool);  // NOLINT

From 32d6367e88732aca6edc760fe29dbc0600c971fb Mon Sep 17 00:00:00 2001
From: ymski <114902604+ymski@users.noreply.github.com>
Date: Mon, 30 Oct 2023 16:28:17 +0900
Subject: [PATCH 3/7] ignore cspell check

Signed-off-by: ymski <114902604+ymski@users.noreply.github.com>
---
 CARET_trace/src/hooked_trace_points.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/CARET_trace/src/hooked_trace_points.cpp b/CARET_trace/src/hooked_trace_points.cpp
index 161569c3..e1544b54 100644
--- a/CARET_trace/src/hooked_trace_points.cpp
+++ b/CARET_trace/src/hooked_trace_points.cpp
@@ -54,6 +54,7 @@ extern thread_local bool trace_filter_is_rcl_publish_recorded;
 // Declare a prototype in order to use the functions implemented in cyclonedds.
 rmw_ret_t rmw_get_gid_for_publisher(const rmw_publisher_t * publisher, rmw_gid_t * gid);
 
+// cspell: ignore WRITECDR
 namespace CYCLONEDDS
 {
 void * DDS_WRITE_IMPL;
@@ -238,6 +239,7 @@ int dds_write_impl(void * wr, void * data, long tstamp, int action)  // NOLINT
 
 // for cyclonedds
 // bind : &ros_message -> source_timestamp
+// cspell: ignore ddsi, serdata, dinp
 int dds_writecdr_impl(void * wr, void * xp, struct ddsi_serdata * dinp, bool flush)  // NOLINT
 {
   static auto & context = Singleton<Context>::get_instance();

From 820baf92c0e0e831702350615038b6a86997b235 Mon Sep 17 00:00:00 2001
From: ymski <114902604+ymski@users.noreply.github.com>
Date: Tue, 31 Oct 2023 17:04:45 +0900
Subject: [PATCH 4/7] store message addres in thread local memory

Signed-off-by: ymski <114902604+ymski@users.noreply.github.com>
---
 CARET_trace/src/hooked_trace_points.cpp | 14 +++++++++++++-
 1 file changed, 13 insertions(+), 1 deletion(-)

diff --git a/CARET_trace/src/hooked_trace_points.cpp b/CARET_trace/src/hooked_trace_points.cpp
index e1544b54..7431e6a6 100644
--- a/CARET_trace/src/hooked_trace_points.cpp
+++ b/CARET_trace/src/hooked_trace_points.cpp
@@ -49,6 +49,7 @@
 #define SYMBOL_CONCAT_2(x, y) x##y
 #define SYMBOL_CONCAT_3(x, y, z) x##y##z
 
+thread_local const void* serialized_message_addr;
 extern thread_local bool trace_filter_is_rcl_publish_recorded;
 
 // Declare a prototype in order to use the functions implemented in cyclonedds.
@@ -252,7 +253,7 @@ int dds_writecdr_impl(void * wr, void * xp, struct ddsi_serdata * dinp, bool flu
   int dds_return = ((functionT)CYCLONEDDS::DDS_WRITECDR_IMPL)(wr, xp, dinp, flush);
 
   if (context.is_recording_allowed()) {
-    tracepoint(TRACEPOINT_PROVIDER, dds_bind_addr_to_stamp, dinp, dinp->timestamp.v);
+    tracepoint(TRACEPOINT_PROVIDER, dds_bind_addr_to_stamp, serialized_message_addr, dinp->timestamp.v);
 #ifdef DEBUG_OUTPUT
     std::cerr << "dds_bind_addr_to_stamp," << data << "," << tstamp << std::endl;
 #endif
@@ -281,6 +282,17 @@ void _ZN8eprosima8fastrtps4rtps13WriterHistory13set_fragmentsEPNS1_13CacheChange
   }
 }
 
+// for rmw_cyclonedds_cpp
+// store message address in tread local memory
+rmw_ret_t rmw_publish_serialized_message(const rmw_publisher_t * publisher, const rmw_serialized_message_t * serialized_message, rmw_publisher_allocation_t * allocation)
+{
+  serialized_message_addr = static_cast<const void *> (serialized_message);
+  using functionT = int (*)(const void *, const void *, const void *);
+  static void * orig_func = dlsym(RTLD_NEXT, __func__);
+  int dds_return = ((functionT) orig_func)(publisher, serialized_message, allocation);
+  return dds_return;
+}
+
 // rclcpp::executors::SingleThreadedExecutor::SingleThreadedExecutor(rclcpp::ExecutorOptions const&)
 void _ZN6rclcpp9executors22SingleThreadedExecutorC1ERKNS_15ExecutorOptionsE(
   void * obj, const void * option)

From 96d848105da0c6540a92906d5c0c5d3b6ff2560f Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Tue, 31 Oct 2023 08:06:34 +0000
Subject: [PATCH 5/7] ci(pre-commit): autofix

---
 CARET_trace/src/hooked_trace_points.cpp | 13 ++++++++-----
 1 file changed, 8 insertions(+), 5 deletions(-)

diff --git a/CARET_trace/src/hooked_trace_points.cpp b/CARET_trace/src/hooked_trace_points.cpp
index 7431e6a6..8ea10fb3 100644
--- a/CARET_trace/src/hooked_trace_points.cpp
+++ b/CARET_trace/src/hooked_trace_points.cpp
@@ -49,7 +49,7 @@
 #define SYMBOL_CONCAT_2(x, y) x##y
 #define SYMBOL_CONCAT_3(x, y, z) x##y##z
 
-thread_local const void* serialized_message_addr;
+thread_local const void * serialized_message_addr;
 extern thread_local bool trace_filter_is_rcl_publish_recorded;
 
 // Declare a prototype in order to use the functions implemented in cyclonedds.
@@ -253,7 +253,8 @@ int dds_writecdr_impl(void * wr, void * xp, struct ddsi_serdata * dinp, bool flu
   int dds_return = ((functionT)CYCLONEDDS::DDS_WRITECDR_IMPL)(wr, xp, dinp, flush);
 
   if (context.is_recording_allowed()) {
-    tracepoint(TRACEPOINT_PROVIDER, dds_bind_addr_to_stamp, serialized_message_addr, dinp->timestamp.v);
+    tracepoint(
+      TRACEPOINT_PROVIDER, dds_bind_addr_to_stamp, serialized_message_addr, dinp->timestamp.v);
 #ifdef DEBUG_OUTPUT
     std::cerr << "dds_bind_addr_to_stamp," << data << "," << tstamp << std::endl;
 #endif
@@ -284,12 +285,14 @@ void _ZN8eprosima8fastrtps4rtps13WriterHistory13set_fragmentsEPNS1_13CacheChange
 
 // for rmw_cyclonedds_cpp
 // store message address in tread local memory
-rmw_ret_t rmw_publish_serialized_message(const rmw_publisher_t * publisher, const rmw_serialized_message_t * serialized_message, rmw_publisher_allocation_t * allocation)
+rmw_ret_t rmw_publish_serialized_message(
+  const rmw_publisher_t * publisher, const rmw_serialized_message_t * serialized_message,
+  rmw_publisher_allocation_t * allocation)
 {
-  serialized_message_addr = static_cast<const void *> (serialized_message);
+  serialized_message_addr = static_cast<const void *>(serialized_message);
   using functionT = int (*)(const void *, const void *, const void *);
   static void * orig_func = dlsym(RTLD_NEXT, __func__);
-  int dds_return = ((functionT) orig_func)(publisher, serialized_message, allocation);
+  int dds_return = ((functionT)orig_func)(publisher, serialized_message, allocation);
   return dds_return;
 }
 

From c3eb33e930a43bb2d7737aa5dd07a18e9139f46f Mon Sep 17 00:00:00 2001
From: ymski <114902604+ymski@users.noreply.github.com>
Date: Wed, 1 Nov 2023 12:44:11 +0900
Subject: [PATCH 6/7] Update CARET_trace/src/hooked_trace_points.cpp

Co-authored-by: isp-uetsuki <35490433+isp-uetsuki@users.noreply.github.com>
---
 CARET_trace/src/hooked_trace_points.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/CARET_trace/src/hooked_trace_points.cpp b/CARET_trace/src/hooked_trace_points.cpp
index 8ea10fb3..f92764f6 100644
--- a/CARET_trace/src/hooked_trace_points.cpp
+++ b/CARET_trace/src/hooked_trace_points.cpp
@@ -290,10 +290,10 @@ rmw_ret_t rmw_publish_serialized_message(
   rmw_publisher_allocation_t * allocation)
 {
   serialized_message_addr = static_cast<const void *>(serialized_message);
-  using functionT = int (*)(const void *, const void *, const void *);
+  using functionT = rmw_ret_t (*)(const void *, const void *, const void *);
   static void * orig_func = dlsym(RTLD_NEXT, __func__);
-  int dds_return = ((functionT)orig_func)(publisher, serialized_message, allocation);
-  return dds_return;
+  rmw_ret_t ret = ((functionT)orig_func)(publisher, serialized_message, allocation);
+  return ret;
 }
 
 // rclcpp::executors::SingleThreadedExecutor::SingleThreadedExecutor(rclcpp::ExecutorOptions const&)

From 5c47648d367c93773458fb2985dc859d80d8f48b Mon Sep 17 00:00:00 2001
From: ymski <114902604+ymski@users.noreply.github.com>
Date: Wed, 1 Nov 2023 12:44:18 +0900
Subject: [PATCH 7/7] Update CARET_trace/src/hooked_trace_points.cpp

Co-authored-by: isp-uetsuki <35490433+isp-uetsuki@users.noreply.github.com>
---
 CARET_trace/src/hooked_trace_points.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/CARET_trace/src/hooked_trace_points.cpp b/CARET_trace/src/hooked_trace_points.cpp
index f92764f6..afe65b34 100644
--- a/CARET_trace/src/hooked_trace_points.cpp
+++ b/CARET_trace/src/hooked_trace_points.cpp
@@ -49,7 +49,7 @@
 #define SYMBOL_CONCAT_2(x, y) x##y
 #define SYMBOL_CONCAT_3(x, y, z) x##y##z
 
-thread_local const void * serialized_message_addr;
+static thread_local const void * serialized_message_addr;
 extern thread_local bool trace_filter_is_rcl_publish_recorded;
 
 // Declare a prototype in order to use the functions implemented in cyclonedds.