From 8306a63313cd9e21883344deff9abc782636a464 Mon Sep 17 00:00:00 2001
From: Chris Lalancette <clalancette@gmail.com>
Date: Wed, 20 Nov 2024 17:46:09 -0500
Subject: [PATCH] Fix the query timestamp. (#321)

In particular, make it the time we received it in the
service_data_handler, not the time it was "taken" (which
may be quite a bit later).  This aligns it with what
the client reply timestamp does.

Signed-off-by: Chris Lalancette <clalancette@gmail.com>
---
 rmw_zenoh_cpp/src/detail/rmw_service_data.cpp | 10 ++++++----
 rmw_zenoh_cpp/src/detail/zenoh_utils.cpp      |  9 ++++++++-
 rmw_zenoh_cpp/src/detail/zenoh_utils.hpp      |  5 ++++-
 3 files changed, 18 insertions(+), 6 deletions(-)

diff --git a/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp
index 9be89277..42366ef0 100644
--- a/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp
+++ b/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp
@@ -59,7 +59,10 @@ void service_data_handler(const z_query_t * query, void * data)
     return;
   }
 
-  service_data->add_new_query(std::make_unique<ZenohQuery>(query));
+  std::chrono::nanoseconds::rep received_timestamp =
+    std::chrono::system_clock::now().time_since_epoch().count();
+
+  service_data->add_new_query(std::make_unique<ZenohQuery>(query, received_timestamp));
 }
 
 ///=============================================================================
@@ -339,9 +342,8 @@ rmw_ret_t ServiceData::take_request(
     RMW_SET_ERROR_MSG("Could not get client GID from attachment");
     return RMW_RET_ERROR;
   }
-  auto now = std::chrono::system_clock::now().time_since_epoch();
-  auto now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(now);
-  request_header->received_timestamp = now_ns.count();
+
+  request_header->received_timestamp = query->get_received_timestamp();
 
   // Add this query to the map, so that rmw_send_response can quickly look it up later.
   const size_t hash = rmw_zenoh_cpp::hash_gid(request_header->request_id.writer_guid);
diff --git a/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp b/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp
index 2e6c8e0c..e30e80d0 100644
--- a/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp
+++ b/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp
@@ -64,9 +64,16 @@ create_map_and_set_sequence_num(
 }
 
 ///=============================================================================
-ZenohQuery::ZenohQuery(const z_query_t * query)
+ZenohQuery::ZenohQuery(const z_query_t * query, std::chrono::nanoseconds::rep received_timestamp)
 {
   query_ = z_query_clone(query);
+  received_timestamp_ = received_timestamp;
+}
+
+///=============================================================================
+std::chrono::nanoseconds::rep ZenohQuery::get_received_timestamp() const
+{
+  return received_timestamp_;
 }
 
 ///=============================================================================
diff --git a/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp b/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp
index f7ec26b2..a750cbf2 100644
--- a/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp
+++ b/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp
@@ -56,14 +56,17 @@ class ZenohReply final
 class ZenohQuery final
 {
 public:
-  ZenohQuery(const z_query_t * query);
+  ZenohQuery(const z_query_t * query, std::chrono::nanoseconds::rep received_timestamp);
 
   ~ZenohQuery();
 
   const z_query_t get_query() const;
 
+  std::chrono::nanoseconds::rep get_received_timestamp() const;
+
 private:
   z_owned_query_t query_;
+  std::chrono::nanoseconds::rep received_timestamp_;
 };
 }  // namespace rmw_zenoh_cpp