diff --git a/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp b/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp
index 9b85e49f2..51f6003cc 100644
--- a/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp
+++ b/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp
@@ -109,7 +109,10 @@ __rmw_publish_serialized_message(
   data.type = FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER;
   data.data = &ser;
   data.impl = nullptr;  // not used when type is FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER
-  if (!info->data_writer_->write(&data)) {
+  eprosima::fastrtps::Time_t stamp;
+  eprosima::fastrtps::Time_t::now(stamp);
+  TRACETOOLS_TRACEPOINT(rmw_publish, publisher, serialized_message, stamp.to_ns());
+  if (!info->data_writer_->write_w_timestamp(&data, eprosima::fastdds::dds::HANDLE_NIL, stamp)) {
     RMW_SET_ERROR_MSG("cannot publish data");
     return RMW_RET_ERROR;
   }
diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp
index 7421d6211..44d9d86e5 100644
--- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp
+++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp
@@ -348,7 +348,12 @@ _take_serialized_message(
       break;
     }
   }
-
+  TRACETOOLS_TRACEPOINT(
+    rmw_take,
+    static_cast<const void *>(subscription),
+    static_cast<const void *>(serialized_message),
+    (message_info ? message_info->source_timestamp : 0LL),
+    *taken);
   return RMW_RET_OK;
 }