From dbffe83709deda96b65c6b1a3893d9b240f99637 Mon Sep 17 00:00:00 2001
From: Christophe Bedard <christophe.bedard@apex.ai>
Date: Sat, 9 Nov 2024 14:58:39 -0800
Subject: [PATCH] Instrument client/service for end-to-end request/response
 tracking

Signed-off-by: Christophe Bedard <christophe.bedard@apex.ai>
---
 rmw_cyclonedds_cpp/src/rmw_node.cpp | 50 +++++++++++++++++++++++++++--
 1 file changed, 48 insertions(+), 2 deletions(-)

diff --git a/rmw_cyclonedds_cpp/src/rmw_node.cpp b/rmw_cyclonedds_cpp/src/rmw_node.cpp
index 6973963c..bae8bfa2 100644
--- a/rmw_cyclonedds_cpp/src/rmw_node.cpp
+++ b/rmw_cyclonedds_cpp/src/rmw_node.cpp
@@ -4676,6 +4676,12 @@ extern "C" rmw_ret_t rmw_take_response(
     info->reqtime.erase(seq);
   }
 #endif
+  TRACETOOLS_TRACEPOINT(
+    rmw_take_response,
+    static_cast<const void *>(client),
+    static_cast<const void *>(ros_response),
+    (nullptr != request_header ? request_header->request_id.sequence_number : 0LL),
+    *taken);
   return ret;
 }
 
@@ -4706,9 +4712,22 @@ extern "C" rmw_ret_t rmw_take_request(
     service, service->implementation_identifier, eclipse_cyclonedds_identifier,
     return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
   auto info = static_cast<CddsService *>(service->data);
-  return rmw_take_response_request(
+  rmw_ret_t ret = rmw_take_response_request(
     &info->service, request_header, ros_request, taken, nullptr,
     false);
+  if (TRACETOOLS_TRACEPOINT_ENABLED(rmw_take_request)) {
+    // Do not use the whole request_header->writer_guid, see the rmw_client_init tracepoint trigger
+    rmw_gid_t gid{};
+    memcpy(gid.data, &request_header->request_id.writer_guid, sizeof(info->service.pub->pubiid));
+    TRACETOOLS_DO_TRACEPOINT(
+      rmw_take_request,
+      static_cast<const void *>(service),
+      static_cast<const void *>(ros_request),
+      gid.data,
+      (nullptr != request_header ? request_header->request_id.sequence_number : 0LL),
+      *taken);
+  }
+  return ret;
 }
 
 static rmw_ret_t rmw_send_response_request(
@@ -4818,7 +4837,20 @@ extern "C" rmw_ret_t rmw_send_response(
     case client_present_t::MAYBE:
       return RMW_RET_TIMEOUT;
     case client_present_t::YES:
-      return rmw_send_response_request(&info->service, header, ros_response);
+      {
+        if (TRACETOOLS_TRACEPOINT_ENABLED(rmw_send_response)) {
+          // Do not use request_header->writer_guid, see the rmw_client_init tracepoint trigger
+          rmw_gid_t gid{};
+          memcpy(gid.data, &header.guid, sizeof(header.guid));
+          TRACETOOLS_DO_TRACEPOINT(
+            rmw_send_response,
+            static_cast<const void *>(service),
+            static_cast<const void *>(ros_response),
+            gid.data,
+            header.seq);
+        }
+        return rmw_send_response_request(&info->service, header, ros_response);
+      }
     case client_present_t::GONE:
       return RMW_RET_OK;
   }
@@ -4849,6 +4881,11 @@ extern "C" rmw_ret_t rmw_send_request(
   }
 #endif
 
+  TRACETOOLS_TRACEPOINT(
+    rmw_send_request,
+    static_cast<const void *>(client),
+    static_cast<const void *>(ros_request),
+    header.seq);
   return rmw_send_response_request(&info->client, header, ros_request);
 }
 
@@ -5151,6 +5188,15 @@ extern "C" rmw_client_t * rmw_create_client(
   cleanup_client.cancel();
   cleanup_fini_cs.cancel();
   cleanup_info.cancel();
+  if (TRACETOOLS_TRACEPOINT_ENABLED(rmw_client_init)) {
+    // rmw_cyclonedds uses info->client.pub->pubiid as the internal request header GUID, which is
+    // the first half (8 bytes out of 16 bytes) of the rmw_request_id_t's writer_guid. The second
+    // half doesn't match when read from the client side and the service side, so only use the first
+    // half. The second half will be zeros on both client side and service side.
+    rmw_gid_t gid{};
+    memcpy(gid.data, &info->client.pub->pubiid, sizeof(info->client.pub->pubiid));
+    TRACETOOLS_DO_TRACEPOINT(rmw_client_init, static_cast<const void *>(rmw_client), gid.data);
+  }
   return rmw_client;
 }