Fix the received_timestamp for client data.

rmw_zenoh_cpp/src/detail/rmw_data_types.cpp

@@ -12,17 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <condition_variable>
-#include <cstring>
+#include <chrono>
 #include <memory>
 #include <mutex>
 #include <utility>
 
-#include "liveliness_utils.hpp"
 #include "logging_macros.hpp"
 
-#include "rcpputils/scope_exit.hpp"
-
 #include "rmw/error_handling.h"
 #include "rmw/impl/cpp/macros.hpp"
 
@@ -188,7 +184,10 @@ void client_data_handler(z_owned_reply_t * reply, void * data)
     return;
   }
 
-  client_data->add_new_reply(std::make_unique<ZenohReply>(reply));
+  std::chrono::nanoseconds::rep received_timestamp =
+    std::chrono::system_clock::now().time_since_epoch().count();
+
+  client_data->add_new_reply(std::make_unique<ZenohReply>(reply, received_timestamp));
   // Since we took ownership of the reply, null it out here
   *reply = z_reply_null();
 }

rmw_zenoh_cpp/src/detail/rmw_data_types.hpp

@@ -31,6 +31,7 @@
 #include "rosidl_runtime_c/type_hash.h"
 
 #include "event.hpp"
+#include "liveliness_utils.hpp"
 #include "message_type_support.hpp"
 #include "rmw_wait_set_data.hpp"
 #include "service_type_support.hpp"

rmw_zenoh_cpp/src/detail/zenoh_utils.cpp

@@ -82,9 +82,12 @@ const z_query_t ZenohQuery::get_query() const
 }
 
 ///=============================================================================
-ZenohReply::ZenohReply(const z_owned_reply_t * reply)
+ZenohReply::ZenohReply(
+  const z_owned_reply_t * reply,
+  std::chrono::nanoseconds::rep received_timestamp)
 {
   reply_ = *reply;
+  received_timestamp_ = received_timestamp;
 }
 
 ///=============================================================================
@@ -102,4 +105,10 @@ std::optional<z_sample_t> ZenohReply::get_sample() const
 
   return std::nullopt;
 }
+
+///=============================================================================
+std::chrono::nanoseconds::rep ZenohReply::get_received_timestamp() const
+{
+  return received_timestamp_;
+}
 }  // namespace rmw_zenoh_cpp

rmw_zenoh_cpp/src/detail/zenoh_utils.hpp

@@ -17,6 +17,7 @@
 
 #include <zenoh.h>
 
+#include <chrono>
 #include <functional>
 #include <optional>
 
@@ -37,14 +38,17 @@ create_map_and_set_sequence_num(int64_t sequence_number, GIDCopier gid_copier);
 class ZenohReply final
 {
 public:
-  ZenohReply(const z_owned_reply_t * reply);
+  ZenohReply(const z_owned_reply_t * reply, std::chrono::nanoseconds::rep received_timestamp);
 
   ~ZenohReply();
 
   std::optional<z_sample_t> get_sample() const;
 
+  std::chrono::nanoseconds::rep get_received_timestamp() const;
+
 private:
   z_owned_reply_t reply_;
+  std::chrono::nanoseconds::rep received_timestamp_;
 };
 
 // A class to store the queries made by clients.

rmw_zenoh_cpp/src/rmw_zenoh.cpp

@@ -1896,9 +1896,7 @@ rmw_take_response(
     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 = latest_reply->get_received_timestamp();
 
   *taken = true;