diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp index c1e56bfd..acf0bc70 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp +++ b/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 -#include +#include #include #include #include -#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(reply)); + std::chrono::nanoseconds::rep received_timestamp = + std::chrono::system_clock::now().time_since_epoch().count(); + + client_data->add_new_reply(std::make_unique(reply, received_timestamp)); // Since we took ownership of the reply, null it out here *reply = z_reply_null(); } diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index 5205f3cf..5605d7f0 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/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" diff --git a/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp b/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp index db07d01e..2e6c8e0c 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp +++ b/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 ZenohReply::get_sample() const return std::nullopt; } + +///============================================================================= +std::chrono::nanoseconds::rep ZenohReply::get_received_timestamp() const +{ + return received_timestamp_; +} } // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp b/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp index c57fc87e..f7ec26b2 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp @@ -17,6 +17,7 @@ #include +#include #include #include @@ -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 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. diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index df2529a6..31956676 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/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(now); - request_header->received_timestamp = now_ns.count(); + request_header->received_timestamp = latest_reply->get_received_timestamp(); *taken = true;