Fix the query timestamp.

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 <[email protected]>

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);

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_;
 }
 
 ///=============================================================================

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