From c108215f8f489d5095b00c48f0bf2c7c628eb1bb Mon Sep 17 00:00:00 2001
From: yamasaki <114902604+ymski@users.noreply.github.com>
Date: Wed, 26 Apr 2023 16:46:23 +0900
Subject: [PATCH] feat: support rmw_take

Signed-off-by: yamasaki <114902604+ymski@users.noreply.github.com>
---
 CARET_trace/src/ros_trace_points.cpp | 26 +++++++++++++++-----------
 1 file changed, 15 insertions(+), 11 deletions(-)

diff --git a/CARET_trace/src/ros_trace_points.cpp b/CARET_trace/src/ros_trace_points.cpp
index 195a480b..5d672793 100644
--- a/CARET_trace/src/ros_trace_points.cpp
+++ b/CARET_trace/src/ros_trace_points.cpp
@@ -914,17 +914,21 @@ void ros_trace_rmw_take(
   const bool taken
 )
 {
-  (void) rmw_subscription_handle;
-  (void) message;
-  (void) source_timestamp;
-  (void) taken;
-// #ifdef DEBUG_OUTPUT
-//   std::cerr << "rmw_take," <<
-//     rmw_subscription_handle << "," <<
-//     message << "," <<
-//     source_timestamp << "," <<
-//     taken << "," << std::endl;
-// #endif
+  static auto & context = Singleton<Context>::get_instance();
+  static void * orig_func = dlsym(RTLD_NEXT, __func__);
+  using functionT = void (*)(const void *, const void *, int64_t, const bool);
+  if (context.is_recording_allowed()) {
+    ((functionT) orig_func)(rmw_subscription_handle, message,
+      source_timestamp, taken);
+
+#ifdef DEBUG_OUTPUT
+  std::cerr << "rmw_take," <<
+    rmw_subscription_handle << "," <<
+    message << "," <<
+    source_timestamp << "," <<
+    taken << "," << std::endl;
+#endif
+  }
 }
 
 void ros_trace_rmw_publish(